958 files changed, 146075 insertions, 20954 deletions

diff --git a/drivers/acpi/acpi_processor.c b/drivers/acpi/acpi_processor.c
index c9311be29a64..c29c2c3ec0ad 100644
--- a/drivers/acpi/acpi_processor.c
+++ b/drivers/acpi/acpi_processor.c
@@ -261,7 +261,7 @@ static int acpi_processor_get_info(struct acpi_device *device)
 
 	apic_id = acpi_get_apicid(pr->handle, device_declaration, pr->acpi_id);
 	if (apic_id < 0) {
-		acpi_handle_err(pr->handle, "failed to get CPU APIC ID.\n");
+		acpi_handle_debug(pr->handle, "failed to get CPU APIC ID.\n");
 		return -ENODEV;
 	}
 	pr->apic_id = apic_id;
diff --git a/drivers/acpi/acpica/acglobal.h b/drivers/acpi/acpica/acglobal.h
index 24db8e153bf0..4ed1aa384df2 100644
--- a/drivers/acpi/acpica/acglobal.h
+++ b/drivers/acpi/acpica/acglobal.h
@@ -108,7 +108,7 @@ u8 ACPI_INIT_GLOBAL(acpi_gbl_use_default_register_widths, TRUE);
 /*
  * Optionally enable output from the AML Debug Object.
  */
-bool ACPI_INIT_GLOBAL(acpi_gbl_enable_aml_debug_object, FALSE);
+u8 ACPI_INIT_GLOBAL(acpi_gbl_enable_aml_debug_object, FALSE);
 
 /*
  * Optionally copy the entire DSDT to local memory (instead of simply
diff --git a/drivers/acpi/battery.c b/drivers/acpi/battery.c
index 470e7542bf31..018a42883706 100644
--- a/drivers/acpi/battery.c
+++ b/drivers/acpi/battery.c
@@ -549,7 +549,7 @@ static ssize_t acpi_battery_alarm_store(struct device *dev,
 {
 	unsigned long x;
 	struct acpi_battery *battery = to_acpi_battery(dev_get_drvdata(dev));
-	if (sscanf(buf, "%ld\n", &x) == 1)
+	if (sscanf(buf, "%lu\n", &x) == 1)
 		battery->alarm = x/1000;
 	if (acpi_battery_present(battery))
 		acpi_battery_set_alarm(battery);
diff --git a/drivers/acpi/bus.c b/drivers/acpi/bus.c
index 384da5ab5955..fcb59c21c68d 100644
--- a/drivers/acpi/bus.c
+++ b/drivers/acpi/bus.c
@@ -33,6 +33,7 @@
 #include <linux/proc_fs.h>
 #include <linux/acpi.h>
 #include <linux/slab.h>
+#include <linux/regulator/machine.h>
 #ifdef CONFIG_X86
 #include <asm/mpspec.h>
 #endif
@@ -509,6 +510,14 @@ void __init acpi_early_init(void)
 		goto error0;
 	}
 
+	/*
+	 * If the system is using ACPI then we can be reasonably
+	 * confident that any regulators are managed by the firmware
+	 * so tell the regulator core it has everything it needs to
+	 * know.
+	 */
+	regulator_has_full_constraints();
+
 	return;
 
       error0:
diff --git a/drivers/acpi/device_pm.c b/drivers/acpi/device_pm.c
index d49f1e464703..c14a00d3dca6 100644
--- a/drivers/acpi/device_pm.c
+++ b/drivers/acpi/device_pm.c
@@ -727,18 +727,6 @@ int acpi_pm_device_sleep_wake(struct device *dev, bool enable)
 #endif /* CONFIG_PM_SLEEP */
 
 /**
- * acpi_dev_pm_get_node - Get ACPI device node for the given physical device.
- * @dev: Device to get the ACPI node for.
- */
-struct acpi_device *acpi_dev_pm_get_node(struct device *dev)
-{
-	acpi_handle handle = ACPI_HANDLE(dev);
-	struct acpi_device *adev;
-
-	return handle && !acpi_bus_get_device(handle, &adev) ? adev : NULL;
-}
-
-/**
  * acpi_dev_pm_low_power - Put ACPI device into a low-power state.
  * @dev: Device to put into a low-power state.
  * @adev: ACPI device node corresponding to @dev.
@@ -778,7 +766,7 @@ static int acpi_dev_pm_full_power(struct acpi_device *adev)
  */
 int acpi_dev_runtime_suspend(struct device *dev)
 {
-	struct acpi_device *adev = acpi_dev_pm_get_node(dev);
+	struct acpi_device *adev = ACPI_COMPANION(dev);
 	bool remote_wakeup;
 	int error;
 
@@ -809,7 +797,7 @@ EXPORT_SYMBOL_GPL(acpi_dev_runtime_suspend);
  */
 int acpi_dev_runtime_resume(struct device *dev)
 {
-	struct acpi_device *adev = acpi_dev_pm_get_node(dev);
+	struct acpi_device *adev = ACPI_COMPANION(dev);
 	int error;
 
 	if (!adev)
@@ -862,7 +850,7 @@ EXPORT_SYMBOL_GPL(acpi_subsys_runtime_resume);
  */
 int acpi_dev_suspend_late(struct device *dev)
 {
-	struct acpi_device *adev = acpi_dev_pm_get_node(dev);
+	struct acpi_device *adev = ACPI_COMPANION(dev);
 	u32 target_state;
 	bool wakeup;
 	int error;
@@ -894,7 +882,7 @@ EXPORT_SYMBOL_GPL(acpi_dev_suspend_late);
  */
 int acpi_dev_resume_early(struct device *dev)
 {
-	struct acpi_device *adev = acpi_dev_pm_get_node(dev);
+	struct acpi_device *adev = ACPI_COMPANION(dev);
 	int error;
 
 	if (!adev)
@@ -985,7 +973,7 @@ static struct dev_pm_domain acpi_general_pm_domain = {
  */
 int acpi_dev_pm_attach(struct device *dev, bool power_on)
 {
-	struct acpi_device *adev = acpi_dev_pm_get_node(dev);
+	struct acpi_device *adev = ACPI_COMPANION(dev);
 
 	if (!adev)
 		return -ENODEV;
@@ -1017,7 +1005,7 @@ EXPORT_SYMBOL_GPL(acpi_dev_pm_attach);
  */
 void acpi_dev_pm_detach(struct device *dev, bool power_off)
 {
-	struct acpi_device *adev = acpi_dev_pm_get_node(dev);
+	struct acpi_device *adev = ACPI_COMPANION(dev);
 
 	if (adev && dev->pm_domain == &acpi_general_pm_domain) {
 		dev->pm_domain = NULL;
diff --git a/drivers/acpi/proc.c b/drivers/acpi/proc.c
index 50fe34ffe932..75c28eae8860 100644
--- a/drivers/acpi/proc.c
+++ b/drivers/acpi/proc.c
@@ -60,7 +60,7 @@ acpi_system_wakeup_device_seq_show(struct seq_file *seq, void *offset)
 				seq_printf(seq, "%c%-8s  %s:%s\n",
 					dev->wakeup.flags.run_wake ? '*' : ' ',
 					(device_may_wakeup(&dev->dev) ||
-					(ldev && device_may_wakeup(ldev))) ?
+					device_may_wakeup(ldev)) ?
 					"enabled" : "disabled",
 					ldev->bus ? ldev->bus->name :
 					"no-bus", dev_name(ldev));
diff --git a/drivers/acpi/processor_core.c b/drivers/acpi/processor_core.c
index 34e7b3c6a08d..a4eea9a508d3 100644
--- a/drivers/acpi/processor_core.c
+++ b/drivers/acpi/processor_core.c
@@ -44,13 +44,13 @@ static int map_lapic_id(struct acpi_subtable_header *entry,
 		(struct acpi_madt_local_apic *)entry;
 
 	if (!(lapic->lapic_flags & ACPI_MADT_ENABLED))
-		return 0;
+		return -ENODEV;
 
 	if (lapic->processor_id != acpi_id)
-		return 0;
+		return -EINVAL;
 
 	*apic_id = lapic->id;
-	return 1;
+	return 0;
 }
 
 static int map_x2apic_id(struct acpi_subtable_header *entry,
@@ -60,14 +60,14 @@ static int map_x2apic_id(struct acpi_subtable_header *entry,
 		(struct acpi_madt_local_x2apic *)entry;
 
 	if (!(apic->lapic_flags & ACPI_MADT_ENABLED))
-		return 0;
+		return -ENODEV;
 
 	if (device_declaration && (apic->uid == acpi_id)) {
 		*apic_id = apic->local_apic_id;
-		return 1;
+		return 0;
 	}
 
-	return 0;
+	return -EINVAL;
 }
 
 static int map_lsapic_id(struct acpi_subtable_header *entry,
@@ -77,16 +77,16 @@ static int map_lsapic_id(struct acpi_subtable_header *entry,
 		(struct acpi_madt_local_sapic *)entry;
 
 	if (!(lsapic->lapic_flags & ACPI_MADT_ENABLED))
-		return 0;
+		return -ENODEV;
 
 	if (device_declaration) {
 		if ((entry->length < 16) || (lsapic->uid != acpi_id))
-			return 0;
+			return -EINVAL;
 	} else if (lsapic->processor_id != acpi_id)
-		return 0;
+		return -EINVAL;
 
 	*apic_id = (lsapic->id << 8) | lsapic->eid;
-	return 1;
+	return 0;
 }
 
 static int map_madt_entry(int type, u32 acpi_id)
@@ -116,13 +116,13 @@ static int map_madt_entry(int type, u32 acpi_id)
 		struct acpi_subtable_header *header =
 			(struct acpi_subtable_header *)entry;
 		if (header->type == ACPI_MADT_TYPE_LOCAL_APIC) {
-			if (map_lapic_id(header, acpi_id, &apic_id))
+			if (!map_lapic_id(header, acpi_id, &apic_id))
 				break;
 		} else if (header->type == ACPI_MADT_TYPE_LOCAL_X2APIC) {
-			if (map_x2apic_id(header, type, acpi_id, &apic_id))
+			if (!map_x2apic_id(header, type, acpi_id, &apic_id))
 				break;
 		} else if (header->type == ACPI_MADT_TYPE_LOCAL_SAPIC) {
-			if (map_lsapic_id(header, type, acpi_id, &apic_id))
+			if (!map_lsapic_id(header, type, acpi_id, &apic_id))
 				break;
 		}
 		entry += header->length;
diff --git a/drivers/acpi/scan.c b/drivers/acpi/scan.c
index e00365ccb897..57b053f424d1 100644
--- a/drivers/acpi/scan.c
+++ b/drivers/acpi/scan.c
@@ -484,7 +484,6 @@ static void acpi_device_hotplug(void *data, u32 src)
 static void acpi_hotplug_notify_cb(acpi_handle handle, u32 type, void *data)
 {
 	u32 ost_code = ACPI_OST_SC_NON_SPECIFIC_FAILURE;
-	struct acpi_scan_handler *handler = data;
 	struct acpi_device *adev;
 	acpi_status status;
 
@@ -500,7 +499,10 @@ static void acpi_hotplug_notify_cb(acpi_handle handle, u32 type, void *data)
 		break;
 	case ACPI_NOTIFY_EJECT_REQUEST:
 		acpi_handle_debug(handle, "ACPI_NOTIFY_EJECT_REQUEST event\n");
-		if (!handler->hotplug.enabled) {
+		if (!adev->handler)
+			goto err_out;
+
+		if (!adev->handler->hotplug.enabled) {
 			acpi_handle_err(handle, "Eject disabled\n");
 			ost_code = ACPI_OST_SC_EJECT_NOT_SUPPORTED;
 			goto err_out;
@@ -2105,6 +2107,7 @@ void acpi_bus_trim(struct acpi_device *adev)
 	list_for_each_entry_reverse(child, &adev->children, node)
 		acpi_bus_trim(child);
 
+	adev->flags.match_driver = false;
 	if (handler) {
 		if (handler->detach)
 			handler->detach(adev);
diff --git a/drivers/acpi/sysfs.c b/drivers/acpi/sysfs.c
index 443dc9366052..91a32cefb11f 100644
--- a/drivers/acpi/sysfs.c
+++ b/drivers/acpi/sysfs.c
@@ -226,7 +226,7 @@ module_param_call(trace_state, param_set_trace_state, param_get_trace_state,
 /* /sys/modules/acpi/parameters/aml_debug_output */
 
 module_param_named(aml_debug_output, acpi_gbl_enable_aml_debug_object,
-		   bool, 0644);
+		   byte, 0644);
 MODULE_PARM_DESC(aml_debug_output,
 		 "To enable/disable the ACPI Debug Object output.");
 
diff --git a/drivers/acpi/utils.c b/drivers/acpi/utils.c
index 0347a37eb438..85e3b612bdc0 100644
--- a/drivers/acpi/utils.c
+++ b/drivers/acpi/utils.c
@@ -99,10 +99,6 @@ acpi_extract_package(union acpi_object *package,
 
 		union acpi_object *element = &(package->package.elements[i]);
 
-		if (!element) {
-			return AE_BAD_DATA;
-		}
-
 		switch (element->type) {
 
 		case ACPI_TYPE_INTEGER:
diff --git a/drivers/acpi/video_detect.c b/drivers/acpi/video_detect.c
index f0447d3daf2c..a697b77b8865 100644
--- a/drivers/acpi/video_detect.c
+++ b/drivers/acpi/video_detect.c
@@ -170,6 +170,14 @@ static struct dmi_system_id video_detect_dmi_table[] = {
 	},
 	{
 	.callback = video_detect_force_vendor,
+	.ident = "HP EliteBook Revolve 810",
+	.matches = {
+		DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
+		DMI_MATCH(DMI_PRODUCT_NAME, "HP EliteBook Revolve 810 G1"),
+		},
+	},
+	{
+	.callback = video_detect_force_vendor,
 	.ident = "Lenovo Yoga 13",
 	.matches = {
 		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
diff --git a/drivers/bcma/Kconfig b/drivers/bcma/Kconfig
index 7c081b38ef3e..0ee48be23837 100644
--- a/drivers/bcma/Kconfig
+++ b/drivers/bcma/Kconfig
@@ -75,6 +75,7 @@ config BCMA_DRIVER_GMAC_CMN
 config BCMA_DRIVER_GPIO
 	bool "BCMA GPIO driver"
 	depends on BCMA && GPIOLIB
+	select IRQ_DOMAIN if BCMA_HOST_SOC
 	help
 	  Driver to provide access to the GPIO pins of the bcma bus.
 
diff --git a/drivers/bcma/driver_gpio.c b/drivers/bcma/driver_gpio.c
index 45f0996a3752..25f9887a35d0 100644
--- a/drivers/bcma/driver_gpio.c
+++ b/drivers/bcma/driver_gpio.c
@@ -9,6 +9,9 @@
  */
 
 #include <linux/gpio.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/irqdomain.h>
 #include <linux/export.h>
 #include <linux/bcma/bcma.h>
 
@@ -73,19 +76,136 @@ static void bcma_gpio_free(struct gpio_chip *chip, unsigned gpio)
 	bcma_chipco_gpio_pullup(cc, 1 << gpio, 0);
 }
 
+#if IS_BUILTIN(CONFIG_BCMA_HOST_SOC)
 static int bcma_gpio_to_irq(struct gpio_chip *chip, unsigned gpio)
 {
 	struct bcma_drv_cc *cc = bcma_gpio_get_cc(chip);
 
 	if (cc->core->bus->hosttype == BCMA_HOSTTYPE_SOC)
-		return bcma_core_irq(cc->core);
+		return irq_find_mapping(cc->irq_domain, gpio);
 	else
 		return -EINVAL;
 }
 
+static void bcma_gpio_irq_unmask(struct irq_data *d)
+{
+	struct bcma_drv_cc *cc = irq_data_get_irq_chip_data(d);
+	int gpio = irqd_to_hwirq(d);
+	u32 val = bcma_chipco_gpio_in(cc, BIT(gpio));
+
+	bcma_chipco_gpio_polarity(cc, BIT(gpio), val);
+	bcma_chipco_gpio_intmask(cc, BIT(gpio), BIT(gpio));
+}
+
+static void bcma_gpio_irq_mask(struct irq_data *d)
+{
+	struct bcma_drv_cc *cc = irq_data_get_irq_chip_data(d);
+	int gpio = irqd_to_hwirq(d);
+
+	bcma_chipco_gpio_intmask(cc, BIT(gpio), 0);
+}
+
+static struct irq_chip bcma_gpio_irq_chip = {
+	.name		= "BCMA-GPIO",
+	.irq_mask	= bcma_gpio_irq_mask,
+	.irq_unmask	= bcma_gpio_irq_unmask,
+};
+
+static irqreturn_t bcma_gpio_irq_handler(int irq, void *dev_id)
+{
+	struct bcma_drv_cc *cc = dev_id;
+	u32 val = bcma_cc_read32(cc, BCMA_CC_GPIOIN);
+	u32 mask = bcma_cc_read32(cc, BCMA_CC_GPIOIRQ);
+	u32 pol = bcma_cc_read32(cc, BCMA_CC_GPIOPOL);
+	unsigned long irqs = (val ^ pol) & mask;
+	int gpio;
+
+	if (!irqs)
+		return IRQ_NONE;
+
+	for_each_set_bit(gpio, &irqs, cc->gpio.ngpio)
+		generic_handle_irq(bcma_gpio_to_irq(&cc->gpio, gpio));
+	bcma_chipco_gpio_polarity(cc, irqs, val & irqs);
+
+	return IRQ_HANDLED;
+}
+
+static int bcma_gpio_irq_domain_init(struct bcma_drv_cc *cc)
+{
+	struct gpio_chip *chip = &cc->gpio;
+	int gpio, hwirq, err;
+
+	if (cc->core->bus->hosttype != BCMA_HOSTTYPE_SOC)
+		return 0;
+
+	cc->irq_domain = irq_domain_add_linear(NULL, chip->ngpio,
+					       &irq_domain_simple_ops, cc);
+	if (!cc->irq_domain) {
+		err = -ENODEV;
+		goto err_irq_domain;
+	}
+	for (gpio = 0; gpio < chip->ngpio; gpio++) {
+		int irq = irq_create_mapping(cc->irq_domain, gpio);
+
+		irq_set_chip_data(irq, cc);
+		irq_set_chip_and_handler(irq, &bcma_gpio_irq_chip,
+					 handle_simple_irq);
+	}
+
+	hwirq = bcma_core_irq(cc->core);
+	err = request_irq(hwirq, bcma_gpio_irq_handler, IRQF_SHARED, "gpio",
+			  cc);
+	if (err)
+		goto err_req_irq;
+
+	bcma_chipco_gpio_intmask(cc, ~0, 0);
+	bcma_cc_set32(cc, BCMA_CC_IRQMASK, BCMA_CC_IRQ_GPIO);
+
+	return 0;
+
+err_req_irq:
+	for (gpio = 0; gpio < chip->ngpio; gpio++) {
+		int irq = irq_find_mapping(cc->irq_domain, gpio);
+
+		irq_dispose_mapping(irq);
+	}
+	irq_domain_remove(cc->irq_domain);
+err_irq_domain:
+	return err;
+}
+
+static void bcma_gpio_irq_domain_exit(struct bcma_drv_cc *cc)
+{
+	struct gpio_chip *chip = &cc->gpio;
+	int gpio;
+
+	if (cc->core->bus->hosttype != BCMA_HOSTTYPE_SOC)
+		return;
+
+	bcma_cc_mask32(cc, BCMA_CC_IRQMASK, ~BCMA_CC_IRQ_GPIO);
+	free_irq(bcma_core_irq(cc->core), cc);
+	for (gpio = 0; gpio < chip->ngpio; gpio++) {
+		int irq = irq_find_mapping(cc->irq_domain, gpio);
+
+		irq_dispose_mapping(irq);
+	}
+	irq_domain_remove(cc->irq_domain);
+}
+#else
+static int bcma_gpio_irq_domain_init(struct bcma_drv_cc *cc)
+{
+	return 0;
+}
+
+static void bcma_gpio_irq_domain_exit(struct bcma_drv_cc *cc)
+{
+}
+#endif
+
 int bcma_gpio_init(struct bcma_drv_cc *cc)
 {
 	struct gpio_chip *chip = &cc->gpio;
+	int err;
 
 	chip->label		= "bcma_gpio";
 	chip->owner		= THIS_MODULE;
@@ -95,7 +215,9 @@ int bcma_gpio_init(struct bcma_drv_cc *cc)
 	chip->set		= bcma_gpio_set_value;
 	chip->direction_input	= bcma_gpio_direction_input;
 	chip->direction_output	= bcma_gpio_direction_output;
+#if IS_BUILTIN(CONFIG_BCMA_HOST_SOC)
 	chip->to_irq		= bcma_gpio_to_irq;
+#endif
 	chip->ngpio		= 16;
 	/* There is just one SoC in one device and its GPIO addresses should be
 	 * deterministic to address them more easily. The other buses could get
@@ -105,10 +227,21 @@ int bcma_gpio_init(struct bcma_drv_cc *cc)
 	else
 		chip->base		= -1;
 
-	return gpiochip_add(chip);
+	err = bcma_gpio_irq_domain_init(cc);
+	if (err)
+		return err;
+
+	err = gpiochip_add(chip);
+	if (err) {
+		bcma_gpio_irq_domain_exit(cc);
+		return err;
+	}
+
+	return 0;
 }
 
 int bcma_gpio_unregister(struct bcma_drv_cc *cc)
 {
+	bcma_gpio_irq_domain_exit(cc);
 	return gpiochip_remove(&cc->gpio);
 }
diff --git a/drivers/block/Kconfig b/drivers/block/Kconfig
index 9ffa90c6201c..014a1cfc41c5 100644
--- a/drivers/block/Kconfig
+++ b/drivers/block/Kconfig
@@ -108,6 +108,8 @@ source "drivers/block/paride/Kconfig"
 
 source "drivers/block/mtip32xx/Kconfig"
 
+source "drivers/block/zram/Kconfig"
+
 config BLK_CPQ_DA
 	tristate "Compaq SMART2 support"
 	depends on PCI && VIRT_TO_BUS && 0
diff --git a/drivers/block/Makefile b/drivers/block/Makefile
index 816d979c3266..02b688d1438d 100644
--- a/drivers/block/Makefile
+++ b/drivers/block/Makefile
@@ -42,6 +42,7 @@ obj-$(CONFIG_BLK_DEV_PCIESSD_MTIP32XX)	+= mtip32xx/
 
 obj-$(CONFIG_BLK_DEV_RSXX) += rsxx/
 obj-$(CONFIG_BLK_DEV_NULL_BLK)	+= null_blk.o
+obj-$(CONFIG_ZRAM) += zram/
 
 nvme-y		:= nvme-core.o nvme-scsi.o
 skd-y		:= skd_main.o
diff --git a/drivers/block/aoe/aoe.h b/drivers/block/aoe/aoe.h
index 14a9d1912318..9220f8e833d0 100644
--- a/drivers/block/aoe/aoe.h
+++ b/drivers/block/aoe/aoe.h
@@ -100,11 +100,8 @@ enum {
 
 struct buf {
 	ulong nframesout;
-	ulong resid;
-	ulong bv_resid;
-	sector_t sector;
 	struct bio *bio;
-	struct bio_vec *bv;
+	struct bvec_iter iter;
 	struct request *rq;
 };
 
@@ -120,13 +117,10 @@ struct frame {
 	ulong waited;
 	ulong waited_total;
 	struct aoetgt *t;		/* parent target I belong to */
-	sector_t lba;
 	struct sk_buff *skb;		/* command skb freed on module exit */
 	struct sk_buff *r_skb;		/* response skb for async processing */
 	struct buf *buf;
-	struct bio_vec *bv;
-	ulong bcnt;
-	ulong bv_off;
+	struct bvec_iter iter;
 	char flags;
 };
 
diff --git a/drivers/block/aoe/aoecmd.c b/drivers/block/aoe/aoecmd.c
index d2515435e23f..8184451b57c0 100644
--- a/drivers/block/aoe/aoecmd.c
+++ b/drivers/block/aoe/aoecmd.c
@@ -196,8 +196,7 @@ aoe_freetframe(struct frame *f)
 
 	t = f->t;
 	f->buf = NULL;
-	f->lba = 0;
-	f->bv = NULL;
+	memset(&f->iter, 0, sizeof(f->iter));
 	f->r_skb = NULL;
 	f->flags = 0;
 	list_add(&f->head, &t->ffree);
@@ -295,21 +294,14 @@ newframe(struct aoedev *d)
 }
 
 static void
-skb_fillup(struct sk_buff *skb, struct bio_vec *bv, ulong off, ulong cnt)
+skb_fillup(struct sk_buff *skb, struct bio *bio, struct bvec_iter iter)
 {
 	int frag = 0;
-	ulong fcnt;
-loop:
-	fcnt = bv->bv_len - (off - bv->bv_offset);
-	if (fcnt > cnt)
-		fcnt = cnt;
-	skb_fill_page_desc(skb, frag++, bv->bv_page, off, fcnt);
-	cnt -= fcnt;
-	if (cnt <= 0)
-		return;
-	bv++;
-	off = bv->bv_offset;
-	goto loop;
+	struct bio_vec bv;
+
+	__bio_for_each_segment(bv, bio, iter, iter)
+		skb_fill_page_desc(skb, frag++, bv.bv_page,
+				   bv.bv_offset, bv.bv_len);
 }
 
 static void
@@ -346,12 +338,10 @@ ata_rw_frameinit(struct frame *f)
 	t->nout++;
 	f->waited = 0;
 	f->waited_total = 0;
-	if (f->buf)
-		f->lba = f->buf->sector;
 
 	/* set up ata header */
-	ah->scnt = f->bcnt >> 9;
-	put_lba(ah, f->lba);
+	ah->scnt = f->iter.bi_size >> 9;
+	put_lba(ah, f->iter.bi_sector);
 	if (t->d->flags & DEVFL_EXT) {
 		ah->aflags |= AOEAFL_EXT;
 	} else {
@@ -360,11 +350,11 @@ ata_rw_frameinit(struct frame *f)
 		ah->lba3 |= 0xe0;	/* LBA bit + obsolete 0xa0 */
 	}
 	if (f->buf && bio_data_dir(f->buf->bio) == WRITE) {
-		skb_fillup(skb, f->bv, f->bv_off, f->bcnt);
+		skb_fillup(skb, f->buf->bio, f->iter);
 		ah->aflags |= AOEAFL_WRITE;
-		skb->len += f->bcnt;
-		skb->data_len = f->bcnt;
-		skb->truesize += f->bcnt;
+		skb->len += f->iter.bi_size;
+		skb->data_len = f->iter.bi_size;
+		skb->truesize += f->iter.bi_size;
 		t->wpkts++;
 	} else {
 		t->rpkts++;
@@ -382,7 +372,6 @@ aoecmd_ata_rw(struct aoedev *d)
 	struct buf *buf;
 	struct sk_buff *skb;
 	struct sk_buff_head queue;
-	ulong bcnt, fbcnt;
 
 	buf = nextbuf(d);
 	if (buf == NULL)
@@ -390,39 +379,22 @@ aoecmd_ata_rw(struct aoedev *d)
 	f = newframe(d);
 	if (f == NULL)
 		return 0;
-	bcnt = d->maxbcnt;
-	if (bcnt == 0)
-		bcnt = DEFAULTBCNT;
-	if (bcnt > buf->resid)
-		bcnt = buf->resid;
-	fbcnt = bcnt;
-	f->bv = buf->bv;
-	f->bv_off = f->bv->bv_offset + (f->bv->bv_len - buf->bv_resid);
-	do {
-		if (fbcnt < buf->bv_resid) {
-			buf->bv_resid -= fbcnt;
-			buf->resid -= fbcnt;
-			break;
-		}
-		fbcnt -= buf->bv_resid;
-		buf->resid -= buf->bv_resid;
-		if (buf->resid == 0) {
-			d->ip.buf = NULL;
-			break;
-		}
-		buf->bv++;
-		buf->bv_resid = buf->bv->bv_len;
-		WARN_ON(buf->bv_resid == 0);
-	} while (fbcnt);
 
 	/* initialize the headers & frame */
 	f->buf = buf;
-	f->bcnt = bcnt;
-	ata_rw_frameinit(f);
+	f->iter = buf->iter;
+	f->iter.bi_size = min_t(unsigned long,
+				d->maxbcnt ?: DEFAULTBCNT,
+				f->iter.bi_size);
+	bio_advance_iter(buf->bio, &buf->iter, f->iter.bi_size);
+
+	if (!buf->iter.bi_size)
+		d->ip.buf = NULL;
 
 	/* mark all tracking fields and load out */
 	buf->nframesout += 1;
-	buf->sector += bcnt >> 9;
+
+	ata_rw_frameinit(f);
 
 	skb = skb_clone(f->skb, GFP_ATOMIC);
 	if (skb) {
@@ -613,10 +585,7 @@ reassign_frame(struct frame *f)
 	skb = nf->skb;
 	nf->skb = f->skb;
 	nf->buf = f->buf;
-	nf->bcnt = f->bcnt;
-	nf->lba = f->lba;
-	nf->bv = f->bv;
-	nf->bv_off = f->bv_off;
+	nf->iter = f->iter;
 	nf->waited = 0;
 	nf->waited_total = f->waited_total;
 	nf->sent = f->sent;
@@ -648,19 +617,19 @@ probe(struct aoetgt *t)
 	}
 	f->flags |= FFL_PROBE;
 	ifrotate(t);
-	f->bcnt = t->d->maxbcnt ? t->d->maxbcnt : DEFAULTBCNT;
+	f->iter.bi_size = t->d->maxbcnt ? t->d->maxbcnt : DEFAULTBCNT;
 	ata_rw_frameinit(f);
 	skb = f->skb;
-	for (frag = 0, n = f->bcnt; n > 0; ++frag, n -= m) {
+	for (frag = 0, n = f->iter.bi_size; n > 0; ++frag, n -= m) {
 		if (n < PAGE_SIZE)
 			m = n;
 		else
 			m = PAGE_SIZE;
 		skb_fill_page_desc(skb, frag, empty_page, 0, m);
 	}
-	skb->len += f->bcnt;
-	skb->data_len = f->bcnt;
-	skb->truesize += f->bcnt;
+	skb->len += f->iter.bi_size;
+	skb->data_len = f->iter.bi_size;
+	skb->truesize += f->iter.bi_size;
 
 	skb = skb_clone(f->skb, GFP_ATOMIC);
 	if (skb) {
@@ -897,15 +866,15 @@ rqbiocnt(struct request *r)
 static void
 bio_pageinc(struct bio *bio)
 {
-	struct bio_vec *bv;
+	struct bio_vec bv;
 	struct page *page;
-	int i;
+	struct bvec_iter iter;
 
-	bio_for_each_segment(bv, bio, i) {
+	bio_for_each_segment(bv, bio, iter) {
 		/* Non-zero page count for non-head members of
 		 * compound pages is no longer allowed by the kernel.
 		 */
-		page = compound_trans_head(bv->bv_page);
+		page = compound_trans_head(bv.bv_page);
 		atomic_inc(&page->_count);
 	}
 }
@@ -913,12 +882,12 @@ bio_pageinc(struct bio *bio)
 static void
 bio_pagedec(struct bio *bio)
 {
-	struct bio_vec *bv;
 	struct page *page;
-	int i;
+	struct bio_vec bv;
+	struct bvec_iter iter;
 
-	bio_for_each_segment(bv, bio, i) {
-		page = compound_trans_head(bv->bv_page);
+	bio_for_each_segment(bv, bio, iter) {
+		page = compound_trans_head(bv.bv_page);
 		atomic_dec(&page->_count);
 	}
 }
@@ -929,12 +898,8 @@ bufinit(struct buf *buf, struct request *rq, struct bio *bio)
 	memset(buf, 0, sizeof(*buf));
 	buf->rq = rq;
 	buf->bio = bio;
-	buf->resid = bio->bi_size;
-	buf->sector = bio->bi_sector;
+	buf->iter = bio->bi_iter;
 	bio_pageinc(bio);
-	buf->bv = bio_iovec(bio);
-	buf->bv_resid = buf->bv->bv_len;
-	WARN_ON(buf->bv_resid == 0);
 }
 
 static struct buf *
@@ -1119,24 +1084,18 @@ gettgt(struct aoedev *d, char *addr)
 }
 
 static void
-bvcpy(struct bio_vec *bv, ulong off, struct sk_buff *skb, long cnt)
+bvcpy(struct sk_buff *skb, struct bio *bio, struct bvec_iter iter, long cnt)
 {
-	ulong fcnt;
-	char *p;
 	int soff = 0;
-loop:
-	fcnt = bv->bv_len - (off - bv->bv_offset);
-	if (fcnt > cnt)
-		fcnt = cnt;
-	p = page_address(bv->bv_page) + off;
-	skb_copy_bits(skb, soff, p, fcnt);
-	soff += fcnt;
-	cnt -= fcnt;
-	if (cnt <= 0)
-		return;
-	bv++;
-	off = bv->bv_offset;
-	goto loop;
+	struct bio_vec bv;
+
+	iter.bi_size = cnt;
+
+	__bio_for_each_segment(bv, bio, iter, iter) {
+		char *p = page_address(bv.bv_page) + bv.bv_offset;
+		skb_copy_bits(skb, soff, p, bv.bv_len);
+		soff += bv.bv_len;
+	}
 }
 
 void
@@ -1152,7 +1111,7 @@ aoe_end_request(struct aoedev *d, struct request *rq, int fastfail)
 	do {
 		bio = rq->bio;
 		bok = !fastfail && test_bit(BIO_UPTODATE, &bio->bi_flags);
-	} while (__blk_end_request(rq, bok ? 0 : -EIO, bio->bi_size));
+	} while (__blk_end_request(rq, bok ? 0 : -EIO, bio->bi_iter.bi_size));
 
 	/* cf. http://lkml.org/lkml/2006/10/31/28 */
 	if (!fastfail)
@@ -1229,7 +1188,15 @@ noskb:		if (buf)
 			clear_bit(BIO_UPTODATE, &buf->bio->bi_flags);
 			break;
 		}
-		bvcpy(f->bv, f->bv_off, skb, n);
+		if (n > f->iter.bi_size) {
+			pr_err_ratelimited("%s e%ld.%d.  bytes=%ld need=%u\n",
+				"aoe: too-large data size in read from",
+				(long) d->aoemajor, d->aoeminor,
+				n, f->iter.bi_size);
+			clear_bit(BIO_UPTODATE, &buf->bio->bi_flags);
+			break;
+		}
+		bvcpy(skb, f->buf->bio, f->iter, n);
 	case ATA_CMD_PIO_WRITE:
 	case ATA_CMD_PIO_WRITE_EXT:
 		spin_lock_irq(&d->lock);
@@ -1272,7 +1239,7 @@ out:
 
 	aoe_freetframe(f);
 
-	if (buf && --buf->nframesout == 0 && buf->resid == 0)
+	if (buf && --buf->nframesout == 0 && buf->iter.bi_size == 0)
 		aoe_end_buf(d, buf);
 
 	spin_unlock_irq(&d->lock);
@@ -1727,7 +1694,7 @@ aoe_failbuf(struct aoedev *d, struct buf *buf)
 {
 	if (buf == NULL)
 		return;
-	buf->resid = 0;
+	buf->iter.bi_size = 0;
 	clear_bit(BIO_UPTODATE, &buf->bio->bi_flags);
 	if (buf->nframesout == 0)
 		aoe_end_buf(d, buf);
diff --git a/drivers/block/brd.c b/drivers/block/brd.c
index d91f1a56e861..e73b85cf0756 100644
--- a/drivers/block/brd.c
+++ b/drivers/block/brd.c
@@ -328,18 +328,18 @@ static void brd_make_request(struct request_queue *q, struct bio *bio)
 	struct block_device *bdev = bio->bi_bdev;
 	struct brd_device *brd = bdev->bd_disk->private_data;
 	int rw;
-	struct bio_vec *bvec;
+	struct bio_vec bvec;
 	sector_t sector;
-	int i;
+	struct bvec_iter iter;
 	int err = -EIO;
 
-	sector = bio->bi_sector;
+	sector = bio->bi_iter.bi_sector;
 	if (bio_end_sector(bio) > get_capacity(bdev->bd_disk))
 		goto out;
 
 	if (unlikely(bio->bi_rw & REQ_DISCARD)) {
 		err = 0;
-		discard_from_brd(brd, sector, bio->bi_size);
+		discard_from_brd(brd, sector, bio->bi_iter.bi_size);
 		goto out;
 	}
 
@@ -347,10 +347,10 @@ static void brd_make_request(struct request_queue *q, struct bio *bio)
 	if (rw == READA)
 		rw = READ;
 
-	bio_for_each_segment(bvec, bio, i) {
-		unsigned int len = bvec->bv_len;
-		err = brd_do_bvec(brd, bvec->bv_page, len,
-					bvec->bv_offset, rw, sector);
+	bio_for_each_segment(bvec, bio, iter) {
+		unsigned int len = bvec.bv_len;
+		err = brd_do_bvec(brd, bvec.bv_page, len,
+					bvec.bv_offset, rw, sector);
 		if (err)
 			break;
 		sector += len >> SECTOR_SHIFT;
diff --git a/drivers/block/cciss.c b/drivers/block/cciss.c
index b35fc4f5237c..036e8ab86c71 100644
--- a/drivers/block/cciss.c
+++ b/drivers/block/cciss.c
@@ -5004,7 +5004,7 @@ reinit_after_soft_reset:
 
 	i = alloc_cciss_hba(pdev);
 	if (i < 0)
-		return -1;
+		return -ENOMEM;
 
 	h = hba[i];
 	h->pdev = pdev;
@@ -5205,7 +5205,7 @@ clean_no_release_regions:
 	 */
 	pci_set_drvdata(pdev, NULL);
 	free_hba(h);
-	return -1;
+	return -ENODEV;
 }
 
 static void cciss_shutdown(struct pci_dev *pdev)
diff --git a/drivers/block/drbd/drbd_actlog.c b/drivers/block/drbd/drbd_actlog.c
index 28c73ca320a8..a9b13f2cc420 100644
--- a/drivers/block/drbd/drbd_actlog.c
+++ b/drivers/block/drbd/drbd_actlog.c
@@ -159,7 +159,7 @@ static int _drbd_md_sync_page_io(struct drbd_conf *mdev,
 
 	bio = bio_alloc_drbd(GFP_NOIO);
 	bio->bi_bdev = bdev->md_bdev;
-	bio->bi_sector = sector;
+	bio->bi_iter.bi_sector = sector;
 	err = -EIO;
 	if (bio_add_page(bio, page, size, 0) != size)
 		goto out;
diff --git a/drivers/block/drbd/drbd_bitmap.c b/drivers/block/drbd/drbd_bitmap.c
index b12c11ec4bd2..597f111df67b 100644
--- a/drivers/block/drbd/drbd_bitmap.c
+++ b/drivers/block/drbd/drbd_bitmap.c
@@ -1028,7 +1028,7 @@ static void bm_page_io_async(struct bm_aio_ctx *ctx, int page_nr, int rw) __must
 	} else
 		page = b->bm_pages[page_nr];
 	bio->bi_bdev = mdev->ldev->md_bdev;
-	bio->bi_sector = on_disk_sector;
+	bio->bi_iter.bi_sector = on_disk_sector;
 	/* bio_add_page of a single page to an empty bio will always succeed,
 	 * according to api.  Do we want to assert that? */
 	bio_add_page(bio, page, len, 0);
diff --git a/drivers/block/drbd/drbd_main.c b/drivers/block/drbd/drbd_main.c
index 9e3818b1bc83..929468e1512a 100644
--- a/drivers/block/drbd/drbd_main.c
+++ b/drivers/block/drbd/drbd_main.c
@@ -1537,15 +1537,17 @@ static int _drbd_send_page(struct drbd_conf *mdev, struct page *page,
 
 static int _drbd_send_bio(struct drbd_conf *mdev, struct bio *bio)
 {
-	struct bio_vec *bvec;
-	int i;
+	struct bio_vec bvec;
+	struct bvec_iter iter;
+
 	/* hint all but last page with MSG_MORE */
-	bio_for_each_segment(bvec, bio, i) {
+	bio_for_each_segment(bvec, bio, iter) {
 		int err;
 
-		err = _drbd_no_send_page(mdev, bvec->bv_page,
-					 bvec->bv_offset, bvec->bv_len,
-					 i == bio->bi_vcnt - 1 ? 0 : MSG_MORE);
+		err = _drbd_no_send_page(mdev, bvec.bv_page,
+					 bvec.bv_offset, bvec.bv_len,
+					 bio_iter_last(bvec, iter)
+					 ? 0 : MSG_MORE);
 		if (err)
 			return err;
 	}
@@ -1554,15 +1556,16 @@ static int _drbd_send_bio(struct drbd_conf *mdev, struct bio *bio)
 
 static int _drbd_send_zc_bio(struct drbd_conf *mdev, struct bio *bio)
 {
-	struct bio_vec *bvec;
-	int i;
+	struct bio_vec bvec;
+	struct bvec_iter iter;
+
 	/* hint all but last page with MSG_MORE */
-	bio_for_each_segment(bvec, bio, i) {
+	bio_for_each_segment(bvec, bio, iter) {
 		int err;
 
-		err = _drbd_send_page(mdev, bvec->bv_page,
-				      bvec->bv_offset, bvec->bv_len,
-				      i == bio->bi_vcnt - 1 ? 0 : MSG_MORE);
+		err = _drbd_send_page(mdev, bvec.bv_page,
+				      bvec.bv_offset, bvec.bv_len,
+				      bio_iter_last(bvec, iter) ? 0 : MSG_MORE);
 		if (err)
 			return err;
 	}
diff --git a/drivers/block/drbd/drbd_receiver.c b/drivers/block/drbd/drbd_receiver.c
index 6fa6673b36b3..d073305ffd5e 100644
--- a/drivers/block/drbd/drbd_receiver.c
+++ b/drivers/block/drbd/drbd_receiver.c
@@ -1333,7 +1333,7 @@ next_bio:
 		goto fail;
 	}
 	/* > peer_req->i.sector, unless this is the first bio */
-	bio->bi_sector = sector;
+	bio->bi_iter.bi_sector = sector;
 	bio->bi_bdev = mdev->ldev->backing_bdev;
 	bio->bi_rw = rw;
 	bio->bi_private = peer_req;
@@ -1353,7 +1353,7 @@ next_bio:
 				dev_err(DEV,
 					"bio_add_page failed for len=%u, "
 					"bi_vcnt=0 (bi_sector=%llu)\n",
-					len, (unsigned long long)bio->bi_sector);
+					len, (uint64_t)bio->bi_iter.bi_sector);
 				err = -ENOSPC;
 				goto fail;
 			}
@@ -1595,9 +1595,10 @@ static int drbd_drain_block(struct drbd_conf *mdev, int data_size)
 static int recv_dless_read(struct drbd_conf *mdev, struct drbd_request *req,
 			   sector_t sector, int data_size)
 {
-	struct bio_vec *bvec;
+	struct bio_vec bvec;
+	struct bvec_iter iter;
 	struct bio *bio;
-	int dgs, err, i, expect;
+	int dgs, err, expect;
 	void *dig_in = mdev->tconn->int_dig_in;
 	void *dig_vv = mdev->tconn->int_dig_vv;
 
@@ -1615,13 +1616,13 @@ static int recv_dless_read(struct drbd_conf *mdev, struct drbd_request *req,
 	mdev->recv_cnt += data_size>>9;
 
 	bio = req->master_bio;
-	D_ASSERT(sector == bio->bi_sector);
+	D_ASSERT(sector == bio->bi_iter.bi_sector);
 
-	bio_for_each_segment(bvec, bio, i) {
-		void *mapped = kmap(bvec->bv_page) + bvec->bv_offset;
-		expect = min_t(int, data_size, bvec->bv_len);
+	bio_for_each_segment(bvec, bio, iter) {
+		void *mapped = kmap(bvec.bv_page) + bvec.bv_offset;
+		expect = min_t(int, data_size, bvec.bv_len);
 		err = drbd_recv_all_warn(mdev->tconn, mapped, expect);
-		kunmap(bvec->bv_page);
+		kunmap(bvec.bv_page);
 		if (err)
 			return err;
 		data_size -= expect;
diff --git a/drivers/block/drbd/drbd_req.c b/drivers/block/drbd/drbd_req.c
index fec7bef44994..104a040f24de 100644
--- a/drivers/block/drbd/drbd_req.c
+++ b/drivers/block/drbd/drbd_req.c
@@ -77,8 +77,8 @@ static struct drbd_request *drbd_req_new(struct drbd_conf *mdev,
 	req->epoch       = 0;
 
 	drbd_clear_interval(&req->i);
-	req->i.sector     = bio_src->bi_sector;
-	req->i.size      = bio_src->bi_size;
+	req->i.sector     = bio_src->bi_iter.bi_sector;
+	req->i.size      = bio_src->bi_iter.bi_size;
 	req->i.local = true;
 	req->i.waiting = false;
 
@@ -1280,7 +1280,7 @@ void drbd_make_request(struct request_queue *q, struct bio *bio)
 	/*
 	 * what we "blindly" assume:
 	 */
-	D_ASSERT(IS_ALIGNED(bio->bi_size, 512));
+	D_ASSERT(IS_ALIGNED(bio->bi_iter.bi_size, 512));
 
 	inc_ap_bio(mdev);
 	__drbd_make_request(mdev, bio, start_time);
diff --git a/drivers/block/drbd/drbd_req.h b/drivers/block/drbd/drbd_req.h
index 978cb1addc98..28e15d91197a 100644
--- a/drivers/block/drbd/drbd_req.h
+++ b/drivers/block/drbd/drbd_req.h
@@ -269,7 +269,7 @@ static inline void drbd_req_make_private_bio(struct drbd_request *req, struct bi
 
 /* Short lived temporary struct on the stack.
  * We could squirrel the error to be returned into
- * bio->bi_size, or similar. But that would be too ugly. */
+ * bio->bi_iter.bi_size, or similar. But that would be too ugly. */
 struct bio_and_error {
 	struct bio *bio;
 	int error;
diff --git a/drivers/block/drbd/drbd_worker.c b/drivers/block/drbd/drbd_worker.c
index 891c0ecaa292..84d3175d493a 100644
--- a/drivers/block/drbd/drbd_worker.c
+++ b/drivers/block/drbd/drbd_worker.c
@@ -313,8 +313,8 @@ void drbd_csum_bio(struct drbd_conf *mdev, struct crypto_hash *tfm, struct bio *
 {
 	struct hash_desc desc;
 	struct scatterlist sg;
-	struct bio_vec *bvec;
-	int i;
+	struct bio_vec bvec;
+	struct bvec_iter iter;
 
 	desc.tfm = tfm;
 	desc.flags = 0;
@@ -322,8 +322,8 @@ void drbd_csum_bio(struct drbd_conf *mdev, struct crypto_hash *tfm, struct bio *
 	sg_init_table(&sg, 1);
 	crypto_hash_init(&desc);
 
-	bio_for_each_segment(bvec, bio, i) {
-		sg_set_page(&sg, bvec->bv_page, bvec->bv_len, bvec->bv_offset);
+	bio_for_each_segment(bvec, bio, iter) {
+		sg_set_page(&sg, bvec.bv_page, bvec.bv_len, bvec.bv_offset);
 		crypto_hash_update(&desc, &sg, sg.length);
 	}
 	crypto_hash_final(&desc, digest);
diff --git a/drivers/block/floppy.c b/drivers/block/floppy.c
index 000abe2f105c..2023043ce7c0 100644
--- a/drivers/block/floppy.c
+++ b/drivers/block/floppy.c
@@ -2351,7 +2351,7 @@ static void rw_interrupt(void)
 /* Compute maximal contiguous buffer size. */
 static int buffer_chain_size(void)
 {
-	struct bio_vec *bv;
+	struct bio_vec bv;
 	int size;
 	struct req_iterator iter;
 	char *base;
@@ -2360,10 +2360,10 @@ static int buffer_chain_size(void)
 	size = 0;
 
 	rq_for_each_segment(bv, current_req, iter) {
-		if (page_address(bv->bv_page) + bv->bv_offset != base + size)
+		if (page_address(bv.bv_page) + bv.bv_offset != base + size)
 			break;
 
-		size += bv->bv_len;
+		size += bv.bv_len;
 	}
 
 	return size >> 9;
@@ -2389,7 +2389,7 @@ static int transfer_size(int ssize, int max_sector, int max_size)
 static void copy_buffer(int ssize, int max_sector, int max_sector_2)
 {
 	int remaining;		/* number of transferred 512-byte sectors */
-	struct bio_vec *bv;
+	struct bio_vec bv;
 	char *buffer;
 	char *dma_buffer;
 	int size;
@@ -2427,10 +2427,10 @@ static void copy_buffer(int ssize, int max_sector, int max_sector_2)
 		if (!remaining)
 			break;
 
-		size = bv->bv_len;
+		size = bv.bv_len;
 		SUPBOUND(size, remaining);
 
-		buffer = page_address(bv->bv_page) + bv->bv_offset;
+		buffer = page_address(bv.bv_page) + bv.bv_offset;
 		if (dma_buffer + size >
 		    floppy_track_buffer + (max_buffer_sectors << 10) ||
 		    dma_buffer < floppy_track_buffer) {
@@ -3691,9 +3691,12 @@ static int floppy_open(struct block_device *bdev, fmode_t mode)
 	if (!(mode & FMODE_NDELAY)) {
 		if (mode & (FMODE_READ|FMODE_WRITE)) {
 			UDRS->last_checked = 0;
+			clear_bit(FD_OPEN_SHOULD_FAIL_BIT, &UDRS->flags);
 			check_disk_change(bdev);
 			if (test_bit(FD_DISK_CHANGED_BIT, &UDRS->flags))
 				goto out;
+			if (test_bit(FD_OPEN_SHOULD_FAIL_BIT, &UDRS->flags))
+				goto out;
 		}
 		res = -EROFS;
 		if ((mode & FMODE_WRITE) &&
@@ -3746,17 +3749,29 @@ static unsigned int floppy_check_events(struct gendisk *disk,
  * a disk in the drive, and whether that disk is writable.
  */
 
-static void floppy_rb0_complete(struct bio *bio, int err)
+struct rb0_cbdata {
+	int drive;
+	struct completion complete;
+};
+
+static void floppy_rb0_cb(struct bio *bio, int err)
 {
-	complete((struct completion *)bio->bi_private);
+	struct rb0_cbdata *cbdata = (struct rb0_cbdata *)bio->bi_private;
+	int drive = cbdata->drive;
+
+	if (err) {
+		pr_info("floppy: error %d while reading block 0", err);
+		set_bit(FD_OPEN_SHOULD_FAIL_BIT, &UDRS->flags);
+	}
+	complete(&cbdata->complete);
 }
 
-static int __floppy_read_block_0(struct block_device *bdev)
+static int __floppy_read_block_0(struct block_device *bdev, int drive)
 {
 	struct bio bio;
 	struct bio_vec bio_vec;
-	struct completion complete;
 	struct page *page;
+	struct rb0_cbdata cbdata;
 	size_t size;
 
 	page = alloc_page(GFP_NOIO);
@@ -3769,23 +3784,26 @@ static int __floppy_read_block_0(struct block_device *bdev)
 	if (!size)
 		size = 1024;
 
+	cbdata.drive = drive;
+
 	bio_init(&bio);
 	bio.bi_io_vec = &bio_vec;
 	bio_vec.bv_page = page;
 	bio_vec.bv_len = size;
 	bio_vec.bv_offset = 0;
 	bio.bi_vcnt = 1;
-	bio.bi_size = size;
+	bio.bi_iter.bi_size = size;
 	bio.bi_bdev = bdev;
-	bio.bi_sector = 0;
+	bio.bi_iter.bi_sector = 0;
 	bio.bi_flags = (1 << BIO_QUIET);
-	init_completion(&complete);
-	bio.bi_private = &complete;
-	bio.bi_end_io = floppy_rb0_complete;
+	bio.bi_private = &cbdata;
+	bio.bi_end_io = floppy_rb0_cb;
 
 	submit_bio(READ, &bio);
 	process_fd_request();
-	wait_for_completion(&complete);
+
+	init_completion(&cbdata.complete);
+	wait_for_completion(&cbdata.complete);
 
 	__free_page(page);
 
@@ -3827,7 +3845,7 @@ static int floppy_revalidate(struct gendisk *disk)
 			UDRS->generation++;
 		if (drive_no_geom(drive)) {
 			/* auto-sensing */
-			res = __floppy_read_block_0(opened_bdev[drive]);
+			res = __floppy_read_block_0(opened_bdev[drive], drive);
 		} else {
 			if (cf)
 				poll_drive(false, FD_RAW_NEED_DISK);
diff --git a/drivers/block/loop.c b/drivers/block/loop.c
index c8dac7305244..66e8c3b94ef3 100644
--- a/drivers/block/loop.c
+++ b/drivers/block/loop.c
@@ -288,9 +288,10 @@ static int lo_send(struct loop_device *lo, struct bio *bio, loff_t pos)
 {
 	int (*do_lo_send)(struct loop_device *, struct bio_vec *, loff_t,
 			struct page *page);
-	struct bio_vec *bvec;
+	struct bio_vec bvec;
+	struct bvec_iter iter;
 	struct page *page = NULL;
-	int i, ret = 0;
+	int ret = 0;
 
 	if (lo->transfer != transfer_none) {
 		page = alloc_page(GFP_NOIO | __GFP_HIGHMEM);
@@ -302,11 +303,11 @@ static int lo_send(struct loop_device *lo, struct bio *bio, loff_t pos)
 		do_lo_send = do_lo_send_direct_write;
 	}
 
-	bio_for_each_segment(bvec, bio, i) {
-		ret = do_lo_send(lo, bvec, pos, page);
+	bio_for_each_segment(bvec, bio, iter) {
+		ret = do_lo_send(lo, &bvec, pos, page);
 		if (ret < 0)
 			break;
-		pos += bvec->bv_len;
+		pos += bvec.bv_len;
 	}
 	if (page) {
 		kunmap(page);
@@ -392,20 +393,20 @@ do_lo_receive(struct loop_device *lo,
 static int
 lo_receive(struct loop_device *lo, struct bio *bio, int bsize, loff_t pos)
 {
-	struct bio_vec *bvec;
+	struct bio_vec bvec;
+	struct bvec_iter iter;
 	ssize_t s;
-	int i;
 
-	bio_for_each_segment(bvec, bio, i) {
-		s = do_lo_receive(lo, bvec, bsize, pos);
+	bio_for_each_segment(bvec, bio, iter) {
+		s = do_lo_receive(lo, &bvec, bsize, pos);
 		if (s < 0)
 			return s;
 
-		if (s != bvec->bv_len) {
+		if (s != bvec.bv_len) {
 			zero_fill_bio(bio);
 			break;
 		}
-		pos += bvec->bv_len;
+		pos += bvec.bv_len;
 	}
 	return 0;
 }
@@ -415,7 +416,7 @@ static int do_bio_filebacked(struct loop_device *lo, struct bio *bio)
 	loff_t pos;
 	int ret;
 
-	pos = ((loff_t) bio->bi_sector << 9) + lo->lo_offset;
+	pos = ((loff_t) bio->bi_iter.bi_sector << 9) + lo->lo_offset;
 
 	if (bio_rw(bio) == WRITE) {
 		struct file *file = lo->lo_backing_file;
@@ -444,7 +445,7 @@ static int do_bio_filebacked(struct loop_device *lo, struct bio *bio)
 				goto out;
 			}
 			ret = file->f_op->fallocate(file, mode, pos,
-						    bio->bi_size);
+						    bio->bi_iter.bi_size);
 			if (unlikely(ret && ret != -EINVAL &&
 				     ret != -EOPNOTSUPP))
 				ret = -EIO;
@@ -798,7 +799,7 @@ static void loop_config_discard(struct loop_device *lo)
 
 	/*
 	 * We use punch hole to reclaim the free space used by the
-	 * image a.k.a. discard. However we do support discard if
+	 * image a.k.a. discard. However we do not support discard if
 	 * encryption is enabled, because it may give an attacker
 	 * useful information.
 	 */
diff --git a/drivers/block/mg_disk.c b/drivers/block/mg_disk.c
index 7bc363f1ee82..eb59b1241366 100644
--- a/drivers/block/mg_disk.c
+++ b/drivers/block/mg_disk.c
@@ -915,7 +915,7 @@ static int mg_probe(struct platform_device *plat_dev)
 
 	/* disk reset */
 	if (prv_data->dev_attr == MG_STORAGE_DEV) {
-		/* If POR seq. not yet finised, wait */
+		/* If POR seq. not yet finished, wait */
 		err = mg_wait_rstout(host->rstout, MG_TMAX_RSTOUT);
 		if (err)
 			goto probe_err_3b;
diff --git a/drivers/block/mtip32xx/mtip32xx.c b/drivers/block/mtip32xx/mtip32xx.c
index 050c71267f14..516026954be6 100644
--- a/drivers/block/mtip32xx/mtip32xx.c
+++ b/drivers/block/mtip32xx/mtip32xx.c
@@ -41,10 +41,31 @@
 #include "mtip32xx.h"
 
 #define HW_CMD_SLOT_SZ		(MTIP_MAX_COMMAND_SLOTS * 32)
-#define HW_CMD_TBL_SZ		(AHCI_CMD_TBL_HDR_SZ + (MTIP_MAX_SG * 16))
-#define HW_CMD_TBL_AR_SZ	(HW_CMD_TBL_SZ * MTIP_MAX_COMMAND_SLOTS)
-#define HW_PORT_PRIV_DMA_SZ \
-		(HW_CMD_SLOT_SZ + HW_CMD_TBL_AR_SZ + AHCI_RX_FIS_SZ)
+
+/* DMA region containing RX Fis, Identify, RLE10, and SMART buffers */
+#define AHCI_RX_FIS_SZ          0x100
+#define AHCI_RX_FIS_OFFSET      0x0
+#define AHCI_IDFY_SZ            ATA_SECT_SIZE
+#define AHCI_IDFY_OFFSET        0x400
+#define AHCI_SECTBUF_SZ         ATA_SECT_SIZE
+#define AHCI_SECTBUF_OFFSET     0x800
+#define AHCI_SMARTBUF_SZ        ATA_SECT_SIZE
+#define AHCI_SMARTBUF_OFFSET    0xC00
+/* 0x100 + 0x200 + 0x200 + 0x200 is smaller than 4k but we pad it out */
+#define BLOCK_DMA_ALLOC_SZ      4096
+
+/* DMA region containing command table (should be 8192 bytes) */
+#define AHCI_CMD_SLOT_SZ        sizeof(struct mtip_cmd_hdr)
+#define AHCI_CMD_TBL_SZ         (MTIP_MAX_COMMAND_SLOTS * AHCI_CMD_SLOT_SZ)
+#define AHCI_CMD_TBL_OFFSET     0x0
+
+/* DMA region per command (contains header and SGL) */
+#define AHCI_CMD_TBL_HDR_SZ     0x80
+#define AHCI_CMD_TBL_HDR_OFFSET 0x0
+#define AHCI_CMD_TBL_SGL_SZ     (MTIP_MAX_SG * sizeof(struct mtip_cmd_sg))
+#define AHCI_CMD_TBL_SGL_OFFSET AHCI_CMD_TBL_HDR_SZ
+#define CMD_DMA_ALLOC_SZ        (AHCI_CMD_TBL_SGL_SZ + AHCI_CMD_TBL_HDR_SZ)
+
 
 #define HOST_CAP_NZDMA		(1 << 19)
 #define HOST_HSORG		0xFC
@@ -899,8 +920,9 @@ static void mtip_handle_tfe(struct driver_data *dd)
 			fail_reason = "thermal shutdown";
 		}
 		if (buf[288] == 0xBF) {
+			set_bit(MTIP_DDF_SEC_LOCK_BIT, &dd->dd_flag);
 			dev_info(&dd->pdev->dev,
-				"Drive indicates rebuild has failed.\n");
+				"Drive indicates rebuild has failed. Secure erase required.\n");
 			fail_all_ncq_cmds = 1;
 			fail_reason = "rebuild failed";
 		}
@@ -1566,6 +1588,12 @@ static int mtip_get_identify(struct mtip_port *port, void __user *user_buffer)
 	}
 #endif
 
+	/* Check security locked state */
+	if (port->identify[128] & 0x4)
+		set_bit(MTIP_DDF_SEC_LOCK_BIT, &port->dd->dd_flag);
+	else
+		clear_bit(MTIP_DDF_SEC_LOCK_BIT, &port->dd->dd_flag);
+
 #ifdef MTIP_TRIM /* Disabling TRIM support temporarily */
 	/* Demux ID.DRAT & ID.RZAT to determine trim support */
 	if (port->identify[69] & (1 << 14) && port->identify[69] & (1 << 5))
@@ -1887,6 +1915,10 @@ static void mtip_dump_identify(struct mtip_port *port)
 	strlcpy(cbuf, (char *)(port->identify+27), 41);
 	dev_info(&port->dd->pdev->dev, "Model: %s\n", cbuf);
 
+	dev_info(&port->dd->pdev->dev, "Security: %04x %s\n",
+		port->identify[128],
+		port->identify[128] & 0x4 ? "(LOCKED)" : "");
+
 	if (mtip_hw_get_capacity(port->dd, &sectors))
 		dev_info(&port->dd->pdev->dev,
 			"Capacity: %llu sectors (%llu MB)\n",
@@ -3313,6 +3345,118 @@ st_out:
 }
 
 /*
+ * DMA region teardown
+ *
+ * @dd Pointer to driver_data structure
+ *
+ * return value
+ *      None
+ */
+static void mtip_dma_free(struct driver_data *dd)
+{
+	int i;
+	struct mtip_port *port = dd->port;
+
+	if (port->block1)
+		dmam_free_coherent(&dd->pdev->dev, BLOCK_DMA_ALLOC_SZ,
+					port->block1, port->block1_dma);
+
+	if (port->command_list) {
+		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);
+	}
+}
+
+/*
+ * DMA region setup
+ *
+ * @dd Pointer to driver_data structure
+ *
+ * return value
+ *      -ENOMEM Not enough free DMA region space to initialize driver
+ */
+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 =
+		dmam_alloc_coherent(&dd->pdev->dev, BLOCK_DMA_ALLOC_SZ,
+					&port->block1_dma, GFP_KERNEL);
+	if (!port->block1)
+		return -ENOMEM;
+	memset(port->block1, 0, BLOCK_DMA_ALLOC_SZ);
+
+	/* Allocate dma memory for command list */
+	port->command_list =
+		dmam_alloc_coherent(&dd->pdev->dev, AHCI_CMD_TBL_SZ,
+					&port->command_list_dma, GFP_KERNEL);
+	if (!port->command_list) {
+		dmam_free_coherent(&dd->pdev->dev, BLOCK_DMA_ALLOC_SZ,
+					port->block1, port->block1_dma);
+		port->block1 = NULL;
+		port->block1_dma = 0;
+		return -ENOMEM;
+	}
+	memset(port->command_list, 0, AHCI_CMD_TBL_SZ);
+
+	/* Setup all pointers into first DMA region */
+	port->rxfis         = port->block1 + AHCI_RX_FIS_OFFSET;
+	port->rxfis_dma     = port->block1_dma + AHCI_RX_FIS_OFFSET;
+	port->identify      = port->block1 + AHCI_IDFY_OFFSET;
+	port->identify_dma  = port->block1_dma + AHCI_IDFY_OFFSET;
+	port->log_buf       = port->block1 + AHCI_SECTBUF_OFFSET;
+	port->log_buf_dma   = port->block1_dma + AHCI_SECTBUF_OFFSET;
+	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);
+
+		if (host_cap_64)
+			port->commands[i].command_header->ctbau =
+				__force_bit2int cpu_to_le32(
+				(port->commands[i].command_dma >> 16) >> 16);
+
+		port->commands[i].command_header->ctba =
+				__force_bit2int cpu_to_le32(
+				port->commands[i].command_dma & 0xFFFFFFFF);
+
+		sg_init_table(port->commands[i].sg, MTIP_MAX_SG);
+
+		/* Mark command as currently inactive */
+		atomic_set(&dd->port->commands[i].active, 0);
+	}
+	return 0;
+}
+
+/*
  * Called once for each card.
  *
  * @dd Pointer to the driver data structure.
@@ -3370,83 +3514,10 @@ static int mtip_hw_init(struct driver_data *dd)
 	dd->port->mmio	= dd->mmio + PORT_OFFSET;
 	dd->port->dd	= dd;
 
-	/* Allocate memory for the command list. */
-	dd->port->command_list =
-		dmam_alloc_coherent(&dd->pdev->dev,
-			HW_PORT_PRIV_DMA_SZ + (ATA_SECT_SIZE * 4),
-			&dd->port->command_list_dma,
-			GFP_KERNEL);
-	if (!dd->port->command_list) {
-		dev_err(&dd->pdev->dev,
-			"Memory allocation: command list\n");
-		rv = -ENOMEM;
+	/* DMA allocations */
+	rv = mtip_dma_alloc(dd);
+	if (rv < 0)
 		goto out1;
-	}
-
-	/* Clear the memory we have allocated. */
-	memset(dd->port->command_list,
-		0,
-		HW_PORT_PRIV_DMA_SZ + (ATA_SECT_SIZE * 4));
-
-	/* Setup the addresse of the RX FIS. */
-	dd->port->rxfis	    = dd->port->command_list + HW_CMD_SLOT_SZ;
-	dd->port->rxfis_dma = dd->port->command_list_dma + HW_CMD_SLOT_SZ;
-
-	/* Setup the address of the command tables. */
-	dd->port->command_table	  = dd->port->rxfis + AHCI_RX_FIS_SZ;
-	dd->port->command_tbl_dma = dd->port->rxfis_dma + AHCI_RX_FIS_SZ;
-
-	/* Setup the address of the identify data. */
-	dd->port->identify     = dd->port->command_table +
-					HW_CMD_TBL_AR_SZ;
-	dd->port->identify_dma = dd->port->command_tbl_dma +
-					HW_CMD_TBL_AR_SZ;
-
-	/* Setup the address of the sector buffer - for some non-ncq cmds */
-	dd->port->sector_buffer	= (void *) dd->port->identify + ATA_SECT_SIZE;
-	dd->port->sector_buffer_dma = dd->port->identify_dma + ATA_SECT_SIZE;
-
-	/* Setup the address of the log buf - for read log command */
-	dd->port->log_buf = (void *)dd->port->sector_buffer  + ATA_SECT_SIZE;
-	dd->port->log_buf_dma = dd->port->sector_buffer_dma + ATA_SECT_SIZE;
-
-	/* Setup the address of the smart buf - for smart read data command */
-	dd->port->smart_buf = (void *)dd->port->log_buf  + ATA_SECT_SIZE;
-	dd->port->smart_buf_dma = dd->port->log_buf_dma + ATA_SECT_SIZE;
-
-
-	/* Point the command headers at the command tables. */
-	for (i = 0; i < num_command_slots; i++) {
-		dd->port->commands[i].command_header =
-					dd->port->command_list +
-					(sizeof(struct mtip_cmd_hdr) * i);
-		dd->port->commands[i].command_header_dma =
-					dd->port->command_list_dma +
-					(sizeof(struct mtip_cmd_hdr) * i);
-
-		dd->port->commands[i].command =
-			dd->port->command_table + (HW_CMD_TBL_SZ * i);
-		dd->port->commands[i].command_dma =
-			dd->port->command_tbl_dma + (HW_CMD_TBL_SZ * i);
-
-		if (readl(dd->mmio + HOST_CAP) & HOST_CAP_64)
-			dd->port->commands[i].command_header->ctbau =
-			__force_bit2int cpu_to_le32(
-			(dd->port->commands[i].command_dma >> 16) >> 16);
-		dd->port->commands[i].command_header->ctba =
-			__force_bit2int cpu_to_le32(
-			dd->port->commands[i].command_dma & 0xFFFFFFFF);
-
-		/*
-		 * If this is not done, a bug is reported by the stock
-		 * FC11 i386. Due to the fact that it has lots of kernel
-		 * debugging enabled.
-		 */
-		sg_init_table(dd->port->commands[i].sg, MTIP_MAX_SG);
-
-		/* Mark all commands as currently inactive.*/
-		atomic_set(&dd->port->commands[i].active, 0);
-	}
 
 	/* Setup the pointers to the extended s_active and CI registers. */
 	for (i = 0; i < dd->slot_groups; i++) {
@@ -3594,12 +3665,8 @@ out3:
 
 out2:
 	mtip_deinit_port(dd->port);
+	mtip_dma_free(dd);
 
-	/* Free the command/command header memory. */
-	dmam_free_coherent(&dd->pdev->dev,
-				HW_PORT_PRIV_DMA_SZ + (ATA_SECT_SIZE * 4),
-				dd->port->command_list,
-				dd->port->command_list_dma);
 out1:
 	/* Free the memory allocated for the for structure. */
 	kfree(dd->port);
@@ -3622,7 +3689,8 @@ static int mtip_hw_exit(struct driver_data *dd)
 	 * saves its state.
 	 */
 	if (!dd->sr) {
-		if (!test_bit(MTIP_DDF_REBUILD_FAILED_BIT, &dd->dd_flag))
+		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");
@@ -3641,11 +3709,9 @@ static int mtip_hw_exit(struct driver_data *dd)
 	irq_set_affinity_hint(dd->pdev->irq, NULL);
 	devm_free_irq(&dd->pdev->dev, dd->pdev->irq, dd);
 
-	/* Free the command/command header memory. */
-	dmam_free_coherent(&dd->pdev->dev,
-			HW_PORT_PRIV_DMA_SZ + (ATA_SECT_SIZE * 4),
-			dd->port->command_list,
-			dd->port->command_list_dma);
+	/* Free dma regions */
+	mtip_dma_free(dd);
+
 	/* Free the memory allocated for the for structure. */
 	kfree(dd->port);
 	dd->port = NULL;
@@ -3962,8 +4028,9 @@ static void mtip_make_request(struct request_queue *queue, struct bio *bio)
 {
 	struct driver_data *dd = queue->queuedata;
 	struct scatterlist *sg;
-	struct bio_vec *bvec;
-	int i, nents = 0;
+	struct bio_vec bvec;
+	struct bvec_iter iter;
+	int nents = 0;
 	int tag = 0, unaligned = 0;
 
 	if (unlikely(dd->dd_flag & MTIP_DDF_STOP_IO)) {
@@ -3993,7 +4060,7 @@ static void mtip_make_request(struct request_queue *queue, struct bio *bio)
 	}
 
 	if (unlikely(bio->bi_rw & REQ_DISCARD)) {
-		bio_endio(bio, mtip_send_trim(dd, bio->bi_sector,
+		bio_endio(bio, mtip_send_trim(dd, bio->bi_iter.bi_sector,
 						bio_sectors(bio)));
 		return;
 	}
@@ -4006,7 +4073,8 @@ static void mtip_make_request(struct request_queue *queue, struct bio *bio)
 
 	if (bio_data_dir(bio) == WRITE && bio_sectors(bio) <= 64 &&
 							dd->unal_qdepth) {
-		if (bio->bi_sector % 8 != 0) /* Unaligned on 4k boundaries */
+		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;
@@ -4025,17 +4093,17 @@ static void mtip_make_request(struct request_queue *queue, struct bio *bio)
 		}
 
 		/* Create the scatter list for this bio. */
-		bio_for_each_segment(bvec, bio, i) {
+		bio_for_each_segment(bvec, bio, iter) {
 			sg_set_page(&sg[nents],
-					bvec->bv_page,
-					bvec->bv_len,
-					bvec->bv_offset);
+					bvec.bv_page,
+					bvec.bv_len,
+					bvec.bv_offset);
 			nents++;
 		}
 
 		/* Issue the read/write. */
 		mtip_hw_submit_io(dd,
-				bio->bi_sector,
+				bio->bi_iter.bi_sector,
 				bio_sectors(bio),
 				nents,
 				tag,
diff --git a/drivers/block/mtip32xx/mtip32xx.h b/drivers/block/mtip32xx/mtip32xx.h
index 9be7a1582ad3..b52e9a6d6aad 100644
--- a/drivers/block/mtip32xx/mtip32xx.h
+++ b/drivers/block/mtip32xx/mtip32xx.h
@@ -69,7 +69,7 @@
  * Maximum number of scatter gather entries
  * a single command may have.
  */
-#define MTIP_MAX_SG		128
+#define MTIP_MAX_SG		504
 
 /*
  * Maximum number of slot groups (Command Issue & s_active registers)
@@ -92,7 +92,7 @@
 
 /* Driver name and version strings */
 #define MTIP_DRV_NAME		"mtip32xx"
-#define MTIP_DRV_VERSION	"1.2.6os3"
+#define MTIP_DRV_VERSION	"1.3.0"
 
 /* Maximum number of minor device numbers per device. */
 #define MTIP_MAX_MINORS		16
@@ -391,15 +391,13 @@ struct mtip_port {
 	 */
 	dma_addr_t rxfis_dma;
 	/*
-	 * Pointer to the beginning of the command table memory as used
-	 * by the driver.
+	 * Pointer to the DMA region for RX Fis, Identify, RLE10, and SMART
 	 */
-	void *command_table;
+	void *block1;
 	/*
-	 * Pointer to the beginning of the command table memory as used
-	 * by the DMA.
+	 * DMA address of region for RX Fis, Identify, RLE10, and SMART
 	 */
-	dma_addr_t command_tbl_dma;
+	dma_addr_t block1_dma;
 	/*
 	 * Pointer to the beginning of the identify data memory as used
 	 * by the driver.
diff --git a/drivers/block/nbd.c b/drivers/block/nbd.c
index 2dc3b5153f0d..55298db36b2d 100644
--- a/drivers/block/nbd.c
+++ b/drivers/block/nbd.c
@@ -271,18 +271,18 @@ static int nbd_send_req(struct nbd_device *nbd, struct request *req)
 
 	if (nbd_cmd(req) == NBD_CMD_WRITE) {
 		struct req_iterator iter;
-		struct bio_vec *bvec;
+		struct bio_vec bvec;
 		/*
 		 * we are really probing at internals to determine
 		 * whether to set MSG_MORE or not...
 		 */
 		rq_for_each_segment(bvec, req, iter) {
 			flags = 0;
-			if (!rq_iter_last(req, iter))
+			if (!rq_iter_last(bvec, iter))
 				flags = MSG_MORE;
 			dprintk(DBG_TX, "%s: request %p: sending %d bytes data\n",
-					nbd->disk->disk_name, req, bvec->bv_len);
-			result = sock_send_bvec(nbd, bvec, flags);
+					nbd->disk->disk_name, req, bvec.bv_len);
+			result = sock_send_bvec(nbd, &bvec, flags);
 			if (result <= 0) {
 				dev_err(disk_to_dev(nbd->disk),
 					"Send data failed (result %d)\n",
@@ -378,10 +378,10 @@ static struct request *nbd_read_stat(struct nbd_device *nbd)
 			nbd->disk->disk_name, req);
 	if (nbd_cmd(req) == NBD_CMD_READ) {
 		struct req_iterator iter;
-		struct bio_vec *bvec;
+		struct bio_vec bvec;
 
 		rq_for_each_segment(bvec, req, iter) {
-			result = sock_recv_bvec(nbd, bvec);
+			result = sock_recv_bvec(nbd, &bvec);
 			if (result <= 0) {
 				dev_err(disk_to_dev(nbd->disk), "Receive data failed (result %d)\n",
 					result);
@@ -389,7 +389,7 @@ static struct request *nbd_read_stat(struct nbd_device *nbd)
 				return req;
 			}
 			dprintk(DBG_RX, "%s: request %p: got %d bytes data\n",
-				nbd->disk->disk_name, req, bvec->bv_len);
+				nbd->disk->disk_name, req, bvec.bv_len);
 		}
 	}
 	return req;
diff --git a/drivers/block/null_blk.c b/drivers/block/null_blk.c
index 83a598ebb65a..3107282a9741 100644
--- a/drivers/block/null_blk.c
+++ b/drivers/block/null_blk.c
@@ -616,6 +616,11 @@ static int __init null_init(void)
 		irqmode = NULL_IRQ_NONE;
 	}
 #endif
+	if (bs > PAGE_SIZE) {
+		pr_warn("null_blk: invalid block size\n");
+		pr_warn("null_blk: defaults block size to %lu\n", PAGE_SIZE);
+		bs = PAGE_SIZE;
+	}
 
 	if (queue_mode == NULL_Q_MQ && use_per_node_hctx) {
 		if (submit_queues < nr_online_nodes) {
diff --git a/drivers/block/nvme-core.c b/drivers/block/nvme-core.c
index 26d03fa0bf26..51824d1f23ea 100644
--- a/drivers/block/nvme-core.c
+++ b/drivers/block/nvme-core.c
@@ -46,7 +46,6 @@
 #define NVME_Q_DEPTH 1024
 #define SQ_SIZE(depth)		(depth * sizeof(struct nvme_command))
 #define CQ_SIZE(depth)		(depth * sizeof(struct nvme_completion))
-#define NVME_MINORS 64
 #define ADMIN_TIMEOUT	(60 * HZ)
 
 static int nvme_major;
@@ -58,6 +57,17 @@ module_param(use_threaded_interrupts, int, 0);
 static DEFINE_SPINLOCK(dev_list_lock);
 static LIST_HEAD(dev_list);
 static struct task_struct *nvme_thread;
+static struct workqueue_struct *nvme_workq;
+
+static void nvme_reset_failed_dev(struct work_struct *ws);
+
+struct async_cmd_info {
+	struct kthread_work work;
+	struct kthread_worker *worker;
+	u32 result;
+	int status;
+	void *ctx;
+};
 
 /*
  * An NVM Express queue.  Each device has at least two (one for admin
@@ -66,6 +76,7 @@ static struct task_struct *nvme_thread;
 struct nvme_queue {
 	struct device *q_dmadev;
 	struct nvme_dev *dev;
+	char irqname[24];	/* nvme4294967295-65535\0 */
 	spinlock_t q_lock;
 	struct nvme_command *sq_cmds;
 	volatile struct nvme_completion *cqes;
@@ -80,9 +91,11 @@ struct nvme_queue {
 	u16 sq_head;
 	u16 sq_tail;
 	u16 cq_head;
+	u16 qid;
 	u8 cq_phase;
 	u8 cqe_seen;
 	u8 q_suspended;
+	struct async_cmd_info cmdinfo;
 	unsigned long cmdid_data[];
 };
 
@@ -97,6 +110,7 @@ static inline void _nvme_check_size(void)
 	BUILD_BUG_ON(sizeof(struct nvme_delete_queue) != 64);
 	BUILD_BUG_ON(sizeof(struct nvme_features) != 64);
 	BUILD_BUG_ON(sizeof(struct nvme_format_cmd) != 64);
+	BUILD_BUG_ON(sizeof(struct nvme_abort_cmd) != 64);
 	BUILD_BUG_ON(sizeof(struct nvme_command) != 64);
 	BUILD_BUG_ON(sizeof(struct nvme_id_ctrl) != 4096);
 	BUILD_BUG_ON(sizeof(struct nvme_id_ns) != 4096);
@@ -111,6 +125,7 @@ struct nvme_cmd_info {
 	nvme_completion_fn fn;
 	void *ctx;
 	unsigned long timeout;
+	int aborted;
 };
 
 static struct nvme_cmd_info *nvme_cmd_info(struct nvme_queue *nvmeq)
@@ -154,6 +169,7 @@ static int alloc_cmdid(struct nvme_queue *nvmeq, void *ctx,
 	info[cmdid].fn = handler;
 	info[cmdid].ctx = ctx;
 	info[cmdid].timeout = jiffies + timeout;
+	info[cmdid].aborted = 0;
 	return cmdid;
 }
 
@@ -172,6 +188,7 @@ static int alloc_cmdid_killable(struct nvme_queue *nvmeq, void *ctx,
 #define CMD_CTX_COMPLETED	(0x310 + CMD_CTX_BASE)
 #define CMD_CTX_INVALID		(0x314 + CMD_CTX_BASE)
 #define CMD_CTX_FLUSH		(0x318 + CMD_CTX_BASE)
+#define CMD_CTX_ABORT		(0x31C + CMD_CTX_BASE)
 
 static void special_completion(struct nvme_dev *dev, void *ctx,
 						struct nvme_completion *cqe)
@@ -180,6 +197,10 @@ static void special_completion(struct nvme_dev *dev, void *ctx,
 		return;
 	if (ctx == CMD_CTX_FLUSH)
 		return;
+	if (ctx == CMD_CTX_ABORT) {
+		++dev->abort_limit;
+		return;
+	}
 	if (ctx == CMD_CTX_COMPLETED) {
 		dev_warn(&dev->pci_dev->dev,
 				"completed id %d twice on queue %d\n",
@@ -196,6 +217,15 @@ static void special_completion(struct nvme_dev *dev, void *ctx,
 	dev_warn(&dev->pci_dev->dev, "Unknown special completion %p\n", ctx);
 }
 
+static void async_completion(struct nvme_dev *dev, void *ctx,
+						struct nvme_completion *cqe)
+{
+	struct async_cmd_info *cmdinfo = ctx;
+	cmdinfo->result = le32_to_cpup(&cqe->result);
+	cmdinfo->status = le16_to_cpup(&cqe->status) >> 1;
+	queue_kthread_work(cmdinfo->worker, &cmdinfo->work);
+}
+
 /*
  * Called with local interrupts disabled and the q_lock held.  May not sleep.
  */
@@ -441,104 +471,19 @@ int nvme_setup_prps(struct nvme_dev *dev, struct nvme_common_command *cmd,
 	return total_len;
 }
 
-struct nvme_bio_pair {
-	struct bio b1, b2, *parent;
-	struct bio_vec *bv1, *bv2;
-	int err;
-	atomic_t cnt;
-};
-
-static void nvme_bio_pair_endio(struct bio *bio, int err)
-{
-	struct nvme_bio_pair *bp = bio->bi_private;
-
-	if (err)
-		bp->err = err;
-
-	if (atomic_dec_and_test(&bp->cnt)) {
-		bio_endio(bp->parent, bp->err);
-		kfree(bp->bv1);
-		kfree(bp->bv2);
-		kfree(bp);
-	}
-}
-
-static struct nvme_bio_pair *nvme_bio_split(struct bio *bio, int idx,
-							int len, int offset)
-{
-	struct nvme_bio_pair *bp;
-
-	BUG_ON(len > bio->bi_size);
-	BUG_ON(idx > bio->bi_vcnt);
-
-	bp = kmalloc(sizeof(*bp), GFP_ATOMIC);
-	if (!bp)
-		return NULL;
-	bp->err = 0;
-
-	bp->b1 = *bio;
-	bp->b2 = *bio;
-
-	bp->b1.bi_size = len;
-	bp->b2.bi_size -= len;
-	bp->b1.bi_vcnt = idx;
-	bp->b2.bi_idx = idx;
-	bp->b2.bi_sector += len >> 9;
-
-	if (offset) {
-		bp->bv1 = kmalloc(bio->bi_max_vecs * sizeof(struct bio_vec),
-								GFP_ATOMIC);
-		if (!bp->bv1)
-			goto split_fail_1;
-
-		bp->bv2 = kmalloc(bio->bi_max_vecs * sizeof(struct bio_vec),
-								GFP_ATOMIC);
-		if (!bp->bv2)
-			goto split_fail_2;
-
-		memcpy(bp->bv1, bio->bi_io_vec,
-			bio->bi_max_vecs * sizeof(struct bio_vec));
-		memcpy(bp->bv2, bio->bi_io_vec,
-			bio->bi_max_vecs * sizeof(struct bio_vec));
-
-		bp->b1.bi_io_vec = bp->bv1;
-		bp->b2.bi_io_vec = bp->bv2;
-		bp->b2.bi_io_vec[idx].bv_offset += offset;
-		bp->b2.bi_io_vec[idx].bv_len -= offset;
-		bp->b1.bi_io_vec[idx].bv_len = offset;
-		bp->b1.bi_vcnt++;
-	} else
-		bp->bv1 = bp->bv2 = NULL;
-
-	bp->b1.bi_private = bp;
-	bp->b2.bi_private = bp;
-
-	bp->b1.bi_end_io = nvme_bio_pair_endio;
-	bp->b2.bi_end_io = nvme_bio_pair_endio;
-
-	bp->parent = bio;
-	atomic_set(&bp->cnt, 2);
-
-	return bp;
-
- split_fail_2:
-	kfree(bp->bv1);
- split_fail_1:
-	kfree(bp);
-	return NULL;
-}
-
 static int nvme_split_and_submit(struct bio *bio, struct nvme_queue *nvmeq,
-						int idx, int len, int offset)
+				 int len)
 {
-	struct nvme_bio_pair *bp = nvme_bio_split(bio, idx, len, offset);
-	if (!bp)
+	struct bio *split = bio_split(bio, len >> 9, GFP_ATOMIC, NULL);
+	if (!split)
 		return -ENOMEM;
 
+	bio_chain(split, bio);
+
 	if (bio_list_empty(&nvmeq->sq_cong))
 		add_wait_queue(&nvmeq->sq_full, &nvmeq->sq_cong_wait);
-	bio_list_add(&nvmeq->sq_cong, &bp->b1);
-	bio_list_add(&nvmeq->sq_cong, &bp->b2);
+	bio_list_add(&nvmeq->sq_cong, split);
+	bio_list_add(&nvmeq->sq_cong, bio);
 
 	return 0;
 }
@@ -550,41 +495,44 @@ static int nvme_split_and_submit(struct bio *bio, struct nvme_queue *nvmeq,
 static int nvme_map_bio(struct nvme_queue *nvmeq, struct nvme_iod *iod,
 		struct bio *bio, enum dma_data_direction dma_dir, int psegs)
 {
-	struct bio_vec *bvec, *bvprv = NULL;
+	struct bio_vec bvec, bvprv;
+	struct bvec_iter iter;
 	struct scatterlist *sg = NULL;
-	int i, length = 0, nsegs = 0, split_len = bio->bi_size;
+	int length = 0, nsegs = 0, split_len = bio->bi_iter.bi_size;
+	int first = 1;
 
 	if (nvmeq->dev->stripe_size)
 		split_len = nvmeq->dev->stripe_size -
-			((bio->bi_sector << 9) & (nvmeq->dev->stripe_size - 1));
+			((bio->bi_iter.bi_sector << 9) &
+			 (nvmeq->dev->stripe_size - 1));
 
 	sg_init_table(iod->sg, psegs);
-	bio_for_each_segment(bvec, bio, i) {
-		if (bvprv && BIOVEC_PHYS_MERGEABLE(bvprv, bvec)) {
-			sg->length += bvec->bv_len;
+	bio_for_each_segment(bvec, bio, iter) {
+		if (!first && BIOVEC_PHYS_MERGEABLE(&bvprv, &bvec)) {
+			sg->length += bvec.bv_len;
 		} else {
-			if (bvprv && BIOVEC_NOT_VIRT_MERGEABLE(bvprv, bvec))
-				return nvme_split_and_submit(bio, nvmeq, i,
-								length, 0);
+			if (!first && BIOVEC_NOT_VIRT_MERGEABLE(&bvprv, &bvec))
+				return nvme_split_and_submit(bio, nvmeq,
+							     length);
 
 			sg = sg ? sg + 1 : iod->sg;
-			sg_set_page(sg, bvec->bv_page, bvec->bv_len,
-							bvec->bv_offset);
+			sg_set_page(sg, bvec.bv_page,
+				    bvec.bv_len, bvec.bv_offset);
 			nsegs++;
 		}
 
-		if (split_len - length < bvec->bv_len)
-			return nvme_split_and_submit(bio, nvmeq, i, split_len,
-							split_len - length);
-		length += bvec->bv_len;
+		if (split_len - length < bvec.bv_len)
+			return nvme_split_and_submit(bio, nvmeq, split_len);
+		length += bvec.bv_len;
 		bvprv = bvec;
+		first = 0;
 	}
 	iod->nents = nsegs;
 	sg_mark_end(sg);
 	if (dma_map_sg(nvmeq->q_dmadev, iod->sg, iod->nents, dma_dir) == 0)
 		return -ENOMEM;
 
-	BUG_ON(length != bio->bi_size);
+	BUG_ON(length != bio->bi_iter.bi_size);
 	return length;
 }
 
@@ -608,8 +556,8 @@ static int nvme_submit_discard(struct nvme_queue *nvmeq, struct nvme_ns *ns,
 	iod->npages = 0;
 
 	range->cattr = cpu_to_le32(0);
-	range->nlb = cpu_to_le32(bio->bi_size >> ns->lba_shift);
-	range->slba = cpu_to_le64(nvme_block_nr(ns, bio->bi_sector));
+	range->nlb = cpu_to_le32(bio->bi_iter.bi_size >> ns->lba_shift);
+	range->slba = cpu_to_le64(nvme_block_nr(ns, bio->bi_iter.bi_sector));
 
 	memset(cmnd, 0, sizeof(*cmnd));
 	cmnd->dsm.opcode = nvme_cmd_dsm;
@@ -674,7 +622,7 @@ static int nvme_submit_bio_queue(struct nvme_queue *nvmeq, struct nvme_ns *ns,
 	}
 
 	result = -ENOMEM;
-	iod = nvme_alloc_iod(psegs, bio->bi_size, GFP_ATOMIC);
+	iod = nvme_alloc_iod(psegs, bio->bi_iter.bi_size, GFP_ATOMIC);
 	if (!iod)
 		goto nomem;
 	iod->private = bio;
@@ -723,7 +671,7 @@ static int nvme_submit_bio_queue(struct nvme_queue *nvmeq, struct nvme_ns *ns,
 	cmnd->rw.nsid = cpu_to_le32(ns->ns_id);
 	length = nvme_setup_prps(nvmeq->dev, &cmnd->common, iod, length,
 								GFP_ATOMIC);
-	cmnd->rw.slba = cpu_to_le64(nvme_block_nr(ns, bio->bi_sector));
+	cmnd->rw.slba = cpu_to_le64(nvme_block_nr(ns, bio->bi_iter.bi_sector));
 	cmnd->rw.length = cpu_to_le16((length >> ns->lba_shift) - 1);
 	cmnd->rw.control = cpu_to_le16(control);
 	cmnd->rw.dsmgmt = cpu_to_le32(dsmgmt);
@@ -775,7 +723,7 @@ static int nvme_process_cq(struct nvme_queue *nvmeq)
 	if (head == nvmeq->cq_head && phase == nvmeq->cq_phase)
 		return 0;
 
-	writel(head, nvmeq->q_db + (1 << nvmeq->dev->db_stride));
+	writel(head, nvmeq->q_db + nvmeq->dev->db_stride);
 	nvmeq->cq_head = head;
 	nvmeq->cq_phase = phase;
 
@@ -886,12 +834,34 @@ int nvme_submit_sync_cmd(struct nvme_queue *nvmeq, struct nvme_command *cmd,
 	return cmdinfo.status;
 }
 
+static int nvme_submit_async_cmd(struct nvme_queue *nvmeq,
+			struct nvme_command *cmd,
+			struct async_cmd_info *cmdinfo, unsigned timeout)
+{
+	int cmdid;
+
+	cmdid = alloc_cmdid_killable(nvmeq, cmdinfo, async_completion, timeout);
+	if (cmdid < 0)
+		return cmdid;
+	cmdinfo->status = -EINTR;
+	cmd->common.command_id = cmdid;
+	nvme_submit_cmd(nvmeq, cmd);
+	return 0;
+}
+
 int nvme_submit_admin_cmd(struct nvme_dev *dev, struct nvme_command *cmd,
 								u32 *result)
 {
 	return nvme_submit_sync_cmd(dev->queues[0], cmd, result, ADMIN_TIMEOUT);
 }
 
+static int nvme_submit_admin_cmd_async(struct nvme_dev *dev,
+		struct nvme_command *cmd, struct async_cmd_info *cmdinfo)
+{
+	return nvme_submit_async_cmd(dev->queues[0], cmd, cmdinfo,
+								ADMIN_TIMEOUT);
+}
+
 static int adapter_delete_queue(struct nvme_dev *dev, u8 opcode, u16 id)
 {
 	int status;
@@ -1002,6 +972,56 @@ int nvme_set_features(struct nvme_dev *dev, unsigned fid, unsigned dword11,
 }
 
 /**
+ * nvme_abort_cmd - Attempt aborting a command
+ * @cmdid: Command id of a timed out IO
+ * @queue: The queue with timed out IO
+ *
+ * Schedule controller reset if the command was already aborted once before and
+ * still hasn't been returned to the driver, or if this is the admin queue.
+ */
+static void nvme_abort_cmd(int cmdid, struct nvme_queue *nvmeq)
+{
+	int a_cmdid;
+	struct nvme_command cmd;
+	struct nvme_dev *dev = nvmeq->dev;
+	struct nvme_cmd_info *info = nvme_cmd_info(nvmeq);
+
+	if (!nvmeq->qid || info[cmdid].aborted) {
+		if (work_busy(&dev->reset_work))
+			return;
+		list_del_init(&dev->node);
+		dev_warn(&dev->pci_dev->dev,
+			"I/O %d QID %d timeout, reset controller\n", cmdid,
+								nvmeq->qid);
+		PREPARE_WORK(&dev->reset_work, nvme_reset_failed_dev);
+		queue_work(nvme_workq, &dev->reset_work);
+		return;
+	}
+
+	if (!dev->abort_limit)
+		return;
+
+	a_cmdid = alloc_cmdid(dev->queues[0], CMD_CTX_ABORT, special_completion,
+								ADMIN_TIMEOUT);
+	if (a_cmdid < 0)
+		return;
+
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.abort.opcode = nvme_admin_abort_cmd;
+	cmd.abort.cid = cmdid;
+	cmd.abort.sqid = cpu_to_le16(nvmeq->qid);
+	cmd.abort.command_id = a_cmdid;
+
+	--dev->abort_limit;
+	info[cmdid].aborted = 1;
+	info[cmdid].timeout = jiffies + ADMIN_TIMEOUT;
+
+	dev_warn(nvmeq->q_dmadev, "Aborting I/O %d QID %d\n", cmdid,
+							nvmeq->qid);
+	nvme_submit_cmd(dev->queues[0], &cmd);
+}
+
+/**
  * nvme_cancel_ios - Cancel outstanding I/Os
  * @queue: The queue to cancel I/Os on
  * @timeout: True to only cancel I/Os which have timed out
@@ -1024,7 +1044,12 @@ static void nvme_cancel_ios(struct nvme_queue *nvmeq, bool timeout)
 			continue;
 		if (info[cmdid].ctx == CMD_CTX_CANCELLED)
 			continue;
-		dev_warn(nvmeq->q_dmadev, "Cancelling I/O %d\n", cmdid);
+		if (timeout && nvmeq->dev->initialized) {
+			nvme_abort_cmd(cmdid, nvmeq);
+			continue;
+		}
+		dev_warn(nvmeq->q_dmadev, "Cancelling I/O %d QID %d\n", cmdid,
+								nvmeq->qid);
 		ctx = cancel_cmdid(nvmeq, cmdid, &fn);
 		fn(nvmeq->dev, ctx, &cqe);
 	}
@@ -1046,26 +1071,31 @@ static void nvme_free_queue(struct nvme_queue *nvmeq)
 	kfree(nvmeq);
 }
 
-static void nvme_free_queues(struct nvme_dev *dev)
+static void nvme_free_queues(struct nvme_dev *dev, int lowest)
 {
 	int i;
 
-	for (i = dev->queue_count - 1; i >= 0; i--) {
+	for (i = dev->queue_count - 1; i >= lowest; i--) {
 		nvme_free_queue(dev->queues[i]);
 		dev->queue_count--;
 		dev->queues[i] = NULL;
 	}
 }
 
-static void nvme_disable_queue(struct nvme_dev *dev, int qid)
+/**
+ * nvme_suspend_queue - put queue into suspended state
+ * @nvmeq - queue to suspend
+ *
+ * Returns 1 if already suspended, 0 otherwise.
+ */
+static int nvme_suspend_queue(struct nvme_queue *nvmeq)
 {
-	struct nvme_queue *nvmeq = dev->queues[qid];
-	int vector = dev->entry[nvmeq->cq_vector].vector;
+	int vector = nvmeq->dev->entry[nvmeq->cq_vector].vector;
 
 	spin_lock_irq(&nvmeq->q_lock);
 	if (nvmeq->q_suspended) {
 		spin_unlock_irq(&nvmeq->q_lock);
-		return;
+		return 1;
 	}
 	nvmeq->q_suspended = 1;
 	spin_unlock_irq(&nvmeq->q_lock);
@@ -1073,18 +1103,35 @@ static void nvme_disable_queue(struct nvme_dev *dev, int qid)
 	irq_set_affinity_hint(vector, NULL);
 	free_irq(vector, nvmeq);
 
-	/* Don't tell the adapter to delete the admin queue */
-	if (qid) {
-		adapter_delete_sq(dev, qid);
-		adapter_delete_cq(dev, qid);
-	}
+	return 0;
+}
 
+static void nvme_clear_queue(struct nvme_queue *nvmeq)
+{
 	spin_lock_irq(&nvmeq->q_lock);
 	nvme_process_cq(nvmeq);
 	nvme_cancel_ios(nvmeq, false);
 	spin_unlock_irq(&nvmeq->q_lock);
 }
 
+static void nvme_disable_queue(struct nvme_dev *dev, int qid)
+{
+	struct nvme_queue *nvmeq = dev->queues[qid];
+
+	if (!nvmeq)
+		return;
+	if (nvme_suspend_queue(nvmeq))
+		return;
+
+	/* Don't tell the adapter to delete the admin queue.
+	 * Don't tell a removed adapter to delete IO queues. */
+	if (qid && readl(&dev->bar->csts) != -1) {
+		adapter_delete_sq(dev, qid);
+		adapter_delete_cq(dev, qid);
+	}
+	nvme_clear_queue(nvmeq);
+}
+
 static struct nvme_queue *nvme_alloc_queue(struct nvme_dev *dev, int qid,
 							int depth, int vector)
 {
@@ -1107,15 +1154,18 @@ static struct nvme_queue *nvme_alloc_queue(struct nvme_dev *dev, int qid,
 
 	nvmeq->q_dmadev = dmadev;
 	nvmeq->dev = dev;
+	snprintf(nvmeq->irqname, sizeof(nvmeq->irqname), "nvme%dq%d",
+			dev->instance, qid);
 	spin_lock_init(&nvmeq->q_lock);
 	nvmeq->cq_head = 0;
 	nvmeq->cq_phase = 1;
 	init_waitqueue_head(&nvmeq->sq_full);
 	init_waitqueue_entry(&nvmeq->sq_cong_wait, nvme_thread);
 	bio_list_init(&nvmeq->sq_cong);
-	nvmeq->q_db = &dev->dbs[qid << (dev->db_stride + 1)];
+	nvmeq->q_db = &dev->dbs[qid * 2 * dev->db_stride];
 	nvmeq->q_depth = depth;
 	nvmeq->cq_vector = vector;
+	nvmeq->qid = qid;
 	nvmeq->q_suspended = 1;
 	dev->queue_count++;
 
@@ -1134,11 +1184,10 @@ static int queue_request_irq(struct nvme_dev *dev, struct nvme_queue *nvmeq,
 {
 	if (use_threaded_interrupts)
 		return request_threaded_irq(dev->entry[nvmeq->cq_vector].vector,
-					nvme_irq_check, nvme_irq,
-					IRQF_DISABLED | IRQF_SHARED,
+					nvme_irq_check, nvme_irq, IRQF_SHARED,
 					name, nvmeq);
 	return request_irq(dev->entry[nvmeq->cq_vector].vector, nvme_irq,
-				IRQF_DISABLED | IRQF_SHARED, name, nvmeq);
+				IRQF_SHARED, name, nvmeq);
 }
 
 static void nvme_init_queue(struct nvme_queue *nvmeq, u16 qid)
@@ -1149,7 +1198,7 @@ static void nvme_init_queue(struct nvme_queue *nvmeq, u16 qid)
 	nvmeq->sq_tail = 0;
 	nvmeq->cq_head = 0;
 	nvmeq->cq_phase = 1;
-	nvmeq->q_db = &dev->dbs[qid << (dev->db_stride + 1)];
+	nvmeq->q_db = &dev->dbs[qid * 2 * dev->db_stride];
 	memset(nvmeq->cmdid_data, 0, extra);
 	memset((void *)nvmeq->cqes, 0, CQ_SIZE(nvmeq->q_depth));
 	nvme_cancel_ios(nvmeq, false);
@@ -1169,13 +1218,13 @@ static int nvme_create_queue(struct nvme_queue *nvmeq, int qid)
 	if (result < 0)
 		goto release_cq;
 
-	result = queue_request_irq(dev, nvmeq, "nvme");
+	result = queue_request_irq(dev, nvmeq, nvmeq->irqname);
 	if (result < 0)
 		goto release_sq;
 
-	spin_lock(&nvmeq->q_lock);
+	spin_lock_irq(&nvmeq->q_lock);
 	nvme_init_queue(nvmeq, qid);
-	spin_unlock(&nvmeq->q_lock);
+	spin_unlock_irq(&nvmeq->q_lock);
 
 	return result;
 
@@ -1287,13 +1336,13 @@ static int nvme_configure_admin_queue(struct nvme_dev *dev)
 	if (result)
 		return result;
 
-	result = queue_request_irq(dev, nvmeq, "nvme admin");
+	result = queue_request_irq(dev, nvmeq, nvmeq->irqname);
 	if (result)
 		return result;
 
-	spin_lock(&nvmeq->q_lock);
+	spin_lock_irq(&nvmeq->q_lock);
 	nvme_init_queue(nvmeq, 0);
-	spin_unlock(&nvmeq->q_lock);
+	spin_unlock_irq(&nvmeq->q_lock);
 	return result;
 }
 
@@ -1569,10 +1618,47 @@ static int nvme_ioctl(struct block_device *bdev, fmode_t mode, unsigned int cmd,
 	}
 }
 
+#ifdef CONFIG_COMPAT
+static int nvme_compat_ioctl(struct block_device *bdev, fmode_t mode,
+					unsigned int cmd, unsigned long arg)
+{
+	struct nvme_ns *ns = bdev->bd_disk->private_data;
+
+	switch (cmd) {
+	case SG_IO:
+		return nvme_sg_io32(ns, arg);
+	}
+	return nvme_ioctl(bdev, mode, cmd, arg);
+}
+#else
+#define nvme_compat_ioctl	NULL
+#endif
+
+static int nvme_open(struct block_device *bdev, fmode_t mode)
+{
+	struct nvme_ns *ns = bdev->bd_disk->private_data;
+	struct nvme_dev *dev = ns->dev;
+
+	kref_get(&dev->kref);
+	return 0;
+}
+
+static void nvme_free_dev(struct kref *kref);
+
+static void nvme_release(struct gendisk *disk, fmode_t mode)
+{
+	struct nvme_ns *ns = disk->private_data;
+	struct nvme_dev *dev = ns->dev;
+
+	kref_put(&dev->kref, nvme_free_dev);
+}
+
 static const struct block_device_operations nvme_fops = {
 	.owner		= THIS_MODULE,
 	.ioctl		= nvme_ioctl,
-	.compat_ioctl	= nvme_ioctl,
+	.compat_ioctl	= nvme_compat_ioctl,
+	.open		= nvme_open,
+	.release	= nvme_release,
 };
 
 static void nvme_resubmit_bios(struct nvme_queue *nvmeq)
@@ -1596,13 +1682,25 @@ static void nvme_resubmit_bios(struct nvme_queue *nvmeq)
 
 static int nvme_kthread(void *data)
 {
-	struct nvme_dev *dev;
+	struct nvme_dev *dev, *next;
 
 	while (!kthread_should_stop()) {
 		set_current_state(TASK_INTERRUPTIBLE);
 		spin_lock(&dev_list_lock);
-		list_for_each_entry(dev, &dev_list, node) {
+		list_for_each_entry_safe(dev, next, &dev_list, node) {
 			int i;
+			if (readl(&dev->bar->csts) & NVME_CSTS_CFS &&
+							dev->initialized) {
+				if (work_busy(&dev->reset_work))
+					continue;
+				list_del_init(&dev->node);
+				dev_warn(&dev->pci_dev->dev,
+					"Failed status, reset controller\n");
+				PREPARE_WORK(&dev->reset_work,
+							nvme_reset_failed_dev);
+				queue_work(nvme_workq, &dev->reset_work);
+				continue;
+			}
 			for (i = 0; i < dev->queue_count; i++) {
 				struct nvme_queue *nvmeq = dev->queues[i];
 				if (!nvmeq)
@@ -1623,33 +1721,6 @@ static int nvme_kthread(void *data)
 	return 0;
 }
 
-static DEFINE_IDA(nvme_index_ida);
-
-static int nvme_get_ns_idx(void)
-{
-	int index, error;
-
-	do {
-		if (!ida_pre_get(&nvme_index_ida, GFP_KERNEL))
-			return -1;
-
-		spin_lock(&dev_list_lock);
-		error = ida_get_new(&nvme_index_ida, &index);
-		spin_unlock(&dev_list_lock);
-	} while (error == -EAGAIN);
-
-	if (error)
-		index = -1;
-	return index;
-}
-
-static void nvme_put_ns_idx(int index)
-{
-	spin_lock(&dev_list_lock);
-	ida_remove(&nvme_index_ida, index);
-	spin_unlock(&dev_list_lock);
-}
-
 static void nvme_config_discard(struct nvme_ns *ns)
 {
 	u32 logical_block_size = queue_logical_block_size(ns->queue);
@@ -1683,7 +1754,7 @@ static struct nvme_ns *nvme_alloc_ns(struct nvme_dev *dev, unsigned nsid,
 	ns->dev = dev;
 	ns->queue->queuedata = ns;
 
-	disk = alloc_disk(NVME_MINORS);
+	disk = alloc_disk(0);
 	if (!disk)
 		goto out_free_queue;
 	ns->ns_id = nsid;
@@ -1696,12 +1767,12 @@ static struct nvme_ns *nvme_alloc_ns(struct nvme_dev *dev, unsigned nsid,
 		blk_queue_max_hw_sectors(ns->queue, dev->max_hw_sectors);
 
 	disk->major = nvme_major;
-	disk->minors = NVME_MINORS;
-	disk->first_minor = NVME_MINORS * nvme_get_ns_idx();
+	disk->first_minor = 0;
 	disk->fops = &nvme_fops;
 	disk->private_data = ns;
 	disk->queue = ns->queue;
 	disk->driverfs_dev = &dev->pci_dev->dev;
+	disk->flags = GENHD_FL_EXT_DEVT;
 	sprintf(disk->disk_name, "nvme%dn%d", dev->instance, nsid);
 	set_capacity(disk, le64_to_cpup(&id->nsze) << (ns->lba_shift - 9));
 
@@ -1717,15 +1788,6 @@ static struct nvme_ns *nvme_alloc_ns(struct nvme_dev *dev, unsigned nsid,
 	return NULL;
 }
 
-static void nvme_ns_free(struct nvme_ns *ns)
-{
-	int index = ns->disk->first_minor / NVME_MINORS;
-	put_disk(ns->disk);
-	nvme_put_ns_idx(index);
-	blk_cleanup_queue(ns->queue);
-	kfree(ns);
-}
-
 static int set_queue_count(struct nvme_dev *dev, int count)
 {
 	int status;
@@ -1741,11 +1803,12 @@ static int set_queue_count(struct nvme_dev *dev, int count)
 
 static size_t db_bar_size(struct nvme_dev *dev, unsigned nr_io_queues)
 {
-	return 4096 + ((nr_io_queues + 1) << (dev->db_stride + 3));
+	return 4096 + ((nr_io_queues + 1) * 8 * dev->db_stride);
 }
 
 static int nvme_setup_io_queues(struct nvme_dev *dev)
 {
+	struct nvme_queue *adminq = dev->queues[0];
 	struct pci_dev *pdev = dev->pci_dev;
 	int result, cpu, i, vecs, nr_io_queues, size, q_depth;
 
@@ -1772,7 +1835,7 @@ static int nvme_setup_io_queues(struct nvme_dev *dev)
 	}
 
 	/* Deregister the admin queue's interrupt */
-	free_irq(dev->entry[0].vector, dev->queues[0]);
+	free_irq(dev->entry[0].vector, adminq);
 
 	vecs = nr_io_queues;
 	for (i = 0; i < vecs; i++)
@@ -1810,9 +1873,9 @@ static int nvme_setup_io_queues(struct nvme_dev *dev)
 	 */
 	nr_io_queues = vecs;
 
-	result = queue_request_irq(dev, dev->queues[0], "nvme admin");
+	result = queue_request_irq(dev, adminq, adminq->irqname);
 	if (result) {
-		dev->queues[0]->q_suspended = 1;
+		adminq->q_suspended = 1;
 		goto free_queues;
 	}
 
@@ -1821,9 +1884,9 @@ static int nvme_setup_io_queues(struct nvme_dev *dev)
 	for (i = dev->queue_count - 1; i > nr_io_queues; i--) {
 		struct nvme_queue *nvmeq = dev->queues[i];
 
-		spin_lock(&nvmeq->q_lock);
+		spin_lock_irq(&nvmeq->q_lock);
 		nvme_cancel_ios(nvmeq, false);
-		spin_unlock(&nvmeq->q_lock);
+		spin_unlock_irq(&nvmeq->q_lock);
 
 		nvme_free_queue(nvmeq);
 		dev->queue_count--;
@@ -1864,7 +1927,7 @@ static int nvme_setup_io_queues(struct nvme_dev *dev)
 	return 0;
 
  free_queues:
-	nvme_free_queues(dev);
+	nvme_free_queues(dev, 1);
 	return result;
 }
 
@@ -1876,6 +1939,7 @@ static int nvme_setup_io_queues(struct nvme_dev *dev)
  */
 static int nvme_dev_add(struct nvme_dev *dev)
 {
+	struct pci_dev *pdev = dev->pci_dev;
 	int res;
 	unsigned nn, i;
 	struct nvme_ns *ns;
@@ -1885,8 +1949,7 @@ static int nvme_dev_add(struct nvme_dev *dev)
 	dma_addr_t dma_addr;
 	int shift = NVME_CAP_MPSMIN(readq(&dev->bar->cap)) + 12;
 
-	mem = dma_alloc_coherent(&dev->pci_dev->dev, 8192, &dma_addr,
-								GFP_KERNEL);
+	mem = dma_alloc_coherent(&pdev->dev, 8192, &dma_addr, GFP_KERNEL);
 	if (!mem)
 		return -ENOMEM;
 
@@ -1899,13 +1962,14 @@ static int nvme_dev_add(struct nvme_dev *dev)
 	ctrl = mem;
 	nn = le32_to_cpup(&ctrl->nn);
 	dev->oncs = le16_to_cpup(&ctrl->oncs);
+	dev->abort_limit = ctrl->acl + 1;
 	memcpy(dev->serial, ctrl->sn, sizeof(ctrl->sn));
 	memcpy(dev->model, ctrl->mn, sizeof(ctrl->mn));
 	memcpy(dev->firmware_rev, ctrl->fr, sizeof(ctrl->fr));
 	if (ctrl->mdts)
 		dev->max_hw_sectors = 1 << (ctrl->mdts + shift - 9);
-	if ((dev->pci_dev->vendor == PCI_VENDOR_ID_INTEL) &&
-			(dev->pci_dev->device == 0x0953) && ctrl->vs[3])
+	if ((pdev->vendor == PCI_VENDOR_ID_INTEL) &&
+			(pdev->device == 0x0953) && ctrl->vs[3])
 		dev->stripe_size = 1 << (ctrl->vs[3] + shift);
 
 	id_ns = mem;
@@ -1953,16 +2017,21 @@ static int nvme_dev_map(struct nvme_dev *dev)
 	    dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(32)))
 		goto disable;
 
-	pci_set_drvdata(pdev, dev);
 	dev->bar = ioremap(pci_resource_start(pdev, 0), 8192);
 	if (!dev->bar)
 		goto disable;
-
-	dev->db_stride = NVME_CAP_STRIDE(readq(&dev->bar->cap));
+	if (readl(&dev->bar->csts) == -1) {
+		result = -ENODEV;
+		goto unmap;
+	}
+	dev->db_stride = 1 << NVME_CAP_STRIDE(readq(&dev->bar->cap));
 	dev->dbs = ((void __iomem *)dev->bar) + 4096;
 
 	return 0;
 
+ unmap:
+	iounmap(dev->bar);
+	dev->bar = NULL;
  disable:
 	pci_release_regions(pdev);
  disable_pci:
@@ -1980,37 +2049,183 @@ static void nvme_dev_unmap(struct nvme_dev *dev)
 	if (dev->bar) {
 		iounmap(dev->bar);
 		dev->bar = NULL;
+		pci_release_regions(dev->pci_dev);
 	}
 
-	pci_release_regions(dev->pci_dev);
 	if (pci_is_enabled(dev->pci_dev))
 		pci_disable_device(dev->pci_dev);
 }
 
+struct nvme_delq_ctx {
+	struct task_struct *waiter;
+	struct kthread_worker *worker;
+	atomic_t refcount;
+};
+
+static void nvme_wait_dq(struct nvme_delq_ctx *dq, struct nvme_dev *dev)
+{
+	dq->waiter = current;
+	mb();
+
+	for (;;) {
+		set_current_state(TASK_KILLABLE);
+		if (!atomic_read(&dq->refcount))
+			break;
+		if (!schedule_timeout(ADMIN_TIMEOUT) ||
+					fatal_signal_pending(current)) {
+			set_current_state(TASK_RUNNING);
+
+			nvme_disable_ctrl(dev, readq(&dev->bar->cap));
+			nvme_disable_queue(dev, 0);
+
+			send_sig(SIGKILL, dq->worker->task, 1);
+			flush_kthread_worker(dq->worker);
+			return;
+		}
+	}
+	set_current_state(TASK_RUNNING);
+}
+
+static void nvme_put_dq(struct nvme_delq_ctx *dq)
+{
+	atomic_dec(&dq->refcount);
+	if (dq->waiter)
+		wake_up_process(dq->waiter);
+}
+
+static struct nvme_delq_ctx *nvme_get_dq(struct nvme_delq_ctx *dq)
+{
+	atomic_inc(&dq->refcount);
+	return dq;
+}
+
+static void nvme_del_queue_end(struct nvme_queue *nvmeq)
+{
+	struct nvme_delq_ctx *dq = nvmeq->cmdinfo.ctx;
+
+	nvme_clear_queue(nvmeq);
+	nvme_put_dq(dq);
+}
+
+static int adapter_async_del_queue(struct nvme_queue *nvmeq, u8 opcode,
+						kthread_work_func_t fn)
+{
+	struct nvme_command c;
+
+	memset(&c, 0, sizeof(c));
+	c.delete_queue.opcode = opcode;
+	c.delete_queue.qid = cpu_to_le16(nvmeq->qid);
+
+	init_kthread_work(&nvmeq->cmdinfo.work, fn);
+	return nvme_submit_admin_cmd_async(nvmeq->dev, &c, &nvmeq->cmdinfo);
+}
+
+static void nvme_del_cq_work_handler(struct kthread_work *work)
+{
+	struct nvme_queue *nvmeq = container_of(work, struct nvme_queue,
+							cmdinfo.work);
+	nvme_del_queue_end(nvmeq);
+}
+
+static int nvme_delete_cq(struct nvme_queue *nvmeq)
+{
+	return adapter_async_del_queue(nvmeq, nvme_admin_delete_cq,
+						nvme_del_cq_work_handler);
+}
+
+static void nvme_del_sq_work_handler(struct kthread_work *work)
+{
+	struct nvme_queue *nvmeq = container_of(work, struct nvme_queue,
+							cmdinfo.work);
+	int status = nvmeq->cmdinfo.status;
+
+	if (!status)
+		status = nvme_delete_cq(nvmeq);
+	if (status)
+		nvme_del_queue_end(nvmeq);
+}
+
+static int nvme_delete_sq(struct nvme_queue *nvmeq)
+{
+	return adapter_async_del_queue(nvmeq, nvme_admin_delete_sq,
+						nvme_del_sq_work_handler);
+}
+
+static void nvme_del_queue_start(struct kthread_work *work)
+{
+	struct nvme_queue *nvmeq = container_of(work, struct nvme_queue,
+							cmdinfo.work);
+	allow_signal(SIGKILL);
+	if (nvme_delete_sq(nvmeq))
+		nvme_del_queue_end(nvmeq);
+}
+
+static void nvme_disable_io_queues(struct nvme_dev *dev)
+{
+	int i;
+	DEFINE_KTHREAD_WORKER_ONSTACK(worker);
+	struct nvme_delq_ctx dq;
+	struct task_struct *kworker_task = kthread_run(kthread_worker_fn,
+					&worker, "nvme%d", dev->instance);
+
+	if (IS_ERR(kworker_task)) {
+		dev_err(&dev->pci_dev->dev,
+			"Failed to create queue del task\n");
+		for (i = dev->queue_count - 1; i > 0; i--)
+			nvme_disable_queue(dev, i);
+		return;
+	}
+
+	dq.waiter = NULL;
+	atomic_set(&dq.refcount, 0);
+	dq.worker = &worker;
+	for (i = dev->queue_count - 1; i > 0; i--) {
+		struct nvme_queue *nvmeq = dev->queues[i];
+
+		if (nvme_suspend_queue(nvmeq))
+			continue;
+		nvmeq->cmdinfo.ctx = nvme_get_dq(&dq);
+		nvmeq->cmdinfo.worker = dq.worker;
+		init_kthread_work(&nvmeq->cmdinfo.work, nvme_del_queue_start);
+		queue_kthread_work(dq.worker, &nvmeq->cmdinfo.work);
+	}
+	nvme_wait_dq(&dq, dev);
+	kthread_stop(kworker_task);
+}
+
 static void nvme_dev_shutdown(struct nvme_dev *dev)
 {
 	int i;
 
-	for (i = dev->queue_count - 1; i >= 0; i--)
-		nvme_disable_queue(dev, i);
+	dev->initialized = 0;
 
 	spin_lock(&dev_list_lock);
 	list_del_init(&dev->node);
 	spin_unlock(&dev_list_lock);
 
-	if (dev->bar)
+	if (!dev->bar || (dev->bar && readl(&dev->bar->csts) == -1)) {
+		for (i = dev->queue_count - 1; i >= 0; i--) {
+			struct nvme_queue *nvmeq = dev->queues[i];
+			nvme_suspend_queue(nvmeq);
+			nvme_clear_queue(nvmeq);
+		}
+	} else {
+		nvme_disable_io_queues(dev);
 		nvme_shutdown_ctrl(dev);
+		nvme_disable_queue(dev, 0);
+	}
 	nvme_dev_unmap(dev);
 }
 
 static void nvme_dev_remove(struct nvme_dev *dev)
 {
-	struct nvme_ns *ns, *next;
+	struct nvme_ns *ns;
 
-	list_for_each_entry_safe(ns, next, &dev->namespaces, list) {
-		list_del(&ns->list);
-		del_gendisk(ns->disk);
-		nvme_ns_free(ns);
+	list_for_each_entry(ns, &dev->namespaces, list) {
+		if (ns->disk->flags & GENHD_FL_UP)
+			del_gendisk(ns->disk);
+		if (!blk_queue_dying(ns->queue))
+			blk_cleanup_queue(ns->queue);
 	}
 }
 
@@ -2067,14 +2282,22 @@ static void nvme_release_instance(struct nvme_dev *dev)
 	spin_unlock(&dev_list_lock);
 }
 
+static void nvme_free_namespaces(struct nvme_dev *dev)
+{
+	struct nvme_ns *ns, *next;
+
+	list_for_each_entry_safe(ns, next, &dev->namespaces, list) {
+		list_del(&ns->list);
+		put_disk(ns->disk);
+		kfree(ns);
+	}
+}
+
 static void nvme_free_dev(struct kref *kref)
 {
 	struct nvme_dev *dev = container_of(kref, struct nvme_dev, kref);
-	nvme_dev_remove(dev);
-	nvme_dev_shutdown(dev);
-	nvme_free_queues(dev);
-	nvme_release_instance(dev);
-	nvme_release_prp_pools(dev);
+
+	nvme_free_namespaces(dev);
 	kfree(dev->queues);
 	kfree(dev->entry);
 	kfree(dev);
@@ -2138,6 +2361,7 @@ static int nvme_dev_start(struct nvme_dev *dev)
 	return result;
 
  disable:
+	nvme_disable_queue(dev, 0);
 	spin_lock(&dev_list_lock);
 	list_del_init(&dev->node);
 	spin_unlock(&dev_list_lock);
@@ -2146,6 +2370,71 @@ static int nvme_dev_start(struct nvme_dev *dev)
 	return result;
 }
 
+static int nvme_remove_dead_ctrl(void *arg)
+{
+	struct nvme_dev *dev = (struct nvme_dev *)arg;
+	struct pci_dev *pdev = dev->pci_dev;
+
+	if (pci_get_drvdata(pdev))
+		pci_stop_and_remove_bus_device(pdev);
+	kref_put(&dev->kref, nvme_free_dev);
+	return 0;
+}
+
+static void nvme_remove_disks(struct work_struct *ws)
+{
+	int i;
+	struct nvme_dev *dev = container_of(ws, struct nvme_dev, reset_work);
+
+	nvme_dev_remove(dev);
+	spin_lock(&dev_list_lock);
+	for (i = dev->queue_count - 1; i > 0; i--) {
+		BUG_ON(!dev->queues[i] || !dev->queues[i]->q_suspended);
+		nvme_free_queue(dev->queues[i]);
+		dev->queue_count--;
+		dev->queues[i] = NULL;
+	}
+	spin_unlock(&dev_list_lock);
+}
+
+static int nvme_dev_resume(struct nvme_dev *dev)
+{
+	int ret;
+
+	ret = nvme_dev_start(dev);
+	if (ret && ret != -EBUSY)
+		return ret;
+	if (ret == -EBUSY) {
+		spin_lock(&dev_list_lock);
+		PREPARE_WORK(&dev->reset_work, nvme_remove_disks);
+		queue_work(nvme_workq, &dev->reset_work);
+		spin_unlock(&dev_list_lock);
+	}
+	dev->initialized = 1;
+	return 0;
+}
+
+static void nvme_dev_reset(struct nvme_dev *dev)
+{
+	nvme_dev_shutdown(dev);
+	if (nvme_dev_resume(dev)) {
+		dev_err(&dev->pci_dev->dev, "Device failed to resume\n");
+		kref_get(&dev->kref);
+		if (IS_ERR(kthread_run(nvme_remove_dead_ctrl, dev, "nvme%d",
+							dev->instance))) {
+			dev_err(&dev->pci_dev->dev,
+				"Failed to start controller remove task\n");
+			kref_put(&dev->kref, nvme_free_dev);
+		}
+	}
+}
+
+static void nvme_reset_failed_dev(struct work_struct *ws)
+{
+	struct nvme_dev *dev = container_of(ws, struct nvme_dev, reset_work);
+	nvme_dev_reset(dev);
+}
+
 static int nvme_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 {
 	int result = -ENOMEM;
@@ -2164,8 +2453,9 @@ static int nvme_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		goto free;
 
 	INIT_LIST_HEAD(&dev->namespaces);
+	INIT_WORK(&dev->reset_work, nvme_reset_failed_dev);
 	dev->pci_dev = pdev;
-
+	pci_set_drvdata(pdev, dev);
 	result = nvme_set_instance(dev);
 	if (result)
 		goto free;
@@ -2181,6 +2471,7 @@ static int nvme_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		goto release_pools;
 	}
 
+	kref_init(&dev->kref);
 	result = nvme_dev_add(dev);
 	if (result)
 		goto shutdown;
@@ -2195,15 +2486,16 @@ static int nvme_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	if (result)
 		goto remove;
 
-	kref_init(&dev->kref);
+	dev->initialized = 1;
 	return 0;
 
  remove:
 	nvme_dev_remove(dev);
+	nvme_free_namespaces(dev);
  shutdown:
 	nvme_dev_shutdown(dev);
  release_pools:
-	nvme_free_queues(dev);
+	nvme_free_queues(dev, 0);
 	nvme_release_prp_pools(dev);
  release:
 	nvme_release_instance(dev);
@@ -2214,10 +2506,28 @@ static int nvme_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	return result;
 }
 
+static void nvme_shutdown(struct pci_dev *pdev)
+{
+	struct nvme_dev *dev = pci_get_drvdata(pdev);
+	nvme_dev_shutdown(dev);
+}
+
 static void nvme_remove(struct pci_dev *pdev)
 {
 	struct nvme_dev *dev = pci_get_drvdata(pdev);
+
+	spin_lock(&dev_list_lock);
+	list_del_init(&dev->node);
+	spin_unlock(&dev_list_lock);
+
+	pci_set_drvdata(pdev, NULL);
+	flush_work(&dev->reset_work);
 	misc_deregister(&dev->miscdev);
+	nvme_dev_remove(dev);
+	nvme_dev_shutdown(dev);
+	nvme_free_queues(dev, 0);
+	nvme_release_instance(dev);
+	nvme_release_prp_pools(dev);
 	kref_put(&dev->kref, nvme_free_dev);
 }
 
@@ -2241,13 +2551,12 @@ static int nvme_resume(struct device *dev)
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
 	struct nvme_dev *ndev = pci_get_drvdata(pdev);
-	int ret;
 
-	ret = nvme_dev_start(ndev);
-	/* XXX: should remove gendisks if resume fails */
-	if (ret)
-		nvme_free_queues(ndev);
-	return ret;
+	if (nvme_dev_resume(ndev) && !work_busy(&ndev->reset_work)) {
+		PREPARE_WORK(&ndev->reset_work, nvme_reset_failed_dev);
+		queue_work(nvme_workq, &ndev->reset_work);
+	}
+	return 0;
 }
 
 static SIMPLE_DEV_PM_OPS(nvme_dev_pm_ops, nvme_suspend, nvme_resume);
@@ -2274,6 +2583,7 @@ static struct pci_driver nvme_driver = {
 	.id_table	= nvme_id_table,
 	.probe		= nvme_probe,
 	.remove		= nvme_remove,
+	.shutdown	= nvme_shutdown,
 	.driver		= {
 		.pm	= &nvme_dev_pm_ops,
 	},
@@ -2288,9 +2598,14 @@ static int __init nvme_init(void)
 	if (IS_ERR(nvme_thread))
 		return PTR_ERR(nvme_thread);
 
+	result = -ENOMEM;
+	nvme_workq = create_singlethread_workqueue("nvme");
+	if (!nvme_workq)
+		goto kill_kthread;
+
 	result = register_blkdev(nvme_major, "nvme");
 	if (result < 0)
-		goto kill_kthread;
+		goto kill_workq;
 	else if (result > 0)
 		nvme_major = result;
 
@@ -2301,6 +2616,8 @@ static int __init nvme_init(void)
 
  unregister_blkdev:
 	unregister_blkdev(nvme_major, "nvme");
+ kill_workq:
+	destroy_workqueue(nvme_workq);
  kill_kthread:
 	kthread_stop(nvme_thread);
 	return result;
@@ -2310,6 +2627,7 @@ static void __exit nvme_exit(void)
 {
 	pci_unregister_driver(&nvme_driver);
 	unregister_blkdev(nvme_major, "nvme");
+	destroy_workqueue(nvme_workq);
 	kthread_stop(nvme_thread);
 }
 
diff --git a/drivers/block/nvme-scsi.c b/drivers/block/nvme-scsi.c
index 4a4ff4eb8e23..4a0ceb64e269 100644
--- a/drivers/block/nvme-scsi.c
+++ b/drivers/block/nvme-scsi.c
@@ -25,6 +25,7 @@
 #include <linux/bio.h>
 #include <linux/bitops.h>
 #include <linux/blkdev.h>
+#include <linux/compat.h>
 #include <linux/delay.h>
 #include <linux/errno.h>
 #include <linux/fs.h>
@@ -3038,6 +3039,152 @@ int nvme_sg_io(struct nvme_ns *ns, struct sg_io_hdr __user *u_hdr)
 	return retcode;
 }
 
+#ifdef CONFIG_COMPAT
+typedef struct sg_io_hdr32 {
+	compat_int_t interface_id;	/* [i] 'S' for SCSI generic (required) */
+	compat_int_t dxfer_direction;	/* [i] data transfer direction  */
+	unsigned char cmd_len;		/* [i] SCSI command length ( <= 16 bytes) */
+	unsigned char mx_sb_len;		/* [i] max length to write to sbp */
+	unsigned short iovec_count;	/* [i] 0 implies no scatter gather */
+	compat_uint_t dxfer_len;		/* [i] byte count of data transfer */
+	compat_uint_t dxferp;		/* [i], [*io] points to data transfer memory
+					      or scatter gather list */
+	compat_uptr_t cmdp;		/* [i], [*i] points to command to perform */
+	compat_uptr_t sbp;		/* [i], [*o] points to sense_buffer memory */
+	compat_uint_t timeout;		/* [i] MAX_UINT->no timeout (unit: millisec) */
+	compat_uint_t flags;		/* [i] 0 -> default, see SG_FLAG... */
+	compat_int_t pack_id;		/* [i->o] unused internally (normally) */
+	compat_uptr_t usr_ptr;		/* [i->o] unused internally */
+	unsigned char status;		/* [o] scsi status */
+	unsigned char masked_status;	/* [o] shifted, masked scsi status */
+	unsigned char msg_status;		/* [o] messaging level data (optional) */
+	unsigned char sb_len_wr;		/* [o] byte count actually written to sbp */
+	unsigned short host_status;	/* [o] errors from host adapter */
+	unsigned short driver_status;	/* [o] errors from software driver */
+	compat_int_t resid;		/* [o] dxfer_len - actual_transferred */
+	compat_uint_t duration;		/* [o] time taken by cmd (unit: millisec) */
+	compat_uint_t info;		/* [o] auxiliary information */
+} sg_io_hdr32_t;  /* 64 bytes long (on sparc32) */
+
+typedef struct sg_iovec32 {
+	compat_uint_t iov_base;
+	compat_uint_t iov_len;
+} sg_iovec32_t;
+
+static int sg_build_iovec(sg_io_hdr_t __user *sgio, void __user *dxferp, u16 iovec_count)
+{
+	sg_iovec_t __user *iov = (sg_iovec_t __user *) (sgio + 1);
+	sg_iovec32_t __user *iov32 = dxferp;
+	int i;
+
+	for (i = 0; i < iovec_count; i++) {
+		u32 base, len;
+
+		if (get_user(base, &iov32[i].iov_base) ||
+		    get_user(len, &iov32[i].iov_len) ||
+		    put_user(compat_ptr(base), &iov[i].iov_base) ||
+		    put_user(len, &iov[i].iov_len))
+			return -EFAULT;
+	}
+
+	if (put_user(iov, &sgio->dxferp))
+		return -EFAULT;
+	return 0;
+}
+
+int nvme_sg_io32(struct nvme_ns *ns, unsigned long arg)
+{
+	sg_io_hdr32_t __user *sgio32 = (sg_io_hdr32_t __user *)arg;
+	sg_io_hdr_t __user *sgio;
+	u16 iovec_count;
+	u32 data;
+	void __user *dxferp;
+	int err;
+	int interface_id;
+
+	if (get_user(interface_id, &sgio32->interface_id))
+		return -EFAULT;
+	if (interface_id != 'S')
+		return -EINVAL;
+
+	if (get_user(iovec_count, &sgio32->iovec_count))
+		return -EFAULT;
+
+	{
+		void __user *top = compat_alloc_user_space(0);
+		void __user *new = compat_alloc_user_space(sizeof(sg_io_hdr_t) +
+				       (iovec_count * sizeof(sg_iovec_t)));
+		if (new > top)
+			return -EINVAL;
+
+		sgio = new;
+	}
+
+	/* Ok, now construct.  */
+	if (copy_in_user(&sgio->interface_id, &sgio32->interface_id,
+			 (2 * sizeof(int)) +
+			 (2 * sizeof(unsigned char)) +
+			 (1 * sizeof(unsigned short)) +
+			 (1 * sizeof(unsigned int))))
+		return -EFAULT;
+
+	if (get_user(data, &sgio32->dxferp))
+		return -EFAULT;
+	dxferp = compat_ptr(data);
+	if (iovec_count) {
+		if (sg_build_iovec(sgio, dxferp, iovec_count))
+			return -EFAULT;
+	} else {
+		if (put_user(dxferp, &sgio->dxferp))
+			return -EFAULT;
+	}
+
+	{
+		unsigned char __user *cmdp;
+		unsigned char __user *sbp;
+
+		if (get_user(data, &sgio32->cmdp))
+			return -EFAULT;
+		cmdp = compat_ptr(data);
+
+		if (get_user(data, &sgio32->sbp))
+			return -EFAULT;
+		sbp = compat_ptr(data);
+
+		if (put_user(cmdp, &sgio->cmdp) ||
+		    put_user(sbp, &sgio->sbp))
+			return -EFAULT;
+	}
+
+	if (copy_in_user(&sgio->timeout, &sgio32->timeout,
+			 3 * sizeof(int)))
+		return -EFAULT;
+
+	if (get_user(data, &sgio32->usr_ptr))
+		return -EFAULT;
+	if (put_user(compat_ptr(data), &sgio->usr_ptr))
+		return -EFAULT;
+
+	err = nvme_sg_io(ns, sgio);
+	if (err >= 0) {
+		void __user *datap;
+
+		if (copy_in_user(&sgio32->pack_id, &sgio->pack_id,
+				 sizeof(int)) ||
+		    get_user(datap, &sgio->usr_ptr) ||
+		    put_user((u32)(unsigned long)datap,
+			     &sgio32->usr_ptr) ||
+		    copy_in_user(&sgio32->status, &sgio->status,
+				 (4 * sizeof(unsigned char)) +
+				 (2 * sizeof(unsigned short)) +
+				 (3 * sizeof(int))))
+			err = -EFAULT;
+	}
+
+	return err;
+}
+#endif
+
 int nvme_sg_get_version_num(int __user *ip)
 {
 	return put_user(sg_version_num, ip);
diff --git a/drivers/block/paride/pg.c b/drivers/block/paride/pg.c
index 4a27b1de5fcb..2ce3dfd7e6b9 100644
--- a/drivers/block/paride/pg.c
+++ b/drivers/block/paride/pg.c
@@ -581,7 +581,7 @@ static ssize_t pg_write(struct file *filp, const char __user *buf, size_t count,
 
 	if (hdr.magic != PG_MAGIC)
 		return -EINVAL;
-	if (hdr.dlen > PG_MAX_DATA)
+	if (hdr.dlen < 0 || hdr.dlen > PG_MAX_DATA)
 		return -EINVAL;
 	if ((count - hs) > PG_MAX_DATA)
 		return -EINVAL;
diff --git a/drivers/block/pktcdvd.c b/drivers/block/pktcdvd.c
index ff8668c5efb1..a2af73db187b 100644
--- a/drivers/block/pktcdvd.c
+++ b/drivers/block/pktcdvd.c
@@ -651,7 +651,7 @@ static struct pkt_rb_node *pkt_rbtree_find(struct pktcdvd_device *pd, sector_t s
 
 	for (;;) {
 		tmp = rb_entry(n, struct pkt_rb_node, rb_node);
-		if (s <= tmp->bio->bi_sector)
+		if (s <= tmp->bio->bi_iter.bi_sector)
 			next = n->rb_left;
 		else
 			next = n->rb_right;
@@ -660,12 +660,12 @@ static struct pkt_rb_node *pkt_rbtree_find(struct pktcdvd_device *pd, sector_t s
 		n = next;
 	}
 
-	if (s > tmp->bio->bi_sector) {
+	if (s > tmp->bio->bi_iter.bi_sector) {
 		tmp = pkt_rbtree_next(tmp);
 		if (!tmp)
 			return NULL;
 	}
-	BUG_ON(s > tmp->bio->bi_sector);
+	BUG_ON(s > tmp->bio->bi_iter.bi_sector);
 	return tmp;
 }
 
@@ -676,13 +676,13 @@ static void pkt_rbtree_insert(struct pktcdvd_device *pd, struct pkt_rb_node *nod
 {
 	struct rb_node **p = &pd->bio_queue.rb_node;
 	struct rb_node *parent = NULL;
-	sector_t s = node->bio->bi_sector;
+	sector_t s = node->bio->bi_iter.bi_sector;
 	struct pkt_rb_node *tmp;
 
 	while (*p) {
 		parent = *p;
 		tmp = rb_entry(parent, struct pkt_rb_node, rb_node);
-		if (s < tmp->bio->bi_sector)
+		if (s < tmp->bio->bi_iter.bi_sector)
 			p = &(*p)->rb_left;
 		else
 			p = &(*p)->rb_right;
@@ -706,7 +706,9 @@ static int pkt_generic_packet(struct pktcdvd_device *pd, struct packet_command *
 			     WRITE : READ, __GFP_WAIT);
 
 	if (cgc->buflen) {
-		if (blk_rq_map_kern(q, rq, cgc->buffer, cgc->buflen, __GFP_WAIT))
+		ret = blk_rq_map_kern(q, rq, cgc->buffer, cgc->buflen,
+				      __GFP_WAIT);
+		if (ret)
 			goto out;
 	}
 
@@ -857,7 +859,8 @@ static void pkt_iosched_process_queue(struct pktcdvd_device *pd)
 			spin_lock(&pd->iosched.lock);
 			bio = bio_list_peek(&pd->iosched.write_queue);
 			spin_unlock(&pd->iosched.lock);
-			if (bio && (bio->bi_sector == pd->iosched.last_write))
+			if (bio && (bio->bi_iter.bi_sector ==
+				    pd->iosched.last_write))
 				need_write_seek = 0;
 			if (need_write_seek && reads_queued) {
 				if (atomic_read(&pd->cdrw.pending_bios) > 0) {
@@ -888,7 +891,8 @@ static void pkt_iosched_process_queue(struct pktcdvd_device *pd)
 			continue;
 
 		if (bio_data_dir(bio) == READ)
-			pd->iosched.successive_reads += bio->bi_size >> 10;
+			pd->iosched.successive_reads +=
+				bio->bi_iter.bi_size >> 10;
 		else {
 			pd->iosched.successive_reads = 0;
 			pd->iosched.last_write = bio_end_sector(bio);
@@ -978,7 +982,7 @@ static void pkt_end_io_read(struct bio *bio, int err)
 
 	pkt_dbg(2, pd, "bio=%p sec0=%llx sec=%llx err=%d\n",
 		bio, (unsigned long long)pkt->sector,
-		(unsigned long long)bio->bi_sector, err);
+		(unsigned long long)bio->bi_iter.bi_sector, err);
 
 	if (err)
 		atomic_inc(&pkt->io_errors);
@@ -1026,8 +1030,9 @@ static void pkt_gather_data(struct pktcdvd_device *pd, struct packet_data *pkt)
 	memset(written, 0, sizeof(written));
 	spin_lock(&pkt->lock);
 	bio_list_for_each(bio, &pkt->orig_bios) {
-		int first_frame = (bio->bi_sector - pkt->sector) / (CD_FRAMESIZE >> 9);
-		int num_frames = bio->bi_size / CD_FRAMESIZE;
+		int first_frame = (bio->bi_iter.bi_sector - pkt->sector) /
+			(CD_FRAMESIZE >> 9);
+		int num_frames = bio->bi_iter.bi_size / CD_FRAMESIZE;
 		pd->stats.secs_w += num_frames * (CD_FRAMESIZE >> 9);
 		BUG_ON(first_frame < 0);
 		BUG_ON(first_frame + num_frames > pkt->frames);
@@ -1053,7 +1058,7 @@ static void pkt_gather_data(struct pktcdvd_device *pd, struct packet_data *pkt)
 
 		bio = pkt->r_bios[f];
 		bio_reset(bio);
-		bio->bi_sector = pkt->sector + f * (CD_FRAMESIZE >> 9);
+		bio->bi_iter.bi_sector = pkt->sector + f * (CD_FRAMESIZE >> 9);
 		bio->bi_bdev = pd->bdev;
 		bio->bi_end_io = pkt_end_io_read;
 		bio->bi_private = pkt;
@@ -1150,8 +1155,8 @@ static int pkt_start_recovery(struct packet_data *pkt)
 	bio_reset(pkt->bio);
 	pkt->bio->bi_bdev = pd->bdev;
 	pkt->bio->bi_rw = REQ_WRITE;
-	pkt->bio->bi_sector = new_sector;
-	pkt->bio->bi_size = pkt->frames * CD_FRAMESIZE;
+	pkt->bio->bi_iter.bi_sector = new_sector;
+	pkt->bio->bi_iter.bi_size = pkt->frames * CD_FRAMESIZE;
 	pkt->bio->bi_vcnt = pkt->frames;
 
 	pkt->bio->bi_end_io = pkt_end_io_packet_write;
@@ -1213,7 +1218,7 @@ static int pkt_handle_queue(struct pktcdvd_device *pd)
 	node = first_node;
 	while (node) {
 		bio = node->bio;
-		zone = get_zone(bio->bi_sector, pd);
+		zone = get_zone(bio->bi_iter.bi_sector, pd);
 		list_for_each_entry(p, &pd->cdrw.pkt_active_list, list) {
 			if (p->sector == zone) {
 				bio = NULL;
@@ -1252,14 +1257,14 @@ try_next_bio:
 	pkt_dbg(2, pd, "looking for zone %llx\n", (unsigned long long)zone);
 	while ((node = pkt_rbtree_find(pd, zone)) != NULL) {
 		bio = node->bio;
-		pkt_dbg(2, pd, "found zone=%llx\n",
-			(unsigned long long)get_zone(bio->bi_sector, pd));
-		if (get_zone(bio->bi_sector, pd) != zone)
+		pkt_dbg(2, pd, "found zone=%llx\n", (unsigned long long)
+			get_zone(bio->bi_iter.bi_sector, pd));
+		if (get_zone(bio->bi_iter.bi_sector, pd) != zone)
 			break;
 		pkt_rbtree_erase(pd, node);
 		spin_lock(&pkt->lock);
 		bio_list_add(&pkt->orig_bios, bio);
-		pkt->write_size += bio->bi_size / CD_FRAMESIZE;
+		pkt->write_size += bio->bi_iter.bi_size / CD_FRAMESIZE;
 		spin_unlock(&pkt->lock);
 	}
 	/* check write congestion marks, and if bio_queue_size is
@@ -1293,7 +1298,7 @@ static void pkt_start_write(struct pktcdvd_device *pd, struct packet_data *pkt)
 	struct bio_vec *bvec = pkt->w_bio->bi_io_vec;
 
 	bio_reset(pkt->w_bio);
-	pkt->w_bio->bi_sector = pkt->sector;
+	pkt->w_bio->bi_iter.bi_sector = pkt->sector;
 	pkt->w_bio->bi_bdev = pd->bdev;
 	pkt->w_bio->bi_end_io = pkt_end_io_packet_write;
 	pkt->w_bio->bi_private = pkt;
@@ -2335,75 +2340,29 @@ static void pkt_end_io_read_cloned(struct bio *bio, int err)
 	pkt_bio_finished(pd);
 }
 
-static void pkt_make_request(struct request_queue *q, struct bio *bio)
+static void pkt_make_request_read(struct pktcdvd_device *pd, struct bio *bio)
 {
-	struct pktcdvd_device *pd;
-	char b[BDEVNAME_SIZE];
+	struct bio *cloned_bio = bio_clone(bio, GFP_NOIO);
+	struct packet_stacked_data *psd = mempool_alloc(psd_pool, GFP_NOIO);
+
+	psd->pd = pd;
+	psd->bio = bio;
+	cloned_bio->bi_bdev = pd->bdev;
+	cloned_bio->bi_private = psd;
+	cloned_bio->bi_end_io = pkt_end_io_read_cloned;
+	pd->stats.secs_r += bio_sectors(bio);
+	pkt_queue_bio(pd, cloned_bio);
+}
+
+static void pkt_make_request_write(struct request_queue *q, struct bio *bio)
+{
+	struct pktcdvd_device *pd = q->queuedata;
 	sector_t zone;
 	struct packet_data *pkt;
 	int was_empty, blocked_bio;
 	struct pkt_rb_node *node;
 
-	pd = q->queuedata;
-	if (!pd) {
-		pr_err("%s incorrect request queue\n",
-		       bdevname(bio->bi_bdev, b));
-		goto end_io;
-	}
-
-	/*
-	 * Clone READ bios so we can have our own bi_end_io callback.
-	 */
-	if (bio_data_dir(bio) == READ) {
-		struct bio *cloned_bio = bio_clone(bio, GFP_NOIO);
-		struct packet_stacked_data *psd = mempool_alloc(psd_pool, GFP_NOIO);
-
-		psd->pd = pd;
-		psd->bio = bio;
-		cloned_bio->bi_bdev = pd->bdev;
-		cloned_bio->bi_private = psd;
-		cloned_bio->bi_end_io = pkt_end_io_read_cloned;
-		pd->stats.secs_r += bio_sectors(bio);
-		pkt_queue_bio(pd, cloned_bio);
-		return;
-	}
-
-	if (!test_bit(PACKET_WRITABLE, &pd->flags)) {
-		pkt_notice(pd, "WRITE for ro device (%llu)\n",
-			   (unsigned long long)bio->bi_sector);
-		goto end_io;
-	}
-
-	if (!bio->bi_size || (bio->bi_size % CD_FRAMESIZE)) {
-		pkt_err(pd, "wrong bio size\n");
-		goto end_io;
-	}
-
-	blk_queue_bounce(q, &bio);
-
-	zone = get_zone(bio->bi_sector, pd);
-	pkt_dbg(2, pd, "start = %6llx stop = %6llx\n",
-		(unsigned long long)bio->bi_sector,
-		(unsigned long long)bio_end_sector(bio));
-
-	/* Check if we have to split the bio */
-	{
-		struct bio_pair *bp;
-		sector_t last_zone;
-		int first_sectors;
-
-		last_zone = get_zone(bio_end_sector(bio) - 1, pd);
-		if (last_zone != zone) {
-			BUG_ON(last_zone != zone + pd->settings.size);
-			first_sectors = last_zone - bio->bi_sector;
-			bp = bio_split(bio, first_sectors);
-			BUG_ON(!bp);
-			pkt_make_request(q, &bp->bio1);
-			pkt_make_request(q, &bp->bio2);
-			bio_pair_release(bp);
-			return;
-		}
-	}
+	zone = get_zone(bio->bi_iter.bi_sector, pd);
 
 	/*
 	 * If we find a matching packet in state WAITING or READ_WAIT, we can
@@ -2417,7 +2376,8 @@ static void pkt_make_request(struct request_queue *q, struct bio *bio)
 			if ((pkt->state == PACKET_WAITING_STATE) ||
 			    (pkt->state == PACKET_READ_WAIT_STATE)) {
 				bio_list_add(&pkt->orig_bios, bio);
-				pkt->write_size += bio->bi_size / CD_FRAMESIZE;
+				pkt->write_size +=
+					bio->bi_iter.bi_size / CD_FRAMESIZE;
 				if ((pkt->write_size >= pkt->frames) &&
 				    (pkt->state == PACKET_WAITING_STATE)) {
 					atomic_inc(&pkt->run_sm);
@@ -2476,6 +2436,64 @@ static void pkt_make_request(struct request_queue *q, struct bio *bio)
 		 */
 		wake_up(&pd->wqueue);
 	}
+}
+
+static void pkt_make_request(struct request_queue *q, struct bio *bio)
+{
+	struct pktcdvd_device *pd;
+	char b[BDEVNAME_SIZE];
+	struct bio *split;
+
+	pd = q->queuedata;
+	if (!pd) {
+		pr_err("%s incorrect request queue\n",
+		       bdevname(bio->bi_bdev, b));
+		goto end_io;
+	}
+
+	pkt_dbg(2, pd, "start = %6llx stop = %6llx\n",
+		(unsigned long long)bio->bi_iter.bi_sector,
+		(unsigned long long)bio_end_sector(bio));
+
+	/*
+	 * Clone READ bios so we can have our own bi_end_io callback.
+	 */
+	if (bio_data_dir(bio) == READ) {
+		pkt_make_request_read(pd, bio);
+		return;
+	}
+
+	if (!test_bit(PACKET_WRITABLE, &pd->flags)) {
+		pkt_notice(pd, "WRITE for ro device (%llu)\n",
+			   (unsigned long long)bio->bi_iter.bi_sector);
+		goto end_io;
+	}
+
+	if (!bio->bi_iter.bi_size || (bio->bi_iter.bi_size % CD_FRAMESIZE)) {
+		pkt_err(pd, "wrong bio size\n");
+		goto end_io;
+	}
+
+	blk_queue_bounce(q, &bio);
+
+	do {
+		sector_t zone = get_zone(bio->bi_iter.bi_sector, pd);
+		sector_t last_zone = get_zone(bio_end_sector(bio) - 1, pd);
+
+		if (last_zone != zone) {
+			BUG_ON(last_zone != zone + pd->settings.size);
+
+			split = bio_split(bio, last_zone -
+					  bio->bi_iter.bi_sector,
+					  GFP_NOIO, fs_bio_set);
+			bio_chain(split, bio);
+		} else {
+			split = bio;
+		}
+
+		pkt_make_request_write(q, split);
+	} while (split != bio);
+
 	return;
 end_io:
 	bio_io_error(bio);
diff --git a/drivers/block/ps3disk.c b/drivers/block/ps3disk.c
index d754a88d7585..c120d70d3fb3 100644
--- a/drivers/block/ps3disk.c
+++ b/drivers/block/ps3disk.c
@@ -94,26 +94,25 @@ static void ps3disk_scatter_gather(struct ps3_storage_device *dev,
 {
 	unsigned int offset = 0;
 	struct req_iterator iter;
-	struct bio_vec *bvec;
+	struct bio_vec bvec;
 	unsigned int i = 0;
 	size_t size;
 	void *buf;
 
 	rq_for_each_segment(bvec, req, iter) {
 		unsigned long flags;
-		dev_dbg(&dev->sbd.core,
-			"%s:%u: bio %u: %u segs %u sectors from %lu\n",
-			__func__, __LINE__, i, bio_segments(iter.bio),
-			bio_sectors(iter.bio), iter.bio->bi_sector);
+		dev_dbg(&dev->sbd.core, "%s:%u: bio %u: %u sectors from %lu\n",
+			__func__, __LINE__, i, bio_sectors(iter.bio),
+			iter.bio->bi_iter.bi_sector);
 
-		size = bvec->bv_len;
-		buf = bvec_kmap_irq(bvec, &flags);
+		size = bvec.bv_len;
+		buf = bvec_kmap_irq(&bvec, &flags);
 		if (gather)
 			memcpy(dev->bounce_buf+offset, buf, size);
 		else
 			memcpy(buf, dev->bounce_buf+offset, size);
 		offset += size;
-		flush_kernel_dcache_page(bvec->bv_page);
+		flush_kernel_dcache_page(bvec.bv_page);
 		bvec_kunmap_irq(buf, &flags);
 		i++;
 	}
@@ -130,7 +129,7 @@ static int ps3disk_submit_request_sg(struct ps3_storage_device *dev,
 
 #ifdef DEBUG
 	unsigned int n = 0;
-	struct bio_vec *bv;
+	struct bio_vec bv;
 	struct req_iterator iter;
 
 	rq_for_each_segment(bv, req, iter)
diff --git a/drivers/block/ps3vram.c b/drivers/block/ps3vram.c
index 06a2e53e5f37..ef45cfb98fd2 100644
--- a/drivers/block/ps3vram.c
+++ b/drivers/block/ps3vram.c
@@ -553,16 +553,16 @@ static struct bio *ps3vram_do_bio(struct ps3_system_bus_device *dev,
 	struct ps3vram_priv *priv = ps3_system_bus_get_drvdata(dev);
 	int write = bio_data_dir(bio) == WRITE;
 	const char *op = write ? "write" : "read";
-	loff_t offset = bio->bi_sector << 9;
+	loff_t offset = bio->bi_iter.bi_sector << 9;
 	int error = 0;
-	struct bio_vec *bvec;
-	unsigned int i;
+	struct bio_vec bvec;
+	struct bvec_iter iter;
 	struct bio *next;
 
-	bio_for_each_segment(bvec, bio, i) {
+	bio_for_each_segment(bvec, bio, iter) {
 		/* PS3 is ppc64, so we don't handle highmem */
-		char *ptr = page_address(bvec->bv_page) + bvec->bv_offset;
-		size_t len = bvec->bv_len, retlen;
+		char *ptr = page_address(bvec.bv_page) + bvec.bv_offset;
+		size_t len = bvec.bv_len, retlen;
 
 		dev_dbg(&dev->core, "    %s %zu bytes at offset %llu\n", op,
 			len, offset);
diff --git a/drivers/block/rbd.c b/drivers/block/rbd.c
index 16cab6635163..b365e0dfccb6 100644
--- a/drivers/block/rbd.c
+++ b/drivers/block/rbd.c
@@ -1156,23 +1156,23 @@ static void bio_chain_put(struct bio *chain)
  */
 static void zero_bio_chain(struct bio *chain, int start_ofs)
 {
-	struct bio_vec *bv;
+	struct bio_vec bv;
+	struct bvec_iter iter;
 	unsigned long flags;
 	void *buf;
-	int i;
 	int pos = 0;
 
 	while (chain) {
-		bio_for_each_segment(bv, chain, i) {
-			if (pos + bv->bv_len > start_ofs) {
+		bio_for_each_segment(bv, chain, iter) {
+			if (pos + bv.bv_len > start_ofs) {
 				int remainder = max(start_ofs - pos, 0);
-				buf = bvec_kmap_irq(bv, &flags);
+				buf = bvec_kmap_irq(&bv, &flags);
 				memset(buf + remainder, 0,
-				       bv->bv_len - remainder);
-				flush_dcache_page(bv->bv_page);
+				       bv.bv_len - remainder);
+				flush_dcache_page(bv.bv_page);
 				bvec_kunmap_irq(buf, &flags);
 			}
-			pos += bv->bv_len;
+			pos += bv.bv_len;
 		}
 
 		chain = chain->bi_next;
@@ -1220,74 +1220,14 @@ static struct bio *bio_clone_range(struct bio *bio_src,
 					unsigned int len,
 					gfp_t gfpmask)
 {
-	struct bio_vec *bv;
-	unsigned int resid;
-	unsigned short idx;
-	unsigned int voff;
-	unsigned short end_idx;
-	unsigned short vcnt;
 	struct bio *bio;
 
-	/* Handle the easy case for the caller */
-
-	if (!offset && len == bio_src->bi_size)
-		return bio_clone(bio_src, gfpmask);
-
-	if (WARN_ON_ONCE(!len))
-		return NULL;
-	if (WARN_ON_ONCE(len > bio_src->bi_size))
-		return NULL;
-	if (WARN_ON_ONCE(offset > bio_src->bi_size - len))
-		return NULL;
-
-	/* Find first affected segment... */
-
-	resid = offset;
-	bio_for_each_segment(bv, bio_src, idx) {
-		if (resid < bv->bv_len)
-			break;
-		resid -= bv->bv_len;
-	}
-	voff = resid;
-
-	/* ...and the last affected segment */
-
-	resid += len;
-	__bio_for_each_segment(bv, bio_src, end_idx, idx) {
-		if (resid <= bv->bv_len)
-			break;
-		resid -= bv->bv_len;
-	}
-	vcnt = end_idx - idx + 1;
-
-	/* Build the clone */
-
-	bio = bio_alloc(gfpmask, (unsigned int) vcnt);
+	bio = bio_clone(bio_src, gfpmask);
 	if (!bio)
 		return NULL;	/* ENOMEM */
 
-	bio->bi_bdev = bio_src->bi_bdev;
-	bio->bi_sector = bio_src->bi_sector + (offset >> SECTOR_SHIFT);
-	bio->bi_rw = bio_src->bi_rw;
-	bio->bi_flags |= 1 << BIO_CLONED;
-
-	/*
-	 * Copy over our part of the bio_vec, then update the first
-	 * and last (or only) entries.
-	 */
-	memcpy(&bio->bi_io_vec[0], &bio_src->bi_io_vec[idx],
-			vcnt * sizeof (struct bio_vec));
-	bio->bi_io_vec[0].bv_offset += voff;
-	if (vcnt > 1) {
-		bio->bi_io_vec[0].bv_len -= voff;
-		bio->bi_io_vec[vcnt - 1].bv_len = resid;
-	} else {
-		bio->bi_io_vec[0].bv_len = len;
-	}
-
-	bio->bi_vcnt = vcnt;
-	bio->bi_size = len;
-	bio->bi_idx = 0;
+	bio_advance(bio, offset);
+	bio->bi_iter.bi_size = len;
 
 	return bio;
 }
@@ -1318,7 +1258,7 @@ static struct bio *bio_chain_clone_range(struct bio **bio_src,
 
 	/* Build up a chain of clone bios up to the limit */
 
-	if (!bi || off >= bi->bi_size || !len)
+	if (!bi || off >= bi->bi_iter.bi_size || !len)
 		return NULL;		/* Nothing to clone */
 
 	end = &chain;
@@ -1330,7 +1270,7 @@ static struct bio *bio_chain_clone_range(struct bio **bio_src,
 			rbd_warn(NULL, "bio_chain exhausted with %u left", len);
 			goto out_err;	/* EINVAL; ran out of bio's */
 		}
-		bi_size = min_t(unsigned int, bi->bi_size - off, len);
+		bi_size = min_t(unsigned int, bi->bi_iter.bi_size - off, len);
 		bio = bio_clone_range(bi, off, bi_size, gfpmask);
 		if (!bio)
 			goto out_err;	/* ENOMEM */
@@ -1339,7 +1279,7 @@ static struct bio *bio_chain_clone_range(struct bio **bio_src,
 		end = &bio->bi_next;
 
 		off += bi_size;
-		if (off == bi->bi_size) {
+		if (off == bi->bi_iter.bi_size) {
 			bi = bi->bi_next;
 			off = 0;
 		}
@@ -2227,7 +2167,8 @@ static int rbd_img_request_fill(struct rbd_img_request *img_request,
 
 	if (type == OBJ_REQUEST_BIO) {
 		bio_list = data_desc;
-		rbd_assert(img_offset == bio_list->bi_sector << SECTOR_SHIFT);
+		rbd_assert(img_offset ==
+			   bio_list->bi_iter.bi_sector << SECTOR_SHIFT);
 	} else {
 		rbd_assert(type == OBJ_REQUEST_PAGES);
 		pages = data_desc;
diff --git a/drivers/block/rsxx/dev.c b/drivers/block/rsxx/dev.c
index 2284f5d3a54a..2839d37e5af7 100644
--- a/drivers/block/rsxx/dev.c
+++ b/drivers/block/rsxx/dev.c
@@ -174,7 +174,7 @@ static void rsxx_make_request(struct request_queue *q, struct bio *bio)
 	if (!card)
 		goto req_err;
 
-	if (bio->bi_sector + (bio->bi_size >> 9) > get_capacity(card->gendisk))
+	if (bio_end_sector(bio) > get_capacity(card->gendisk))
 		goto req_err;
 
 	if (unlikely(card->halt)) {
@@ -187,7 +187,7 @@ static void rsxx_make_request(struct request_queue *q, struct bio *bio)
 		goto req_err;
 	}
 
-	if (bio->bi_size == 0) {
+	if (bio->bi_iter.bi_size == 0) {
 		dev_err(CARD_TO_DEV(card), "size zero BIO!\n");
 		goto req_err;
 	}
@@ -208,7 +208,7 @@ static void rsxx_make_request(struct request_queue *q, struct bio *bio)
 
 	dev_dbg(CARD_TO_DEV(card), "BIO[%c]: meta: %p addr8: x%llx size: %d\n",
 		 bio_data_dir(bio) ? 'W' : 'R', bio_meta,
-		 (u64)bio->bi_sector << 9, bio->bi_size);
+		 (u64)bio->bi_iter.bi_sector << 9, bio->bi_iter.bi_size);
 
 	st = rsxx_dma_queue_bio(card, bio, &bio_meta->pending_dmas,
 				    bio_dma_done_cb, bio_meta);
diff --git a/drivers/block/rsxx/dma.c b/drivers/block/rsxx/dma.c
index fc88ba3e1bd2..cf8cd293abb5 100644
--- a/drivers/block/rsxx/dma.c
+++ b/drivers/block/rsxx/dma.c
@@ -684,7 +684,8 @@ int rsxx_dma_queue_bio(struct rsxx_cardinfo *card,
 			   void *cb_data)
 {
 	struct list_head dma_list[RSXX_MAX_TARGETS];
-	struct bio_vec *bvec;
+	struct bio_vec bvec;
+	struct bvec_iter iter;
 	unsigned long long addr8;
 	unsigned int laddr;
 	unsigned int bv_len;
@@ -696,7 +697,7 @@ int rsxx_dma_queue_bio(struct rsxx_cardinfo *card,
 	int st;
 	int i;
 
-	addr8 = bio->bi_sector << 9; /* sectors are 512 bytes */
+	addr8 = bio->bi_iter.bi_sector << 9; /* sectors are 512 bytes */
 	atomic_set(n_dmas, 0);
 
 	for (i = 0; i < card->n_targets; i++) {
@@ -705,7 +706,7 @@ int rsxx_dma_queue_bio(struct rsxx_cardinfo *card,
 	}
 
 	if (bio->bi_rw & REQ_DISCARD) {
-		bv_len = bio->bi_size;
+		bv_len = bio->bi_iter.bi_size;
 
 		while (bv_len > 0) {
 			tgt   = rsxx_get_dma_tgt(card, addr8);
@@ -722,9 +723,9 @@ int rsxx_dma_queue_bio(struct rsxx_cardinfo *card,
 			bv_len -= RSXX_HW_BLK_SIZE;
 		}
 	} else {
-		bio_for_each_segment(bvec, bio, i) {
-			bv_len = bvec->bv_len;
-			bv_off = bvec->bv_offset;
+		bio_for_each_segment(bvec, bio, iter) {
+			bv_len = bvec.bv_len;
+			bv_off = bvec.bv_offset;
 
 			while (bv_len > 0) {
 				tgt   = rsxx_get_dma_tgt(card, addr8);
@@ -736,7 +737,7 @@ int rsxx_dma_queue_bio(struct rsxx_cardinfo *card,
 				st = rsxx_queue_dma(card, &dma_list[tgt],
 							bio_data_dir(bio),
 							dma_off, dma_len,
-							laddr, bvec->bv_page,
+							laddr, bvec.bv_page,
 							bv_off, cb, cb_data);
 				if (st)
 					goto bvec_err;
diff --git a/drivers/block/sx8.c b/drivers/block/sx8.c
index 3fb6ab4c8b4e..d5e2d12b9d9e 100644
--- a/drivers/block/sx8.c
+++ b/drivers/block/sx8.c
@@ -1744,20 +1744,6 @@ static void carm_remove_one (struct pci_dev *pdev)
 	kfree(host);
 	pci_release_regions(pdev);
 	pci_disable_device(pdev);
-	pci_set_drvdata(pdev, NULL);
 }
 
-static int __init carm_init(void)
-{
-	return pci_register_driver(&carm_driver);
-}
-
-static void __exit carm_exit(void)
-{
-	pci_unregister_driver(&carm_driver);
-}
-
-module_init(carm_init);
-module_exit(carm_exit);
-
-
+module_pci_driver(carm_driver);
diff --git a/drivers/block/umem.c b/drivers/block/umem.c
index ad70868f8a96..4cf81b5bf0f7 100644
--- a/drivers/block/umem.c
+++ b/drivers/block/umem.c
@@ -108,8 +108,7 @@ struct cardinfo {
 				    * have been written
 				    */
 	struct bio	*bio, *currentbio, **biotail;
-	int		current_idx;
-	sector_t	current_sector;
+	struct bvec_iter current_iter;
 
 	struct request_queue *queue;
 
@@ -118,7 +117,7 @@ struct cardinfo {
 		struct mm_dma_desc	*desc;
 		int	 		cnt, headcnt;
 		struct bio		*bio, **biotail;
-		int			idx;
+		struct bvec_iter	iter;
 	} mm_pages[2];
 #define DESC_PER_PAGE ((PAGE_SIZE*2)/sizeof(struct mm_dma_desc))
 
@@ -344,16 +343,13 @@ static int add_bio(struct cardinfo *card)
 	dma_addr_t dma_handle;
 	int offset;
 	struct bio *bio;
-	struct bio_vec *vec;
-	int idx;
+	struct bio_vec vec;
 	int rw;
-	int len;
 
 	bio = card->currentbio;
 	if (!bio && card->bio) {
 		card->currentbio = card->bio;
-		card->current_idx = card->bio->bi_idx;
-		card->current_sector = card->bio->bi_sector;
+		card->current_iter = card->bio->bi_iter;
 		card->bio = card->bio->bi_next;
 		if (card->bio == NULL)
 			card->biotail = &card->bio;
@@ -362,18 +358,17 @@ static int add_bio(struct cardinfo *card)
 	}
 	if (!bio)
 		return 0;
-	idx = card->current_idx;
 
 	rw = bio_rw(bio);
 	if (card->mm_pages[card->Ready].cnt >= DESC_PER_PAGE)
 		return 0;
 
-	vec = bio_iovec_idx(bio, idx);
-	len = vec->bv_len;
+	vec = bio_iter_iovec(bio, card->current_iter);
+
 	dma_handle = pci_map_page(card->dev,
-				  vec->bv_page,
-				  vec->bv_offset,
-				  len,
+				  vec.bv_page,
+				  vec.bv_offset,
+				  vec.bv_len,
 				  (rw == READ) ?
 				  PCI_DMA_FROMDEVICE : PCI_DMA_TODEVICE);
 
@@ -381,7 +376,7 @@ static int add_bio(struct cardinfo *card)
 	desc = &p->desc[p->cnt];
 	p->cnt++;
 	if (p->bio == NULL)
-		p->idx = idx;
+		p->iter = card->current_iter;
 	if ((p->biotail) != &bio->bi_next) {
 		*(p->biotail) = bio;
 		p->biotail = &(bio->bi_next);
@@ -391,8 +386,8 @@ static int add_bio(struct cardinfo *card)
 	desc->data_dma_handle = dma_handle;
 
 	desc->pci_addr = cpu_to_le64((u64)desc->data_dma_handle);
-	desc->local_addr = cpu_to_le64(card->current_sector << 9);
-	desc->transfer_size = cpu_to_le32(len);
+	desc->local_addr = cpu_to_le64(card->current_iter.bi_sector << 9);
+	desc->transfer_size = cpu_to_le32(vec.bv_len);
 	offset = (((char *)&desc->sem_control_bits) - ((char *)p->desc));
 	desc->sem_addr = cpu_to_le64((u64)(p->page_dma+offset));
 	desc->zero1 = desc->zero2 = 0;
@@ -407,10 +402,9 @@ static int add_bio(struct cardinfo *card)
 		desc->control_bits |= cpu_to_le32(DMASCR_TRANSFER_READ);
 	desc->sem_control_bits = desc->control_bits;
 
-	card->current_sector += (len >> 9);
-	idx++;
-	card->current_idx = idx;
-	if (idx >= bio->bi_vcnt)
+
+	bio_advance_iter(bio, &card->current_iter, vec.bv_len);
+	if (!card->current_iter.bi_size)
 		card->currentbio = NULL;
 
 	return 1;
@@ -439,23 +433,25 @@ static void process_page(unsigned long data)
 		struct mm_dma_desc *desc = &page->desc[page->headcnt];
 		int control = le32_to_cpu(desc->sem_control_bits);
 		int last = 0;
-		int idx;
+		struct bio_vec vec;
 
 		if (!(control & DMASCR_DMA_COMPLETE)) {
 			control = dma_status;
 			last = 1;
 		}
+
 		page->headcnt++;
-		idx = page->idx;
-		page->idx++;
-		if (page->idx >= bio->bi_vcnt) {
+		vec = bio_iter_iovec(bio, page->iter);
+		bio_advance_iter(bio, &page->iter, vec.bv_len);
+
+		if (!page->iter.bi_size) {
 			page->bio = bio->bi_next;
 			if (page->bio)
-				page->idx = page->bio->bi_idx;
+				page->iter = page->bio->bi_iter;
 		}
 
 		pci_unmap_page(card->dev, desc->data_dma_handle,
-			       bio_iovec_idx(bio, idx)->bv_len,
+			       vec.bv_len,
 				 (control & DMASCR_TRANSFER_READ) ?
 				PCI_DMA_TODEVICE : PCI_DMA_FROMDEVICE);
 		if (control & DMASCR_HARD_ERROR) {
@@ -532,7 +528,8 @@ static void mm_make_request(struct request_queue *q, struct bio *bio)
 {
 	struct cardinfo *card = q->queuedata;
 	pr_debug("mm_make_request %llu %u\n",
-		 (unsigned long long)bio->bi_sector, bio->bi_size);
+		 (unsigned long long)bio->bi_iter.bi_sector,
+		 bio->bi_iter.bi_size);
 
 	spin_lock_irq(&card->lock);
 	*card->biotail = bio;
diff --git a/drivers/block/xen-blkback/blkback.c b/drivers/block/xen-blkback/blkback.c
index 6620b73d0490..4b97b86da926 100644
--- a/drivers/block/xen-blkback/blkback.c
+++ b/drivers/block/xen-blkback/blkback.c
@@ -1257,7 +1257,7 @@ static int dispatch_rw_block_io(struct xen_blkif *blkif,
 			bio->bi_bdev    = preq.bdev;
 			bio->bi_private = pending_req;
 			bio->bi_end_io  = end_block_io_op;
-			bio->bi_sector  = preq.sector_number;
+			bio->bi_iter.bi_sector  = preq.sector_number;
 		}
 
 		preq.sector_number += seg[i].nsec;
diff --git a/drivers/block/xen-blkfront.c b/drivers/block/xen-blkfront.c
index f9c43f91f03e..8dcfb54f1603 100644
--- a/drivers/block/xen-blkfront.c
+++ b/drivers/block/xen-blkfront.c
@@ -1547,7 +1547,7 @@ static int blkif_recover(struct blkfront_info *info)
 			for (i = 0; i < pending; i++) {
 				offset = (i * segs * PAGE_SIZE) >> 9;
 				size = min((unsigned int)(segs * PAGE_SIZE) >> 9,
-					   (unsigned int)(bio->bi_size >> 9) - offset);
+					   (unsigned int)bio_sectors(bio) - offset);
 				cloned_bio = bio_clone(bio, GFP_NOIO);
 				BUG_ON(cloned_bio == NULL);
 				bio_trim(cloned_bio, offset, size);
diff --git a/drivers/staging/zram/Kconfig b/drivers/block/zram/Kconfig
index 983314c41349..3450be850399 100644
--- a/drivers/staging/zram/Kconfig
+++ b/drivers/block/zram/Kconfig
@@ -14,7 +14,6 @@ config ZRAM
 	  disks and maybe many more.
 
 	  See zram.txt for more information.
-	  Project home: <https://compcache.googlecode.com/>
 
 config ZRAM_DEBUG
 	bool "Compressed RAM block device debug support"
diff --git a/drivers/staging/zram/Makefile b/drivers/block/zram/Makefile
index cb0f9ced6a93..cb0f9ced6a93 100644
--- a/drivers/staging/zram/Makefile
+++ b/drivers/block/zram/Makefile
diff --git a/drivers/staging/zram/zram_drv.c b/drivers/block/zram/zram_drv.c
index 3277d9838f4e..011e55d820b1 100644
--- a/drivers/staging/zram/zram_drv.c
+++ b/drivers/block/zram/zram_drv.c
@@ -2,6 +2,7 @@
  * Compressed RAM block device
  *
  * Copyright (C) 2008, 2009, 2010  Nitin Gupta
+ *               2012, 2013 Minchan Kim
  *
  * This code is released using a dual license strategy: BSD/GPL
  * You can choose the licence that better fits your requirements.
@@ -9,7 +10,6 @@
  * Released under the terms of 3-clause BSD License
  * Released under the terms of GNU General Public License Version 2.0
  *
- * Project home: http://compcache.googlecode.com
  */
 
 #define KMSG_COMPONENT "zram"
@@ -104,7 +104,7 @@ static ssize_t zero_pages_show(struct device *dev,
 {
 	struct zram *zram = dev_to_zram(dev);
 
-	return sprintf(buf, "%u\n", zram->stats.pages_zero);
+	return sprintf(buf, "%u\n", atomic_read(&zram->stats.pages_zero));
 }
 
 static ssize_t orig_data_size_show(struct device *dev,
@@ -113,7 +113,7 @@ static ssize_t orig_data_size_show(struct device *dev,
 	struct zram *zram = dev_to_zram(dev);
 
 	return sprintf(buf, "%llu\n",
-		(u64)(zram->stats.pages_stored) << PAGE_SHIFT);
+		(u64)(atomic_read(&zram->stats.pages_stored)) << PAGE_SHIFT);
 }
 
 static ssize_t compr_data_size_show(struct device *dev,
@@ -140,6 +140,7 @@ static ssize_t mem_used_total_show(struct device *dev,
 	return sprintf(buf, "%llu\n", val);
 }
 
+/* flag operations needs meta->tb_lock */
 static int zram_test_flag(struct zram_meta *meta, u32 index,
 			enum zram_pageflags flag)
 {
@@ -171,13 +172,14 @@ static inline int valid_io_request(struct zram *zram, struct bio *bio)
 	u64 start, end, bound;
 
 	/* unaligned request */
-	if (unlikely(bio->bi_sector & (ZRAM_SECTOR_PER_LOGICAL_BLOCK - 1)))
+	if (unlikely(bio->bi_iter.bi_sector &
+		     (ZRAM_SECTOR_PER_LOGICAL_BLOCK - 1)))
 		return 0;
-	if (unlikely(bio->bi_size & (ZRAM_LOGICAL_BLOCK_SIZE - 1)))
+	if (unlikely(bio->bi_iter.bi_size & (ZRAM_LOGICAL_BLOCK_SIZE - 1)))
 		return 0;
 
-	start = bio->bi_sector;
-	end = start + (bio->bi_size >> SECTOR_SHIFT);
+	start = bio->bi_iter.bi_sector;
+	end = start + (bio->bi_iter.bi_size >> SECTOR_SHIFT);
 	bound = zram->disksize >> SECTOR_SHIFT;
 	/* out of range range */
 	if (unlikely(start >= bound || end > bound || start > end))
@@ -227,6 +229,8 @@ static struct zram_meta *zram_meta_alloc(u64 disksize)
 		goto free_table;
 	}
 
+	rwlock_init(&meta->tb_lock);
+	mutex_init(&meta->buffer_lock);
 	return meta;
 
 free_table:
@@ -279,6 +283,7 @@ static void handle_zero_page(struct bio_vec *bvec)
 	flush_dcache_page(page);
 }
 
+/* NOTE: caller should hold meta->tb_lock with write-side */
 static void zram_free_page(struct zram *zram, size_t index)
 {
 	struct zram_meta *meta = zram->meta;
@@ -292,21 +297,21 @@ static void zram_free_page(struct zram *zram, size_t index)
 		 */
 		if (zram_test_flag(meta, index, ZRAM_ZERO)) {
 			zram_clear_flag(meta, index, ZRAM_ZERO);
-			zram->stats.pages_zero--;
+			atomic_dec(&zram->stats.pages_zero);
 		}
 		return;
 	}
 
 	if (unlikely(size > max_zpage_size))
-		zram->stats.bad_compress--;
+		atomic_dec(&zram->stats.bad_compress);
 
 	zs_free(meta->mem_pool, handle);
 
 	if (size <= PAGE_SIZE / 2)
-		zram->stats.good_compress--;
+		atomic_dec(&zram->stats.good_compress);
 
 	atomic64_sub(meta->table[index].size, &zram->stats.compr_size);
-	zram->stats.pages_stored--;
+	atomic_dec(&zram->stats.pages_stored);
 
 	meta->table[index].handle = 0;
 	meta->table[index].size = 0;
@@ -318,20 +323,26 @@ static int zram_decompress_page(struct zram *zram, char *mem, u32 index)
 	size_t clen = PAGE_SIZE;
 	unsigned char *cmem;
 	struct zram_meta *meta = zram->meta;
-	unsigned long handle = meta->table[index].handle;
+	unsigned long handle;
+	u16 size;
+
+	read_lock(&meta->tb_lock);
+	handle = meta->table[index].handle;
+	size = meta->table[index].size;
 
 	if (!handle || zram_test_flag(meta, index, ZRAM_ZERO)) {
+		read_unlock(&meta->tb_lock);
 		clear_page(mem);
 		return 0;
 	}
 
 	cmem = zs_map_object(meta->mem_pool, handle, ZS_MM_RO);
-	if (meta->table[index].size == PAGE_SIZE)
+	if (size == PAGE_SIZE)
 		copy_page(mem, cmem);
 	else
-		ret = lzo1x_decompress_safe(cmem, meta->table[index].size,
-						mem, &clen);
+		ret = lzo1x_decompress_safe(cmem, size,	mem, &clen);
 	zs_unmap_object(meta->mem_pool, handle);
+	read_unlock(&meta->tb_lock);
 
 	/* Should NEVER happen. Return bio error if it does. */
 	if (unlikely(ret != LZO_E_OK)) {
@@ -352,11 +363,14 @@ static int zram_bvec_read(struct zram *zram, struct bio_vec *bvec,
 	struct zram_meta *meta = zram->meta;
 	page = bvec->bv_page;
 
+	read_lock(&meta->tb_lock);
 	if (unlikely(!meta->table[index].handle) ||
 			zram_test_flag(meta, index, ZRAM_ZERO)) {
+		read_unlock(&meta->tb_lock);
 		handle_zero_page(bvec);
 		return 0;
 	}
+	read_unlock(&meta->tb_lock);
 
 	if (is_partial_io(bvec))
 		/* Use  a temporary buffer to decompress the page */
@@ -399,6 +413,7 @@ static int zram_bvec_write(struct zram *zram, struct bio_vec *bvec, u32 index,
 	struct page *page;
 	unsigned char *user_mem, *cmem, *src, *uncmem = NULL;
 	struct zram_meta *meta = zram->meta;
+	bool locked = false;
 
 	page = bvec->bv_page;
 	src = meta->compress_buffer;
@@ -418,6 +433,8 @@ static int zram_bvec_write(struct zram *zram, struct bio_vec *bvec, u32 index,
 			goto out;
 	}
 
+	mutex_lock(&meta->buffer_lock);
+	locked = true;
 	user_mem = kmap_atomic(page);
 
 	if (is_partial_io(bvec)) {
@@ -432,25 +449,18 @@ static int zram_bvec_write(struct zram *zram, struct bio_vec *bvec, u32 index,
 	if (page_zero_filled(uncmem)) {
 		kunmap_atomic(user_mem);
 		/* Free memory associated with this sector now. */
+		write_lock(&zram->meta->tb_lock);
 		zram_free_page(zram, index);
-
-		zram->stats.pages_zero++;
 		zram_set_flag(meta, index, ZRAM_ZERO);
+		write_unlock(&zram->meta->tb_lock);
+
+		atomic_inc(&zram->stats.pages_zero);
 		ret = 0;
 		goto out;
 	}
 
-	/*
-	 * zram_slot_free_notify could miss free so that let's
-	 * double check.
-	 */
-	if (unlikely(meta->table[index].handle ||
-			zram_test_flag(meta, index, ZRAM_ZERO)))
-		zram_free_page(zram, index);
-
 	ret = lzo1x_1_compress(uncmem, PAGE_SIZE, src, &clen,
 			       meta->compress_workmem);
-
 	if (!is_partial_io(bvec)) {
 		kunmap_atomic(user_mem);
 		user_mem = NULL;
@@ -463,7 +473,7 @@ static int zram_bvec_write(struct zram *zram, struct bio_vec *bvec, u32 index,
 	}
 
 	if (unlikely(clen > max_zpage_size)) {
-		zram->stats.bad_compress++;
+		atomic_inc(&zram->stats.bad_compress);
 		clen = PAGE_SIZE;
 		src = NULL;
 		if (is_partial_io(bvec))
@@ -493,18 +503,22 @@ static int zram_bvec_write(struct zram *zram, struct bio_vec *bvec, u32 index,
 	 * Free memory associated with this sector
 	 * before overwriting unused sectors.
 	 */
+	write_lock(&zram->meta->tb_lock);
 	zram_free_page(zram, index);
 
 	meta->table[index].handle = handle;
 	meta->table[index].size = clen;
+	write_unlock(&zram->meta->tb_lock);
 
 	/* Update stats */
 	atomic64_add(clen, &zram->stats.compr_size);
-	zram->stats.pages_stored++;
+	atomic_inc(&zram->stats.pages_stored);
 	if (clen <= PAGE_SIZE / 2)
-		zram->stats.good_compress++;
+		atomic_inc(&zram->stats.good_compress);
 
 out:
+	if (locked)
+		mutex_unlock(&meta->buffer_lock);
 	if (is_partial_io(bvec))
 		kfree(uncmem);
 
@@ -513,36 +527,15 @@ out:
 	return ret;
 }
 
-static void handle_pending_slot_free(struct zram *zram)
-{
-	struct zram_slot_free *free_rq;
-
-	spin_lock(&zram->slot_free_lock);
-	while (zram->slot_free_rq) {
-		free_rq = zram->slot_free_rq;
-		zram->slot_free_rq = free_rq->next;
-		zram_free_page(zram, free_rq->index);
-		kfree(free_rq);
-	}
-	spin_unlock(&zram->slot_free_lock);
-}
-
 static int zram_bvec_rw(struct zram *zram, struct bio_vec *bvec, u32 index,
 			int offset, struct bio *bio, int rw)
 {
 	int ret;
 
-	if (rw == READ) {
-		down_read(&zram->lock);
-		handle_pending_slot_free(zram);
+	if (rw == READ)
 		ret = zram_bvec_read(zram, bvec, index, offset, bio);
-		up_read(&zram->lock);
-	} else {
-		down_write(&zram->lock);
-		handle_pending_slot_free(zram);
+	else
 		ret = zram_bvec_write(zram, bvec, index, offset);
-		up_write(&zram->lock);
-	}
 
 	return ret;
 }
@@ -552,8 +545,6 @@ static void zram_reset_device(struct zram *zram, bool reset_capacity)
 	size_t index;
 	struct zram_meta *meta;
 
-	flush_work(&zram->free_work);
-
 	down_write(&zram->init_lock);
 	if (!zram->init_done) {
 		up_write(&zram->init_lock);
@@ -680,9 +671,10 @@ out:
 
 static void __zram_make_request(struct zram *zram, struct bio *bio, int rw)
 {
-	int i, offset;
+	int offset;
 	u32 index;
-	struct bio_vec *bvec;
+	struct bio_vec bvec;
+	struct bvec_iter iter;
 
 	switch (rw) {
 	case READ:
@@ -693,36 +685,37 @@ static void __zram_make_request(struct zram *zram, struct bio *bio, int rw)
 		break;
 	}
 
-	index = bio->bi_sector >> SECTORS_PER_PAGE_SHIFT;
-	offset = (bio->bi_sector & (SECTORS_PER_PAGE - 1)) << SECTOR_SHIFT;
+	index = bio->bi_iter.bi_sector >> SECTORS_PER_PAGE_SHIFT;
+	offset = (bio->bi_iter.bi_sector &
+		  (SECTORS_PER_PAGE - 1)) << SECTOR_SHIFT;
 
-	bio_for_each_segment(bvec, bio, i) {
+	bio_for_each_segment(bvec, bio, iter) {
 		int max_transfer_size = PAGE_SIZE - offset;
 
-		if (bvec->bv_len > max_transfer_size) {
+		if (bvec.bv_len > max_transfer_size) {
 			/*
 			 * zram_bvec_rw() can only make operation on a single
 			 * zram page. Split the bio vector.
 			 */
 			struct bio_vec bv;
 
-			bv.bv_page = bvec->bv_page;
+			bv.bv_page = bvec.bv_page;
 			bv.bv_len = max_transfer_size;
-			bv.bv_offset = bvec->bv_offset;
+			bv.bv_offset = bvec.bv_offset;
 
 			if (zram_bvec_rw(zram, &bv, index, offset, bio, rw) < 0)
 				goto out;
 
-			bv.bv_len = bvec->bv_len - max_transfer_size;
+			bv.bv_len = bvec.bv_len - max_transfer_size;
 			bv.bv_offset += max_transfer_size;
 			if (zram_bvec_rw(zram, &bv, index+1, 0, bio, rw) < 0)
 				goto out;
 		} else
-			if (zram_bvec_rw(zram, bvec, index, offset, bio, rw)
+			if (zram_bvec_rw(zram, &bvec, index, offset, bio, rw)
 			    < 0)
 				goto out;
 
-		update_position(&index, &offset, bvec);
+		update_position(&index, &offset, &bvec);
 	}
 
 	set_bit(BIO_UPTODATE, &bio->bi_flags);
@@ -759,40 +752,19 @@ error:
 	bio_io_error(bio);
 }
 
-static void zram_slot_free(struct work_struct *work)
-{
-	struct zram *zram;
-
-	zram = container_of(work, struct zram, free_work);
-	down_write(&zram->lock);
-	handle_pending_slot_free(zram);
-	up_write(&zram->lock);
-}
-
-static void add_slot_free(struct zram *zram, struct zram_slot_free *free_rq)
-{
-	spin_lock(&zram->slot_free_lock);
-	free_rq->next = zram->slot_free_rq;
-	zram->slot_free_rq = free_rq;
-	spin_unlock(&zram->slot_free_lock);
-}
-
 static void zram_slot_free_notify(struct block_device *bdev,
 				unsigned long index)
 {
 	struct zram *zram;
-	struct zram_slot_free *free_rq;
+	struct zram_meta *meta;
 
 	zram = bdev->bd_disk->private_data;
-	atomic64_inc(&zram->stats.notify_free);
-
-	free_rq = kmalloc(sizeof(struct zram_slot_free), GFP_ATOMIC);
-	if (!free_rq)
-		return;
+	meta = zram->meta;
 
-	free_rq->index = index;
-	add_slot_free(zram, free_rq);
-	schedule_work(&zram->free_work);
+	write_lock(&meta->tb_lock);
+	zram_free_page(zram, index);
+	write_unlock(&meta->tb_lock);
+	atomic64_inc(&zram->stats.notify_free);
 }
 
 static const struct block_device_operations zram_devops = {
@@ -836,13 +808,8 @@ static int create_device(struct zram *zram, int device_id)
 {
 	int ret = -ENOMEM;
 
-	init_rwsem(&zram->lock);
 	init_rwsem(&zram->init_lock);
 
-	INIT_WORK(&zram->free_work, zram_slot_free);
-	spin_lock_init(&zram->slot_free_lock);
-	zram->slot_free_rq = NULL;
-
 	zram->queue = blk_alloc_queue(GFP_KERNEL);
 	if (!zram->queue) {
 		pr_err("Error allocating disk queue for device %d\n",
diff --git a/drivers/staging/zram/zram_drv.h b/drivers/block/zram/zram_drv.h
index 97a3acf6ab76..ad8aa35bae00 100644
--- a/drivers/staging/zram/zram_drv.h
+++ b/drivers/block/zram/zram_drv.h
@@ -2,6 +2,7 @@
  * Compressed RAM block device
  *
  * Copyright (C) 2008, 2009, 2010  Nitin Gupta
+ *               2012, 2013 Minchan Kim
  *
  * This code is released using a dual license strategy: BSD/GPL
  * You can choose the licence that better fits your requirements.
@@ -9,7 +10,6 @@
  * Released under the terms of 3-clause BSD License
  * Released under the terms of GNU General Public License Version 2.0
  *
- * Project home: http://compcache.googlecode.com
  */
 
 #ifndef _ZRAM_DRV_H_
@@ -17,8 +17,7 @@
 
 #include <linux/spinlock.h>
 #include <linux/mutex.h>
-
-#include "../zsmalloc/zsmalloc.h"
+#include <linux/zsmalloc.h>
 
 /*
  * Some arbitrary value. This is just to catch
@@ -69,10 +68,6 @@ struct table {
 	u8 flags;
 } __aligned(4);
 
-/*
- * All 64bit fields should only be manipulated by 64bit atomic accessors.
- * All modifications to 32bit counter should be protected by zram->lock.
- */
 struct zram_stats {
 	atomic64_t compr_size;	/* compressed size of pages stored */
 	atomic64_t num_reads;	/* failed + successful */
@@ -81,33 +76,23 @@ struct zram_stats {
 	atomic64_t failed_writes;	/* can happen when memory is too low */
 	atomic64_t invalid_io;	/* non-page-aligned I/O requests */
 	atomic64_t notify_free;	/* no. of swap slot free notifications */
-	u32 pages_zero;		/* no. of zero filled pages */
-	u32 pages_stored;	/* no. of pages currently stored */
-	u32 good_compress;	/* % of pages with compression ratio<=50% */
-	u32 bad_compress;	/* % of pages with compression ratio>=75% */
+	atomic_t pages_zero;		/* no. of zero filled pages */
+	atomic_t pages_stored;	/* no. of pages currently stored */
+	atomic_t good_compress;	/* % of pages with compression ratio<=50% */
+	atomic_t bad_compress;	/* % of pages with compression ratio>=75% */
 };
 
 struct zram_meta {
+	rwlock_t tb_lock;	/* protect table */
 	void *compress_workmem;
 	void *compress_buffer;
 	struct table *table;
 	struct zs_pool *mem_pool;
-};
-
-struct zram_slot_free {
-	unsigned long index;
-	struct zram_slot_free *next;
+	struct mutex buffer_lock; /* protect compress buffers */
 };
 
 struct zram {
 	struct zram_meta *meta;
-	struct rw_semaphore lock; /* protect compression buffers, table,
-				   * 32bit stat counters against concurrent
-				   * notifications, reads and writes */
-
-	struct work_struct free_work;  /* handle pending free request */
-	struct zram_slot_free *slot_free_rq; /* list head of free request */
-
 	struct request_queue *queue;
 	struct gendisk *disk;
 	int init_done;
@@ -118,7 +103,6 @@ struct zram {
 	 * we can store in a disk.
 	 */
 	u64 disksize;	/* bytes */
-	spinlock_t slot_free_lock;
 
 	struct zram_stats stats;
 };
diff --git a/drivers/bluetooth/ath3k.c b/drivers/bluetooth/ath3k.c
index 106d1d8e16ad..4f78a9d39dc8 100644
--- a/drivers/bluetooth/ath3k.c
+++ b/drivers/bluetooth/ath3k.c
@@ -62,50 +62,53 @@ static const struct usb_device_id ath3k_table[] = {
 	{ USB_DEVICE(0x0CF3, 0x3000) },
 
 	/* Atheros AR3011 with sflash firmware*/
+	{ USB_DEVICE(0x0489, 0xE027) },
+	{ USB_DEVICE(0x0489, 0xE03D) },
+	{ USB_DEVICE(0x0930, 0x0215) },
 	{ USB_DEVICE(0x0CF3, 0x3002) },
 	{ USB_DEVICE(0x0CF3, 0xE019) },
 	{ USB_DEVICE(0x13d3, 0x3304) },
-	{ USB_DEVICE(0x0930, 0x0215) },
-	{ USB_DEVICE(0x0489, 0xE03D) },
-	{ USB_DEVICE(0x0489, 0xE027) },
 
 	/* Atheros AR9285 Malbec with sflash firmware */
 	{ USB_DEVICE(0x03F0, 0x311D) },
 
 	/* Atheros AR3012 with sflash firmware*/
-	{ USB_DEVICE(0x0CF3, 0x0036) },
-	{ USB_DEVICE(0x0CF3, 0x3004) },
-	{ USB_DEVICE(0x0CF3, 0x3008) },
-	{ USB_DEVICE(0x0CF3, 0x311D) },
-	{ USB_DEVICE(0x0CF3, 0x817a) },
-	{ USB_DEVICE(0x13d3, 0x3375) },
+	{ USB_DEVICE(0x0489, 0xe04d) },
+	{ USB_DEVICE(0x0489, 0xe04e) },
+	{ USB_DEVICE(0x0489, 0xe057) },
+	{ USB_DEVICE(0x0489, 0xe056) },
+	{ USB_DEVICE(0x0489, 0xe05f) },
+	{ USB_DEVICE(0x04c5, 0x1330) },
 	{ USB_DEVICE(0x04CA, 0x3004) },
 	{ USB_DEVICE(0x04CA, 0x3005) },
 	{ USB_DEVICE(0x04CA, 0x3006) },
 	{ USB_DEVICE(0x04CA, 0x3008) },
 	{ USB_DEVICE(0x04CA, 0x300b) },
-	{ USB_DEVICE(0x13d3, 0x3362) },
-	{ USB_DEVICE(0x0CF3, 0xE004) },
-	{ USB_DEVICE(0x0CF3, 0xE005) },
 	{ USB_DEVICE(0x0930, 0x0219) },
 	{ USB_DEVICE(0x0930, 0x0220) },
-	{ USB_DEVICE(0x0489, 0xe057) },
-	{ USB_DEVICE(0x13d3, 0x3393) },
-	{ USB_DEVICE(0x0489, 0xe04e) },
-	{ USB_DEVICE(0x0489, 0xe056) },
-	{ USB_DEVICE(0x0489, 0xe04d) },
-	{ USB_DEVICE(0x04c5, 0x1330) },
-	{ USB_DEVICE(0x13d3, 0x3402) },
+	{ USB_DEVICE(0x0b05, 0x17d0) },
+	{ USB_DEVICE(0x0CF3, 0x0036) },
+	{ USB_DEVICE(0x0CF3, 0x3004) },
+	{ USB_DEVICE(0x0CF3, 0x3008) },
+	{ USB_DEVICE(0x0CF3, 0x311D) },
+	{ USB_DEVICE(0x0CF3, 0x311E) },
+	{ USB_DEVICE(0x0CF3, 0x311F) },
 	{ USB_DEVICE(0x0cf3, 0x3121) },
+	{ USB_DEVICE(0x0CF3, 0x817a) },
 	{ USB_DEVICE(0x0cf3, 0xe003) },
-	{ USB_DEVICE(0x0489, 0xe05f) },
+	{ USB_DEVICE(0x0CF3, 0xE004) },
+	{ USB_DEVICE(0x0CF3, 0xE005) },
+	{ USB_DEVICE(0x13d3, 0x3362) },
+	{ USB_DEVICE(0x13d3, 0x3375) },
+	{ USB_DEVICE(0x13d3, 0x3393) },
+	{ USB_DEVICE(0x13d3, 0x3402) },
 
 	/* Atheros AR5BBU12 with sflash firmware */
 	{ USB_DEVICE(0x0489, 0xE02C) },
 
 	/* Atheros AR5BBU22 with sflash firmware */
-	{ USB_DEVICE(0x0489, 0xE03C) },
 	{ USB_DEVICE(0x0489, 0xE036) },
+	{ USB_DEVICE(0x0489, 0xE03C) },
 
 	{ }	/* Terminating entry */
 };
@@ -118,36 +121,39 @@ MODULE_DEVICE_TABLE(usb, ath3k_table);
 static const struct usb_device_id ath3k_blist_tbl[] = {
 
 	/* Atheros AR3012 with sflash firmware*/
-	{ USB_DEVICE(0x0CF3, 0x0036), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x0cf3, 0x3004), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x0cf3, 0x3008), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x0cf3, 0x311D), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x0CF3, 0x817a), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x13d3, 0x3375), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0489, 0xe04e), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0489, 0xe04d), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0489, 0xe056), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0489, 0xe057), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0489, 0xe05f), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x04c5, 0x1330), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x3004), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x3005), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x3006), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x3008), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x300b), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x13d3, 0x3362), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x0cf3, 0xe004), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x0cf3, 0xe005), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0930, 0x0219), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0930, 0x0220), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x0489, 0xe057), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x13d3, 0x3393), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x0489, 0xe04e), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x0489, 0xe056), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x0489, 0xe04d), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x04c5, 0x1330), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x13d3, 0x3402), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0b05, 0x17d0), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0CF3, 0x0036), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0cf3, 0x3004), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0cf3, 0x3008), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0cf3, 0x311D), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0cf3, 0x311E), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0cf3, 0x311F), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0cf3, 0x3121), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0CF3, 0x817a), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0cf3, 0xe004), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0cf3, 0xe005), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0cf3, 0xe003), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x0489, 0xe05f), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x13d3, 0x3362), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x13d3, 0x3375), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x13d3, 0x3393), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x13d3, 0x3402), .driver_info = BTUSB_ATH3012 },
 
 	/* Atheros AR5BBU22 with sflash firmware */
-	{ USB_DEVICE(0x0489, 0xE03C), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0489, 0xE036), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0489, 0xE03C), .driver_info = BTUSB_ATH3012 },
 
 	{ }	/* Terminating entry */
 };
diff --git a/drivers/bluetooth/btusb.c b/drivers/bluetooth/btusb.c
index baeaaed299e4..199b9d42489c 100644
--- a/drivers/bluetooth/btusb.c
+++ b/drivers/bluetooth/btusb.c
@@ -101,21 +101,24 @@ static const struct usb_device_id btusb_table[] = {
 	{ USB_DEVICE(0x0c10, 0x0000) },
 
 	/* Broadcom BCM20702A0 */
+	{ USB_DEVICE(0x0489, 0xe042) },
+	{ USB_DEVICE(0x04ca, 0x2003) },
 	{ USB_DEVICE(0x0b05, 0x17b5) },
 	{ USB_DEVICE(0x0b05, 0x17cb) },
-	{ USB_DEVICE(0x04ca, 0x2003) },
-	{ USB_DEVICE(0x0489, 0xe042) },
 	{ USB_DEVICE(0x413c, 0x8197) },
 
 	/* Foxconn - Hon Hai */
 	{ USB_VENDOR_AND_INTERFACE_INFO(0x0489, 0xff, 0x01, 0x01) },
 
-	/*Broadcom devices with vendor specific id */
+	/* Broadcom devices with vendor specific id */
 	{ USB_VENDOR_AND_INTERFACE_INFO(0x0a5c, 0xff, 0x01, 0x01) },
 
 	/* Belkin F8065bf - Broadcom based */
 	{ USB_VENDOR_AND_INTERFACE_INFO(0x050d, 0xff, 0x01, 0x01) },
 
+	/* IMC Networks - Broadcom based */
+	{ USB_VENDOR_AND_INTERFACE_INFO(0x13d3, 0xff, 0x01, 0x01) },
+
 	{ }	/* Terminating entry */
 };
 
@@ -129,55 +132,58 @@ static const struct usb_device_id blacklist_table[] = {
 	{ USB_DEVICE(0x0a5c, 0x2033), .driver_info = BTUSB_IGNORE },
 
 	/* Atheros 3011 with sflash firmware */
+	{ USB_DEVICE(0x0489, 0xe027), .driver_info = BTUSB_IGNORE },
+	{ USB_DEVICE(0x0489, 0xe03d), .driver_info = BTUSB_IGNORE },
+	{ USB_DEVICE(0x0930, 0x0215), .driver_info = BTUSB_IGNORE },
 	{ USB_DEVICE(0x0cf3, 0x3002), .driver_info = BTUSB_IGNORE },
 	{ USB_DEVICE(0x0cf3, 0xe019), .driver_info = BTUSB_IGNORE },
 	{ USB_DEVICE(0x13d3, 0x3304), .driver_info = BTUSB_IGNORE },
-	{ USB_DEVICE(0x0930, 0x0215), .driver_info = BTUSB_IGNORE },
-	{ USB_DEVICE(0x0489, 0xe03d), .driver_info = BTUSB_IGNORE },
-	{ USB_DEVICE(0x0489, 0xe027), .driver_info = BTUSB_IGNORE },
 
 	/* Atheros AR9285 Malbec with sflash firmware */
 	{ USB_DEVICE(0x03f0, 0x311d), .driver_info = BTUSB_IGNORE },
 
 	/* Atheros 3012 with sflash firmware */
-	{ USB_DEVICE(0x0cf3, 0x0036), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x0cf3, 0x3004), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x0cf3, 0x3008), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x0cf3, 0x311d), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x0cf3, 0x817a), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x13d3, 0x3375), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0489, 0xe04d), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0489, 0xe04e), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0489, 0xe056), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0489, 0xe057), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0489, 0xe05f), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x04c5, 0x1330), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x3004), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x3005), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x3006), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x3008), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x300b), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x13d3, 0x3362), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x0cf3, 0xe004), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x0cf3, 0xe005), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0930, 0x0219), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0930, 0x0220), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x0489, 0xe057), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x13d3, 0x3393), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x0489, 0xe04e), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x0489, 0xe056), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x0489, 0xe04d), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x04c5, 0x1330), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x13d3, 0x3402), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0b05, 0x17d0), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0cf3, 0x0036), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0cf3, 0x3004), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0cf3, 0x3008), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0cf3, 0x311d), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0cf3, 0x311e), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0cf3, 0x311f), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0cf3, 0x3121), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0cf3, 0x817a), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0cf3, 0xe003), .driver_info = BTUSB_ATH3012 },
-	{ USB_DEVICE(0x0489, 0xe05f), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0cf3, 0xe004), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0cf3, 0xe005), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x13d3, 0x3362), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x13d3, 0x3375), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x13d3, 0x3393), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x13d3, 0x3402), .driver_info = BTUSB_ATH3012 },
 
 	/* Atheros AR5BBU12 with sflash firmware */
 	{ USB_DEVICE(0x0489, 0xe02c), .driver_info = BTUSB_IGNORE },
 
 	/* Atheros AR5BBU12 with sflash firmware */
-	{ USB_DEVICE(0x0489, 0xe03c), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0489, 0xe036), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0489, 0xe03c), .driver_info = BTUSB_ATH3012 },
 
 	/* Broadcom BCM2035 */
-	{ USB_DEVICE(0x0a5c, 0x2035), .driver_info = BTUSB_WRONG_SCO_MTU },
-	{ USB_DEVICE(0x0a5c, 0x200a), .driver_info = BTUSB_WRONG_SCO_MTU },
 	{ USB_DEVICE(0x0a5c, 0x2009), .driver_info = BTUSB_BCM92035 },
+	{ USB_DEVICE(0x0a5c, 0x200a), .driver_info = BTUSB_WRONG_SCO_MTU },
+	{ USB_DEVICE(0x0a5c, 0x2035), .driver_info = BTUSB_WRONG_SCO_MTU },
 
 	/* Broadcom BCM2045 */
 	{ USB_DEVICE(0x0a5c, 0x2039), .driver_info = BTUSB_WRONG_SCO_MTU },
diff --git a/drivers/bluetooth/hci_vhci.c b/drivers/bluetooth/hci_vhci.c
index 1ef6990a5c7e..add1c6a72063 100644
--- a/drivers/bluetooth/hci_vhci.c
+++ b/drivers/bluetooth/hci_vhci.c
@@ -359,7 +359,7 @@ static const struct file_operations vhci_fops = {
 static struct miscdevice vhci_miscdev= {
 	.name	= "vhci",
 	.fops	= &vhci_fops,
-	.minor	= MISC_DYNAMIC_MINOR,
+	.minor	= VHCI_MINOR,
 };
 
 static int __init vhci_init(void)
@@ -385,3 +385,4 @@ MODULE_DESCRIPTION("Bluetooth virtual HCI driver ver " VERSION);
 MODULE_VERSION(VERSION);
 MODULE_LICENSE("GPL");
 MODULE_ALIAS("devname:vhci");
+MODULE_ALIAS_MISCDEV(VHCI_MINOR);
diff --git a/drivers/bus/arm-cci.c b/drivers/bus/arm-cci.c
index b6739cb78e32..962fd35cbd8d 100644
--- a/drivers/bus/arm-cci.c
+++ b/drivers/bus/arm-cci.c
@@ -979,7 +979,7 @@ static int cci_probe(void)
 
 	nb_cci_ports = cci_config->nb_ace + cci_config->nb_ace_lite;
 
-	ports = kcalloc(sizeof(*ports), nb_cci_ports, GFP_KERNEL);
+	ports = kcalloc(nb_cci_ports, sizeof(*ports), GFP_KERNEL);
 	if (!ports)
 		return -ENOMEM;
 
diff --git a/drivers/cdrom/gdrom.c b/drivers/cdrom/gdrom.c
index 5980cb9af857..51e75ad96422 100644
--- a/drivers/cdrom/gdrom.c
+++ b/drivers/cdrom/gdrom.c
@@ -561,11 +561,11 @@ static int gdrom_set_interrupt_handlers(void)
 	int err;
 
 	err = request_irq(HW_EVENT_GDROM_CMD, gdrom_command_interrupt,
-		IRQF_DISABLED, "gdrom_command", &gd);
+		0, "gdrom_command", &gd);
 	if (err)
 		return err;
 	err = request_irq(HW_EVENT_GDROM_DMA, gdrom_dma_interrupt,
-		IRQF_DISABLED, "gdrom_dma", &gd);
+		0, "gdrom_dma", &gd);
 	if (err)
 		free_irq(HW_EVENT_GDROM_CMD, &gd);
 	return err;
diff --git a/drivers/char/ipmi/ipmi_si_intf.c b/drivers/char/ipmi/ipmi_si_intf.c
index 671c3852d359..03f41896d090 100644
--- a/drivers/char/ipmi/ipmi_si_intf.c
+++ b/drivers/char/ipmi/ipmi_si_intf.c
@@ -2724,6 +2724,7 @@ static struct platform_driver ipmi_driver = {
 static int ipmi_parisc_probe(struct parisc_device *dev)
 {
 	struct smi_info *info;
+	int rv;
 
 	info = smi_info_alloc();
 
diff --git a/drivers/char/virtio_console.c b/drivers/char/virtio_console.c
index feea87cc6b8f..6928d094451d 100644
--- a/drivers/char/virtio_console.c
+++ b/drivers/char/virtio_console.c
@@ -890,12 +890,10 @@ static int pipe_to_sg(struct pipe_inode_info *pipe, struct pipe_buffer *buf,
 	} else {
 		/* Failback to copying a page */
 		struct page *page = alloc_page(GFP_KERNEL);
-		char *src = buf->ops->map(pipe, buf, 1);
-		char *dst;
+		char *src;
 
 		if (!page)
 			return -ENOMEM;
-		dst = kmap(page);
 
 		offset = sd->pos & ~PAGE_MASK;
 
@@ -903,9 +901,8 @@ static int pipe_to_sg(struct pipe_inode_info *pipe, struct pipe_buffer *buf,
 		if (len + offset > PAGE_SIZE)
 			len = PAGE_SIZE - offset;
 
-		memcpy(dst + offset, src + buf->offset, len);
-
-		kunmap(page);
+		src = buf->ops->map(pipe, buf, 1);
+		memcpy(page_address(page) + offset, src + buf->offset, len);
 		buf->ops->unmap(pipe, buf, src);
 
 		sg_set_page(&(sgl->sg[sgl->n]), page, len, offset);
diff --git a/drivers/clocksource/bcm_kona_timer.c b/drivers/clocksource/bcm_kona_timer.c
index 5176e761166b..974b2db2fe10 100644
--- a/drivers/clocksource/bcm_kona_timer.c
+++ b/drivers/clocksource/bcm_kona_timer.c
@@ -17,6 +17,7 @@
 #include <linux/jiffies.h>
 #include <linux/clockchips.h>
 #include <linux/types.h>
+#include <linux/clk.h>
 
 #include <linux/io.h>
 #include <asm/mach/time.h>
@@ -101,11 +102,18 @@ kona_timer_get_counter(void *timer_base, uint32_t *msw, uint32_t *lsw)
 static void __init kona_timers_init(struct device_node *node)
 {
 	u32 freq;
+	struct clk *external_clk;
 
-	if (!of_property_read_u32(node, "clock-frequency", &freq))
+	external_clk = of_clk_get_by_name(node, NULL);
+
+	if (!IS_ERR(external_clk)) {
+		arch_timer_rate = clk_get_rate(external_clk);
+		clk_prepare_enable(external_clk);
+	} else if (!of_property_read_u32(node, "clock-frequency", &freq)) {
 		arch_timer_rate = freq;
-	else
-		panic("clock-frequency not set in the .dts file");
+	} else {
+		panic("unable to determine clock-frequency");
+	}
 
 	/* Setup IRQ numbers */
 	timers.tmr_irq = irq_of_parse_and_map(node, 0);
diff --git a/drivers/cpufreq/acpi-cpufreq.c b/drivers/cpufreq/acpi-cpufreq.c
index 79e5608e71b5..18448a7e9f86 100644
--- a/drivers/cpufreq/acpi-cpufreq.c
+++ b/drivers/cpufreq/acpi-cpufreq.c
@@ -919,7 +919,7 @@ static void __init acpi_cpufreq_boost_init(void)
 	}
 }
 
-static void __exit acpi_cpufreq_boost_exit(void)
+static void acpi_cpufreq_boost_exit(void)
 {
 	if (msrs) {
 		unregister_cpu_notifier(&boost_nb);
@@ -969,9 +969,10 @@ static int __init acpi_cpufreq_init(void)
 	acpi_cpufreq_boost_init();
 
 	ret = cpufreq_register_driver(&acpi_cpufreq_driver);
-	if (ret)
+	if (ret) {
 		free_acpi_perf_data();
-
+		acpi_cpufreq_boost_exit();
+	}
 	return ret;
 }
 
diff --git a/drivers/cpufreq/intel_pstate.c b/drivers/cpufreq/intel_pstate.c
index 7e257b233602..79606f473f48 100644
--- a/drivers/cpufreq/intel_pstate.c
+++ b/drivers/cpufreq/intel_pstate.c
@@ -57,6 +57,7 @@ struct sample {
 	int32_t core_pct_busy;
 	u64 aperf;
 	u64 mperf;
+	unsigned long long tsc;
 	int freq;
 };
 
@@ -96,6 +97,7 @@ struct cpudata {
 
 	u64	prev_aperf;
 	u64	prev_mperf;
+	unsigned long long prev_tsc;
 	int	sample_ptr;
 	struct sample samples[SAMPLE_COUNT];
 };
@@ -548,30 +550,41 @@ static inline void intel_pstate_calc_busy(struct cpudata *cpu,
 					struct sample *sample)
 {
 	u64 core_pct;
-	core_pct = div64_u64(int_tofp(sample->aperf * 100),
-			     sample->mperf);
-	sample->freq = fp_toint(cpu->pstate.max_pstate * core_pct * 1000);
+	u64 c0_pct;
 
-	sample->core_pct_busy = core_pct;
+	core_pct = div64_u64(sample->aperf * 100, sample->mperf);
+
+	c0_pct = div64_u64(sample->mperf * 100, sample->tsc);
+	sample->freq = fp_toint(
+		mul_fp(int_tofp(cpu->pstate.max_pstate),
+			int_tofp(core_pct * 1000)));
+
+	sample->core_pct_busy = mul_fp(int_tofp(core_pct),
+				div_fp(int_tofp(c0_pct + 1), int_tofp(100)));
 }
 
 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();
 
 	cpu->sample_ptr = (cpu->sample_ptr + 1) % SAMPLE_COUNT;
 	cpu->samples[cpu->sample_ptr].aperf = aperf;
 	cpu->samples[cpu->sample_ptr].mperf = mperf;
+	cpu->samples[cpu->sample_ptr].tsc = tsc;
 	cpu->samples[cpu->sample_ptr].aperf -= cpu->prev_aperf;
 	cpu->samples[cpu->sample_ptr].mperf -= cpu->prev_mperf;
+	cpu->samples[cpu->sample_ptr].tsc -= cpu->prev_tsc;
 
 	intel_pstate_calc_busy(cpu, &cpu->samples[cpu->sample_ptr]);
 
 	cpu->prev_aperf = aperf;
 	cpu->prev_mperf = mperf;
+	cpu->prev_tsc = tsc;
 }
 
 static inline void intel_pstate_set_sample_time(struct cpudata *cpu)
diff --git a/drivers/cpuidle/Kconfig b/drivers/cpuidle/Kconfig
index b3fb81d7cf04..f04e25f6c98d 100644
--- a/drivers/cpuidle/Kconfig
+++ b/drivers/cpuidle/Kconfig
@@ -35,6 +35,11 @@ depends on ARM
 source "drivers/cpuidle/Kconfig.arm"
 endmenu
 
+menu "POWERPC CPU Idle Drivers"
+depends on PPC
+source "drivers/cpuidle/Kconfig.powerpc"
+endmenu
+
 endif
 
 config ARCH_NEEDS_CPU_IDLE_COUPLED
diff --git a/drivers/cpuidle/Kconfig.powerpc b/drivers/cpuidle/Kconfig.powerpc
new file mode 100644
index 000000000000..66c3a09574e9
--- /dev/null
+++ b/drivers/cpuidle/Kconfig.powerpc
@@ -0,0 +1,20 @@
+#
+# POWERPC CPU Idle Drivers
+#
+config PSERIES_CPUIDLE
+	bool "Cpuidle driver for pSeries platforms"
+	depends on CPU_IDLE
+	depends on PPC_PSERIES
+	default y
+	help
+	  Select this option to enable processor idle state management
+	  through cpuidle subsystem.
+
+config POWERNV_CPUIDLE
+	bool "Cpuidle driver for powernv platforms"
+	depends on CPU_IDLE
+	depends on PPC_POWERNV
+	default y
+	help
+	  Select this option to enable processor idle state management
+	  through cpuidle subsystem.
diff --git a/drivers/cpuidle/Makefile b/drivers/cpuidle/Makefile
index 527be28e5c1e..f71ae1b373c5 100644
--- a/drivers/cpuidle/Makefile
+++ b/drivers/cpuidle/Makefile
@@ -13,3 +13,8 @@ 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
+
+###############################################################################
+# POWERPC drivers
+obj-$(CONFIG_PSERIES_CPUIDLE)		+= cpuidle-pseries.o
+obj-$(CONFIG_POWERNV_CPUIDLE)		+= cpuidle-powernv.o
diff --git a/drivers/cpuidle/cpuidle-powernv.c b/drivers/cpuidle/cpuidle-powernv.c
new file mode 100644
index 000000000000..78fd174c57e8
--- /dev/null
+++ b/drivers/cpuidle/cpuidle-powernv.c
@@ -0,0 +1,169 @@
+/*
+ *  cpuidle-powernv - idle state cpuidle driver.
+ *  Adapted from drivers/cpuidle/cpuidle-pseries
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/moduleparam.h>
+#include <linux/cpuidle.h>
+#include <linux/cpu.h>
+#include <linux/notifier.h>
+
+#include <asm/machdep.h>
+#include <asm/firmware.h>
+
+struct cpuidle_driver powernv_idle_driver = {
+	.name             = "powernv_idle",
+	.owner            = THIS_MODULE,
+};
+
+static int max_idle_state;
+static struct cpuidle_state *cpuidle_state_table;
+
+static int snooze_loop(struct cpuidle_device *dev,
+			struct cpuidle_driver *drv,
+			int index)
+{
+	local_irq_enable();
+	set_thread_flag(TIF_POLLING_NRFLAG);
+
+	while (!need_resched()) {
+		HMT_low();
+		HMT_very_low();
+	}
+
+	HMT_medium();
+	clear_thread_flag(TIF_POLLING_NRFLAG);
+	smp_mb();
+	return index;
+}
+
+static int nap_loop(struct cpuidle_device *dev,
+			struct cpuidle_driver *drv,
+			int index)
+{
+	power7_idle();
+	return index;
+}
+
+/*
+ * States for dedicated partition case.
+ */
+static struct cpuidle_state powernv_states[] = {
+	{ /* Snooze */
+		.name = "snooze",
+		.desc = "snooze",
+		.flags = CPUIDLE_FLAG_TIME_VALID,
+		.exit_latency = 0,
+		.target_residency = 0,
+		.enter = &snooze_loop },
+	{ /* NAP */
+		.name = "NAP",
+		.desc = "NAP",
+		.flags = CPUIDLE_FLAG_TIME_VALID,
+		.exit_latency = 10,
+		.target_residency = 100,
+		.enter = &nap_loop },
+};
+
+static int powernv_cpuidle_add_cpu_notifier(struct notifier_block *n,
+			unsigned long action, void *hcpu)
+{
+	int hotcpu = (unsigned long)hcpu;
+	struct cpuidle_device *dev =
+				per_cpu(cpuidle_devices, hotcpu);
+
+	if (dev && cpuidle_get_driver()) {
+		switch (action) {
+		case CPU_ONLINE:
+		case CPU_ONLINE_FROZEN:
+			cpuidle_pause_and_lock();
+			cpuidle_enable_device(dev);
+			cpuidle_resume_and_unlock();
+			break;
+
+		case CPU_DEAD:
+		case CPU_DEAD_FROZEN:
+			cpuidle_pause_and_lock();
+			cpuidle_disable_device(dev);
+			cpuidle_resume_and_unlock();
+			break;
+
+		default:
+			return NOTIFY_DONE;
+		}
+	}
+	return NOTIFY_OK;
+}
+
+static struct notifier_block setup_hotplug_notifier = {
+	.notifier_call = powernv_cpuidle_add_cpu_notifier,
+};
+
+/*
+ * powernv_cpuidle_driver_init()
+ */
+static int powernv_cpuidle_driver_init(void)
+{
+	int idle_state;
+	struct cpuidle_driver *drv = &powernv_idle_driver;
+
+	drv->state_count = 0;
+
+	for (idle_state = 0; idle_state < max_idle_state; ++idle_state) {
+		/* Is the state not enabled? */
+		if (cpuidle_state_table[idle_state].enter == NULL)
+			continue;
+
+		drv->states[drv->state_count] =	/* structure copy */
+			cpuidle_state_table[idle_state];
+
+		drv->state_count += 1;
+	}
+
+	return 0;
+}
+
+/*
+ * powernv_idle_probe()
+ * Choose state table for shared versus dedicated partition
+ */
+static int powernv_idle_probe(void)
+{
+
+	if (cpuidle_disable != IDLE_NO_OVERRIDE)
+		return -ENODEV;
+
+	if (firmware_has_feature(FW_FEATURE_OPALv3)) {
+		cpuidle_state_table = powernv_states;
+		max_idle_state = ARRAY_SIZE(powernv_states);
+ 	} else
+ 		return -ENODEV;
+
+	return 0;
+}
+
+static int __init powernv_processor_idle_init(void)
+{
+	int retval;
+
+	retval = powernv_idle_probe();
+	if (retval)
+		return retval;
+
+	powernv_cpuidle_driver_init();
+	retval = cpuidle_register(&powernv_idle_driver, NULL);
+	if (retval) {
+		printk(KERN_DEBUG "Registration of powernv driver failed.\n");
+		return retval;
+	}
+
+	register_cpu_notifier(&setup_hotplug_notifier);
+	printk(KERN_DEBUG "powernv_idle_driver registered\n");
+	return 0;
+}
+
+device_initcall(powernv_processor_idle_init);
diff --git a/drivers/cpuidle/cpuidle-pseries.c b/drivers/cpuidle/cpuidle-pseries.c
new file mode 100644
index 000000000000..7ab564aa0b1c
--- /dev/null
+++ b/drivers/cpuidle/cpuidle-pseries.c
@@ -0,0 +1,267 @@
+/*
+ *  cpuidle-pseries - idle state cpuidle driver.
+ *  Adapted from drivers/idle/intel_idle.c and
+ *  drivers/acpi/processor_idle.c
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/moduleparam.h>
+#include <linux/cpuidle.h>
+#include <linux/cpu.h>
+#include <linux/notifier.h>
+
+#include <asm/paca.h>
+#include <asm/reg.h>
+#include <asm/machdep.h>
+#include <asm/firmware.h>
+#include <asm/plpar_wrappers.h>
+
+struct cpuidle_driver pseries_idle_driver = {
+	.name             = "pseries_idle",
+	.owner            = THIS_MODULE,
+};
+
+static int max_idle_state;
+static struct cpuidle_state *cpuidle_state_table;
+
+static inline void idle_loop_prolog(unsigned long *in_purr)
+{
+	*in_purr = mfspr(SPRN_PURR);
+	/*
+	 * Indicate to the HV that we are idle. Now would be
+	 * a good time to find other work to dispatch.
+	 */
+	get_lppaca()->idle = 1;
+}
+
+static inline void idle_loop_epilog(unsigned long in_purr)
+{
+	u64 wait_cycles;
+
+	wait_cycles = be64_to_cpu(get_lppaca()->wait_state_cycles);
+	wait_cycles += mfspr(SPRN_PURR) - in_purr;
+	get_lppaca()->wait_state_cycles = cpu_to_be64(wait_cycles);
+	get_lppaca()->idle = 0;
+}
+
+static int snooze_loop(struct cpuidle_device *dev,
+			struct cpuidle_driver *drv,
+			int index)
+{
+	unsigned long in_purr;
+
+	idle_loop_prolog(&in_purr);
+	local_irq_enable();
+	set_thread_flag(TIF_POLLING_NRFLAG);
+
+	while (!need_resched()) {
+		HMT_low();
+		HMT_very_low();
+	}
+
+	HMT_medium();
+	clear_thread_flag(TIF_POLLING_NRFLAG);
+	smp_mb();
+
+	idle_loop_epilog(in_purr);
+
+	return index;
+}
+
+static void check_and_cede_processor(void)
+{
+	/*
+	 * Ensure our interrupt state is properly tracked,
+	 * also checks if no interrupt has occurred while we
+	 * were soft-disabled
+	 */
+	if (prep_irq_for_idle()) {
+		cede_processor();
+#ifdef CONFIG_TRACE_IRQFLAGS
+		/* Ensure that H_CEDE returns with IRQs on */
+		if (WARN_ON(!(mfmsr() & MSR_EE)))
+			__hard_irq_enable();
+#endif
+	}
+}
+
+static int dedicated_cede_loop(struct cpuidle_device *dev,
+				struct cpuidle_driver *drv,
+				int index)
+{
+	unsigned long in_purr;
+
+	idle_loop_prolog(&in_purr);
+	get_lppaca()->donate_dedicated_cpu = 1;
+
+	HMT_medium();
+	check_and_cede_processor();
+
+	get_lppaca()->donate_dedicated_cpu = 0;
+
+	idle_loop_epilog(in_purr);
+
+	return index;
+}
+
+static int shared_cede_loop(struct cpuidle_device *dev,
+			struct cpuidle_driver *drv,
+			int index)
+{
+	unsigned long in_purr;
+
+	idle_loop_prolog(&in_purr);
+
+	/*
+	 * Yield the processor to the hypervisor.  We return if
+	 * an external interrupt occurs (which are driven prior
+	 * to returning here) or if a prod occurs from another
+	 * processor. When returning here, external interrupts
+	 * are enabled.
+	 */
+	check_and_cede_processor();
+
+	idle_loop_epilog(in_purr);
+
+	return index;
+}
+
+/*
+ * States for dedicated partition case.
+ */
+static struct cpuidle_state dedicated_states[] = {
+	{ /* Snooze */
+		.name = "snooze",
+		.desc = "snooze",
+		.flags = CPUIDLE_FLAG_TIME_VALID,
+		.exit_latency = 0,
+		.target_residency = 0,
+		.enter = &snooze_loop },
+	{ /* CEDE */
+		.name = "CEDE",
+		.desc = "CEDE",
+		.flags = CPUIDLE_FLAG_TIME_VALID,
+		.exit_latency = 10,
+		.target_residency = 100,
+		.enter = &dedicated_cede_loop },
+};
+
+/*
+ * States for shared partition case.
+ */
+static struct cpuidle_state shared_states[] = {
+	{ /* Shared Cede */
+		.name = "Shared Cede",
+		.desc = "Shared Cede",
+		.flags = CPUIDLE_FLAG_TIME_VALID,
+		.exit_latency = 0,
+		.target_residency = 0,
+		.enter = &shared_cede_loop },
+};
+
+static int pseries_cpuidle_add_cpu_notifier(struct notifier_block *n,
+			unsigned long action, void *hcpu)
+{
+	int hotcpu = (unsigned long)hcpu;
+	struct cpuidle_device *dev =
+				per_cpu(cpuidle_devices, hotcpu);
+
+	if (dev && cpuidle_get_driver()) {
+		switch (action) {
+		case CPU_ONLINE:
+		case CPU_ONLINE_FROZEN:
+			cpuidle_pause_and_lock();
+			cpuidle_enable_device(dev);
+			cpuidle_resume_and_unlock();
+			break;
+
+		case CPU_DEAD:
+		case CPU_DEAD_FROZEN:
+			cpuidle_pause_and_lock();
+			cpuidle_disable_device(dev);
+			cpuidle_resume_and_unlock();
+			break;
+
+		default:
+			return NOTIFY_DONE;
+		}
+	}
+	return NOTIFY_OK;
+}
+
+static struct notifier_block setup_hotplug_notifier = {
+	.notifier_call = pseries_cpuidle_add_cpu_notifier,
+};
+
+/*
+ * pseries_cpuidle_driver_init()
+ */
+static int pseries_cpuidle_driver_init(void)
+{
+	int idle_state;
+	struct cpuidle_driver *drv = &pseries_idle_driver;
+
+	drv->state_count = 0;
+
+	for (idle_state = 0; idle_state < max_idle_state; ++idle_state) {
+		/* Is the state not enabled? */
+		if (cpuidle_state_table[idle_state].enter == NULL)
+			continue;
+
+		drv->states[drv->state_count] =	/* structure copy */
+			cpuidle_state_table[idle_state];
+
+		drv->state_count += 1;
+	}
+
+	return 0;
+}
+
+/*
+ * pseries_idle_probe()
+ * Choose state table for shared versus dedicated partition
+ */
+static int pseries_idle_probe(void)
+{
+
+	if (cpuidle_disable != IDLE_NO_OVERRIDE)
+		return -ENODEV;
+
+	if (firmware_has_feature(FW_FEATURE_SPLPAR)) {
+		if (lppaca_shared_proc(get_lppaca())) {
+			cpuidle_state_table = shared_states;
+			max_idle_state = ARRAY_SIZE(shared_states);
+		} else {
+			cpuidle_state_table = dedicated_states;
+			max_idle_state = ARRAY_SIZE(dedicated_states);
+		}
+	} else
+		return -ENODEV;
+
+	return 0;
+}
+
+static int __init pseries_processor_idle_init(void)
+{
+	int retval;
+
+	retval = pseries_idle_probe();
+	if (retval)
+		return retval;
+
+	pseries_cpuidle_driver_init();
+	retval = cpuidle_register(&pseries_idle_driver, NULL);
+	if (retval) {
+		printk(KERN_DEBUG "Registration of pseries driver failed.\n");
+		return retval;
+	}
+
+	register_cpu_notifier(&setup_hotplug_notifier);
+	printk(KERN_DEBUG "pseries_idle_driver registered\n");
+	return 0;
+}
+
+device_initcall(pseries_processor_idle_init);
diff --git a/drivers/devfreq/Kconfig b/drivers/devfreq/Kconfig
index 31f3adba4cf3..7d2f43550700 100644
--- a/drivers/devfreq/Kconfig
+++ b/drivers/devfreq/Kconfig
@@ -67,7 +67,7 @@ comment "DEVFREQ Drivers"
 
 config ARM_EXYNOS4_BUS_DEVFREQ
 	bool "ARM Exynos4210/4212/4412 Memory Bus DEVFREQ Driver"
-	depends on CPU_EXYNOS4210 || SOC_EXYNOS4212 || SOC_EXYNOS4412
+	depends on (CPU_EXYNOS4210 || SOC_EXYNOS4212 || SOC_EXYNOS4412) && !ARCH_MULTIPLATFORM
 	select ARCH_HAS_OPP
 	select DEVFREQ_GOV_SIMPLE_ONDEMAND
 	help
diff --git a/drivers/gpu/drm/ast/ast_fb.c b/drivers/gpu/drm/ast/ast_fb.c
index 3f65dd6676b2..a28640f47c27 100644
--- a/drivers/gpu/drm/ast/ast_fb.c
+++ b/drivers/gpu/drm/ast/ast_fb.c
@@ -65,7 +65,7 @@ static void ast_dirty_update(struct ast_fbdev *afbdev,
 	 * then the BO is being moved and we should
 	 * store up the damage until later.
 	 */
-	if (!drm_can_sleep())
+	if (drm_can_sleep())
 		ret = ast_bo_reserve(bo, true);
 	if (ret) {
 		if (ret != -EBUSY)
diff --git a/drivers/gpu/drm/cirrus/cirrus_fbdev.c b/drivers/gpu/drm/cirrus/cirrus_fbdev.c
index 2fd4a92162cb..32bbba0a787b 100644
--- a/drivers/gpu/drm/cirrus/cirrus_fbdev.c
+++ b/drivers/gpu/drm/cirrus/cirrus_fbdev.c
@@ -39,7 +39,7 @@ static void cirrus_dirty_update(struct cirrus_fbdev *afbdev,
 	 * then the BO is being moved and we should
 	 * store up the damage until later.
 	 */
-	if (!drm_can_sleep())
+	if (drm_can_sleep())
 		ret = cirrus_bo_reserve(bo, true);
 	if (ret) {
 		if (ret != -EBUSY)
diff --git a/drivers/gpu/drm/mgag200/mgag200_fb.c b/drivers/gpu/drm/mgag200/mgag200_fb.c
index f9adc27ef32a..13b7dd83faa9 100644
--- a/drivers/gpu/drm/mgag200/mgag200_fb.c
+++ b/drivers/gpu/drm/mgag200/mgag200_fb.c
@@ -41,7 +41,7 @@ static void mga_dirty_update(struct mga_fbdev *mfbdev,
 	 * then the BO is being moved and we should
 	 * store up the damage until later.
 	 */
-	if (!drm_can_sleep())
+	if (drm_can_sleep())
 		ret = mgag200_bo_reserve(bo, true);
 	if (ret) {
 		if (ret != -EBUSY)
diff --git a/drivers/gpu/drm/mgag200/mgag200_mode.c b/drivers/gpu/drm/mgag200/mgag200_mode.c
index b8583f275e80..968374776db9 100644
--- a/drivers/gpu/drm/mgag200/mgag200_mode.c
+++ b/drivers/gpu/drm/mgag200/mgag200_mode.c
@@ -1519,11 +1519,11 @@ static int mga_vga_mode_valid(struct drm_connector *connector,
 		(mga_vga_calculate_mode_bandwidth(mode, bpp)
 			> (32700 * 1024))) {
 		return MODE_BANDWIDTH;
-	} else if (mode->type == G200_EH &&
+	} else if (mdev->type == G200_EH &&
 		(mga_vga_calculate_mode_bandwidth(mode, bpp)
 			> (37500 * 1024))) {
 		return MODE_BANDWIDTH;
-	} else if (mode->type == G200_ER &&
+	} else if (mdev->type == G200_ER &&
 		(mga_vga_calculate_mode_bandwidth(mode,
 			bpp) > (55000 * 1024))) {
 		return MODE_BANDWIDTH;
diff --git a/drivers/gpu/drm/radeon/r600_cs.c b/drivers/gpu/drm/radeon/r600_cs.c
index 7b399dc5fd54..2812c7d1ae6f 100644
--- a/drivers/gpu/drm/radeon/r600_cs.c
+++ b/drivers/gpu/drm/radeon/r600_cs.c
@@ -1007,8 +1007,22 @@ static int r600_cs_check_reg(struct radeon_cs_parser *p, u32 reg, u32 idx)
 	case R_008C64_SQ_VSTMP_RING_SIZE:
 	case R_0288C8_SQ_GS_VERT_ITEMSIZE:
 		/* get value to populate the IB don't remove */
-		tmp =radeon_get_ib_value(p, idx);
-		ib[idx] = 0;
+		/*tmp =radeon_get_ib_value(p, idx);
+		  ib[idx] = 0;*/
+		break;
+	case SQ_ESGS_RING_BASE:
+	case SQ_GSVS_RING_BASE:
+	case SQ_ESTMP_RING_BASE:
+	case SQ_GSTMP_RING_BASE:
+	case SQ_PSTMP_RING_BASE:
+	case SQ_VSTMP_RING_BASE:
+		r = radeon_cs_packet_next_reloc(p, &reloc, 0);
+		if (r) {
+			dev_warn(p->dev, "bad SET_CONTEXT_REG "
+					"0x%04X\n", reg);
+			return -EINVAL;
+		}
+		ib[idx] += (u32)((reloc->lobj.gpu_offset >> 8) & 0xffffffff);
 		break;
 	case SQ_CONFIG:
 		track->sq_config = radeon_get_ib_value(p, idx);
diff --git a/drivers/gpu/drm/radeon/radeon_drv.c b/drivers/gpu/drm/radeon/radeon_drv.c
index ec8c388eec17..84a1bbb75f91 100644
--- a/drivers/gpu/drm/radeon/radeon_drv.c
+++ b/drivers/gpu/drm/radeon/radeon_drv.c
@@ -78,9 +78,10 @@
  *   2.34.0 - Add CIK tiling mode array query
  *   2.35.0 - Add CIK macrotile mode array query
  *   2.36.0 - Fix CIK DCE tiling setup
+ *   2.37.0 - allow GS ring setup on r6xx/r7xx
  */
 #define KMS_DRIVER_MAJOR	2
-#define KMS_DRIVER_MINOR	36
+#define KMS_DRIVER_MINOR	37
 #define KMS_DRIVER_PATCHLEVEL	0
 int radeon_driver_load_kms(struct drm_device *dev, unsigned long flags);
 int radeon_driver_unload_kms(struct drm_device *dev);
diff --git a/drivers/gpu/drm/radeon/reg_srcs/r600 b/drivers/gpu/drm/radeon/reg_srcs/r600
index 20bfbda7b3f1..ec0c6829c1dc 100644
--- a/drivers/gpu/drm/radeon/reg_srcs/r600
+++ b/drivers/gpu/drm/radeon/reg_srcs/r600
@@ -18,6 +18,7 @@ r600 0x9400
 0x00028A3C VGT_GROUP_VECT_1_FMT_CNTL
 0x00028A40 VGT_GS_MODE
 0x00028A6C VGT_GS_OUT_PRIM_TYPE
+0x00028B38 VGT_GS_MAX_VERT_OUT
 0x000088C8 VGT_GS_PER_ES
 0x000088E8 VGT_GS_PER_VS
 0x000088D4 VGT_GS_VERTEX_REUSE
diff --git a/drivers/gpu/drm/ttm/ttm_object.c b/drivers/gpu/drm/ttm/ttm_object.c
index 37079859afc8..53b51c4e671a 100644
--- a/drivers/gpu/drm/ttm/ttm_object.c
+++ b/drivers/gpu/drm/ttm/ttm_object.c
@@ -292,7 +292,7 @@ int ttm_ref_object_add(struct ttm_object_file *tfile,
 
 		if (ret == 0) {
 			ref = drm_hash_entry(hash, struct ttm_ref_object, hash);
-			if (!kref_get_unless_zero(&ref->kref)) {
+			if (kref_get_unless_zero(&ref->kref)) {
 				rcu_read_unlock();
 				break;
 			}
diff --git a/drivers/gpu/drm/ttm/ttm_tt.c b/drivers/gpu/drm/ttm/ttm_tt.c
index 9af99084b344..75f319090043 100644
--- a/drivers/gpu/drm/ttm/ttm_tt.c
+++ b/drivers/gpu/drm/ttm/ttm_tt.c
@@ -380,6 +380,9 @@ static void ttm_tt_clear_mapping(struct ttm_tt *ttm)
 	pgoff_t i;
 	struct page **page = ttm->pages;
 
+	if (ttm->page_flags & TTM_PAGE_FLAG_SG)
+		return;
+
 	for (i = 0; i < ttm->num_pages; ++i) {
 		(*page)->mapping = NULL;
 		(*page++)->index = 0;
diff --git a/drivers/gpu/drm/vmwgfx/svga3d_reg.h b/drivers/gpu/drm/vmwgfx/svga3d_reg.h
index d95335cb90bd..b645647b7776 100644
--- a/drivers/gpu/drm/vmwgfx/svga3d_reg.h
+++ b/drivers/gpu/drm/vmwgfx/svga3d_reg.h
@@ -2583,4 +2583,28 @@ typedef union {
    float  f;
 } SVGA3dDevCapResult;
 
+typedef enum {
+   SVGA3DCAPS_RECORD_UNKNOWN        = 0,
+   SVGA3DCAPS_RECORD_DEVCAPS_MIN    = 0x100,
+   SVGA3DCAPS_RECORD_DEVCAPS        = 0x100,
+   SVGA3DCAPS_RECORD_DEVCAPS_MAX    = 0x1ff,
+} SVGA3dCapsRecordType;
+
+typedef
+struct SVGA3dCapsRecordHeader {
+   uint32 length;
+   SVGA3dCapsRecordType type;
+}
+SVGA3dCapsRecordHeader;
+
+typedef
+struct SVGA3dCapsRecord {
+   SVGA3dCapsRecordHeader header;
+   uint32 data[1];
+}
+SVGA3dCapsRecord;
+
+
+typedef uint32 SVGA3dCapPair[2];
+
 #endif /* _SVGA3D_REG_H_ */
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_context.c b/drivers/gpu/drm/vmwgfx/vmwgfx_context.c
index 82c41daebc0e..9426c53fb483 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_context.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_context.c
@@ -37,7 +37,7 @@ struct vmw_user_context {
 
 
 
-typedef int (*vmw_scrub_func)(struct vmw_ctx_bindinfo *);
+typedef int (*vmw_scrub_func)(struct vmw_ctx_bindinfo *, bool);
 
 static void vmw_user_context_free(struct vmw_resource *res);
 static struct vmw_resource *
@@ -50,9 +50,11 @@ static int vmw_gb_context_unbind(struct vmw_resource *res,
 				 bool readback,
 				 struct ttm_validate_buffer *val_buf);
 static int vmw_gb_context_destroy(struct vmw_resource *res);
-static int vmw_context_scrub_shader(struct vmw_ctx_bindinfo *bi);
-static int vmw_context_scrub_render_target(struct vmw_ctx_bindinfo *bi);
-static int vmw_context_scrub_texture(struct vmw_ctx_bindinfo *bi);
+static int vmw_context_scrub_shader(struct vmw_ctx_bindinfo *bi, bool rebind);
+static int vmw_context_scrub_render_target(struct vmw_ctx_bindinfo *bi,
+					   bool rebind);
+static int vmw_context_scrub_texture(struct vmw_ctx_bindinfo *bi, bool rebind);
+static void vmw_context_binding_state_scrub(struct vmw_ctx_binding_state *cbs);
 static void vmw_context_binding_state_kill(struct vmw_ctx_binding_state *cbs);
 static uint64_t vmw_user_context_size;
 
@@ -111,10 +113,14 @@ static void vmw_hw_context_destroy(struct vmw_resource *res)
 
 	if (res->func->destroy == vmw_gb_context_destroy) {
 		mutex_lock(&dev_priv->cmdbuf_mutex);
+		mutex_lock(&dev_priv->binding_mutex);
+		(void) vmw_context_binding_state_kill
+			(&container_of(res, struct vmw_user_context, res)->cbs);
 		(void) vmw_gb_context_destroy(res);
 		if (dev_priv->pinned_bo != NULL &&
 		    !dev_priv->query_cid_valid)
 			__vmw_execbuf_release_pinned_bo(dev_priv, NULL);
+		mutex_unlock(&dev_priv->binding_mutex);
 		mutex_unlock(&dev_priv->cmdbuf_mutex);
 		return;
 	}
@@ -328,7 +334,7 @@ static int vmw_gb_context_unbind(struct vmw_resource *res,
 	BUG_ON(bo->mem.mem_type != VMW_PL_MOB);
 
 	mutex_lock(&dev_priv->binding_mutex);
-	vmw_context_binding_state_kill(&uctx->cbs);
+	vmw_context_binding_state_scrub(&uctx->cbs);
 
 	submit_size = sizeof(*cmd2) + (readback ? sizeof(*cmd1) : 0);
 
@@ -378,10 +384,6 @@ static int vmw_gb_context_destroy(struct vmw_resource *res)
 		SVGA3dCmdHeader header;
 		SVGA3dCmdDestroyGBContext body;
 	} *cmd;
-	struct vmw_user_context *uctx =
-		container_of(res, struct vmw_user_context, res);
-
-	BUG_ON(!list_empty(&uctx->cbs.list));
 
 	if (likely(res->id == -1))
 		return 0;
@@ -528,8 +530,9 @@ out_unlock:
  * vmw_context_scrub_shader - scrub a shader binding from a context.
  *
  * @bi: single binding information.
+ * @rebind: Whether to issue a bind instead of scrub command.
  */
-static int vmw_context_scrub_shader(struct vmw_ctx_bindinfo *bi)
+static int vmw_context_scrub_shader(struct vmw_ctx_bindinfo *bi, bool rebind)
 {
 	struct vmw_private *dev_priv = bi->ctx->dev_priv;
 	struct {
@@ -548,7 +551,8 @@ static int vmw_context_scrub_shader(struct vmw_ctx_bindinfo *bi)
 	cmd->header.size = sizeof(cmd->body);
 	cmd->body.cid = bi->ctx->id;
 	cmd->body.type = bi->i1.shader_type;
-	cmd->body.shid = SVGA3D_INVALID_ID;
+	cmd->body.shid =
+		cpu_to_le32((rebind) ? bi->res->id : SVGA3D_INVALID_ID);
 	vmw_fifo_commit(dev_priv, sizeof(*cmd));
 
 	return 0;
@@ -559,8 +563,10 @@ static int vmw_context_scrub_shader(struct vmw_ctx_bindinfo *bi)
  * from a context.
  *
  * @bi: single binding information.
+ * @rebind: Whether to issue a bind instead of scrub command.
  */
-static int vmw_context_scrub_render_target(struct vmw_ctx_bindinfo *bi)
+static int vmw_context_scrub_render_target(struct vmw_ctx_bindinfo *bi,
+					   bool rebind)
 {
 	struct vmw_private *dev_priv = bi->ctx->dev_priv;
 	struct {
@@ -579,7 +585,8 @@ static int vmw_context_scrub_render_target(struct vmw_ctx_bindinfo *bi)
 	cmd->header.size = sizeof(cmd->body);
 	cmd->body.cid = bi->ctx->id;
 	cmd->body.type = bi->i1.rt_type;
-	cmd->body.target.sid = SVGA3D_INVALID_ID;
+	cmd->body.target.sid =
+		cpu_to_le32((rebind) ? bi->res->id : SVGA3D_INVALID_ID);
 	cmd->body.target.face = 0;
 	cmd->body.target.mipmap = 0;
 	vmw_fifo_commit(dev_priv, sizeof(*cmd));
@@ -591,11 +598,13 @@ static int vmw_context_scrub_render_target(struct vmw_ctx_bindinfo *bi)
  * vmw_context_scrub_texture - scrub a texture binding from a context.
  *
  * @bi: single binding information.
+ * @rebind: Whether to issue a bind instead of scrub command.
  *
  * TODO: Possibly complement this function with a function that takes
  * a list of texture bindings and combines them to a single command.
  */
-static int vmw_context_scrub_texture(struct vmw_ctx_bindinfo *bi)
+static int vmw_context_scrub_texture(struct vmw_ctx_bindinfo *bi,
+				     bool rebind)
 {
 	struct vmw_private *dev_priv = bi->ctx->dev_priv;
 	struct {
@@ -619,7 +628,8 @@ static int vmw_context_scrub_texture(struct vmw_ctx_bindinfo *bi)
 	cmd->body.c.cid = bi->ctx->id;
 	cmd->body.s1.stage = bi->i1.texture_stage;
 	cmd->body.s1.name = SVGA3D_TS_BIND_TEXTURE;
-	cmd->body.s1.value = (uint32) SVGA3D_INVALID_ID;
+	cmd->body.s1.value =
+		cpu_to_le32((rebind) ? bi->res->id : SVGA3D_INVALID_ID);
 	vmw_fifo_commit(dev_priv, sizeof(*cmd));
 
 	return 0;
@@ -692,6 +702,7 @@ int vmw_context_binding_add(struct vmw_ctx_binding_state *cbs,
 		vmw_context_binding_drop(loc);
 
 	loc->bi = *bi;
+	loc->bi.scrubbed = false;
 	list_add_tail(&loc->ctx_list, &cbs->list);
 	INIT_LIST_HEAD(&loc->res_list);
 
@@ -727,12 +738,11 @@ static void vmw_context_binding_transfer(struct vmw_ctx_binding_state *cbs,
 	if (loc->bi.ctx != NULL)
 		vmw_context_binding_drop(loc);
 
-	loc->bi = *bi;
-	list_add_tail(&loc->ctx_list, &cbs->list);
-	if (bi->res != NULL)
+	if (bi->res != NULL) {
+		loc->bi = *bi;
+		list_add_tail(&loc->ctx_list, &cbs->list);
 		list_add_tail(&loc->res_list, &bi->res->binding_head);
-	else
-		INIT_LIST_HEAD(&loc->res_list);
+	}
 }
 
 /**
@@ -746,7 +756,10 @@ static void vmw_context_binding_transfer(struct vmw_ctx_binding_state *cbs,
  */
 static void vmw_context_binding_kill(struct vmw_ctx_binding *cb)
 {
-	(void) vmw_scrub_funcs[cb->bi.bt](&cb->bi);
+	if (!cb->bi.scrubbed) {
+		(void) vmw_scrub_funcs[cb->bi.bt](&cb->bi, false);
+		cb->bi.scrubbed = true;
+	}
 	vmw_context_binding_drop(cb);
 }
 
@@ -768,6 +781,27 @@ static void vmw_context_binding_state_kill(struct vmw_ctx_binding_state *cbs)
 }
 
 /**
+ * vmw_context_binding_state_scrub - Scrub all bindings associated with a
+ * struct vmw_ctx_binding state structure.
+ *
+ * @cbs: Pointer to the context binding state tracker.
+ *
+ * Emits commands to scrub all bindings associated with the
+ * context binding state tracker.
+ */
+static void vmw_context_binding_state_scrub(struct vmw_ctx_binding_state *cbs)
+{
+	struct vmw_ctx_binding *entry;
+
+	list_for_each_entry(entry, &cbs->list, ctx_list) {
+		if (!entry->bi.scrubbed) {
+			(void) vmw_scrub_funcs[entry->bi.bt](&entry->bi, false);
+			entry->bi.scrubbed = true;
+		}
+	}
+}
+
+/**
  * vmw_context_binding_res_list_kill - Kill all bindings on a
  * resource binding list
  *
@@ -785,6 +819,27 @@ void vmw_context_binding_res_list_kill(struct list_head *head)
 }
 
 /**
+ * vmw_context_binding_res_list_scrub - Scrub all bindings on a
+ * resource binding list
+ *
+ * @head: list head of resource binding list
+ *
+ * Scrub all bindings associated with a specific resource. Typically
+ * called before the resource is evicted.
+ */
+void vmw_context_binding_res_list_scrub(struct list_head *head)
+{
+	struct vmw_ctx_binding *entry;
+
+	list_for_each_entry(entry, head, res_list) {
+		if (!entry->bi.scrubbed) {
+			(void) vmw_scrub_funcs[entry->bi.bt](&entry->bi, false);
+			entry->bi.scrubbed = true;
+		}
+	}
+}
+
+/**
  * vmw_context_binding_state_transfer - Commit staged binding info
  *
  * @ctx: Pointer to context to commit the staged binding info to.
@@ -803,3 +858,50 @@ void vmw_context_binding_state_transfer(struct vmw_resource *ctx,
 	list_for_each_entry_safe(entry, next, &from->list, ctx_list)
 		vmw_context_binding_transfer(&uctx->cbs, &entry->bi);
 }
+
+/**
+ * vmw_context_rebind_all - Rebind all scrubbed bindings of a context
+ *
+ * @ctx: The context resource
+ *
+ * Walks through the context binding list and rebinds all scrubbed
+ * resources.
+ */
+int vmw_context_rebind_all(struct vmw_resource *ctx)
+{
+	struct vmw_ctx_binding *entry;
+	struct vmw_user_context *uctx =
+		container_of(ctx, struct vmw_user_context, res);
+	struct vmw_ctx_binding_state *cbs = &uctx->cbs;
+	int ret;
+
+	list_for_each_entry(entry, &cbs->list, ctx_list) {
+		if (likely(!entry->bi.scrubbed))
+			continue;
+
+		if (WARN_ON(entry->bi.res == NULL || entry->bi.res->id ==
+			    SVGA3D_INVALID_ID))
+			continue;
+
+		ret = vmw_scrub_funcs[entry->bi.bt](&entry->bi, true);
+		if (unlikely(ret != 0))
+			return ret;
+
+		entry->bi.scrubbed = false;
+	}
+
+	return 0;
+}
+
+/**
+ * vmw_context_binding_list - Return a list of context bindings
+ *
+ * @ctx: The context resource
+ *
+ * Returns the current list of bindings of the given context. Note that
+ * this list becomes stale as soon as the dev_priv::binding_mutex is unlocked.
+ */
+struct list_head *vmw_context_binding_list(struct vmw_resource *ctx)
+{
+	return &(container_of(ctx, struct vmw_user_context, res)->cbs.list);
+}
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c b/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
index 9893328f8fdc..3bdc0adc656d 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
@@ -941,6 +941,7 @@ static void vmw_postclose(struct drm_device *dev,
 		drm_master_put(&vmw_fp->locked_master);
 	}
 
+	vmw_compat_shader_man_destroy(vmw_fp->shman);
 	ttm_object_file_release(&vmw_fp->tfile);
 	kfree(vmw_fp);
 }
@@ -960,11 +961,17 @@ static int vmw_driver_open(struct drm_device *dev, struct drm_file *file_priv)
 	if (unlikely(vmw_fp->tfile == NULL))
 		goto out_no_tfile;
 
+	vmw_fp->shman = vmw_compat_shader_man_create(dev_priv);
+	if (IS_ERR(vmw_fp->shman))
+		goto out_no_shman;
+
 	file_priv->driver_priv = vmw_fp;
 	dev_priv->bdev.dev_mapping = dev->dev_mapping;
 
 	return 0;
 
+out_no_shman:
+	ttm_object_file_release(&vmw_fp->tfile);
 out_no_tfile:
 	kfree(vmw_fp);
 	return ret;
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_drv.h b/drivers/gpu/drm/vmwgfx/vmwgfx_drv.h
index 554e7fa33082..ecaa302a6154 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_drv.h
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_drv.h
@@ -75,10 +75,14 @@
 #define VMW_RES_FENCE ttm_driver_type3
 #define VMW_RES_SHADER ttm_driver_type4
 
+struct vmw_compat_shader_manager;
+
 struct vmw_fpriv {
 	struct drm_master *locked_master;
 	struct ttm_object_file *tfile;
 	struct list_head fence_events;
+	bool gb_aware;
+	struct vmw_compat_shader_manager *shman;
 };
 
 struct vmw_dma_buffer {
@@ -272,6 +276,7 @@ struct vmw_ctx_bindinfo {
 	struct vmw_resource *ctx;
 	struct vmw_resource *res;
 	enum vmw_ctx_binding_type bt;
+	bool scrubbed;
 	union {
 		SVGA3dShaderType shader_type;
 		SVGA3dRenderTargetType rt_type;
@@ -318,7 +323,7 @@ struct vmw_sw_context{
 	struct drm_open_hash res_ht;
 	bool res_ht_initialized;
 	bool kernel; /**< is the called made from the kernel */
-	struct ttm_object_file *tfile;
+	struct vmw_fpriv *fp;
 	struct list_head validate_nodes;
 	struct vmw_relocation relocs[VMWGFX_MAX_RELOCATIONS];
 	uint32_t cur_reloc;
@@ -336,6 +341,7 @@ struct vmw_sw_context{
 	bool needs_post_query_barrier;
 	struct vmw_resource *error_resource;
 	struct vmw_ctx_binding_state staged_bindings;
+	struct list_head staged_shaders;
 };
 
 struct vmw_legacy_display;
@@ -569,6 +575,8 @@ struct vmw_user_resource_conv;
 
 extern void vmw_resource_unreference(struct vmw_resource **p_res);
 extern struct vmw_resource *vmw_resource_reference(struct vmw_resource *res);
+extern struct vmw_resource *
+vmw_resource_reference_unless_doomed(struct vmw_resource *res);
 extern int vmw_resource_validate(struct vmw_resource *res);
 extern int vmw_resource_reserve(struct vmw_resource *res, bool no_backup);
 extern bool vmw_resource_needs_backup(const struct vmw_resource *res);
@@ -957,6 +965,9 @@ extern void
 vmw_context_binding_state_transfer(struct vmw_resource *res,
 				   struct vmw_ctx_binding_state *cbs);
 extern void vmw_context_binding_res_list_kill(struct list_head *head);
+extern void vmw_context_binding_res_list_scrub(struct list_head *head);
+extern int vmw_context_rebind_all(struct vmw_resource *ctx);
+extern struct list_head *vmw_context_binding_list(struct vmw_resource *ctx);
 
 /*
  * Surface management - vmwgfx_surface.c
@@ -991,6 +1002,28 @@ extern int vmw_shader_define_ioctl(struct drm_device *dev, void *data,
 				   struct drm_file *file_priv);
 extern int vmw_shader_destroy_ioctl(struct drm_device *dev, void *data,
 				    struct drm_file *file_priv);
+extern int vmw_compat_shader_lookup(struct vmw_compat_shader_manager *man,
+				    SVGA3dShaderType shader_type,
+				    u32 *user_key);
+extern void vmw_compat_shaders_commit(struct vmw_compat_shader_manager *man,
+				      struct list_head *list);
+extern void vmw_compat_shaders_revert(struct vmw_compat_shader_manager *man,
+				      struct list_head *list);
+extern int vmw_compat_shader_remove(struct vmw_compat_shader_manager *man,
+				    u32 user_key,
+				    SVGA3dShaderType shader_type,
+				    struct list_head *list);
+extern int vmw_compat_shader_add(struct vmw_compat_shader_manager *man,
+				 u32 user_key, const void *bytecode,
+				 SVGA3dShaderType shader_type,
+				 size_t size,
+				 struct ttm_object_file *tfile,
+				 struct list_head *list);
+extern struct vmw_compat_shader_manager *
+vmw_compat_shader_man_create(struct vmw_private *dev_priv);
+extern void
+vmw_compat_shader_man_destroy(struct vmw_compat_shader_manager *man);
+
 
 /**
  * Inline helper functions
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_execbuf.c b/drivers/gpu/drm/vmwgfx/vmwgfx_execbuf.c
index 7a5f1eb55c5a..269b85cc875a 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_execbuf.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_execbuf.c
@@ -114,8 +114,10 @@ static void vmw_resource_list_unreserve(struct list_head *list,
 		 * persistent context binding tracker.
 		 */
 		if (unlikely(val->staged_bindings)) {
-			vmw_context_binding_state_transfer
-				(val->res, val->staged_bindings);
+			if (!backoff) {
+				vmw_context_binding_state_transfer
+					(val->res, val->staged_bindings);
+			}
 			kfree(val->staged_bindings);
 			val->staged_bindings = NULL;
 		}
@@ -178,6 +180,44 @@ static int vmw_resource_val_add(struct vmw_sw_context *sw_context,
 }
 
 /**
+ * vmw_resource_context_res_add - Put resources previously bound to a context on
+ * the validation list
+ *
+ * @dev_priv: Pointer to a device private structure
+ * @sw_context: Pointer to a software context used for this command submission
+ * @ctx: Pointer to the context resource
+ *
+ * This function puts all resources that were previously bound to @ctx on
+ * the resource validation list. This is part of the context state reemission
+ */
+static int vmw_resource_context_res_add(struct vmw_private *dev_priv,
+					struct vmw_sw_context *sw_context,
+					struct vmw_resource *ctx)
+{
+	struct list_head *binding_list;
+	struct vmw_ctx_binding *entry;
+	int ret = 0;
+	struct vmw_resource *res;
+
+	mutex_lock(&dev_priv->binding_mutex);
+	binding_list = vmw_context_binding_list(ctx);
+
+	list_for_each_entry(entry, binding_list, ctx_list) {
+		res = vmw_resource_reference_unless_doomed(entry->bi.res);
+		if (unlikely(res == NULL))
+			continue;
+
+		ret = vmw_resource_val_add(sw_context, entry->bi.res, NULL);
+		vmw_resource_unreference(&res);
+		if (unlikely(ret != 0))
+			break;
+	}
+
+	mutex_unlock(&dev_priv->binding_mutex);
+	return ret;
+}
+
+/**
  * vmw_resource_relocation_add - Add a relocation to the relocation list
  *
  * @list: Pointer to head of relocation list.
@@ -233,8 +273,12 @@ static void vmw_resource_relocations_apply(uint32_t *cb,
 {
 	struct vmw_resource_relocation *rel;
 
-	list_for_each_entry(rel, list, head)
-		cb[rel->offset] = rel->res->id;
+	list_for_each_entry(rel, list, head) {
+		if (likely(rel->res != NULL))
+			cb[rel->offset] = rel->res->id;
+		else
+			cb[rel->offset] = SVGA_3D_CMD_NOP;
+	}
 }
 
 static int vmw_cmd_invalid(struct vmw_private *dev_priv,
@@ -379,22 +423,27 @@ static int vmw_resources_validate(struct vmw_sw_context *sw_context)
 }
 
 /**
- * vmw_cmd_res_check - Check that a resource is present and if so, put it
+ * vmw_cmd_compat_res_check - Check that a resource is present and if so, put it
  * on the resource validate list unless it's already there.
  *
  * @dev_priv: Pointer to a device private structure.
  * @sw_context: Pointer to the software context.
  * @res_type: Resource type.
  * @converter: User-space visisble type specific information.
- * @id: Pointer to the location in the command buffer currently being
+ * @id: user-space resource id handle.
+ * @id_loc: Pointer to the location in the command buffer currently being
  * parsed from where the user-space resource id handle is located.
+ * @p_val: Pointer to pointer to resource validalidation node. Populated
+ * on exit.
  */
-static int vmw_cmd_res_check(struct vmw_private *dev_priv,
-			     struct vmw_sw_context *sw_context,
-			     enum vmw_res_type res_type,
-			     const struct vmw_user_resource_conv *converter,
-			     uint32_t *id,
-			     struct vmw_resource_val_node **p_val)
+static int
+vmw_cmd_compat_res_check(struct vmw_private *dev_priv,
+			 struct vmw_sw_context *sw_context,
+			 enum vmw_res_type res_type,
+			 const struct vmw_user_resource_conv *converter,
+			 uint32_t id,
+			 uint32_t *id_loc,
+			 struct vmw_resource_val_node **p_val)
 {
 	struct vmw_res_cache_entry *rcache =
 		&sw_context->res_cache[res_type];
@@ -402,7 +451,7 @@ static int vmw_cmd_res_check(struct vmw_private *dev_priv,
 	struct vmw_resource_val_node *node;
 	int ret;
 
-	if (*id == SVGA3D_INVALID_ID) {
+	if (id == SVGA3D_INVALID_ID) {
 		if (p_val)
 			*p_val = NULL;
 		if (res_type == vmw_res_context) {
@@ -417,7 +466,7 @@ static int vmw_cmd_res_check(struct vmw_private *dev_priv,
 	 * resource
 	 */
 
-	if (likely(rcache->valid && *id == rcache->handle)) {
+	if (likely(rcache->valid && id == rcache->handle)) {
 		const struct vmw_resource *res = rcache->res;
 
 		rcache->node->first_usage = false;
@@ -426,28 +475,28 @@ static int vmw_cmd_res_check(struct vmw_private *dev_priv,
 
 		return vmw_resource_relocation_add
 			(&sw_context->res_relocations, res,
-			 id - sw_context->buf_start);
+			 id_loc - sw_context->buf_start);
 	}
 
 	ret = vmw_user_resource_lookup_handle(dev_priv,
-					      sw_context->tfile,
-					      *id,
+					      sw_context->fp->tfile,
+					      id,
 					      converter,
 					      &res);
 	if (unlikely(ret != 0)) {
 		DRM_ERROR("Could not find or use resource 0x%08x.\n",
-			  (unsigned) *id);
+			  (unsigned) id);
 		dump_stack();
 		return ret;
 	}
 
 	rcache->valid = true;
 	rcache->res = res;
-	rcache->handle = *id;
+	rcache->handle = id;
 
 	ret = vmw_resource_relocation_add(&sw_context->res_relocations,
 					  res,
-					  id - sw_context->buf_start);
+					  id_loc - sw_context->buf_start);
 	if (unlikely(ret != 0))
 		goto out_no_reloc;
 
@@ -459,7 +508,11 @@ static int vmw_cmd_res_check(struct vmw_private *dev_priv,
 	if (p_val)
 		*p_val = node;
 
-	if (node->first_usage && res_type == vmw_res_context) {
+	if (dev_priv->has_mob && node->first_usage &&
+	    res_type == vmw_res_context) {
+		ret = vmw_resource_context_res_add(dev_priv, sw_context, res);
+		if (unlikely(ret != 0))
+			goto out_no_reloc;
 		node->staged_bindings =
 			kzalloc(sizeof(*node->staged_bindings), GFP_KERNEL);
 		if (node->staged_bindings == NULL) {
@@ -481,6 +534,59 @@ out_no_reloc:
 }
 
 /**
+ * vmw_cmd_res_check - Check that a resource is present and if so, put it
+ * on the resource validate list unless it's already there.
+ *
+ * @dev_priv: Pointer to a device private structure.
+ * @sw_context: Pointer to the software context.
+ * @res_type: Resource type.
+ * @converter: User-space visisble type specific information.
+ * @id_loc: Pointer to the location in the command buffer currently being
+ * parsed from where the user-space resource id handle is located.
+ * @p_val: Pointer to pointer to resource validalidation node. Populated
+ * on exit.
+ */
+static int
+vmw_cmd_res_check(struct vmw_private *dev_priv,
+		  struct vmw_sw_context *sw_context,
+		  enum vmw_res_type res_type,
+		  const struct vmw_user_resource_conv *converter,
+		  uint32_t *id_loc,
+		  struct vmw_resource_val_node **p_val)
+{
+	return vmw_cmd_compat_res_check(dev_priv, sw_context, res_type,
+					converter, *id_loc, id_loc, p_val);
+}
+
+/**
+ * vmw_rebind_contexts - Rebind all resources previously bound to
+ * referenced contexts.
+ *
+ * @sw_context: Pointer to the software context.
+ *
+ * Rebind context binding points that have been scrubbed because of eviction.
+ */
+static int vmw_rebind_contexts(struct vmw_sw_context *sw_context)
+{
+	struct vmw_resource_val_node *val;
+	int ret;
+
+	list_for_each_entry(val, &sw_context->resource_list, head) {
+		if (likely(!val->staged_bindings))
+			continue;
+
+		ret = vmw_context_rebind_all(val->res);
+		if (unlikely(ret != 0)) {
+			if (ret != -ERESTARTSYS)
+				DRM_ERROR("Failed to rebind context.\n");
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+/**
  * vmw_cmd_cid_check - Check a command header for valid context information.
  *
  * @dev_priv: Pointer to a device private structure.
@@ -767,7 +873,7 @@ static int vmw_translate_mob_ptr(struct vmw_private *dev_priv,
 	struct vmw_relocation *reloc;
 	int ret;
 
-	ret = vmw_user_dmabuf_lookup(sw_context->tfile, handle, &vmw_bo);
+	ret = vmw_user_dmabuf_lookup(sw_context->fp->tfile, handle, &vmw_bo);
 	if (unlikely(ret != 0)) {
 		DRM_ERROR("Could not find or use MOB buffer.\n");
 		return -EINVAL;
@@ -828,7 +934,7 @@ static int vmw_translate_guest_ptr(struct vmw_private *dev_priv,
 	struct vmw_relocation *reloc;
 	int ret;
 
-	ret = vmw_user_dmabuf_lookup(sw_context->tfile, handle, &vmw_bo);
+	ret = vmw_user_dmabuf_lookup(sw_context->fp->tfile, handle, &vmw_bo);
 	if (unlikely(ret != 0)) {
 		DRM_ERROR("Could not find or use GMR region.\n");
 		return -EINVAL;
@@ -1127,7 +1233,8 @@ static int vmw_cmd_dma(struct vmw_private *dev_priv,
 
 	srf = vmw_res_to_srf(sw_context->res_cache[vmw_res_surface].res);
 
-	vmw_kms_cursor_snoop(srf, sw_context->tfile, &vmw_bo->base, header);
+	vmw_kms_cursor_snoop(srf, sw_context->fp->tfile, &vmw_bo->base,
+			     header);
 
 out_no_surface:
 	vmw_dmabuf_unreference(&vmw_bo);
@@ -1478,6 +1585,98 @@ static int vmw_cmd_invalidate_gb_surface(struct vmw_private *dev_priv,
 				 &cmd->body.sid, NULL);
 }
 
+
+/**
+ * vmw_cmd_shader_define - Validate an SVGA_3D_CMD_SHADER_DEFINE
+ * command
+ *
+ * @dev_priv: Pointer to a device private struct.
+ * @sw_context: The software context being used for this batch.
+ * @header: Pointer to the command header in the command stream.
+ */
+static int vmw_cmd_shader_define(struct vmw_private *dev_priv,
+				 struct vmw_sw_context *sw_context,
+				 SVGA3dCmdHeader *header)
+{
+	struct vmw_shader_define_cmd {
+		SVGA3dCmdHeader header;
+		SVGA3dCmdDefineShader body;
+	} *cmd;
+	int ret;
+	size_t size;
+
+	cmd = container_of(header, struct vmw_shader_define_cmd,
+			   header);
+
+	ret = vmw_cmd_res_check(dev_priv, sw_context, vmw_res_context,
+				user_context_converter, &cmd->body.cid,
+				NULL);
+	if (unlikely(ret != 0))
+		return ret;
+
+	if (unlikely(!dev_priv->has_mob))
+		return 0;
+
+	size = cmd->header.size - sizeof(cmd->body);
+	ret = vmw_compat_shader_add(sw_context->fp->shman,
+				    cmd->body.shid, cmd + 1,
+				    cmd->body.type, size,
+				    sw_context->fp->tfile,
+				    &sw_context->staged_shaders);
+	if (unlikely(ret != 0))
+		return ret;
+
+	return vmw_resource_relocation_add(&sw_context->res_relocations,
+					   NULL, &cmd->header.id -
+					   sw_context->buf_start);
+
+	return 0;
+}
+
+/**
+ * vmw_cmd_shader_destroy - Validate an SVGA_3D_CMD_SHADER_DESTROY
+ * command
+ *
+ * @dev_priv: Pointer to a device private struct.
+ * @sw_context: The software context being used for this batch.
+ * @header: Pointer to the command header in the command stream.
+ */
+static int vmw_cmd_shader_destroy(struct vmw_private *dev_priv,
+				  struct vmw_sw_context *sw_context,
+				  SVGA3dCmdHeader *header)
+{
+	struct vmw_shader_destroy_cmd {
+		SVGA3dCmdHeader header;
+		SVGA3dCmdDestroyShader body;
+	} *cmd;
+	int ret;
+
+	cmd = container_of(header, struct vmw_shader_destroy_cmd,
+			   header);
+
+	ret = vmw_cmd_res_check(dev_priv, sw_context, vmw_res_context,
+				user_context_converter, &cmd->body.cid,
+				NULL);
+	if (unlikely(ret != 0))
+		return ret;
+
+	if (unlikely(!dev_priv->has_mob))
+		return 0;
+
+	ret = vmw_compat_shader_remove(sw_context->fp->shman,
+				       cmd->body.shid,
+				       cmd->body.type,
+				       &sw_context->staged_shaders);
+	if (unlikely(ret != 0))
+		return ret;
+
+	return vmw_resource_relocation_add(&sw_context->res_relocations,
+					   NULL, &cmd->header.id -
+					   sw_context->buf_start);
+
+	return 0;
+}
+
 /**
  * vmw_cmd_set_shader - Validate an SVGA_3D_CMD_SET_SHADER
  * command
@@ -1509,10 +1708,18 @@ static int vmw_cmd_set_shader(struct vmw_private *dev_priv,
 	if (dev_priv->has_mob) {
 		struct vmw_ctx_bindinfo bi;
 		struct vmw_resource_val_node *res_node;
-
-		ret = vmw_cmd_res_check(dev_priv, sw_context, vmw_res_shader,
-					user_shader_converter,
-					&cmd->body.shid, &res_node);
+		u32 shid = cmd->body.shid;
+
+		if (shid != SVGA3D_INVALID_ID)
+			(void) vmw_compat_shader_lookup(sw_context->fp->shman,
+							cmd->body.type,
+							&shid);
+
+		ret = vmw_cmd_compat_res_check(dev_priv, sw_context,
+					       vmw_res_shader,
+					       user_shader_converter,
+					       shid,
+					       &cmd->body.shid, &res_node);
 		if (unlikely(ret != 0))
 			return ret;
 
@@ -1527,6 +1734,39 @@ static int vmw_cmd_set_shader(struct vmw_private *dev_priv,
 }
 
 /**
+ * vmw_cmd_set_shader_const - Validate an SVGA_3D_CMD_SET_SHADER_CONST
+ * command
+ *
+ * @dev_priv: Pointer to a device private struct.
+ * @sw_context: The software context being used for this batch.
+ * @header: Pointer to the command header in the command stream.
+ */
+static int vmw_cmd_set_shader_const(struct vmw_private *dev_priv,
+				    struct vmw_sw_context *sw_context,
+				    SVGA3dCmdHeader *header)
+{
+	struct vmw_set_shader_const_cmd {
+		SVGA3dCmdHeader header;
+		SVGA3dCmdSetShaderConst body;
+	} *cmd;
+	int ret;
+
+	cmd = container_of(header, struct vmw_set_shader_const_cmd,
+			   header);
+
+	ret = vmw_cmd_res_check(dev_priv, sw_context, vmw_res_context,
+				user_context_converter, &cmd->body.cid,
+				NULL);
+	if (unlikely(ret != 0))
+		return ret;
+
+	if (dev_priv->has_mob)
+		header->id = SVGA_3D_CMD_SET_GB_SHADERCONSTS_INLINE;
+
+	return 0;
+}
+
+/**
  * vmw_cmd_bind_gb_shader - Validate an SVGA_3D_CMD_BIND_GB_SHADER
  * command
  *
@@ -1634,14 +1874,14 @@ static const struct vmw_cmd_entry const vmw_cmd_entries[SVGA_3D_CMD_MAX] = {
 		    true, false, false),
 	VMW_CMD_DEF(SVGA_3D_CMD_PRESENT, &vmw_cmd_present_check,
 		    false, false, false),
-	VMW_CMD_DEF(SVGA_3D_CMD_SHADER_DEFINE, &vmw_cmd_cid_check,
-		    true, true, false),
-	VMW_CMD_DEF(SVGA_3D_CMD_SHADER_DESTROY, &vmw_cmd_cid_check,
-		    true, true, false),
+	VMW_CMD_DEF(SVGA_3D_CMD_SHADER_DEFINE, &vmw_cmd_shader_define,
+		    true, false, false),
+	VMW_CMD_DEF(SVGA_3D_CMD_SHADER_DESTROY, &vmw_cmd_shader_destroy,
+		    true, false, false),
 	VMW_CMD_DEF(SVGA_3D_CMD_SET_SHADER, &vmw_cmd_set_shader,
 		    true, false, false),
-	VMW_CMD_DEF(SVGA_3D_CMD_SET_SHADER_CONST, &vmw_cmd_cid_check,
-		    true, true, false),
+	VMW_CMD_DEF(SVGA_3D_CMD_SET_SHADER_CONST, &vmw_cmd_set_shader_const,
+		    true, false, false),
 	VMW_CMD_DEF(SVGA_3D_CMD_DRAW_PRIMITIVES, &vmw_cmd_draw,
 		    true, false, false),
 	VMW_CMD_DEF(SVGA_3D_CMD_SETSCISSORRECT, &vmw_cmd_cid_check,
@@ -2171,7 +2411,7 @@ int vmw_execbuf_process(struct drm_file *file_priv,
 	} else
 		sw_context->kernel = true;
 
-	sw_context->tfile = vmw_fpriv(file_priv)->tfile;
+	sw_context->fp = vmw_fpriv(file_priv);
 	sw_context->cur_reloc = 0;
 	sw_context->cur_val_buf = 0;
 	sw_context->fence_flags = 0;
@@ -2188,16 +2428,17 @@ int vmw_execbuf_process(struct drm_file *file_priv,
 			goto out_unlock;
 		sw_context->res_ht_initialized = true;
 	}
+	INIT_LIST_HEAD(&sw_context->staged_shaders);
 
 	INIT_LIST_HEAD(&resource_list);
 	ret = vmw_cmd_check_all(dev_priv, sw_context, kernel_commands,
 				command_size);
 	if (unlikely(ret != 0))
-		goto out_err;
+		goto out_err_nores;
 
 	ret = vmw_resources_reserve(sw_context);
 	if (unlikely(ret != 0))
-		goto out_err;
+		goto out_err_nores;
 
 	ret = ttm_eu_reserve_buffers(&ticket, &sw_context->validate_nodes);
 	if (unlikely(ret != 0))
@@ -2225,6 +2466,12 @@ int vmw_execbuf_process(struct drm_file *file_priv,
 		goto out_err;
 	}
 
+	if (dev_priv->has_mob) {
+		ret = vmw_rebind_contexts(sw_context);
+		if (unlikely(ret != 0))
+			goto out_err;
+	}
+
 	cmd = vmw_fifo_reserve(dev_priv, command_size);
 	if (unlikely(cmd == NULL)) {
 		DRM_ERROR("Failed reserving fifo space for commands.\n");
@@ -2276,6 +2523,8 @@ int vmw_execbuf_process(struct drm_file *file_priv,
 	}
 
 	list_splice_init(&sw_context->resource_list, &resource_list);
+	vmw_compat_shaders_commit(sw_context->fp->shman,
+				  &sw_context->staged_shaders);
 	mutex_unlock(&dev_priv->cmdbuf_mutex);
 
 	/*
@@ -2289,10 +2538,11 @@ int vmw_execbuf_process(struct drm_file *file_priv,
 out_unlock_binding:
 	mutex_unlock(&dev_priv->binding_mutex);
 out_err:
-	vmw_resource_relocations_free(&sw_context->res_relocations);
-	vmw_free_relocations(sw_context);
 	ttm_eu_backoff_reservation(&ticket, &sw_context->validate_nodes);
+out_err_nores:
 	vmw_resource_list_unreserve(&sw_context->resource_list, true);
+	vmw_resource_relocations_free(&sw_context->res_relocations);
+	vmw_free_relocations(sw_context);
 	vmw_clear_validations(sw_context);
 	if (unlikely(dev_priv->pinned_bo != NULL &&
 		     !dev_priv->query_cid_valid))
@@ -2301,6 +2551,8 @@ out_unlock:
 	list_splice_init(&sw_context->resource_list, &resource_list);
 	error_resource = sw_context->error_resource;
 	sw_context->error_resource = NULL;
+	vmw_compat_shaders_revert(sw_context->fp->shman,
+				  &sw_context->staged_shaders);
 	mutex_unlock(&dev_priv->cmdbuf_mutex);
 
 	/*
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_ioctl.c b/drivers/gpu/drm/vmwgfx/vmwgfx_ioctl.c
index 116c49736763..f9881f9e62bd 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_ioctl.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_ioctl.c
@@ -29,12 +29,18 @@
 #include <drm/vmwgfx_drm.h>
 #include "vmwgfx_kms.h"
 
+struct svga_3d_compat_cap {
+	SVGA3dCapsRecordHeader header;
+	SVGA3dCapPair pairs[SVGA3D_DEVCAP_MAX];
+};
+
 int vmw_getparam_ioctl(struct drm_device *dev, void *data,
 		       struct drm_file *file_priv)
 {
 	struct vmw_private *dev_priv = vmw_priv(dev);
 	struct drm_vmw_getparam_arg *param =
 	    (struct drm_vmw_getparam_arg *)data;
+	struct vmw_fpriv *vmw_fp = vmw_fpriv(file_priv);
 
 	switch (param->param) {
 	case DRM_VMW_PARAM_NUM_STREAMS:
@@ -60,6 +66,11 @@ int vmw_getparam_ioctl(struct drm_device *dev, void *data,
 		__le32 __iomem *fifo_mem = dev_priv->mmio_virt;
 		const struct vmw_fifo_state *fifo = &dev_priv->fifo;
 
+		if ((dev_priv->capabilities & SVGA_CAP_GBOBJECTS)) {
+			param->value = SVGA3D_HWVERSION_WS8_B1;
+			break;
+		}
+
 		param->value =
 			ioread32(fifo_mem +
 				 ((fifo->capabilities &
@@ -69,17 +80,26 @@ int vmw_getparam_ioctl(struct drm_device *dev, void *data,
 		break;
 	}
 	case DRM_VMW_PARAM_MAX_SURF_MEMORY:
-		param->value = dev_priv->memory_size;
+		if ((dev_priv->capabilities & SVGA_CAP_GBOBJECTS) &&
+		    !vmw_fp->gb_aware)
+			param->value = dev_priv->max_mob_pages * PAGE_SIZE / 2;
+		else
+			param->value = dev_priv->memory_size;
 		break;
 	case DRM_VMW_PARAM_3D_CAPS_SIZE:
-		if (dev_priv->capabilities & SVGA_CAP_GBOBJECTS)
-			param->value = SVGA3D_DEVCAP_MAX;
+		if ((dev_priv->capabilities & SVGA_CAP_GBOBJECTS) &&
+		    vmw_fp->gb_aware)
+			param->value = SVGA3D_DEVCAP_MAX * sizeof(uint32_t);
+		else if (dev_priv->capabilities & SVGA_CAP_GBOBJECTS)
+			param->value = sizeof(struct svga_3d_compat_cap) +
+				sizeof(uint32_t);
 		else
 			param->value = (SVGA_FIFO_3D_CAPS_LAST -
-					SVGA_FIFO_3D_CAPS + 1);
-		param->value *= sizeof(uint32_t);
+					SVGA_FIFO_3D_CAPS + 1) *
+				sizeof(uint32_t);
 		break;
 	case DRM_VMW_PARAM_MAX_MOB_MEMORY:
+		vmw_fp->gb_aware = true;
 		param->value = dev_priv->max_mob_pages * PAGE_SIZE;
 		break;
 	default:
@@ -91,6 +111,38 @@ int vmw_getparam_ioctl(struct drm_device *dev, void *data,
 	return 0;
 }
 
+static int vmw_fill_compat_cap(struct vmw_private *dev_priv, void *bounce,
+			       size_t size)
+{
+	struct svga_3d_compat_cap *compat_cap =
+		(struct svga_3d_compat_cap *) bounce;
+	unsigned int i;
+	size_t pair_offset = offsetof(struct svga_3d_compat_cap, pairs);
+	unsigned int max_size;
+
+	if (size < pair_offset)
+		return -EINVAL;
+
+	max_size = (size - pair_offset) / sizeof(SVGA3dCapPair);
+
+	if (max_size > SVGA3D_DEVCAP_MAX)
+		max_size = SVGA3D_DEVCAP_MAX;
+
+	compat_cap->header.length =
+		(pair_offset + max_size * sizeof(SVGA3dCapPair)) / sizeof(u32);
+	compat_cap->header.type = SVGA3DCAPS_RECORD_DEVCAPS;
+
+	mutex_lock(&dev_priv->hw_mutex);
+	for (i = 0; i < max_size; ++i) {
+		vmw_write(dev_priv, SVGA_REG_DEV_CAP, i);
+		compat_cap->pairs[i][0] = i;
+		compat_cap->pairs[i][1] = vmw_read(dev_priv, SVGA_REG_DEV_CAP);
+	}
+	mutex_unlock(&dev_priv->hw_mutex);
+
+	return 0;
+}
+
 
 int vmw_get_cap_3d_ioctl(struct drm_device *dev, void *data,
 			 struct drm_file *file_priv)
@@ -104,41 +156,49 @@ int vmw_get_cap_3d_ioctl(struct drm_device *dev, void *data,
 	void *bounce;
 	int ret;
 	bool gb_objects = !!(dev_priv->capabilities & SVGA_CAP_GBOBJECTS);
+	struct vmw_fpriv *vmw_fp = vmw_fpriv(file_priv);
 
 	if (unlikely(arg->pad64 != 0)) {
 		DRM_ERROR("Illegal GET_3D_CAP argument.\n");
 		return -EINVAL;
 	}
 
-	if (gb_objects)
-		size = SVGA3D_DEVCAP_MAX;
+	if (gb_objects && vmw_fp->gb_aware)
+		size = SVGA3D_DEVCAP_MAX * sizeof(uint32_t);
+	else if (gb_objects)
+		size = sizeof(struct svga_3d_compat_cap) + sizeof(uint32_t);
 	else
-		size = (SVGA_FIFO_3D_CAPS_LAST - SVGA_FIFO_3D_CAPS + 1);
-
-	size *= sizeof(uint32_t);
+		size = (SVGA_FIFO_3D_CAPS_LAST - SVGA_FIFO_3D_CAPS + 1) *
+			sizeof(uint32_t);
 
 	if (arg->max_size < size)
 		size = arg->max_size;
 
-	bounce = vmalloc(size);
+	bounce = vzalloc(size);
 	if (unlikely(bounce == NULL)) {
 		DRM_ERROR("Failed to allocate bounce buffer for 3D caps.\n");
 		return -ENOMEM;
 	}
 
-	if (gb_objects) {
-		int i;
+	if (gb_objects && vmw_fp->gb_aware) {
+		int i, num;
 		uint32_t *bounce32 = (uint32_t *) bounce;
 
+		num = size / sizeof(uint32_t);
+		if (num > SVGA3D_DEVCAP_MAX)
+			num = SVGA3D_DEVCAP_MAX;
+
 		mutex_lock(&dev_priv->hw_mutex);
-		for (i = 0; i < SVGA3D_DEVCAP_MAX; ++i) {
+		for (i = 0; i < num; ++i) {
 			vmw_write(dev_priv, SVGA_REG_DEV_CAP, i);
 			*bounce32++ = vmw_read(dev_priv, SVGA_REG_DEV_CAP);
 		}
 		mutex_unlock(&dev_priv->hw_mutex);
-
+	} else if (gb_objects) {
+		ret = vmw_fill_compat_cap(dev_priv, bounce, size);
+		if (unlikely(ret != 0))
+			goto out_err;
 	} else {
-
 		fifo_mem = dev_priv->mmio_virt;
 		memcpy_fromio(bounce, &fifo_mem[SVGA_FIFO_3D_CAPS], size);
 	}
@@ -146,6 +206,7 @@ int vmw_get_cap_3d_ioctl(struct drm_device *dev, void *data,
 	ret = copy_to_user(buffer, bounce, size);
 	if (ret)
 		ret = -EFAULT;
+out_err:
 	vfree(bounce);
 
 	if (unlikely(ret != 0))
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_mob.c b/drivers/gpu/drm/vmwgfx/vmwgfx_mob.c
index 4910e7b81811..d4a5a19cb8c3 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_mob.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_mob.c
@@ -134,6 +134,7 @@ static int vmw_setup_otable_base(struct vmw_private *dev_priv,
 	cmd = vmw_fifo_reserve(dev_priv, sizeof(*cmd));
 	if (unlikely(cmd == NULL)) {
 		DRM_ERROR("Failed reserving FIFO space for OTable setup.\n");
+		ret = -ENOMEM;
 		goto out_no_fifo;
 	}
 
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_resource.c b/drivers/gpu/drm/vmwgfx/vmwgfx_resource.c
index 6fdd82d42f65..2aa4bc6a4d60 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_resource.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_resource.c
@@ -88,6 +88,11 @@ struct vmw_resource *vmw_resource_reference(struct vmw_resource *res)
 	return res;
 }
 
+struct vmw_resource *
+vmw_resource_reference_unless_doomed(struct vmw_resource *res)
+{
+	return kref_get_unless_zero(&res->kref) ? res : NULL;
+}
 
 /**
  * vmw_resource_release_id - release a resource id to the id manager.
@@ -136,8 +141,12 @@ static void vmw_resource_release(struct kref *kref)
 		vmw_dmabuf_unreference(&res->backup);
 	}
 
-	if (likely(res->hw_destroy != NULL))
+	if (likely(res->hw_destroy != NULL)) {
 		res->hw_destroy(res);
+		mutex_lock(&dev_priv->binding_mutex);
+		vmw_context_binding_res_list_kill(&res->binding_head);
+		mutex_unlock(&dev_priv->binding_mutex);
+	}
 
 	id = res->id;
 	if (res->res_free != NULL)
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_shader.c b/drivers/gpu/drm/vmwgfx/vmwgfx_shader.c
index 1457ec4b7125..217d941b8176 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_shader.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_shader.c
@@ -29,6 +29,8 @@
 #include "vmwgfx_resource_priv.h"
 #include "ttm/ttm_placement.h"
 
+#define VMW_COMPAT_SHADER_HT_ORDER 12
+
 struct vmw_shader {
 	struct vmw_resource res;
 	SVGA3dShaderType type;
@@ -40,6 +42,50 @@ struct vmw_user_shader {
 	struct vmw_shader shader;
 };
 
+/**
+ * enum vmw_compat_shader_state - Staging state for compat shaders
+ */
+enum vmw_compat_shader_state {
+	VMW_COMPAT_COMMITED,
+	VMW_COMPAT_ADD,
+	VMW_COMPAT_DEL
+};
+
+/**
+ * struct vmw_compat_shader - Metadata for compat shaders.
+ *
+ * @handle: The TTM handle of the guest backed shader.
+ * @tfile: The struct ttm_object_file the guest backed shader is registered
+ * with.
+ * @hash: Hash item for lookup.
+ * @head: List head for staging lists or the compat shader manager list.
+ * @state: Staging state.
+ *
+ * The structure is protected by the cmdbuf lock.
+ */
+struct vmw_compat_shader {
+	u32 handle;
+	struct ttm_object_file *tfile;
+	struct drm_hash_item hash;
+	struct list_head head;
+	enum vmw_compat_shader_state state;
+};
+
+/**
+ * struct vmw_compat_shader_manager - Compat shader manager.
+ *
+ * @shaders: Hash table containing staged and commited compat shaders
+ * @list: List of commited shaders.
+ * @dev_priv: Pointer to a device private structure.
+ *
+ * @shaders and @list are protected by the cmdbuf mutex for now.
+ */
+struct vmw_compat_shader_manager {
+	struct drm_open_hash shaders;
+	struct list_head list;
+	struct vmw_private *dev_priv;
+};
+
 static void vmw_user_shader_free(struct vmw_resource *res);
 static struct vmw_resource *
 vmw_user_shader_base_to_res(struct ttm_base_object *base);
@@ -258,7 +304,7 @@ static int vmw_gb_shader_destroy(struct vmw_resource *res)
 		return 0;
 
 	mutex_lock(&dev_priv->binding_mutex);
-	vmw_context_binding_res_list_kill(&res->binding_head);
+	vmw_context_binding_res_list_scrub(&res->binding_head);
 
 	cmd = vmw_fifo_reserve(dev_priv, sizeof(*cmd));
 	if (unlikely(cmd == NULL)) {
@@ -325,13 +371,81 @@ int vmw_shader_destroy_ioctl(struct drm_device *dev, void *data,
 					 TTM_REF_USAGE);
 }
 
+int vmw_shader_alloc(struct vmw_private *dev_priv,
+		     struct vmw_dma_buffer *buffer,
+		     size_t shader_size,
+		     size_t offset,
+		     SVGA3dShaderType shader_type,
+		     struct ttm_object_file *tfile,
+		     u32 *handle)
+{
+	struct vmw_user_shader *ushader;
+	struct vmw_resource *res, *tmp;
+	int ret;
+
+	/*
+	 * Approximate idr memory usage with 128 bytes. It will be limited
+	 * by maximum number_of shaders anyway.
+	 */
+	if (unlikely(vmw_user_shader_size == 0))
+		vmw_user_shader_size =
+			ttm_round_pot(sizeof(struct vmw_user_shader)) + 128;
+
+	ret = ttm_mem_global_alloc(vmw_mem_glob(dev_priv),
+				   vmw_user_shader_size,
+				   false, true);
+	if (unlikely(ret != 0)) {
+		if (ret != -ERESTARTSYS)
+			DRM_ERROR("Out of graphics memory for shader "
+				  "creation.\n");
+		goto out;
+	}
+
+	ushader = kzalloc(sizeof(*ushader), GFP_KERNEL);
+	if (unlikely(ushader == NULL)) {
+		ttm_mem_global_free(vmw_mem_glob(dev_priv),
+				    vmw_user_shader_size);
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	res = &ushader->shader.res;
+	ushader->base.shareable = false;
+	ushader->base.tfile = NULL;
+
+	/*
+	 * From here on, the destructor takes over resource freeing.
+	 */
+
+	ret = vmw_gb_shader_init(dev_priv, res, shader_size,
+				 offset, shader_type, buffer,
+				 vmw_user_shader_free);
+	if (unlikely(ret != 0))
+		goto out;
+
+	tmp = vmw_resource_reference(res);
+	ret = ttm_base_object_init(tfile, &ushader->base, false,
+				   VMW_RES_SHADER,
+				   &vmw_user_shader_base_release, NULL);
+
+	if (unlikely(ret != 0)) {
+		vmw_resource_unreference(&tmp);
+		goto out_err;
+	}
+
+	if (handle)
+		*handle = ushader->base.hash.key;
+out_err:
+	vmw_resource_unreference(&res);
+out:
+	return ret;
+}
+
+
 int vmw_shader_define_ioctl(struct drm_device *dev, void *data,
 			     struct drm_file *file_priv)
 {
 	struct vmw_private *dev_priv = vmw_priv(dev);
-	struct vmw_user_shader *ushader;
-	struct vmw_resource *res;
-	struct vmw_resource *tmp;
 	struct drm_vmw_shader_create_arg *arg =
 		(struct drm_vmw_shader_create_arg *)data;
 	struct ttm_object_file *tfile = vmw_fpriv(file_priv)->tfile;
@@ -373,69 +487,324 @@ int vmw_shader_define_ioctl(struct drm_device *dev, void *data,
 		goto out_bad_arg;
 	}
 
-	/*
-	 * Approximate idr memory usage with 128 bytes. It will be limited
-	 * by maximum number_of shaders anyway.
-	 */
+	ret = ttm_read_lock(&vmaster->lock, true);
+	if (unlikely(ret != 0))
+		goto out_bad_arg;
 
-	if (unlikely(vmw_user_shader_size == 0))
-		vmw_user_shader_size = ttm_round_pot(sizeof(*ushader))
-			+ 128;
+	ret = vmw_shader_alloc(dev_priv, buffer, arg->size, arg->offset,
+			       shader_type, tfile, &arg->shader_handle);
 
-	ret = ttm_read_lock(&vmaster->lock, true);
+	ttm_read_unlock(&vmaster->lock);
+out_bad_arg:
+	vmw_dmabuf_unreference(&buffer);
+	return ret;
+}
+
+/**
+ * vmw_compat_shader_lookup - Look up a compat shader
+ *
+ * @man: Pointer to the compat shader manager.
+ * @shader_type: The shader type, that combined with the user_key identifies
+ * the shader.
+ * @user_key: On entry, this should be a pointer to the user_key.
+ * On successful exit, it will contain the guest-backed shader's TTM handle.
+ *
+ * Returns 0 on success. Non-zero on failure, in which case the value pointed
+ * to by @user_key is unmodified.
+ */
+int vmw_compat_shader_lookup(struct vmw_compat_shader_manager *man,
+			     SVGA3dShaderType shader_type,
+			     u32 *user_key)
+{
+	struct drm_hash_item *hash;
+	int ret;
+	unsigned long key = *user_key | (shader_type << 24);
+
+	ret = drm_ht_find_item(&man->shaders, key, &hash);
 	if (unlikely(ret != 0))
 		return ret;
 
-	ret = ttm_mem_global_alloc(vmw_mem_glob(dev_priv),
-				   vmw_user_shader_size,
-				   false, true);
-	if (unlikely(ret != 0)) {
-		if (ret != -ERESTARTSYS)
-			DRM_ERROR("Out of graphics memory for shader"
-				  " creation.\n");
-		goto out_unlock;
+	*user_key = drm_hash_entry(hash, struct vmw_compat_shader,
+				   hash)->handle;
+
+	return 0;
+}
+
+/**
+ * vmw_compat_shader_free - Free a compat shader.
+ *
+ * @man: Pointer to the compat shader manager.
+ * @entry: Pointer to a struct vmw_compat_shader.
+ *
+ * Frees a struct vmw_compat_shder entry and drops its reference to the
+ * guest backed shader.
+ */
+static void vmw_compat_shader_free(struct vmw_compat_shader_manager *man,
+				   struct vmw_compat_shader *entry)
+{
+	list_del(&entry->head);
+	WARN_ON(drm_ht_remove_item(&man->shaders, &entry->hash));
+	WARN_ON(ttm_ref_object_base_unref(entry->tfile, entry->handle,
+					  TTM_REF_USAGE));
+	kfree(entry);
+}
+
+/**
+ * vmw_compat_shaders_commit - Commit a list of compat shader actions.
+ *
+ * @man: Pointer to the compat shader manager.
+ * @list: Caller's list of compat shader actions.
+ *
+ * This function commits a list of compat shader additions or removals.
+ * It is typically called when the execbuf ioctl call triggering these
+ * actions has commited the fifo contents to the device.
+ */
+void vmw_compat_shaders_commit(struct vmw_compat_shader_manager *man,
+			       struct list_head *list)
+{
+	struct vmw_compat_shader *entry, *next;
+
+	list_for_each_entry_safe(entry, next, list, head) {
+		list_del(&entry->head);
+		switch (entry->state) {
+		case VMW_COMPAT_ADD:
+			entry->state = VMW_COMPAT_COMMITED;
+			list_add_tail(&entry->head, &man->list);
+			break;
+		case VMW_COMPAT_DEL:
+			ttm_ref_object_base_unref(entry->tfile, entry->handle,
+						  TTM_REF_USAGE);
+			kfree(entry);
+			break;
+		default:
+			BUG();
+			break;
+		}
 	}
+}
 
-	ushader = kzalloc(sizeof(*ushader), GFP_KERNEL);
-	if (unlikely(ushader == NULL)) {
-		ttm_mem_global_free(vmw_mem_glob(dev_priv),
-				    vmw_user_shader_size);
-		ret = -ENOMEM;
-		goto out_unlock;
+/**
+ * vmw_compat_shaders_revert - Revert a list of compat shader actions
+ *
+ * @man: Pointer to the compat shader manager.
+ * @list: Caller's list of compat shader actions.
+ *
+ * This function reverts a list of compat shader additions or removals.
+ * It is typically called when the execbuf ioctl call triggering these
+ * actions failed for some reason, and the command stream was never
+ * submitted.
+ */
+void vmw_compat_shaders_revert(struct vmw_compat_shader_manager *man,
+			       struct list_head *list)
+{
+	struct vmw_compat_shader *entry, *next;
+	int ret;
+
+	list_for_each_entry_safe(entry, next, list, head) {
+		switch (entry->state) {
+		case VMW_COMPAT_ADD:
+			vmw_compat_shader_free(man, entry);
+			break;
+		case VMW_COMPAT_DEL:
+			ret = drm_ht_insert_item(&man->shaders, &entry->hash);
+			list_del(&entry->head);
+			list_add_tail(&entry->head, &man->list);
+			entry->state = VMW_COMPAT_COMMITED;
+			break;
+		default:
+			BUG();
+			break;
+		}
 	}
+}
 
-	res = &ushader->shader.res;
-	ushader->base.shareable = false;
-	ushader->base.tfile = NULL;
+/**
+ * vmw_compat_shader_remove - Stage a compat shader for removal.
+ *
+ * @man: Pointer to the compat shader manager
+ * @user_key: The key that is used to identify the shader. The key is
+ * unique to the shader type.
+ * @shader_type: Shader type.
+ * @list: Caller's list of staged shader actions.
+ *
+ * This function stages a compat shader for removal and removes the key from
+ * the shader manager's hash table. If the shader was previously only staged
+ * for addition it is completely removed (But the execbuf code may keep a
+ * reference if it was bound to a context between addition and removal). If
+ * it was previously commited to the manager, it is staged for removal.
+ */
+int vmw_compat_shader_remove(struct vmw_compat_shader_manager *man,
+			     u32 user_key, SVGA3dShaderType shader_type,
+			     struct list_head *list)
+{
+	struct vmw_compat_shader *entry;
+	struct drm_hash_item *hash;
+	int ret;
 
-	/*
-	 * From here on, the destructor takes over resource freeing.
-	 */
+	ret = drm_ht_find_item(&man->shaders, user_key | (shader_type << 24),
+			       &hash);
+	if (likely(ret != 0))
+		return -EINVAL;
 
-	ret = vmw_gb_shader_init(dev_priv, res, arg->size,
-				 arg->offset, shader_type, buffer,
-				 vmw_user_shader_free);
+	entry = drm_hash_entry(hash, struct vmw_compat_shader, hash);
+
+	switch (entry->state) {
+	case VMW_COMPAT_ADD:
+		vmw_compat_shader_free(man, entry);
+		break;
+	case VMW_COMPAT_COMMITED:
+		(void) drm_ht_remove_item(&man->shaders, &entry->hash);
+		list_del(&entry->head);
+		entry->state = VMW_COMPAT_DEL;
+		list_add_tail(&entry->head, list);
+		break;
+	default:
+		BUG();
+		break;
+	}
+
+	return 0;
+}
+
+/**
+ * vmw_compat_shader_add - Create a compat shader and add the
+ * key to the manager
+ *
+ * @man: Pointer to the compat shader manager
+ * @user_key: The key that is used to identify the shader. The key is
+ * unique to the shader type.
+ * @bytecode: Pointer to the bytecode of the shader.
+ * @shader_type: Shader type.
+ * @tfile: Pointer to a struct ttm_object_file that the guest-backed shader is
+ * to be created with.
+ * @list: Caller's list of staged shader actions.
+ *
+ * Note that only the key is added to the shader manager's hash table.
+ * The shader is not yet added to the shader manager's list of shaders.
+ */
+int vmw_compat_shader_add(struct vmw_compat_shader_manager *man,
+			  u32 user_key, const void *bytecode,
+			  SVGA3dShaderType shader_type,
+			  size_t size,
+			  struct ttm_object_file *tfile,
+			  struct list_head *list)
+{
+	struct vmw_dma_buffer *buf;
+	struct ttm_bo_kmap_obj map;
+	bool is_iomem;
+	struct vmw_compat_shader *compat;
+	u32 handle;
+	int ret;
+
+	if (user_key > ((1 << 24) - 1) || (unsigned) shader_type > 16)
+		return -EINVAL;
+
+	/* Allocate and pin a DMA buffer */
+	buf = kzalloc(sizeof(*buf), GFP_KERNEL);
+	if (unlikely(buf == NULL))
+		return -ENOMEM;
+
+	ret = vmw_dmabuf_init(man->dev_priv, buf, size, &vmw_sys_ne_placement,
+			      true, vmw_dmabuf_bo_free);
 	if (unlikely(ret != 0))
-		goto out_unlock;
+		goto out;
 
-	tmp = vmw_resource_reference(res);
-	ret = ttm_base_object_init(tfile, &ushader->base, false,
-				   VMW_RES_SHADER,
-				   &vmw_user_shader_base_release, NULL);
+	ret = ttm_bo_reserve(&buf->base, false, true, false, NULL);
+	if (unlikely(ret != 0))
+		goto no_reserve;
 
+	/* Map and copy shader bytecode. */
+	ret = ttm_bo_kmap(&buf->base, 0, PAGE_ALIGN(size) >> PAGE_SHIFT,
+			  &map);
 	if (unlikely(ret != 0)) {
-		vmw_resource_unreference(&tmp);
-		goto out_err;
+		ttm_bo_unreserve(&buf->base);
+		goto no_reserve;
 	}
 
-	arg->shader_handle = ushader->base.hash.key;
-out_err:
-	vmw_resource_unreference(&res);
-out_unlock:
-	ttm_read_unlock(&vmaster->lock);
-out_bad_arg:
-	vmw_dmabuf_unreference(&buffer);
+	memcpy(ttm_kmap_obj_virtual(&map, &is_iomem), bytecode, size);
+	WARN_ON(is_iomem);
+
+	ttm_bo_kunmap(&map);
+	ret = ttm_bo_validate(&buf->base, &vmw_sys_placement, false, true);
+	WARN_ON(ret != 0);
+	ttm_bo_unreserve(&buf->base);
+
+	/* Create a guest-backed shader container backed by the dma buffer */
+	ret = vmw_shader_alloc(man->dev_priv, buf, size, 0, shader_type,
+			       tfile, &handle);
+	vmw_dmabuf_unreference(&buf);
+	if (unlikely(ret != 0))
+		goto no_reserve;
+	/*
+	 * Create a compat shader structure and stage it for insertion
+	 * in the manager
+	 */
+	compat = kzalloc(sizeof(*compat), GFP_KERNEL);
+	if (compat == NULL)
+		goto no_compat;
+
+	compat->hash.key = user_key |  (shader_type << 24);
+	ret = drm_ht_insert_item(&man->shaders, &compat->hash);
+	if (unlikely(ret != 0))
+		goto out_invalid_key;
+
+	compat->state = VMW_COMPAT_ADD;
+	compat->handle = handle;
+	compat->tfile = tfile;
+	list_add_tail(&compat->head, list);
 
+	return 0;
+
+out_invalid_key:
+	kfree(compat);
+no_compat:
+	ttm_ref_object_base_unref(tfile, handle, TTM_REF_USAGE);
+no_reserve:
+out:
 	return ret;
+}
+
+/**
+ * vmw_compat_shader_man_create - Create a compat shader manager
+ *
+ * @dev_priv: Pointer to a device private structure.
+ *
+ * Typically done at file open time. If successful returns a pointer to a
+ * compat shader manager. Otherwise returns an error pointer.
+ */
+struct vmw_compat_shader_manager *
+vmw_compat_shader_man_create(struct vmw_private *dev_priv)
+{
+	struct vmw_compat_shader_manager *man;
+	int ret;
+
+	man = kzalloc(sizeof(*man), GFP_KERNEL);
+
+	man->dev_priv = dev_priv;
+	INIT_LIST_HEAD(&man->list);
+	ret = drm_ht_create(&man->shaders, VMW_COMPAT_SHADER_HT_ORDER);
+	if (ret == 0)
+		return man;
+
+	kfree(man);
+	return ERR_PTR(ret);
+}
+
+/**
+ * vmw_compat_shader_man_destroy - Destroy a compat shader manager
+ *
+ * @man: Pointer to the shader manager to destroy.
+ *
+ * Typically done at file close time.
+ */
+void vmw_compat_shader_man_destroy(struct vmw_compat_shader_manager *man)
+{
+	struct vmw_compat_shader *entry, *next;
+
+	mutex_lock(&man->dev_priv->cmdbuf_mutex);
+	list_for_each_entry_safe(entry, next, &man->list, head)
+		vmw_compat_shader_free(man, entry);
 
+	mutex_unlock(&man->dev_priv->cmdbuf_mutex);
+	kfree(man);
 }
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_surface.c b/drivers/gpu/drm/vmwgfx/vmwgfx_surface.c
index 979da1c246a5..82468d902915 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_surface.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_surface.c
@@ -908,8 +908,8 @@ int vmw_surface_reference_ioctl(struct drm_device *dev, void *data,
 	    rep->size_addr;
 
 	if (user_sizes)
-		ret = copy_to_user(user_sizes, srf->sizes,
-				   srf->num_sizes * sizeof(*srf->sizes));
+		ret = copy_to_user(user_sizes, &srf->base_size,
+				   sizeof(srf->base_size));
 	if (unlikely(ret != 0)) {
 		DRM_ERROR("copy_to_user failed %p %u\n",
 			  user_sizes, srf->num_sizes);
@@ -1111,7 +1111,7 @@ static int vmw_gb_surface_destroy(struct vmw_resource *res)
 		return 0;
 
 	mutex_lock(&dev_priv->binding_mutex);
-	vmw_context_binding_res_list_kill(&res->binding_head);
+	vmw_context_binding_res_list_scrub(&res->binding_head);
 
 	cmd = vmw_fifo_reserve(dev_priv, sizeof(*cmd));
 	if (unlikely(cmd == NULL)) {
diff --git a/drivers/hid/hid-core.c b/drivers/hid/hid-core.c
index 026ab0fc06f7..3bfac3accd22 100644
--- a/drivers/hid/hid-core.c
+++ b/drivers/hid/hid-core.c
@@ -2118,6 +2118,7 @@ static const struct hid_device_id hid_ignore_list[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_BERKSHIRE, USB_DEVICE_ID_BERKSHIRE_PCWD) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CIDC, 0x0103) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CYGNAL, USB_DEVICE_ID_CYGNAL_RADIO_SI470X) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_CYGNAL, USB_DEVICE_ID_CYGNAL_RADIO_SI4713) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CMEDIA, USB_DEVICE_ID_CM109) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CYPRESS, USB_DEVICE_ID_CYPRESS_HIDCOM) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CYPRESS, USB_DEVICE_ID_CYPRESS_ULTRAMOUSE) },
diff --git a/drivers/hid/hid-ids.h b/drivers/hid/hid-ids.h
index 92b40c09d917..5a5248f2cc07 100644
--- a/drivers/hid/hid-ids.h
+++ b/drivers/hid/hid-ids.h
@@ -241,6 +241,8 @@
 #define USB_VENDOR_ID_CYGNAL		0x10c4
 #define USB_DEVICE_ID_CYGNAL_RADIO_SI470X	0x818a
 
+#define USB_DEVICE_ID_CYGNAL_RADIO_SI4713       0x8244
+
 #define USB_VENDOR_ID_CYPRESS		0x04b4
 #define USB_DEVICE_ID_CYPRESS_MOUSE	0x0001
 #define USB_DEVICE_ID_CYPRESS_HIDCOM	0x5500
diff --git a/drivers/hwmon/Kconfig b/drivers/hwmon/Kconfig
index f6ca3b21aebd..5ce43d8dfa98 100644
--- a/drivers/hwmon/Kconfig
+++ b/drivers/hwmon/Kconfig
@@ -650,6 +650,7 @@ config SENSORS_LM73
 config SENSORS_LM75
 	tristate "National Semiconductor LM75 and compatibles"
 	depends on I2C
+	depends on THERMAL || !THERMAL_OF
 	help
 	  If you say yes here you get support for one common type of
 	  temperature sensor chip, with models including:
@@ -1285,6 +1286,7 @@ config SENSORS_THMC50
 config SENSORS_TMP102
 	tristate "Texas Instruments TMP102"
 	depends on I2C
+	depends on THERMAL || !THERMAL_OF
 	help
 	  If you say yes here you get support for Texas Instruments TMP102
 	  sensor chips.
diff --git a/drivers/hwmon/da9055-hwmon.c b/drivers/hwmon/da9055-hwmon.c
index 029ecabc4380..73b3865f1207 100644
--- a/drivers/hwmon/da9055-hwmon.c
+++ b/drivers/hwmon/da9055-hwmon.c
@@ -278,10 +278,6 @@ static int da9055_hwmon_probe(struct platform_device *pdev)
 	if (hwmon_irq < 0)
 		return hwmon_irq;
 
-	hwmon_irq = regmap_irq_get_virq(hwmon->da9055->irq_data, hwmon_irq);
-	if (hwmon_irq < 0)
-		return hwmon_irq;
-
 	ret = devm_request_threaded_irq(&pdev->dev, hwmon_irq,
 					NULL, da9055_auxadc_irq,
 					IRQF_TRIGGER_HIGH | IRQF_ONESHOT,
diff --git a/drivers/hwmon/pmbus/pmbus_core.c b/drivers/hwmon/pmbus/pmbus_core.c
index 3cbf66e9d861..291d11fe93e7 100644
--- a/drivers/hwmon/pmbus/pmbus_core.c
+++ b/drivers/hwmon/pmbus/pmbus_core.c
@@ -90,7 +90,8 @@ struct pmbus_data {
 
 	u32 flags;		/* from platform data */
 
-	int exponent;		/* linear mode: exponent for output voltages */
+	int exponent[PMBUS_PAGES];
+				/* linear mode: exponent for output voltages */
 
 	const struct pmbus_driver_info *info;
 
@@ -410,7 +411,7 @@ static long pmbus_reg2data_linear(struct pmbus_data *data,
 	long val;
 
 	if (sensor->class == PSC_VOLTAGE_OUT) {	/* LINEAR16 */
-		exponent = data->exponent;
+		exponent = data->exponent[sensor->page];
 		mantissa = (u16) sensor->data;
 	} else {				/* LINEAR11 */
 		exponent = ((s16)sensor->data) >> 11;
@@ -516,7 +517,7 @@ static long pmbus_reg2data(struct pmbus_data *data, struct pmbus_sensor *sensor)
 #define MIN_MANTISSA	(511 * 1000)
 
 static u16 pmbus_data2reg_linear(struct pmbus_data *data,
-				 enum pmbus_sensor_classes class, long val)
+				 struct pmbus_sensor *sensor, long val)
 {
 	s16 exponent = 0, mantissa;
 	bool negative = false;
@@ -525,7 +526,7 @@ static u16 pmbus_data2reg_linear(struct pmbus_data *data,
 	if (val == 0)
 		return 0;
 
-	if (class == PSC_VOLTAGE_OUT) {
+	if (sensor->class == PSC_VOLTAGE_OUT) {
 		/* LINEAR16 does not support negative voltages */
 		if (val < 0)
 			return 0;
@@ -534,10 +535,10 @@ static u16 pmbus_data2reg_linear(struct pmbus_data *data,
 		 * For a static exponents, we don't have a choice
 		 * but to adjust the value to it.
 		 */
-		if (data->exponent < 0)
-			val <<= -data->exponent;
+		if (data->exponent[sensor->page] < 0)
+			val <<= -data->exponent[sensor->page];
 		else
-			val >>= data->exponent;
+			val >>= data->exponent[sensor->page];
 		val = DIV_ROUND_CLOSEST(val, 1000);
 		return val & 0xffff;
 	}
@@ -548,14 +549,14 @@ static u16 pmbus_data2reg_linear(struct pmbus_data *data,
 	}
 
 	/* Power is in uW. Convert to mW before converting. */
-	if (class == PSC_POWER)
+	if (sensor->class == PSC_POWER)
 		val = DIV_ROUND_CLOSEST(val, 1000L);
 
 	/*
 	 * For simplicity, convert fan data to milli-units
 	 * before calculating the exponent.
 	 */
-	if (class == PSC_FAN)
+	if (sensor->class == PSC_FAN)
 		val = val * 1000;
 
 	/* Reduce large mantissa until it fits into 10 bit */
@@ -585,22 +586,22 @@ static u16 pmbus_data2reg_linear(struct pmbus_data *data,
 }
 
 static u16 pmbus_data2reg_direct(struct pmbus_data *data,
-				 enum pmbus_sensor_classes class, long val)
+				 struct pmbus_sensor *sensor, long val)
 {
 	long m, b, R;
 
-	m = data->info->m[class];
-	b = data->info->b[class];
-	R = data->info->R[class];
+	m = data->info->m[sensor->class];
+	b = data->info->b[sensor->class];
+	R = data->info->R[sensor->class];
 
 	/* Power is in uW. Adjust R and b. */
-	if (class == PSC_POWER) {
+	if (sensor->class == PSC_POWER) {
 		R -= 3;
 		b *= 1000;
 	}
 
 	/* Calculate Y = (m * X + b) * 10^R */
-	if (class != PSC_FAN) {
+	if (sensor->class != PSC_FAN) {
 		R -= 3;		/* Adjust R and b for data in milli-units */
 		b *= 1000;
 	}
@@ -619,7 +620,7 @@ static u16 pmbus_data2reg_direct(struct pmbus_data *data,
 }
 
 static u16 pmbus_data2reg_vid(struct pmbus_data *data,
-			      enum pmbus_sensor_classes class, long val)
+			      struct pmbus_sensor *sensor, long val)
 {
 	val = clamp_val(val, 500, 1600);
 
@@ -627,20 +628,20 @@ static u16 pmbus_data2reg_vid(struct pmbus_data *data,
 }
 
 static u16 pmbus_data2reg(struct pmbus_data *data,
-			  enum pmbus_sensor_classes class, long val)
+			  struct pmbus_sensor *sensor, long val)
 {
 	u16 regval;
 
-	switch (data->info->format[class]) {
+	switch (data->info->format[sensor->class]) {
 	case direct:
-		regval = pmbus_data2reg_direct(data, class, val);
+		regval = pmbus_data2reg_direct(data, sensor, val);
 		break;
 	case vid:
-		regval = pmbus_data2reg_vid(data, class, val);
+		regval = pmbus_data2reg_vid(data, sensor, val);
 		break;
 	case linear:
 	default:
-		regval = pmbus_data2reg_linear(data, class, val);
+		regval = pmbus_data2reg_linear(data, sensor, val);
 		break;
 	}
 	return regval;
@@ -746,7 +747,7 @@ static ssize_t pmbus_set_sensor(struct device *dev,
 		return -EINVAL;
 
 	mutex_lock(&data->update_lock);
-	regval = pmbus_data2reg(data, sensor->class, val);
+	regval = pmbus_data2reg(data, sensor, val);
 	ret = _pmbus_write_word_data(client, sensor->page, sensor->reg, regval);
 	if (ret < 0)
 		rv = ret;
@@ -1643,12 +1644,13 @@ static int pmbus_find_attributes(struct i2c_client *client,
  * This function is called for all chips.
  */
 static int pmbus_identify_common(struct i2c_client *client,
-				 struct pmbus_data *data)
+				 struct pmbus_data *data, int page)
 {
 	int vout_mode = -1;
 
-	if (pmbus_check_byte_register(client, 0, PMBUS_VOUT_MODE))
-		vout_mode = _pmbus_read_byte_data(client, 0, PMBUS_VOUT_MODE);
+	if (pmbus_check_byte_register(client, page, PMBUS_VOUT_MODE))
+		vout_mode = _pmbus_read_byte_data(client, page,
+						  PMBUS_VOUT_MODE);
 	if (vout_mode >= 0 && vout_mode != 0xff) {
 		/*
 		 * Not all chips support the VOUT_MODE command,
@@ -1659,7 +1661,7 @@ static int pmbus_identify_common(struct i2c_client *client,
 			if (data->info->format[PSC_VOLTAGE_OUT] != linear)
 				return -ENODEV;
 
-			data->exponent = ((s8)(vout_mode << 3)) >> 3;
+			data->exponent[page] = ((s8)(vout_mode << 3)) >> 3;
 			break;
 		case 1: /* VID mode         */
 			if (data->info->format[PSC_VOLTAGE_OUT] != vid)
@@ -1674,7 +1676,7 @@ static int pmbus_identify_common(struct i2c_client *client,
 		}
 	}
 
-	pmbus_clear_fault_page(client, 0);
+	pmbus_clear_fault_page(client, page);
 	return 0;
 }
 
@@ -1682,7 +1684,7 @@ static int pmbus_init_common(struct i2c_client *client, struct pmbus_data *data,
 			     struct pmbus_driver_info *info)
 {
 	struct device *dev = &client->dev;
-	int ret;
+	int page, ret;
 
 	/*
 	 * Some PMBus chips don't support PMBUS_STATUS_BYTE, so try
@@ -1715,10 +1717,12 @@ static int pmbus_init_common(struct i2c_client *client, struct pmbus_data *data,
 		return -ENODEV;
 	}
 
-	ret = pmbus_identify_common(client, data);
-	if (ret < 0) {
-		dev_err(dev, "Failed to identify chip capabilities\n");
-		return ret;
+	for (page = 0; page < info->pages; page++) {
+		ret = pmbus_identify_common(client, data, page);
+		if (ret < 0) {
+			dev_err(dev, "Failed to identify chip capabilities\n");
+			return ret;
+		}
 	}
 	return 0;
 }
diff --git a/drivers/infiniband/ulp/isert/ib_isert.c b/drivers/infiniband/ulp/isert/ib_isert.c
index 9804fca6bf06..2b161be3c1a3 100644
--- a/drivers/infiniband/ulp/isert/ib_isert.c
+++ b/drivers/infiniband/ulp/isert/ib_isert.c
@@ -47,10 +47,10 @@ static int
 isert_map_rdma(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
 	       struct isert_rdma_wr *wr);
 static void
-isert_unreg_rdma_frwr(struct isert_cmd *isert_cmd, struct isert_conn *isert_conn);
+isert_unreg_rdma(struct isert_cmd *isert_cmd, struct isert_conn *isert_conn);
 static int
-isert_reg_rdma_frwr(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
-		    struct isert_rdma_wr *wr);
+isert_reg_rdma(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
+	       struct isert_rdma_wr *wr);
 
 static void
 isert_qp_event_callback(struct ib_event *e, void *context)
@@ -227,11 +227,11 @@ isert_create_device_ib_res(struct isert_device *device)
 
 	/* asign function handlers */
 	if (dev_attr->device_cap_flags & IB_DEVICE_MEM_MGT_EXTENSIONS) {
-		device->use_frwr = 1;
-		device->reg_rdma_mem = isert_reg_rdma_frwr;
-		device->unreg_rdma_mem = isert_unreg_rdma_frwr;
+		device->use_fastreg = 1;
+		device->reg_rdma_mem = isert_reg_rdma;
+		device->unreg_rdma_mem = isert_unreg_rdma;
 	} else {
-		device->use_frwr = 0;
+		device->use_fastreg = 0;
 		device->reg_rdma_mem = isert_map_rdma;
 		device->unreg_rdma_mem = isert_unmap_cmd;
 	}
@@ -239,9 +239,10 @@ isert_create_device_ib_res(struct isert_device *device)
 	device->cqs_used = min_t(int, num_online_cpus(),
 				 device->ib_device->num_comp_vectors);
 	device->cqs_used = min(ISERT_MAX_CQ, device->cqs_used);
-	pr_debug("Using %d CQs, device %s supports %d vectors support FRWR %d\n",
+	pr_debug("Using %d CQs, device %s supports %d vectors support "
+		 "Fast registration %d\n",
 		 device->cqs_used, device->ib_device->name,
-		 device->ib_device->num_comp_vectors, device->use_frwr);
+		 device->ib_device->num_comp_vectors, device->use_fastreg);
 	device->cq_desc = kzalloc(sizeof(struct isert_cq_desc) *
 				device->cqs_used, GFP_KERNEL);
 	if (!device->cq_desc) {
@@ -250,13 +251,6 @@ isert_create_device_ib_res(struct isert_device *device)
 	}
 	cq_desc = device->cq_desc;
 
-	device->dev_pd = ib_alloc_pd(ib_dev);
-	if (IS_ERR(device->dev_pd)) {
-		ret = PTR_ERR(device->dev_pd);
-		pr_err("ib_alloc_pd failed for dev_pd: %d\n", ret);
-		goto out_cq_desc;
-	}
-
 	for (i = 0; i < device->cqs_used; i++) {
 		cq_desc[i].device = device;
 		cq_desc[i].cq_index = i;
@@ -294,13 +288,6 @@ isert_create_device_ib_res(struct isert_device *device)
 			goto out_cq;
 	}
 
-	device->dev_mr = ib_get_dma_mr(device->dev_pd, IB_ACCESS_LOCAL_WRITE);
-	if (IS_ERR(device->dev_mr)) {
-		ret = PTR_ERR(device->dev_mr);
-		pr_err("ib_get_dma_mr failed for dev_mr: %d\n", ret);
-		goto out_cq;
-	}
-
 	return 0;
 
 out_cq:
@@ -316,9 +303,6 @@ out_cq:
 			ib_destroy_cq(device->dev_tx_cq[j]);
 		}
 	}
-	ib_dealloc_pd(device->dev_pd);
-
-out_cq_desc:
 	kfree(device->cq_desc);
 
 	return ret;
@@ -341,8 +325,6 @@ isert_free_device_ib_res(struct isert_device *device)
 		device->dev_tx_cq[i] = NULL;
 	}
 
-	ib_dereg_mr(device->dev_mr);
-	ib_dealloc_pd(device->dev_pd);
 	kfree(device->cq_desc);
 }
 
@@ -398,18 +380,18 @@ isert_device_find_by_ib_dev(struct rdma_cm_id *cma_id)
 }
 
 static void
-isert_conn_free_frwr_pool(struct isert_conn *isert_conn)
+isert_conn_free_fastreg_pool(struct isert_conn *isert_conn)
 {
 	struct fast_reg_descriptor *fr_desc, *tmp;
 	int i = 0;
 
-	if (list_empty(&isert_conn->conn_frwr_pool))
+	if (list_empty(&isert_conn->conn_fr_pool))
 		return;
 
-	pr_debug("Freeing conn %p frwr pool", isert_conn);
+	pr_debug("Freeing conn %p fastreg pool", isert_conn);
 
 	list_for_each_entry_safe(fr_desc, tmp,
-				 &isert_conn->conn_frwr_pool, list) {
+				 &isert_conn->conn_fr_pool, list) {
 		list_del(&fr_desc->list);
 		ib_free_fast_reg_page_list(fr_desc->data_frpl);
 		ib_dereg_mr(fr_desc->data_mr);
@@ -417,20 +399,47 @@ isert_conn_free_frwr_pool(struct isert_conn *isert_conn)
 		++i;
 	}
 
-	if (i < isert_conn->conn_frwr_pool_size)
+	if (i < isert_conn->conn_fr_pool_size)
 		pr_warn("Pool still has %d regions registered\n",
-			isert_conn->conn_frwr_pool_size - i);
+			isert_conn->conn_fr_pool_size - i);
+}
+
+static int
+isert_create_fr_desc(struct ib_device *ib_device, struct ib_pd *pd,
+		     struct fast_reg_descriptor *fr_desc)
+{
+	fr_desc->data_frpl = ib_alloc_fast_reg_page_list(ib_device,
+							 ISCSI_ISER_SG_TABLESIZE);
+	if (IS_ERR(fr_desc->data_frpl)) {
+		pr_err("Failed to allocate data frpl err=%ld\n",
+		       PTR_ERR(fr_desc->data_frpl));
+		return PTR_ERR(fr_desc->data_frpl);
+	}
+
+	fr_desc->data_mr = ib_alloc_fast_reg_mr(pd, ISCSI_ISER_SG_TABLESIZE);
+	if (IS_ERR(fr_desc->data_mr)) {
+		pr_err("Failed to allocate data frmr err=%ld\n",
+		       PTR_ERR(fr_desc->data_mr));
+		ib_free_fast_reg_page_list(fr_desc->data_frpl);
+		return PTR_ERR(fr_desc->data_mr);
+	}
+	pr_debug("Create fr_desc %p page_list %p\n",
+		 fr_desc, fr_desc->data_frpl->page_list);
+
+	fr_desc->valid = true;
+
+	return 0;
 }
 
 static int
-isert_conn_create_frwr_pool(struct isert_conn *isert_conn)
+isert_conn_create_fastreg_pool(struct isert_conn *isert_conn)
 {
 	struct fast_reg_descriptor *fr_desc;
 	struct isert_device *device = isert_conn->conn_device;
 	int i, ret;
 
-	INIT_LIST_HEAD(&isert_conn->conn_frwr_pool);
-	isert_conn->conn_frwr_pool_size = 0;
+	INIT_LIST_HEAD(&isert_conn->conn_fr_pool);
+	isert_conn->conn_fr_pool_size = 0;
 	for (i = 0; i < ISCSI_DEF_XMIT_CMDS_MAX; i++) {
 		fr_desc = kzalloc(sizeof(*fr_desc), GFP_KERNEL);
 		if (!fr_desc) {
@@ -439,40 +448,25 @@ isert_conn_create_frwr_pool(struct isert_conn *isert_conn)
 			goto err;
 		}
 
-		fr_desc->data_frpl =
-			ib_alloc_fast_reg_page_list(device->ib_device,
-						    ISCSI_ISER_SG_TABLESIZE);
-		if (IS_ERR(fr_desc->data_frpl)) {
-			pr_err("Failed to allocate fr_pg_list err=%ld\n",
-			       PTR_ERR(fr_desc->data_frpl));
-			ret = PTR_ERR(fr_desc->data_frpl);
-			goto err;
-		}
-
-		fr_desc->data_mr = ib_alloc_fast_reg_mr(device->dev_pd,
-					ISCSI_ISER_SG_TABLESIZE);
-		if (IS_ERR(fr_desc->data_mr)) {
-			pr_err("Failed to allocate frmr err=%ld\n",
-			       PTR_ERR(fr_desc->data_mr));
-			ret = PTR_ERR(fr_desc->data_mr);
-			ib_free_fast_reg_page_list(fr_desc->data_frpl);
+		ret = isert_create_fr_desc(device->ib_device,
+					   isert_conn->conn_pd, fr_desc);
+		if (ret) {
+			pr_err("Failed to create fastreg descriptor err=%d\n",
+			       ret);
 			goto err;
 		}
-		pr_debug("Create fr_desc %p page_list %p\n",
-			 fr_desc, fr_desc->data_frpl->page_list);
 
-		fr_desc->valid = true;
-		list_add_tail(&fr_desc->list, &isert_conn->conn_frwr_pool);
-		isert_conn->conn_frwr_pool_size++;
+		list_add_tail(&fr_desc->list, &isert_conn->conn_fr_pool);
+		isert_conn->conn_fr_pool_size++;
 	}
 
-	pr_debug("Creating conn %p frwr pool size=%d",
-		 isert_conn, isert_conn->conn_frwr_pool_size);
+	pr_debug("Creating conn %p fastreg pool size=%d",
+		 isert_conn, isert_conn->conn_fr_pool_size);
 
 	return 0;
 
 err:
-	isert_conn_free_frwr_pool(isert_conn);
+	isert_conn_free_fastreg_pool(isert_conn);
 	return ret;
 }
 
@@ -558,14 +552,29 @@ isert_connect_request(struct rdma_cm_id *cma_id, struct rdma_cm_event *event)
 	}
 
 	isert_conn->conn_device = device;
-	isert_conn->conn_pd = device->dev_pd;
-	isert_conn->conn_mr = device->dev_mr;
+	isert_conn->conn_pd = ib_alloc_pd(isert_conn->conn_device->ib_device);
+	if (IS_ERR(isert_conn->conn_pd)) {
+		ret = PTR_ERR(isert_conn->conn_pd);
+		pr_err("ib_alloc_pd failed for conn %p: ret=%d\n",
+		       isert_conn, ret);
+		goto out_pd;
+	}
 
-	if (device->use_frwr) {
-		ret = isert_conn_create_frwr_pool(isert_conn);
+	isert_conn->conn_mr = ib_get_dma_mr(isert_conn->conn_pd,
+					   IB_ACCESS_LOCAL_WRITE);
+	if (IS_ERR(isert_conn->conn_mr)) {
+		ret = PTR_ERR(isert_conn->conn_mr);
+		pr_err("ib_get_dma_mr failed for conn %p: ret=%d\n",
+		       isert_conn, ret);
+		goto out_mr;
+	}
+
+	if (device->use_fastreg) {
+		ret = isert_conn_create_fastreg_pool(isert_conn);
 		if (ret) {
-			pr_err("Conn: %p failed to create frwr_pool\n", isert_conn);
-			goto out_frwr;
+			pr_err("Conn: %p failed to create fastreg pool\n",
+			       isert_conn);
+			goto out_fastreg;
 		}
 	}
 
@@ -582,9 +591,13 @@ isert_connect_request(struct rdma_cm_id *cma_id, struct rdma_cm_event *event)
 	return 0;
 
 out_conn_dev:
-	if (device->use_frwr)
-		isert_conn_free_frwr_pool(isert_conn);
-out_frwr:
+	if (device->use_fastreg)
+		isert_conn_free_fastreg_pool(isert_conn);
+out_fastreg:
+	ib_dereg_mr(isert_conn->conn_mr);
+out_mr:
+	ib_dealloc_pd(isert_conn->conn_pd);
+out_pd:
 	isert_device_try_release(device);
 out_rsp_dma_map:
 	ib_dma_unmap_single(ib_dev, isert_conn->login_rsp_dma,
@@ -608,8 +621,8 @@ isert_connect_release(struct isert_conn *isert_conn)
 
 	pr_debug("Entering isert_connect_release(): >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
 
-	if (device && device->use_frwr)
-		isert_conn_free_frwr_pool(isert_conn);
+	if (device && device->use_fastreg)
+		isert_conn_free_fastreg_pool(isert_conn);
 
 	if (isert_conn->conn_qp) {
 		cq_index = ((struct isert_cq_desc *)
@@ -623,6 +636,9 @@ isert_connect_release(struct isert_conn *isert_conn)
 	isert_free_rx_descriptors(isert_conn);
 	rdma_destroy_id(isert_conn->conn_cm_id);
 
+	ib_dereg_mr(isert_conn->conn_mr);
+	ib_dealloc_pd(isert_conn->conn_pd);
+
 	if (isert_conn->login_buf) {
 		ib_dma_unmap_single(ib_dev, isert_conn->login_rsp_dma,
 				    ISER_RX_LOGIN_SIZE, DMA_TO_DEVICE);
@@ -1024,13 +1040,13 @@ isert_rx_login_req(struct iser_rx_desc *rx_desc, int rx_buflen,
 }
 
 static struct iscsi_cmd
-*isert_allocate_cmd(struct iscsi_conn *conn, gfp_t gfp)
+*isert_allocate_cmd(struct iscsi_conn *conn)
 {
 	struct isert_conn *isert_conn = (struct isert_conn *)conn->context;
 	struct isert_cmd *isert_cmd;
 	struct iscsi_cmd *cmd;
 
-	cmd = iscsit_allocate_cmd(conn, gfp);
+	cmd = iscsit_allocate_cmd(conn, TASK_INTERRUPTIBLE);
 	if (!cmd) {
 		pr_err("Unable to allocate iscsi_cmd + isert_cmd\n");
 		return NULL;
@@ -1219,7 +1235,7 @@ isert_rx_opcode(struct isert_conn *isert_conn, struct iser_rx_desc *rx_desc,
 
 	switch (opcode) {
 	case ISCSI_OP_SCSI_CMD:
-		cmd = isert_allocate_cmd(conn, GFP_KERNEL);
+		cmd = isert_allocate_cmd(conn);
 		if (!cmd)
 			break;
 
@@ -1233,7 +1249,7 @@ isert_rx_opcode(struct isert_conn *isert_conn, struct iser_rx_desc *rx_desc,
 					rx_desc, (unsigned char *)hdr);
 		break;
 	case ISCSI_OP_NOOP_OUT:
-		cmd = isert_allocate_cmd(conn, GFP_KERNEL);
+		cmd = isert_allocate_cmd(conn);
 		if (!cmd)
 			break;
 
@@ -1246,7 +1262,7 @@ isert_rx_opcode(struct isert_conn *isert_conn, struct iser_rx_desc *rx_desc,
 						(unsigned char *)hdr);
 		break;
 	case ISCSI_OP_SCSI_TMFUNC:
-		cmd = isert_allocate_cmd(conn, GFP_KERNEL);
+		cmd = isert_allocate_cmd(conn);
 		if (!cmd)
 			break;
 
@@ -1254,7 +1270,7 @@ isert_rx_opcode(struct isert_conn *isert_conn, struct iser_rx_desc *rx_desc,
 						(unsigned char *)hdr);
 		break;
 	case ISCSI_OP_LOGOUT:
-		cmd = isert_allocate_cmd(conn, GFP_KERNEL);
+		cmd = isert_allocate_cmd(conn);
 		if (!cmd)
 			break;
 
@@ -1265,7 +1281,7 @@ isert_rx_opcode(struct isert_conn *isert_conn, struct iser_rx_desc *rx_desc,
 						    HZ);
 		break;
 	case ISCSI_OP_TEXT:
-		cmd = isert_allocate_cmd(conn, GFP_KERNEL);
+		cmd = isert_allocate_cmd(conn);
 		if (!cmd)
 			break;
 
@@ -1404,25 +1420,25 @@ isert_unmap_cmd(struct isert_cmd *isert_cmd, struct isert_conn *isert_conn)
 }
 
 static void
-isert_unreg_rdma_frwr(struct isert_cmd *isert_cmd, struct isert_conn *isert_conn)
+isert_unreg_rdma(struct isert_cmd *isert_cmd, struct isert_conn *isert_conn)
 {
 	struct isert_rdma_wr *wr = &isert_cmd->rdma_wr;
 	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
 	LIST_HEAD(unmap_list);
 
-	pr_debug("unreg_frwr_cmd: %p\n", isert_cmd);
+	pr_debug("unreg_fastreg_cmd: %p\n", isert_cmd);
 
 	if (wr->fr_desc) {
-		pr_debug("unreg_frwr_cmd: %p free fr_desc %p\n",
+		pr_debug("unreg_fastreg_cmd: %p free fr_desc %p\n",
 			 isert_cmd, wr->fr_desc);
 		spin_lock_bh(&isert_conn->conn_lock);
-		list_add_tail(&wr->fr_desc->list, &isert_conn->conn_frwr_pool);
+		list_add_tail(&wr->fr_desc->list, &isert_conn->conn_fr_pool);
 		spin_unlock_bh(&isert_conn->conn_lock);
 		wr->fr_desc = NULL;
 	}
 
 	if (wr->sge) {
-		pr_debug("unreg_frwr_cmd: %p unmap_sg op\n", isert_cmd);
+		pr_debug("unreg_fastreg_cmd: %p unmap_sg op\n", isert_cmd);
 		ib_dma_unmap_sg(ib_dev, wr->sge, wr->num_sge,
 				(wr->iser_ib_op == ISER_IB_RDMA_WRITE) ?
 				DMA_TO_DEVICE : DMA_FROM_DEVICE);
@@ -2163,26 +2179,22 @@ isert_map_fr_pagelist(struct ib_device *ib_dev,
 
 static int
 isert_fast_reg_mr(struct fast_reg_descriptor *fr_desc,
-		  struct isert_cmd *isert_cmd, struct isert_conn *isert_conn,
-		  struct ib_sge *ib_sge, u32 offset, unsigned int data_len)
+		  struct isert_conn *isert_conn, struct scatterlist *sg_start,
+		  struct ib_sge *ib_sge, u32 sg_nents, u32 offset,
+		  unsigned int data_len)
 {
-	struct iscsi_cmd *cmd = isert_cmd->iscsi_cmd;
 	struct ib_device *ib_dev = isert_conn->conn_cm_id->device;
-	struct scatterlist *sg_start;
-	u32 sg_off, page_off;
 	struct ib_send_wr fr_wr, inv_wr;
 	struct ib_send_wr *bad_wr, *wr = NULL;
+	int ret, pagelist_len;
+	u32 page_off;
 	u8 key;
-	int ret, sg_nents, pagelist_len;
 
-	sg_off = offset / PAGE_SIZE;
-	sg_start = &cmd->se_cmd.t_data_sg[sg_off];
-	sg_nents = min_t(unsigned int, cmd->se_cmd.t_data_nents - sg_off,
-			 ISCSI_ISER_SG_TABLESIZE);
+	sg_nents = min_t(unsigned int, sg_nents, ISCSI_ISER_SG_TABLESIZE);
 	page_off = offset % PAGE_SIZE;
 
-	pr_debug("Cmd: %p use fr_desc %p sg_nents %d sg_off %d offset %u\n",
-		 isert_cmd, fr_desc, sg_nents, sg_off, offset);
+	pr_debug("Use fr_desc %p sg_nents %d offset %u\n",
+		 fr_desc, sg_nents, offset);
 
 	pagelist_len = isert_map_fr_pagelist(ib_dev, sg_start, sg_nents,
 					     &fr_desc->data_frpl->page_list[0]);
@@ -2232,8 +2244,8 @@ isert_fast_reg_mr(struct fast_reg_descriptor *fr_desc,
 }
 
 static int
-isert_reg_rdma_frwr(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
-		    struct isert_rdma_wr *wr)
+isert_reg_rdma(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
+	       struct isert_rdma_wr *wr)
 {
 	struct se_cmd *se_cmd = &cmd->se_cmd;
 	struct isert_cmd *isert_cmd = iscsit_priv_cmd(cmd);
@@ -2251,9 +2263,9 @@ isert_reg_rdma_frwr(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
 	if (wr->iser_ib_op == ISER_IB_RDMA_WRITE) {
 		data_left = se_cmd->data_length;
 	} else {
-		sg_off = cmd->write_data_done / PAGE_SIZE;
-		data_left = se_cmd->data_length - cmd->write_data_done;
 		offset = cmd->write_data_done;
+		sg_off = offset / PAGE_SIZE;
+		data_left = se_cmd->data_length - cmd->write_data_done;
 		isert_cmd->tx_desc.isert_cmd = isert_cmd;
 	}
 
@@ -2311,16 +2323,16 @@ isert_reg_rdma_frwr(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
 		wr->fr_desc = NULL;
 	} else {
 		spin_lock_irqsave(&isert_conn->conn_lock, flags);
-		fr_desc = list_first_entry(&isert_conn->conn_frwr_pool,
+		fr_desc = list_first_entry(&isert_conn->conn_fr_pool,
 					   struct fast_reg_descriptor, list);
 		list_del(&fr_desc->list);
 		spin_unlock_irqrestore(&isert_conn->conn_lock, flags);
 		wr->fr_desc = fr_desc;
 
-		ret = isert_fast_reg_mr(fr_desc, isert_cmd, isert_conn,
-				  ib_sge, offset, data_len);
+		ret = isert_fast_reg_mr(fr_desc, isert_conn, sg_start,
+					ib_sge, sg_nents, offset, data_len);
 		if (ret) {
-			list_add_tail(&fr_desc->list, &isert_conn->conn_frwr_pool);
+			list_add_tail(&fr_desc->list, &isert_conn->conn_fr_pool);
 			goto unmap_sg;
 		}
 	}
diff --git a/drivers/infiniband/ulp/isert/ib_isert.h b/drivers/infiniband/ulp/isert/ib_isert.h
index 691f90ff2d83..708a069002f3 100644
--- a/drivers/infiniband/ulp/isert/ib_isert.h
+++ b/drivers/infiniband/ulp/isert/ib_isert.h
@@ -119,9 +119,9 @@ struct isert_conn {
 	wait_queue_head_t	conn_wait;
 	wait_queue_head_t	conn_wait_comp_err;
 	struct kref		conn_kref;
-	struct list_head	conn_frwr_pool;
-	int			conn_frwr_pool_size;
-	/* lock to protect frwr_pool */
+	struct list_head	conn_fr_pool;
+	int			conn_fr_pool_size;
+	/* lock to protect fastreg pool */
 	spinlock_t		conn_lock;
 #define ISERT_COMP_BATCH_COUNT	8
 	int			conn_comp_batch;
@@ -139,13 +139,11 @@ struct isert_cq_desc {
 };
 
 struct isert_device {
-	int			use_frwr;
+	int			use_fastreg;
 	int			cqs_used;
 	int			refcount;
 	int			cq_active_qps[ISERT_MAX_CQ];
 	struct ib_device	*ib_device;
-	struct ib_pd		*dev_pd;
-	struct ib_mr		*dev_mr;
 	struct ib_cq		*dev_rx_cq[ISERT_MAX_CQ];
 	struct ib_cq		*dev_tx_cq[ISERT_MAX_CQ];
 	struct isert_cq_desc	*cq_desc;
diff --git a/drivers/irqchip/Makefile b/drivers/irqchip/Makefile
index 86b484cb3ec2..5194afb39e78 100644
--- a/drivers/irqchip/Makefile
+++ b/drivers/irqchip/Makefile
@@ -21,6 +21,7 @@ obj-$(CONFIG_SIRF_IRQ)			+= irq-sirfsoc.o
 obj-$(CONFIG_RENESAS_INTC_IRQPIN)	+= irq-renesas-intc-irqpin.o
 obj-$(CONFIG_RENESAS_IRQC)		+= irq-renesas-irqc.o
 obj-$(CONFIG_VERSATILE_FPGA_IRQ)	+= irq-versatile-fpga.o
+obj-$(CONFIG_ARCH_NSPIRE)		+= irq-zevio.o
 obj-$(CONFIG_ARCH_VT8500)		+= irq-vt8500.o
 obj-$(CONFIG_TB10X_IRQC)		+= irq-tb10x.o
 obj-$(CONFIG_XTENSA)			+= irq-xtensa-pic.o
diff --git a/drivers/irqchip/irq-armada-370-xp.c b/drivers/irqchip/irq-armada-370-xp.c
index 433cc8568dec..540956465ed2 100644
--- a/drivers/irqchip/irq-armada-370-xp.c
+++ b/drivers/irqchip/irq-armada-370-xp.c
@@ -59,8 +59,6 @@
 #define PCI_MSI_DOORBELL_END                    (32)
 #define PCI_MSI_DOORBELL_MASK                   0xFFFF0000
 
-static DEFINE_RAW_SPINLOCK(irq_controller_lock);
-
 static void __iomem *per_cpu_int_base;
 static void __iomem *main_int_base;
 static struct irq_domain *armada_370_xp_mpic_domain;
@@ -239,6 +237,8 @@ static inline int armada_370_xp_msi_init(struct device_node *node,
 #endif
 
 #ifdef CONFIG_SMP
+static DEFINE_RAW_SPINLOCK(irq_controller_lock);
+
 static int armada_xp_set_affinity(struct irq_data *d,
 				  const struct cpumask *mask_val, bool force)
 {
@@ -381,7 +381,7 @@ armada_370_xp_handle_irq(struct pt_regs *regs)
 						ARMADA_370_XP_IN_DRBEL_CAUSE_OFFS)
 				& PCI_MSI_DOORBELL_MASK;
 
-			writel(~PCI_MSI_DOORBELL_MASK, per_cpu_int_base +
+			writel(~msimask, per_cpu_int_base +
 			       ARMADA_370_XP_IN_DRBEL_CAUSE_OFFS);
 
 			for (msinr = PCI_MSI_DOORBELL_START;
@@ -407,7 +407,7 @@ armada_370_xp_handle_irq(struct pt_regs *regs)
 						ARMADA_370_XP_IN_DRBEL_CAUSE_OFFS)
 				& IPI_DOORBELL_MASK;
 
-			writel(~IPI_DOORBELL_MASK, per_cpu_int_base +
+			writel(~ipimask, per_cpu_int_base +
 				ARMADA_370_XP_IN_DRBEL_CAUSE_OFFS);
 
 			/* Handle all pending doorbells */
diff --git a/drivers/irqchip/irq-zevio.c b/drivers/irqchip/irq-zevio.c
new file mode 100644
index 000000000000..8ed04c4a43ee
--- /dev/null
+++ b/drivers/irqchip/irq-zevio.c
@@ -0,0 +1,127 @@
+/*
+ *  linux/drivers/irqchip/irq-zevio.c
+ *
+ *  Copyright (C) 2013 Daniel Tang <tangrs@tangrs.id.au>
+ *
+ * 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/irq.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+
+#include <asm/mach/irq.h>
+#include <asm/exception.h>
+
+#include "irqchip.h"
+
+#define IO_STATUS	0x000
+#define IO_RAW_STATUS	0x004
+#define IO_ENABLE	0x008
+#define IO_DISABLE	0x00C
+#define IO_CURRENT	0x020
+#define IO_RESET	0x028
+#define IO_MAX_PRIOTY	0x02C
+
+#define IO_IRQ_BASE	0x000
+#define IO_FIQ_BASE	0x100
+
+#define IO_INVERT_SEL	0x200
+#define IO_STICKY_SEL	0x204
+#define IO_PRIORITY_SEL	0x300
+
+#define MAX_INTRS	32
+#define FIQ_START	MAX_INTRS
+
+static struct irq_domain *zevio_irq_domain;
+static void __iomem *zevio_irq_io;
+
+static void zevio_irq_ack(struct irq_data *irqd)
+{
+	struct irq_chip_generic *gc = irq_data_get_irq_chip_data(irqd);
+	struct irq_chip_regs *regs =
+		&container_of(irqd->chip, struct irq_chip_type, chip)->regs;
+
+	readl(gc->reg_base + regs->ack);
+}
+
+static asmlinkage void __exception_irq_entry zevio_handle_irq(struct pt_regs *regs)
+{
+	int irqnr;
+
+	while (readl(zevio_irq_io + IO_STATUS)) {
+		irqnr = readl(zevio_irq_io + IO_CURRENT);
+		irqnr = irq_find_mapping(zevio_irq_domain, irqnr);
+		handle_IRQ(irqnr, regs);
+	};
+}
+
+static void __init zevio_init_irq_base(void __iomem *base)
+{
+	/* Disable all interrupts */
+	writel(~0, base + IO_DISABLE);
+
+	/* Accept interrupts of all priorities */
+	writel(0xF, base + IO_MAX_PRIOTY);
+
+	/* Reset existing interrupts */
+	readl(base + IO_RESET);
+}
+
+static int __init zevio_of_init(struct device_node *node,
+				struct device_node *parent)
+{
+	unsigned int clr = IRQ_NOREQUEST | IRQ_NOPROBE | IRQ_NOAUTOEN;
+	struct irq_chip_generic *gc;
+	int ret;
+
+	if (WARN_ON(zevio_irq_io || zevio_irq_domain))
+		return -EBUSY;
+
+	zevio_irq_io = of_iomap(node, 0);
+	BUG_ON(!zevio_irq_io);
+
+	/* Do not invert interrupt status bits */
+	writel(~0, zevio_irq_io + IO_INVERT_SEL);
+
+	/* Disable sticky interrupts */
+	writel(0, zevio_irq_io + IO_STICKY_SEL);
+
+	/* We don't use IRQ priorities. Set each IRQ to highest priority. */
+	memset_io(zevio_irq_io + IO_PRIORITY_SEL, 0, MAX_INTRS * sizeof(u32));
+
+	/* Init IRQ and FIQ */
+	zevio_init_irq_base(zevio_irq_io + IO_IRQ_BASE);
+	zevio_init_irq_base(zevio_irq_io + IO_FIQ_BASE);
+
+	zevio_irq_domain = irq_domain_add_linear(node, MAX_INTRS,
+						 &irq_generic_chip_ops, NULL);
+	BUG_ON(!zevio_irq_domain);
+
+	ret = irq_alloc_domain_generic_chips(zevio_irq_domain, MAX_INTRS, 1,
+					     "zevio_intc", handle_level_irq,
+					     clr, 0, IRQ_GC_INIT_MASK_CACHE);
+	BUG_ON(ret);
+
+	gc = irq_get_domain_generic_chip(zevio_irq_domain, 0);
+	gc->reg_base				= zevio_irq_io;
+	gc->chip_types[0].chip.irq_ack		= zevio_irq_ack;
+	gc->chip_types[0].chip.irq_mask		= irq_gc_mask_disable_reg;
+	gc->chip_types[0].chip.irq_unmask	= irq_gc_unmask_enable_reg;
+	gc->chip_types[0].regs.mask		= IO_IRQ_BASE + IO_ENABLE;
+	gc->chip_types[0].regs.enable		= IO_IRQ_BASE + IO_ENABLE;
+	gc->chip_types[0].regs.disable		= IO_IRQ_BASE + IO_DISABLE;
+	gc->chip_types[0].regs.ack		= IO_IRQ_BASE + IO_RESET;
+
+	set_handle_irq(zevio_handle_irq);
+
+	pr_info("TI-NSPIRE classic IRQ controller\n");
+	return 0;
+}
+
+IRQCHIP_DECLARE(zevio_irq, "lsi,zevio-intc", zevio_of_init);
diff --git a/drivers/md/bcache/Makefile b/drivers/md/bcache/Makefile
index 0e9c82523be6..c488b846f831 100644
--- a/drivers/md/bcache/Makefile
+++ b/drivers/md/bcache/Makefile
@@ -1,7 +1,8 @@
 
 obj-$(CONFIG_BCACHE)	+= bcache.o
 
-bcache-y		:= alloc.o btree.o bset.o io.o journal.o writeback.o\
-	movinggc.o request.o super.o sysfs.o debug.o util.o trace.o stats.o closure.o
+bcache-y		:= alloc.o bset.o btree.o closure.o debug.o extents.o\
+	io.o journal.o movinggc.o request.o stats.o super.o sysfs.o trace.o\
+	util.o writeback.o
 
 CFLAGS_request.o	+= -Iblock
diff --git a/drivers/md/bcache/alloc.c b/drivers/md/bcache/alloc.c
index 4c9852d92b0a..c0d37d082443 100644
--- a/drivers/md/bcache/alloc.c
+++ b/drivers/md/bcache/alloc.c
@@ -132,10 +132,16 @@ bool bch_bucket_add_unused(struct cache *ca, struct bucket *b)
 {
 	BUG_ON(GC_MARK(b) || GC_SECTORS_USED(b));
 
-	if (fifo_used(&ca->free) > ca->watermark[WATERMARK_MOVINGGC] &&
-	    CACHE_REPLACEMENT(&ca->sb) == CACHE_REPLACEMENT_FIFO)
-		return false;
+	if (CACHE_REPLACEMENT(&ca->sb) == CACHE_REPLACEMENT_FIFO) {
+		unsigned i;
+
+		for (i = 0; i < RESERVE_NONE; i++)
+			if (!fifo_full(&ca->free[i]))
+				goto add;
 
+		return false;
+	}
+add:
 	b->prio = 0;
 
 	if (can_inc_bucket_gen(b) &&
@@ -162,8 +168,21 @@ static void invalidate_one_bucket(struct cache *ca, struct bucket *b)
 	fifo_push(&ca->free_inc, b - ca->buckets);
 }
 
-#define bucket_prio(b)				\
-	(((unsigned) (b->prio - ca->set->min_prio)) * GC_SECTORS_USED(b))
+/*
+ * Determines what order we're going to reuse buckets, smallest bucket_prio()
+ * first: we also take into account the number of sectors of live data in that
+ * bucket, and in order for that multiply to make sense we have to scale bucket
+ *
+ * Thus, we scale the bucket priorities so that the bucket with the smallest
+ * prio is worth 1/8th of what INITIAL_PRIO is worth.
+ */
+
+#define bucket_prio(b)							\
+({									\
+	unsigned min_prio = (INITIAL_PRIO - ca->set->min_prio) / 8;	\
+									\
+	(b->prio - ca->set->min_prio + min_prio) * GC_SECTORS_USED(b);	\
+})
 
 #define bucket_max_cmp(l, r)	(bucket_prio(l) < bucket_prio(r))
 #define bucket_min_cmp(l, r)	(bucket_prio(l) > bucket_prio(r))
@@ -304,6 +323,21 @@ do {									\
 	__set_current_state(TASK_RUNNING);				\
 } while (0)
 
+static int bch_allocator_push(struct cache *ca, long bucket)
+{
+	unsigned i;
+
+	/* Prios/gens are actually the most important reserve */
+	if (fifo_push(&ca->free[RESERVE_PRIO], bucket))
+		return true;
+
+	for (i = 0; i < RESERVE_NR; i++)
+		if (fifo_push(&ca->free[i], bucket))
+			return true;
+
+	return false;
+}
+
 static int bch_allocator_thread(void *arg)
 {
 	struct cache *ca = arg;
@@ -336,9 +370,7 @@ static int bch_allocator_thread(void *arg)
 				mutex_lock(&ca->set->bucket_lock);
 			}
 
-			allocator_wait(ca, !fifo_full(&ca->free));
-
-			fifo_push(&ca->free, bucket);
+			allocator_wait(ca, bch_allocator_push(ca, bucket));
 			wake_up(&ca->set->bucket_wait);
 		}
 
@@ -365,34 +397,29 @@ static int bch_allocator_thread(void *arg)
 	}
 }
 
-long bch_bucket_alloc(struct cache *ca, unsigned watermark, bool wait)
+long bch_bucket_alloc(struct cache *ca, unsigned reserve, bool wait)
 {
 	DEFINE_WAIT(w);
 	struct bucket *b;
 	long r;
 
 	/* fastpath */
-	if (fifo_used(&ca->free) > ca->watermark[watermark]) {
-		fifo_pop(&ca->free, r);
+	if (fifo_pop(&ca->free[RESERVE_NONE], r) ||
+	    fifo_pop(&ca->free[reserve], r))
 		goto out;
-	}
 
 	if (!wait)
 		return -1;
 
-	while (1) {
-		if (fifo_used(&ca->free) > ca->watermark[watermark]) {
-			fifo_pop(&ca->free, r);
-			break;
-		}
-
+	do {
 		prepare_to_wait(&ca->set->bucket_wait, &w,
 				TASK_UNINTERRUPTIBLE);
 
 		mutex_unlock(&ca->set->bucket_lock);
 		schedule();
 		mutex_lock(&ca->set->bucket_lock);
-	}
+	} while (!fifo_pop(&ca->free[RESERVE_NONE], r) &&
+		 !fifo_pop(&ca->free[reserve], r));
 
 	finish_wait(&ca->set->bucket_wait, &w);
 out:
@@ -401,12 +428,14 @@ out:
 	if (expensive_debug_checks(ca->set)) {
 		size_t iter;
 		long i;
+		unsigned j;
 
 		for (iter = 0; iter < prio_buckets(ca) * 2; iter++)
 			BUG_ON(ca->prio_buckets[iter] == (uint64_t) r);
 
-		fifo_for_each(i, &ca->free, iter)
-			BUG_ON(i == r);
+		for (j = 0; j < RESERVE_NR; j++)
+			fifo_for_each(i, &ca->free[j], iter)
+				BUG_ON(i == r);
 		fifo_for_each(i, &ca->free_inc, iter)
 			BUG_ON(i == r);
 		fifo_for_each(i, &ca->unused, iter)
@@ -419,7 +448,7 @@ out:
 
 	SET_GC_SECTORS_USED(b, ca->sb.bucket_size);
 
-	if (watermark <= WATERMARK_METADATA) {
+	if (reserve <= RESERVE_PRIO) {
 		SET_GC_MARK(b, GC_MARK_METADATA);
 		SET_GC_MOVE(b, 0);
 		b->prio = BTREE_PRIO;
@@ -445,7 +474,7 @@ void bch_bucket_free(struct cache_set *c, struct bkey *k)
 	}
 }
 
-int __bch_bucket_alloc_set(struct cache_set *c, unsigned watermark,
+int __bch_bucket_alloc_set(struct cache_set *c, unsigned reserve,
 			   struct bkey *k, int n, bool wait)
 {
 	int i;
@@ -459,7 +488,7 @@ int __bch_bucket_alloc_set(struct cache_set *c, unsigned watermark,
 
 	for (i = 0; i < n; i++) {
 		struct cache *ca = c->cache_by_alloc[i];
-		long b = bch_bucket_alloc(ca, watermark, wait);
+		long b = bch_bucket_alloc(ca, reserve, wait);
 
 		if (b == -1)
 			goto err;
@@ -478,12 +507,12 @@ err:
 	return -1;
 }
 
-int bch_bucket_alloc_set(struct cache_set *c, unsigned watermark,
+int bch_bucket_alloc_set(struct cache_set *c, unsigned reserve,
 			 struct bkey *k, int n, bool wait)
 {
 	int ret;
 	mutex_lock(&c->bucket_lock);
-	ret = __bch_bucket_alloc_set(c, watermark, k, n, wait);
+	ret = __bch_bucket_alloc_set(c, reserve, k, n, wait);
 	mutex_unlock(&c->bucket_lock);
 	return ret;
 }
@@ -573,8 +602,8 @@ bool bch_alloc_sectors(struct cache_set *c, struct bkey *k, unsigned sectors,
 
 	while (!(b = pick_data_bucket(c, k, write_point, &alloc.key))) {
 		unsigned watermark = write_prio
-			? WATERMARK_MOVINGGC
-			: WATERMARK_NONE;
+			? RESERVE_MOVINGGC
+			: RESERVE_NONE;
 
 		spin_unlock(&c->data_bucket_lock);
 
@@ -689,7 +718,7 @@ int bch_cache_allocator_init(struct cache *ca)
 	 * Then 8 for btree allocations
 	 * Then half for the moving garbage collector
 	 */
-
+#if 0
 	ca->watermark[WATERMARK_PRIO] = 0;
 
 	ca->watermark[WATERMARK_METADATA] = prio_buckets(ca);
@@ -699,6 +728,6 @@ int bch_cache_allocator_init(struct cache *ca)
 
 	ca->watermark[WATERMARK_NONE] = ca->free.size / 2 +
 		ca->watermark[WATERMARK_MOVINGGC];
-
+#endif
 	return 0;
 }
diff --git a/drivers/md/bcache/bcache.h b/drivers/md/bcache/bcache.h
index 754f43177483..0c707e4f4eaf 100644
--- a/drivers/md/bcache/bcache.h
+++ b/drivers/md/bcache/bcache.h
@@ -187,6 +187,7 @@
 #include <linux/types.h>
 #include <linux/workqueue.h>
 
+#include "bset.h"
 #include "util.h"
 #include "closure.h"
 
@@ -280,7 +281,6 @@ struct bcache_device {
 	unsigned long		sectors_dirty_last;
 	long			sectors_dirty_derivative;
 
-	mempool_t		*unaligned_bvec;
 	struct bio_set		*bio_split;
 
 	unsigned		data_csum:1;
@@ -310,7 +310,8 @@ struct cached_dev {
 	struct cache_sb		sb;
 	struct bio		sb_bio;
 	struct bio_vec		sb_bv[1];
-	struct closure_with_waitlist sb_write;
+	struct closure		sb_write;
+	struct semaphore	sb_write_mutex;
 
 	/* Refcount on the cache set. Always nonzero when we're caching. */
 	atomic_t		count;
@@ -383,12 +384,12 @@ struct cached_dev {
 	unsigned		writeback_rate_p_term_inverse;
 };
 
-enum alloc_watermarks {
-	WATERMARK_PRIO,
-	WATERMARK_METADATA,
-	WATERMARK_MOVINGGC,
-	WATERMARK_NONE,
-	WATERMARK_MAX
+enum alloc_reserve {
+	RESERVE_BTREE,
+	RESERVE_PRIO,
+	RESERVE_MOVINGGC,
+	RESERVE_NONE,
+	RESERVE_NR,
 };
 
 struct cache {
@@ -400,8 +401,6 @@ struct cache {
 	struct kobject		kobj;
 	struct block_device	*bdev;
 
-	unsigned		watermark[WATERMARK_MAX];
-
 	struct task_struct	*alloc_thread;
 
 	struct closure		prio;
@@ -430,7 +429,7 @@ struct cache {
 	 * because all the data they contained was overwritten), so we only
 	 * need to discard them before they can be moved to the free list.
 	 */
-	DECLARE_FIFO(long, free);
+	DECLARE_FIFO(long, free)[RESERVE_NR];
 	DECLARE_FIFO(long, free_inc);
 	DECLARE_FIFO(long, unused);
 
@@ -515,7 +514,8 @@ struct cache_set {
 	uint64_t		cached_dev_sectors;
 	struct closure		caching;
 
-	struct closure_with_waitlist sb_write;
+	struct closure		sb_write;
+	struct semaphore	sb_write_mutex;
 
 	mempool_t		*search;
 	mempool_t		*bio_meta;
@@ -630,13 +630,15 @@ struct cache_set {
 
 #ifdef CONFIG_BCACHE_DEBUG
 	struct btree		*verify_data;
+	struct bset		*verify_ondisk;
 	struct mutex		verify_lock;
 #endif
 
 	unsigned		nr_uuids;
 	struct uuid_entry	*uuids;
 	BKEY_PADDED(uuid_bucket);
-	struct closure_with_waitlist uuid_write;
+	struct closure		uuid_write;
+	struct semaphore	uuid_write_mutex;
 
 	/*
 	 * A btree node on disk could have too many bsets for an iterator to fit
@@ -644,13 +646,7 @@ struct cache_set {
 	 */
 	mempool_t		*fill_iter;
 
-	/*
-	 * btree_sort() is a merge sort and requires temporary space - single
-	 * element mempool
-	 */
-	struct mutex		sort_lock;
-	struct bset		*sort;
-	unsigned		sort_crit_factor;
+	struct bset_sort_state	sort;
 
 	/* List of buckets we're currently writing data to */
 	struct list_head	data_buckets;
@@ -666,7 +662,6 @@ struct cache_set {
 	unsigned		congested_read_threshold_us;
 	unsigned		congested_write_threshold_us;
 
-	struct time_stats	sort_time;
 	struct time_stats	btree_gc_time;
 	struct time_stats	btree_split_time;
 	struct time_stats	btree_read_time;
@@ -684,9 +679,9 @@ struct cache_set {
 	unsigned		error_decay;
 
 	unsigned short		journal_delay_ms;
+	bool			expensive_debug_checks;
 	unsigned		verify:1;
 	unsigned		key_merging_disabled:1;
-	unsigned		expensive_debug_checks:1;
 	unsigned		gc_always_rewrite:1;
 	unsigned		shrinker_disabled:1;
 	unsigned		copy_gc_enabled:1;
@@ -708,13 +703,8 @@ struct bbio {
 	struct bio		bio;
 };
 
-static inline unsigned local_clock_us(void)
-{
-	return local_clock() >> 10;
-}
-
 #define BTREE_PRIO		USHRT_MAX
-#define INITIAL_PRIO		32768
+#define INITIAL_PRIO		32768U
 
 #define btree_bytes(c)		((c)->btree_pages * PAGE_SIZE)
 #define btree_blocks(b)							\
@@ -727,21 +717,6 @@ static inline unsigned local_clock_us(void)
 #define bucket_bytes(c)		((c)->sb.bucket_size << 9)
 #define block_bytes(c)		((c)->sb.block_size << 9)
 
-#define __set_bytes(i, k)	(sizeof(*(i)) + (k) * sizeof(uint64_t))
-#define set_bytes(i)		__set_bytes(i, i->keys)
-
-#define __set_blocks(i, k, c)	DIV_ROUND_UP(__set_bytes(i, k), block_bytes(c))
-#define set_blocks(i, c)	__set_blocks(i, (i)->keys, c)
-
-#define node(i, j)		((struct bkey *) ((i)->d + (j)))
-#define end(i)			node(i, (i)->keys)
-
-#define index(i, b)							\
-	((size_t) (((void *) i - (void *) (b)->sets[0].data) /		\
-		   block_bytes(b->c)))
-
-#define btree_data_space(b)	(PAGE_SIZE << (b)->page_order)
-
 #define prios_per_bucket(c)				\
 	((bucket_bytes(c) - sizeof(struct prio_set)) /	\
 	 sizeof(struct bucket_disk))
@@ -784,20 +759,34 @@ static inline struct bucket *PTR_BUCKET(struct cache_set *c,
 	return PTR_CACHE(c, k, ptr)->buckets + PTR_BUCKET_NR(c, k, ptr);
 }
 
-/* Btree key macros */
+static inline uint8_t gen_after(uint8_t a, uint8_t b)
+{
+	uint8_t r = a - b;
+	return r > 128U ? 0 : r;
+}
 
-static inline void bkey_init(struct bkey *k)
+static inline uint8_t ptr_stale(struct cache_set *c, const struct bkey *k,
+				unsigned i)
 {
-	*k = ZERO_KEY;
+	return gen_after(PTR_BUCKET(c, k, i)->gen, PTR_GEN(k, i));
 }
 
+static inline bool ptr_available(struct cache_set *c, const struct bkey *k,
+				 unsigned i)
+{
+	return (PTR_DEV(k, i) < MAX_CACHES_PER_SET) && PTR_CACHE(c, k, i);
+}
+
+/* Btree key macros */
+
 /*
  * This is used for various on disk data structures - cache_sb, prio_set, bset,
  * jset: The checksum is _always_ the first 8 bytes of these structs
  */
 #define csum_set(i)							\
 	bch_crc64(((void *) (i)) + sizeof(uint64_t),			\
-	      ((void *) end(i)) - (((void *) (i)) + sizeof(uint64_t)))
+		  ((void *) bset_bkey_last(i)) -			\
+		  (((void *) (i)) + sizeof(uint64_t)))
 
 /* Error handling macros */
 
@@ -902,7 +891,6 @@ void bch_bbio_endio(struct cache_set *, struct bio *, int, const char *);
 void bch_bbio_free(struct bio *, struct cache_set *);
 struct bio *bch_bbio_alloc(struct cache_set *);
 
-struct bio *bch_bio_split(struct bio *, int, gfp_t, struct bio_set *);
 void bch_generic_make_request(struct bio *, struct bio_split_pool *);
 void __bch_submit_bbio(struct bio *, struct cache_set *);
 void bch_submit_bbio(struct bio *, struct cache_set *, struct bkey *, unsigned);
diff --git a/drivers/md/bcache/bset.c b/drivers/md/bcache/bset.c
index 7d388b8bb50e..4f6b5940e609 100644
--- a/drivers/md/bcache/bset.c
+++ b/drivers/md/bcache/bset.c
@@ -5,30 +5,134 @@
  * Copyright 2012 Google, Inc.
  */
 
-#include "bcache.h"
-#include "btree.h"
-#include "debug.h"
+#define pr_fmt(fmt) "bcache: %s() " fmt "\n", __func__
 
+#include "util.h"
+#include "bset.h"
+
+#include <linux/console.h>
 #include <linux/random.h>
 #include <linux/prefetch.h>
 
+#ifdef CONFIG_BCACHE_DEBUG
+
+void bch_dump_bset(struct btree_keys *b, struct bset *i, unsigned set)
+{
+	struct bkey *k, *next;
+
+	for (k = i->start; k < bset_bkey_last(i); k = next) {
+		next = bkey_next(k);
+
+		printk(KERN_ERR "block %u key %zi/%u: ", set,
+		       (uint64_t *) k - i->d, i->keys);
+
+		if (b->ops->key_dump)
+			b->ops->key_dump(b, k);
+		else
+			printk("%llu:%llu\n", KEY_INODE(k), KEY_OFFSET(k));
+
+		if (next < bset_bkey_last(i) &&
+		    bkey_cmp(k, b->ops->is_extents ?
+			     &START_KEY(next) : next) > 0)
+			printk(KERN_ERR "Key skipped backwards\n");
+	}
+}
+
+void bch_dump_bucket(struct btree_keys *b)
+{
+	unsigned i;
+
+	console_lock();
+	for (i = 0; i <= b->nsets; i++)
+		bch_dump_bset(b, b->set[i].data,
+			      bset_sector_offset(b, b->set[i].data));
+	console_unlock();
+}
+
+int __bch_count_data(struct btree_keys *b)
+{
+	unsigned ret = 0;
+	struct btree_iter iter;
+	struct bkey *k;
+
+	if (b->ops->is_extents)
+		for_each_key(b, k, &iter)
+			ret += KEY_SIZE(k);
+	return ret;
+}
+
+void __bch_check_keys(struct btree_keys *b, const char *fmt, ...)
+{
+	va_list args;
+	struct bkey *k, *p = NULL;
+	struct btree_iter iter;
+	const char *err;
+
+	for_each_key(b, k, &iter) {
+		if (b->ops->is_extents) {
+			err = "Keys out of order";
+			if (p && bkey_cmp(&START_KEY(p), &START_KEY(k)) > 0)
+				goto bug;
+
+			if (bch_ptr_invalid(b, k))
+				continue;
+
+			err =  "Overlapping keys";
+			if (p && bkey_cmp(p, &START_KEY(k)) > 0)
+				goto bug;
+		} else {
+			if (bch_ptr_bad(b, k))
+				continue;
+
+			err = "Duplicate keys";
+			if (p && !bkey_cmp(p, k))
+				goto bug;
+		}
+		p = k;
+	}
+#if 0
+	err = "Key larger than btree node key";
+	if (p && bkey_cmp(p, &b->key) > 0)
+		goto bug;
+#endif
+	return;
+bug:
+	bch_dump_bucket(b);
+
+	va_start(args, fmt);
+	vprintk(fmt, args);
+	va_end(args);
+
+	panic("bch_check_keys error:  %s:\n", err);
+}
+
+static void bch_btree_iter_next_check(struct btree_iter *iter)
+{
+	struct bkey *k = iter->data->k, *next = bkey_next(k);
+
+	if (next < iter->data->end &&
+	    bkey_cmp(k, iter->b->ops->is_extents ?
+		     &START_KEY(next) : next) > 0) {
+		bch_dump_bucket(iter->b);
+		panic("Key skipped backwards\n");
+	}
+}
+
+#else
+
+static inline void bch_btree_iter_next_check(struct btree_iter *iter) {}
+
+#endif
+
 /* Keylists */
 
-int bch_keylist_realloc(struct keylist *l, int nptrs, struct cache_set *c)
+int __bch_keylist_realloc(struct keylist *l, unsigned u64s)
 {
 	size_t oldsize = bch_keylist_nkeys(l);
-	size_t newsize = oldsize + 2 + nptrs;
+	size_t newsize = oldsize + u64s;
 	uint64_t *old_keys = l->keys_p == l->inline_keys ? NULL : l->keys_p;
 	uint64_t *new_keys;
 
-	/* The journalling code doesn't handle the case where the keys to insert
-	 * is bigger than an empty write: If we just return -ENOMEM here,
-	 * bio_insert() and bio_invalidate() will insert the keys created so far
-	 * and finish the rest when the keylist is empty.
-	 */
-	if (newsize * sizeof(uint64_t) > block_bytes(c) - sizeof(struct jset))
-		return -ENOMEM;
-
 	newsize = roundup_pow_of_two(newsize);
 
 	if (newsize <= KEYLIST_INLINE ||
@@ -71,136 +175,6 @@ void bch_keylist_pop_front(struct keylist *l)
 		bch_keylist_bytes(l));
 }
 
-/* Pointer validation */
-
-static bool __ptr_invalid(struct cache_set *c, const struct bkey *k)
-{
-	unsigned i;
-
-	for (i = 0; i < KEY_PTRS(k); i++)
-		if (ptr_available(c, k, i)) {
-			struct cache *ca = PTR_CACHE(c, k, i);
-			size_t bucket = PTR_BUCKET_NR(c, k, i);
-			size_t r = bucket_remainder(c, PTR_OFFSET(k, i));
-
-			if (KEY_SIZE(k) + r > c->sb.bucket_size ||
-			    bucket <  ca->sb.first_bucket ||
-			    bucket >= ca->sb.nbuckets)
-				return true;
-		}
-
-	return false;
-}
-
-bool bch_btree_ptr_invalid(struct cache_set *c, const struct bkey *k)
-{
-	char buf[80];
-
-	if (!KEY_PTRS(k) || !KEY_SIZE(k) || KEY_DIRTY(k))
-		goto bad;
-
-	if (__ptr_invalid(c, k))
-		goto bad;
-
-	return false;
-bad:
-	bch_bkey_to_text(buf, sizeof(buf), k);
-	cache_bug(c, "spotted btree ptr %s: %s", buf, bch_ptr_status(c, k));
-	return true;
-}
-
-bool bch_extent_ptr_invalid(struct cache_set *c, const struct bkey *k)
-{
-	char buf[80];
-
-	if (!KEY_SIZE(k))
-		return true;
-
-	if (KEY_SIZE(k) > KEY_OFFSET(k))
-		goto bad;
-
-	if (__ptr_invalid(c, k))
-		goto bad;
-
-	return false;
-bad:
-	bch_bkey_to_text(buf, sizeof(buf), k);
-	cache_bug(c, "spotted extent %s: %s", buf, bch_ptr_status(c, k));
-	return true;
-}
-
-static bool ptr_bad_expensive_checks(struct btree *b, const struct bkey *k,
-				     unsigned ptr)
-{
-	struct bucket *g = PTR_BUCKET(b->c, k, ptr);
-	char buf[80];
-
-	if (mutex_trylock(&b->c->bucket_lock)) {
-		if (b->level) {
-			if (KEY_DIRTY(k) ||
-			    g->prio != BTREE_PRIO ||
-			    (b->c->gc_mark_valid &&
-			     GC_MARK(g) != GC_MARK_METADATA))
-				goto err;
-
-		} else {
-			if (g->prio == BTREE_PRIO)
-				goto err;
-
-			if (KEY_DIRTY(k) &&
-			    b->c->gc_mark_valid &&
-			    GC_MARK(g) != GC_MARK_DIRTY)
-				goto err;
-		}
-		mutex_unlock(&b->c->bucket_lock);
-	}
-
-	return false;
-err:
-	mutex_unlock(&b->c->bucket_lock);
-	bch_bkey_to_text(buf, sizeof(buf), k);
-	btree_bug(b,
-"inconsistent pointer %s: bucket %zu pin %i prio %i gen %i last_gc %i mark %llu gc_gen %i",
-		  buf, PTR_BUCKET_NR(b->c, k, ptr), atomic_read(&g->pin),
-		  g->prio, g->gen, g->last_gc, GC_MARK(g), g->gc_gen);
-	return true;
-}
-
-bool bch_ptr_bad(struct btree *b, const struct bkey *k)
-{
-	struct bucket *g;
-	unsigned i, stale;
-
-	if (!bkey_cmp(k, &ZERO_KEY) ||
-	    !KEY_PTRS(k) ||
-	    bch_ptr_invalid(b, k))
-		return true;
-
-	for (i = 0; i < KEY_PTRS(k); i++) {
-		if (!ptr_available(b->c, k, i))
-			return true;
-
-		g = PTR_BUCKET(b->c, k, i);
-		stale = ptr_stale(b->c, k, i);
-
-		btree_bug_on(stale > 96, b,
-			     "key too stale: %i, need_gc %u",
-			     stale, b->c->need_gc);
-
-		btree_bug_on(stale && KEY_DIRTY(k) && KEY_SIZE(k),
-			     b, "stale dirty pointer");
-
-		if (stale)
-			return true;
-
-		if (expensive_debug_checks(b->c) &&
-		    ptr_bad_expensive_checks(b, k, i))
-			return true;
-	}
-
-	return false;
-}
-
 /* Key/pointer manipulation */
 
 void bch_bkey_copy_single_ptr(struct bkey *dest, const struct bkey *src,
@@ -255,56 +229,138 @@ bool __bch_cut_back(const struct bkey *where, struct bkey *k)
 	return true;
 }
 
-static uint64_t merge_chksums(struct bkey *l, struct bkey *r)
+/* Auxiliary search trees */
+
+/* 32 bits total: */
+#define BKEY_MID_BITS		3
+#define BKEY_EXPONENT_BITS	7
+#define BKEY_MANTISSA_BITS	(32 - BKEY_MID_BITS - BKEY_EXPONENT_BITS)
+#define BKEY_MANTISSA_MASK	((1 << BKEY_MANTISSA_BITS) - 1)
+
+struct bkey_float {
+	unsigned	exponent:BKEY_EXPONENT_BITS;
+	unsigned	m:BKEY_MID_BITS;
+	unsigned	mantissa:BKEY_MANTISSA_BITS;
+} __packed;
+
+/*
+ * BSET_CACHELINE was originally intended to match the hardware cacheline size -
+ * it used to be 64, but I realized the lookup code would touch slightly less
+ * memory if it was 128.
+ *
+ * It definites the number of bytes (in struct bset) per struct bkey_float in
+ * the auxiliar search tree - when we're done searching the bset_float tree we
+ * have this many bytes left that we do a linear search over.
+ *
+ * Since (after level 5) every level of the bset_tree is on a new cacheline,
+ * we're touching one fewer cacheline in the bset tree in exchange for one more
+ * cacheline in the linear search - but the linear search might stop before it
+ * gets to the second cacheline.
+ */
+
+#define BSET_CACHELINE		128
+
+/* Space required for the btree node keys */
+static inline size_t btree_keys_bytes(struct btree_keys *b)
 {
-	return (l->ptr[KEY_PTRS(l)] + r->ptr[KEY_PTRS(r)]) &
-		~((uint64_t)1 << 63);
+	return PAGE_SIZE << b->page_order;
 }
 
-/* Tries to merge l and r: l should be lower than r
- * Returns true if we were able to merge. If we did merge, l will be the merged
- * key, r will be untouched.
- */
-bool bch_bkey_try_merge(struct btree *b, struct bkey *l, struct bkey *r)
+static inline size_t btree_keys_cachelines(struct btree_keys *b)
 {
-	unsigned i;
+	return btree_keys_bytes(b) / BSET_CACHELINE;
+}
 
-	if (key_merging_disabled(b->c))
-		return false;
+/* Space required for the auxiliary search trees */
+static inline size_t bset_tree_bytes(struct btree_keys *b)
+{
+	return btree_keys_cachelines(b) * sizeof(struct bkey_float);
+}
 
-	if (KEY_PTRS(l) != KEY_PTRS(r) ||
-	    KEY_DIRTY(l) != KEY_DIRTY(r) ||
-	    bkey_cmp(l, &START_KEY(r)))
-		return false;
+/* Space required for the prev pointers */
+static inline size_t bset_prev_bytes(struct btree_keys *b)
+{
+	return btree_keys_cachelines(b) * sizeof(uint8_t);
+}
 
-	for (i = 0; i < KEY_PTRS(l); i++)
-		if (l->ptr[i] + PTR(0, KEY_SIZE(l), 0) != r->ptr[i] ||
-		    PTR_BUCKET_NR(b->c, l, i) != PTR_BUCKET_NR(b->c, r, i))
-			return false;
+/* Memory allocation */
 
-	/* Keys with no pointers aren't restricted to one bucket and could
-	 * overflow KEY_SIZE
-	 */
-	if (KEY_SIZE(l) + KEY_SIZE(r) > USHRT_MAX) {
-		SET_KEY_OFFSET(l, KEY_OFFSET(l) + USHRT_MAX - KEY_SIZE(l));
-		SET_KEY_SIZE(l, USHRT_MAX);
+void bch_btree_keys_free(struct btree_keys *b)
+{
+	struct bset_tree *t = b->set;
 
-		bch_cut_front(l, r);
-		return false;
-	}
+	if (bset_prev_bytes(b) < PAGE_SIZE)
+		kfree(t->prev);
+	else
+		free_pages((unsigned long) t->prev,
+			   get_order(bset_prev_bytes(b)));
 
-	if (KEY_CSUM(l)) {
-		if (KEY_CSUM(r))
-			l->ptr[KEY_PTRS(l)] = merge_chksums(l, r);
-		else
-			SET_KEY_CSUM(l, 0);
-	}
+	if (bset_tree_bytes(b) < PAGE_SIZE)
+		kfree(t->tree);
+	else
+		free_pages((unsigned long) t->tree,
+			   get_order(bset_tree_bytes(b)));
 
-	SET_KEY_OFFSET(l, KEY_OFFSET(l) + KEY_SIZE(r));
-	SET_KEY_SIZE(l, KEY_SIZE(l) + KEY_SIZE(r));
+	free_pages((unsigned long) t->data, b->page_order);
 
-	return true;
+	t->prev = NULL;
+	t->tree = NULL;
+	t->data = NULL;
+}
+EXPORT_SYMBOL(bch_btree_keys_free);
+
+int bch_btree_keys_alloc(struct btree_keys *b, unsigned page_order, gfp_t gfp)
+{
+	struct bset_tree *t = b->set;
+
+	BUG_ON(t->data);
+
+	b->page_order = page_order;
+
+	t->data = (void *) __get_free_pages(gfp, b->page_order);
+	if (!t->data)
+		goto err;
+
+	t->tree = bset_tree_bytes(b) < PAGE_SIZE
+		? kmalloc(bset_tree_bytes(b), gfp)
+		: (void *) __get_free_pages(gfp, get_order(bset_tree_bytes(b)));
+	if (!t->tree)
+		goto err;
+
+	t->prev = bset_prev_bytes(b) < PAGE_SIZE
+		? kmalloc(bset_prev_bytes(b), gfp)
+		: (void *) __get_free_pages(gfp, get_order(bset_prev_bytes(b)));
+	if (!t->prev)
+		goto err;
+
+	return 0;
+err:
+	bch_btree_keys_free(b);
+	return -ENOMEM;
 }
+EXPORT_SYMBOL(bch_btree_keys_alloc);
+
+void bch_btree_keys_init(struct btree_keys *b, const struct btree_keys_ops *ops,
+			 bool *expensive_debug_checks)
+{
+	unsigned i;
+
+	b->ops = ops;
+	b->expensive_debug_checks = expensive_debug_checks;
+	b->nsets = 0;
+	b->last_set_unwritten = 0;
+
+	/* XXX: shouldn't be needed */
+	for (i = 0; i < MAX_BSETS; i++)
+		b->set[i].size = 0;
+	/*
+	 * Second loop starts at 1 because b->keys[0]->data is the memory we
+	 * allocated
+	 */
+	for (i = 1; i < MAX_BSETS; i++)
+		b->set[i].data = NULL;
+}
+EXPORT_SYMBOL(bch_btree_keys_init);
 
 /* Binary tree stuff for auxiliary search trees */
 
@@ -455,9 +511,11 @@ static unsigned bkey_to_cacheline(struct bset_tree *t, struct bkey *k)
 	return ((void *) k - (void *) t->data) / BSET_CACHELINE;
 }
 
-static unsigned bkey_to_cacheline_offset(struct bkey *k)
+static unsigned bkey_to_cacheline_offset(struct bset_tree *t,
+					 unsigned cacheline,
+					 struct bkey *k)
 {
-	return ((size_t) k & (BSET_CACHELINE - 1)) / sizeof(uint64_t);
+	return (u64 *) k - (u64 *) cacheline_to_bkey(t, cacheline, 0);
 }
 
 static struct bkey *tree_to_bkey(struct bset_tree *t, unsigned j)
@@ -504,7 +562,7 @@ static void make_bfloat(struct bset_tree *t, unsigned j)
 		: tree_to_prev_bkey(t, j >> ffs(j));
 
 	struct bkey *r = is_power_of_2(j + 1)
-		? node(t->data, t->data->keys - bkey_u64s(&t->end))
+		? bset_bkey_idx(t->data, t->data->keys - bkey_u64s(&t->end))
 		: tree_to_bkey(t, j >> (ffz(j) + 1));
 
 	BUG_ON(m < l || m > r);
@@ -528,9 +586,9 @@ static void make_bfloat(struct bset_tree *t, unsigned j)
 		f->exponent = 127;
 }
 
-static void bset_alloc_tree(struct btree *b, struct bset_tree *t)
+static void bset_alloc_tree(struct btree_keys *b, struct bset_tree *t)
 {
-	if (t != b->sets) {
+	if (t != b->set) {
 		unsigned j = roundup(t[-1].size,
 				     64 / sizeof(struct bkey_float));
 
@@ -538,33 +596,54 @@ static void bset_alloc_tree(struct btree *b, struct bset_tree *t)
 		t->prev = t[-1].prev + j;
 	}
 
-	while (t < b->sets + MAX_BSETS)
+	while (t < b->set + MAX_BSETS)
 		t++->size = 0;
 }
 
-static void bset_build_unwritten_tree(struct btree *b)
+static void bch_bset_build_unwritten_tree(struct btree_keys *b)
 {
-	struct bset_tree *t = b->sets + b->nsets;
+	struct bset_tree *t = bset_tree_last(b);
+
+	BUG_ON(b->last_set_unwritten);
+	b->last_set_unwritten = 1;
 
 	bset_alloc_tree(b, t);
 
-	if (t->tree != b->sets->tree + bset_tree_space(b)) {
-		t->prev[0] = bkey_to_cacheline_offset(t->data->start);
+	if (t->tree != b->set->tree + btree_keys_cachelines(b)) {
+		t->prev[0] = bkey_to_cacheline_offset(t, 0, t->data->start);
 		t->size = 1;
 	}
 }
 
-static void bset_build_written_tree(struct btree *b)
+void bch_bset_init_next(struct btree_keys *b, struct bset *i, uint64_t magic)
+{
+	if (i != b->set->data) {
+		b->set[++b->nsets].data = i;
+		i->seq = b->set->data->seq;
+	} else
+		get_random_bytes(&i->seq, sizeof(uint64_t));
+
+	i->magic	= magic;
+	i->version	= 0;
+	i->keys		= 0;
+
+	bch_bset_build_unwritten_tree(b);
+}
+EXPORT_SYMBOL(bch_bset_init_next);
+
+void bch_bset_build_written_tree(struct btree_keys *b)
 {
-	struct bset_tree *t = b->sets + b->nsets;
-	struct bkey *k = t->data->start;
+	struct bset_tree *t = bset_tree_last(b);
+	struct bkey *prev = NULL, *k = t->data->start;
 	unsigned j, cacheline = 1;
 
+	b->last_set_unwritten = 0;
+
 	bset_alloc_tree(b, t);
 
 	t->size = min_t(unsigned,
-			bkey_to_cacheline(t, end(t->data)),
-			b->sets->tree + bset_tree_space(b) - t->tree);
+			bkey_to_cacheline(t, bset_bkey_last(t->data)),
+			b->set->tree + btree_keys_cachelines(b) - t->tree);
 
 	if (t->size < 2) {
 		t->size = 0;
@@ -577,16 +656,14 @@ static void bset_build_written_tree(struct btree *b)
 	for (j = inorder_next(0, t->size);
 	     j;
 	     j = inorder_next(j, t->size)) {
-		while (bkey_to_cacheline(t, k) != cacheline)
-			k = bkey_next(k);
+		while (bkey_to_cacheline(t, k) < cacheline)
+			prev = k, k = bkey_next(k);
 
-		t->prev[j] = bkey_u64s(k);
-		k = bkey_next(k);
-		cacheline++;
-		t->tree[j].m = bkey_to_cacheline_offset(k);
+		t->prev[j] = bkey_u64s(prev);
+		t->tree[j].m = bkey_to_cacheline_offset(t, cacheline++, k);
 	}
 
-	while (bkey_next(k) != end(t->data))
+	while (bkey_next(k) != bset_bkey_last(t->data))
 		k = bkey_next(k);
 
 	t->end = *k;
@@ -597,14 +674,17 @@ static void bset_build_written_tree(struct btree *b)
 	     j = inorder_next(j, t->size))
 		make_bfloat(t, j);
 }
+EXPORT_SYMBOL(bch_bset_build_written_tree);
 
-void bch_bset_fix_invalidated_key(struct btree *b, struct bkey *k)
+/* Insert */
+
+void bch_bset_fix_invalidated_key(struct btree_keys *b, struct bkey *k)
 {
 	struct bset_tree *t;
 	unsigned inorder, j = 1;
 
-	for (t = b->sets; t <= &b->sets[b->nsets]; t++)
-		if (k < end(t->data))
+	for (t = b->set; t <= bset_tree_last(b); t++)
+		if (k < bset_bkey_last(t->data))
 			goto found_set;
 
 	BUG();
@@ -617,7 +697,7 @@ found_set:
 	if (k == t->data->start)
 		goto fix_left;
 
-	if (bkey_next(k) == end(t->data)) {
+	if (bkey_next(k) == bset_bkey_last(t->data)) {
 		t->end = *k;
 		goto fix_right;
 	}
@@ -642,10 +722,12 @@ fix_right:	do {
 			j = j * 2 + 1;
 		} while (j < t->size);
 }
+EXPORT_SYMBOL(bch_bset_fix_invalidated_key);
 
-void bch_bset_fix_lookup_table(struct btree *b, struct bkey *k)
+static void bch_bset_fix_lookup_table(struct btree_keys *b,
+				      struct bset_tree *t,
+				      struct bkey *k)
 {
-	struct bset_tree *t = &b->sets[b->nsets];
 	unsigned shift = bkey_u64s(k);
 	unsigned j = bkey_to_cacheline(t, k);
 
@@ -657,8 +739,8 @@ void bch_bset_fix_lookup_table(struct btree *b, struct bkey *k)
 	 * lookup table for the first key that is strictly greater than k:
 	 * it's either k's cacheline or the next one
 	 */
-	if (j < t->size &&
-	    table_to_bkey(t, j) <= k)
+	while (j < t->size &&
+	       table_to_bkey(t, j) <= k)
 		j++;
 
 	/* Adjust all the lookup table entries, and find a new key for any that
@@ -673,54 +755,124 @@ void bch_bset_fix_lookup_table(struct btree *b, struct bkey *k)
 			while (k < cacheline_to_bkey(t, j, 0))
 				k = bkey_next(k);
 
-			t->prev[j] = bkey_to_cacheline_offset(k);
+			t->prev[j] = bkey_to_cacheline_offset(t, j, k);
 		}
 	}
 
-	if (t->size == b->sets->tree + bset_tree_space(b) - t->tree)
+	if (t->size == b->set->tree + btree_keys_cachelines(b) - t->tree)
 		return;
 
 	/* Possibly add a new entry to the end of the lookup table */
 
 	for (k = table_to_bkey(t, t->size - 1);
-	     k != end(t->data);
+	     k != bset_bkey_last(t->data);
 	     k = bkey_next(k))
 		if (t->size == bkey_to_cacheline(t, k)) {
-			t->prev[t->size] = bkey_to_cacheline_offset(k);
+			t->prev[t->size] = bkey_to_cacheline_offset(t, t->size, k);
 			t->size++;
 		}
 }
 
-void bch_bset_init_next(struct btree *b)
+/*
+ * Tries to merge l and r: l should be lower than r
+ * Returns true if we were able to merge. If we did merge, l will be the merged
+ * key, r will be untouched.
+ */
+bool bch_bkey_try_merge(struct btree_keys *b, struct bkey *l, struct bkey *r)
 {
-	struct bset *i = write_block(b);
+	if (!b->ops->key_merge)
+		return false;
 
-	if (i != b->sets[0].data) {
-		b->sets[++b->nsets].data = i;
-		i->seq = b->sets[0].data->seq;
-	} else
-		get_random_bytes(&i->seq, sizeof(uint64_t));
+	/*
+	 * Generic header checks
+	 * Assumes left and right are in order
+	 * Left and right must be exactly aligned
+	 */
+	if (!bch_bkey_equal_header(l, r) ||
+	     bkey_cmp(l, &START_KEY(r)))
+		return false;
 
-	i->magic	= bset_magic(&b->c->sb);
-	i->version	= 0;
-	i->keys		= 0;
+	return b->ops->key_merge(b, l, r);
+}
+EXPORT_SYMBOL(bch_bkey_try_merge);
 
-	bset_build_unwritten_tree(b);
+void bch_bset_insert(struct btree_keys *b, struct bkey *where,
+		     struct bkey *insert)
+{
+	struct bset_tree *t = bset_tree_last(b);
+
+	BUG_ON(!b->last_set_unwritten);
+	BUG_ON(bset_byte_offset(b, t->data) +
+	       __set_bytes(t->data, t->data->keys + bkey_u64s(insert)) >
+	       PAGE_SIZE << b->page_order);
+
+	memmove((uint64_t *) where + bkey_u64s(insert),
+		where,
+		(void *) bset_bkey_last(t->data) - (void *) where);
+
+	t->data->keys += bkey_u64s(insert);
+	bkey_copy(where, insert);
+	bch_bset_fix_lookup_table(b, t, where);
 }
+EXPORT_SYMBOL(bch_bset_insert);
+
+unsigned bch_btree_insert_key(struct btree_keys *b, struct bkey *k,
+			      struct bkey *replace_key)
+{
+	unsigned status = BTREE_INSERT_STATUS_NO_INSERT;
+	struct bset *i = bset_tree_last(b)->data;
+	struct bkey *m, *prev = NULL;
+	struct btree_iter iter;
+
+	BUG_ON(b->ops->is_extents && !KEY_SIZE(k));
+
+	m = bch_btree_iter_init(b, &iter, b->ops->is_extents
+				? PRECEDING_KEY(&START_KEY(k))
+				: PRECEDING_KEY(k));
+
+	if (b->ops->insert_fixup(b, k, &iter, replace_key))
+		return status;
+
+	status = BTREE_INSERT_STATUS_INSERT;
+
+	while (m != bset_bkey_last(i) &&
+	       bkey_cmp(k, b->ops->is_extents ? &START_KEY(m) : m) > 0)
+		prev = m, m = bkey_next(m);
+
+	/* prev is in the tree, if we merge we're done */
+	status = BTREE_INSERT_STATUS_BACK_MERGE;
+	if (prev &&
+	    bch_bkey_try_merge(b, prev, k))
+		goto merged;
+#if 0
+	status = BTREE_INSERT_STATUS_OVERWROTE;
+	if (m != bset_bkey_last(i) &&
+	    KEY_PTRS(m) == KEY_PTRS(k) && !KEY_SIZE(m))
+		goto copy;
+#endif
+	status = BTREE_INSERT_STATUS_FRONT_MERGE;
+	if (m != bset_bkey_last(i) &&
+	    bch_bkey_try_merge(b, k, m))
+		goto copy;
+
+	bch_bset_insert(b, m, k);
+copy:	bkey_copy(m, k);
+merged:
+	return status;
+}
+EXPORT_SYMBOL(bch_btree_insert_key);
+
+/* Lookup */
 
 struct bset_search_iter {
 	struct bkey *l, *r;
 };
 
-static struct bset_search_iter bset_search_write_set(struct btree *b,
-						     struct bset_tree *t,
+static struct bset_search_iter bset_search_write_set(struct bset_tree *t,
 						     const struct bkey *search)
 {
 	unsigned li = 0, ri = t->size;
 
-	BUG_ON(!b->nsets &&
-	       t->size < bkey_to_cacheline(t, end(t->data)));
-
 	while (li + 1 != ri) {
 		unsigned m = (li + ri) >> 1;
 
@@ -732,12 +884,11 @@ static struct bset_search_iter bset_search_write_set(struct btree *b,
 
 	return (struct bset_search_iter) {
 		table_to_bkey(t, li),
-		ri < t->size ? table_to_bkey(t, ri) : end(t->data)
+		ri < t->size ? table_to_bkey(t, ri) : bset_bkey_last(t->data)
 	};
 }
 
-static struct bset_search_iter bset_search_tree(struct btree *b,
-						struct bset_tree *t,
+static struct bset_search_iter bset_search_tree(struct bset_tree *t,
 						const struct bkey *search)
 {
 	struct bkey *l, *r;
@@ -784,7 +935,7 @@ static struct bset_search_iter bset_search_tree(struct btree *b,
 			f = &t->tree[inorder_next(j, t->size)];
 			r = cacheline_to_bkey(t, inorder, f->m);
 		} else
-			r = end(t->data);
+			r = bset_bkey_last(t->data);
 	} else {
 		r = cacheline_to_bkey(t, inorder, f->m);
 
@@ -798,7 +949,7 @@ static struct bset_search_iter bset_search_tree(struct btree *b,
 	return (struct bset_search_iter) {l, r};
 }
 
-struct bkey *__bch_bset_search(struct btree *b, struct bset_tree *t,
+struct bkey *__bch_bset_search(struct btree_keys *b, struct bset_tree *t,
 			       const struct bkey *search)
 {
 	struct bset_search_iter i;
@@ -820,7 +971,7 @@ struct bkey *__bch_bset_search(struct btree *b, struct bset_tree *t,
 
 	if (unlikely(!t->size)) {
 		i.l = t->data->start;
-		i.r = end(t->data);
+		i.r = bset_bkey_last(t->data);
 	} else if (bset_written(b, t)) {
 		/*
 		 * Each node in the auxiliary search tree covers a certain range
@@ -830,23 +981,27 @@ struct bkey *__bch_bset_search(struct btree *b, struct bset_tree *t,
 		 */
 
 		if (unlikely(bkey_cmp(search, &t->end) >= 0))
-			return end(t->data);
+			return bset_bkey_last(t->data);
 
 		if (unlikely(bkey_cmp(search, t->data->start) < 0))
 			return t->data->start;
 
-		i = bset_search_tree(b, t, search);
-	} else
-		i = bset_search_write_set(b, t, search);
+		i = bset_search_tree(t, search);
+	} else {
+		BUG_ON(!b->nsets &&
+		       t->size < bkey_to_cacheline(t, bset_bkey_last(t->data)));
 
-	if (expensive_debug_checks(b->c)) {
+		i = bset_search_write_set(t, search);
+	}
+
+	if (btree_keys_expensive_checks(b)) {
 		BUG_ON(bset_written(b, t) &&
 		       i.l != t->data->start &&
 		       bkey_cmp(tree_to_prev_bkey(t,
 			  inorder_to_tree(bkey_to_cacheline(t, i.l), t)),
 				search) > 0);
 
-		BUG_ON(i.r != end(t->data) &&
+		BUG_ON(i.r != bset_bkey_last(t->data) &&
 		       bkey_cmp(i.r, search) <= 0);
 	}
 
@@ -856,22 +1011,17 @@ struct bkey *__bch_bset_search(struct btree *b, struct bset_tree *t,
 
 	return i.l;
 }
+EXPORT_SYMBOL(__bch_bset_search);
 
 /* Btree iterator */
 
-/*
- * Returns true if l > r - unless l == r, in which case returns true if l is
- * older than r.
- *
- * Necessary for btree_sort_fixup() - if there are multiple keys that compare
- * equal in different sets, we have to process them newest to oldest.
- */
+typedef bool (btree_iter_cmp_fn)(struct btree_iter_set,
+				 struct btree_iter_set);
+
 static inline bool btree_iter_cmp(struct btree_iter_set l,
 				  struct btree_iter_set r)
 {
-	int64_t c = bkey_cmp(&START_KEY(l.k), &START_KEY(r.k));
-
-	return c ? c > 0 : l.k < r.k;
+	return bkey_cmp(l.k, r.k) > 0;
 }
 
 static inline bool btree_iter_end(struct btree_iter *iter)
@@ -888,8 +1038,10 @@ void bch_btree_iter_push(struct btree_iter *iter, struct bkey *k,
 				 btree_iter_cmp));
 }
 
-struct bkey *__bch_btree_iter_init(struct btree *b, struct btree_iter *iter,
-				   struct bkey *search, struct bset_tree *start)
+static struct bkey *__bch_btree_iter_init(struct btree_keys *b,
+					  struct btree_iter *iter,
+					  struct bkey *search,
+					  struct bset_tree *start)
 {
 	struct bkey *ret = NULL;
 	iter->size = ARRAY_SIZE(iter->data);
@@ -899,15 +1051,24 @@ struct bkey *__bch_btree_iter_init(struct btree *b, struct btree_iter *iter,
 	iter->b = b;
 #endif
 
-	for (; start <= &b->sets[b->nsets]; start++) {
+	for (; start <= bset_tree_last(b); start++) {
 		ret = bch_bset_search(b, start, search);
-		bch_btree_iter_push(iter, ret, end(start->data));
+		bch_btree_iter_push(iter, ret, bset_bkey_last(start->data));
 	}
 
 	return ret;
 }
 
-struct bkey *bch_btree_iter_next(struct btree_iter *iter)
+struct bkey *bch_btree_iter_init(struct btree_keys *b,
+				 struct btree_iter *iter,
+				 struct bkey *search)
+{
+	return __bch_btree_iter_init(b, iter, search, b->set);
+}
+EXPORT_SYMBOL(bch_btree_iter_init);
+
+static inline struct bkey *__bch_btree_iter_next(struct btree_iter *iter,
+						 btree_iter_cmp_fn *cmp)
 {
 	struct btree_iter_set unused;
 	struct bkey *ret = NULL;
@@ -924,16 +1085,23 @@ struct bkey *bch_btree_iter_next(struct btree_iter *iter)
 		}
 
 		if (iter->data->k == iter->data->end)
-			heap_pop(iter, unused, btree_iter_cmp);
+			heap_pop(iter, unused, cmp);
 		else
-			heap_sift(iter, 0, btree_iter_cmp);
+			heap_sift(iter, 0, cmp);
 	}
 
 	return ret;
 }
 
+struct bkey *bch_btree_iter_next(struct btree_iter *iter)
+{
+	return __bch_btree_iter_next(iter, btree_iter_cmp);
+
+}
+EXPORT_SYMBOL(bch_btree_iter_next);
+
 struct bkey *bch_btree_iter_next_filter(struct btree_iter *iter,
-					struct btree *b, ptr_filter_fn fn)
+					struct btree_keys *b, ptr_filter_fn fn)
 {
 	struct bkey *ret;
 
@@ -946,70 +1114,58 @@ struct bkey *bch_btree_iter_next_filter(struct btree_iter *iter,
 
 /* Mergesort */
 
-static void sort_key_next(struct btree_iter *iter,
-			  struct btree_iter_set *i)
+void bch_bset_sort_state_free(struct bset_sort_state *state)
 {
-	i->k = bkey_next(i->k);
-
-	if (i->k == i->end)
-		*i = iter->data[--iter->used];
+	if (state->pool)
+		mempool_destroy(state->pool);
 }
 
-static void btree_sort_fixup(struct btree_iter *iter)
+int bch_bset_sort_state_init(struct bset_sort_state *state, unsigned page_order)
 {
-	while (iter->used > 1) {
-		struct btree_iter_set *top = iter->data, *i = top + 1;
+	spin_lock_init(&state->time.lock);
 
-		if (iter->used > 2 &&
-		    btree_iter_cmp(i[0], i[1]))
-			i++;
+	state->page_order = page_order;
+	state->crit_factor = int_sqrt(1 << page_order);
 
-		if (bkey_cmp(top->k, &START_KEY(i->k)) <= 0)
-			break;
-
-		if (!KEY_SIZE(i->k)) {
-			sort_key_next(iter, i);
-			heap_sift(iter, i - top, btree_iter_cmp);
-			continue;
-		}
-
-		if (top->k > i->k) {
-			if (bkey_cmp(top->k, i->k) >= 0)
-				sort_key_next(iter, i);
-			else
-				bch_cut_front(top->k, i->k);
+	state->pool = mempool_create_page_pool(1, page_order);
+	if (!state->pool)
+		return -ENOMEM;
 
-			heap_sift(iter, i - top, btree_iter_cmp);
-		} else {
-			/* can't happen because of comparison func */
-			BUG_ON(!bkey_cmp(&START_KEY(top->k), &START_KEY(i->k)));
-			bch_cut_back(&START_KEY(i->k), top->k);
-		}
-	}
+	return 0;
 }
+EXPORT_SYMBOL(bch_bset_sort_state_init);
 
-static void btree_mergesort(struct btree *b, struct bset *out,
+static void btree_mergesort(struct btree_keys *b, struct bset *out,
 			    struct btree_iter *iter,
 			    bool fixup, bool remove_stale)
 {
+	int i;
 	struct bkey *k, *last = NULL;
-	bool (*bad)(struct btree *, const struct bkey *) = remove_stale
+	BKEY_PADDED(k) tmp;
+	bool (*bad)(struct btree_keys *, const struct bkey *) = remove_stale
 		? bch_ptr_bad
 		: bch_ptr_invalid;
 
+	/* Heapify the iterator, using our comparison function */
+	for (i = iter->used / 2 - 1; i >= 0; --i)
+		heap_sift(iter, i, b->ops->sort_cmp);
+
 	while (!btree_iter_end(iter)) {
-		if (fixup && !b->level)
-			btree_sort_fixup(iter);
+		if (b->ops->sort_fixup && fixup)
+			k = b->ops->sort_fixup(iter, &tmp.k);
+		else
+			k = NULL;
+
+		if (!k)
+			k = __bch_btree_iter_next(iter, b->ops->sort_cmp);
 
-		k = bch_btree_iter_next(iter);
 		if (bad(b, k))
 			continue;
 
 		if (!last) {
 			last = out->start;
 			bkey_copy(last, k);
-		} else if (b->level ||
-			   !bch_bkey_try_merge(b, last, k)) {
+		} else if (!bch_bkey_try_merge(b, last, k)) {
 			last = bkey_next(last);
 			bkey_copy(last, k);
 		}
@@ -1020,27 +1176,27 @@ static void btree_mergesort(struct btree *b, struct bset *out,
 	pr_debug("sorted %i keys", out->keys);
 }
 
-static void __btree_sort(struct btree *b, struct btree_iter *iter,
-			 unsigned start, unsigned order, bool fixup)
+static void __btree_sort(struct btree_keys *b, struct btree_iter *iter,
+			 unsigned start, unsigned order, bool fixup,
+			 struct bset_sort_state *state)
 {
 	uint64_t start_time;
-	bool remove_stale = !b->written;
+	bool used_mempool = false;
 	struct bset *out = (void *) __get_free_pages(__GFP_NOWARN|GFP_NOIO,
 						     order);
 	if (!out) {
-		mutex_lock(&b->c->sort_lock);
-		out = b->c->sort;
-		order = ilog2(bucket_pages(b->c));
+		BUG_ON(order > state->page_order);
+
+		out = page_address(mempool_alloc(state->pool, GFP_NOIO));
+		used_mempool = true;
+		order = state->page_order;
 	}
 
 	start_time = local_clock();
 
-	btree_mergesort(b, out, iter, fixup, remove_stale);
+	btree_mergesort(b, out, iter, fixup, false);
 	b->nsets = start;
 
-	if (!fixup && !start && b->written)
-		bch_btree_verify(b, out);
-
 	if (!start && order == b->page_order) {
 		/*
 		 * Our temporary buffer is the same size as the btree node's
@@ -1048,84 +1204,76 @@ static void __btree_sort(struct btree *b, struct btree_iter *iter,
 		 * memcpy()
 		 */
 
-		out->magic	= bset_magic(&b->c->sb);
-		out->seq	= b->sets[0].data->seq;
-		out->version	= b->sets[0].data->version;
-		swap(out, b->sets[0].data);
-
-		if (b->c->sort == b->sets[0].data)
-			b->c->sort = out;
+		out->magic	= b->set->data->magic;
+		out->seq	= b->set->data->seq;
+		out->version	= b->set->data->version;
+		swap(out, b->set->data);
 	} else {
-		b->sets[start].data->keys = out->keys;
-		memcpy(b->sets[start].data->start, out->start,
-		       (void *) end(out) - (void *) out->start);
+		b->set[start].data->keys = out->keys;
+		memcpy(b->set[start].data->start, out->start,
+		       (void *) bset_bkey_last(out) - (void *) out->start);
 	}
 
-	if (out == b->c->sort)
-		mutex_unlock(&b->c->sort_lock);
+	if (used_mempool)
+		mempool_free(virt_to_page(out), state->pool);
 	else
 		free_pages((unsigned long) out, order);
 
-	if (b->written)
-		bset_build_written_tree(b);
+	bch_bset_build_written_tree(b);
 
 	if (!start)
-		bch_time_stats_update(&b->c->sort_time, start_time);
+		bch_time_stats_update(&state->time, start_time);
 }
 
-void bch_btree_sort_partial(struct btree *b, unsigned start)
+void bch_btree_sort_partial(struct btree_keys *b, unsigned start,
+			    struct bset_sort_state *state)
 {
 	size_t order = b->page_order, keys = 0;
 	struct btree_iter iter;
 	int oldsize = bch_count_data(b);
 
-	__bch_btree_iter_init(b, &iter, NULL, &b->sets[start]);
-
-	BUG_ON(b->sets[b->nsets].data == write_block(b) &&
-	       (b->sets[b->nsets].size || b->nsets));
-
+	__bch_btree_iter_init(b, &iter, NULL, &b->set[start]);
 
 	if (start) {
 		unsigned i;
 
 		for (i = start; i <= b->nsets; i++)
-			keys += b->sets[i].data->keys;
+			keys += b->set[i].data->keys;
 
-		order = roundup_pow_of_two(__set_bytes(b->sets->data,
-						       keys)) / PAGE_SIZE;
-		if (order)
-			order = ilog2(order);
+		order = get_order(__set_bytes(b->set->data, keys));
 	}
 
-	__btree_sort(b, &iter, start, order, false);
+	__btree_sort(b, &iter, start, order, false, state);
 
-	EBUG_ON(b->written && oldsize >= 0 && bch_count_data(b) != oldsize);
+	EBUG_ON(oldsize >= 0 && bch_count_data(b) != oldsize);
 }
+EXPORT_SYMBOL(bch_btree_sort_partial);
 
-void bch_btree_sort_and_fix_extents(struct btree *b, struct btree_iter *iter)
+void bch_btree_sort_and_fix_extents(struct btree_keys *b,
+				    struct btree_iter *iter,
+				    struct bset_sort_state *state)
 {
-	BUG_ON(!b->written);
-	__btree_sort(b, iter, 0, b->page_order, true);
+	__btree_sort(b, iter, 0, b->page_order, true, state);
 }
 
-void bch_btree_sort_into(struct btree *b, struct btree *new)
+void bch_btree_sort_into(struct btree_keys *b, struct btree_keys *new,
+			 struct bset_sort_state *state)
 {
 	uint64_t start_time = local_clock();
 
 	struct btree_iter iter;
 	bch_btree_iter_init(b, &iter, NULL);
 
-	btree_mergesort(b, new->sets->data, &iter, false, true);
+	btree_mergesort(b, new->set->data, &iter, false, true);
 
-	bch_time_stats_update(&b->c->sort_time, start_time);
+	bch_time_stats_update(&state->time, start_time);
 
-	bkey_copy_key(&new->key, &b->key);
-	new->sets->size = 0;
+	new->set->size = 0; // XXX: why?
 }
 
 #define SORT_CRIT	(4096 / sizeof(uint64_t))
 
-void bch_btree_sort_lazy(struct btree *b)
+void bch_btree_sort_lazy(struct btree_keys *b, struct bset_sort_state *state)
 {
 	unsigned crit = SORT_CRIT;
 	int i;
@@ -1134,50 +1282,32 @@ void bch_btree_sort_lazy(struct btree *b)
 	if (!b->nsets)
 		goto out;
 
-	/* If not a leaf node, always sort */
-	if (b->level) {
-		bch_btree_sort(b);
-		return;
-	}
-
 	for (i = b->nsets - 1; i >= 0; --i) {
-		crit *= b->c->sort_crit_factor;
+		crit *= state->crit_factor;
 
-		if (b->sets[i].data->keys < crit) {
-			bch_btree_sort_partial(b, i);
+		if (b->set[i].data->keys < crit) {
+			bch_btree_sort_partial(b, i, state);
 			return;
 		}
 	}
 
 	/* Sort if we'd overflow */
 	if (b->nsets + 1 == MAX_BSETS) {
-		bch_btree_sort(b);
+		bch_btree_sort(b, state);
 		return;
 	}
 
 out:
-	bset_build_written_tree(b);
+	bch_bset_build_written_tree(b);
 }
+EXPORT_SYMBOL(bch_btree_sort_lazy);
 
-/* Sysfs stuff */
-
-struct bset_stats {
-	struct btree_op op;
-	size_t nodes;
-	size_t sets_written, sets_unwritten;
-	size_t bytes_written, bytes_unwritten;
-	size_t floats, failed;
-};
-
-static int btree_bset_stats(struct btree_op *op, struct btree *b)
+void bch_btree_keys_stats(struct btree_keys *b, struct bset_stats *stats)
 {
-	struct bset_stats *stats = container_of(op, struct bset_stats, op);
 	unsigned i;
 
-	stats->nodes++;
-
 	for (i = 0; i <= b->nsets; i++) {
-		struct bset_tree *t = &b->sets[i];
+		struct bset_tree *t = &b->set[i];
 		size_t bytes = t->data->keys * sizeof(uint64_t);
 		size_t j;
 
@@ -1195,32 +1325,4 @@ static int btree_bset_stats(struct btree_op *op, struct btree *b)
 			stats->bytes_unwritten += bytes;
 		}
 	}
-
-	return MAP_CONTINUE;
-}
-
-int bch_bset_print_stats(struct cache_set *c, char *buf)
-{
-	struct bset_stats t;
-	int ret;
-
-	memset(&t, 0, sizeof(struct bset_stats));
-	bch_btree_op_init(&t.op, -1);
-
-	ret = bch_btree_map_nodes(&t.op, c, &ZERO_KEY, btree_bset_stats);
-	if (ret < 0)
-		return ret;
-
-	return snprintf(buf, PAGE_SIZE,
-			"btree nodes:		%zu\n"
-			"written sets:		%zu\n"
-			"unwritten sets:		%zu\n"
-			"written key bytes:	%zu\n"
-			"unwritten key bytes:	%zu\n"
-			"floats:			%zu\n"
-			"failed:			%zu\n",
-			t.nodes,
-			t.sets_written, t.sets_unwritten,
-			t.bytes_written, t.bytes_unwritten,
-			t.floats, t.failed);
 }
diff --git a/drivers/md/bcache/bset.h b/drivers/md/bcache/bset.h
index 1d3c24f9fa0e..003260f4ddf6 100644
--- a/drivers/md/bcache/bset.h
+++ b/drivers/md/bcache/bset.h
@@ -1,7 +1,11 @@
 #ifndef _BCACHE_BSET_H
 #define _BCACHE_BSET_H
 
-#include <linux/slab.h>
+#include <linux/bcache.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+
+#include "util.h" /* for time_stats */
 
 /*
  * BKEYS:
@@ -142,20 +146,13 @@
  * first key in that range of bytes again.
  */
 
-/* Btree key comparison/iteration */
+struct btree_keys;
+struct btree_iter;
+struct btree_iter_set;
+struct bkey_float;
 
 #define MAX_BSETS		4U
 
-struct btree_iter {
-	size_t size, used;
-#ifdef CONFIG_BCACHE_DEBUG
-	struct btree *b;
-#endif
-	struct btree_iter_set {
-		struct bkey *k, *end;
-	} data[MAX_BSETS];
-};
-
 struct bset_tree {
 	/*
 	 * We construct a binary tree in an array as if the array
@@ -165,14 +162,14 @@ struct bset_tree {
 	 */
 
 	/* size of the binary tree and prev array */
-	unsigned	size;
+	unsigned		size;
 
 	/* function of size - precalculated for to_inorder() */
-	unsigned	extra;
+	unsigned		extra;
 
 	/* copy of the last key in the set */
-	struct bkey	end;
-	struct bkey_float *tree;
+	struct bkey		end;
+	struct bkey_float	*tree;
 
 	/*
 	 * The nodes in the bset tree point to specific keys - this
@@ -182,12 +179,219 @@ struct bset_tree {
 	 * to keep bkey_float to 4 bytes and prev isn't used in the fast
 	 * path.
 	 */
-	uint8_t		*prev;
+	uint8_t			*prev;
 
 	/* The actual btree node, with pointers to each sorted set */
-	struct bset	*data;
+	struct bset		*data;
+};
+
+struct btree_keys_ops {
+	bool		(*sort_cmp)(struct btree_iter_set,
+				    struct btree_iter_set);
+	struct bkey	*(*sort_fixup)(struct btree_iter *, struct bkey *);
+	bool		(*insert_fixup)(struct btree_keys *, struct bkey *,
+					struct btree_iter *, struct bkey *);
+	bool		(*key_invalid)(struct btree_keys *,
+				       const struct bkey *);
+	bool		(*key_bad)(struct btree_keys *, const struct bkey *);
+	bool		(*key_merge)(struct btree_keys *,
+				     struct bkey *, struct bkey *);
+	void		(*key_to_text)(char *, size_t, const struct bkey *);
+	void		(*key_dump)(struct btree_keys *, const struct bkey *);
+
+	/*
+	 * Only used for deciding whether to use START_KEY(k) or just the key
+	 * itself in a couple places
+	 */
+	bool		is_extents;
+};
+
+struct btree_keys {
+	const struct btree_keys_ops	*ops;
+	uint8_t			page_order;
+	uint8_t			nsets;
+	unsigned		last_set_unwritten:1;
+	bool			*expensive_debug_checks;
+
+	/*
+	 * Sets of sorted keys - the real btree node - plus a binary search tree
+	 *
+	 * set[0] is special; set[0]->tree, set[0]->prev and set[0]->data point
+	 * to the memory we have allocated for this btree node. Additionally,
+	 * set[0]->data points to the entire btree node as it exists on disk.
+	 */
+	struct bset_tree	set[MAX_BSETS];
+};
+
+static inline struct bset_tree *bset_tree_last(struct btree_keys *b)
+{
+	return b->set + b->nsets;
+}
+
+static inline bool bset_written(struct btree_keys *b, struct bset_tree *t)
+{
+	return t <= b->set + b->nsets - b->last_set_unwritten;
+}
+
+static inline bool bkey_written(struct btree_keys *b, struct bkey *k)
+{
+	return !b->last_set_unwritten || k < b->set[b->nsets].data->start;
+}
+
+static inline unsigned bset_byte_offset(struct btree_keys *b, struct bset *i)
+{
+	return ((size_t) i) - ((size_t) b->set->data);
+}
+
+static inline unsigned bset_sector_offset(struct btree_keys *b, struct bset *i)
+{
+	return bset_byte_offset(b, i) >> 9;
+}
+
+#define __set_bytes(i, k)	(sizeof(*(i)) + (k) * sizeof(uint64_t))
+#define set_bytes(i)		__set_bytes(i, i->keys)
+
+#define __set_blocks(i, k, block_bytes)				\
+	DIV_ROUND_UP(__set_bytes(i, k), block_bytes)
+#define set_blocks(i, block_bytes)				\
+	__set_blocks(i, (i)->keys, block_bytes)
+
+static inline size_t bch_btree_keys_u64s_remaining(struct btree_keys *b)
+{
+	struct bset_tree *t = bset_tree_last(b);
+
+	BUG_ON((PAGE_SIZE << b->page_order) <
+	       (bset_byte_offset(b, t->data) + set_bytes(t->data)));
+
+	if (!b->last_set_unwritten)
+		return 0;
+
+	return ((PAGE_SIZE << b->page_order) -
+		(bset_byte_offset(b, t->data) + set_bytes(t->data))) /
+		sizeof(u64);
+}
+
+static inline struct bset *bset_next_set(struct btree_keys *b,
+					 unsigned block_bytes)
+{
+	struct bset *i = bset_tree_last(b)->data;
+
+	return ((void *) i) + roundup(set_bytes(i), block_bytes);
+}
+
+void bch_btree_keys_free(struct btree_keys *);
+int bch_btree_keys_alloc(struct btree_keys *, unsigned, gfp_t);
+void bch_btree_keys_init(struct btree_keys *, const struct btree_keys_ops *,
+			 bool *);
+
+void bch_bset_init_next(struct btree_keys *, struct bset *, uint64_t);
+void bch_bset_build_written_tree(struct btree_keys *);
+void bch_bset_fix_invalidated_key(struct btree_keys *, struct bkey *);
+bool bch_bkey_try_merge(struct btree_keys *, struct bkey *, struct bkey *);
+void bch_bset_insert(struct btree_keys *, struct bkey *, struct bkey *);
+unsigned bch_btree_insert_key(struct btree_keys *, struct bkey *,
+			      struct bkey *);
+
+enum {
+	BTREE_INSERT_STATUS_NO_INSERT = 0,
+	BTREE_INSERT_STATUS_INSERT,
+	BTREE_INSERT_STATUS_BACK_MERGE,
+	BTREE_INSERT_STATUS_OVERWROTE,
+	BTREE_INSERT_STATUS_FRONT_MERGE,
 };
 
+/* Btree key iteration */
+
+struct btree_iter {
+	size_t size, used;
+#ifdef CONFIG_BCACHE_DEBUG
+	struct btree_keys *b;
+#endif
+	struct btree_iter_set {
+		struct bkey *k, *end;
+	} data[MAX_BSETS];
+};
+
+typedef bool (*ptr_filter_fn)(struct btree_keys *, const struct bkey *);
+
+struct bkey *bch_btree_iter_next(struct btree_iter *);
+struct bkey *bch_btree_iter_next_filter(struct btree_iter *,
+					struct btree_keys *, ptr_filter_fn);
+
+void bch_btree_iter_push(struct btree_iter *, struct bkey *, struct bkey *);
+struct bkey *bch_btree_iter_init(struct btree_keys *, struct btree_iter *,
+				 struct bkey *);
+
+struct bkey *__bch_bset_search(struct btree_keys *, struct bset_tree *,
+			       const struct bkey *);
+
+/*
+ * Returns the first key that is strictly greater than search
+ */
+static inline struct bkey *bch_bset_search(struct btree_keys *b,
+					   struct bset_tree *t,
+					   const struct bkey *search)
+{
+	return search ? __bch_bset_search(b, t, search) : t->data->start;
+}
+
+#define for_each_key_filter(b, k, iter, filter)				\
+	for (bch_btree_iter_init((b), (iter), NULL);			\
+	     ((k) = bch_btree_iter_next_filter((iter), (b), filter));)
+
+#define for_each_key(b, k, iter)					\
+	for (bch_btree_iter_init((b), (iter), NULL);			\
+	     ((k) = bch_btree_iter_next(iter));)
+
+/* Sorting */
+
+struct bset_sort_state {
+	mempool_t		*pool;
+
+	unsigned		page_order;
+	unsigned		crit_factor;
+
+	struct time_stats	time;
+};
+
+void bch_bset_sort_state_free(struct bset_sort_state *);
+int bch_bset_sort_state_init(struct bset_sort_state *, unsigned);
+void bch_btree_sort_lazy(struct btree_keys *, struct bset_sort_state *);
+void bch_btree_sort_into(struct btree_keys *, struct btree_keys *,
+			 struct bset_sort_state *);
+void bch_btree_sort_and_fix_extents(struct btree_keys *, struct btree_iter *,
+				    struct bset_sort_state *);
+void bch_btree_sort_partial(struct btree_keys *, unsigned,
+			    struct bset_sort_state *);
+
+static inline void bch_btree_sort(struct btree_keys *b,
+				  struct bset_sort_state *state)
+{
+	bch_btree_sort_partial(b, 0, state);
+}
+
+struct bset_stats {
+	size_t sets_written, sets_unwritten;
+	size_t bytes_written, bytes_unwritten;
+	size_t floats, failed;
+};
+
+void bch_btree_keys_stats(struct btree_keys *, struct bset_stats *);
+
+/* Bkey utility code */
+
+#define bset_bkey_last(i)	bkey_idx((struct bkey *) (i)->d, (i)->keys)
+
+static inline struct bkey *bset_bkey_idx(struct bset *i, unsigned idx)
+{
+	return bkey_idx(i->start, idx);
+}
+
+static inline void bkey_init(struct bkey *k)
+{
+	*k = ZERO_KEY;
+}
+
 static __always_inline int64_t bkey_cmp(const struct bkey *l,
 					const struct bkey *r)
 {
@@ -196,6 +400,62 @@ static __always_inline int64_t bkey_cmp(const struct bkey *l,
 		: (int64_t) KEY_OFFSET(l) - (int64_t) KEY_OFFSET(r);
 }
 
+void bch_bkey_copy_single_ptr(struct bkey *, const struct bkey *,
+			      unsigned);
+bool __bch_cut_front(const struct bkey *, struct bkey *);
+bool __bch_cut_back(const struct bkey *, struct bkey *);
+
+static inline bool bch_cut_front(const struct bkey *where, struct bkey *k)
+{
+	BUG_ON(bkey_cmp(where, k) > 0);
+	return __bch_cut_front(where, k);
+}
+
+static inline bool bch_cut_back(const struct bkey *where, struct bkey *k)
+{
+	BUG_ON(bkey_cmp(where, &START_KEY(k)) < 0);
+	return __bch_cut_back(where, k);
+}
+
+#define PRECEDING_KEY(_k)					\
+({								\
+	struct bkey *_ret = NULL;				\
+								\
+	if (KEY_INODE(_k) || KEY_OFFSET(_k)) {			\
+		_ret = &KEY(KEY_INODE(_k), KEY_OFFSET(_k), 0);	\
+								\
+		if (!_ret->low)					\
+			_ret->high--;				\
+		_ret->low--;					\
+	}							\
+								\
+	_ret;							\
+})
+
+static inline bool bch_ptr_invalid(struct btree_keys *b, const struct bkey *k)
+{
+	return b->ops->key_invalid(b, k);
+}
+
+static inline bool bch_ptr_bad(struct btree_keys *b, const struct bkey *k)
+{
+	return b->ops->key_bad(b, k);
+}
+
+static inline void bch_bkey_to_text(struct btree_keys *b, char *buf,
+				    size_t size, const struct bkey *k)
+{
+	return b->ops->key_to_text(buf, size, k);
+}
+
+static inline bool bch_bkey_equal_header(const struct bkey *l,
+					 const struct bkey *r)
+{
+	return (KEY_DIRTY(l) == KEY_DIRTY(r) &&
+		KEY_PTRS(l) == KEY_PTRS(r) &&
+		KEY_CSUM(l) == KEY_CSUM(l));
+}
+
 /* Keylists */
 
 struct keylist {
@@ -257,136 +517,44 @@ static inline size_t bch_keylist_bytes(struct keylist *l)
 
 struct bkey *bch_keylist_pop(struct keylist *);
 void bch_keylist_pop_front(struct keylist *);
-int bch_keylist_realloc(struct keylist *, int, struct cache_set *);
-
-void bch_bkey_copy_single_ptr(struct bkey *, const struct bkey *,
-			      unsigned);
-bool __bch_cut_front(const struct bkey *, struct bkey *);
-bool __bch_cut_back(const struct bkey *, struct bkey *);
+int __bch_keylist_realloc(struct keylist *, unsigned);
 
-static inline bool bch_cut_front(const struct bkey *where, struct bkey *k)
-{
-	BUG_ON(bkey_cmp(where, k) > 0);
-	return __bch_cut_front(where, k);
-}
+/* Debug stuff */
 
-static inline bool bch_cut_back(const struct bkey *where, struct bkey *k)
-{
-	BUG_ON(bkey_cmp(where, &START_KEY(k)) < 0);
-	return __bch_cut_back(where, k);
-}
-
-const char *bch_ptr_status(struct cache_set *, const struct bkey *);
-bool bch_btree_ptr_invalid(struct cache_set *, const struct bkey *);
-bool bch_extent_ptr_invalid(struct cache_set *, const struct bkey *);
-
-bool bch_ptr_bad(struct btree *, const struct bkey *);
-
-static inline uint8_t gen_after(uint8_t a, uint8_t b)
-{
-	uint8_t r = a - b;
-	return r > 128U ? 0 : r;
-}
-
-static inline uint8_t ptr_stale(struct cache_set *c, const struct bkey *k,
-				unsigned i)
-{
-	return gen_after(PTR_BUCKET(c, k, i)->gen, PTR_GEN(k, i));
-}
-
-static inline bool ptr_available(struct cache_set *c, const struct bkey *k,
-				 unsigned i)
-{
-	return (PTR_DEV(k, i) < MAX_CACHES_PER_SET) && PTR_CACHE(c, k, i);
-}
-
-
-typedef bool (*ptr_filter_fn)(struct btree *, const struct bkey *);
-
-struct bkey *bch_btree_iter_next(struct btree_iter *);
-struct bkey *bch_btree_iter_next_filter(struct btree_iter *,
-					struct btree *, ptr_filter_fn);
-
-void bch_btree_iter_push(struct btree_iter *, struct bkey *, struct bkey *);
-struct bkey *__bch_btree_iter_init(struct btree *, struct btree_iter *,
-				   struct bkey *, struct bset_tree *);
-
-/* 32 bits total: */
-#define BKEY_MID_BITS		3
-#define BKEY_EXPONENT_BITS	7
-#define BKEY_MANTISSA_BITS	22
-#define BKEY_MANTISSA_MASK	((1 << BKEY_MANTISSA_BITS) - 1)
-
-struct bkey_float {
-	unsigned	exponent:BKEY_EXPONENT_BITS;
-	unsigned	m:BKEY_MID_BITS;
-	unsigned	mantissa:BKEY_MANTISSA_BITS;
-} __packed;
-
-/*
- * BSET_CACHELINE was originally intended to match the hardware cacheline size -
- * it used to be 64, but I realized the lookup code would touch slightly less
- * memory if it was 128.
- *
- * It definites the number of bytes (in struct bset) per struct bkey_float in
- * the auxiliar search tree - when we're done searching the bset_float tree we
- * have this many bytes left that we do a linear search over.
- *
- * Since (after level 5) every level of the bset_tree is on a new cacheline,
- * we're touching one fewer cacheline in the bset tree in exchange for one more
- * cacheline in the linear search - but the linear search might stop before it
- * gets to the second cacheline.
- */
-
-#define BSET_CACHELINE		128
-#define bset_tree_space(b)	(btree_data_space(b) / BSET_CACHELINE)
+#ifdef CONFIG_BCACHE_DEBUG
 
-#define bset_tree_bytes(b)	(bset_tree_space(b) * sizeof(struct bkey_float))
-#define bset_prev_bytes(b)	(bset_tree_space(b) * sizeof(uint8_t))
+int __bch_count_data(struct btree_keys *);
+void __bch_check_keys(struct btree_keys *, const char *, ...);
+void bch_dump_bset(struct btree_keys *, struct bset *, unsigned);
+void bch_dump_bucket(struct btree_keys *);
 
-void bch_bset_init_next(struct btree *);
+#else
 
-void bch_bset_fix_invalidated_key(struct btree *, struct bkey *);
-void bch_bset_fix_lookup_table(struct btree *, struct bkey *);
+static inline int __bch_count_data(struct btree_keys *b) { return -1; }
+static inline void __bch_check_keys(struct btree_keys *b, const char *fmt, ...) {}
+static inline void bch_dump_bucket(struct btree_keys *b) {}
+void bch_dump_bset(struct btree_keys *, struct bset *, unsigned);
 
-struct bkey *__bch_bset_search(struct btree *, struct bset_tree *,
-			   const struct bkey *);
+#endif
 
-/*
- * Returns the first key that is strictly greater than search
- */
-static inline struct bkey *bch_bset_search(struct btree *b, struct bset_tree *t,
-					   const struct bkey *search)
+static inline bool btree_keys_expensive_checks(struct btree_keys *b)
 {
-	return search ? __bch_bset_search(b, t, search) : t->data->start;
+#ifdef CONFIG_BCACHE_DEBUG
+	return *b->expensive_debug_checks;
+#else
+	return false;
+#endif
 }
 
-#define PRECEDING_KEY(_k)					\
-({								\
-	struct bkey *_ret = NULL;				\
-								\
-	if (KEY_INODE(_k) || KEY_OFFSET(_k)) {			\
-		_ret = &KEY(KEY_INODE(_k), KEY_OFFSET(_k), 0);	\
-								\
-		if (!_ret->low)					\
-			_ret->high--;				\
-		_ret->low--;					\
-	}							\
-								\
-	_ret;							\
-})
-
-bool bch_bkey_try_merge(struct btree *, struct bkey *, struct bkey *);
-void bch_btree_sort_lazy(struct btree *);
-void bch_btree_sort_into(struct btree *, struct btree *);
-void bch_btree_sort_and_fix_extents(struct btree *, struct btree_iter *);
-void bch_btree_sort_partial(struct btree *, unsigned);
-
-static inline void bch_btree_sort(struct btree *b)
+static inline int bch_count_data(struct btree_keys *b)
 {
-	bch_btree_sort_partial(b, 0);
+	return btree_keys_expensive_checks(b) ? __bch_count_data(b) : -1;
 }
 
-int bch_bset_print_stats(struct cache_set *, char *);
+#define bch_check_keys(b, ...)						\
+do {									\
+	if (btree_keys_expensive_checks(b))				\
+		__bch_check_keys(b, __VA_ARGS__);			\
+} while (0)
 
 #endif
diff --git a/drivers/md/bcache/btree.c b/drivers/md/bcache/btree.c
index 31bb53fcc67a..98cc0a810a36 100644
--- a/drivers/md/bcache/btree.c
+++ b/drivers/md/bcache/btree.c
@@ -23,7 +23,7 @@
 #include "bcache.h"
 #include "btree.h"
 #include "debug.h"
-#include "writeback.h"
+#include "extents.h"
 
 #include <linux/slab.h>
 #include <linux/bitops.h>
@@ -89,13 +89,6 @@
  * Test module load/unload
  */
 
-enum {
-	BTREE_INSERT_STATUS_INSERT,
-	BTREE_INSERT_STATUS_BACK_MERGE,
-	BTREE_INSERT_STATUS_OVERWROTE,
-	BTREE_INSERT_STATUS_FRONT_MERGE,
-};
-
 #define MAX_NEED_GC		64
 #define MAX_SAVE_PRIO		72
 
@@ -106,14 +99,6 @@ enum {
 
 static struct workqueue_struct *btree_io_wq;
 
-static inline bool should_split(struct btree *b)
-{
-	struct bset *i = write_block(b);
-	return b->written >= btree_blocks(b) ||
-		(b->written + __set_blocks(i, i->keys + 15, b->c)
-		 > btree_blocks(b));
-}
-
 #define insert_lock(s, b)	((b)->level <= (s)->lock)
 
 /*
@@ -167,6 +152,8 @@ static inline bool should_split(struct btree *b)
 			_r = bch_btree_ ## fn(_b, op, ##__VA_ARGS__);	\
 		}							\
 		rw_unlock(_w, _b);					\
+		if (_r == -EINTR)					\
+			schedule();					\
 		bch_cannibalize_unlock(c);				\
 		if (_r == -ENOSPC) {					\
 			wait_event((c)->try_wait,			\
@@ -175,9 +162,15 @@ static inline bool should_split(struct btree *b)
 		}							\
 	} while (_r == -EINTR);						\
 									\
+	finish_wait(&(c)->bucket_wait, &(op)->wait);			\
 	_r;								\
 })
 
+static inline struct bset *write_block(struct btree *b)
+{
+	return ((void *) btree_bset_first(b)) + b->written * block_bytes(b->c);
+}
+
 /* Btree key manipulation */
 
 void bkey_put(struct cache_set *c, struct bkey *k)
@@ -194,16 +187,16 @@ void bkey_put(struct cache_set *c, struct bkey *k)
 static uint64_t btree_csum_set(struct btree *b, struct bset *i)
 {
 	uint64_t crc = b->key.ptr[0];
-	void *data = (void *) i + 8, *end = end(i);
+	void *data = (void *) i + 8, *end = bset_bkey_last(i);
 
 	crc = bch_crc64_update(crc, data, end - data);
 	return crc ^ 0xffffffffffffffffULL;
 }
 
-static void bch_btree_node_read_done(struct btree *b)
+void bch_btree_node_read_done(struct btree *b)
 {
 	const char *err = "bad btree header";
-	struct bset *i = b->sets[0].data;
+	struct bset *i = btree_bset_first(b);
 	struct btree_iter *iter;
 
 	iter = mempool_alloc(b->c->fill_iter, GFP_NOWAIT);
@@ -211,21 +204,22 @@ static void bch_btree_node_read_done(struct btree *b)
 	iter->used = 0;
 
 #ifdef CONFIG_BCACHE_DEBUG
-	iter->b = b;
+	iter->b = &b->keys;
 #endif
 
 	if (!i->seq)
 		goto err;
 
 	for (;
-	     b->written < btree_blocks(b) && i->seq == b->sets[0].data->seq;
+	     b->written < btree_blocks(b) && i->seq == b->keys.set[0].data->seq;
 	     i = write_block(b)) {
 		err = "unsupported bset version";
 		if (i->version > BCACHE_BSET_VERSION)
 			goto err;
 
 		err = "bad btree header";
-		if (b->written + set_blocks(i, b->c) > btree_blocks(b))
+		if (b->written + set_blocks(i, block_bytes(b->c)) >
+		    btree_blocks(b))
 			goto err;
 
 		err = "bad magic";
@@ -245,39 +239,40 @@ static void bch_btree_node_read_done(struct btree *b)
 		}
 
 		err = "empty set";
-		if (i != b->sets[0].data && !i->keys)
+		if (i != b->keys.set[0].data && !i->keys)
 			goto err;
 
-		bch_btree_iter_push(iter, i->start, end(i));
+		bch_btree_iter_push(iter, i->start, bset_bkey_last(i));
 
-		b->written += set_blocks(i, b->c);
+		b->written += set_blocks(i, block_bytes(b->c));
 	}
 
 	err = "corrupted btree";
 	for (i = write_block(b);
-	     index(i, b) < btree_blocks(b);
+	     bset_sector_offset(&b->keys, i) < KEY_SIZE(&b->key);
 	     i = ((void *) i) + block_bytes(b->c))
-		if (i->seq == b->sets[0].data->seq)
+		if (i->seq == b->keys.set[0].data->seq)
 			goto err;
 
-	bch_btree_sort_and_fix_extents(b, iter);
+	bch_btree_sort_and_fix_extents(&b->keys, iter, &b->c->sort);
 
-	i = b->sets[0].data;
+	i = b->keys.set[0].data;
 	err = "short btree key";
-	if (b->sets[0].size &&
-	    bkey_cmp(&b->key, &b->sets[0].end) < 0)
+	if (b->keys.set[0].size &&
+	    bkey_cmp(&b->key, &b->keys.set[0].end) < 0)
 		goto err;
 
 	if (b->written < btree_blocks(b))
-		bch_bset_init_next(b);
+		bch_bset_init_next(&b->keys, write_block(b),
+				   bset_magic(&b->c->sb));
 out:
 	mempool_free(iter, b->c->fill_iter);
 	return;
 err:
 	set_btree_node_io_error(b);
-	bch_cache_set_error(b->c, "%s at bucket %zu, block %zu, %u keys",
+	bch_cache_set_error(b->c, "%s at bucket %zu, block %u, %u keys",
 			    err, PTR_BUCKET_NR(b->c, &b->key, 0),
-			    index(i, b), i->keys);
+			    bset_block_offset(b, i), i->keys);
 	goto out;
 }
 
@@ -287,7 +282,7 @@ static void btree_node_read_endio(struct bio *bio, int error)
 	closure_put(cl);
 }
 
-void bch_btree_node_read(struct btree *b)
+static void bch_btree_node_read(struct btree *b)
 {
 	uint64_t start_time = local_clock();
 	struct closure cl;
@@ -299,11 +294,11 @@ void bch_btree_node_read(struct btree *b)
 
 	bio = bch_bbio_alloc(b->c);
 	bio->bi_rw	= REQ_META|READ_SYNC;
-	bio->bi_size	= KEY_SIZE(&b->key) << 9;
+	bio->bi_iter.bi_size = KEY_SIZE(&b->key) << 9;
 	bio->bi_end_io	= btree_node_read_endio;
 	bio->bi_private	= &cl;
 
-	bch_bio_map(bio, b->sets[0].data);
+	bch_bio_map(bio, b->keys.set[0].data);
 
 	bch_submit_bbio(bio, b->c, &b->key, 0);
 	closure_sync(&cl);
@@ -340,9 +335,16 @@ static void btree_complete_write(struct btree *b, struct btree_write *w)
 	w->journal	= NULL;
 }
 
+static void btree_node_write_unlock(struct closure *cl)
+{
+	struct btree *b = container_of(cl, struct btree, io);
+
+	up(&b->io_mutex);
+}
+
 static void __btree_node_write_done(struct closure *cl)
 {
-	struct btree *b = container_of(cl, struct btree, io.cl);
+	struct btree *b = container_of(cl, struct btree, io);
 	struct btree_write *w = btree_prev_write(b);
 
 	bch_bbio_free(b->bio, b->c);
@@ -353,16 +355,16 @@ static void __btree_node_write_done(struct closure *cl)
 		queue_delayed_work(btree_io_wq, &b->work,
 				   msecs_to_jiffies(30000));
 
-	closure_return(cl);
+	closure_return_with_destructor(cl, btree_node_write_unlock);
 }
 
 static void btree_node_write_done(struct closure *cl)
 {
-	struct btree *b = container_of(cl, struct btree, io.cl);
+	struct btree *b = container_of(cl, struct btree, io);
 	struct bio_vec *bv;
 	int n;
 
-	__bio_for_each_segment(bv, b->bio, n, 0)
+	bio_for_each_segment_all(bv, b->bio, n)
 		__free_page(bv->bv_page);
 
 	__btree_node_write_done(cl);
@@ -371,7 +373,7 @@ static void btree_node_write_done(struct closure *cl)
 static void btree_node_write_endio(struct bio *bio, int error)
 {
 	struct closure *cl = bio->bi_private;
-	struct btree *b = container_of(cl, struct btree, io.cl);
+	struct btree *b = container_of(cl, struct btree, io);
 
 	if (error)
 		set_btree_node_io_error(b);
@@ -382,8 +384,8 @@ static void btree_node_write_endio(struct bio *bio, int error)
 
 static void do_btree_node_write(struct btree *b)
 {
-	struct closure *cl = &b->io.cl;
-	struct bset *i = b->sets[b->nsets].data;
+	struct closure *cl = &b->io;
+	struct bset *i = btree_bset_last(b);
 	BKEY_PADDED(key) k;
 
 	i->version	= BCACHE_BSET_VERSION;
@@ -395,7 +397,7 @@ static void do_btree_node_write(struct btree *b)
 	b->bio->bi_end_io	= btree_node_write_endio;
 	b->bio->bi_private	= cl;
 	b->bio->bi_rw		= REQ_META|WRITE_SYNC|REQ_FUA;
-	b->bio->bi_size		= set_blocks(i, b->c) * block_bytes(b->c);
+	b->bio->bi_iter.bi_size	= roundup(set_bytes(i), block_bytes(b->c));
 	bch_bio_map(b->bio, i);
 
 	/*
@@ -414,14 +416,15 @@ static void do_btree_node_write(struct btree *b)
 	 */
 
 	bkey_copy(&k.key, &b->key);
-	SET_PTR_OFFSET(&k.key, 0, PTR_OFFSET(&k.key, 0) + bset_offset(b, i));
+	SET_PTR_OFFSET(&k.key, 0, PTR_OFFSET(&k.key, 0) +
+		       bset_sector_offset(&b->keys, i));
 
 	if (!bio_alloc_pages(b->bio, GFP_NOIO)) {
 		int j;
 		struct bio_vec *bv;
 		void *base = (void *) ((unsigned long) i & ~(PAGE_SIZE - 1));
 
-		bio_for_each_segment(bv, b->bio, j)
+		bio_for_each_segment_all(bv, b->bio, j)
 			memcpy(page_address(bv->bv_page),
 			       base + j * PAGE_SIZE, PAGE_SIZE);
 
@@ -435,40 +438,54 @@ static void do_btree_node_write(struct btree *b)
 		bch_submit_bbio(b->bio, b->c, &k.key, 0);
 
 		closure_sync(cl);
-		__btree_node_write_done(cl);
+		continue_at_nobarrier(cl, __btree_node_write_done, NULL);
 	}
 }
 
 void bch_btree_node_write(struct btree *b, struct closure *parent)
 {
-	struct bset *i = b->sets[b->nsets].data;
+	struct bset *i = btree_bset_last(b);
 
 	trace_bcache_btree_write(b);
 
 	BUG_ON(current->bio_list);
 	BUG_ON(b->written >= btree_blocks(b));
 	BUG_ON(b->written && !i->keys);
-	BUG_ON(b->sets->data->seq != i->seq);
-	bch_check_keys(b, "writing");
+	BUG_ON(btree_bset_first(b)->seq != i->seq);
+	bch_check_keys(&b->keys, "writing");
 
 	cancel_delayed_work(&b->work);
 
 	/* If caller isn't waiting for write, parent refcount is cache set */
-	closure_lock(&b->io, parent ?: &b->c->cl);
+	down(&b->io_mutex);
+	closure_init(&b->io, parent ?: &b->c->cl);
 
 	clear_bit(BTREE_NODE_dirty,	 &b->flags);
 	change_bit(BTREE_NODE_write_idx, &b->flags);
 
 	do_btree_node_write(b);
 
-	b->written += set_blocks(i, b->c);
-	atomic_long_add(set_blocks(i, b->c) * b->c->sb.block_size,
+	atomic_long_add(set_blocks(i, block_bytes(b->c)) * b->c->sb.block_size,
 			&PTR_CACHE(b->c, &b->key, 0)->btree_sectors_written);
 
-	bch_btree_sort_lazy(b);
+	b->written += set_blocks(i, block_bytes(b->c));
+
+	/* If not a leaf node, always sort */
+	if (b->level && b->keys.nsets)
+		bch_btree_sort(&b->keys, &b->c->sort);
+	else
+		bch_btree_sort_lazy(&b->keys, &b->c->sort);
+
+	/*
+	 * do verify if there was more than one set initially (i.e. we did a
+	 * sort) and we sorted down to a single set:
+	 */
+	if (i != b->keys.set->data && !b->keys.nsets)
+		bch_btree_verify(b);
 
 	if (b->written < btree_blocks(b))
-		bch_bset_init_next(b);
+		bch_bset_init_next(&b->keys, write_block(b),
+				   bset_magic(&b->c->sb));
 }
 
 static void bch_btree_node_write_sync(struct btree *b)
@@ -493,7 +510,7 @@ static void btree_node_write_work(struct work_struct *w)
 
 static void bch_btree_leaf_dirty(struct btree *b, atomic_t *journal_ref)
 {
-	struct bset *i = b->sets[b->nsets].data;
+	struct bset *i = btree_bset_last(b);
 	struct btree_write *w = btree_current_write(b);
 
 	BUG_ON(!b->written);
@@ -528,24 +545,6 @@ static void bch_btree_leaf_dirty(struct btree *b, atomic_t *journal_ref)
  * mca -> memory cache
  */
 
-static void mca_reinit(struct btree *b)
-{
-	unsigned i;
-
-	b->flags	= 0;
-	b->written	= 0;
-	b->nsets	= 0;
-
-	for (i = 0; i < MAX_BSETS; i++)
-		b->sets[i].size = 0;
-	/*
-	 * Second loop starts at 1 because b->sets[0]->data is the memory we
-	 * allocated
-	 */
-	for (i = 1; i < MAX_BSETS; i++)
-		b->sets[i].data = NULL;
-}
-
 #define mca_reserve(c)	(((c->root && c->root->level)		\
 			  ? c->root->level : 1) * 8 + 16)
 #define mca_can_free(c)						\
@@ -553,28 +552,12 @@ static void mca_reinit(struct btree *b)
 
 static void mca_data_free(struct btree *b)
 {
-	struct bset_tree *t = b->sets;
-	BUG_ON(!closure_is_unlocked(&b->io.cl));
+	BUG_ON(b->io_mutex.count != 1);
 
-	if (bset_prev_bytes(b) < PAGE_SIZE)
-		kfree(t->prev);
-	else
-		free_pages((unsigned long) t->prev,
-			   get_order(bset_prev_bytes(b)));
+	bch_btree_keys_free(&b->keys);
 
-	if (bset_tree_bytes(b) < PAGE_SIZE)
-		kfree(t->tree);
-	else
-		free_pages((unsigned long) t->tree,
-			   get_order(bset_tree_bytes(b)));
-
-	free_pages((unsigned long) t->data, b->page_order);
-
-	t->prev = NULL;
-	t->tree = NULL;
-	t->data = NULL;
-	list_move(&b->list, &b->c->btree_cache_freed);
 	b->c->bucket_cache_used--;
+	list_move(&b->list, &b->c->btree_cache_freed);
 }
 
 static void mca_bucket_free(struct btree *b)
@@ -593,34 +576,16 @@ static unsigned btree_order(struct bkey *k)
 
 static void mca_data_alloc(struct btree *b, struct bkey *k, gfp_t gfp)
 {
-	struct bset_tree *t = b->sets;
-	BUG_ON(t->data);
-
-	b->page_order = max_t(unsigned,
-			      ilog2(b->c->btree_pages),
-			      btree_order(k));
-
-	t->data = (void *) __get_free_pages(gfp, b->page_order);
-	if (!t->data)
-		goto err;
-
-	t->tree = bset_tree_bytes(b) < PAGE_SIZE
-		? kmalloc(bset_tree_bytes(b), gfp)
-		: (void *) __get_free_pages(gfp, get_order(bset_tree_bytes(b)));
-	if (!t->tree)
-		goto err;
-
-	t->prev = bset_prev_bytes(b) < PAGE_SIZE
-		? kmalloc(bset_prev_bytes(b), gfp)
-		: (void *) __get_free_pages(gfp, get_order(bset_prev_bytes(b)));
-	if (!t->prev)
-		goto err;
-
-	list_move(&b->list, &b->c->btree_cache);
-	b->c->bucket_cache_used++;
-	return;
-err:
-	mca_data_free(b);
+	if (!bch_btree_keys_alloc(&b->keys,
+				  max_t(unsigned,
+					ilog2(b->c->btree_pages),
+					btree_order(k)),
+				  gfp)) {
+		b->c->bucket_cache_used++;
+		list_move(&b->list, &b->c->btree_cache);
+	} else {
+		list_move(&b->list, &b->c->btree_cache_freed);
+	}
 }
 
 static struct btree *mca_bucket_alloc(struct cache_set *c,
@@ -635,7 +600,7 @@ static struct btree *mca_bucket_alloc(struct cache_set *c,
 	INIT_LIST_HEAD(&b->list);
 	INIT_DELAYED_WORK(&b->work, btree_node_write_work);
 	b->c = c;
-	closure_init_unlocked(&b->io);
+	sema_init(&b->io_mutex, 1);
 
 	mca_data_alloc(b, k, gfp);
 	return b;
@@ -651,24 +616,31 @@ static int mca_reap(struct btree *b, unsigned min_order, bool flush)
 	if (!down_write_trylock(&b->lock))
 		return -ENOMEM;
 
-	BUG_ON(btree_node_dirty(b) && !b->sets[0].data);
+	BUG_ON(btree_node_dirty(b) && !b->keys.set[0].data);
 
-	if (b->page_order < min_order ||
-	    (!flush &&
-	     (btree_node_dirty(b) ||
-	      atomic_read(&b->io.cl.remaining) != -1))) {
-		rw_unlock(true, b);
-		return -ENOMEM;
+	if (b->keys.page_order < min_order)
+		goto out_unlock;
+
+	if (!flush) {
+		if (btree_node_dirty(b))
+			goto out_unlock;
+
+		if (down_trylock(&b->io_mutex))
+			goto out_unlock;
+		up(&b->io_mutex);
 	}
 
 	if (btree_node_dirty(b))
 		bch_btree_node_write_sync(b);
 
 	/* wait for any in flight btree write */
-	closure_wait_event(&b->io.wait, &cl,
-			   atomic_read(&b->io.cl.remaining) == -1);
+	down(&b->io_mutex);
+	up(&b->io_mutex);
 
 	return 0;
+out_unlock:
+	rw_unlock(true, b);
+	return -ENOMEM;
 }
 
 static unsigned long bch_mca_scan(struct shrinker *shrink,
@@ -714,14 +686,10 @@ static unsigned long bch_mca_scan(struct shrinker *shrink,
 		}
 	}
 
-	/*
-	 * Can happen right when we first start up, before we've read in any
-	 * btree nodes
-	 */
-	if (list_empty(&c->btree_cache))
-		goto out;
-
 	for (i = 0; (nr--) && i < c->bucket_cache_used; i++) {
+		if (list_empty(&c->btree_cache))
+			goto out;
+
 		b = list_first_entry(&c->btree_cache, struct btree, list);
 		list_rotate_left(&c->btree_cache);
 
@@ -767,6 +735,8 @@ void bch_btree_cache_free(struct cache_set *c)
 #ifdef CONFIG_BCACHE_DEBUG
 	if (c->verify_data)
 		list_move(&c->verify_data->list, &c->btree_cache);
+
+	free_pages((unsigned long) c->verify_ondisk, ilog2(bucket_pages(c)));
 #endif
 
 	list_splice(&c->btree_cache_freeable,
@@ -807,10 +777,13 @@ int bch_btree_cache_alloc(struct cache_set *c)
 #ifdef CONFIG_BCACHE_DEBUG
 	mutex_init(&c->verify_lock);
 
+	c->verify_ondisk = (void *)
+		__get_free_pages(GFP_KERNEL, ilog2(bucket_pages(c)));
+
 	c->verify_data = mca_bucket_alloc(c, &ZERO_KEY, GFP_KERNEL);
 
 	if (c->verify_data &&
-	    c->verify_data->sets[0].data)
+	    c->verify_data->keys.set->data)
 		list_del_init(&c->verify_data->list);
 	else
 		c->verify_data = NULL;
@@ -908,7 +881,7 @@ static struct btree *mca_alloc(struct cache_set *c, struct bkey *k, int level)
 	list_for_each_entry(b, &c->btree_cache_freed, list)
 		if (!mca_reap(b, 0, false)) {
 			mca_data_alloc(b, k, __GFP_NOWARN|GFP_NOIO);
-			if (!b->sets[0].data)
+			if (!b->keys.set[0].data)
 				goto err;
 			else
 				goto out;
@@ -919,10 +892,10 @@ static struct btree *mca_alloc(struct cache_set *c, struct bkey *k, int level)
 		goto err;
 
 	BUG_ON(!down_write_trylock(&b->lock));
-	if (!b->sets->data)
+	if (!b->keys.set->data)
 		goto err;
 out:
-	BUG_ON(!closure_is_unlocked(&b->io.cl));
+	BUG_ON(b->io_mutex.count != 1);
 
 	bkey_copy(&b->key, k);
 	list_move(&b->list, &c->btree_cache);
@@ -930,10 +903,17 @@ out:
 	hlist_add_head_rcu(&b->hash, mca_hash(c, k));
 
 	lock_set_subclass(&b->lock.dep_map, level + 1, _THIS_IP_);
-	b->level	= level;
 	b->parent	= (void *) ~0UL;
+	b->flags	= 0;
+	b->written	= 0;
+	b->level	= level;
 
-	mca_reinit(b);
+	if (!b->level)
+		bch_btree_keys_init(&b->keys, &bch_extent_keys_ops,
+				    &b->c->expensive_debug_checks);
+	else
+		bch_btree_keys_init(&b->keys, &bch_btree_keys_ops,
+				    &b->c->expensive_debug_checks);
 
 	return b;
 err:
@@ -994,13 +974,13 @@ retry:
 
 	b->accessed = 1;
 
-	for (; i <= b->nsets && b->sets[i].size; i++) {
-		prefetch(b->sets[i].tree);
-		prefetch(b->sets[i].data);
+	for (; i <= b->keys.nsets && b->keys.set[i].size; i++) {
+		prefetch(b->keys.set[i].tree);
+		prefetch(b->keys.set[i].data);
 	}
 
-	for (; i <= b->nsets; i++)
-		prefetch(b->sets[i].data);
+	for (; i <= b->keys.nsets; i++)
+		prefetch(b->keys.set[i].data);
 
 	if (btree_node_io_error(b)) {
 		rw_unlock(write, b);
@@ -1063,7 +1043,7 @@ struct btree *bch_btree_node_alloc(struct cache_set *c, int level, bool wait)
 
 	mutex_lock(&c->bucket_lock);
 retry:
-	if (__bch_bucket_alloc_set(c, WATERMARK_METADATA, &k.key, 1, wait))
+	if (__bch_bucket_alloc_set(c, RESERVE_BTREE, &k.key, 1, wait))
 		goto err;
 
 	bkey_put(c, &k.key);
@@ -1080,7 +1060,7 @@ retry:
 	}
 
 	b->accessed = 1;
-	bch_bset_init_next(b);
+	bch_bset_init_next(&b->keys, b->keys.set->data, bset_magic(&b->c->sb));
 
 	mutex_unlock(&c->bucket_lock);
 
@@ -1098,8 +1078,10 @@ err:
 static struct btree *btree_node_alloc_replacement(struct btree *b, bool wait)
 {
 	struct btree *n = bch_btree_node_alloc(b->c, b->level, wait);
-	if (!IS_ERR_OR_NULL(n))
-		bch_btree_sort_into(b, n);
+	if (!IS_ERR_OR_NULL(n)) {
+		bch_btree_sort_into(&b->keys, &n->keys, &b->c->sort);
+		bkey_copy_key(&n->key, &b->key);
+	}
 
 	return n;
 }
@@ -1120,6 +1102,28 @@ static void make_btree_freeing_key(struct btree *b, struct bkey *k)
 	atomic_inc(&b->c->prio_blocked);
 }
 
+static int btree_check_reserve(struct btree *b, struct btree_op *op)
+{
+	struct cache_set *c = b->c;
+	struct cache *ca;
+	unsigned i, reserve = c->root->level * 2 + 1;
+	int ret = 0;
+
+	mutex_lock(&c->bucket_lock);
+
+	for_each_cache(ca, c, i)
+		if (fifo_used(&ca->free[RESERVE_BTREE]) < reserve) {
+			if (op)
+				prepare_to_wait(&c->bucket_wait, &op->wait,
+						TASK_UNINTERRUPTIBLE);
+			ret = -EINTR;
+			break;
+		}
+
+	mutex_unlock(&c->bucket_lock);
+	return ret;
+}
+
 /* Garbage collection */
 
 uint8_t __bch_btree_mark_key(struct cache_set *c, int level, struct bkey *k)
@@ -1183,11 +1187,11 @@ static bool btree_gc_mark_node(struct btree *b, struct gc_stat *gc)
 
 	gc->nodes++;
 
-	for_each_key_filter(b, k, &iter, bch_ptr_invalid) {
+	for_each_key_filter(&b->keys, k, &iter, bch_ptr_invalid) {
 		stale = max(stale, btree_mark_key(b, k));
 		keys++;
 
-		if (bch_ptr_bad(b, k))
+		if (bch_ptr_bad(&b->keys, k))
 			continue;
 
 		gc->key_bytes += bkey_u64s(k);
@@ -1197,9 +1201,9 @@ static bool btree_gc_mark_node(struct btree *b, struct gc_stat *gc)
 		gc->data += KEY_SIZE(k);
 	}
 
-	for (t = b->sets; t <= &b->sets[b->nsets]; t++)
+	for (t = b->keys.set; t <= &b->keys.set[b->keys.nsets]; t++)
 		btree_bug_on(t->size &&
-			     bset_written(b, t) &&
+			     bset_written(&b->keys, t) &&
 			     bkey_cmp(&b->key, &t->end) < 0,
 			     b, "found short btree key in gc");
 
@@ -1243,7 +1247,8 @@ static int btree_gc_coalesce(struct btree *b, struct btree_op *op,
 	blocks = btree_default_blocks(b->c) * 2 / 3;
 
 	if (nodes < 2 ||
-	    __set_blocks(b->sets[0].data, keys, b->c) > blocks * (nodes - 1))
+	    __set_blocks(b->keys.set[0].data, keys,
+			 block_bytes(b->c)) > blocks * (nodes - 1))
 		return 0;
 
 	for (i = 0; i < nodes; i++) {
@@ -1253,18 +1258,19 @@ static int btree_gc_coalesce(struct btree *b, struct btree_op *op,
 	}
 
 	for (i = nodes - 1; i > 0; --i) {
-		struct bset *n1 = new_nodes[i]->sets->data;
-		struct bset *n2 = new_nodes[i - 1]->sets->data;
+		struct bset *n1 = btree_bset_first(new_nodes[i]);
+		struct bset *n2 = btree_bset_first(new_nodes[i - 1]);
 		struct bkey *k, *last = NULL;
 
 		keys = 0;
 
 		if (i > 1) {
 			for (k = n2->start;
-			     k < end(n2);
+			     k < bset_bkey_last(n2);
 			     k = bkey_next(k)) {
 				if (__set_blocks(n1, n1->keys + keys +
-						 bkey_u64s(k), b->c) > blocks)
+						 bkey_u64s(k),
+						 block_bytes(b->c)) > blocks)
 					break;
 
 				last = k;
@@ -1280,7 +1286,8 @@ static int btree_gc_coalesce(struct btree *b, struct btree_op *op,
 			 * though)
 			 */
 			if (__set_blocks(n1, n1->keys + n2->keys,
-					 b->c) > btree_blocks(new_nodes[i]))
+					 block_bytes(b->c)) >
+			    btree_blocks(new_nodes[i]))
 				goto out_nocoalesce;
 
 			keys = n2->keys;
@@ -1288,27 +1295,28 @@ static int btree_gc_coalesce(struct btree *b, struct btree_op *op,
 			last = &r->b->key;
 		}
 
-		BUG_ON(__set_blocks(n1, n1->keys + keys,
-				    b->c) > btree_blocks(new_nodes[i]));
+		BUG_ON(__set_blocks(n1, n1->keys + keys, block_bytes(b->c)) >
+		       btree_blocks(new_nodes[i]));
 
 		if (last)
 			bkey_copy_key(&new_nodes[i]->key, last);
 
-		memcpy(end(n1),
+		memcpy(bset_bkey_last(n1),
 		       n2->start,
-		       (void *) node(n2, keys) - (void *) n2->start);
+		       (void *) bset_bkey_idx(n2, keys) - (void *) n2->start);
 
 		n1->keys += keys;
 		r[i].keys = n1->keys;
 
 		memmove(n2->start,
-			node(n2, keys),
-			(void *) end(n2) - (void *) node(n2, keys));
+			bset_bkey_idx(n2, keys),
+			(void *) bset_bkey_last(n2) -
+			(void *) bset_bkey_idx(n2, keys));
 
 		n2->keys -= keys;
 
-		if (bch_keylist_realloc(keylist,
-					KEY_PTRS(&new_nodes[i]->key), b->c))
+		if (__bch_keylist_realloc(keylist,
+					  bkey_u64s(&new_nodes[i]->key)))
 			goto out_nocoalesce;
 
 		bch_btree_node_write(new_nodes[i], &cl);
@@ -1316,7 +1324,7 @@ static int btree_gc_coalesce(struct btree *b, struct btree_op *op,
 	}
 
 	for (i = 0; i < nodes; i++) {
-		if (bch_keylist_realloc(keylist, KEY_PTRS(&r[i].b->key), b->c))
+		if (__bch_keylist_realloc(keylist, bkey_u64s(&r[i].b->key)))
 			goto out_nocoalesce;
 
 		make_btree_freeing_key(r[i].b, keylist->top);
@@ -1324,7 +1332,7 @@ static int btree_gc_coalesce(struct btree *b, struct btree_op *op,
 	}
 
 	/* We emptied out this node */
-	BUG_ON(new_nodes[0]->sets->data->keys);
+	BUG_ON(btree_bset_first(new_nodes[0])->keys);
 	btree_node_free(new_nodes[0]);
 	rw_unlock(true, new_nodes[0]);
 
@@ -1370,7 +1378,7 @@ static unsigned btree_gc_count_keys(struct btree *b)
 	struct btree_iter iter;
 	unsigned ret = 0;
 
-	for_each_key_filter(b, k, &iter, bch_ptr_bad)
+	for_each_key_filter(&b->keys, k, &iter, bch_ptr_bad)
 		ret += bkey_u64s(k);
 
 	return ret;
@@ -1390,13 +1398,13 @@ static int btree_gc_recurse(struct btree *b, struct btree_op *op,
 	struct gc_merge_info *last = r + GC_MERGE_NODES - 1;
 
 	bch_keylist_init(&keys);
-	bch_btree_iter_init(b, &iter, &b->c->gc_done);
+	bch_btree_iter_init(&b->keys, &iter, &b->c->gc_done);
 
 	for (i = 0; i < GC_MERGE_NODES; i++)
 		r[i].b = ERR_PTR(-EINTR);
 
 	while (1) {
-		k = bch_btree_iter_next_filter(&iter, b, bch_ptr_bad);
+		k = bch_btree_iter_next_filter(&iter, &b->keys, bch_ptr_bad);
 		if (k) {
 			r->b = bch_btree_node_get(b->c, k, b->level - 1, true);
 			if (IS_ERR(r->b)) {
@@ -1416,7 +1424,8 @@ static int btree_gc_recurse(struct btree *b, struct btree_op *op,
 
 		if (!IS_ERR(last->b)) {
 			should_rewrite = btree_gc_mark_node(last->b, gc);
-			if (should_rewrite) {
+			if (should_rewrite &&
+			    !btree_check_reserve(b, NULL)) {
 				n = btree_node_alloc_replacement(last->b,
 								 false);
 
@@ -1705,7 +1714,7 @@ static int bch_btree_check_recurse(struct btree *b, struct btree_op *op,
 	struct bucket *g;
 	struct btree_iter iter;
 
-	for_each_key_filter(b, k, &iter, bch_ptr_invalid) {
+	for_each_key_filter(&b->keys, k, &iter, bch_ptr_invalid) {
 		for (i = 0; i < KEY_PTRS(k); i++) {
 			if (!ptr_available(b->c, k, i))
 				continue;
@@ -1728,10 +1737,11 @@ static int bch_btree_check_recurse(struct btree *b, struct btree_op *op,
 	}
 
 	if (b->level) {
-		bch_btree_iter_init(b, &iter, NULL);
+		bch_btree_iter_init(&b->keys, &iter, NULL);
 
 		do {
-			k = bch_btree_iter_next_filter(&iter, b, bch_ptr_bad);
+			k = bch_btree_iter_next_filter(&iter, &b->keys,
+						       bch_ptr_bad);
 			if (k)
 				btree_node_prefetch(b->c, k, b->level - 1);
 
@@ -1774,235 +1784,36 @@ err:
 
 /* Btree insertion */
 
-static void shift_keys(struct btree *b, struct bkey *where, struct bkey *insert)
-{
-	struct bset *i = b->sets[b->nsets].data;
-
-	memmove((uint64_t *) where + bkey_u64s(insert),
-		where,
-		(void *) end(i) - (void *) where);
-
-	i->keys += bkey_u64s(insert);
-	bkey_copy(where, insert);
-	bch_bset_fix_lookup_table(b, where);
-}
-
-static bool fix_overlapping_extents(struct btree *b, struct bkey *insert,
-				    struct btree_iter *iter,
-				    struct bkey *replace_key)
+static bool btree_insert_key(struct btree *b, struct bkey *k,
+			     struct bkey *replace_key)
 {
-	void subtract_dirty(struct bkey *k, uint64_t offset, int sectors)
-	{
-		if (KEY_DIRTY(k))
-			bcache_dev_sectors_dirty_add(b->c, KEY_INODE(k),
-						     offset, -sectors);
-	}
-
-	uint64_t old_offset;
-	unsigned old_size, sectors_found = 0;
-
-	while (1) {
-		struct bkey *k = bch_btree_iter_next(iter);
-		if (!k ||
-		    bkey_cmp(&START_KEY(k), insert) >= 0)
-			break;
-
-		if (bkey_cmp(k, &START_KEY(insert)) <= 0)
-			continue;
-
-		old_offset = KEY_START(k);
-		old_size = KEY_SIZE(k);
-
-		/*
-		 * We might overlap with 0 size extents; we can't skip these
-		 * because if they're in the set we're inserting to we have to
-		 * adjust them so they don't overlap with the key we're
-		 * inserting. But we don't want to check them for replace
-		 * operations.
-		 */
-
-		if (replace_key && KEY_SIZE(k)) {
-			/*
-			 * k might have been split since we inserted/found the
-			 * key we're replacing
-			 */
-			unsigned i;
-			uint64_t offset = KEY_START(k) -
-				KEY_START(replace_key);
-
-			/* But it must be a subset of the replace key */
-			if (KEY_START(k) < KEY_START(replace_key) ||
-			    KEY_OFFSET(k) > KEY_OFFSET(replace_key))
-				goto check_failed;
-
-			/* We didn't find a key that we were supposed to */
-			if (KEY_START(k) > KEY_START(insert) + sectors_found)
-				goto check_failed;
-
-			if (KEY_PTRS(k) != KEY_PTRS(replace_key) ||
-			    KEY_DIRTY(k) != KEY_DIRTY(replace_key))
-				goto check_failed;
-
-			/* skip past gen */
-			offset <<= 8;
-
-			BUG_ON(!KEY_PTRS(replace_key));
+	unsigned status;
 
-			for (i = 0; i < KEY_PTRS(replace_key); i++)
-				if (k->ptr[i] != replace_key->ptr[i] + offset)
-					goto check_failed;
-
-			sectors_found = KEY_OFFSET(k) - KEY_START(insert);
-		}
-
-		if (bkey_cmp(insert, k) < 0 &&
-		    bkey_cmp(&START_KEY(insert), &START_KEY(k)) > 0) {
-			/*
-			 * We overlapped in the middle of an existing key: that
-			 * means we have to split the old key. But we have to do
-			 * slightly different things depending on whether the
-			 * old key has been written out yet.
-			 */
-
-			struct bkey *top;
-
-			subtract_dirty(k, KEY_START(insert), KEY_SIZE(insert));
-
-			if (bkey_written(b, k)) {
-				/*
-				 * We insert a new key to cover the top of the
-				 * old key, and the old key is modified in place
-				 * to represent the bottom split.
-				 *
-				 * It's completely arbitrary whether the new key
-				 * is the top or the bottom, but it has to match
-				 * up with what btree_sort_fixup() does - it
-				 * doesn't check for this kind of overlap, it
-				 * depends on us inserting a new key for the top
-				 * here.
-				 */
-				top = bch_bset_search(b, &b->sets[b->nsets],
-						      insert);
-				shift_keys(b, top, k);
-			} else {
-				BKEY_PADDED(key) temp;
-				bkey_copy(&temp.key, k);
-				shift_keys(b, k, &temp.key);
-				top = bkey_next(k);
-			}
-
-			bch_cut_front(insert, top);
-			bch_cut_back(&START_KEY(insert), k);
-			bch_bset_fix_invalidated_key(b, k);
-			return false;
-		}
-
-		if (bkey_cmp(insert, k) < 0) {
-			bch_cut_front(insert, k);
-		} else {
-			if (bkey_cmp(&START_KEY(insert), &START_KEY(k)) > 0)
-				old_offset = KEY_START(insert);
-
-			if (bkey_written(b, k) &&
-			    bkey_cmp(&START_KEY(insert), &START_KEY(k)) <= 0) {
-				/*
-				 * Completely overwrote, so we don't have to
-				 * invalidate the binary search tree
-				 */
-				bch_cut_front(k, k);
-			} else {
-				__bch_cut_back(&START_KEY(insert), k);
-				bch_bset_fix_invalidated_key(b, k);
-			}
-		}
-
-		subtract_dirty(k, old_offset, old_size - KEY_SIZE(k));
-	}
+	BUG_ON(bkey_cmp(k, &b->key) > 0);
 
-check_failed:
-	if (replace_key) {
-		if (!sectors_found) {
-			return true;
-		} else if (sectors_found < KEY_SIZE(insert)) {
-			SET_KEY_OFFSET(insert, KEY_OFFSET(insert) -
-				       (KEY_SIZE(insert) - sectors_found));
-			SET_KEY_SIZE(insert, sectors_found);
-		}
-	}
+	status = bch_btree_insert_key(&b->keys, k, replace_key);
+	if (status != BTREE_INSERT_STATUS_NO_INSERT) {
+		bch_check_keys(&b->keys, "%u for %s", status,
+			       replace_key ? "replace" : "insert");
 
-	return false;
+		trace_bcache_btree_insert_key(b, k, replace_key != NULL,
+					      status);
+		return true;
+	} else
+		return false;
 }
 
-static bool btree_insert_key(struct btree *b, struct btree_op *op,
-			     struct bkey *k, struct bkey *replace_key)
+static size_t insert_u64s_remaining(struct btree *b)
 {
-	struct bset *i = b->sets[b->nsets].data;
-	struct bkey *m, *prev;
-	unsigned status = BTREE_INSERT_STATUS_INSERT;
-
-	BUG_ON(bkey_cmp(k, &b->key) > 0);
-	BUG_ON(b->level && !KEY_PTRS(k));
-	BUG_ON(!b->level && !KEY_OFFSET(k));
-
-	if (!b->level) {
-		struct btree_iter iter;
-
-		/*
-		 * bset_search() returns the first key that is strictly greater
-		 * than the search key - but for back merging, we want to find
-		 * the previous key.
-		 */
-		prev = NULL;
-		m = bch_btree_iter_init(b, &iter, PRECEDING_KEY(&START_KEY(k)));
+	ssize_t ret = bch_btree_keys_u64s_remaining(&b->keys);
 
-		if (fix_overlapping_extents(b, k, &iter, replace_key)) {
-			op->insert_collision = true;
-			return false;
-		}
-
-		if (KEY_DIRTY(k))
-			bcache_dev_sectors_dirty_add(b->c, KEY_INODE(k),
-						     KEY_START(k), KEY_SIZE(k));
-
-		while (m != end(i) &&
-		       bkey_cmp(k, &START_KEY(m)) > 0)
-			prev = m, m = bkey_next(m);
-
-		if (key_merging_disabled(b->c))
-			goto insert;
-
-		/* prev is in the tree, if we merge we're done */
-		status = BTREE_INSERT_STATUS_BACK_MERGE;
-		if (prev &&
-		    bch_bkey_try_merge(b, prev, k))
-			goto merged;
-
-		status = BTREE_INSERT_STATUS_OVERWROTE;
-		if (m != end(i) &&
-		    KEY_PTRS(m) == KEY_PTRS(k) && !KEY_SIZE(m))
-			goto copy;
-
-		status = BTREE_INSERT_STATUS_FRONT_MERGE;
-		if (m != end(i) &&
-		    bch_bkey_try_merge(b, k, m))
-			goto copy;
-	} else {
-		BUG_ON(replace_key);
-		m = bch_bset_search(b, &b->sets[b->nsets], k);
-	}
-
-insert:	shift_keys(b, m, k);
-copy:	bkey_copy(m, k);
-merged:
-	bch_check_keys(b, "%u for %s", status,
-		       replace_key ? "replace" : "insert");
-
-	if (b->level && !KEY_OFFSET(k))
-		btree_current_write(b)->prio_blocked++;
-
-	trace_bcache_btree_insert_key(b, k, replace_key != NULL, status);
+	/*
+	 * Might land in the middle of an existing extent and have to split it
+	 */
+	if (b->keys.ops->is_extents)
+		ret -= KEY_MAX_U64S;
 
-	return true;
+	return max(ret, 0L);
 }
 
 static bool bch_btree_insert_keys(struct btree *b, struct btree_op *op,
@@ -2010,21 +1821,19 @@ static bool bch_btree_insert_keys(struct btree *b, struct btree_op *op,
 				  struct bkey *replace_key)
 {
 	bool ret = false;
-	int oldsize = bch_count_data(b);
+	int oldsize = bch_count_data(&b->keys);
 
 	while (!bch_keylist_empty(insert_keys)) {
-		struct bset *i = write_block(b);
 		struct bkey *k = insert_keys->keys;
 
-		if (b->written + __set_blocks(i, i->keys + bkey_u64s(k), b->c)
-		    > btree_blocks(b))
+		if (bkey_u64s(k) > insert_u64s_remaining(b))
 			break;
 
 		if (bkey_cmp(k, &b->key) <= 0) {
 			if (!b->level)
 				bkey_put(b->c, k);
 
-			ret |= btree_insert_key(b, op, k, replace_key);
+			ret |= btree_insert_key(b, k, replace_key);
 			bch_keylist_pop_front(insert_keys);
 		} else if (bkey_cmp(&START_KEY(k), &b->key) < 0) {
 			BKEY_PADDED(key) temp;
@@ -2033,16 +1842,19 @@ static bool bch_btree_insert_keys(struct btree *b, struct btree_op *op,
 			bch_cut_back(&b->key, &temp.key);
 			bch_cut_front(&b->key, insert_keys->keys);
 
-			ret |= btree_insert_key(b, op, &temp.key, replace_key);
+			ret |= btree_insert_key(b, &temp.key, replace_key);
 			break;
 		} else {
 			break;
 		}
 	}
 
+	if (!ret)
+		op->insert_collision = true;
+
 	BUG_ON(!bch_keylist_empty(insert_keys) && b->level);
 
-	BUG_ON(bch_count_data(b) < oldsize);
+	BUG_ON(bch_count_data(&b->keys) < oldsize);
 	return ret;
 }
 
@@ -2059,16 +1871,21 @@ static int btree_split(struct btree *b, struct btree_op *op,
 	closure_init_stack(&cl);
 	bch_keylist_init(&parent_keys);
 
+	if (!b->level &&
+	    btree_check_reserve(b, op))
+		return -EINTR;
+
 	n1 = btree_node_alloc_replacement(b, true);
 	if (IS_ERR(n1))
 		goto err;
 
-	split = set_blocks(n1->sets[0].data, n1->c) > (btree_blocks(b) * 4) / 5;
+	split = set_blocks(btree_bset_first(n1),
+			   block_bytes(n1->c)) > (btree_blocks(b) * 4) / 5;
 
 	if (split) {
 		unsigned keys = 0;
 
-		trace_bcache_btree_node_split(b, n1->sets[0].data->keys);
+		trace_bcache_btree_node_split(b, btree_bset_first(n1)->keys);
 
 		n2 = bch_btree_node_alloc(b->c, b->level, true);
 		if (IS_ERR(n2))
@@ -2087,18 +1904,20 @@ static int btree_split(struct btree *b, struct btree_op *op,
 		 * search tree yet
 		 */
 
-		while (keys < (n1->sets[0].data->keys * 3) / 5)
-			keys += bkey_u64s(node(n1->sets[0].data, keys));
+		while (keys < (btree_bset_first(n1)->keys * 3) / 5)
+			keys += bkey_u64s(bset_bkey_idx(btree_bset_first(n1),
+							keys));
 
-		bkey_copy_key(&n1->key, node(n1->sets[0].data, keys));
-		keys += bkey_u64s(node(n1->sets[0].data, keys));
+		bkey_copy_key(&n1->key,
+			      bset_bkey_idx(btree_bset_first(n1), keys));
+		keys += bkey_u64s(bset_bkey_idx(btree_bset_first(n1), keys));
 
-		n2->sets[0].data->keys = n1->sets[0].data->keys - keys;
-		n1->sets[0].data->keys = keys;
+		btree_bset_first(n2)->keys = btree_bset_first(n1)->keys - keys;
+		btree_bset_first(n1)->keys = keys;
 
-		memcpy(n2->sets[0].data->start,
-		       end(n1->sets[0].data),
-		       n2->sets[0].data->keys * sizeof(uint64_t));
+		memcpy(btree_bset_first(n2)->start,
+		       bset_bkey_last(btree_bset_first(n1)),
+		       btree_bset_first(n2)->keys * sizeof(uint64_t));
 
 		bkey_copy_key(&n2->key, &b->key);
 
@@ -2106,7 +1925,7 @@ static int btree_split(struct btree *b, struct btree_op *op,
 		bch_btree_node_write(n2, &cl);
 		rw_unlock(true, n2);
 	} else {
-		trace_bcache_btree_node_compact(b, n1->sets[0].data->keys);
+		trace_bcache_btree_node_compact(b, btree_bset_first(n1)->keys);
 
 		bch_btree_insert_keys(n1, op, insert_keys, replace_key);
 	}
@@ -2149,18 +1968,21 @@ static int btree_split(struct btree *b, struct btree_op *op,
 
 	return 0;
 err_free2:
+	bkey_put(b->c, &n2->key);
 	btree_node_free(n2);
 	rw_unlock(true, n2);
 err_free1:
+	bkey_put(b->c, &n1->key);
 	btree_node_free(n1);
 	rw_unlock(true, n1);
 err:
+	WARN(1, "bcache: btree split failed");
+
 	if (n3 == ERR_PTR(-EAGAIN) ||
 	    n2 == ERR_PTR(-EAGAIN) ||
 	    n1 == ERR_PTR(-EAGAIN))
 		return -EAGAIN;
 
-	pr_warn("couldn't split");
 	return -ENOMEM;
 }
 
@@ -2171,7 +1993,7 @@ static int bch_btree_insert_node(struct btree *b, struct btree_op *op,
 {
 	BUG_ON(b->level && replace_key);
 
-	if (should_split(b)) {
+	if (bch_keylist_nkeys(insert_keys) > insert_u64s_remaining(b)) {
 		if (current->bio_list) {
 			op->lock = b->c->root->level + 1;
 			return -EAGAIN;
@@ -2180,11 +2002,13 @@ static int bch_btree_insert_node(struct btree *b, struct btree_op *op,
 			return -EINTR;
 		} else {
 			/* Invalidated all iterators */
-			return btree_split(b, op, insert_keys, replace_key) ?:
-				-EINTR;
+			int ret = btree_split(b, op, insert_keys, replace_key);
+
+			return bch_keylist_empty(insert_keys) ?
+				0 : ret ?: -EINTR;
 		}
 	} else {
-		BUG_ON(write_block(b) != b->sets[b->nsets].data);
+		BUG_ON(write_block(b) != btree_bset_last(b));
 
 		if (bch_btree_insert_keys(b, op, insert_keys, replace_key)) {
 			if (!b->level)
@@ -2323,9 +2147,9 @@ static int bch_btree_map_nodes_recurse(struct btree *b, struct btree_op *op,
 		struct bkey *k;
 		struct btree_iter iter;
 
-		bch_btree_iter_init(b, &iter, from);
+		bch_btree_iter_init(&b->keys, &iter, from);
 
-		while ((k = bch_btree_iter_next_filter(&iter, b,
+		while ((k = bch_btree_iter_next_filter(&iter, &b->keys,
 						       bch_ptr_bad))) {
 			ret = btree(map_nodes_recurse, k, b,
 				    op, from, fn, flags);
@@ -2356,9 +2180,9 @@ static int bch_btree_map_keys_recurse(struct btree *b, struct btree_op *op,
 	struct bkey *k;
 	struct btree_iter iter;
 
-	bch_btree_iter_init(b, &iter, from);
+	bch_btree_iter_init(&b->keys, &iter, from);
 
-	while ((k = bch_btree_iter_next_filter(&iter, b, bch_ptr_bad))) {
+	while ((k = bch_btree_iter_next_filter(&iter, &b->keys, bch_ptr_bad))) {
 		ret = !b->level
 			? fn(op, b, k)
 			: btree(map_keys_recurse, k, b, op, from, fn, flags);
diff --git a/drivers/md/bcache/btree.h b/drivers/md/bcache/btree.h
index 767e75570896..af065e97e55c 100644
--- a/drivers/md/bcache/btree.h
+++ b/drivers/md/bcache/btree.h
@@ -130,20 +130,12 @@ struct btree {
 	unsigned long		flags;
 	uint16_t		written;	/* would be nice to kill */
 	uint8_t			level;
-	uint8_t			nsets;
-	uint8_t			page_order;
-
-	/*
-	 * Set of sorted keys - the real btree node - plus a binary search tree
-	 *
-	 * sets[0] is special; set[0]->tree, set[0]->prev and set[0]->data point
-	 * to the memory we have allocated for this btree node. Additionally,
-	 * set[0]->data points to the entire btree node as it exists on disk.
-	 */
-	struct bset_tree	sets[MAX_BSETS];
+
+	struct btree_keys	keys;
 
 	/* For outstanding btree writes, used as a lock - protects write_idx */
-	struct closure_with_waitlist	io;
+	struct closure		io;
+	struct semaphore	io_mutex;
 
 	struct list_head	list;
 	struct delayed_work	work;
@@ -179,24 +171,19 @@ static inline struct btree_write *btree_prev_write(struct btree *b)
 	return b->writes + (btree_node_write_idx(b) ^ 1);
 }
 
-static inline unsigned bset_offset(struct btree *b, struct bset *i)
+static inline struct bset *btree_bset_first(struct btree *b)
 {
-	return (((size_t) i) - ((size_t) b->sets->data)) >> 9;
+	return b->keys.set->data;
 }
 
-static inline struct bset *write_block(struct btree *b)
+static inline struct bset *btree_bset_last(struct btree *b)
 {
-	return ((void *) b->sets[0].data) + b->written * block_bytes(b->c);
+	return bset_tree_last(&b->keys)->data;
 }
 
-static inline bool bset_written(struct btree *b, struct bset_tree *t)
+static inline unsigned bset_block_offset(struct btree *b, struct bset *i)
 {
-	return t->data < write_block(b);
-}
-
-static inline bool bkey_written(struct btree *b, struct bkey *k)
-{
-	return k < write_block(b)->start;
+	return bset_sector_offset(&b->keys, i) >> b->c->block_bits;
 }
 
 static inline void set_gc_sectors(struct cache_set *c)
@@ -204,21 +191,6 @@ static inline void set_gc_sectors(struct cache_set *c)
 	atomic_set(&c->sectors_to_gc, c->sb.bucket_size * c->nbuckets / 16);
 }
 
-static inline struct bkey *bch_btree_iter_init(struct btree *b,
-					       struct btree_iter *iter,
-					       struct bkey *search)
-{
-	return __bch_btree_iter_init(b, iter, search, b->sets);
-}
-
-static inline bool bch_ptr_invalid(struct btree *b, const struct bkey *k)
-{
-	if (b->level)
-		return bch_btree_ptr_invalid(b->c, k);
-	else
-		return bch_extent_ptr_invalid(b->c, k);
-}
-
 void bkey_put(struct cache_set *c, struct bkey *k);
 
 /* Looping macros */
@@ -229,17 +201,12 @@ void bkey_put(struct cache_set *c, struct bkey *k);
 	     iter++)							\
 		hlist_for_each_entry_rcu((b), (c)->bucket_hash + iter, hash)
 
-#define for_each_key_filter(b, k, iter, filter)				\
-	for (bch_btree_iter_init((b), (iter), NULL);			\
-	     ((k) = bch_btree_iter_next_filter((iter), b, filter));)
-
-#define for_each_key(b, k, iter)					\
-	for (bch_btree_iter_init((b), (iter), NULL);			\
-	     ((k) = bch_btree_iter_next(iter));)
-
 /* Recursing down the btree */
 
 struct btree_op {
+	/* for waiting on btree reserve in btree_split() */
+	wait_queue_t		wait;
+
 	/* Btree level at which we start taking write locks */
 	short			lock;
 
@@ -249,6 +216,7 @@ struct btree_op {
 static inline void bch_btree_op_init(struct btree_op *op, int write_lock_level)
 {
 	memset(op, 0, sizeof(struct btree_op));
+	init_wait(&op->wait);
 	op->lock = write_lock_level;
 }
 
@@ -267,7 +235,7 @@ static inline void rw_unlock(bool w, struct btree *b)
 	(w ? up_write : up_read)(&b->lock);
 }
 
-void bch_btree_node_read(struct btree *);
+void bch_btree_node_read_done(struct btree *);
 void bch_btree_node_write(struct btree *, struct closure *);
 
 void bch_btree_set_root(struct btree *);
diff --git a/drivers/md/bcache/closure.c b/drivers/md/bcache/closure.c
index dfff2410322e..7a228de95fd7 100644
--- a/drivers/md/bcache/closure.c
+++ b/drivers/md/bcache/closure.c
@@ -11,19 +11,6 @@
 
 #include "closure.h"
 
-#define CL_FIELD(type, field)					\
-	case TYPE_ ## type:					\
-	return &container_of(cl, struct type, cl)->field
-
-static struct closure_waitlist *closure_waitlist(struct closure *cl)
-{
-	switch (cl->type) {
-		CL_FIELD(closure_with_waitlist, wait);
-	default:
-		return NULL;
-	}
-}
-
 static inline void closure_put_after_sub(struct closure *cl, int flags)
 {
 	int r = flags & CLOSURE_REMAINING_MASK;
@@ -42,17 +29,10 @@ static inline void closure_put_after_sub(struct closure *cl, int flags)
 			closure_queue(cl);
 		} else {
 			struct closure *parent = cl->parent;
-			struct closure_waitlist *wait = closure_waitlist(cl);
 			closure_fn *destructor = cl->fn;
 
 			closure_debug_destroy(cl);
 
-			smp_mb();
-			atomic_set(&cl->remaining, -1);
-
-			if (wait)
-				closure_wake_up(wait);
-
 			if (destructor)
 				destructor(cl);
 
@@ -69,19 +49,18 @@ void closure_sub(struct closure *cl, int v)
 }
 EXPORT_SYMBOL(closure_sub);
 
+/**
+ * closure_put - decrement a closure's refcount
+ */
 void closure_put(struct closure *cl)
 {
 	closure_put_after_sub(cl, atomic_dec_return(&cl->remaining));
 }
 EXPORT_SYMBOL(closure_put);
 
-static void set_waiting(struct closure *cl, unsigned long f)
-{
-#ifdef CONFIG_BCACHE_CLOSURES_DEBUG
-	cl->waiting_on = f;
-#endif
-}
-
+/**
+ * closure_wake_up - wake up all closures on a wait list, without memory barrier
+ */
 void __closure_wake_up(struct closure_waitlist *wait_list)
 {
 	struct llist_node *list;
@@ -106,27 +85,34 @@ void __closure_wake_up(struct closure_waitlist *wait_list)
 		cl = container_of(reverse, struct closure, list);
 		reverse = llist_next(reverse);
 
-		set_waiting(cl, 0);
+		closure_set_waiting(cl, 0);
 		closure_sub(cl, CLOSURE_WAITING + 1);
 	}
 }
 EXPORT_SYMBOL(__closure_wake_up);
 
-bool closure_wait(struct closure_waitlist *list, struct closure *cl)
+/**
+ * closure_wait - add a closure to a waitlist
+ *
+ * @waitlist will own a ref on @cl, which will be released when
+ * closure_wake_up() is called on @waitlist.
+ *
+ */
+bool closure_wait(struct closure_waitlist *waitlist, struct closure *cl)
 {
 	if (atomic_read(&cl->remaining) & CLOSURE_WAITING)
 		return false;
 
-	set_waiting(cl, _RET_IP_);
+	closure_set_waiting(cl, _RET_IP_);
 	atomic_add(CLOSURE_WAITING + 1, &cl->remaining);
-	llist_add(&cl->list, &list->list);
+	llist_add(&cl->list, &waitlist->list);
 
 	return true;
 }
 EXPORT_SYMBOL(closure_wait);
 
 /**
- * closure_sync() - sleep until a closure a closure has nothing left to wait on
+ * closure_sync - sleep until a closure a closure has nothing left to wait on
  *
  * Sleeps until the refcount hits 1 - the thread that's running the closure owns
  * the last refcount.
@@ -148,46 +134,6 @@ void closure_sync(struct closure *cl)
 }
 EXPORT_SYMBOL(closure_sync);
 
-/**
- * closure_trylock() - try to acquire the closure, without waiting
- * @cl:		closure to lock
- *
- * Returns true if the closure was succesfully locked.
- */
-bool closure_trylock(struct closure *cl, struct closure *parent)
-{
-	if (atomic_cmpxchg(&cl->remaining, -1,
-			   CLOSURE_REMAINING_INITIALIZER) != -1)
-		return false;
-
-	smp_mb();
-
-	cl->parent = parent;
-	if (parent)
-		closure_get(parent);
-
-	closure_set_ret_ip(cl);
-	closure_debug_create(cl);
-	return true;
-}
-EXPORT_SYMBOL(closure_trylock);
-
-void __closure_lock(struct closure *cl, struct closure *parent,
-		    struct closure_waitlist *wait_list)
-{
-	struct closure wait;
-	closure_init_stack(&wait);
-
-	while (1) {
-		if (closure_trylock(cl, parent))
-			return;
-
-		closure_wait_event(wait_list, &wait,
-				   atomic_read(&cl->remaining) == -1);
-	}
-}
-EXPORT_SYMBOL(__closure_lock);
-
 #ifdef CONFIG_BCACHE_CLOSURES_DEBUG
 
 static LIST_HEAD(closure_list);
diff --git a/drivers/md/bcache/closure.h b/drivers/md/bcache/closure.h
index 9762f1be3304..7ef7461912be 100644
--- a/drivers/md/bcache/closure.h
+++ b/drivers/md/bcache/closure.h
@@ -72,30 +72,6 @@
  * closure - _always_ use continue_at(). Doing so consistently will help
  * eliminate an entire class of particularly pernicious races.
  *
- * For a closure to wait on an arbitrary event, we need to introduce waitlists:
- *
- * struct closure_waitlist list;
- * closure_wait_event(list, cl, condition);
- * closure_wake_up(wait_list);
- *
- * These work analagously to wait_event() and wake_up() - except that instead of
- * operating on the current thread (for wait_event()) and lists of threads, they
- * operate on an explicit closure and lists of closures.
- *
- * Because it's a closure we can now wait either synchronously or
- * asynchronously. closure_wait_event() returns the current value of the
- * condition, and if it returned false continue_at() or closure_sync() can be
- * used to wait for it to become true.
- *
- * It's useful for waiting on things when you can't sleep in the context in
- * which you must check the condition (perhaps a spinlock held, or you might be
- * beneath generic_make_request() - in which case you can't sleep on IO).
- *
- * closure_wait_event() will wait either synchronously or asynchronously,
- * depending on whether the closure is in blocking mode or not. You can pick a
- * mode explicitly with closure_wait_event_sync() and
- * closure_wait_event_async(), which do just what you might expect.
- *
  * Lastly, you might have a wait list dedicated to a specific event, and have no
  * need for specifying the condition - you just want to wait until someone runs
  * closure_wake_up() on the appropriate wait list. In that case, just use
@@ -121,40 +97,6 @@
  * All this implies that a closure should typically be embedded in a particular
  * struct (which its refcount will normally control the lifetime of), and that
  * struct can very much be thought of as a stack frame.
- *
- * Locking:
- *
- * Closures are based on work items but they can be thought of as more like
- * threads - in that like threads and unlike work items they have a well
- * defined lifetime; they are created (with closure_init()) and eventually
- * complete after a continue_at(cl, NULL, NULL).
- *
- * Suppose you've got some larger structure with a closure embedded in it that's
- * used for periodically doing garbage collection. You only want one garbage
- * collection happening at a time, so the natural thing to do is protect it with
- * a lock. However, it's difficult to use a lock protecting a closure correctly
- * because the unlock should come after the last continue_to() (additionally, if
- * you're using the closure asynchronously a mutex won't work since a mutex has
- * to be unlocked by the same process that locked it).
- *
- * So to make it less error prone and more efficient, we also have the ability
- * to use closures as locks:
- *
- * closure_init_unlocked();
- * closure_trylock();
- *
- * That's all we need for trylock() - the last closure_put() implicitly unlocks
- * it for you.  But for closure_lock(), we also need a wait list:
- *
- * struct closure_with_waitlist frobnicator_cl;
- *
- * closure_init_unlocked(&frobnicator_cl);
- * closure_lock(&frobnicator_cl);
- *
- * A closure_with_waitlist embeds a closure and a wait list - much like struct
- * delayed_work embeds a work item and a timer_list. The important thing is, use
- * it exactly like you would a regular closure and closure_put() will magically
- * handle everything for you.
  */
 
 struct closure;
@@ -164,12 +106,6 @@ struct closure_waitlist {
 	struct llist_head	list;
 };
 
-enum closure_type {
-	TYPE_closure				= 0,
-	TYPE_closure_with_waitlist		= 1,
-	MAX_CLOSURE_TYPE			= 1,
-};
-
 enum closure_state {
 	/*
 	 * CLOSURE_WAITING: Set iff the closure is on a waitlist. Must be set by
@@ -224,8 +160,6 @@ struct closure {
 
 	atomic_t		remaining;
 
-	enum closure_type	type;
-
 #ifdef CONFIG_BCACHE_CLOSURES_DEBUG
 #define CLOSURE_MAGIC_DEAD	0xc054dead
 #define CLOSURE_MAGIC_ALIVE	0xc054a11e
@@ -237,34 +171,12 @@ struct closure {
 #endif
 };
 
-struct closure_with_waitlist {
-	struct closure		cl;
-	struct closure_waitlist	wait;
-};
-
-extern unsigned invalid_closure_type(void);
-
-#define __CLOSURE_TYPE(cl, _t)						\
-	  __builtin_types_compatible_p(typeof(cl), struct _t)		\
-		? TYPE_ ## _t :						\
-
-#define __closure_type(cl)						\
-(									\
-	__CLOSURE_TYPE(cl, closure)					\
-	__CLOSURE_TYPE(cl, closure_with_waitlist)			\
-	invalid_closure_type()						\
-)
-
 void closure_sub(struct closure *cl, int v);
 void closure_put(struct closure *cl);
 void __closure_wake_up(struct closure_waitlist *list);
 bool closure_wait(struct closure_waitlist *list, struct closure *cl);
 void closure_sync(struct closure *cl);
 
-bool closure_trylock(struct closure *cl, struct closure *parent);
-void __closure_lock(struct closure *cl, struct closure *parent,
-		    struct closure_waitlist *wait_list);
-
 #ifdef CONFIG_BCACHE_CLOSURES_DEBUG
 
 void closure_debug_init(void);
@@ -293,134 +205,97 @@ static inline void closure_set_ret_ip(struct closure *cl)
 #endif
 }
 
-static inline void closure_get(struct closure *cl)
+static inline void closure_set_waiting(struct closure *cl, unsigned long f)
 {
 #ifdef CONFIG_BCACHE_CLOSURES_DEBUG
-	BUG_ON((atomic_inc_return(&cl->remaining) &
-		CLOSURE_REMAINING_MASK) <= 1);
-#else
-	atomic_inc(&cl->remaining);
+	cl->waiting_on = f;
 #endif
 }
 
-static inline void closure_set_stopped(struct closure *cl)
+static inline void __closure_end_sleep(struct closure *cl)
 {
-	atomic_sub(CLOSURE_RUNNING, &cl->remaining);
+	__set_current_state(TASK_RUNNING);
+
+	if (atomic_read(&cl->remaining) & CLOSURE_SLEEPING)
+		atomic_sub(CLOSURE_SLEEPING, &cl->remaining);
 }
 
-static inline bool closure_is_unlocked(struct closure *cl)
+static inline void __closure_start_sleep(struct closure *cl)
 {
-	return atomic_read(&cl->remaining) == -1;
+	closure_set_ip(cl);
+	cl->task = current;
+	set_current_state(TASK_UNINTERRUPTIBLE);
+
+	if (!(atomic_read(&cl->remaining) & CLOSURE_SLEEPING))
+		atomic_add(CLOSURE_SLEEPING, &cl->remaining);
 }
 
-static inline void do_closure_init(struct closure *cl, struct closure *parent,
-				   bool running)
+static inline void closure_set_stopped(struct closure *cl)
 {
-	cl->parent = parent;
-	if (parent)
-		closure_get(parent);
-
-	if (running) {
-		closure_debug_create(cl);
-		atomic_set(&cl->remaining, CLOSURE_REMAINING_INITIALIZER);
-	} else
-		atomic_set(&cl->remaining, -1);
+	atomic_sub(CLOSURE_RUNNING, &cl->remaining);
+}
 
+static inline void set_closure_fn(struct closure *cl, closure_fn *fn,
+				  struct workqueue_struct *wq)
+{
+	BUG_ON(object_is_on_stack(cl));
 	closure_set_ip(cl);
+	cl->fn = fn;
+	cl->wq = wq;
+	/* between atomic_dec() in closure_put() */
+	smp_mb__before_atomic_dec();
 }
 
-/*
- * Hack to get at the embedded closure if there is one, by doing an unsafe cast:
- * the result of __closure_type() is thrown away, it's used merely for type
- * checking.
- */
-#define __to_internal_closure(cl)				\
-({								\
-	BUILD_BUG_ON(__closure_type(*cl) > MAX_CLOSURE_TYPE);	\
-	(struct closure *) cl;					\
-})
-
-#define closure_init_type(cl, parent, running)			\
-do {								\
-	struct closure *_cl = __to_internal_closure(cl);	\
-	_cl->type = __closure_type(*(cl));			\
-	do_closure_init(_cl, parent, running);			\
-} while (0)
+static inline void closure_queue(struct closure *cl)
+{
+	struct workqueue_struct *wq = cl->wq;
+	if (wq) {
+		INIT_WORK(&cl->work, cl->work.func);
+		BUG_ON(!queue_work(wq, &cl->work));
+	} else
+		cl->fn(cl);
+}
 
 /**
- * __closure_init() - Initialize a closure, skipping the memset()
- *
- * May be used instead of closure_init() when memory has already been zeroed.
+ * closure_get - increment a closure's refcount
  */
-#define __closure_init(cl, parent)				\
-	closure_init_type(cl, parent, true)
+static inline void closure_get(struct closure *cl)
+{
+#ifdef CONFIG_BCACHE_CLOSURES_DEBUG
+	BUG_ON((atomic_inc_return(&cl->remaining) &
+		CLOSURE_REMAINING_MASK) <= 1);
+#else
+	atomic_inc(&cl->remaining);
+#endif
+}
 
 /**
- * closure_init() - Initialize a closure, setting the refcount to 1
+ * closure_init - Initialize a closure, setting the refcount to 1
  * @cl:		closure to initialize
  * @parent:	parent of the new closure. cl will take a refcount on it for its
  *		lifetime; may be NULL.
  */
-#define closure_init(cl, parent)				\
-do {								\
-	memset((cl), 0, sizeof(*(cl)));				\
-	__closure_init(cl, parent);				\
-} while (0)
-
-static inline void closure_init_stack(struct closure *cl)
+static inline void closure_init(struct closure *cl, struct closure *parent)
 {
 	memset(cl, 0, sizeof(struct closure));
-	atomic_set(&cl->remaining, CLOSURE_REMAINING_INITIALIZER|CLOSURE_STACK);
-}
-
-/**
- * closure_init_unlocked() - Initialize a closure but leave it unlocked.
- * @cl:		closure to initialize
- *
- * For when the closure will be used as a lock. The closure may not be used
- * until after a closure_lock() or closure_trylock().
- */
-#define closure_init_unlocked(cl)				\
-do {								\
-	memset((cl), 0, sizeof(*(cl)));				\
-	closure_init_type(cl, NULL, false);			\
-} while (0)
-
-/**
- * closure_lock() - lock and initialize a closure.
- * @cl:		the closure to lock
- * @parent:	the new parent for this closure
- *
- * The closure must be of one of the types that has a waitlist (otherwise we
- * wouldn't be able to sleep on contention).
- *
- * @parent has exactly the same meaning as in closure_init(); if non null, the
- * closure will take a reference on @parent which will be released when it is
- * unlocked.
- */
-#define closure_lock(cl, parent)				\
-	__closure_lock(__to_internal_closure(cl), parent, &(cl)->wait)
+	cl->parent = parent;
+	if (parent)
+		closure_get(parent);
 
-static inline void __closure_end_sleep(struct closure *cl)
-{
-	__set_current_state(TASK_RUNNING);
+	atomic_set(&cl->remaining, CLOSURE_REMAINING_INITIALIZER);
 
-	if (atomic_read(&cl->remaining) & CLOSURE_SLEEPING)
-		atomic_sub(CLOSURE_SLEEPING, &cl->remaining);
+	closure_debug_create(cl);
+	closure_set_ip(cl);
 }
 
-static inline void __closure_start_sleep(struct closure *cl)
+static inline void closure_init_stack(struct closure *cl)
 {
-	closure_set_ip(cl);
-	cl->task = current;
-	set_current_state(TASK_UNINTERRUPTIBLE);
-
-	if (!(atomic_read(&cl->remaining) & CLOSURE_SLEEPING))
-		atomic_add(CLOSURE_SLEEPING, &cl->remaining);
+	memset(cl, 0, sizeof(struct closure));
+	atomic_set(&cl->remaining, CLOSURE_REMAINING_INITIALIZER|CLOSURE_STACK);
 }
 
 /**
- * closure_wake_up() - wake up all closures on a wait list.
+ * closure_wake_up - wake up all closures on a wait list.
  */
 static inline void closure_wake_up(struct closure_waitlist *list)
 {
@@ -428,69 +303,19 @@ static inline void closure_wake_up(struct closure_waitlist *list)
 	__closure_wake_up(list);
 }
 
-/*
- * Wait on an event, synchronously or asynchronously - analogous to wait_event()
- * but for closures.
- *
- * The loop is oddly structured so as to avoid a race; we must check the
- * condition again after we've added ourself to the waitlist. We know if we were
- * already on the waitlist because closure_wait() returns false; thus, we only
- * schedule or break if closure_wait() returns false. If it returns true, we
- * just loop again - rechecking the condition.
- *
- * The __closure_wake_up() is necessary because we may race with the event
- * becoming true; i.e. we see event false -> wait -> recheck condition, but the
- * thread that made the event true may have called closure_wake_up() before we
- * added ourself to the wait list.
- *
- * We have to call closure_sync() at the end instead of just
- * __closure_end_sleep() because a different thread might've called
- * closure_wake_up() before us and gotten preempted before they dropped the
- * refcount on our closure. If this was a stack allocated closure, that would be
- * bad.
+/**
+ * continue_at - jump to another function with barrier
+ *
+ * After @cl is no longer waiting on anything (i.e. all outstanding refs have
+ * been dropped with closure_put()), it will resume execution at @fn running out
+ * of @wq (or, if @wq is NULL, @fn will be called by closure_put() directly).
+ *
+ * NOTE: This macro expands to a return in the calling function!
+ *
+ * This is because after calling continue_at() you no longer have a ref on @cl,
+ * and whatever @cl owns may be freed out from under you - a running closure fn
+ * has a ref on its own closure which continue_at() drops.
  */
-#define closure_wait_event(list, cl, condition)				\
-({									\
-	typeof(condition) ret;						\
-									\
-	while (1) {							\
-		ret = (condition);					\
-		if (ret) {						\
-			__closure_wake_up(list);			\
-			closure_sync(cl);				\
-			break;						\
-		}							\
-									\
-		__closure_start_sleep(cl);				\
-									\
-		if (!closure_wait(list, cl))				\
-			schedule();					\
-	}								\
-									\
-	ret;								\
-})
-
-static inline void closure_queue(struct closure *cl)
-{
-	struct workqueue_struct *wq = cl->wq;
-	if (wq) {
-		INIT_WORK(&cl->work, cl->work.func);
-		BUG_ON(!queue_work(wq, &cl->work));
-	} else
-		cl->fn(cl);
-}
-
-static inline void set_closure_fn(struct closure *cl, closure_fn *fn,
-				  struct workqueue_struct *wq)
-{
-	BUG_ON(object_is_on_stack(cl));
-	closure_set_ip(cl);
-	cl->fn = fn;
-	cl->wq = wq;
-	/* between atomic_dec() in closure_put() */
-	smp_mb__before_atomic_dec();
-}
-
 #define continue_at(_cl, _fn, _wq)					\
 do {									\
 	set_closure_fn(_cl, _fn, _wq);					\
@@ -498,8 +323,28 @@ do {									\
 	return;								\
 } while (0)
 
+/**
+ * closure_return - finish execution of a closure
+ *
+ * This is used to indicate that @cl is finished: when all outstanding refs on
+ * @cl have been dropped @cl's ref on its parent closure (as passed to
+ * closure_init()) will be dropped, if one was specified - thus this can be
+ * thought of as returning to the parent closure.
+ */
 #define closure_return(_cl)	continue_at((_cl), NULL, NULL)
 
+/**
+ * continue_at_nobarrier - jump to another function without barrier
+ *
+ * Causes @fn to be executed out of @cl, in @wq context (or called directly if
+ * @wq is NULL).
+ *
+ * NOTE: like continue_at(), this macro expands to a return in the caller!
+ *
+ * The ref the caller of continue_at_nobarrier() had on @cl is now owned by @fn,
+ * thus it's not safe to touch anything protected by @cl after a
+ * continue_at_nobarrier().
+ */
 #define continue_at_nobarrier(_cl, _fn, _wq)				\
 do {									\
 	set_closure_fn(_cl, _fn, _wq);					\
@@ -507,6 +352,15 @@ do {									\
 	return;								\
 } while (0)
 
+/**
+ * closure_return - finish execution of a closure, with destructor
+ *
+ * Works like closure_return(), except @destructor will be called when all
+ * outstanding refs on @cl have been dropped; @destructor may be used to safely
+ * free the memory occupied by @cl, and it is called with the ref on the parent
+ * closure still held - so @destructor could safely return an item to a
+ * freelist protected by @cl's parent.
+ */
 #define closure_return_with_destructor(_cl, _destructor)		\
 do {									\
 	set_closure_fn(_cl, _destructor, NULL);				\
@@ -514,6 +368,13 @@ do {									\
 	return;								\
 } while (0)
 
+/**
+ * closure_call - execute @fn out of a new, uninitialized closure
+ *
+ * Typically used when running out of one closure, and we want to run @fn
+ * asynchronously out of a new closure - @parent will then wait for @cl to
+ * finish.
+ */
 static inline void closure_call(struct closure *cl, closure_fn fn,
 				struct workqueue_struct *wq,
 				struct closure *parent)
@@ -522,12 +383,4 @@ static inline void closure_call(struct closure *cl, closure_fn fn,
 	continue_at_nobarrier(cl, fn, wq);
 }
 
-static inline void closure_trylock_call(struct closure *cl, closure_fn fn,
-					struct workqueue_struct *wq,
-					struct closure *parent)
-{
-	if (closure_trylock(cl, parent))
-		continue_at_nobarrier(cl, fn, wq);
-}
-
 #endif /* _LINUX_CLOSURE_H */
diff --git a/drivers/md/bcache/debug.c b/drivers/md/bcache/debug.c
index 264fcfbd6290..8b1f1d5c1819 100644
--- a/drivers/md/bcache/debug.c
+++ b/drivers/md/bcache/debug.c
@@ -8,6 +8,7 @@
 #include "bcache.h"
 #include "btree.h"
 #include "debug.h"
+#include "extents.h"
 
 #include <linux/console.h>
 #include <linux/debugfs.h>
@@ -17,163 +18,96 @@
 
 static struct dentry *debug;
 
-const char *bch_ptr_status(struct cache_set *c, const struct bkey *k)
-{
-	unsigned i;
-
-	for (i = 0; i < KEY_PTRS(k); i++)
-		if (ptr_available(c, k, i)) {
-			struct cache *ca = PTR_CACHE(c, k, i);
-			size_t bucket = PTR_BUCKET_NR(c, k, i);
-			size_t r = bucket_remainder(c, PTR_OFFSET(k, i));
-
-			if (KEY_SIZE(k) + r > c->sb.bucket_size)
-				return "bad, length too big";
-			if (bucket <  ca->sb.first_bucket)
-				return "bad, short offset";
-			if (bucket >= ca->sb.nbuckets)
-				return "bad, offset past end of device";
-			if (ptr_stale(c, k, i))
-				return "stale";
-		}
-
-	if (!bkey_cmp(k, &ZERO_KEY))
-		return "bad, null key";
-	if (!KEY_PTRS(k))
-		return "bad, no pointers";
-	if (!KEY_SIZE(k))
-		return "zeroed key";
-	return "";
-}
-
-int bch_bkey_to_text(char *buf, size_t size, const struct bkey *k)
-{
-	unsigned i = 0;
-	char *out = buf, *end = buf + size;
-
-#define p(...)	(out += scnprintf(out, end - out, __VA_ARGS__))
-
-	p("%llu:%llu len %llu -> [", KEY_INODE(k), KEY_OFFSET(k), KEY_SIZE(k));
-
-	if (KEY_PTRS(k))
-		while (1) {
-			p("%llu:%llu gen %llu",
-			  PTR_DEV(k, i), PTR_OFFSET(k, i), PTR_GEN(k, i));
-
-			if (++i == KEY_PTRS(k))
-				break;
-
-			p(", ");
-		}
-
-	p("]");
-
-	if (KEY_DIRTY(k))
-		p(" dirty");
-	if (KEY_CSUM(k))
-		p(" cs%llu %llx", KEY_CSUM(k), k->ptr[1]);
-#undef p
-	return out - buf;
-}
-
 #ifdef CONFIG_BCACHE_DEBUG
 
-static void dump_bset(struct btree *b, struct bset *i)
-{
-	struct bkey *k, *next;
-	unsigned j;
-	char buf[80];
-
-	for (k = i->start; k < end(i); k = next) {
-		next = bkey_next(k);
-
-		bch_bkey_to_text(buf, sizeof(buf), k);
-		printk(KERN_ERR "block %zu key %zi/%u: %s", index(i, b),
-		       (uint64_t *) k - i->d, i->keys, buf);
-
-		for (j = 0; j < KEY_PTRS(k); j++) {
-			size_t n = PTR_BUCKET_NR(b->c, k, j);
-			printk(" bucket %zu", n);
-
-			if (n >= b->c->sb.first_bucket && n < b->c->sb.nbuckets)
-				printk(" prio %i",
-				       PTR_BUCKET(b->c, k, j)->prio);
-		}
+#define for_each_written_bset(b, start, i)				\
+	for (i = (start);						\
+	     (void *) i < (void *) (start) + (KEY_SIZE(&b->key) << 9) &&\
+	     i->seq == (start)->seq;					\
+	     i = (void *) i + set_blocks(i, block_bytes(b->c)) *	\
+		 block_bytes(b->c))
 
-		printk(" %s\n", bch_ptr_status(b->c, k));
-
-		if (next < end(i) &&
-		    bkey_cmp(k, !b->level ? &START_KEY(next) : next) > 0)
-			printk(KERN_ERR "Key skipped backwards\n");
-	}
-}
-
-static void bch_dump_bucket(struct btree *b)
-{
-	unsigned i;
-
-	console_lock();
-	for (i = 0; i <= b->nsets; i++)
-		dump_bset(b, b->sets[i].data);
-	console_unlock();
-}
-
-void bch_btree_verify(struct btree *b, struct bset *new)
+void bch_btree_verify(struct btree *b)
 {
 	struct btree *v = b->c->verify_data;
-	struct closure cl;
-	closure_init_stack(&cl);
+	struct bset *ondisk, *sorted, *inmemory;
+	struct bio *bio;
 
-	if (!b->c->verify)
+	if (!b->c->verify || !b->c->verify_ondisk)
 		return;
 
-	closure_wait_event(&b->io.wait, &cl,
-			   atomic_read(&b->io.cl.remaining) == -1);
-
+	down(&b->io_mutex);
 	mutex_lock(&b->c->verify_lock);
 
+	ondisk = b->c->verify_ondisk;
+	sorted = b->c->verify_data->keys.set->data;
+	inmemory = b->keys.set->data;
+
 	bkey_copy(&v->key, &b->key);
 	v->written = 0;
 	v->level = b->level;
+	v->keys.ops = b->keys.ops;
+
+	bio = bch_bbio_alloc(b->c);
+	bio->bi_bdev		= PTR_CACHE(b->c, &b->key, 0)->bdev;
+	bio->bi_iter.bi_sector	= PTR_OFFSET(&b->key, 0);
+	bio->bi_iter.bi_size	= KEY_SIZE(&v->key) << 9;
+	bch_bio_map(bio, sorted);
 
-	bch_btree_node_read(v);
-	closure_wait_event(&v->io.wait, &cl,
-			   atomic_read(&b->io.cl.remaining) == -1);
+	submit_bio_wait(REQ_META|READ_SYNC, bio);
+	bch_bbio_free(bio, b->c);
 
-	if (new->keys != v->sets[0].data->keys ||
-	    memcmp(new->start,
-		   v->sets[0].data->start,
-		   (void *) end(new) - (void *) new->start)) {
-		unsigned i, j;
+	memcpy(ondisk, sorted, KEY_SIZE(&v->key) << 9);
+
+	bch_btree_node_read_done(v);
+	sorted = v->keys.set->data;
+
+	if (inmemory->keys != sorted->keys ||
+	    memcmp(inmemory->start,
+		   sorted->start,
+		   (void *) bset_bkey_last(inmemory) - (void *) inmemory->start)) {
+		struct bset *i;
+		unsigned j;
 
 		console_lock();
 
-		printk(KERN_ERR "*** original memory node:\n");
-		for (i = 0; i <= b->nsets; i++)
-			dump_bset(b, b->sets[i].data);
+		printk(KERN_ERR "*** in memory:\n");
+		bch_dump_bset(&b->keys, inmemory, 0);
 
-		printk(KERN_ERR "*** sorted memory node:\n");
-		dump_bset(b, new);
+		printk(KERN_ERR "*** read back in:\n");
+		bch_dump_bset(&v->keys, sorted, 0);
 
-		printk(KERN_ERR "*** on disk node:\n");
-		dump_bset(v, v->sets[0].data);
+		for_each_written_bset(b, ondisk, i) {
+			unsigned block = ((void *) i - (void *) ondisk) /
+				block_bytes(b->c);
+
+			printk(KERN_ERR "*** on disk block %u:\n", block);
+			bch_dump_bset(&b->keys, i, block);
+		}
 
-		for (j = 0; j < new->keys; j++)
-			if (new->d[j] != v->sets[0].data->d[j])
+		printk(KERN_ERR "*** block %zu not written\n",
+		       ((void *) i - (void *) ondisk) / block_bytes(b->c));
+
+		for (j = 0; j < inmemory->keys; j++)
+			if (inmemory->d[j] != sorted->d[j])
 				break;
 
+		printk(KERN_ERR "b->written %u\n", b->written);
+
 		console_unlock();
 		panic("verify failed at %u\n", j);
 	}
 
 	mutex_unlock(&b->c->verify_lock);
+	up(&b->io_mutex);
 }
 
 void bch_data_verify(struct cached_dev *dc, struct bio *bio)
 {
 	char name[BDEVNAME_SIZE];
 	struct bio *check;
-	struct bio_vec *bv;
+	struct bio_vec bv, *bv2;
+	struct bvec_iter iter;
 	int i;
 
 	check = bio_clone(bio, GFP_NOIO);
@@ -185,95 +119,27 @@ void bch_data_verify(struct cached_dev *dc, struct bio *bio)
 
 	submit_bio_wait(READ_SYNC, check);
 
-	bio_for_each_segment(bv, bio, i) {
-		void *p1 = kmap_atomic(bv->bv_page);
-		void *p2 = page_address(check->bi_io_vec[i].bv_page);
+	bio_for_each_segment(bv, bio, iter) {
+		void *p1 = kmap_atomic(bv.bv_page);
+		void *p2 = page_address(check->bi_io_vec[iter.bi_idx].bv_page);
 
-		cache_set_err_on(memcmp(p1 + bv->bv_offset,
-					p2 + bv->bv_offset,
-					bv->bv_len),
+		cache_set_err_on(memcmp(p1 + bv.bv_offset,
+					p2 + bv.bv_offset,
+					bv.bv_len),
 				 dc->disk.c,
 				 "verify failed at dev %s sector %llu",
 				 bdevname(dc->bdev, name),
-				 (uint64_t) bio->bi_sector);
+				 (uint64_t) bio->bi_iter.bi_sector);
 
 		kunmap_atomic(p1);
 	}
 
-	bio_for_each_segment_all(bv, check, i)
-		__free_page(bv->bv_page);
+	bio_for_each_segment_all(bv2, check, i)
+		__free_page(bv2->bv_page);
 out_put:
 	bio_put(check);
 }
 
-int __bch_count_data(struct btree *b)
-{
-	unsigned ret = 0;
-	struct btree_iter iter;
-	struct bkey *k;
-
-	if (!b->level)
-		for_each_key(b, k, &iter)
-			ret += KEY_SIZE(k);
-	return ret;
-}
-
-void __bch_check_keys(struct btree *b, const char *fmt, ...)
-{
-	va_list args;
-	struct bkey *k, *p = NULL;
-	struct btree_iter iter;
-	const char *err;
-
-	for_each_key(b, k, &iter) {
-		if (!b->level) {
-			err = "Keys out of order";
-			if (p && bkey_cmp(&START_KEY(p), &START_KEY(k)) > 0)
-				goto bug;
-
-			if (bch_ptr_invalid(b, k))
-				continue;
-
-			err =  "Overlapping keys";
-			if (p && bkey_cmp(p, &START_KEY(k)) > 0)
-				goto bug;
-		} else {
-			if (bch_ptr_bad(b, k))
-				continue;
-
-			err = "Duplicate keys";
-			if (p && !bkey_cmp(p, k))
-				goto bug;
-		}
-		p = k;
-	}
-
-	err = "Key larger than btree node key";
-	if (p && bkey_cmp(p, &b->key) > 0)
-		goto bug;
-
-	return;
-bug:
-	bch_dump_bucket(b);
-
-	va_start(args, fmt);
-	vprintk(fmt, args);
-	va_end(args);
-
-	panic("bcache error: %s:\n", err);
-}
-
-void bch_btree_iter_next_check(struct btree_iter *iter)
-{
-	struct bkey *k = iter->data->k, *next = bkey_next(k);
-
-	if (next < iter->data->end &&
-	    bkey_cmp(k, iter->b->level ? next : &START_KEY(next)) > 0) {
-		bch_dump_bucket(iter->b);
-		panic("Key skipped backwards\n");
-	}
-}
-
 #endif
 
 #ifdef CONFIG_DEBUG_FS
@@ -320,7 +186,7 @@ static ssize_t bch_dump_read(struct file *file, char __user *buf,
 		if (!w)
 			break;
 
-		bch_bkey_to_text(kbuf, sizeof(kbuf), &w->key);
+		bch_extent_to_text(kbuf, sizeof(kbuf), &w->key);
 		i->bytes = snprintf(i->buf, PAGE_SIZE, "%s\n", kbuf);
 		bch_keybuf_del(&i->keys, w);
 	}
diff --git a/drivers/md/bcache/debug.h b/drivers/md/bcache/debug.h
index 2ede60e31874..1f63c195d247 100644
--- a/drivers/md/bcache/debug.h
+++ b/drivers/md/bcache/debug.h
@@ -1,47 +1,30 @@
 #ifndef _BCACHE_DEBUG_H
 #define _BCACHE_DEBUG_H
 
-/* Btree/bkey debug printing */
-
-int bch_bkey_to_text(char *buf, size_t size, const struct bkey *k);
+struct bio;
+struct cached_dev;
+struct cache_set;
 
 #ifdef CONFIG_BCACHE_DEBUG
 
-void bch_btree_verify(struct btree *, struct bset *);
+void bch_btree_verify(struct btree *);
 void bch_data_verify(struct cached_dev *, struct bio *);
-int __bch_count_data(struct btree *);
-void __bch_check_keys(struct btree *, const char *, ...);
-void bch_btree_iter_next_check(struct btree_iter *);
 
-#define EBUG_ON(cond)			BUG_ON(cond)
 #define expensive_debug_checks(c)	((c)->expensive_debug_checks)
 #define key_merging_disabled(c)		((c)->key_merging_disabled)
 #define bypass_torture_test(d)		((d)->bypass_torture_test)
 
 #else /* DEBUG */
 
-static inline void bch_btree_verify(struct btree *b, struct bset *i) {}
+static inline void bch_btree_verify(struct btree *b) {}
 static inline void bch_data_verify(struct cached_dev *dc, struct bio *bio) {}
-static inline int __bch_count_data(struct btree *b) { return -1; }
-static inline void __bch_check_keys(struct btree *b, const char *fmt, ...) {}
-static inline void bch_btree_iter_next_check(struct btree_iter *iter) {}
 
-#define EBUG_ON(cond)			do { if (cond); } while (0)
 #define expensive_debug_checks(c)	0
 #define key_merging_disabled(c)		0
 #define bypass_torture_test(d)		0
 
 #endif
 
-#define bch_count_data(b)						\
-	(expensive_debug_checks((b)->c) ? __bch_count_data(b) : -1)
-
-#define bch_check_keys(b, ...)						\
-do {									\
-	if (expensive_debug_checks((b)->c))				\
-		__bch_check_keys(b, __VA_ARGS__);			\
-} while (0)
-
 #ifdef CONFIG_DEBUG_FS
 void bch_debug_init_cache_set(struct cache_set *);
 #else
diff --git a/drivers/md/bcache/extents.c b/drivers/md/bcache/extents.c
new file mode 100644
index 000000000000..c3ead586dc27
--- /dev/null
+++ b/drivers/md/bcache/extents.c
@@ -0,0 +1,616 @@
+/*
+ * Copyright (C) 2010 Kent Overstreet <kent.overstreet@gmail.com>
+ *
+ * Uses a block device as cache for other block devices; optimized for SSDs.
+ * All allocation is done in buckets, which should match the erase block size
+ * of the device.
+ *
+ * Buckets containing cached data are kept on a heap sorted by priority;
+ * bucket priority is increased on cache hit, and periodically all the buckets
+ * on the heap have their priority scaled down. This currently is just used as
+ * an LRU but in the future should allow for more intelligent heuristics.
+ *
+ * Buckets have an 8 bit counter; freeing is accomplished by incrementing the
+ * counter. Garbage collection is used to remove stale pointers.
+ *
+ * Indexing is done via a btree; nodes are not necessarily fully sorted, rather
+ * as keys are inserted we only sort the pages that have not yet been written.
+ * When garbage collection is run, we resort the entire node.
+ *
+ * All configuration is done via sysfs; see Documentation/bcache.txt.
+ */
+
+#include "bcache.h"
+#include "btree.h"
+#include "debug.h"
+#include "extents.h"
+#include "writeback.h"
+
+static void sort_key_next(struct btree_iter *iter,
+			  struct btree_iter_set *i)
+{
+	i->k = bkey_next(i->k);
+
+	if (i->k == i->end)
+		*i = iter->data[--iter->used];
+}
+
+static bool bch_key_sort_cmp(struct btree_iter_set l,
+			     struct btree_iter_set r)
+{
+	int64_t c = bkey_cmp(l.k, r.k);
+
+	return c ? c > 0 : l.k < r.k;
+}
+
+static bool __ptr_invalid(struct cache_set *c, const struct bkey *k)
+{
+	unsigned i;
+
+	for (i = 0; i < KEY_PTRS(k); i++)
+		if (ptr_available(c, k, i)) {
+			struct cache *ca = PTR_CACHE(c, k, i);
+			size_t bucket = PTR_BUCKET_NR(c, k, i);
+			size_t r = bucket_remainder(c, PTR_OFFSET(k, i));
+
+			if (KEY_SIZE(k) + r > c->sb.bucket_size ||
+			    bucket <  ca->sb.first_bucket ||
+			    bucket >= ca->sb.nbuckets)
+				return true;
+		}
+
+	return false;
+}
+
+/* Common among btree and extent ptrs */
+
+static const char *bch_ptr_status(struct cache_set *c, const struct bkey *k)
+{
+	unsigned i;
+
+	for (i = 0; i < KEY_PTRS(k); i++)
+		if (ptr_available(c, k, i)) {
+			struct cache *ca = PTR_CACHE(c, k, i);
+			size_t bucket = PTR_BUCKET_NR(c, k, i);
+			size_t r = bucket_remainder(c, PTR_OFFSET(k, i));
+
+			if (KEY_SIZE(k) + r > c->sb.bucket_size)
+				return "bad, length too big";
+			if (bucket <  ca->sb.first_bucket)
+				return "bad, short offset";
+			if (bucket >= ca->sb.nbuckets)
+				return "bad, offset past end of device";
+			if (ptr_stale(c, k, i))
+				return "stale";
+		}
+
+	if (!bkey_cmp(k, &ZERO_KEY))
+		return "bad, null key";
+	if (!KEY_PTRS(k))
+		return "bad, no pointers";
+	if (!KEY_SIZE(k))
+		return "zeroed key";
+	return "";
+}
+
+void bch_extent_to_text(char *buf, size_t size, const struct bkey *k)
+{
+	unsigned i = 0;
+	char *out = buf, *end = buf + size;
+
+#define p(...)	(out += scnprintf(out, end - out, __VA_ARGS__))
+
+	p("%llu:%llu len %llu -> [", KEY_INODE(k), KEY_START(k), KEY_SIZE(k));
+
+	for (i = 0; i < KEY_PTRS(k); i++) {
+		if (i)
+			p(", ");
+
+		if (PTR_DEV(k, i) == PTR_CHECK_DEV)
+			p("check dev");
+		else
+			p("%llu:%llu gen %llu", PTR_DEV(k, i),
+			  PTR_OFFSET(k, i), PTR_GEN(k, i));
+	}
+
+	p("]");
+
+	if (KEY_DIRTY(k))
+		p(" dirty");
+	if (KEY_CSUM(k))
+		p(" cs%llu %llx", KEY_CSUM(k), k->ptr[1]);
+#undef p
+}
+
+static void bch_bkey_dump(struct btree_keys *keys, const struct bkey *k)
+{
+	struct btree *b = container_of(keys, struct btree, keys);
+	unsigned j;
+	char buf[80];
+
+	bch_extent_to_text(buf, sizeof(buf), k);
+	printk(" %s", buf);
+
+	for (j = 0; j < KEY_PTRS(k); j++) {
+		size_t n = PTR_BUCKET_NR(b->c, k, j);
+		printk(" bucket %zu", n);
+
+		if (n >= b->c->sb.first_bucket && n < b->c->sb.nbuckets)
+			printk(" prio %i",
+			       PTR_BUCKET(b->c, k, j)->prio);
+	}
+
+	printk(" %s\n", bch_ptr_status(b->c, k));
+}
+
+/* Btree ptrs */
+
+bool __bch_btree_ptr_invalid(struct cache_set *c, const struct bkey *k)
+{
+	char buf[80];
+
+	if (!KEY_PTRS(k) || !KEY_SIZE(k) || KEY_DIRTY(k))
+		goto bad;
+
+	if (__ptr_invalid(c, k))
+		goto bad;
+
+	return false;
+bad:
+	bch_extent_to_text(buf, sizeof(buf), k);
+	cache_bug(c, "spotted btree ptr %s: %s", buf, bch_ptr_status(c, k));
+	return true;
+}
+
+static bool bch_btree_ptr_invalid(struct btree_keys *bk, const struct bkey *k)
+{
+	struct btree *b = container_of(bk, struct btree, keys);
+	return __bch_btree_ptr_invalid(b->c, k);
+}
+
+static bool btree_ptr_bad_expensive(struct btree *b, const struct bkey *k)
+{
+	unsigned i;
+	char buf[80];
+	struct bucket *g;
+
+	if (mutex_trylock(&b->c->bucket_lock)) {
+		for (i = 0; i < KEY_PTRS(k); i++)
+			if (ptr_available(b->c, k, i)) {
+				g = PTR_BUCKET(b->c, k, i);
+
+				if (KEY_DIRTY(k) ||
+				    g->prio != BTREE_PRIO ||
+				    (b->c->gc_mark_valid &&
+				     GC_MARK(g) != GC_MARK_METADATA))
+					goto err;
+			}
+
+		mutex_unlock(&b->c->bucket_lock);
+	}
+
+	return false;
+err:
+	mutex_unlock(&b->c->bucket_lock);
+	bch_extent_to_text(buf, sizeof(buf), k);
+	btree_bug(b,
+"inconsistent btree pointer %s: bucket %li pin %i prio %i gen %i last_gc %i mark %llu gc_gen %i",
+		  buf, PTR_BUCKET_NR(b->c, k, i), atomic_read(&g->pin),
+		  g->prio, g->gen, g->last_gc, GC_MARK(g), g->gc_gen);
+	return true;
+}
+
+static bool bch_btree_ptr_bad(struct btree_keys *bk, const struct bkey *k)
+{
+	struct btree *b = container_of(bk, struct btree, keys);
+	unsigned i;
+
+	if (!bkey_cmp(k, &ZERO_KEY) ||
+	    !KEY_PTRS(k) ||
+	    bch_ptr_invalid(bk, k))
+		return true;
+
+	for (i = 0; i < KEY_PTRS(k); i++)
+		if (!ptr_available(b->c, k, i) ||
+		    ptr_stale(b->c, k, i))
+			return true;
+
+	if (expensive_debug_checks(b->c) &&
+	    btree_ptr_bad_expensive(b, k))
+		return true;
+
+	return false;
+}
+
+static bool bch_btree_ptr_insert_fixup(struct btree_keys *bk,
+				       struct bkey *insert,
+				       struct btree_iter *iter,
+				       struct bkey *replace_key)
+{
+	struct btree *b = container_of(bk, struct btree, keys);
+
+	if (!KEY_OFFSET(insert))
+		btree_current_write(b)->prio_blocked++;
+
+	return false;
+}
+
+const struct btree_keys_ops bch_btree_keys_ops = {
+	.sort_cmp	= bch_key_sort_cmp,
+	.insert_fixup	= bch_btree_ptr_insert_fixup,
+	.key_invalid	= bch_btree_ptr_invalid,
+	.key_bad	= bch_btree_ptr_bad,
+	.key_to_text	= bch_extent_to_text,
+	.key_dump	= bch_bkey_dump,
+};
+
+/* Extents */
+
+/*
+ * Returns true if l > r - unless l == r, in which case returns true if l is
+ * older than r.
+ *
+ * Necessary for btree_sort_fixup() - if there are multiple keys that compare
+ * equal in different sets, we have to process them newest to oldest.
+ */
+static bool bch_extent_sort_cmp(struct btree_iter_set l,
+				struct btree_iter_set r)
+{
+	int64_t c = bkey_cmp(&START_KEY(l.k), &START_KEY(r.k));
+
+	return c ? c > 0 : l.k < r.k;
+}
+
+static struct bkey *bch_extent_sort_fixup(struct btree_iter *iter,
+					  struct bkey *tmp)
+{
+	while (iter->used > 1) {
+		struct btree_iter_set *top = iter->data, *i = top + 1;
+
+		if (iter->used > 2 &&
+		    bch_extent_sort_cmp(i[0], i[1]))
+			i++;
+
+		if (bkey_cmp(top->k, &START_KEY(i->k)) <= 0)
+			break;
+
+		if (!KEY_SIZE(i->k)) {
+			sort_key_next(iter, i);
+			heap_sift(iter, i - top, bch_extent_sort_cmp);
+			continue;
+		}
+
+		if (top->k > i->k) {
+			if (bkey_cmp(top->k, i->k) >= 0)
+				sort_key_next(iter, i);
+			else
+				bch_cut_front(top->k, i->k);
+
+			heap_sift(iter, i - top, bch_extent_sort_cmp);
+		} else {
+			/* can't happen because of comparison func */
+			BUG_ON(!bkey_cmp(&START_KEY(top->k), &START_KEY(i->k)));
+
+			if (bkey_cmp(i->k, top->k) < 0) {
+				bkey_copy(tmp, top->k);
+
+				bch_cut_back(&START_KEY(i->k), tmp);
+				bch_cut_front(i->k, top->k);
+				heap_sift(iter, 0, bch_extent_sort_cmp);
+
+				return tmp;
+			} else {
+				bch_cut_back(&START_KEY(i->k), top->k);
+			}
+		}
+	}
+
+	return NULL;
+}
+
+static bool bch_extent_insert_fixup(struct btree_keys *b,
+				    struct bkey *insert,
+				    struct btree_iter *iter,
+				    struct bkey *replace_key)
+{
+	struct cache_set *c = container_of(b, struct btree, keys)->c;
+
+	void subtract_dirty(struct bkey *k, uint64_t offset, int sectors)
+	{
+		if (KEY_DIRTY(k))
+			bcache_dev_sectors_dirty_add(c, KEY_INODE(k),
+						     offset, -sectors);
+	}
+
+	uint64_t old_offset;
+	unsigned old_size, sectors_found = 0;
+
+	BUG_ON(!KEY_OFFSET(insert));
+	BUG_ON(!KEY_SIZE(insert));
+
+	while (1) {
+		struct bkey *k = bch_btree_iter_next(iter);
+		if (!k)
+			break;
+
+		if (bkey_cmp(&START_KEY(k), insert) >= 0) {
+			if (KEY_SIZE(k))
+				break;
+			else
+				continue;
+		}
+
+		if (bkey_cmp(k, &START_KEY(insert)) <= 0)
+			continue;
+
+		old_offset = KEY_START(k);
+		old_size = KEY_SIZE(k);
+
+		/*
+		 * We might overlap with 0 size extents; we can't skip these
+		 * because if they're in the set we're inserting to we have to
+		 * adjust them so they don't overlap with the key we're
+		 * inserting. But we don't want to check them for replace
+		 * operations.
+		 */
+
+		if (replace_key && KEY_SIZE(k)) {
+			/*
+			 * k might have been split since we inserted/found the
+			 * key we're replacing
+			 */
+			unsigned i;
+			uint64_t offset = KEY_START(k) -
+				KEY_START(replace_key);
+
+			/* But it must be a subset of the replace key */
+			if (KEY_START(k) < KEY_START(replace_key) ||
+			    KEY_OFFSET(k) > KEY_OFFSET(replace_key))
+				goto check_failed;
+
+			/* We didn't find a key that we were supposed to */
+			if (KEY_START(k) > KEY_START(insert) + sectors_found)
+				goto check_failed;
+
+			if (!bch_bkey_equal_header(k, replace_key))
+				goto check_failed;
+
+			/* skip past gen */
+			offset <<= 8;
+
+			BUG_ON(!KEY_PTRS(replace_key));
+
+			for (i = 0; i < KEY_PTRS(replace_key); i++)
+				if (k->ptr[i] != replace_key->ptr[i] + offset)
+					goto check_failed;
+
+			sectors_found = KEY_OFFSET(k) - KEY_START(insert);
+		}
+
+		if (bkey_cmp(insert, k) < 0 &&
+		    bkey_cmp(&START_KEY(insert), &START_KEY(k)) > 0) {
+			/*
+			 * We overlapped in the middle of an existing key: that
+			 * means we have to split the old key. But we have to do
+			 * slightly different things depending on whether the
+			 * old key has been written out yet.
+			 */
+
+			struct bkey *top;
+
+			subtract_dirty(k, KEY_START(insert), KEY_SIZE(insert));
+
+			if (bkey_written(b, k)) {
+				/*
+				 * We insert a new key to cover the top of the
+				 * old key, and the old key is modified in place
+				 * to represent the bottom split.
+				 *
+				 * It's completely arbitrary whether the new key
+				 * is the top or the bottom, but it has to match
+				 * up with what btree_sort_fixup() does - it
+				 * doesn't check for this kind of overlap, it
+				 * depends on us inserting a new key for the top
+				 * here.
+				 */
+				top = bch_bset_search(b, bset_tree_last(b),
+						      insert);
+				bch_bset_insert(b, top, k);
+			} else {
+				BKEY_PADDED(key) temp;
+				bkey_copy(&temp.key, k);
+				bch_bset_insert(b, k, &temp.key);
+				top = bkey_next(k);
+			}
+
+			bch_cut_front(insert, top);
+			bch_cut_back(&START_KEY(insert), k);
+			bch_bset_fix_invalidated_key(b, k);
+			goto out;
+		}
+
+		if (bkey_cmp(insert, k) < 0) {
+			bch_cut_front(insert, k);
+		} else {
+			if (bkey_cmp(&START_KEY(insert), &START_KEY(k)) > 0)
+				old_offset = KEY_START(insert);
+
+			if (bkey_written(b, k) &&
+			    bkey_cmp(&START_KEY(insert), &START_KEY(k)) <= 0) {
+				/*
+				 * Completely overwrote, so we don't have to
+				 * invalidate the binary search tree
+				 */
+				bch_cut_front(k, k);
+			} else {
+				__bch_cut_back(&START_KEY(insert), k);
+				bch_bset_fix_invalidated_key(b, k);
+			}
+		}
+
+		subtract_dirty(k, old_offset, old_size - KEY_SIZE(k));
+	}
+
+check_failed:
+	if (replace_key) {
+		if (!sectors_found) {
+			return true;
+		} else if (sectors_found < KEY_SIZE(insert)) {
+			SET_KEY_OFFSET(insert, KEY_OFFSET(insert) -
+				       (KEY_SIZE(insert) - sectors_found));
+			SET_KEY_SIZE(insert, sectors_found);
+		}
+	}
+out:
+	if (KEY_DIRTY(insert))
+		bcache_dev_sectors_dirty_add(c, KEY_INODE(insert),
+					     KEY_START(insert),
+					     KEY_SIZE(insert));
+
+	return false;
+}
+
+static bool bch_extent_invalid(struct btree_keys *bk, const struct bkey *k)
+{
+	struct btree *b = container_of(bk, struct btree, keys);
+	char buf[80];
+
+	if (!KEY_SIZE(k))
+		return true;
+
+	if (KEY_SIZE(k) > KEY_OFFSET(k))
+		goto bad;
+
+	if (__ptr_invalid(b->c, k))
+		goto bad;
+
+	return false;
+bad:
+	bch_extent_to_text(buf, sizeof(buf), k);
+	cache_bug(b->c, "spotted extent %s: %s", buf, bch_ptr_status(b->c, k));
+	return true;
+}
+
+static bool bch_extent_bad_expensive(struct btree *b, const struct bkey *k,
+				     unsigned ptr)
+{
+	struct bucket *g = PTR_BUCKET(b->c, k, ptr);
+	char buf[80];
+
+	if (mutex_trylock(&b->c->bucket_lock)) {
+		if (b->c->gc_mark_valid &&
+		    ((GC_MARK(g) != GC_MARK_DIRTY &&
+		      KEY_DIRTY(k)) ||
+		     GC_MARK(g) == GC_MARK_METADATA))
+			goto err;
+
+		if (g->prio == BTREE_PRIO)
+			goto err;
+
+		mutex_unlock(&b->c->bucket_lock);
+	}
+
+	return false;
+err:
+	mutex_unlock(&b->c->bucket_lock);
+	bch_extent_to_text(buf, sizeof(buf), k);
+	btree_bug(b,
+"inconsistent extent pointer %s:\nbucket %zu pin %i prio %i gen %i last_gc %i mark %llu gc_gen %i",
+		  buf, PTR_BUCKET_NR(b->c, k, ptr), atomic_read(&g->pin),
+		  g->prio, g->gen, g->last_gc, GC_MARK(g), g->gc_gen);
+	return true;
+}
+
+static bool bch_extent_bad(struct btree_keys *bk, const struct bkey *k)
+{
+	struct btree *b = container_of(bk, struct btree, keys);
+	struct bucket *g;
+	unsigned i, stale;
+
+	if (!KEY_PTRS(k) ||
+	    bch_extent_invalid(bk, k))
+		return true;
+
+	for (i = 0; i < KEY_PTRS(k); i++)
+		if (!ptr_available(b->c, k, i))
+			return true;
+
+	if (!expensive_debug_checks(b->c) && KEY_DIRTY(k))
+		return false;
+
+	for (i = 0; i < KEY_PTRS(k); i++) {
+		g = PTR_BUCKET(b->c, k, i);
+		stale = ptr_stale(b->c, k, i);
+
+		btree_bug_on(stale > 96, b,
+			     "key too stale: %i, need_gc %u",
+			     stale, b->c->need_gc);
+
+		btree_bug_on(stale && KEY_DIRTY(k) && KEY_SIZE(k),
+			     b, "stale dirty pointer");
+
+		if (stale)
+			return true;
+
+		if (expensive_debug_checks(b->c) &&
+		    bch_extent_bad_expensive(b, k, i))
+			return true;
+	}
+
+	return false;
+}
+
+static uint64_t merge_chksums(struct bkey *l, struct bkey *r)
+{
+	return (l->ptr[KEY_PTRS(l)] + r->ptr[KEY_PTRS(r)]) &
+		~((uint64_t)1 << 63);
+}
+
+static bool bch_extent_merge(struct btree_keys *bk, struct bkey *l, struct bkey *r)
+{
+	struct btree *b = container_of(bk, struct btree, keys);
+	unsigned i;
+
+	if (key_merging_disabled(b->c))
+		return false;
+
+	for (i = 0; i < KEY_PTRS(l); i++)
+		if (l->ptr[i] + PTR(0, KEY_SIZE(l), 0) != r->ptr[i] ||
+		    PTR_BUCKET_NR(b->c, l, i) != PTR_BUCKET_NR(b->c, r, i))
+			return false;
+
+	/* Keys with no pointers aren't restricted to one bucket and could
+	 * overflow KEY_SIZE
+	 */
+	if (KEY_SIZE(l) + KEY_SIZE(r) > USHRT_MAX) {
+		SET_KEY_OFFSET(l, KEY_OFFSET(l) + USHRT_MAX - KEY_SIZE(l));
+		SET_KEY_SIZE(l, USHRT_MAX);
+
+		bch_cut_front(l, r);
+		return false;
+	}
+
+	if (KEY_CSUM(l)) {
+		if (KEY_CSUM(r))
+			l->ptr[KEY_PTRS(l)] = merge_chksums(l, r);
+		else
+			SET_KEY_CSUM(l, 0);
+	}
+
+	SET_KEY_OFFSET(l, KEY_OFFSET(l) + KEY_SIZE(r));
+	SET_KEY_SIZE(l, KEY_SIZE(l) + KEY_SIZE(r));
+
+	return true;
+}
+
+const struct btree_keys_ops bch_extent_keys_ops = {
+	.sort_cmp	= bch_extent_sort_cmp,
+	.sort_fixup	= bch_extent_sort_fixup,
+	.insert_fixup	= bch_extent_insert_fixup,
+	.key_invalid	= bch_extent_invalid,
+	.key_bad	= bch_extent_bad,
+	.key_merge	= bch_extent_merge,
+	.key_to_text	= bch_extent_to_text,
+	.key_dump	= bch_bkey_dump,
+	.is_extents	= true,
+};
diff --git a/drivers/md/bcache/extents.h b/drivers/md/bcache/extents.h
new file mode 100644
index 000000000000..e4e23409782d
--- /dev/null
+++ b/drivers/md/bcache/extents.h
@@ -0,0 +1,13 @@
+#ifndef _BCACHE_EXTENTS_H
+#define _BCACHE_EXTENTS_H
+
+extern const struct btree_keys_ops bch_btree_keys_ops;
+extern const struct btree_keys_ops bch_extent_keys_ops;
+
+struct bkey;
+struct cache_set;
+
+void bch_extent_to_text(char *, size_t, const struct bkey *);
+bool __bch_btree_ptr_invalid(struct cache_set *, const struct bkey *);
+
+#endif /* _BCACHE_EXTENTS_H */
diff --git a/drivers/md/bcache/io.c b/drivers/md/bcache/io.c
index 9056632995b1..fa028fa82df4 100644
--- a/drivers/md/bcache/io.c
+++ b/drivers/md/bcache/io.c
@@ -11,178 +11,40 @@
 
 #include <linux/blkdev.h>
 
-static void bch_bi_idx_hack_endio(struct bio *bio, int error)
-{
-	struct bio *p = bio->bi_private;
-
-	bio_endio(p, error);
-	bio_put(bio);
-}
-
-static void bch_generic_make_request_hack(struct bio *bio)
-{
-	if (bio->bi_idx) {
-		struct bio *clone = bio_alloc(GFP_NOIO, bio_segments(bio));
-
-		memcpy(clone->bi_io_vec,
-		       bio_iovec(bio),
-		       bio_segments(bio) * sizeof(struct bio_vec));
-
-		clone->bi_sector	= bio->bi_sector;
-		clone->bi_bdev		= bio->bi_bdev;
-		clone->bi_rw		= bio->bi_rw;
-		clone->bi_vcnt		= bio_segments(bio);
-		clone->bi_size		= bio->bi_size;
-
-		clone->bi_private	= bio;
-		clone->bi_end_io	= bch_bi_idx_hack_endio;
-
-		bio = clone;
-	}
-
-	/*
-	 * Hack, since drivers that clone bios clone up to bi_max_vecs, but our
-	 * bios might have had more than that (before we split them per device
-	 * limitations).
-	 *
-	 * To be taken out once immutable bvec stuff is in.
-	 */
-	bio->bi_max_vecs = bio->bi_vcnt;
-
-	generic_make_request(bio);
-}
-
-/**
- * bch_bio_split - split a bio
- * @bio:	bio to split
- * @sectors:	number of sectors to split from the front of @bio
- * @gfp:	gfp mask
- * @bs:		bio set to allocate from
- *
- * Allocates and returns a new bio which represents @sectors from the start of
- * @bio, and updates @bio to represent the remaining sectors.
- *
- * If bio_sectors(@bio) was less than or equal to @sectors, returns @bio
- * unchanged.
- *
- * The newly allocated bio will point to @bio's bi_io_vec, if the split was on a
- * bvec boundry; it is the caller's responsibility to ensure that @bio is not
- * freed before the split.
- */
-struct bio *bch_bio_split(struct bio *bio, int sectors,
-			  gfp_t gfp, struct bio_set *bs)
-{
-	unsigned idx = bio->bi_idx, vcnt = 0, nbytes = sectors << 9;
-	struct bio_vec *bv;
-	struct bio *ret = NULL;
-
-	BUG_ON(sectors <= 0);
-
-	if (sectors >= bio_sectors(bio))
-		return bio;
-
-	if (bio->bi_rw & REQ_DISCARD) {
-		ret = bio_alloc_bioset(gfp, 1, bs);
-		if (!ret)
-			return NULL;
-		idx = 0;
-		goto out;
-	}
-
-	bio_for_each_segment(bv, bio, idx) {
-		vcnt = idx - bio->bi_idx;
-
-		if (!nbytes) {
-			ret = bio_alloc_bioset(gfp, vcnt, bs);
-			if (!ret)
-				return NULL;
-
-			memcpy(ret->bi_io_vec, bio_iovec(bio),
-			       sizeof(struct bio_vec) * vcnt);
-
-			break;
-		} else if (nbytes < bv->bv_len) {
-			ret = bio_alloc_bioset(gfp, ++vcnt, bs);
-			if (!ret)
-				return NULL;
-
-			memcpy(ret->bi_io_vec, bio_iovec(bio),
-			       sizeof(struct bio_vec) * vcnt);
-
-			ret->bi_io_vec[vcnt - 1].bv_len = nbytes;
-			bv->bv_offset	+= nbytes;
-			bv->bv_len	-= nbytes;
-			break;
-		}
-
-		nbytes -= bv->bv_len;
-	}
-out:
-	ret->bi_bdev	= bio->bi_bdev;
-	ret->bi_sector	= bio->bi_sector;
-	ret->bi_size	= sectors << 9;
-	ret->bi_rw	= bio->bi_rw;
-	ret->bi_vcnt	= vcnt;
-	ret->bi_max_vecs = vcnt;
-
-	bio->bi_sector	+= sectors;
-	bio->bi_size	-= sectors << 9;
-	bio->bi_idx	 = idx;
-
-	if (bio_integrity(bio)) {
-		if (bio_integrity_clone(ret, bio, gfp)) {
-			bio_put(ret);
-			return NULL;
-		}
-
-		bio_integrity_trim(ret, 0, bio_sectors(ret));
-		bio_integrity_trim(bio, bio_sectors(ret), bio_sectors(bio));
-	}
-
-	return ret;
-}
-
 static unsigned bch_bio_max_sectors(struct bio *bio)
 {
-	unsigned ret = bio_sectors(bio);
 	struct request_queue *q = bdev_get_queue(bio->bi_bdev);
-	unsigned max_segments = min_t(unsigned, BIO_MAX_PAGES,
-				      queue_max_segments(q));
+	struct bio_vec bv;
+	struct bvec_iter iter;
+	unsigned ret = 0, seg = 0;
 
 	if (bio->bi_rw & REQ_DISCARD)
-		return min(ret, q->limits.max_discard_sectors);
-
-	if (bio_segments(bio) > max_segments ||
-	    q->merge_bvec_fn) {
-		struct bio_vec *bv;
-		int i, seg = 0;
-
-		ret = 0;
-
-		bio_for_each_segment(bv, bio, i) {
-			struct bvec_merge_data bvm = {
-				.bi_bdev	= bio->bi_bdev,
-				.bi_sector	= bio->bi_sector,
-				.bi_size	= ret << 9,
-				.bi_rw		= bio->bi_rw,
-			};
-
-			if (seg == max_segments)
-				break;
+		return min(bio_sectors(bio), q->limits.max_discard_sectors);
+
+	bio_for_each_segment(bv, bio, iter) {
+		struct bvec_merge_data bvm = {
+			.bi_bdev	= bio->bi_bdev,
+			.bi_sector	= bio->bi_iter.bi_sector,
+			.bi_size	= ret << 9,
+			.bi_rw		= bio->bi_rw,
+		};
+
+		if (seg == min_t(unsigned, BIO_MAX_PAGES,
+				 queue_max_segments(q)))
+			break;
 
-			if (q->merge_bvec_fn &&
-			    q->merge_bvec_fn(q, &bvm, bv) < (int) bv->bv_len)
-				break;
+		if (q->merge_bvec_fn &&
+		    q->merge_bvec_fn(q, &bvm, &bv) < (int) bv.bv_len)
+			break;
 
-			seg++;
-			ret += bv->bv_len >> 9;
-		}
+		seg++;
+		ret += bv.bv_len >> 9;
 	}
 
 	ret = min(ret, queue_max_sectors(q));
 
 	WARN_ON(!ret);
-	ret = max_t(int, ret, bio_iovec(bio)->bv_len >> 9);
+	ret = max_t(int, ret, bio_iovec(bio).bv_len >> 9);
 
 	return ret;
 }
@@ -193,7 +55,7 @@ static void bch_bio_submit_split_done(struct closure *cl)
 
 	s->bio->bi_end_io = s->bi_end_io;
 	s->bio->bi_private = s->bi_private;
-	bio_endio(s->bio, 0);
+	bio_endio_nodec(s->bio, 0);
 
 	closure_debug_destroy(&s->cl);
 	mempool_free(s, s->p->bio_split_hook);
@@ -232,19 +94,19 @@ void bch_generic_make_request(struct bio *bio, struct bio_split_pool *p)
 	bio_get(bio);
 
 	do {
-		n = bch_bio_split(bio, bch_bio_max_sectors(bio),
-				  GFP_NOIO, s->p->bio_split);
+		n = bio_next_split(bio, bch_bio_max_sectors(bio),
+				   GFP_NOIO, s->p->bio_split);
 
 		n->bi_end_io	= bch_bio_submit_split_endio;
 		n->bi_private	= &s->cl;
 
 		closure_get(&s->cl);
-		bch_generic_make_request_hack(n);
+		generic_make_request(n);
 	} while (n != bio);
 
 	continue_at(&s->cl, bch_bio_submit_split_done, NULL);
 submit:
-	bch_generic_make_request_hack(bio);
+	generic_make_request(bio);
 }
 
 /* Bios with headers */
@@ -272,8 +134,8 @@ void __bch_submit_bbio(struct bio *bio, struct cache_set *c)
 {
 	struct bbio *b = container_of(bio, struct bbio, bio);
 
-	bio->bi_sector	= PTR_OFFSET(&b->key, 0);
-	bio->bi_bdev	= PTR_CACHE(c, &b->key, 0)->bdev;
+	bio->bi_iter.bi_sector	= PTR_OFFSET(&b->key, 0);
+	bio->bi_bdev		= PTR_CACHE(c, &b->key, 0)->bdev;
 
 	b->submit_time_us = local_clock_us();
 	closure_bio_submit(bio, bio->bi_private, PTR_CACHE(c, &b->key, 0));
diff --git a/drivers/md/bcache/journal.c b/drivers/md/bcache/journal.c
index ecdaa671bd50..18039affc306 100644
--- a/drivers/md/bcache/journal.c
+++ b/drivers/md/bcache/journal.c
@@ -44,17 +44,17 @@ static int journal_read_bucket(struct cache *ca, struct list_head *list,
 
 	closure_init_stack(&cl);
 
-	pr_debug("reading %llu", (uint64_t) bucket);
+	pr_debug("reading %u", bucket_index);
 
 	while (offset < ca->sb.bucket_size) {
 reread:		left = ca->sb.bucket_size - offset;
-		len = min_t(unsigned, left, PAGE_SECTORS * 8);
+		len = min_t(unsigned, left, PAGE_SECTORS << JSET_BITS);
 
 		bio_reset(bio);
-		bio->bi_sector	= bucket + offset;
+		bio->bi_iter.bi_sector	= bucket + offset;
 		bio->bi_bdev	= ca->bdev;
 		bio->bi_rw	= READ;
-		bio->bi_size	= len << 9;
+		bio->bi_iter.bi_size	= len << 9;
 
 		bio->bi_end_io	= journal_read_endio;
 		bio->bi_private = &cl;
@@ -74,19 +74,28 @@ reread:		left = ca->sb.bucket_size - offset;
 			struct list_head *where;
 			size_t blocks, bytes = set_bytes(j);
 
-			if (j->magic != jset_magic(&ca->sb))
+			if (j->magic != jset_magic(&ca->sb)) {
+				pr_debug("%u: bad magic", bucket_index);
 				return ret;
+			}
 
-			if (bytes > left << 9)
+			if (bytes > left << 9 ||
+			    bytes > PAGE_SIZE << JSET_BITS) {
+				pr_info("%u: too big, %zu bytes, offset %u",
+					bucket_index, bytes, offset);
 				return ret;
+			}
 
 			if (bytes > len << 9)
 				goto reread;
 
-			if (j->csum != csum_set(j))
+			if (j->csum != csum_set(j)) {
+				pr_info("%u: bad csum, %zu bytes, offset %u",
+					bucket_index, bytes, offset);
 				return ret;
+			}
 
-			blocks = set_blocks(j, ca->set);
+			blocks = set_blocks(j, block_bytes(ca->set));
 
 			while (!list_empty(list)) {
 				i = list_first_entry(list,
@@ -275,7 +284,7 @@ void bch_journal_mark(struct cache_set *c, struct list_head *list)
 		}
 
 		for (k = i->j.start;
-		     k < end(&i->j);
+		     k < bset_bkey_last(&i->j);
 		     k = bkey_next(k)) {
 			unsigned j;
 
@@ -313,7 +322,7 @@ int bch_journal_replay(struct cache_set *s, struct list_head *list)
 				 n, i->j.seq - 1, start, end);
 
 		for (k = i->j.start;
-		     k < end(&i->j);
+		     k < bset_bkey_last(&i->j);
 		     k = bkey_next(k)) {
 			trace_bcache_journal_replay_key(k);
 
@@ -437,13 +446,13 @@ static void do_journal_discard(struct cache *ca)
 		atomic_set(&ja->discard_in_flight, DISCARD_IN_FLIGHT);
 
 		bio_init(bio);
-		bio->bi_sector		= bucket_to_sector(ca->set,
+		bio->bi_iter.bi_sector	= bucket_to_sector(ca->set,
 						ca->sb.d[ja->discard_idx]);
 		bio->bi_bdev		= ca->bdev;
 		bio->bi_rw		= REQ_WRITE|REQ_DISCARD;
 		bio->bi_max_vecs	= 1;
 		bio->bi_io_vec		= bio->bi_inline_vecs;
-		bio->bi_size		= bucket_bytes(ca);
+		bio->bi_iter.bi_size	= bucket_bytes(ca);
 		bio->bi_end_io		= journal_discard_endio;
 
 		closure_get(&ca->set->cl);
@@ -555,6 +564,14 @@ static void journal_write_done(struct closure *cl)
 	continue_at_nobarrier(cl, journal_write, system_wq);
 }
 
+static void journal_write_unlock(struct closure *cl)
+{
+	struct cache_set *c = container_of(cl, struct cache_set, journal.io);
+
+	c->journal.io_in_flight = 0;
+	spin_unlock(&c->journal.lock);
+}
+
 static void journal_write_unlocked(struct closure *cl)
 	__releases(c->journal.lock)
 {
@@ -562,22 +579,15 @@ static void journal_write_unlocked(struct closure *cl)
 	struct cache *ca;
 	struct journal_write *w = c->journal.cur;
 	struct bkey *k = &c->journal.key;
-	unsigned i, sectors = set_blocks(w->data, c) * c->sb.block_size;
+	unsigned i, sectors = set_blocks(w->data, block_bytes(c)) *
+		c->sb.block_size;
 
 	struct bio *bio;
 	struct bio_list list;
 	bio_list_init(&list);
 
 	if (!w->need_write) {
-		/*
-		 * XXX: have to unlock closure before we unlock journal lock,
-		 * else we race with bch_journal(). But this way we race
-		 * against cache set unregister. Doh.
-		 */
-		set_closure_fn(cl, NULL, NULL);
-		closure_sub(cl, CLOSURE_RUNNING + 1);
-		spin_unlock(&c->journal.lock);
-		return;
+		closure_return_with_destructor(cl, journal_write_unlock);
 	} else if (journal_full(&c->journal)) {
 		journal_reclaim(c);
 		spin_unlock(&c->journal.lock);
@@ -586,7 +596,7 @@ static void journal_write_unlocked(struct closure *cl)
 		continue_at(cl, journal_write, system_wq);
 	}
 
-	c->journal.blocks_free -= set_blocks(w->data, c);
+	c->journal.blocks_free -= set_blocks(w->data, block_bytes(c));
 
 	w->data->btree_level = c->root->level;
 
@@ -608,10 +618,10 @@ static void journal_write_unlocked(struct closure *cl)
 		atomic_long_add(sectors, &ca->meta_sectors_written);
 
 		bio_reset(bio);
-		bio->bi_sector	= PTR_OFFSET(k, i);
+		bio->bi_iter.bi_sector	= PTR_OFFSET(k, i);
 		bio->bi_bdev	= ca->bdev;
 		bio->bi_rw	= REQ_WRITE|REQ_SYNC|REQ_META|REQ_FLUSH|REQ_FUA;
-		bio->bi_size	= sectors << 9;
+		bio->bi_iter.bi_size = sectors << 9;
 
 		bio->bi_end_io	= journal_write_endio;
 		bio->bi_private = w;
@@ -653,10 +663,12 @@ static void journal_try_write(struct cache_set *c)
 
 	w->need_write = true;
 
-	if (closure_trylock(cl, &c->cl))
-		journal_write_unlocked(cl);
-	else
+	if (!c->journal.io_in_flight) {
+		c->journal.io_in_flight = 1;
+		closure_call(cl, journal_write_unlocked, NULL, &c->cl);
+	} else {
 		spin_unlock(&c->journal.lock);
+	}
 }
 
 static struct journal_write *journal_wait_for_write(struct cache_set *c,
@@ -664,6 +676,7 @@ static struct journal_write *journal_wait_for_write(struct cache_set *c,
 {
 	size_t sectors;
 	struct closure cl;
+	bool wait = false;
 
 	closure_init_stack(&cl);
 
@@ -673,16 +686,19 @@ static struct journal_write *journal_wait_for_write(struct cache_set *c,
 		struct journal_write *w = c->journal.cur;
 
 		sectors = __set_blocks(w->data, w->data->keys + nkeys,
-				       c) * c->sb.block_size;
+				       block_bytes(c)) * c->sb.block_size;
 
 		if (sectors <= min_t(size_t,
 				     c->journal.blocks_free * c->sb.block_size,
 				     PAGE_SECTORS << JSET_BITS))
 			return w;
 
-		/* XXX: tracepoint */
+		if (wait)
+			closure_wait(&c->journal.wait, &cl);
+
 		if (!journal_full(&c->journal)) {
-			trace_bcache_journal_entry_full(c);
+			if (wait)
+				trace_bcache_journal_entry_full(c);
 
 			/*
 			 * XXX: If we were inserting so many keys that they
@@ -692,12 +708,11 @@ static struct journal_write *journal_wait_for_write(struct cache_set *c,
 			 */
 			BUG_ON(!w->data->keys);
 
-			closure_wait(&w->wait, &cl);
 			journal_try_write(c); /* unlocks */
 		} else {
-			trace_bcache_journal_full(c);
+			if (wait)
+				trace_bcache_journal_full(c);
 
-			closure_wait(&c->journal.wait, &cl);
 			journal_reclaim(c);
 			spin_unlock(&c->journal.lock);
 
@@ -706,6 +721,7 @@ static struct journal_write *journal_wait_for_write(struct cache_set *c,
 
 		closure_sync(&cl);
 		spin_lock(&c->journal.lock);
+		wait = true;
 	}
 }
 
@@ -736,7 +752,7 @@ atomic_t *bch_journal(struct cache_set *c,
 
 	w = journal_wait_for_write(c, bch_keylist_nkeys(keys));
 
-	memcpy(end(w->data), keys->keys, bch_keylist_bytes(keys));
+	memcpy(bset_bkey_last(w->data), keys->keys, bch_keylist_bytes(keys));
 	w->data->keys += bch_keylist_nkeys(keys);
 
 	ret = &fifo_back(&c->journal.pin);
@@ -780,7 +796,6 @@ int bch_journal_alloc(struct cache_set *c)
 {
 	struct journal *j = &c->journal;
 
-	closure_init_unlocked(&j->io);
 	spin_lock_init(&j->lock);
 	INIT_DELAYED_WORK(&j->work, journal_write_work);
 
diff --git a/drivers/md/bcache/journal.h b/drivers/md/bcache/journal.h
index a6472fda94b2..9180c4465075 100644
--- a/drivers/md/bcache/journal.h
+++ b/drivers/md/bcache/journal.h
@@ -104,6 +104,7 @@ struct journal {
 	/* used when waiting because the journal was full */
 	struct closure_waitlist	wait;
 	struct closure		io;
+	int			io_in_flight;
 	struct delayed_work	work;
 
 	/* Number of blocks free in the bucket(s) we're currently writing to */
diff --git a/drivers/md/bcache/movinggc.c b/drivers/md/bcache/movinggc.c
index f2f0998c4a91..9eb60d102de8 100644
--- a/drivers/md/bcache/movinggc.c
+++ b/drivers/md/bcache/movinggc.c
@@ -86,7 +86,7 @@ static void moving_init(struct moving_io *io)
 	bio_get(bio);
 	bio_set_prio(bio, IOPRIO_PRIO_VALUE(IOPRIO_CLASS_IDLE, 0));
 
-	bio->bi_size		= KEY_SIZE(&io->w->key) << 9;
+	bio->bi_iter.bi_size	= KEY_SIZE(&io->w->key) << 9;
 	bio->bi_max_vecs	= DIV_ROUND_UP(KEY_SIZE(&io->w->key),
 					       PAGE_SECTORS);
 	bio->bi_private		= &io->cl;
@@ -102,7 +102,7 @@ static void write_moving(struct closure *cl)
 	if (!op->error) {
 		moving_init(io);
 
-		io->bio.bio.bi_sector = KEY_START(&io->w->key);
+		io->bio.bio.bi_iter.bi_sector = KEY_START(&io->w->key);
 		op->write_prio		= 1;
 		op->bio			= &io->bio.bio;
 
@@ -211,7 +211,7 @@ void bch_moving_gc(struct cache_set *c)
 	for_each_cache(ca, c, i) {
 		unsigned sectors_to_move = 0;
 		unsigned reserve_sectors = ca->sb.bucket_size *
-			min(fifo_used(&ca->free), ca->free.size / 2);
+			fifo_used(&ca->free[RESERVE_MOVINGGC]);
 
 		ca->heap.used = 0;
 
diff --git a/drivers/md/bcache/request.c b/drivers/md/bcache/request.c
index 61bcfc21d2a0..72cd213f213f 100644
--- a/drivers/md/bcache/request.c
+++ b/drivers/md/bcache/request.c
@@ -197,14 +197,14 @@ static bool verify(struct cached_dev *dc, struct bio *bio)
 
 static void bio_csum(struct bio *bio, struct bkey *k)
 {
-	struct bio_vec *bv;
+	struct bio_vec bv;
+	struct bvec_iter iter;
 	uint64_t csum = 0;
-	int i;
 
-	bio_for_each_segment(bv, bio, i) {
-		void *d = kmap(bv->bv_page) + bv->bv_offset;
-		csum = bch_crc64_update(csum, d, bv->bv_len);
-		kunmap(bv->bv_page);
+	bio_for_each_segment(bv, bio, iter) {
+		void *d = kmap(bv.bv_page) + bv.bv_offset;
+		csum = bch_crc64_update(csum, d, bv.bv_len);
+		kunmap(bv.bv_page);
 	}
 
 	k->ptr[KEY_PTRS(k)] = csum & (~0ULL >> 1);
@@ -254,26 +254,44 @@ static void bch_data_insert_keys(struct closure *cl)
 	closure_return(cl);
 }
 
+static int bch_keylist_realloc(struct keylist *l, unsigned u64s,
+			       struct cache_set *c)
+{
+	size_t oldsize = bch_keylist_nkeys(l);
+	size_t newsize = oldsize + u64s;
+
+	/*
+	 * The journalling code doesn't handle the case where the keys to insert
+	 * is bigger than an empty write: If we just return -ENOMEM here,
+	 * bio_insert() and bio_invalidate() will insert the keys created so far
+	 * and finish the rest when the keylist is empty.
+	 */
+	if (newsize * sizeof(uint64_t) > block_bytes(c) - sizeof(struct jset))
+		return -ENOMEM;
+
+	return __bch_keylist_realloc(l, u64s);
+}
+
 static void bch_data_invalidate(struct closure *cl)
 {
 	struct data_insert_op *op = container_of(cl, struct data_insert_op, cl);
 	struct bio *bio = op->bio;
 
 	pr_debug("invalidating %i sectors from %llu",
-		 bio_sectors(bio), (uint64_t) bio->bi_sector);
+		 bio_sectors(bio), (uint64_t) bio->bi_iter.bi_sector);
 
 	while (bio_sectors(bio)) {
 		unsigned sectors = min(bio_sectors(bio),
 				       1U << (KEY_SIZE_BITS - 1));
 
-		if (bch_keylist_realloc(&op->insert_keys, 0, op->c))
+		if (bch_keylist_realloc(&op->insert_keys, 2, op->c))
 			goto out;
 
-		bio->bi_sector	+= sectors;
-		bio->bi_size	-= sectors << 9;
+		bio->bi_iter.bi_sector	+= sectors;
+		bio->bi_iter.bi_size	-= sectors << 9;
 
 		bch_keylist_add(&op->insert_keys,
-				&KEY(op->inode, bio->bi_sector, sectors));
+				&KEY(op->inode, bio->bi_iter.bi_sector, sectors));
 	}
 
 	op->insert_data_done = true;
@@ -356,21 +374,21 @@ static void bch_data_insert_start(struct closure *cl)
 
 		/* 1 for the device pointer and 1 for the chksum */
 		if (bch_keylist_realloc(&op->insert_keys,
-					1 + (op->csum ? 1 : 0),
+					3 + (op->csum ? 1 : 0),
 					op->c))
 			continue_at(cl, bch_data_insert_keys, bcache_wq);
 
 		k = op->insert_keys.top;
 		bkey_init(k);
 		SET_KEY_INODE(k, op->inode);
-		SET_KEY_OFFSET(k, bio->bi_sector);
+		SET_KEY_OFFSET(k, bio->bi_iter.bi_sector);
 
 		if (!bch_alloc_sectors(op->c, k, bio_sectors(bio),
 				       op->write_point, op->write_prio,
 				       op->writeback))
 			goto err;
 
-		n = bch_bio_split(bio, KEY_SIZE(k), GFP_NOIO, split);
+		n = bio_next_split(bio, KEY_SIZE(k), GFP_NOIO, split);
 
 		n->bi_end_io	= bch_data_insert_endio;
 		n->bi_private	= cl;
@@ -521,7 +539,7 @@ static bool check_should_bypass(struct cached_dev *dc, struct bio *bio)
 	     (bio->bi_rw & REQ_WRITE)))
 		goto skip;
 
-	if (bio->bi_sector & (c->sb.block_size - 1) ||
+	if (bio->bi_iter.bi_sector & (c->sb.block_size - 1) ||
 	    bio_sectors(bio) & (c->sb.block_size - 1)) {
 		pr_debug("skipping unaligned io");
 		goto skip;
@@ -545,8 +563,8 @@ static bool check_should_bypass(struct cached_dev *dc, struct bio *bio)
 
 	spin_lock(&dc->io_lock);
 
-	hlist_for_each_entry(i, iohash(dc, bio->bi_sector), hash)
-		if (i->last == bio->bi_sector &&
+	hlist_for_each_entry(i, iohash(dc, bio->bi_iter.bi_sector), hash)
+		if (i->last == bio->bi_iter.bi_sector &&
 		    time_before(jiffies, i->jiffies))
 			goto found;
 
@@ -555,8 +573,8 @@ static bool check_should_bypass(struct cached_dev *dc, struct bio *bio)
 	add_sequential(task);
 	i->sequential = 0;
 found:
-	if (i->sequential + bio->bi_size > i->sequential)
-		i->sequential	+= bio->bi_size;
+	if (i->sequential + bio->bi_iter.bi_size > i->sequential)
+		i->sequential	+= bio->bi_iter.bi_size;
 
 	i->last			 = bio_end_sector(bio);
 	i->jiffies		 = jiffies + msecs_to_jiffies(5000);
@@ -596,16 +614,13 @@ struct search {
 	/* Stack frame for bio_complete */
 	struct closure		cl;
 
-	struct bcache_device	*d;
-
 	struct bbio		bio;
 	struct bio		*orig_bio;
 	struct bio		*cache_miss;
+	struct bcache_device	*d;
 
 	unsigned		insert_bio_sectors;
-
 	unsigned		recoverable:1;
-	unsigned		unaligned_bvec:1;
 	unsigned		write:1;
 	unsigned		read_dirty_data:1;
 
@@ -630,7 +645,8 @@ static void bch_cache_read_endio(struct bio *bio, int error)
 
 	if (error)
 		s->iop.error = error;
-	else if (ptr_stale(s->iop.c, &b->key, 0)) {
+	else if (!KEY_DIRTY(&b->key) &&
+		 ptr_stale(s->iop.c, &b->key, 0)) {
 		atomic_long_inc(&s->iop.c->cache_read_races);
 		s->iop.error = -EINTR;
 	}
@@ -649,15 +665,15 @@ static int cache_lookup_fn(struct btree_op *op, struct btree *b, struct bkey *k)
 	struct bkey *bio_key;
 	unsigned ptr;
 
-	if (bkey_cmp(k, &KEY(s->iop.inode, bio->bi_sector, 0)) <= 0)
+	if (bkey_cmp(k, &KEY(s->iop.inode, bio->bi_iter.bi_sector, 0)) <= 0)
 		return MAP_CONTINUE;
 
 	if (KEY_INODE(k) != s->iop.inode ||
-	    KEY_START(k) > bio->bi_sector) {
+	    KEY_START(k) > bio->bi_iter.bi_sector) {
 		unsigned bio_sectors = bio_sectors(bio);
 		unsigned sectors = KEY_INODE(k) == s->iop.inode
 			? min_t(uint64_t, INT_MAX,
-				KEY_START(k) - bio->bi_sector)
+				KEY_START(k) - bio->bi_iter.bi_sector)
 			: INT_MAX;
 
 		int ret = s->d->cache_miss(b, s, bio, sectors);
@@ -679,14 +695,14 @@ static int cache_lookup_fn(struct btree_op *op, struct btree *b, struct bkey *k)
 	if (KEY_DIRTY(k))
 		s->read_dirty_data = true;
 
-	n = bch_bio_split(bio, min_t(uint64_t, INT_MAX,
-				     KEY_OFFSET(k) - bio->bi_sector),
-			  GFP_NOIO, s->d->bio_split);
+	n = bio_next_split(bio, min_t(uint64_t, INT_MAX,
+				      KEY_OFFSET(k) - bio->bi_iter.bi_sector),
+			   GFP_NOIO, s->d->bio_split);
 
 	bio_key = &container_of(n, struct bbio, bio)->key;
 	bch_bkey_copy_single_ptr(bio_key, k, ptr);
 
-	bch_cut_front(&KEY(s->iop.inode, n->bi_sector, 0), bio_key);
+	bch_cut_front(&KEY(s->iop.inode, n->bi_iter.bi_sector, 0), bio_key);
 	bch_cut_back(&KEY(s->iop.inode, bio_end_sector(n), 0), bio_key);
 
 	n->bi_end_io	= bch_cache_read_endio;
@@ -711,10 +727,13 @@ static void cache_lookup(struct closure *cl)
 {
 	struct search *s = container_of(cl, struct search, iop.cl);
 	struct bio *bio = &s->bio.bio;
+	int ret;
 
-	int ret = bch_btree_map_keys(&s->op, s->iop.c,
-				     &KEY(s->iop.inode, bio->bi_sector, 0),
-				     cache_lookup_fn, MAP_END_KEY);
+	bch_btree_op_init(&s->op, -1);
+
+	ret = bch_btree_map_keys(&s->op, s->iop.c,
+				 &KEY(s->iop.inode, bio->bi_iter.bi_sector, 0),
+				 cache_lookup_fn, MAP_END_KEY);
 	if (ret == -EAGAIN)
 		continue_at(cl, cache_lookup, bcache_wq);
 
@@ -755,13 +774,15 @@ static void bio_complete(struct search *s)
 	}
 }
 
-static void do_bio_hook(struct search *s)
+static void do_bio_hook(struct search *s, struct bio *orig_bio)
 {
 	struct bio *bio = &s->bio.bio;
-	memcpy(bio, s->orig_bio, sizeof(struct bio));
 
+	bio_init(bio);
+	__bio_clone_fast(bio, orig_bio);
 	bio->bi_end_io		= request_endio;
 	bio->bi_private		= &s->cl;
+
 	atomic_set(&bio->bi_cnt, 3);
 }
 
@@ -773,43 +794,36 @@ static void search_free(struct closure *cl)
 	if (s->iop.bio)
 		bio_put(s->iop.bio);
 
-	if (s->unaligned_bvec)
-		mempool_free(s->bio.bio.bi_io_vec, s->d->unaligned_bvec);
-
 	closure_debug_destroy(cl);
 	mempool_free(s, s->d->c->search);
 }
 
-static struct search *search_alloc(struct bio *bio, struct bcache_device *d)
+static inline struct search *search_alloc(struct bio *bio,
+					  struct bcache_device *d)
 {
 	struct search *s;
-	struct bio_vec *bv;
 
 	s = mempool_alloc(d->c->search, GFP_NOIO);
-	memset(s, 0, offsetof(struct search, iop.insert_keys));
 
-	__closure_init(&s->cl, NULL);
+	closure_init(&s->cl, NULL);
+	do_bio_hook(s, bio);
 
-	s->iop.inode		= d->id;
-	s->iop.c		= d->c;
-	s->d			= d;
-	s->op.lock		= -1;
-	s->iop.write_point	= hash_long((unsigned long) current, 16);
 	s->orig_bio		= bio;
-	s->write		= (bio->bi_rw & REQ_WRITE) != 0;
-	s->iop.flush_journal	= (bio->bi_rw & (REQ_FLUSH|REQ_FUA)) != 0;
+	s->cache_miss		= NULL;
+	s->d			= d;
 	s->recoverable		= 1;
+	s->write		= (bio->bi_rw & REQ_WRITE) != 0;
+	s->read_dirty_data	= 0;
 	s->start_time		= jiffies;
-	do_bio_hook(s);
 
-	if (bio->bi_size != bio_segments(bio) * PAGE_SIZE) {
-		bv = mempool_alloc(d->unaligned_bvec, GFP_NOIO);
-		memcpy(bv, bio_iovec(bio),
-		       sizeof(struct bio_vec) * bio_segments(bio));
-
-		s->bio.bio.bi_io_vec	= bv;
-		s->unaligned_bvec	= 1;
-	}
+	s->iop.c		= d->c;
+	s->iop.bio		= NULL;
+	s->iop.inode		= d->id;
+	s->iop.write_point	= hash_long((unsigned long) current, 16);
+	s->iop.write_prio	= 0;
+	s->iop.error		= 0;
+	s->iop.flags		= 0;
+	s->iop.flush_journal	= (bio->bi_rw & (REQ_FLUSH|REQ_FUA)) != 0;
 
 	return s;
 }
@@ -849,26 +863,13 @@ static void cached_dev_read_error(struct closure *cl)
 {
 	struct search *s = container_of(cl, struct search, cl);
 	struct bio *bio = &s->bio.bio;
-	struct bio_vec *bv;
-	int i;
 
 	if (s->recoverable) {
 		/* Retry from the backing device: */
 		trace_bcache_read_retry(s->orig_bio);
 
 		s->iop.error = 0;
-		bv = s->bio.bio.bi_io_vec;
-		do_bio_hook(s);
-		s->bio.bio.bi_io_vec = bv;
-
-		if (!s->unaligned_bvec)
-			bio_for_each_segment(bv, s->orig_bio, i)
-				bv->bv_offset = 0, bv->bv_len = PAGE_SIZE;
-		else
-			memcpy(s->bio.bio.bi_io_vec,
-			       bio_iovec(s->orig_bio),
-			       sizeof(struct bio_vec) *
-			       bio_segments(s->orig_bio));
+		do_bio_hook(s, s->orig_bio);
 
 		/* XXX: invalidate cache */
 
@@ -893,9 +894,9 @@ static void cached_dev_read_done(struct closure *cl)
 
 	if (s->iop.bio) {
 		bio_reset(s->iop.bio);
-		s->iop.bio->bi_sector = s->cache_miss->bi_sector;
+		s->iop.bio->bi_iter.bi_sector = s->cache_miss->bi_iter.bi_sector;
 		s->iop.bio->bi_bdev = s->cache_miss->bi_bdev;
-		s->iop.bio->bi_size = s->insert_bio_sectors << 9;
+		s->iop.bio->bi_iter.bi_size = s->insert_bio_sectors << 9;
 		bch_bio_map(s->iop.bio, NULL);
 
 		bio_copy_data(s->cache_miss, s->iop.bio);
@@ -904,8 +905,7 @@ static void cached_dev_read_done(struct closure *cl)
 		s->cache_miss = NULL;
 	}
 
-	if (verify(dc, &s->bio.bio) && s->recoverable &&
-	    !s->unaligned_bvec && !s->read_dirty_data)
+	if (verify(dc, &s->bio.bio) && s->recoverable && !s->read_dirty_data)
 		bch_data_verify(dc, s->orig_bio);
 
 	bio_complete(s);
@@ -945,7 +945,7 @@ static int cached_dev_cache_miss(struct btree *b, struct search *s,
 	struct bio *miss, *cache_bio;
 
 	if (s->cache_miss || s->iop.bypass) {
-		miss = bch_bio_split(bio, sectors, GFP_NOIO, s->d->bio_split);
+		miss = bio_next_split(bio, sectors, GFP_NOIO, s->d->bio_split);
 		ret = miss == bio ? MAP_DONE : MAP_CONTINUE;
 		goto out_submit;
 	}
@@ -959,7 +959,7 @@ static int cached_dev_cache_miss(struct btree *b, struct search *s,
 	s->insert_bio_sectors = min(sectors, bio_sectors(bio) + reada);
 
 	s->iop.replace_key = KEY(s->iop.inode,
-				 bio->bi_sector + s->insert_bio_sectors,
+				 bio->bi_iter.bi_sector + s->insert_bio_sectors,
 				 s->insert_bio_sectors);
 
 	ret = bch_btree_insert_check_key(b, &s->op, &s->iop.replace_key);
@@ -968,7 +968,7 @@ static int cached_dev_cache_miss(struct btree *b, struct search *s,
 
 	s->iop.replace = true;
 
-	miss = bch_bio_split(bio, sectors, GFP_NOIO, s->d->bio_split);
+	miss = bio_next_split(bio, sectors, GFP_NOIO, s->d->bio_split);
 
 	/* btree_search_recurse()'s btree iterator is no good anymore */
 	ret = miss == bio ? MAP_DONE : -EINTR;
@@ -979,9 +979,9 @@ static int cached_dev_cache_miss(struct btree *b, struct search *s,
 	if (!cache_bio)
 		goto out_submit;
 
-	cache_bio->bi_sector	= miss->bi_sector;
-	cache_bio->bi_bdev	= miss->bi_bdev;
-	cache_bio->bi_size	= s->insert_bio_sectors << 9;
+	cache_bio->bi_iter.bi_sector	= miss->bi_iter.bi_sector;
+	cache_bio->bi_bdev		= miss->bi_bdev;
+	cache_bio->bi_iter.bi_size	= s->insert_bio_sectors << 9;
 
 	cache_bio->bi_end_io	= request_endio;
 	cache_bio->bi_private	= &s->cl;
@@ -1031,7 +1031,7 @@ static void cached_dev_write(struct cached_dev *dc, struct search *s)
 {
 	struct closure *cl = &s->cl;
 	struct bio *bio = &s->bio.bio;
-	struct bkey start = KEY(dc->disk.id, bio->bi_sector, 0);
+	struct bkey start = KEY(dc->disk.id, bio->bi_iter.bi_sector, 0);
 	struct bkey end = KEY(dc->disk.id, bio_end_sector(bio), 0);
 
 	bch_keybuf_check_overlapping(&s->iop.c->moving_gc_keys, &start, &end);
@@ -1087,8 +1087,7 @@ static void cached_dev_write(struct cached_dev *dc, struct search *s)
 			closure_bio_submit(flush, cl, s->d);
 		}
 	} else {
-		s->iop.bio = bio_clone_bioset(bio, GFP_NOIO,
-					      dc->disk.bio_split);
+		s->iop.bio = bio_clone_fast(bio, GFP_NOIO, dc->disk.bio_split);
 
 		closure_bio_submit(bio, cl, s->d);
 	}
@@ -1126,13 +1125,13 @@ static void cached_dev_make_request(struct request_queue *q, struct bio *bio)
 	part_stat_unlock();
 
 	bio->bi_bdev = dc->bdev;
-	bio->bi_sector += dc->sb.data_offset;
+	bio->bi_iter.bi_sector += dc->sb.data_offset;
 
 	if (cached_dev_get(dc)) {
 		s = search_alloc(bio, d);
 		trace_bcache_request_start(s->d, bio);
 
-		if (!bio->bi_size) {
+		if (!bio->bi_iter.bi_size) {
 			/*
 			 * can't call bch_journal_meta from under
 			 * generic_make_request
@@ -1204,24 +1203,24 @@ void bch_cached_dev_request_init(struct cached_dev *dc)
 static int flash_dev_cache_miss(struct btree *b, struct search *s,
 				struct bio *bio, unsigned sectors)
 {
-	struct bio_vec *bv;
-	int i;
+	struct bio_vec bv;
+	struct bvec_iter iter;
 
 	/* Zero fill bio */
 
-	bio_for_each_segment(bv, bio, i) {
-		unsigned j = min(bv->bv_len >> 9, sectors);
+	bio_for_each_segment(bv, bio, iter) {
+		unsigned j = min(bv.bv_len >> 9, sectors);
 
-		void *p = kmap(bv->bv_page);
-		memset(p + bv->bv_offset, 0, j << 9);
-		kunmap(bv->bv_page);
+		void *p = kmap(bv.bv_page);
+		memset(p + bv.bv_offset, 0, j << 9);
+		kunmap(bv.bv_page);
 
 		sectors	-= j;
 	}
 
-	bio_advance(bio, min(sectors << 9, bio->bi_size));
+	bio_advance(bio, min(sectors << 9, bio->bi_iter.bi_size));
 
-	if (!bio->bi_size)
+	if (!bio->bi_iter.bi_size)
 		return MAP_DONE;
 
 	return MAP_CONTINUE;
@@ -1255,7 +1254,7 @@ static void flash_dev_make_request(struct request_queue *q, struct bio *bio)
 
 	trace_bcache_request_start(s->d, bio);
 
-	if (!bio->bi_size) {
+	if (!bio->bi_iter.bi_size) {
 		/*
 		 * can't call bch_journal_meta from under
 		 * generic_make_request
@@ -1265,7 +1264,7 @@ static void flash_dev_make_request(struct request_queue *q, struct bio *bio)
 				      bcache_wq);
 	} else if (rw) {
 		bch_keybuf_check_overlapping(&s->iop.c->moving_gc_keys,
-					&KEY(d->id, bio->bi_sector, 0),
+					&KEY(d->id, bio->bi_iter.bi_sector, 0),
 					&KEY(d->id, bio_end_sector(bio), 0));
 
 		s->iop.bypass		= (bio->bi_rw & REQ_DISCARD) != 0;
diff --git a/drivers/md/bcache/request.h b/drivers/md/bcache/request.h
index 2cd65bf073c2..39f21dbedc38 100644
--- a/drivers/md/bcache/request.h
+++ b/drivers/md/bcache/request.h
@@ -13,17 +13,22 @@ struct data_insert_op {
 	uint16_t		write_prio;
 	short			error;
 
-	unsigned		bypass:1;
-	unsigned		writeback:1;
-	unsigned		flush_journal:1;
-	unsigned		csum:1;
+	union {
+		uint16_t	flags;
 
-	unsigned		replace:1;
-	unsigned		replace_collision:1;
+	struct {
+		unsigned	bypass:1;
+		unsigned	writeback:1;
+		unsigned	flush_journal:1;
+		unsigned	csum:1;
 
-	unsigned		insert_data_done:1;
+		unsigned	replace:1;
+		unsigned	replace_collision:1;
+
+		unsigned	insert_data_done:1;
+	};
+	};
 
-	/* Anything past this point won't get zeroed in search_alloc() */
 	struct keylist		insert_keys;
 	BKEY_PADDED(replace_key);
 };
diff --git a/drivers/md/bcache/super.c b/drivers/md/bcache/super.c
index c57bfa071a57..24a3a1546caa 100644
--- a/drivers/md/bcache/super.c
+++ b/drivers/md/bcache/super.c
@@ -9,6 +9,7 @@
 #include "bcache.h"
 #include "btree.h"
 #include "debug.h"
+#include "extents.h"
 #include "request.h"
 #include "writeback.h"
 
@@ -225,7 +226,7 @@ static void write_bdev_super_endio(struct bio *bio, int error)
 	struct cached_dev *dc = bio->bi_private;
 	/* XXX: error checking */
 
-	closure_put(&dc->sb_write.cl);
+	closure_put(&dc->sb_write);
 }
 
 static void __write_super(struct cache_sb *sb, struct bio *bio)
@@ -233,9 +234,9 @@ static void __write_super(struct cache_sb *sb, struct bio *bio)
 	struct cache_sb *out = page_address(bio->bi_io_vec[0].bv_page);
 	unsigned i;
 
-	bio->bi_sector	= SB_SECTOR;
-	bio->bi_rw	= REQ_SYNC|REQ_META;
-	bio->bi_size	= SB_SIZE;
+	bio->bi_iter.bi_sector	= SB_SECTOR;
+	bio->bi_rw		= REQ_SYNC|REQ_META;
+	bio->bi_iter.bi_size	= SB_SIZE;
 	bch_bio_map(bio, NULL);
 
 	out->offset		= cpu_to_le64(sb->offset);
@@ -263,12 +264,20 @@ static void __write_super(struct cache_sb *sb, struct bio *bio)
 	submit_bio(REQ_WRITE, bio);
 }
 
+static void bch_write_bdev_super_unlock(struct closure *cl)
+{
+	struct cached_dev *dc = container_of(cl, struct cached_dev, sb_write);
+
+	up(&dc->sb_write_mutex);
+}
+
 void bch_write_bdev_super(struct cached_dev *dc, struct closure *parent)
 {
-	struct closure *cl = &dc->sb_write.cl;
+	struct closure *cl = &dc->sb_write;
 	struct bio *bio = &dc->sb_bio;
 
-	closure_lock(&dc->sb_write, parent);
+	down(&dc->sb_write_mutex);
+	closure_init(cl, parent);
 
 	bio_reset(bio);
 	bio->bi_bdev	= dc->bdev;
@@ -278,7 +287,7 @@ void bch_write_bdev_super(struct cached_dev *dc, struct closure *parent)
 	closure_get(cl);
 	__write_super(&dc->sb, bio);
 
-	closure_return(cl);
+	closure_return_with_destructor(cl, bch_write_bdev_super_unlock);
 }
 
 static void write_super_endio(struct bio *bio, int error)
@@ -286,16 +295,24 @@ static void write_super_endio(struct bio *bio, int error)
 	struct cache *ca = bio->bi_private;
 
 	bch_count_io_errors(ca, error, "writing superblock");
-	closure_put(&ca->set->sb_write.cl);
+	closure_put(&ca->set->sb_write);
+}
+
+static void bcache_write_super_unlock(struct closure *cl)
+{
+	struct cache_set *c = container_of(cl, struct cache_set, sb_write);
+
+	up(&c->sb_write_mutex);
 }
 
 void bcache_write_super(struct cache_set *c)
 {
-	struct closure *cl = &c->sb_write.cl;
+	struct closure *cl = &c->sb_write;
 	struct cache *ca;
 	unsigned i;
 
-	closure_lock(&c->sb_write, &c->cl);
+	down(&c->sb_write_mutex);
+	closure_init(cl, &c->cl);
 
 	c->sb.seq++;
 
@@ -317,7 +334,7 @@ void bcache_write_super(struct cache_set *c)
 		__write_super(&ca->sb, bio);
 	}
 
-	closure_return(cl);
+	closure_return_with_destructor(cl, bcache_write_super_unlock);
 }
 
 /* UUID io */
@@ -325,29 +342,37 @@ void bcache_write_super(struct cache_set *c)
 static void uuid_endio(struct bio *bio, int error)
 {
 	struct closure *cl = bio->bi_private;
-	struct cache_set *c = container_of(cl, struct cache_set, uuid_write.cl);
+	struct cache_set *c = container_of(cl, struct cache_set, uuid_write);
 
 	cache_set_err_on(error, c, "accessing uuids");
 	bch_bbio_free(bio, c);
 	closure_put(cl);
 }
 
+static void uuid_io_unlock(struct closure *cl)
+{
+	struct cache_set *c = container_of(cl, struct cache_set, uuid_write);
+
+	up(&c->uuid_write_mutex);
+}
+
 static void uuid_io(struct cache_set *c, unsigned long rw,
 		    struct bkey *k, struct closure *parent)
 {
-	struct closure *cl = &c->uuid_write.cl;
+	struct closure *cl = &c->uuid_write;
 	struct uuid_entry *u;
 	unsigned i;
 	char buf[80];
 
 	BUG_ON(!parent);
-	closure_lock(&c->uuid_write, parent);
+	down(&c->uuid_write_mutex);
+	closure_init(cl, parent);
 
 	for (i = 0; i < KEY_PTRS(k); i++) {
 		struct bio *bio = bch_bbio_alloc(c);
 
 		bio->bi_rw	= REQ_SYNC|REQ_META|rw;
-		bio->bi_size	= KEY_SIZE(k) << 9;
+		bio->bi_iter.bi_size = KEY_SIZE(k) << 9;
 
 		bio->bi_end_io	= uuid_endio;
 		bio->bi_private = cl;
@@ -359,7 +384,7 @@ static void uuid_io(struct cache_set *c, unsigned long rw,
 			break;
 	}
 
-	bch_bkey_to_text(buf, sizeof(buf), k);
+	bch_extent_to_text(buf, sizeof(buf), k);
 	pr_debug("%s UUIDs at %s", rw & REQ_WRITE ? "wrote" : "read", buf);
 
 	for (u = c->uuids; u < c->uuids + c->nr_uuids; u++)
@@ -368,14 +393,14 @@ static void uuid_io(struct cache_set *c, unsigned long rw,
 				 u - c->uuids, u->uuid, u->label,
 				 u->first_reg, u->last_reg, u->invalidated);
 
-	closure_return(cl);
+	closure_return_with_destructor(cl, uuid_io_unlock);
 }
 
 static char *uuid_read(struct cache_set *c, struct jset *j, struct closure *cl)
 {
 	struct bkey *k = &j->uuid_bucket;
 
-	if (bch_btree_ptr_invalid(c, k))
+	if (__bch_btree_ptr_invalid(c, k))
 		return "bad uuid pointer";
 
 	bkey_copy(&c->uuid_bucket, k);
@@ -420,7 +445,7 @@ static int __uuid_write(struct cache_set *c)
 
 	lockdep_assert_held(&bch_register_lock);
 
-	if (bch_bucket_alloc_set(c, WATERMARK_METADATA, &k.key, 1, true))
+	if (bch_bucket_alloc_set(c, RESERVE_BTREE, &k.key, 1, true))
 		return 1;
 
 	SET_KEY_SIZE(&k.key, c->sb.bucket_size);
@@ -503,10 +528,10 @@ static void prio_io(struct cache *ca, uint64_t bucket, unsigned long rw)
 
 	closure_init_stack(cl);
 
-	bio->bi_sector	= bucket * ca->sb.bucket_size;
-	bio->bi_bdev	= ca->bdev;
-	bio->bi_rw	= REQ_SYNC|REQ_META|rw;
-	bio->bi_size	= bucket_bytes(ca);
+	bio->bi_iter.bi_sector	= bucket * ca->sb.bucket_size;
+	bio->bi_bdev		= ca->bdev;
+	bio->bi_rw		= REQ_SYNC|REQ_META|rw;
+	bio->bi_iter.bi_size	= bucket_bytes(ca);
 
 	bio->bi_end_io	= prio_endio;
 	bio->bi_private = ca;
@@ -538,8 +563,8 @@ void bch_prio_write(struct cache *ca)
 	atomic_long_add(ca->sb.bucket_size * prio_buckets(ca),
 			&ca->meta_sectors_written);
 
-	pr_debug("free %zu, free_inc %zu, unused %zu", fifo_used(&ca->free),
-		 fifo_used(&ca->free_inc), fifo_used(&ca->unused));
+	//pr_debug("free %zu, free_inc %zu, unused %zu", fifo_used(&ca->free),
+	//	 fifo_used(&ca->free_inc), fifo_used(&ca->unused));
 
 	for (i = prio_buckets(ca) - 1; i >= 0; --i) {
 		long bucket;
@@ -558,7 +583,7 @@ void bch_prio_write(struct cache *ca)
 		p->magic	= pset_magic(&ca->sb);
 		p->csum		= bch_crc64(&p->magic, bucket_bytes(ca) - 8);
 
-		bucket = bch_bucket_alloc(ca, WATERMARK_PRIO, true);
+		bucket = bch_bucket_alloc(ca, RESERVE_PRIO, true);
 		BUG_ON(bucket == -1);
 
 		mutex_unlock(&ca->set->bucket_lock);
@@ -739,8 +764,6 @@ static void bcache_device_free(struct bcache_device *d)
 	}
 
 	bio_split_pool_free(&d->bio_split_hook);
-	if (d->unaligned_bvec)
-		mempool_destroy(d->unaligned_bvec);
 	if (d->bio_split)
 		bioset_free(d->bio_split);
 	if (is_vmalloc_addr(d->full_dirty_stripes))
@@ -793,8 +816,6 @@ static int bcache_device_init(struct bcache_device *d, unsigned block_size,
 		return minor;
 
 	if (!(d->bio_split = bioset_create(4, offsetof(struct bbio, bio))) ||
-	    !(d->unaligned_bvec = mempool_create_kmalloc_pool(1,
-				sizeof(struct bio_vec) * BIO_MAX_PAGES)) ||
 	    bio_split_pool_init(&d->bio_split_hook) ||
 	    !(d->disk = alloc_disk(1))) {
 		ida_simple_remove(&bcache_minor, minor);
@@ -1102,7 +1123,7 @@ static int cached_dev_init(struct cached_dev *dc, unsigned block_size)
 	set_closure_fn(&dc->disk.cl, cached_dev_flush, system_wq);
 	kobject_init(&dc->disk.kobj, &bch_cached_dev_ktype);
 	INIT_WORK(&dc->detach, cached_dev_detach_finish);
-	closure_init_unlocked(&dc->sb_write);
+	sema_init(&dc->sb_write_mutex, 1);
 	INIT_LIST_HEAD(&dc->io_lru);
 	spin_lock_init(&dc->io_lock);
 	bch_cache_accounting_init(&dc->accounting, &dc->disk.cl);
@@ -1114,6 +1135,12 @@ static int cached_dev_init(struct cached_dev *dc, unsigned block_size)
 		hlist_add_head(&io->hash, dc->io_hash + RECENT_IO);
 	}
 
+	dc->disk.stripe_size = q->limits.io_opt >> 9;
+
+	if (dc->disk.stripe_size)
+		dc->partial_stripes_expensive =
+			q->limits.raid_partial_stripes_expensive;
+
 	ret = bcache_device_init(&dc->disk, block_size,
 			 dc->bdev->bd_part->nr_sects - dc->sb.data_offset);
 	if (ret)
@@ -1325,8 +1352,8 @@ static void cache_set_free(struct closure *cl)
 		if (ca)
 			kobject_put(&ca->kobj);
 
+	bch_bset_sort_state_free(&c->sort);
 	free_pages((unsigned long) c->uuids, ilog2(bucket_pages(c)));
-	free_pages((unsigned long) c->sort, ilog2(bucket_pages(c)));
 
 	if (c->bio_split)
 		bioset_free(c->bio_split);
@@ -1451,21 +1478,17 @@ struct cache_set *bch_cache_set_alloc(struct cache_sb *sb)
 	c->block_bits		= ilog2(sb->block_size);
 	c->nr_uuids		= bucket_bytes(c) / sizeof(struct uuid_entry);
 
-	c->btree_pages		= c->sb.bucket_size / PAGE_SECTORS;
+	c->btree_pages		= bucket_pages(c);
 	if (c->btree_pages > BTREE_MAX_PAGES)
 		c->btree_pages = max_t(int, c->btree_pages / 4,
 				       BTREE_MAX_PAGES);
 
-	c->sort_crit_factor = int_sqrt(c->btree_pages);
-
-	closure_init_unlocked(&c->sb_write);
+	sema_init(&c->sb_write_mutex, 1);
 	mutex_init(&c->bucket_lock);
 	init_waitqueue_head(&c->try_wait);
 	init_waitqueue_head(&c->bucket_wait);
-	closure_init_unlocked(&c->uuid_write);
-	mutex_init(&c->sort_lock);
+	sema_init(&c->uuid_write_mutex, 1);
 
-	spin_lock_init(&c->sort_time.lock);
 	spin_lock_init(&c->btree_gc_time.lock);
 	spin_lock_init(&c->btree_split_time.lock);
 	spin_lock_init(&c->btree_read_time.lock);
@@ -1493,11 +1516,11 @@ struct cache_set *bch_cache_set_alloc(struct cache_sb *sb)
 				bucket_pages(c))) ||
 	    !(c->fill_iter = mempool_create_kmalloc_pool(1, iter_size)) ||
 	    !(c->bio_split = bioset_create(4, offsetof(struct bbio, bio))) ||
-	    !(c->sort = alloc_bucket_pages(GFP_KERNEL, c)) ||
 	    !(c->uuids = alloc_bucket_pages(GFP_KERNEL, c)) ||
 	    bch_journal_alloc(c) ||
 	    bch_btree_cache_alloc(c) ||
-	    bch_open_buckets_alloc(c))
+	    bch_open_buckets_alloc(c) ||
+	    bch_bset_sort_state_init(&c->sort, ilog2(c->btree_pages)))
 		goto err;
 
 	c->congested_read_threshold_us	= 2000;
@@ -1553,7 +1576,7 @@ static void run_cache_set(struct cache_set *c)
 		k = &j->btree_root;
 
 		err = "bad btree root";
-		if (bch_btree_ptr_invalid(c, k))
+		if (__bch_btree_ptr_invalid(c, k))
 			goto err;
 
 		err = "error reading btree root";
@@ -1747,6 +1770,7 @@ err:
 void bch_cache_release(struct kobject *kobj)
 {
 	struct cache *ca = container_of(kobj, struct cache, kobj);
+	unsigned i;
 
 	if (ca->set)
 		ca->set->cache[ca->sb.nr_this_dev] = NULL;
@@ -1760,7 +1784,9 @@ void bch_cache_release(struct kobject *kobj)
 	free_heap(&ca->heap);
 	free_fifo(&ca->unused);
 	free_fifo(&ca->free_inc);
-	free_fifo(&ca->free);
+
+	for (i = 0; i < RESERVE_NR; i++)
+		free_fifo(&ca->free[i]);
 
 	if (ca->sb_bio.bi_inline_vecs[0].bv_page)
 		put_page(ca->sb_bio.bi_io_vec[0].bv_page);
@@ -1786,10 +1812,12 @@ static int cache_alloc(struct cache_sb *sb, struct cache *ca)
 	ca->journal.bio.bi_max_vecs = 8;
 	ca->journal.bio.bi_io_vec = ca->journal.bio.bi_inline_vecs;
 
-	free = roundup_pow_of_two(ca->sb.nbuckets) >> 9;
-	free = max_t(size_t, free, (prio_buckets(ca) + 8) * 2);
+	free = roundup_pow_of_two(ca->sb.nbuckets) >> 10;
 
-	if (!init_fifo(&ca->free,	free, GFP_KERNEL) ||
+	if (!init_fifo(&ca->free[RESERVE_BTREE], 8, GFP_KERNEL) ||
+	    !init_fifo(&ca->free[RESERVE_PRIO], prio_buckets(ca), GFP_KERNEL) ||
+	    !init_fifo(&ca->free[RESERVE_MOVINGGC], free, GFP_KERNEL) ||
+	    !init_fifo(&ca->free[RESERVE_NONE], free, GFP_KERNEL) ||
 	    !init_fifo(&ca->free_inc,	free << 2, GFP_KERNEL) ||
 	    !init_fifo(&ca->unused,	free << 2, GFP_KERNEL) ||
 	    !init_heap(&ca->heap,	free << 3, GFP_KERNEL) ||
@@ -2034,7 +2062,8 @@ static void bcache_exit(void)
 		kobject_put(bcache_kobj);
 	if (bcache_wq)
 		destroy_workqueue(bcache_wq);
-	unregister_blkdev(bcache_major, "bcache");
+	if (bcache_major)
+		unregister_blkdev(bcache_major, "bcache");
 	unregister_reboot_notifier(&reboot);
 }
 
diff --git a/drivers/md/bcache/sysfs.c b/drivers/md/bcache/sysfs.c
index a1f85612f0b3..c6ab69333a6d 100644
--- a/drivers/md/bcache/sysfs.c
+++ b/drivers/md/bcache/sysfs.c
@@ -102,7 +102,6 @@ rw_attribute(bypass_torture_test);
 rw_attribute(key_merging_disabled);
 rw_attribute(gc_always_rewrite);
 rw_attribute(expensive_debug_checks);
-rw_attribute(freelist_percent);
 rw_attribute(cache_replacement_policy);
 rw_attribute(btree_shrinker_disabled);
 rw_attribute(copy_gc_enabled);
@@ -401,6 +400,48 @@ static struct attribute *bch_flash_dev_files[] = {
 };
 KTYPE(bch_flash_dev);
 
+struct bset_stats_op {
+	struct btree_op op;
+	size_t nodes;
+	struct bset_stats stats;
+};
+
+static int btree_bset_stats(struct btree_op *b_op, struct btree *b)
+{
+	struct bset_stats_op *op = container_of(b_op, struct bset_stats_op, op);
+
+	op->nodes++;
+	bch_btree_keys_stats(&b->keys, &op->stats);
+
+	return MAP_CONTINUE;
+}
+
+int bch_bset_print_stats(struct cache_set *c, char *buf)
+{
+	struct bset_stats_op op;
+	int ret;
+
+	memset(&op, 0, sizeof(op));
+	bch_btree_op_init(&op.op, -1);
+
+	ret = bch_btree_map_nodes(&op.op, c, &ZERO_KEY, btree_bset_stats);
+	if (ret < 0)
+		return ret;
+
+	return snprintf(buf, PAGE_SIZE,
+			"btree nodes:		%zu\n"
+			"written sets:		%zu\n"
+			"unwritten sets:		%zu\n"
+			"written key bytes:	%zu\n"
+			"unwritten key bytes:	%zu\n"
+			"floats:			%zu\n"
+			"failed:			%zu\n",
+			op.nodes,
+			op.stats.sets_written, op.stats.sets_unwritten,
+			op.stats.bytes_written, op.stats.bytes_unwritten,
+			op.stats.floats, op.stats.failed);
+}
+
 SHOW(__bch_cache_set)
 {
 	unsigned root_usage(struct cache_set *c)
@@ -419,7 +460,7 @@ lock_root:
 			rw_lock(false, b, b->level);
 		} while (b != c->root);
 
-		for_each_key_filter(b, k, &iter, bch_ptr_bad)
+		for_each_key_filter(&b->keys, k, &iter, bch_ptr_bad)
 			bytes += bkey_bytes(k);
 
 		rw_unlock(false, b);
@@ -434,7 +475,7 @@ lock_root:
 
 		mutex_lock(&c->bucket_lock);
 		list_for_each_entry(b, &c->btree_cache, list)
-			ret += 1 << (b->page_order + PAGE_SHIFT);
+			ret += 1 << (b->keys.page_order + PAGE_SHIFT);
 
 		mutex_unlock(&c->bucket_lock);
 		return ret;
@@ -491,7 +532,7 @@ lock_root:
 
 	sysfs_print_time_stats(&c->btree_gc_time,	btree_gc, sec, ms);
 	sysfs_print_time_stats(&c->btree_split_time,	btree_split, sec, us);
-	sysfs_print_time_stats(&c->sort_time,		btree_sort, ms, us);
+	sysfs_print_time_stats(&c->sort.time,		btree_sort, ms, us);
 	sysfs_print_time_stats(&c->btree_read_time,	btree_read, ms, us);
 	sysfs_print_time_stats(&c->try_harder_time,	try_harder, ms, us);
 
@@ -711,9 +752,6 @@ SHOW(__bch_cache)
 	sysfs_print(io_errors,
 		    atomic_read(&ca->io_errors) >> IO_ERROR_SHIFT);
 
-	sysfs_print(freelist_percent, ca->free.size * 100 /
-		    ((size_t) ca->sb.nbuckets));
-
 	if (attr == &sysfs_cache_replacement_policy)
 		return bch_snprint_string_list(buf, PAGE_SIZE,
 					       cache_replacement_policies,
@@ -820,32 +858,6 @@ STORE(__bch_cache)
 		}
 	}
 
-	if (attr == &sysfs_freelist_percent) {
-		DECLARE_FIFO(long, free);
-		long i;
-		size_t p = strtoul_or_return(buf);
-
-		p = clamp_t(size_t,
-			    ((size_t) ca->sb.nbuckets * p) / 100,
-			    roundup_pow_of_two(ca->sb.nbuckets) >> 9,
-			    ca->sb.nbuckets / 2);
-
-		if (!init_fifo_exact(&free, p, GFP_KERNEL))
-			return -ENOMEM;
-
-		mutex_lock(&ca->set->bucket_lock);
-
-		fifo_move(&free, &ca->free);
-		fifo_swap(&free, &ca->free);
-
-		mutex_unlock(&ca->set->bucket_lock);
-
-		while (fifo_pop(&free, i))
-			atomic_dec(&ca->buckets[i].pin);
-
-		free_fifo(&free);
-	}
-
 	if (attr == &sysfs_clear_stats) {
 		atomic_long_set(&ca->sectors_written, 0);
 		atomic_long_set(&ca->btree_sectors_written, 0);
@@ -869,7 +881,6 @@ static struct attribute *bch_cache_files[] = {
 	&sysfs_metadata_written,
 	&sysfs_io_errors,
 	&sysfs_clear_stats,
-	&sysfs_freelist_percent,
 	&sysfs_cache_replacement_policy,
 	NULL
 };
diff --git a/drivers/md/bcache/util.c b/drivers/md/bcache/util.c
index bb37618e7664..db3ae4c2b223 100644
--- a/drivers/md/bcache/util.c
+++ b/drivers/md/bcache/util.c
@@ -224,10 +224,10 @@ uint64_t bch_next_delay(struct bch_ratelimit *d, uint64_t done)
 
 void bch_bio_map(struct bio *bio, void *base)
 {
-	size_t size = bio->bi_size;
+	size_t size = bio->bi_iter.bi_size;
 	struct bio_vec *bv = bio->bi_io_vec;
 
-	BUG_ON(!bio->bi_size);
+	BUG_ON(!bio->bi_iter.bi_size);
 	BUG_ON(bio->bi_vcnt);
 
 	bv->bv_offset = base ? ((unsigned long) base) % PAGE_SIZE : 0;
diff --git a/drivers/md/bcache/util.h b/drivers/md/bcache/util.h
index 1030c6020e98..ac7d0d1f70d7 100644
--- a/drivers/md/bcache/util.h
+++ b/drivers/md/bcache/util.h
@@ -2,6 +2,7 @@
 #ifndef _BCACHE_UTIL_H
 #define _BCACHE_UTIL_H
 
+#include <linux/blkdev.h>
 #include <linux/errno.h>
 #include <linux/kernel.h>
 #include <linux/llist.h>
@@ -17,11 +18,13 @@ struct closure;
 
 #ifdef CONFIG_BCACHE_DEBUG
 
+#define EBUG_ON(cond)			BUG_ON(cond)
 #define atomic_dec_bug(v)	BUG_ON(atomic_dec_return(v) < 0)
 #define atomic_inc_bug(v, i)	BUG_ON(atomic_inc_return(v) <= i)
 
 #else /* DEBUG */
 
+#define EBUG_ON(cond)			do { if (cond); } while (0)
 #define atomic_dec_bug(v)	atomic_dec(v)
 #define atomic_inc_bug(v, i)	atomic_inc(v)
 
@@ -391,6 +394,11 @@ struct time_stats {
 
 void bch_time_stats_update(struct time_stats *stats, uint64_t time);
 
+static inline unsigned local_clock_us(void)
+{
+	return local_clock() >> 10;
+}
+
 #define NSEC_PER_ns			1L
 #define NSEC_PER_us			NSEC_PER_USEC
 #define NSEC_PER_ms			NSEC_PER_MSEC
diff --git a/drivers/md/bcache/writeback.c b/drivers/md/bcache/writeback.c
index 6c44fe059c27..f4300e4c0114 100644
--- a/drivers/md/bcache/writeback.c
+++ b/drivers/md/bcache/writeback.c
@@ -111,7 +111,7 @@ static void dirty_init(struct keybuf_key *w)
 	if (!io->dc->writeback_percent)
 		bio_set_prio(bio, IOPRIO_PRIO_VALUE(IOPRIO_CLASS_IDLE, 0));
 
-	bio->bi_size		= KEY_SIZE(&w->key) << 9;
+	bio->bi_iter.bi_size	= KEY_SIZE(&w->key) << 9;
 	bio->bi_max_vecs	= DIV_ROUND_UP(KEY_SIZE(&w->key), PAGE_SECTORS);
 	bio->bi_private		= w;
 	bio->bi_io_vec		= bio->bi_inline_vecs;
@@ -184,7 +184,7 @@ static void write_dirty(struct closure *cl)
 
 	dirty_init(w);
 	io->bio.bi_rw		= WRITE;
-	io->bio.bi_sector	= KEY_START(&w->key);
+	io->bio.bi_iter.bi_sector = KEY_START(&w->key);
 	io->bio.bi_bdev		= io->dc->bdev;
 	io->bio.bi_end_io	= dirty_endio;
 
@@ -253,7 +253,7 @@ static void read_dirty(struct cached_dev *dc)
 		io->dc		= dc;
 
 		dirty_init(w);
-		io->bio.bi_sector	= PTR_OFFSET(&w->key, 0);
+		io->bio.bi_iter.bi_sector = PTR_OFFSET(&w->key, 0);
 		io->bio.bi_bdev		= PTR_CACHE(dc->disk.c,
 						    &w->key, 0)->bdev;
 		io->bio.bi_rw		= READ;
diff --git a/drivers/md/bcache/writeback.h b/drivers/md/bcache/writeback.h
index c9ddcf4614b9..e2f8598937ac 100644
--- a/drivers/md/bcache/writeback.h
+++ b/drivers/md/bcache/writeback.h
@@ -50,7 +50,7 @@ static inline bool should_writeback(struct cached_dev *dc, struct bio *bio,
 		return false;
 
 	if (dc->partial_stripes_expensive &&
-	    bcache_dev_stripe_dirty(dc, bio->bi_sector,
+	    bcache_dev_stripe_dirty(dc, bio->bi_iter.bi_sector,
 				    bio_sectors(bio)))
 		return true;
 
diff --git a/drivers/md/dm-bio-record.h b/drivers/md/dm-bio-record.h
index 3a8cfa2645c7..dd3646111561 100644
--- a/drivers/md/dm-bio-record.h
+++ b/drivers/md/dm-bio-record.h
@@ -17,55 +17,24 @@
  * original bio state.
  */
 
-struct dm_bio_vec_details {
-#if PAGE_SIZE < 65536
-	__u16 bv_len;
-	__u16 bv_offset;
-#else
-	unsigned bv_len;
-	unsigned bv_offset;
-#endif
-};
-
 struct dm_bio_details {
-	sector_t bi_sector;
 	struct block_device *bi_bdev;
-	unsigned int bi_size;
-	unsigned short bi_idx;
 	unsigned long bi_flags;
-	struct dm_bio_vec_details bi_io_vec[BIO_MAX_PAGES];
+	struct bvec_iter bi_iter;
 };
 
 static inline void dm_bio_record(struct dm_bio_details *bd, struct bio *bio)
 {
-	unsigned i;
-
-	bd->bi_sector = bio->bi_sector;
 	bd->bi_bdev = bio->bi_bdev;
-	bd->bi_size = bio->bi_size;
-	bd->bi_idx = bio->bi_idx;
 	bd->bi_flags = bio->bi_flags;
-
-	for (i = 0; i < bio->bi_vcnt; i++) {
-		bd->bi_io_vec[i].bv_len = bio->bi_io_vec[i].bv_len;
-		bd->bi_io_vec[i].bv_offset = bio->bi_io_vec[i].bv_offset;
-	}
+	bd->bi_iter = bio->bi_iter;
 }
 
 static inline void dm_bio_restore(struct dm_bio_details *bd, struct bio *bio)
 {
-	unsigned i;
-
-	bio->bi_sector = bd->bi_sector;
 	bio->bi_bdev = bd->bi_bdev;
-	bio->bi_size = bd->bi_size;
-	bio->bi_idx = bd->bi_idx;
 	bio->bi_flags = bd->bi_flags;
-
-	for (i = 0; i < bio->bi_vcnt; i++) {
-		bio->bi_io_vec[i].bv_len = bd->bi_io_vec[i].bv_len;
-		bio->bi_io_vec[i].bv_offset = bd->bi_io_vec[i].bv_offset;
-	}
+	bio->bi_iter = bd->bi_iter;
 }
 
 #endif
diff --git a/drivers/md/dm-bufio.c b/drivers/md/dm-bufio.c
index 9ed42125514b..66c5d130c8c2 100644
--- a/drivers/md/dm-bufio.c
+++ b/drivers/md/dm-bufio.c
@@ -540,7 +540,7 @@ static void use_inline_bio(struct dm_buffer *b, int rw, sector_t block,
 	bio_init(&b->bio);
 	b->bio.bi_io_vec = b->bio_vec;
 	b->bio.bi_max_vecs = DM_BUFIO_INLINE_VECS;
-	b->bio.bi_sector = block << b->c->sectors_per_block_bits;
+	b->bio.bi_iter.bi_sector = block << b->c->sectors_per_block_bits;
 	b->bio.bi_bdev = b->c->bdev;
 	b->bio.bi_end_io = end_io;
 
diff --git a/drivers/md/dm-cache-policy-mq.c b/drivers/md/dm-cache-policy-mq.c
index 930e8c3d73e9..1e018e986610 100644
--- a/drivers/md/dm-cache-policy-mq.c
+++ b/drivers/md/dm-cache-policy-mq.c
@@ -72,7 +72,7 @@ static enum io_pattern iot_pattern(struct io_tracker *t)
 
 static void iot_update_stats(struct io_tracker *t, struct bio *bio)
 {
-	if (bio->bi_sector == from_oblock(t->last_end_oblock) + 1)
+	if (bio->bi_iter.bi_sector == from_oblock(t->last_end_oblock) + 1)
 		t->nr_seq_samples++;
 	else {
 		/*
@@ -87,7 +87,7 @@ static void iot_update_stats(struct io_tracker *t, struct bio *bio)
 		t->nr_rand_samples++;
 	}
 
-	t->last_end_oblock = to_oblock(bio->bi_sector + bio_sectors(bio) - 1);
+	t->last_end_oblock = to_oblock(bio_end_sector(bio) - 1);
 }
 
 static void iot_check_for_pattern_switch(struct io_tracker *t)
diff --git a/drivers/md/dm-cache-target.c b/drivers/md/dm-cache-target.c
index 09334c275c79..ffd472e015ca 100644
--- a/drivers/md/dm-cache-target.c
+++ b/drivers/md/dm-cache-target.c
@@ -85,6 +85,12 @@ static void dm_unhook_bio(struct dm_hook_info *h, struct bio *bio)
 {
 	bio->bi_end_io = h->bi_end_io;
 	bio->bi_private = h->bi_private;
+
+	/*
+	 * Must bump bi_remaining to allow bio to complete with
+	 * restored bi_end_io.
+	 */
+	atomic_inc(&bio->bi_remaining);
 }
 
 /*----------------------------------------------------------------*/
@@ -664,15 +670,17 @@ static void remap_to_origin(struct cache *cache, struct bio *bio)
 static void remap_to_cache(struct cache *cache, struct bio *bio,
 			   dm_cblock_t cblock)
 {
-	sector_t bi_sector = bio->bi_sector;
+	sector_t bi_sector = bio->bi_iter.bi_sector;
 
 	bio->bi_bdev = cache->cache_dev->bdev;
 	if (!block_size_is_power_of_two(cache))
-		bio->bi_sector = (from_cblock(cblock) * cache->sectors_per_block) +
-				sector_div(bi_sector, cache->sectors_per_block);
+		bio->bi_iter.bi_sector =
+			(from_cblock(cblock) * cache->sectors_per_block) +
+			sector_div(bi_sector, cache->sectors_per_block);
 	else
-		bio->bi_sector = (from_cblock(cblock) << cache->sectors_per_block_shift) |
-				(bi_sector & (cache->sectors_per_block - 1));
+		bio->bi_iter.bi_sector =
+			(from_cblock(cblock) << cache->sectors_per_block_shift) |
+			(bi_sector & (cache->sectors_per_block - 1));
 }
 
 static void check_if_tick_bio_needed(struct cache *cache, struct bio *bio)
@@ -712,7 +720,7 @@ static void remap_to_cache_dirty(struct cache *cache, struct bio *bio,
 
 static dm_oblock_t get_bio_block(struct cache *cache, struct bio *bio)
 {
-	sector_t block_nr = bio->bi_sector;
+	sector_t block_nr = bio->bi_iter.bi_sector;
 
 	if (!block_size_is_power_of_two(cache))
 		(void) sector_div(block_nr, cache->sectors_per_block);
@@ -1027,7 +1035,7 @@ static void issue_overwrite(struct dm_cache_migration *mg, struct bio *bio)
 static bool bio_writes_complete_block(struct cache *cache, struct bio *bio)
 {
 	return (bio_data_dir(bio) == WRITE) &&
-		(bio->bi_size == (cache->sectors_per_block << SECTOR_SHIFT));
+		(bio->bi_iter.bi_size == (cache->sectors_per_block << SECTOR_SHIFT));
 }
 
 static void avoid_copy(struct dm_cache_migration *mg)
@@ -1252,7 +1260,7 @@ static void process_flush_bio(struct cache *cache, struct bio *bio)
 	size_t pb_data_size = get_per_bio_data_size(cache);
 	struct per_bio_data *pb = get_per_bio_data(bio, pb_data_size);
 
-	BUG_ON(bio->bi_size);
+	BUG_ON(bio->bi_iter.bi_size);
 	if (!pb->req_nr)
 		remap_to_origin(cache, bio);
 	else
@@ -1275,9 +1283,9 @@ static void process_flush_bio(struct cache *cache, struct bio *bio)
  */
 static void process_discard_bio(struct cache *cache, struct bio *bio)
 {
-	dm_block_t start_block = dm_sector_div_up(bio->bi_sector,
+	dm_block_t start_block = dm_sector_div_up(bio->bi_iter.bi_sector,
 						  cache->discard_block_size);
-	dm_block_t end_block = bio->bi_sector + bio_sectors(bio);
+	dm_block_t end_block = bio_end_sector(bio);
 	dm_block_t b;
 
 	end_block = block_div(end_block, cache->discard_block_size);
diff --git a/drivers/md/dm-crypt.c b/drivers/md/dm-crypt.c
index 81b0fa660452..784695d22fde 100644
--- a/drivers/md/dm-crypt.c
+++ b/drivers/md/dm-crypt.c
@@ -39,10 +39,8 @@ struct convert_context {
 	struct completion restart;
 	struct bio *bio_in;
 	struct bio *bio_out;
-	unsigned int offset_in;
-	unsigned int offset_out;
-	unsigned int idx_in;
-	unsigned int idx_out;
+	struct bvec_iter iter_in;
+	struct bvec_iter iter_out;
 	sector_t cc_sector;
 	atomic_t cc_pending;
 };
@@ -826,10 +824,10 @@ static void crypt_convert_init(struct crypt_config *cc,
 {
 	ctx->bio_in = bio_in;
 	ctx->bio_out = bio_out;
-	ctx->offset_in = 0;
-	ctx->offset_out = 0;
-	ctx->idx_in = bio_in ? bio_in->bi_idx : 0;
-	ctx->idx_out = bio_out ? bio_out->bi_idx : 0;
+	if (bio_in)
+		ctx->iter_in = bio_in->bi_iter;
+	if (bio_out)
+		ctx->iter_out = bio_out->bi_iter;
 	ctx->cc_sector = sector + cc->iv_offset;
 	init_completion(&ctx->restart);
 }
@@ -857,8 +855,8 @@ static int crypt_convert_block(struct crypt_config *cc,
 			       struct convert_context *ctx,
 			       struct ablkcipher_request *req)
 {
-	struct bio_vec *bv_in = bio_iovec_idx(ctx->bio_in, ctx->idx_in);
-	struct bio_vec *bv_out = bio_iovec_idx(ctx->bio_out, ctx->idx_out);
+	struct bio_vec bv_in = bio_iter_iovec(ctx->bio_in, ctx->iter_in);
+	struct bio_vec bv_out = bio_iter_iovec(ctx->bio_out, ctx->iter_out);
 	struct dm_crypt_request *dmreq;
 	u8 *iv;
 	int r;
@@ -869,24 +867,15 @@ static int crypt_convert_block(struct crypt_config *cc,
 	dmreq->iv_sector = ctx->cc_sector;
 	dmreq->ctx = ctx;
 	sg_init_table(&dmreq->sg_in, 1);
-	sg_set_page(&dmreq->sg_in, bv_in->bv_page, 1 << SECTOR_SHIFT,
-		    bv_in->bv_offset + ctx->offset_in);
+	sg_set_page(&dmreq->sg_in, bv_in.bv_page, 1 << SECTOR_SHIFT,
+		    bv_in.bv_offset);
 
 	sg_init_table(&dmreq->sg_out, 1);
-	sg_set_page(&dmreq->sg_out, bv_out->bv_page, 1 << SECTOR_SHIFT,
-		    bv_out->bv_offset + ctx->offset_out);
+	sg_set_page(&dmreq->sg_out, bv_out.bv_page, 1 << SECTOR_SHIFT,
+		    bv_out.bv_offset);
 
-	ctx->offset_in += 1 << SECTOR_SHIFT;
-	if (ctx->offset_in >= bv_in->bv_len) {
-		ctx->offset_in = 0;
-		ctx->idx_in++;
-	}
-
-	ctx->offset_out += 1 << SECTOR_SHIFT;
-	if (ctx->offset_out >= bv_out->bv_len) {
-		ctx->offset_out = 0;
-		ctx->idx_out++;
-	}
+	bio_advance_iter(ctx->bio_in, &ctx->iter_in, 1 << SECTOR_SHIFT);
+	bio_advance_iter(ctx->bio_out, &ctx->iter_out, 1 << SECTOR_SHIFT);
 
 	if (cc->iv_gen_ops) {
 		r = cc->iv_gen_ops->generator(cc, iv, dmreq);
@@ -937,8 +926,7 @@ static int crypt_convert(struct crypt_config *cc,
 
 	atomic_set(&ctx->cc_pending, 1);
 
-	while(ctx->idx_in < ctx->bio_in->bi_vcnt &&
-	      ctx->idx_out < ctx->bio_out->bi_vcnt) {
+	while (ctx->iter_in.bi_size && ctx->iter_out.bi_size) {
 
 		crypt_alloc_req(cc, ctx);
 
@@ -1021,7 +1009,7 @@ static struct bio *crypt_alloc_buffer(struct dm_crypt_io *io, unsigned size,
 		size -= len;
 	}
 
-	if (!clone->bi_size) {
+	if (!clone->bi_iter.bi_size) {
 		bio_put(clone);
 		return NULL;
 	}
@@ -1161,7 +1149,7 @@ static int kcryptd_io_read(struct dm_crypt_io *io, gfp_t gfp)
 	crypt_inc_pending(io);
 
 	clone_init(io, clone);
-	clone->bi_sector = cc->start + io->sector;
+	clone->bi_iter.bi_sector = cc->start + io->sector;
 
 	generic_make_request(clone);
 	return 0;
@@ -1207,9 +1195,9 @@ static void kcryptd_crypt_write_io_submit(struct dm_crypt_io *io, int async)
 	}
 
 	/* crypt_convert should have filled the clone bio */
-	BUG_ON(io->ctx.idx_out < clone->bi_vcnt);
+	BUG_ON(io->ctx.iter_out.bi_size);
 
-	clone->bi_sector = cc->start + io->sector;
+	clone->bi_iter.bi_sector = cc->start + io->sector;
 
 	if (async)
 		kcryptd_queue_io(io);
@@ -1224,7 +1212,7 @@ static void kcryptd_crypt_write_convert(struct dm_crypt_io *io)
 	struct dm_crypt_io *new_io;
 	int crypt_finished;
 	unsigned out_of_pages = 0;
-	unsigned remaining = io->base_bio->bi_size;
+	unsigned remaining = io->base_bio->bi_iter.bi_size;
 	sector_t sector = io->sector;
 	int r;
 
@@ -1246,9 +1234,9 @@ static void kcryptd_crypt_write_convert(struct dm_crypt_io *io)
 		}
 
 		io->ctx.bio_out = clone;
-		io->ctx.idx_out = 0;
+		io->ctx.iter_out = clone->bi_iter;
 
-		remaining -= clone->bi_size;
+		remaining -= clone->bi_iter.bi_size;
 		sector += bio_sectors(clone);
 
 		crypt_inc_pending(io);
@@ -1290,8 +1278,7 @@ static void kcryptd_crypt_write_convert(struct dm_crypt_io *io)
 			crypt_inc_pending(new_io);
 			crypt_convert_init(cc, &new_io->ctx, NULL,
 					   io->base_bio, sector);
-			new_io->ctx.idx_in = io->ctx.idx_in;
-			new_io->ctx.offset_in = io->ctx.offset_in;
+			new_io->ctx.iter_in = io->ctx.iter_in;
 
 			/*
 			 * Fragments after the first use the base_io
@@ -1869,11 +1856,12 @@ static int crypt_map(struct dm_target *ti, struct bio *bio)
 	if (unlikely(bio->bi_rw & (REQ_FLUSH | REQ_DISCARD))) {
 		bio->bi_bdev = cc->dev->bdev;
 		if (bio_sectors(bio))
-			bio->bi_sector = cc->start + dm_target_offset(ti, bio->bi_sector);
+			bio->bi_iter.bi_sector = cc->start +
+				dm_target_offset(ti, bio->bi_iter.bi_sector);
 		return DM_MAPIO_REMAPPED;
 	}
 
-	io = crypt_io_alloc(cc, bio, dm_target_offset(ti, bio->bi_sector));
+	io = crypt_io_alloc(cc, bio, dm_target_offset(ti, bio->bi_iter.bi_sector));
 
 	if (bio_data_dir(io->base_bio) == READ) {
 		if (kcryptd_io_read(io, GFP_NOWAIT))
diff --git a/drivers/md/dm-delay.c b/drivers/md/dm-delay.c
index a8a511c053a5..42c3a27a14cc 100644
--- a/drivers/md/dm-delay.c
+++ b/drivers/md/dm-delay.c
@@ -277,14 +277,15 @@ static int delay_map(struct dm_target *ti, struct bio *bio)
 	if ((bio_data_dir(bio) == WRITE) && (dc->dev_write)) {
 		bio->bi_bdev = dc->dev_write->bdev;
 		if (bio_sectors(bio))
-			bio->bi_sector = dc->start_write +
-					 dm_target_offset(ti, bio->bi_sector);
+			bio->bi_iter.bi_sector = dc->start_write +
+				dm_target_offset(ti, bio->bi_iter.bi_sector);
 
 		return delay_bio(dc, dc->write_delay, bio);
 	}
 
 	bio->bi_bdev = dc->dev_read->bdev;
-	bio->bi_sector = dc->start_read + dm_target_offset(ti, bio->bi_sector);
+	bio->bi_iter.bi_sector = dc->start_read +
+		dm_target_offset(ti, bio->bi_iter.bi_sector);
 
 	return delay_bio(dc, dc->read_delay, bio);
 }
diff --git a/drivers/md/dm-flakey.c b/drivers/md/dm-flakey.c
index c80a0ec5f126..b257e46876d3 100644
--- a/drivers/md/dm-flakey.c
+++ b/drivers/md/dm-flakey.c
@@ -248,7 +248,8 @@ static void flakey_map_bio(struct dm_target *ti, struct bio *bio)
 
 	bio->bi_bdev = fc->dev->bdev;
 	if (bio_sectors(bio))
-		bio->bi_sector = flakey_map_sector(ti, bio->bi_sector);
+		bio->bi_iter.bi_sector =
+			flakey_map_sector(ti, bio->bi_iter.bi_sector);
 }
 
 static void corrupt_bio_data(struct bio *bio, struct flakey_c *fc)
@@ -265,8 +266,8 @@ static void corrupt_bio_data(struct bio *bio, struct flakey_c *fc)
 		DMDEBUG("Corrupting data bio=%p by writing %u to byte %u "
 			"(rw=%c bi_rw=%lu bi_sector=%llu cur_bytes=%u)\n",
 			bio, fc->corrupt_bio_value, fc->corrupt_bio_byte,
-			(bio_data_dir(bio) == WRITE) ? 'w' : 'r',
-			bio->bi_rw, (unsigned long long)bio->bi_sector, bio_bytes);
+			(bio_data_dir(bio) == WRITE) ? 'w' : 'r', bio->bi_rw,
+			(unsigned long long)bio->bi_iter.bi_sector, bio_bytes);
 	}
 }
 
diff --git a/drivers/md/dm-io.c b/drivers/md/dm-io.c
index 2a20986a2fec..b2b8a10e8427 100644
--- a/drivers/md/dm-io.c
+++ b/drivers/md/dm-io.c
@@ -201,26 +201,29 @@ static void list_dp_init(struct dpages *dp, struct page_list *pl, unsigned offse
 /*
  * Functions for getting the pages from a bvec.
  */
-static void bvec_get_page(struct dpages *dp,
+static void bio_get_page(struct dpages *dp,
 		  struct page **p, unsigned long *len, unsigned *offset)
 {
-	struct bio_vec *bvec = (struct bio_vec *) dp->context_ptr;
-	*p = bvec->bv_page;
-	*len = bvec->bv_len;
-	*offset = bvec->bv_offset;
+	struct bio *bio = dp->context_ptr;
+	struct bio_vec bvec = bio_iovec(bio);
+	*p = bvec.bv_page;
+	*len = bvec.bv_len;
+	*offset = bvec.bv_offset;
 }
 
-static void bvec_next_page(struct dpages *dp)
+static void bio_next_page(struct dpages *dp)
 {
-	struct bio_vec *bvec = (struct bio_vec *) dp->context_ptr;
-	dp->context_ptr = bvec + 1;
+	struct bio *bio = dp->context_ptr;
+	struct bio_vec bvec = bio_iovec(bio);
+
+	bio_advance(bio, bvec.bv_len);
 }
 
-static void bvec_dp_init(struct dpages *dp, struct bio_vec *bvec)
+static void bio_dp_init(struct dpages *dp, struct bio *bio)
 {
-	dp->get_page = bvec_get_page;
-	dp->next_page = bvec_next_page;
-	dp->context_ptr = bvec;
+	dp->get_page = bio_get_page;
+	dp->next_page = bio_next_page;
+	dp->context_ptr = bio;
 }
 
 /*
@@ -304,14 +307,14 @@ static void do_region(int rw, unsigned region, struct dm_io_region *where,
 					  dm_sector_div_up(remaining, (PAGE_SIZE >> SECTOR_SHIFT)));
 
 		bio = bio_alloc_bioset(GFP_NOIO, num_bvecs, io->client->bios);
-		bio->bi_sector = where->sector + (where->count - remaining);
+		bio->bi_iter.bi_sector = where->sector + (where->count - remaining);
 		bio->bi_bdev = where->bdev;
 		bio->bi_end_io = endio;
 		store_io_and_region_in_bio(bio, io, region);
 
 		if (rw & REQ_DISCARD) {
 			num_sectors = min_t(sector_t, q->limits.max_discard_sectors, remaining);
-			bio->bi_size = num_sectors << SECTOR_SHIFT;
+			bio->bi_iter.bi_size = num_sectors << SECTOR_SHIFT;
 			remaining -= num_sectors;
 		} else if (rw & REQ_WRITE_SAME) {
 			/*
@@ -320,7 +323,7 @@ static void do_region(int rw, unsigned region, struct dm_io_region *where,
 			dp->get_page(dp, &page, &len, &offset);
 			bio_add_page(bio, page, logical_block_size, offset);
 			num_sectors = min_t(sector_t, q->limits.max_write_same_sectors, remaining);
-			bio->bi_size = num_sectors << SECTOR_SHIFT;
+			bio->bi_iter.bi_size = num_sectors << SECTOR_SHIFT;
 
 			offset = 0;
 			remaining -= num_sectors;
@@ -457,8 +460,8 @@ static int dp_init(struct dm_io_request *io_req, struct dpages *dp,
 		list_dp_init(dp, io_req->mem.ptr.pl, io_req->mem.offset);
 		break;
 
-	case DM_IO_BVEC:
-		bvec_dp_init(dp, io_req->mem.ptr.bvec);
+	case DM_IO_BIO:
+		bio_dp_init(dp, io_req->mem.ptr.bio);
 		break;
 
 	case DM_IO_VMA:
diff --git a/drivers/md/dm-linear.c b/drivers/md/dm-linear.c
index 4f99d267340c..53e848c10939 100644
--- a/drivers/md/dm-linear.c
+++ b/drivers/md/dm-linear.c
@@ -85,7 +85,8 @@ static void linear_map_bio(struct dm_target *ti, struct bio *bio)
 
 	bio->bi_bdev = lc->dev->bdev;
 	if (bio_sectors(bio))
-		bio->bi_sector = linear_map_sector(ti, bio->bi_sector);
+		bio->bi_iter.bi_sector =
+			linear_map_sector(ti, bio->bi_iter.bi_sector);
 }
 
 static int linear_map(struct dm_target *ti, struct bio *bio)
diff --git a/drivers/md/dm-raid1.c b/drivers/md/dm-raid1.c
index 9584443c5614..f284e0bfb25f 100644
--- a/drivers/md/dm-raid1.c
+++ b/drivers/md/dm-raid1.c
@@ -432,7 +432,7 @@ static int mirror_available(struct mirror_set *ms, struct bio *bio)
 	region_t region = dm_rh_bio_to_region(ms->rh, bio);
 
 	if (log->type->in_sync(log, region, 0))
-		return choose_mirror(ms,  bio->bi_sector) ? 1 : 0;
+		return choose_mirror(ms,  bio->bi_iter.bi_sector) ? 1 : 0;
 
 	return 0;
 }
@@ -442,15 +442,15 @@ static int mirror_available(struct mirror_set *ms, struct bio *bio)
  */
 static sector_t map_sector(struct mirror *m, struct bio *bio)
 {
-	if (unlikely(!bio->bi_size))
+	if (unlikely(!bio->bi_iter.bi_size))
 		return 0;
-	return m->offset + dm_target_offset(m->ms->ti, bio->bi_sector);
+	return m->offset + dm_target_offset(m->ms->ti, bio->bi_iter.bi_sector);
 }
 
 static void map_bio(struct mirror *m, struct bio *bio)
 {
 	bio->bi_bdev = m->dev->bdev;
-	bio->bi_sector = map_sector(m, bio);
+	bio->bi_iter.bi_sector = map_sector(m, bio);
 }
 
 static void map_region(struct dm_io_region *io, struct mirror *m,
@@ -526,8 +526,8 @@ static void read_async_bio(struct mirror *m, struct bio *bio)
 	struct dm_io_region io;
 	struct dm_io_request io_req = {
 		.bi_rw = READ,
-		.mem.type = DM_IO_BVEC,
-		.mem.ptr.bvec = bio->bi_io_vec + bio->bi_idx,
+		.mem.type = DM_IO_BIO,
+		.mem.ptr.bio = bio,
 		.notify.fn = read_callback,
 		.notify.context = bio,
 		.client = m->ms->io_client,
@@ -559,7 +559,7 @@ static void do_reads(struct mirror_set *ms, struct bio_list *reads)
 		 * We can only read balance if the region is in sync.
 		 */
 		if (likely(region_in_sync(ms, region, 1)))
-			m = choose_mirror(ms, bio->bi_sector);
+			m = choose_mirror(ms, bio->bi_iter.bi_sector);
 		else if (m && atomic_read(&m->error_count))
 			m = NULL;
 
@@ -629,8 +629,8 @@ static void do_write(struct mirror_set *ms, struct bio *bio)
 	struct mirror *m;
 	struct dm_io_request io_req = {
 		.bi_rw = WRITE | (bio->bi_rw & WRITE_FLUSH_FUA),
-		.mem.type = DM_IO_BVEC,
-		.mem.ptr.bvec = bio->bi_io_vec + bio->bi_idx,
+		.mem.type = DM_IO_BIO,
+		.mem.ptr.bio = bio,
 		.notify.fn = write_callback,
 		.notify.context = bio,
 		.client = ms->io_client,
@@ -1181,7 +1181,7 @@ static int mirror_map(struct dm_target *ti, struct bio *bio)
 	 * The region is in-sync and we can perform reads directly.
 	 * Store enough information so we can retry if it fails.
 	 */
-	m = choose_mirror(ms, bio->bi_sector);
+	m = choose_mirror(ms, bio->bi_iter.bi_sector);
 	if (unlikely(!m))
 		return -EIO;
 
diff --git a/drivers/md/dm-region-hash.c b/drivers/md/dm-region-hash.c
index 69732e03eb34..b929fd5f4984 100644
--- a/drivers/md/dm-region-hash.c
+++ b/drivers/md/dm-region-hash.c
@@ -126,7 +126,8 @@ EXPORT_SYMBOL_GPL(dm_rh_region_to_sector);
 
 region_t dm_rh_bio_to_region(struct dm_region_hash *rh, struct bio *bio)
 {
-	return dm_rh_sector_to_region(rh, bio->bi_sector - rh->target_begin);
+	return dm_rh_sector_to_region(rh, bio->bi_iter.bi_sector -
+				      rh->target_begin);
 }
 EXPORT_SYMBOL_GPL(dm_rh_bio_to_region);
 
diff --git a/drivers/md/dm-snap.c b/drivers/md/dm-snap.c
index 717718558bd9..ebddef5237e4 100644
--- a/drivers/md/dm-snap.c
+++ b/drivers/md/dm-snap.c
@@ -1438,6 +1438,7 @@ out:
 	if (full_bio) {
 		full_bio->bi_end_io = pe->full_bio_end_io;
 		full_bio->bi_private = pe->full_bio_private;
+		atomic_inc(&full_bio->bi_remaining);
 	}
 	free_pending_exception(pe);
 
@@ -1619,11 +1620,10 @@ static void remap_exception(struct dm_snapshot *s, struct dm_exception *e,
 			    struct bio *bio, chunk_t chunk)
 {
 	bio->bi_bdev = s->cow->bdev;
-	bio->bi_sector = chunk_to_sector(s->store,
-					 dm_chunk_number(e->new_chunk) +
-					 (chunk - e->old_chunk)) +
-					 (bio->bi_sector &
-					  s->store->chunk_mask);
+	bio->bi_iter.bi_sector =
+		chunk_to_sector(s->store, dm_chunk_number(e->new_chunk) +
+				(chunk - e->old_chunk)) +
+		(bio->bi_iter.bi_sector & s->store->chunk_mask);
 }
 
 static int snapshot_map(struct dm_target *ti, struct bio *bio)
@@ -1641,7 +1641,7 @@ static int snapshot_map(struct dm_target *ti, struct bio *bio)
 		return DM_MAPIO_REMAPPED;
 	}
 
-	chunk = sector_to_chunk(s->store, bio->bi_sector);
+	chunk = sector_to_chunk(s->store, bio->bi_iter.bi_sector);
 
 	/* Full snapshots are not usable */
 	/* To get here the table must be live so s->active is always set. */
@@ -1702,7 +1702,8 @@ static int snapshot_map(struct dm_target *ti, struct bio *bio)
 		r = DM_MAPIO_SUBMITTED;
 
 		if (!pe->started &&
-		    bio->bi_size == (s->store->chunk_size << SECTOR_SHIFT)) {
+		    bio->bi_iter.bi_size ==
+		    (s->store->chunk_size << SECTOR_SHIFT)) {
 			pe->started = 1;
 			up_write(&s->lock);
 			start_full_bio(pe, bio);
@@ -1758,7 +1759,7 @@ static int snapshot_merge_map(struct dm_target *ti, struct bio *bio)
 		return DM_MAPIO_REMAPPED;
 	}
 
-	chunk = sector_to_chunk(s->store, bio->bi_sector);
+	chunk = sector_to_chunk(s->store, bio->bi_iter.bi_sector);
 
 	down_write(&s->lock);
 
@@ -2095,7 +2096,7 @@ static int do_origin(struct dm_dev *origin, struct bio *bio)
 	down_read(&_origins_lock);
 	o = __lookup_origin(origin->bdev);
 	if (o)
-		r = __origin_write(&o->snapshots, bio->bi_sector, bio);
+		r = __origin_write(&o->snapshots, bio->bi_iter.bi_sector, bio);
 	up_read(&_origins_lock);
 
 	return r;
diff --git a/drivers/md/dm-stripe.c b/drivers/md/dm-stripe.c
index 73c1712dad96..d1600d2aa2e2 100644
--- a/drivers/md/dm-stripe.c
+++ b/drivers/md/dm-stripe.c
@@ -259,13 +259,15 @@ static int stripe_map_range(struct stripe_c *sc, struct bio *bio,
 {
 	sector_t begin, end;
 
-	stripe_map_range_sector(sc, bio->bi_sector, target_stripe, &begin);
+	stripe_map_range_sector(sc, bio->bi_iter.bi_sector,
+				target_stripe, &begin);
 	stripe_map_range_sector(sc, bio_end_sector(bio),
 				target_stripe, &end);
 	if (begin < end) {
 		bio->bi_bdev = sc->stripe[target_stripe].dev->bdev;
-		bio->bi_sector = begin + sc->stripe[target_stripe].physical_start;
-		bio->bi_size = to_bytes(end - begin);
+		bio->bi_iter.bi_sector = begin +
+			sc->stripe[target_stripe].physical_start;
+		bio->bi_iter.bi_size = to_bytes(end - begin);
 		return DM_MAPIO_REMAPPED;
 	} else {
 		/* The range doesn't map to the target stripe */
@@ -293,9 +295,10 @@ static int stripe_map(struct dm_target *ti, struct bio *bio)
 		return stripe_map_range(sc, bio, target_bio_nr);
 	}
 
-	stripe_map_sector(sc, bio->bi_sector, &stripe, &bio->bi_sector);
+	stripe_map_sector(sc, bio->bi_iter.bi_sector,
+			  &stripe, &bio->bi_iter.bi_sector);
 
-	bio->bi_sector += sc->stripe[stripe].physical_start;
+	bio->bi_iter.bi_sector += sc->stripe[stripe].physical_start;
 	bio->bi_bdev = sc->stripe[stripe].dev->bdev;
 
 	return DM_MAPIO_REMAPPED;
diff --git a/drivers/md/dm-switch.c b/drivers/md/dm-switch.c
index ff9ac4be4721..09a688b3d48c 100644
--- a/drivers/md/dm-switch.c
+++ b/drivers/md/dm-switch.c
@@ -311,11 +311,11 @@ error:
 static int switch_map(struct dm_target *ti, struct bio *bio)
 {
 	struct switch_ctx *sctx = ti->private;
-	sector_t offset = dm_target_offset(ti, bio->bi_sector);
+	sector_t offset = dm_target_offset(ti, bio->bi_iter.bi_sector);
 	unsigned path_nr = switch_get_path_nr(sctx, offset);
 
 	bio->bi_bdev = sctx->path_list[path_nr].dmdev->bdev;
-	bio->bi_sector = sctx->path_list[path_nr].start + offset;
+	bio->bi_iter.bi_sector = sctx->path_list[path_nr].start + offset;
 
 	return DM_MAPIO_REMAPPED;
 }
diff --git a/drivers/md/dm-thin.c b/drivers/md/dm-thin.c
index 726228b33a01..faaf944597ab 100644
--- a/drivers/md/dm-thin.c
+++ b/drivers/md/dm-thin.c
@@ -414,7 +414,7 @@ static bool block_size_is_power_of_two(struct pool *pool)
 static dm_block_t get_bio_block(struct thin_c *tc, struct bio *bio)
 {
 	struct pool *pool = tc->pool;
-	sector_t block_nr = bio->bi_sector;
+	sector_t block_nr = bio->bi_iter.bi_sector;
 
 	if (block_size_is_power_of_two(pool))
 		block_nr >>= pool->sectors_per_block_shift;
@@ -427,14 +427,15 @@ static dm_block_t get_bio_block(struct thin_c *tc, struct bio *bio)
 static void remap(struct thin_c *tc, struct bio *bio, dm_block_t block)
 {
 	struct pool *pool = tc->pool;
-	sector_t bi_sector = bio->bi_sector;
+	sector_t bi_sector = bio->bi_iter.bi_sector;
 
 	bio->bi_bdev = tc->pool_dev->bdev;
 	if (block_size_is_power_of_two(pool))
-		bio->bi_sector = (block << pool->sectors_per_block_shift) |
-				(bi_sector & (pool->sectors_per_block - 1));
+		bio->bi_iter.bi_sector =
+			(block << pool->sectors_per_block_shift) |
+			(bi_sector & (pool->sectors_per_block - 1));
 	else
-		bio->bi_sector = (block * pool->sectors_per_block) +
+		bio->bi_iter.bi_sector = (block * pool->sectors_per_block) +
 				 sector_div(bi_sector, pool->sectors_per_block);
 }
 
@@ -612,8 +613,10 @@ static void cell_defer_no_holder(struct thin_c *tc, struct dm_bio_prison_cell *c
 
 static void process_prepared_mapping_fail(struct dm_thin_new_mapping *m)
 {
-	if (m->bio)
+	if (m->bio) {
 		m->bio->bi_end_io = m->saved_bi_end_io;
+		atomic_inc(&m->bio->bi_remaining);
+	}
 	cell_error(m->tc->pool, m->cell);
 	list_del(&m->list);
 	mempool_free(m, m->tc->pool->mapping_pool);
@@ -627,8 +630,10 @@ static void process_prepared_mapping(struct dm_thin_new_mapping *m)
 	int r;
 
 	bio = m->bio;
-	if (bio)
+	if (bio) {
 		bio->bi_end_io = m->saved_bi_end_io;
+		atomic_inc(&bio->bi_remaining);
+	}
 
 	if (m->err) {
 		cell_error(pool, m->cell);
@@ -731,7 +736,8 @@ static void process_prepared(struct pool *pool, struct list_head *head,
  */
 static int io_overlaps_block(struct pool *pool, struct bio *bio)
 {
-	return bio->bi_size == (pool->sectors_per_block << SECTOR_SHIFT);
+	return bio->bi_iter.bi_size ==
+		(pool->sectors_per_block << SECTOR_SHIFT);
 }
 
 static int io_overwrites_block(struct pool *pool, struct bio *bio)
@@ -1136,7 +1142,7 @@ static void process_shared_bio(struct thin_c *tc, struct bio *bio,
 	if (bio_detain(pool, &key, bio, &cell))
 		return;
 
-	if (bio_data_dir(bio) == WRITE && bio->bi_size)
+	if (bio_data_dir(bio) == WRITE && bio->bi_iter.bi_size)
 		break_sharing(tc, bio, block, &key, lookup_result, cell);
 	else {
 		struct dm_thin_endio_hook *h = dm_per_bio_data(bio, sizeof(struct dm_thin_endio_hook));
@@ -1159,7 +1165,7 @@ static void provision_block(struct thin_c *tc, struct bio *bio, dm_block_t block
 	/*
 	 * Remap empty bios (flushes) immediately, without provisioning.
 	 */
-	if (!bio->bi_size) {
+	if (!bio->bi_iter.bi_size) {
 		inc_all_io_entry(pool, bio);
 		cell_defer_no_holder(tc, cell);
 
@@ -1258,7 +1264,7 @@ static void process_bio_read_only(struct thin_c *tc, struct bio *bio)
 	r = dm_thin_find_block(tc->td, block, 1, &lookup_result);
 	switch (r) {
 	case 0:
-		if (lookup_result.shared && (rw == WRITE) && bio->bi_size)
+		if (lookup_result.shared && (rw == WRITE) && bio->bi_iter.bi_size)
 			handle_unserviceable_bio(tc->pool, bio);
 		else {
 			inc_all_io_entry(tc->pool, bio);
@@ -2939,7 +2945,7 @@ out_unlock:
 
 static int thin_map(struct dm_target *ti, struct bio *bio)
 {
-	bio->bi_sector = dm_target_offset(ti, bio->bi_sector);
+	bio->bi_iter.bi_sector = dm_target_offset(ti, bio->bi_iter.bi_sector);
 
 	return thin_bio_map(ti, bio);
 }
diff --git a/drivers/md/dm-verity.c b/drivers/md/dm-verity.c
index 4b7941db3aff..796007a5e0e1 100644
--- a/drivers/md/dm-verity.c
+++ b/drivers/md/dm-verity.c
@@ -73,15 +73,10 @@ struct dm_verity_io {
 	sector_t block;
 	unsigned n_blocks;
 
-	/* saved bio vector */
-	struct bio_vec *io_vec;
-	unsigned io_vec_size;
+	struct bvec_iter iter;
 
 	struct work_struct work;
 
-	/* A space for short vectors; longer vectors are allocated separately. */
-	struct bio_vec io_vec_inline[DM_VERITY_IO_VEC_INLINE];
-
 	/*
 	 * Three variably-size fields follow this struct:
 	 *
@@ -284,9 +279,10 @@ release_ret_r:
 static int verity_verify_io(struct dm_verity_io *io)
 {
 	struct dm_verity *v = io->v;
+	struct bio *bio = dm_bio_from_per_bio_data(io,
+						   v->ti->per_bio_data_size);
 	unsigned b;
 	int i;
-	unsigned vector = 0, offset = 0;
 
 	for (b = 0; b < io->n_blocks; b++) {
 		struct shash_desc *desc;
@@ -336,31 +332,22 @@ test_block_hash:
 		}
 
 		todo = 1 << v->data_dev_block_bits;
-		do {
-			struct bio_vec *bv;
+		while (io->iter.bi_size) {
 			u8 *page;
-			unsigned len;
-
-			BUG_ON(vector >= io->io_vec_size);
-			bv = &io->io_vec[vector];
-			page = kmap_atomic(bv->bv_page);
-			len = bv->bv_len - offset;
-			if (likely(len >= todo))
-				len = todo;
-			r = crypto_shash_update(desc,
-					page + bv->bv_offset + offset, len);
+			struct bio_vec bv = bio_iter_iovec(bio, io->iter);
+
+			page = kmap_atomic(bv.bv_page);
+			r = crypto_shash_update(desc, page + bv.bv_offset,
+						bv.bv_len);
 			kunmap_atomic(page);
+
 			if (r < 0) {
 				DMERR("crypto_shash_update failed: %d", r);
 				return r;
 			}
-			offset += len;
-			if (likely(offset == bv->bv_len)) {
-				offset = 0;
-				vector++;
-			}
-			todo -= len;
-		} while (todo);
+
+			bio_advance_iter(bio, &io->iter, bv.bv_len);
+		}
 
 		if (!v->version) {
 			r = crypto_shash_update(desc, v->salt, v->salt_size);
@@ -383,8 +370,6 @@ test_block_hash:
 			return -EIO;
 		}
 	}
-	BUG_ON(vector != io->io_vec_size);
-	BUG_ON(offset);
 
 	return 0;
 }
@@ -400,10 +385,7 @@ static void verity_finish_io(struct dm_verity_io *io, int error)
 	bio->bi_end_io = io->orig_bi_end_io;
 	bio->bi_private = io->orig_bi_private;
 
-	if (io->io_vec != io->io_vec_inline)
-		mempool_free(io->io_vec, v->vec_mempool);
-
-	bio_endio(bio, error);
+	bio_endio_nodec(bio, error);
 }
 
 static void verity_work(struct work_struct *w)
@@ -493,9 +475,9 @@ static int verity_map(struct dm_target *ti, struct bio *bio)
 	struct dm_verity_io *io;
 
 	bio->bi_bdev = v->data_dev->bdev;
-	bio->bi_sector = verity_map_sector(v, bio->bi_sector);
+	bio->bi_iter.bi_sector = verity_map_sector(v, bio->bi_iter.bi_sector);
 
-	if (((unsigned)bio->bi_sector | bio_sectors(bio)) &
+	if (((unsigned)bio->bi_iter.bi_sector | bio_sectors(bio)) &
 	    ((1 << (v->data_dev_block_bits - SECTOR_SHIFT)) - 1)) {
 		DMERR_LIMIT("unaligned io");
 		return -EIO;
@@ -514,18 +496,12 @@ static int verity_map(struct dm_target *ti, struct bio *bio)
 	io->v = v;
 	io->orig_bi_end_io = bio->bi_end_io;
 	io->orig_bi_private = bio->bi_private;
-	io->block = bio->bi_sector >> (v->data_dev_block_bits - SECTOR_SHIFT);
-	io->n_blocks = bio->bi_size >> v->data_dev_block_bits;
+	io->block = bio->bi_iter.bi_sector >> (v->data_dev_block_bits - SECTOR_SHIFT);
+	io->n_blocks = bio->bi_iter.bi_size >> v->data_dev_block_bits;
 
 	bio->bi_end_io = verity_end_io;
 	bio->bi_private = io;
-	io->io_vec_size = bio_segments(bio);
-	if (io->io_vec_size < DM_VERITY_IO_VEC_INLINE)
-		io->io_vec = io->io_vec_inline;
-	else
-		io->io_vec = mempool_alloc(v->vec_mempool, GFP_NOIO);
-	memcpy(io->io_vec, bio_iovec(bio),
-	       io->io_vec_size * sizeof(struct bio_vec));
+	io->iter = bio->bi_iter;
 
 	verity_submit_prefetch(v, io);
 
diff --git a/drivers/md/dm.c b/drivers/md/dm.c
index b49c76284241..8c53b09b9a2c 100644
--- a/drivers/md/dm.c
+++ b/drivers/md/dm.c
@@ -575,7 +575,7 @@ static void start_io_acct(struct dm_io *io)
 		atomic_inc_return(&md->pending[rw]));
 
 	if (unlikely(dm_stats_used(&md->stats)))
-		dm_stats_account_io(&md->stats, bio->bi_rw, bio->bi_sector,
+		dm_stats_account_io(&md->stats, bio->bi_rw, bio->bi_iter.bi_sector,
 				    bio_sectors(bio), false, 0, &io->stats_aux);
 }
 
@@ -593,7 +593,7 @@ static void end_io_acct(struct dm_io *io)
 	part_stat_unlock();
 
 	if (unlikely(dm_stats_used(&md->stats)))
-		dm_stats_account_io(&md->stats, bio->bi_rw, bio->bi_sector,
+		dm_stats_account_io(&md->stats, bio->bi_rw, bio->bi_iter.bi_sector,
 				    bio_sectors(bio), true, duration, &io->stats_aux);
 
 	/*
@@ -742,7 +742,7 @@ static void dec_pending(struct dm_io *io, int error)
 		if (io_error == DM_ENDIO_REQUEUE)
 			return;
 
-		if ((bio->bi_rw & REQ_FLUSH) && bio->bi_size) {
+		if ((bio->bi_rw & REQ_FLUSH) && bio->bi_iter.bi_size) {
 			/*
 			 * Preflush done for flush with data, reissue
 			 * without REQ_FLUSH.
@@ -797,7 +797,7 @@ static void end_clone_bio(struct bio *clone, int error)
 	struct dm_rq_clone_bio_info *info = clone->bi_private;
 	struct dm_rq_target_io *tio = info->tio;
 	struct bio *bio = info->orig;
-	unsigned int nr_bytes = info->orig->bi_size;
+	unsigned int nr_bytes = info->orig->bi_iter.bi_size;
 
 	bio_put(clone);
 
@@ -1128,7 +1128,7 @@ static void __map_bio(struct dm_target_io *tio)
 	 * this io.
 	 */
 	atomic_inc(&tio->io->io_count);
-	sector = clone->bi_sector;
+	sector = clone->bi_iter.bi_sector;
 	r = ti->type->map(ti, clone);
 	if (r == DM_MAPIO_REMAPPED) {
 		/* the bio has been remapped so dispatch it */
@@ -1155,76 +1155,32 @@ struct clone_info {
 	struct dm_io *io;
 	sector_t sector;
 	sector_t sector_count;
-	unsigned short idx;
 };
 
 static void bio_setup_sector(struct bio *bio, sector_t sector, sector_t len)
 {
-	bio->bi_sector = sector;
-	bio->bi_size = to_bytes(len);
-}
-
-static void bio_setup_bv(struct bio *bio, unsigned short idx, unsigned short bv_count)
-{
-	bio->bi_idx = idx;
-	bio->bi_vcnt = idx + bv_count;
-	bio->bi_flags &= ~(1 << BIO_SEG_VALID);
-}
-
-static void clone_bio_integrity(struct bio *bio, struct bio *clone,
-				unsigned short idx, unsigned len, unsigned offset,
-				unsigned trim)
-{
-	if (!bio_integrity(bio))
-		return;
-
-	bio_integrity_clone(clone, bio, GFP_NOIO);
-
-	if (trim)
-		bio_integrity_trim(clone, bio_sector_offset(bio, idx, offset), len);
-}
-
-/*
- * Creates a little bio that just does part of a bvec.
- */
-static void clone_split_bio(struct dm_target_io *tio, struct bio *bio,
-			    sector_t sector, unsigned short idx,
-			    unsigned offset, unsigned len)
-{
-	struct bio *clone = &tio->clone;
-	struct bio_vec *bv = bio->bi_io_vec + idx;
-
-	*clone->bi_io_vec = *bv;
-
-	bio_setup_sector(clone, sector, len);
-
-	clone->bi_bdev = bio->bi_bdev;
-	clone->bi_rw = bio->bi_rw;
-	clone->bi_vcnt = 1;
-	clone->bi_io_vec->bv_offset = offset;
-	clone->bi_io_vec->bv_len = clone->bi_size;
-	clone->bi_flags |= 1 << BIO_CLONED;
-
-	clone_bio_integrity(bio, clone, idx, len, offset, 1);
+	bio->bi_iter.bi_sector = sector;
+	bio->bi_iter.bi_size = to_bytes(len);
 }
 
 /*
  * Creates a bio that consists of range of complete bvecs.
  */
 static void clone_bio(struct dm_target_io *tio, struct bio *bio,
-		      sector_t sector, unsigned short idx,
-		      unsigned short bv_count, unsigned len)
+		      sector_t sector, unsigned len)
 {
 	struct bio *clone = &tio->clone;
-	unsigned trim = 0;
 
-	__bio_clone(clone, bio);
-	bio_setup_sector(clone, sector, len);
-	bio_setup_bv(clone, idx, bv_count);
+	__bio_clone_fast(clone, bio);
+
+	if (bio_integrity(bio))
+		bio_integrity_clone(clone, bio, GFP_NOIO);
+
+	bio_advance(clone, to_bytes(sector - clone->bi_iter.bi_sector));
+	clone->bi_iter.bi_size = to_bytes(len);
 
-	if (idx != bio->bi_idx || clone->bi_size < bio->bi_size)
-		trim = 1;
-	clone_bio_integrity(bio, clone, idx, len, 0, trim);
+	if (bio_integrity(bio))
+		bio_integrity_trim(clone, 0, len);
 }
 
 static struct dm_target_io *alloc_tio(struct clone_info *ci,
@@ -1257,7 +1213,7 @@ static void __clone_and_map_simple_bio(struct clone_info *ci,
 	 * ci->bio->bi_max_vecs is BIO_INLINE_VECS anyway, for both flush
 	 * and discard, so no need for concern about wasted bvec allocations.
 	 */
-	 __bio_clone(clone, ci->bio);
+	 __bio_clone_fast(clone, ci->bio);
 	if (len)
 		bio_setup_sector(clone, ci->sector, len);
 
@@ -1286,10 +1242,7 @@ static int __send_empty_flush(struct clone_info *ci)
 }
 
 static void __clone_and_map_data_bio(struct clone_info *ci, struct dm_target *ti,
-				     sector_t sector, int nr_iovecs,
-				     unsigned short idx, unsigned short bv_count,
-				     unsigned offset, unsigned len,
-				     unsigned split_bvec)
+				     sector_t sector, unsigned len)
 {
 	struct bio *bio = ci->bio;
 	struct dm_target_io *tio;
@@ -1303,11 +1256,8 @@ static void __clone_and_map_data_bio(struct clone_info *ci, struct dm_target *ti
 		num_target_bios = ti->num_write_bios(ti, bio);
 
 	for (target_bio_nr = 0; target_bio_nr < num_target_bios; target_bio_nr++) {
-		tio = alloc_tio(ci, ti, nr_iovecs, target_bio_nr);
-		if (split_bvec)
-			clone_split_bio(tio, bio, sector, idx, offset, len);
-		else
-			clone_bio(tio, bio, sector, idx, bv_count, len);
+		tio = alloc_tio(ci, ti, 0, target_bio_nr);
+		clone_bio(tio, bio, sector, len);
 		__map_bio(tio);
 	}
 }
@@ -1379,68 +1329,13 @@ static int __send_write_same(struct clone_info *ci)
 }
 
 /*
- * Find maximum number of sectors / bvecs we can process with a single bio.
- */
-static sector_t __len_within_target(struct clone_info *ci, sector_t max, int *idx)
-{
-	struct bio *bio = ci->bio;
-	sector_t bv_len, total_len = 0;
-
-	for (*idx = ci->idx; max && (*idx < bio->bi_vcnt); (*idx)++) {
-		bv_len = to_sector(bio->bi_io_vec[*idx].bv_len);
-
-		if (bv_len > max)
-			break;
-
-		max -= bv_len;
-		total_len += bv_len;
-	}
-
-	return total_len;
-}
-
-static int __split_bvec_across_targets(struct clone_info *ci,
-				       struct dm_target *ti, sector_t max)
-{
-	struct bio *bio = ci->bio;
-	struct bio_vec *bv = bio->bi_io_vec + ci->idx;
-	sector_t remaining = to_sector(bv->bv_len);
-	unsigned offset = 0;
-	sector_t len;
-
-	do {
-		if (offset) {
-			ti = dm_table_find_target(ci->map, ci->sector);
-			if (!dm_target_is_valid(ti))
-				return -EIO;
-
-			max = max_io_len(ci->sector, ti);
-		}
-
-		len = min(remaining, max);
-
-		__clone_and_map_data_bio(ci, ti, ci->sector, 1, ci->idx, 0,
-					 bv->bv_offset + offset, len, 1);
-
-		ci->sector += len;
-		ci->sector_count -= len;
-		offset += to_bytes(len);
-	} while (remaining -= len);
-
-	ci->idx++;
-
-	return 0;
-}
-
-/*
  * Select the correct strategy for processing a non-flush bio.
  */
 static int __split_and_process_non_flush(struct clone_info *ci)
 {
 	struct bio *bio = ci->bio;
 	struct dm_target *ti;
-	sector_t len, max;
-	int idx;
+	unsigned len;
 
 	if (unlikely(bio->bi_rw & REQ_DISCARD))
 		return __send_discard(ci);
@@ -1451,41 +1346,14 @@ static int __split_and_process_non_flush(struct clone_info *ci)
 	if (!dm_target_is_valid(ti))
 		return -EIO;
 
-	max = max_io_len(ci->sector, ti);
-
-	/*
-	 * Optimise for the simple case where we can do all of
-	 * the remaining io with a single clone.
-	 */
-	if (ci->sector_count <= max) {
-		__clone_and_map_data_bio(ci, ti, ci->sector, bio->bi_max_vecs,
-					 ci->idx, bio->bi_vcnt - ci->idx, 0,
-					 ci->sector_count, 0);
-		ci->sector_count = 0;
-		return 0;
-	}
-
-	/*
-	 * There are some bvecs that don't span targets.
-	 * Do as many of these as possible.
-	 */
-	if (to_sector(bio->bi_io_vec[ci->idx].bv_len) <= max) {
-		len = __len_within_target(ci, max, &idx);
-
-		__clone_and_map_data_bio(ci, ti, ci->sector, bio->bi_max_vecs,
-					 ci->idx, idx - ci->idx, 0, len, 0);
+	len = min_t(sector_t, max_io_len(ci->sector, ti), ci->sector_count);
 
-		ci->sector += len;
-		ci->sector_count -= len;
-		ci->idx = idx;
+	__clone_and_map_data_bio(ci, ti, ci->sector, len);
 
-		return 0;
-	}
+	ci->sector += len;
+	ci->sector_count -= len;
 
-	/*
-	 * Handle a bvec that must be split between two or more targets.
-	 */
-	return __split_bvec_across_targets(ci, ti, max);
+	return 0;
 }
 
 /*
@@ -1510,8 +1378,7 @@ static void __split_and_process_bio(struct mapped_device *md,
 	ci.io->bio = bio;
 	ci.io->md = md;
 	spin_lock_init(&ci.io->endio_lock);
-	ci.sector = bio->bi_sector;
-	ci.idx = bio->bi_idx;
+	ci.sector = bio->bi_iter.bi_sector;
 
 	start_io_acct(ci.io);
 
diff --git a/drivers/md/faulty.c b/drivers/md/faulty.c
index 3193aefe982b..e8b4574956c7 100644
--- a/drivers/md/faulty.c
+++ b/drivers/md/faulty.c
@@ -74,8 +74,8 @@ static void faulty_fail(struct bio *bio, int error)
 {
 	struct bio *b = bio->bi_private;
 
-	b->bi_size = bio->bi_size;
-	b->bi_sector = bio->bi_sector;
+	b->bi_iter.bi_size = bio->bi_iter.bi_size;
+	b->bi_iter.bi_sector = bio->bi_iter.bi_sector;
 
 	bio_put(bio);
 
@@ -185,26 +185,31 @@ static void make_request(struct mddev *mddev, struct bio *bio)
 			return;
 		}
 
-		if (check_sector(conf, bio->bi_sector, bio_end_sector(bio), WRITE))
+		if (check_sector(conf, bio->bi_iter.bi_sector,
+				 bio_end_sector(bio), WRITE))
 			failit = 1;
 		if (check_mode(conf, WritePersistent)) {
-			add_sector(conf, bio->bi_sector, WritePersistent);
+			add_sector(conf, bio->bi_iter.bi_sector,
+				   WritePersistent);
 			failit = 1;
 		}
 		if (check_mode(conf, WriteTransient))
 			failit = 1;
 	} else {
 		/* read request */
-		if (check_sector(conf, bio->bi_sector, bio_end_sector(bio), READ))
+		if (check_sector(conf, bio->bi_iter.bi_sector,
+				 bio_end_sector(bio), READ))
 			failit = 1;
 		if (check_mode(conf, ReadTransient))
 			failit = 1;
 		if (check_mode(conf, ReadPersistent)) {
-			add_sector(conf, bio->bi_sector, ReadPersistent);
+			add_sector(conf, bio->bi_iter.bi_sector,
+				   ReadPersistent);
 			failit = 1;
 		}
 		if (check_mode(conf, ReadFixable)) {
-			add_sector(conf, bio->bi_sector, ReadFixable);
+			add_sector(conf, bio->bi_iter.bi_sector,
+				   ReadFixable);
 			failit = 1;
 		}
 	}
diff --git a/drivers/md/linear.c b/drivers/md/linear.c
index f03fabd2b37b..56f534b4a2d2 100644
--- a/drivers/md/linear.c
+++ b/drivers/md/linear.c
@@ -288,65 +288,65 @@ static int linear_stop (struct mddev *mddev)
 
 static void linear_make_request(struct mddev *mddev, struct bio *bio)
 {
+	char b[BDEVNAME_SIZE];
 	struct dev_info *tmp_dev;
-	sector_t start_sector;
+	struct bio *split;
+	sector_t start_sector, end_sector, data_offset;
 
 	if (unlikely(bio->bi_rw & REQ_FLUSH)) {
 		md_flush_request(mddev, bio);
 		return;
 	}
 
-	rcu_read_lock();
-	tmp_dev = which_dev(mddev, bio->bi_sector);
-	start_sector = tmp_dev->end_sector - tmp_dev->rdev->sectors;
-
-
-	if (unlikely(bio->bi_sector >= (tmp_dev->end_sector)
-		     || (bio->bi_sector < start_sector))) {
-		char b[BDEVNAME_SIZE];
-
-		printk(KERN_ERR
-		       "md/linear:%s: make_request: Sector %llu out of bounds on "
-		       "dev %s: %llu sectors, offset %llu\n",
-		       mdname(mddev),
-		       (unsigned long long)bio->bi_sector,
-		       bdevname(tmp_dev->rdev->bdev, b),
-		       (unsigned long long)tmp_dev->rdev->sectors,
-		       (unsigned long long)start_sector);
-		rcu_read_unlock();
-		bio_io_error(bio);
-		return;
-	}
-	if (unlikely(bio_end_sector(bio) > tmp_dev->end_sector)) {
-		/* This bio crosses a device boundary, so we have to
-		 * split it.
-		 */
-		struct bio_pair *bp;
-		sector_t end_sector = tmp_dev->end_sector;
+	do {
+		rcu_read_lock();
 
-		rcu_read_unlock();
-
-		bp = bio_split(bio, end_sector - bio->bi_sector);
+		tmp_dev = which_dev(mddev, bio->bi_iter.bi_sector);
+		start_sector = tmp_dev->end_sector - tmp_dev->rdev->sectors;
+		end_sector = tmp_dev->end_sector;
+		data_offset = tmp_dev->rdev->data_offset;
+		bio->bi_bdev = tmp_dev->rdev->bdev;
 
-		linear_make_request(mddev, &bp->bio1);
-		linear_make_request(mddev, &bp->bio2);
-		bio_pair_release(bp);
-		return;
-	}
-		    
-	bio->bi_bdev = tmp_dev->rdev->bdev;
-	bio->bi_sector = bio->bi_sector - start_sector
-		+ tmp_dev->rdev->data_offset;
-	rcu_read_unlock();
+		rcu_read_unlock();
 
-	if (unlikely((bio->bi_rw & REQ_DISCARD) &&
-		     !blk_queue_discard(bdev_get_queue(bio->bi_bdev)))) {
-		/* Just ignore it */
-		bio_endio(bio, 0);
-		return;
-	}
+		if (unlikely(bio->bi_iter.bi_sector >= end_sector ||
+			     bio->bi_iter.bi_sector < start_sector))
+			goto out_of_bounds;
+
+		if (unlikely(bio_end_sector(bio) > end_sector)) {
+			/* This bio crosses a device boundary, so we have to
+			 * split it.
+			 */
+			split = bio_split(bio, end_sector -
+					  bio->bi_iter.bi_sector,
+					  GFP_NOIO, fs_bio_set);
+			bio_chain(split, bio);
+		} else {
+			split = bio;
+		}
 
-	generic_make_request(bio);
+		split->bi_iter.bi_sector = split->bi_iter.bi_sector -
+			start_sector + data_offset;
+
+		if (unlikely((split->bi_rw & REQ_DISCARD) &&
+			 !blk_queue_discard(bdev_get_queue(split->bi_bdev)))) {
+			/* Just ignore it */
+			bio_endio(split, 0);
+		} else
+			generic_make_request(split);
+	} while (split != bio);
+	return;
+
+out_of_bounds:
+	printk(KERN_ERR
+	       "md/linear:%s: make_request: Sector %llu out of bounds on "
+	       "dev %s: %llu sectors, offset %llu\n",
+	       mdname(mddev),
+	       (unsigned long long)bio->bi_iter.bi_sector,
+	       bdevname(tmp_dev->rdev->bdev, b),
+	       (unsigned long long)tmp_dev->rdev->sectors,
+	       (unsigned long long)start_sector);
+	bio_io_error(bio);
 }
 
 static void linear_status (struct seq_file *seq, struct mddev *mddev)
diff --git a/drivers/md/md.c b/drivers/md/md.c
index 40c531359a15..4ad5cc4e63e8 100644
--- a/drivers/md/md.c
+++ b/drivers/md/md.c
@@ -393,7 +393,7 @@ static void md_submit_flush_data(struct work_struct *ws)
 	struct mddev *mddev = container_of(ws, struct mddev, flush_work);
 	struct bio *bio = mddev->flush_bio;
 
-	if (bio->bi_size == 0)
+	if (bio->bi_iter.bi_size == 0)
 		/* an empty barrier - all done */
 		bio_endio(bio, 0);
 	else {
@@ -754,7 +754,7 @@ void md_super_write(struct mddev *mddev, struct md_rdev *rdev,
 	struct bio *bio = bio_alloc_mddev(GFP_NOIO, 1, mddev);
 
 	bio->bi_bdev = rdev->meta_bdev ? rdev->meta_bdev : rdev->bdev;
-	bio->bi_sector = sector;
+	bio->bi_iter.bi_sector = sector;
 	bio_add_page(bio, page, size, 0);
 	bio->bi_private = rdev;
 	bio->bi_end_io = super_written;
@@ -782,18 +782,16 @@ int sync_page_io(struct md_rdev *rdev, sector_t sector, int size,
 	struct bio *bio = bio_alloc_mddev(GFP_NOIO, 1, rdev->mddev);
 	int ret;
 
-	rw |= REQ_SYNC;
-
 	bio->bi_bdev = (metadata_op && rdev->meta_bdev) ?
 		rdev->meta_bdev : rdev->bdev;
 	if (metadata_op)
-		bio->bi_sector = sector + rdev->sb_start;
+		bio->bi_iter.bi_sector = sector + rdev->sb_start;
 	else if (rdev->mddev->reshape_position != MaxSector &&
 		 (rdev->mddev->reshape_backwards ==
 		  (sector >= rdev->mddev->reshape_position)))
-		bio->bi_sector = sector + rdev->new_data_offset;
+		bio->bi_iter.bi_sector = sector + rdev->new_data_offset;
 	else
-		bio->bi_sector = sector + rdev->data_offset;
+		bio->bi_iter.bi_sector = sector + rdev->data_offset;
 	bio_add_page(bio, page, size, 0);
 	submit_bio_wait(rw, bio);
 
diff --git a/drivers/md/multipath.c b/drivers/md/multipath.c
index 1642eae75a33..849ad39f547b 100644
--- a/drivers/md/multipath.c
+++ b/drivers/md/multipath.c
@@ -100,7 +100,7 @@ static void multipath_end_request(struct bio *bio, int error)
 		md_error (mp_bh->mddev, rdev);
 		printk(KERN_ERR "multipath: %s: rescheduling sector %llu\n", 
 		       bdevname(rdev->bdev,b), 
-		       (unsigned long long)bio->bi_sector);
+		       (unsigned long long)bio->bi_iter.bi_sector);
 		multipath_reschedule_retry(mp_bh);
 	} else
 		multipath_end_bh_io(mp_bh, error);
@@ -132,7 +132,7 @@ static void multipath_make_request(struct mddev *mddev, struct bio * bio)
 	multipath = conf->multipaths + mp_bh->path;
 
 	mp_bh->bio = *bio;
-	mp_bh->bio.bi_sector += multipath->rdev->data_offset;
+	mp_bh->bio.bi_iter.bi_sector += multipath->rdev->data_offset;
 	mp_bh->bio.bi_bdev = multipath->rdev->bdev;
 	mp_bh->bio.bi_rw |= REQ_FAILFAST_TRANSPORT;
 	mp_bh->bio.bi_end_io = multipath_end_request;
@@ -355,21 +355,22 @@ static void multipathd(struct md_thread *thread)
 		spin_unlock_irqrestore(&conf->device_lock, flags);
 
 		bio = &mp_bh->bio;
-		bio->bi_sector = mp_bh->master_bio->bi_sector;
+		bio->bi_iter.bi_sector = mp_bh->master_bio->bi_iter.bi_sector;
 		
 		if ((mp_bh->path = multipath_map (conf))<0) {
 			printk(KERN_ALERT "multipath: %s: unrecoverable IO read"
 				" error for block %llu\n",
 				bdevname(bio->bi_bdev,b),
-				(unsigned long long)bio->bi_sector);
+				(unsigned long long)bio->bi_iter.bi_sector);
 			multipath_end_bh_io(mp_bh, -EIO);
 		} else {
 			printk(KERN_ERR "multipath: %s: redirecting sector %llu"
 				" to another IO path\n",
 				bdevname(bio->bi_bdev,b),
-				(unsigned long long)bio->bi_sector);
+				(unsigned long long)bio->bi_iter.bi_sector);
 			*bio = *(mp_bh->master_bio);
-			bio->bi_sector += conf->multipaths[mp_bh->path].rdev->data_offset;
+			bio->bi_iter.bi_sector +=
+				conf->multipaths[mp_bh->path].rdev->data_offset;
 			bio->bi_bdev = conf->multipaths[mp_bh->path].rdev->bdev;
 			bio->bi_rw |= REQ_FAILFAST_TRANSPORT;
 			bio->bi_end_io = multipath_end_request;
diff --git a/drivers/md/raid0.c b/drivers/md/raid0.c
index c4d420b7d2f4..407a99e46f69 100644
--- a/drivers/md/raid0.c
+++ b/drivers/md/raid0.c
@@ -501,10 +501,11 @@ static inline int is_io_in_chunk_boundary(struct mddev *mddev,
 			unsigned int chunk_sects, struct bio *bio)
 {
 	if (likely(is_power_of_2(chunk_sects))) {
-		return chunk_sects >= ((bio->bi_sector & (chunk_sects-1))
+		return chunk_sects >=
+			((bio->bi_iter.bi_sector & (chunk_sects-1))
 					+ bio_sectors(bio));
 	} else{
-		sector_t sector = bio->bi_sector;
+		sector_t sector = bio->bi_iter.bi_sector;
 		return chunk_sects >= (sector_div(sector, chunk_sects)
 						+ bio_sectors(bio));
 	}
@@ -512,64 +513,44 @@ static inline int is_io_in_chunk_boundary(struct mddev *mddev,
 
 static void raid0_make_request(struct mddev *mddev, struct bio *bio)
 {
-	unsigned int chunk_sects;
-	sector_t sector_offset;
 	struct strip_zone *zone;
 	struct md_rdev *tmp_dev;
+	struct bio *split;
 
 	if (unlikely(bio->bi_rw & REQ_FLUSH)) {
 		md_flush_request(mddev, bio);
 		return;
 	}
 
-	chunk_sects = mddev->chunk_sectors;
-	if (unlikely(!is_io_in_chunk_boundary(mddev, chunk_sects, bio))) {
-		sector_t sector = bio->bi_sector;
-		struct bio_pair *bp;
-		/* Sanity check -- queue functions should prevent this happening */
-		if (bio_segments(bio) > 1)
-			goto bad_map;
-		/* This is a one page bio that upper layers
-		 * refuse to split for us, so we need to split it.
-		 */
-		if (likely(is_power_of_2(chunk_sects)))
-			bp = bio_split(bio, chunk_sects - (sector &
-							   (chunk_sects-1)));
-		else
-			bp = bio_split(bio, chunk_sects -
-				       sector_div(sector, chunk_sects));
-		raid0_make_request(mddev, &bp->bio1);
-		raid0_make_request(mddev, &bp->bio2);
-		bio_pair_release(bp);
-		return;
-	}
+	do {
+		sector_t sector = bio->bi_iter.bi_sector;
+		unsigned chunk_sects = mddev->chunk_sectors;
 
-	sector_offset = bio->bi_sector;
-	zone = find_zone(mddev->private, &sector_offset);
-	tmp_dev = map_sector(mddev, zone, bio->bi_sector,
-			     &sector_offset);
-	bio->bi_bdev = tmp_dev->bdev;
-	bio->bi_sector = sector_offset + zone->dev_start +
-		tmp_dev->data_offset;
-
-	if (unlikely((bio->bi_rw & REQ_DISCARD) &&
-		     !blk_queue_discard(bdev_get_queue(bio->bi_bdev)))) {
-		/* Just ignore it */
-		bio_endio(bio, 0);
-		return;
-	}
+		unsigned sectors = chunk_sects -
+			(likely(is_power_of_2(chunk_sects))
+			 ? (sector & (chunk_sects-1))
+			 : sector_div(sector, chunk_sects));
 
-	generic_make_request(bio);
-	return;
-
-bad_map:
-	printk("md/raid0:%s: make_request bug: can't convert block across chunks"
-	       " or bigger than %dk %llu %d\n",
-	       mdname(mddev), chunk_sects / 2,
-	       (unsigned long long)bio->bi_sector, bio_sectors(bio) / 2);
+		if (sectors < bio_sectors(bio)) {
+			split = bio_split(bio, sectors, GFP_NOIO, fs_bio_set);
+			bio_chain(split, bio);
+		} else {
+			split = bio;
+		}
 
-	bio_io_error(bio);
-	return;
+		zone = find_zone(mddev->private, &sector);
+		tmp_dev = map_sector(mddev, zone, sector, &sector);
+		split->bi_bdev = tmp_dev->bdev;
+		split->bi_iter.bi_sector = sector + zone->dev_start +
+			tmp_dev->data_offset;
+
+		if (unlikely((split->bi_rw & REQ_DISCARD) &&
+			 !blk_queue_discard(bdev_get_queue(split->bi_bdev)))) {
+			/* Just ignore it */
+			bio_endio(split, 0);
+		} else
+			generic_make_request(split);
+	} while (split != bio);
 }
 
 static void raid0_status(struct seq_file *seq, struct mddev *mddev)
diff --git a/drivers/md/raid1.c b/drivers/md/raid1.c
index a49cfcc7a343..fd3a2a14b587 100644
--- a/drivers/md/raid1.c
+++ b/drivers/md/raid1.c
@@ -229,7 +229,7 @@ static void call_bio_endio(struct r1bio *r1_bio)
 	int done;
 	struct r1conf *conf = r1_bio->mddev->private;
 	sector_t start_next_window = r1_bio->start_next_window;
-	sector_t bi_sector = bio->bi_sector;
+	sector_t bi_sector = bio->bi_iter.bi_sector;
 
 	if (bio->bi_phys_segments) {
 		unsigned long flags;
@@ -265,9 +265,8 @@ static void raid_end_bio_io(struct r1bio *r1_bio)
 	if (!test_and_set_bit(R1BIO_Returned, &r1_bio->state)) {
 		pr_debug("raid1: sync end %s on sectors %llu-%llu\n",
 			 (bio_data_dir(bio) == WRITE) ? "write" : "read",
-			 (unsigned long long) bio->bi_sector,
-			 (unsigned long long) bio->bi_sector +
-			 bio_sectors(bio) - 1);
+			 (unsigned long long) bio->bi_iter.bi_sector,
+			 (unsigned long long) bio_end_sector(bio) - 1);
 
 		call_bio_endio(r1_bio);
 	}
@@ -466,9 +465,8 @@ static void raid1_end_write_request(struct bio *bio, int error)
 				struct bio *mbio = r1_bio->master_bio;
 				pr_debug("raid1: behind end write sectors"
 					 " %llu-%llu\n",
-					 (unsigned long long) mbio->bi_sector,
-					 (unsigned long long) mbio->bi_sector +
-					 bio_sectors(mbio) - 1);
+					 (unsigned long long) mbio->bi_iter.bi_sector,
+					 (unsigned long long) bio_end_sector(mbio) - 1);
 				call_bio_endio(r1_bio);
 			}
 		}
@@ -875,7 +873,7 @@ static bool need_to_wait_for_sync(struct r1conf *conf, struct bio *bio)
 		else if ((conf->next_resync - RESYNC_WINDOW_SECTORS
 				>= bio_end_sector(bio)) ||
 			 (conf->next_resync + NEXT_NORMALIO_DISTANCE
-				<= bio->bi_sector))
+				<= bio->bi_iter.bi_sector))
 			wait = false;
 		else
 			wait = true;
@@ -913,14 +911,14 @@ static sector_t wait_barrier(struct r1conf *conf, struct bio *bio)
 
 	if (bio && bio_data_dir(bio) == WRITE) {
 		if (conf->next_resync + NEXT_NORMALIO_DISTANCE
-		    <= bio->bi_sector) {
+		    <= bio->bi_iter.bi_sector) {
 			if (conf->start_next_window == MaxSector)
 				conf->start_next_window =
 					conf->next_resync +
 					NEXT_NORMALIO_DISTANCE;
 
 			if ((conf->start_next_window + NEXT_NORMALIO_DISTANCE)
-			    <= bio->bi_sector)
+			    <= bio->bi_iter.bi_sector)
 				conf->next_window_requests++;
 			else
 				conf->current_window_requests++;
@@ -1027,7 +1025,8 @@ do_sync_io:
 		if (bvecs[i].bv_page)
 			put_page(bvecs[i].bv_page);
 	kfree(bvecs);
-	pr_debug("%dB behind alloc failed, doing sync I/O\n", bio->bi_size);
+	pr_debug("%dB behind alloc failed, doing sync I/O\n",
+		 bio->bi_iter.bi_size);
 }
 
 struct raid1_plug_cb {
@@ -1107,7 +1106,7 @@ static void make_request(struct mddev *mddev, struct bio * bio)
 
 	if (bio_data_dir(bio) == WRITE &&
 	    bio_end_sector(bio) > mddev->suspend_lo &&
-	    bio->bi_sector < mddev->suspend_hi) {
+	    bio->bi_iter.bi_sector < mddev->suspend_hi) {
 		/* As the suspend_* range is controlled by
 		 * userspace, we want an interruptible
 		 * wait.
@@ -1118,7 +1117,7 @@ static void make_request(struct mddev *mddev, struct bio * bio)
 			prepare_to_wait(&conf->wait_barrier,
 					&w, TASK_INTERRUPTIBLE);
 			if (bio_end_sector(bio) <= mddev->suspend_lo ||
-			    bio->bi_sector >= mddev->suspend_hi)
+			    bio->bi_iter.bi_sector >= mddev->suspend_hi)
 				break;
 			schedule();
 		}
@@ -1140,7 +1139,7 @@ static void make_request(struct mddev *mddev, struct bio * bio)
 	r1_bio->sectors = bio_sectors(bio);
 	r1_bio->state = 0;
 	r1_bio->mddev = mddev;
-	r1_bio->sector = bio->bi_sector;
+	r1_bio->sector = bio->bi_iter.bi_sector;
 
 	/* We might need to issue multiple reads to different
 	 * devices if there are bad blocks around, so we keep
@@ -1180,12 +1179,13 @@ read_again:
 		r1_bio->read_disk = rdisk;
 
 		read_bio = bio_clone_mddev(bio, GFP_NOIO, mddev);
-		bio_trim(read_bio, r1_bio->sector - bio->bi_sector,
+		bio_trim(read_bio, r1_bio->sector - bio->bi_iter.bi_sector,
 			 max_sectors);
 
 		r1_bio->bios[rdisk] = read_bio;
 
-		read_bio->bi_sector = r1_bio->sector + mirror->rdev->data_offset;
+		read_bio->bi_iter.bi_sector = r1_bio->sector +
+			mirror->rdev->data_offset;
 		read_bio->bi_bdev = mirror->rdev->bdev;
 		read_bio->bi_end_io = raid1_end_read_request;
 		read_bio->bi_rw = READ | do_sync;
@@ -1197,7 +1197,7 @@ read_again:
 			 */
 
 			sectors_handled = (r1_bio->sector + max_sectors
-					   - bio->bi_sector);
+					   - bio->bi_iter.bi_sector);
 			r1_bio->sectors = max_sectors;
 			spin_lock_irq(&conf->device_lock);
 			if (bio->bi_phys_segments == 0)
@@ -1218,7 +1218,8 @@ read_again:
 			r1_bio->sectors = bio_sectors(bio) - sectors_handled;
 			r1_bio->state = 0;
 			r1_bio->mddev = mddev;
-			r1_bio->sector = bio->bi_sector + sectors_handled;
+			r1_bio->sector = bio->bi_iter.bi_sector +
+				sectors_handled;
 			goto read_again;
 		} else
 			generic_make_request(read_bio);
@@ -1321,7 +1322,7 @@ read_again:
 			if (r1_bio->bios[j])
 				rdev_dec_pending(conf->mirrors[j].rdev, mddev);
 		r1_bio->state = 0;
-		allow_barrier(conf, start_next_window, bio->bi_sector);
+		allow_barrier(conf, start_next_window, bio->bi_iter.bi_sector);
 		md_wait_for_blocked_rdev(blocked_rdev, mddev);
 		start_next_window = wait_barrier(conf, bio);
 		/*
@@ -1348,7 +1349,7 @@ read_again:
 			bio->bi_phys_segments++;
 		spin_unlock_irq(&conf->device_lock);
 	}
-	sectors_handled = r1_bio->sector + max_sectors - bio->bi_sector;
+	sectors_handled = r1_bio->sector + max_sectors - bio->bi_iter.bi_sector;
 
 	atomic_set(&r1_bio->remaining, 1);
 	atomic_set(&r1_bio->behind_remaining, 0);
@@ -1360,7 +1361,7 @@ read_again:
 			continue;
 
 		mbio = bio_clone_mddev(bio, GFP_NOIO, mddev);
-		bio_trim(mbio, r1_bio->sector - bio->bi_sector, max_sectors);
+		bio_trim(mbio, r1_bio->sector - bio->bi_iter.bi_sector, max_sectors);
 
 		if (first_clone) {
 			/* do behind I/O ?
@@ -1394,7 +1395,7 @@ read_again:
 
 		r1_bio->bios[i] = mbio;
 
-		mbio->bi_sector	= (r1_bio->sector +
+		mbio->bi_iter.bi_sector	= (r1_bio->sector +
 				   conf->mirrors[i].rdev->data_offset);
 		mbio->bi_bdev = conf->mirrors[i].rdev->bdev;
 		mbio->bi_end_io	= raid1_end_write_request;
@@ -1434,7 +1435,7 @@ read_again:
 		r1_bio->sectors = bio_sectors(bio) - sectors_handled;
 		r1_bio->state = 0;
 		r1_bio->mddev = mddev;
-		r1_bio->sector = bio->bi_sector + sectors_handled;
+		r1_bio->sector = bio->bi_iter.bi_sector + sectors_handled;
 		goto retry_write;
 	}
 
@@ -1958,14 +1959,14 @@ static int process_checks(struct r1bio *r1_bio)
 		/* fixup the bio for reuse */
 		bio_reset(b);
 		b->bi_vcnt = vcnt;
-		b->bi_size = r1_bio->sectors << 9;
-		b->bi_sector = r1_bio->sector +
+		b->bi_iter.bi_size = r1_bio->sectors << 9;
+		b->bi_iter.bi_sector = r1_bio->sector +
 			conf->mirrors[i].rdev->data_offset;
 		b->bi_bdev = conf->mirrors[i].rdev->bdev;
 		b->bi_end_io = end_sync_read;
 		b->bi_private = r1_bio;
 
-		size = b->bi_size;
+		size = b->bi_iter.bi_size;
 		for (j = 0; j < vcnt ; j++) {
 			struct bio_vec *bi;
 			bi = &b->bi_io_vec[j];
@@ -2220,11 +2221,11 @@ static int narrow_write_error(struct r1bio *r1_bio, int i)
 		}
 
 		wbio->bi_rw = WRITE;
-		wbio->bi_sector = r1_bio->sector;
-		wbio->bi_size = r1_bio->sectors << 9;
+		wbio->bi_iter.bi_sector = r1_bio->sector;
+		wbio->bi_iter.bi_size = r1_bio->sectors << 9;
 
 		bio_trim(wbio, sector - r1_bio->sector, sectors);
-		wbio->bi_sector += rdev->data_offset;
+		wbio->bi_iter.bi_sector += rdev->data_offset;
 		wbio->bi_bdev = rdev->bdev;
 		if (submit_bio_wait(WRITE, wbio) == 0)
 			/* failure! */
@@ -2338,7 +2339,8 @@ read_more:
 		}
 		r1_bio->read_disk = disk;
 		bio = bio_clone_mddev(r1_bio->master_bio, GFP_NOIO, mddev);
-		bio_trim(bio, r1_bio->sector - bio->bi_sector, max_sectors);
+		bio_trim(bio, r1_bio->sector - bio->bi_iter.bi_sector,
+			 max_sectors);
 		r1_bio->bios[r1_bio->read_disk] = bio;
 		rdev = conf->mirrors[disk].rdev;
 		printk_ratelimited(KERN_ERR
@@ -2347,7 +2349,7 @@ read_more:
 				   mdname(mddev),
 				   (unsigned long long)r1_bio->sector,
 				   bdevname(rdev->bdev, b));
-		bio->bi_sector = r1_bio->sector + rdev->data_offset;
+		bio->bi_iter.bi_sector = r1_bio->sector + rdev->data_offset;
 		bio->bi_bdev = rdev->bdev;
 		bio->bi_end_io = raid1_end_read_request;
 		bio->bi_rw = READ | do_sync;
@@ -2356,7 +2358,7 @@ read_more:
 			/* Drat - have to split this up more */
 			struct bio *mbio = r1_bio->master_bio;
 			int sectors_handled = (r1_bio->sector + max_sectors
-					       - mbio->bi_sector);
+					       - mbio->bi_iter.bi_sector);
 			r1_bio->sectors = max_sectors;
 			spin_lock_irq(&conf->device_lock);
 			if (mbio->bi_phys_segments == 0)
@@ -2374,7 +2376,8 @@ read_more:
 			r1_bio->state = 0;
 			set_bit(R1BIO_ReadError, &r1_bio->state);
 			r1_bio->mddev = mddev;
-			r1_bio->sector = mbio->bi_sector + sectors_handled;
+			r1_bio->sector = mbio->bi_iter.bi_sector +
+				sectors_handled;
 
 			goto read_more;
 		} else
@@ -2598,7 +2601,7 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr, int *skipp
 		}
 		if (bio->bi_end_io) {
 			atomic_inc(&rdev->nr_pending);
-			bio->bi_sector = sector_nr + rdev->data_offset;
+			bio->bi_iter.bi_sector = sector_nr + rdev->data_offset;
 			bio->bi_bdev = rdev->bdev;
 			bio->bi_private = r1_bio;
 		}
@@ -2698,7 +2701,7 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr, int *skipp
 							continue;
 						/* remove last page from this bio */
 						bio->bi_vcnt--;
-						bio->bi_size -= len;
+						bio->bi_iter.bi_size -= len;
 						bio->bi_flags &= ~(1<< BIO_SEG_VALID);
 					}
 					goto bio_full;
diff --git a/drivers/md/raid10.c b/drivers/md/raid10.c
index 8d39d63281b9..33fc408e5eac 100644
--- a/drivers/md/raid10.c
+++ b/drivers/md/raid10.c
@@ -1152,14 +1152,12 @@ static void raid10_unplug(struct blk_plug_cb *cb, bool from_schedule)
 	kfree(plug);
 }
 
-static void make_request(struct mddev *mddev, struct bio * bio)
+static void __make_request(struct mddev *mddev, struct bio *bio)
 {
 	struct r10conf *conf = mddev->private;
 	struct r10bio *r10_bio;
 	struct bio *read_bio;
 	int i;
-	sector_t chunk_mask = (conf->geo.chunk_mask & conf->prev.chunk_mask);
-	int chunk_sects = chunk_mask + 1;
 	const int rw = bio_data_dir(bio);
 	const unsigned long do_sync = (bio->bi_rw & REQ_SYNC);
 	const unsigned long do_fua = (bio->bi_rw & REQ_FUA);
@@ -1174,88 +1172,27 @@ static void make_request(struct mddev *mddev, struct bio * bio)
 	int max_sectors;
 	int sectors;
 
-	if (unlikely(bio->bi_rw & REQ_FLUSH)) {
-		md_flush_request(mddev, bio);
-		return;
-	}
-
-	/* If this request crosses a chunk boundary, we need to
-	 * split it.  This will only happen for 1 PAGE (or less) requests.
-	 */
-	if (unlikely((bio->bi_sector & chunk_mask) + bio_sectors(bio)
-		     > chunk_sects
-		     && (conf->geo.near_copies < conf->geo.raid_disks
-			 || conf->prev.near_copies < conf->prev.raid_disks))) {
-		struct bio_pair *bp;
-		/* Sanity check -- queue functions should prevent this happening */
-		if (bio_segments(bio) > 1)
-			goto bad_map;
-		/* This is a one page bio that upper layers
-		 * refuse to split for us, so we need to split it.
-		 */
-		bp = bio_split(bio,
-			       chunk_sects - (bio->bi_sector & (chunk_sects - 1)) );
-
-		/* Each of these 'make_request' calls will call 'wait_barrier'.
-		 * If the first succeeds but the second blocks due to the resync
-		 * thread raising the barrier, we will deadlock because the
-		 * IO to the underlying device will be queued in generic_make_request
-		 * and will never complete, so will never reduce nr_pending.
-		 * So increment nr_waiting here so no new raise_barriers will
-		 * succeed, and so the second wait_barrier cannot block.
-		 */
-		spin_lock_irq(&conf->resync_lock);
-		conf->nr_waiting++;
-		spin_unlock_irq(&conf->resync_lock);
-
-		make_request(mddev, &bp->bio1);
-		make_request(mddev, &bp->bio2);
-
-		spin_lock_irq(&conf->resync_lock);
-		conf->nr_waiting--;
-		wake_up(&conf->wait_barrier);
-		spin_unlock_irq(&conf->resync_lock);
-
-		bio_pair_release(bp);
-		return;
-	bad_map:
-		printk("md/raid10:%s: make_request bug: can't convert block across chunks"
-		       " or bigger than %dk %llu %d\n", mdname(mddev), chunk_sects/2,
-		       (unsigned long long)bio->bi_sector, bio_sectors(bio) / 2);
-
-		bio_io_error(bio);
-		return;
-	}
-
-	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);
-
 	sectors = bio_sectors(bio);
 	while (test_bit(MD_RECOVERY_RESHAPE, &mddev->recovery) &&
-	    bio->bi_sector < conf->reshape_progress &&
-	    bio->bi_sector + sectors > conf->reshape_progress) {
+	    bio->bi_iter.bi_sector < conf->reshape_progress &&
+	    bio->bi_iter.bi_sector + sectors > conf->reshape_progress) {
 		/* IO spans the reshape position.  Need to wait for
 		 * reshape to pass
 		 */
 		allow_barrier(conf);
 		wait_event(conf->wait_barrier,
-			   conf->reshape_progress <= bio->bi_sector ||
-			   conf->reshape_progress >= bio->bi_sector + sectors);
+			   conf->reshape_progress <= bio->bi_iter.bi_sector ||
+			   conf->reshape_progress >= bio->bi_iter.bi_sector +
+			   sectors);
 		wait_barrier(conf);
 	}
 	if (test_bit(MD_RECOVERY_RESHAPE, &mddev->recovery) &&
 	    bio_data_dir(bio) == WRITE &&
 	    (mddev->reshape_backwards
-	     ? (bio->bi_sector < conf->reshape_safe &&
-		bio->bi_sector + sectors > conf->reshape_progress)
-	     : (bio->bi_sector + sectors > conf->reshape_safe &&
-		bio->bi_sector < conf->reshape_progress))) {
+	     ? (bio->bi_iter.bi_sector < conf->reshape_safe &&
+		bio->bi_iter.bi_sector + sectors > conf->reshape_progress)
+	     : (bio->bi_iter.bi_sector + sectors > conf->reshape_safe &&
+		bio->bi_iter.bi_sector < conf->reshape_progress))) {
 		/* Need to update reshape_position in metadata */
 		mddev->reshape_position = conf->reshape_progress;
 		set_bit(MD_CHANGE_DEVS, &mddev->flags);
@@ -1273,7 +1210,7 @@ static void make_request(struct mddev *mddev, struct bio * bio)
 	r10_bio->sectors = sectors;
 
 	r10_bio->mddev = mddev;
-	r10_bio->sector = bio->bi_sector;
+	r10_bio->sector = bio->bi_iter.bi_sector;
 	r10_bio->state = 0;
 
 	/* We might need to issue multiple reads to different
@@ -1302,13 +1239,13 @@ read_again:
 		slot = r10_bio->read_slot;
 
 		read_bio = bio_clone_mddev(bio, GFP_NOIO, mddev);
-		bio_trim(read_bio, r10_bio->sector - bio->bi_sector,
+		bio_trim(read_bio, r10_bio->sector - bio->bi_iter.bi_sector,
 			 max_sectors);
 
 		r10_bio->devs[slot].bio = read_bio;
 		r10_bio->devs[slot].rdev = rdev;
 
-		read_bio->bi_sector = r10_bio->devs[slot].addr +
+		read_bio->bi_iter.bi_sector = r10_bio->devs[slot].addr +
 			choose_data_offset(r10_bio, rdev);
 		read_bio->bi_bdev = rdev->bdev;
 		read_bio->bi_end_io = raid10_end_read_request;
@@ -1320,7 +1257,7 @@ read_again:
 			 * need another r10_bio.
 			 */
 			sectors_handled = (r10_bio->sector + max_sectors
-					   - bio->bi_sector);
+					   - bio->bi_iter.bi_sector);
 			r10_bio->sectors = max_sectors;
 			spin_lock_irq(&conf->device_lock);
 			if (bio->bi_phys_segments == 0)
@@ -1341,7 +1278,8 @@ read_again:
 			r10_bio->sectors = bio_sectors(bio) - sectors_handled;
 			r10_bio->state = 0;
 			r10_bio->mddev = mddev;
-			r10_bio->sector = bio->bi_sector + sectors_handled;
+			r10_bio->sector = bio->bi_iter.bi_sector +
+				sectors_handled;
 			goto read_again;
 		} else
 			generic_make_request(read_bio);
@@ -1499,7 +1437,8 @@ retry_write:
 			bio->bi_phys_segments++;
 		spin_unlock_irq(&conf->device_lock);
 	}
-	sectors_handled = r10_bio->sector + max_sectors - bio->bi_sector;
+	sectors_handled = r10_bio->sector + max_sectors -
+		bio->bi_iter.bi_sector;
 
 	atomic_set(&r10_bio->remaining, 1);
 	bitmap_startwrite(mddev->bitmap, r10_bio->sector, r10_bio->sectors, 0);
@@ -1510,11 +1449,11 @@ retry_write:
 		if (r10_bio->devs[i].bio) {
 			struct md_rdev *rdev = conf->mirrors[d].rdev;
 			mbio = bio_clone_mddev(bio, GFP_NOIO, mddev);
-			bio_trim(mbio, r10_bio->sector - bio->bi_sector,
+			bio_trim(mbio, r10_bio->sector - bio->bi_iter.bi_sector,
 				 max_sectors);
 			r10_bio->devs[i].bio = mbio;
 
-			mbio->bi_sector	= (r10_bio->devs[i].addr+
+			mbio->bi_iter.bi_sector	= (r10_bio->devs[i].addr+
 					   choose_data_offset(r10_bio,
 							      rdev));
 			mbio->bi_bdev = rdev->bdev;
@@ -1553,11 +1492,11 @@ retry_write:
 				rdev = conf->mirrors[d].rdev;
 			}
 			mbio = bio_clone_mddev(bio, GFP_NOIO, mddev);
-			bio_trim(mbio, r10_bio->sector - bio->bi_sector,
+			bio_trim(mbio, r10_bio->sector - bio->bi_iter.bi_sector,
 				 max_sectors);
 			r10_bio->devs[i].repl_bio = mbio;
 
-			mbio->bi_sector	= (r10_bio->devs[i].addr +
+			mbio->bi_iter.bi_sector	= (r10_bio->devs[i].addr +
 					   choose_data_offset(
 						   r10_bio, rdev));
 			mbio->bi_bdev = rdev->bdev;
@@ -1591,11 +1530,57 @@ retry_write:
 		r10_bio->sectors = bio_sectors(bio) - sectors_handled;
 
 		r10_bio->mddev = mddev;
-		r10_bio->sector = bio->bi_sector + sectors_handled;
+		r10_bio->sector = bio->bi_iter.bi_sector + sectors_handled;
 		r10_bio->state = 0;
 		goto retry_write;
 	}
 	one_write_done(r10_bio);
+}
+
+static void make_request(struct mddev *mddev, struct bio *bio)
+{
+	struct r10conf *conf = mddev->private;
+	sector_t chunk_mask = (conf->geo.chunk_mask & conf->prev.chunk_mask);
+	int chunk_sects = chunk_mask + 1;
+
+	struct bio *split;
+
+	if (unlikely(bio->bi_rw & REQ_FLUSH)) {
+		md_flush_request(mddev, bio);
+		return;
+	}
+
+	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 {
+
+		/*
+		 * If this request crosses a chunk boundary, we need to split
+		 * it.
+		 */
+		if (unlikely((bio->bi_iter.bi_sector & chunk_mask) +
+			     bio_sectors(bio) > chunk_sects
+			     && (conf->geo.near_copies < conf->geo.raid_disks
+				 || conf->prev.near_copies <
+				 conf->prev.raid_disks))) {
+			split = bio_split(bio, chunk_sects -
+					  (bio->bi_iter.bi_sector &
+					   (chunk_sects - 1)),
+					  GFP_NOIO, fs_bio_set);
+			bio_chain(split, bio);
+		} else {
+			split = bio;
+		}
+
+		__make_request(mddev, split);
+	} while (split != bio);
 
 	/* In case raid10d snuck in to freeze_array */
 	wake_up(&conf->wait_barrier);
@@ -2124,10 +2109,10 @@ static void sync_request_write(struct mddev *mddev, struct r10bio *r10_bio)
 		bio_reset(tbio);
 
 		tbio->bi_vcnt = vcnt;
-		tbio->bi_size = r10_bio->sectors << 9;
+		tbio->bi_iter.bi_size = r10_bio->sectors << 9;
 		tbio->bi_rw = WRITE;
 		tbio->bi_private = r10_bio;
-		tbio->bi_sector = r10_bio->devs[i].addr;
+		tbio->bi_iter.bi_sector = r10_bio->devs[i].addr;
 
 		for (j=0; j < vcnt ; j++) {
 			tbio->bi_io_vec[j].bv_offset = 0;
@@ -2144,7 +2129,7 @@ static void sync_request_write(struct mddev *mddev, struct r10bio *r10_bio)
 		atomic_inc(&r10_bio->remaining);
 		md_sync_acct(conf->mirrors[d].rdev->bdev, bio_sectors(tbio));
 
-		tbio->bi_sector += conf->mirrors[d].rdev->data_offset;
+		tbio->bi_iter.bi_sector += conf->mirrors[d].rdev->data_offset;
 		tbio->bi_bdev = conf->mirrors[d].rdev->bdev;
 		generic_make_request(tbio);
 	}
@@ -2614,8 +2599,8 @@ static int narrow_write_error(struct r10bio *r10_bio, int i)
 			sectors = sect_to_write;
 		/* Write at 'sector' for 'sectors' */
 		wbio = bio_clone_mddev(bio, GFP_NOIO, mddev);
-		bio_trim(wbio, sector - bio->bi_sector, sectors);
-		wbio->bi_sector = (r10_bio->devs[i].addr+
+		bio_trim(wbio, sector - bio->bi_iter.bi_sector, sectors);
+		wbio->bi_iter.bi_sector = (r10_bio->devs[i].addr+
 				   choose_data_offset(r10_bio, rdev) +
 				   (sector - r10_bio->sector));
 		wbio->bi_bdev = rdev->bdev;
@@ -2687,10 +2672,10 @@ read_more:
 		(unsigned long long)r10_bio->sector);
 	bio = bio_clone_mddev(r10_bio->master_bio,
 			      GFP_NOIO, mddev);
-	bio_trim(bio, r10_bio->sector - bio->bi_sector, max_sectors);
+	bio_trim(bio, r10_bio->sector - bio->bi_iter.bi_sector, max_sectors);
 	r10_bio->devs[slot].bio = bio;
 	r10_bio->devs[slot].rdev = rdev;
-	bio->bi_sector = r10_bio->devs[slot].addr
+	bio->bi_iter.bi_sector = r10_bio->devs[slot].addr
 		+ choose_data_offset(r10_bio, rdev);
 	bio->bi_bdev = rdev->bdev;
 	bio->bi_rw = READ | do_sync;
@@ -2701,7 +2686,7 @@ read_more:
 		struct bio *mbio = r10_bio->master_bio;
 		int sectors_handled =
 			r10_bio->sector + max_sectors
-			- mbio->bi_sector;
+			- mbio->bi_iter.bi_sector;
 		r10_bio->sectors = max_sectors;
 		spin_lock_irq(&conf->device_lock);
 		if (mbio->bi_phys_segments == 0)
@@ -2719,7 +2704,7 @@ read_more:
 		set_bit(R10BIO_ReadError,
 			&r10_bio->state);
 		r10_bio->mddev = mddev;
-		r10_bio->sector = mbio->bi_sector
+		r10_bio->sector = mbio->bi_iter.bi_sector
 			+ sectors_handled;
 
 		goto read_more;
@@ -3157,7 +3142,8 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
 				bio->bi_end_io = end_sync_read;
 				bio->bi_rw = READ;
 				from_addr = r10_bio->devs[j].addr;
-				bio->bi_sector = from_addr + rdev->data_offset;
+				bio->bi_iter.bi_sector = from_addr +
+					rdev->data_offset;
 				bio->bi_bdev = rdev->bdev;
 				atomic_inc(&rdev->nr_pending);
 				/* and we write to 'i' (if not in_sync) */
@@ -3181,7 +3167,7 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
 					bio->bi_private = r10_bio;
 					bio->bi_end_io = end_sync_write;
 					bio->bi_rw = WRITE;
-					bio->bi_sector = to_addr
+					bio->bi_iter.bi_sector = to_addr
 						+ rdev->data_offset;
 					bio->bi_bdev = rdev->bdev;
 					atomic_inc(&r10_bio->remaining);
@@ -3210,7 +3196,8 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
 				bio->bi_private = r10_bio;
 				bio->bi_end_io = end_sync_write;
 				bio->bi_rw = WRITE;
-				bio->bi_sector = to_addr + rdev->data_offset;
+				bio->bi_iter.bi_sector = to_addr +
+					rdev->data_offset;
 				bio->bi_bdev = rdev->bdev;
 				atomic_inc(&r10_bio->remaining);
 				break;
@@ -3328,7 +3315,7 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
 			bio->bi_private = r10_bio;
 			bio->bi_end_io = end_sync_read;
 			bio->bi_rw = READ;
-			bio->bi_sector = sector +
+			bio->bi_iter.bi_sector = sector +
 				conf->mirrors[d].rdev->data_offset;
 			bio->bi_bdev = conf->mirrors[d].rdev->bdev;
 			count++;
@@ -3350,7 +3337,7 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
 			bio->bi_private = r10_bio;
 			bio->bi_end_io = end_sync_write;
 			bio->bi_rw = WRITE;
-			bio->bi_sector = sector +
+			bio->bi_iter.bi_sector = sector +
 				conf->mirrors[d].replacement->data_offset;
 			bio->bi_bdev = conf->mirrors[d].replacement->bdev;
 			count++;
@@ -3397,7 +3384,7 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
 			     bio2 = bio2->bi_next) {
 				/* remove last page from this bio */
 				bio2->bi_vcnt--;
-				bio2->bi_size -= len;
+				bio2->bi_iter.bi_size -= len;
 				bio2->bi_flags &= ~(1<< BIO_SEG_VALID);
 			}
 			goto bio_full;
@@ -4418,7 +4405,7 @@ read_more:
 	read_bio = bio_alloc_mddev(GFP_KERNEL, RESYNC_PAGES, mddev);
 
 	read_bio->bi_bdev = rdev->bdev;
-	read_bio->bi_sector = (r10_bio->devs[r10_bio->read_slot].addr
+	read_bio->bi_iter.bi_sector = (r10_bio->devs[r10_bio->read_slot].addr
 			       + rdev->data_offset);
 	read_bio->bi_private = r10_bio;
 	read_bio->bi_end_io = end_sync_read;
@@ -4426,7 +4413,7 @@ read_more:
 	read_bio->bi_flags &= ~(BIO_POOL_MASK - 1);
 	read_bio->bi_flags |= 1 << BIO_UPTODATE;
 	read_bio->bi_vcnt = 0;
-	read_bio->bi_size = 0;
+	read_bio->bi_iter.bi_size = 0;
 	r10_bio->master_bio = read_bio;
 	r10_bio->read_slot = r10_bio->devs[r10_bio->read_slot].devnum;
 
@@ -4452,7 +4439,8 @@ read_more:
 
 		bio_reset(b);
 		b->bi_bdev = rdev2->bdev;
-		b->bi_sector = r10_bio->devs[s/2].addr + rdev2->new_data_offset;
+		b->bi_iter.bi_sector = r10_bio->devs[s/2].addr +
+			rdev2->new_data_offset;
 		b->bi_private = r10_bio;
 		b->bi_end_io = end_reshape_write;
 		b->bi_rw = WRITE;
@@ -4479,7 +4467,7 @@ read_more:
 			     bio2 = bio2->bi_next) {
 				/* Remove last page from this bio */
 				bio2->bi_vcnt--;
-				bio2->bi_size -= len;
+				bio2->bi_iter.bi_size -= len;
 				bio2->bi_flags &= ~(1<<BIO_SEG_VALID);
 			}
 			goto bio_full;
diff --git a/drivers/md/raid5.c b/drivers/md/raid5.c
index 03f82ab87d9e..f1feadeb7bb2 100644
--- a/drivers/md/raid5.c
+++ b/drivers/md/raid5.c
@@ -133,7 +133,7 @@ static inline void unlock_all_device_hash_locks_irq(struct r5conf *conf)
 static inline struct bio *r5_next_bio(struct bio *bio, sector_t sector)
 {
 	int sectors = bio_sectors(bio);
-	if (bio->bi_sector + sectors < sector + STRIPE_SECTORS)
+	if (bio->bi_iter.bi_sector + sectors < sector + STRIPE_SECTORS)
 		return bio->bi_next;
 	else
 		return NULL;
@@ -225,7 +225,7 @@ static void return_io(struct bio *return_bi)
 
 		return_bi = bi->bi_next;
 		bi->bi_next = NULL;
-		bi->bi_size = 0;
+		bi->bi_iter.bi_size = 0;
 		trace_block_bio_complete(bdev_get_queue(bi->bi_bdev),
 					 bi, 0);
 		bio_endio(bi, 0);
@@ -852,10 +852,10 @@ static void ops_run_io(struct stripe_head *sh, struct stripe_head_state *s)
 				bi->bi_rw, i);
 			atomic_inc(&sh->count);
 			if (use_new_offset(conf, sh))
-				bi->bi_sector = (sh->sector
+				bi->bi_iter.bi_sector = (sh->sector
 						 + rdev->new_data_offset);
 			else
-				bi->bi_sector = (sh->sector
+				bi->bi_iter.bi_sector = (sh->sector
 						 + rdev->data_offset);
 			if (test_bit(R5_ReadNoMerge, &sh->dev[i].flags))
 				bi->bi_rw |= REQ_NOMERGE;
@@ -863,7 +863,7 @@ static void ops_run_io(struct stripe_head *sh, struct stripe_head_state *s)
 			bi->bi_vcnt = 1;
 			bi->bi_io_vec[0].bv_len = STRIPE_SIZE;
 			bi->bi_io_vec[0].bv_offset = 0;
-			bi->bi_size = STRIPE_SIZE;
+			bi->bi_iter.bi_size = STRIPE_SIZE;
 			/*
 			 * If this is discard request, set bi_vcnt 0. We don't
 			 * want to confuse SCSI because SCSI will replace payload
@@ -899,15 +899,15 @@ static void ops_run_io(struct stripe_head *sh, struct stripe_head_state *s)
 				rbi->bi_rw, i);
 			atomic_inc(&sh->count);
 			if (use_new_offset(conf, sh))
-				rbi->bi_sector = (sh->sector
+				rbi->bi_iter.bi_sector = (sh->sector
 						  + rrdev->new_data_offset);
 			else
-				rbi->bi_sector = (sh->sector
+				rbi->bi_iter.bi_sector = (sh->sector
 						  + rrdev->data_offset);
 			rbi->bi_vcnt = 1;
 			rbi->bi_io_vec[0].bv_len = STRIPE_SIZE;
 			rbi->bi_io_vec[0].bv_offset = 0;
-			rbi->bi_size = STRIPE_SIZE;
+			rbi->bi_iter.bi_size = STRIPE_SIZE;
 			/*
 			 * If this is discard request, set bi_vcnt 0. We don't
 			 * want to confuse SCSI because SCSI will replace payload
@@ -935,24 +935,24 @@ 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)
 {
-	struct bio_vec *bvl;
+	struct bio_vec bvl;
+	struct bvec_iter iter;
 	struct page *bio_page;
-	int i;
 	int page_offset;
 	struct async_submit_ctl submit;
 	enum async_tx_flags flags = 0;
 
-	if (bio->bi_sector >= sector)
-		page_offset = (signed)(bio->bi_sector - sector) * 512;
+	if (bio->bi_iter.bi_sector >= sector)
+		page_offset = (signed)(bio->bi_iter.bi_sector - sector) * 512;
 	else
-		page_offset = (signed)(sector - bio->bi_sector) * -512;
+		page_offset = (signed)(sector - bio->bi_iter.bi_sector) * -512;
 
 	if (frombio)
 		flags |= ASYNC_TX_FENCE;
 	init_async_submit(&submit, flags, tx, NULL, NULL, NULL);
 
-	bio_for_each_segment(bvl, bio, i) {
-		int len = bvl->bv_len;
+	bio_for_each_segment(bvl, bio, iter) {
+		int len = bvl.bv_len;
 		int clen;
 		int b_offset = 0;
 
@@ -968,8 +968,8 @@ async_copy_data(int frombio, struct bio *bio, struct page *page,
 			clen = len;
 
 		if (clen > 0) {
-			b_offset += bvl->bv_offset;
-			bio_page = bvl->bv_page;
+			b_offset += bvl.bv_offset;
+			bio_page = bvl.bv_page;
 			if (frombio)
 				tx = async_memcpy(page, bio_page, page_offset,
 						  b_offset, clen, &submit);
@@ -1012,7 +1012,7 @@ static void ops_complete_biofill(void *stripe_head_ref)
 			BUG_ON(!dev->read);
 			rbi = dev->read;
 			dev->read = NULL;
-			while (rbi && rbi->bi_sector <
+			while (rbi && rbi->bi_iter.bi_sector <
 				dev->sector + STRIPE_SECTORS) {
 				rbi2 = r5_next_bio(rbi, dev->sector);
 				if (!raid5_dec_bi_active_stripes(rbi)) {
@@ -1048,7 +1048,7 @@ static void ops_run_biofill(struct stripe_head *sh)
 			dev->read = rbi = dev->toread;
 			dev->toread = NULL;
 			spin_unlock_irq(&sh->stripe_lock);
-			while (rbi && rbi->bi_sector <
+			while (rbi && rbi->bi_iter.bi_sector <
 				dev->sector + STRIPE_SECTORS) {
 				tx = async_copy_data(0, rbi, dev->page,
 					dev->sector, tx);
@@ -1390,7 +1390,7 @@ ops_run_biodrain(struct stripe_head *sh, struct dma_async_tx_descriptor *tx)
 			wbi = dev->written = chosen;
 			spin_unlock_irq(&sh->stripe_lock);
 
-			while (wbi && wbi->bi_sector <
+			while (wbi && wbi->bi_iter.bi_sector <
 				dev->sector + STRIPE_SECTORS) {
 				if (wbi->bi_rw & REQ_FUA)
 					set_bit(R5_WantFUA, &dev->flags);
@@ -2615,7 +2615,7 @@ static int add_stripe_bio(struct stripe_head *sh, struct bio *bi, int dd_idx, in
 	int firstwrite=0;
 
 	pr_debug("adding bi b#%llu to stripe s#%llu\n",
-		(unsigned long long)bi->bi_sector,
+		(unsigned long long)bi->bi_iter.bi_sector,
 		(unsigned long long)sh->sector);
 
 	/*
@@ -2633,12 +2633,12 @@ static int add_stripe_bio(struct stripe_head *sh, struct bio *bi, int dd_idx, in
 			firstwrite = 1;
 	} else
 		bip = &sh->dev[dd_idx].toread;
-	while (*bip && (*bip)->bi_sector < bi->bi_sector) {
-		if (bio_end_sector(*bip) > bi->bi_sector)
+	while (*bip && (*bip)->bi_iter.bi_sector < bi->bi_iter.bi_sector) {
+		if (bio_end_sector(*bip) > bi->bi_iter.bi_sector)
 			goto overlap;
 		bip = & (*bip)->bi_next;
 	}
-	if (*bip && (*bip)->bi_sector < bio_end_sector(bi))
+	if (*bip && (*bip)->bi_iter.bi_sector < bio_end_sector(bi))
 		goto overlap;
 
 	BUG_ON(*bip && bi->bi_next && (*bip) != bi->bi_next);
@@ -2652,7 +2652,7 @@ static int add_stripe_bio(struct stripe_head *sh, struct bio *bi, int dd_idx, in
 		sector_t sector = sh->dev[dd_idx].sector;
 		for (bi=sh->dev[dd_idx].towrite;
 		     sector < sh->dev[dd_idx].sector + STRIPE_SECTORS &&
-			     bi && bi->bi_sector <= sector;
+			     bi && bi->bi_iter.bi_sector <= sector;
 		     bi = r5_next_bio(bi, sh->dev[dd_idx].sector)) {
 			if (bio_end_sector(bi) >= sector)
 				sector = bio_end_sector(bi);
@@ -2662,7 +2662,7 @@ static int add_stripe_bio(struct stripe_head *sh, struct bio *bi, int dd_idx, in
 	}
 
 	pr_debug("added bi b#%llu to stripe s#%llu, disk %d.\n",
-		(unsigned long long)(*bip)->bi_sector,
+		(unsigned long long)(*bip)->bi_iter.bi_sector,
 		(unsigned long long)sh->sector, dd_idx);
 	spin_unlock_irq(&sh->stripe_lock);
 
@@ -2737,7 +2737,7 @@ handle_failed_stripe(struct r5conf *conf, struct stripe_head *sh,
 		if (test_and_clear_bit(R5_Overlap, &sh->dev[i].flags))
 			wake_up(&conf->wait_for_overlap);
 
-		while (bi && bi->bi_sector <
+		while (bi && bi->bi_iter.bi_sector <
 			sh->dev[i].sector + STRIPE_SECTORS) {
 			struct bio *nextbi = r5_next_bio(bi, sh->dev[i].sector);
 			clear_bit(BIO_UPTODATE, &bi->bi_flags);
@@ -2756,7 +2756,7 @@ handle_failed_stripe(struct r5conf *conf, struct stripe_head *sh,
 		bi = sh->dev[i].written;
 		sh->dev[i].written = NULL;
 		if (bi) bitmap_end = 1;
-		while (bi && bi->bi_sector <
+		while (bi && bi->bi_iter.bi_sector <
 		       sh->dev[i].sector + STRIPE_SECTORS) {
 			struct bio *bi2 = r5_next_bio(bi, sh->dev[i].sector);
 			clear_bit(BIO_UPTODATE, &bi->bi_flags);
@@ -2780,7 +2780,7 @@ handle_failed_stripe(struct r5conf *conf, struct stripe_head *sh,
 			spin_unlock_irq(&sh->stripe_lock);
 			if (test_and_clear_bit(R5_Overlap, &sh->dev[i].flags))
 				wake_up(&conf->wait_for_overlap);
-			while (bi && bi->bi_sector <
+			while (bi && bi->bi_iter.bi_sector <
 			       sh->dev[i].sector + STRIPE_SECTORS) {
 				struct bio *nextbi =
 					r5_next_bio(bi, sh->dev[i].sector);
@@ -3004,7 +3004,7 @@ static void handle_stripe_clean_event(struct r5conf *conf,
 					clear_bit(R5_UPTODATE, &dev->flags);
 				wbi = dev->written;
 				dev->written = NULL;
-				while (wbi && wbi->bi_sector <
+				while (wbi && wbi->bi_iter.bi_sector <
 					dev->sector + STRIPE_SECTORS) {
 					wbi2 = r5_next_bio(wbi, dev->sector);
 					if (!raid5_dec_bi_active_stripes(wbi)) {
@@ -4096,7 +4096,7 @@ static int raid5_mergeable_bvec(struct request_queue *q,
 
 static int in_chunk_boundary(struct mddev *mddev, struct bio *bio)
 {
-	sector_t sector = bio->bi_sector + get_start_sect(bio->bi_bdev);
+	sector_t sector = bio->bi_iter.bi_sector + get_start_sect(bio->bi_bdev);
 	unsigned int chunk_sectors = mddev->chunk_sectors;
 	unsigned int bio_sectors = bio_sectors(bio);
 
@@ -4233,9 +4233,9 @@ static int chunk_aligned_read(struct mddev *mddev, struct bio * raid_bio)
 	/*
 	 *	compute position
 	 */
-	align_bi->bi_sector =  raid5_compute_sector(conf, raid_bio->bi_sector,
-						    0,
-						    &dd_idx, NULL);
+	align_bi->bi_iter.bi_sector =
+		raid5_compute_sector(conf, raid_bio->bi_iter.bi_sector,
+				     0, &dd_idx, NULL);
 
 	end_sector = bio_end_sector(align_bi);
 	rcu_read_lock();
@@ -4260,7 +4260,8 @@ static int chunk_aligned_read(struct mddev *mddev, struct bio * raid_bio)
 		align_bi->bi_flags &= ~(1 << BIO_SEG_VALID);
 
 		if (!bio_fits_rdev(align_bi) ||
-		    is_badblock(rdev, align_bi->bi_sector, bio_sectors(align_bi),
+		    is_badblock(rdev, align_bi->bi_iter.bi_sector,
+				bio_sectors(align_bi),
 				&first_bad, &bad_sectors)) {
 			/* too big in some way, or has a known bad block */
 			bio_put(align_bi);
@@ -4269,7 +4270,7 @@ static int chunk_aligned_read(struct mddev *mddev, struct bio * raid_bio)
 		}
 
 		/* No reshape active, so we can trust rdev->data_offset */
-		align_bi->bi_sector += rdev->data_offset;
+		align_bi->bi_iter.bi_sector += rdev->data_offset;
 
 		spin_lock_irq(&conf->device_lock);
 		wait_event_lock_irq(conf->wait_for_stripe,
@@ -4281,7 +4282,7 @@ static int chunk_aligned_read(struct mddev *mddev, struct bio * raid_bio)
 		if (mddev->gendisk)
 			trace_block_bio_remap(bdev_get_queue(align_bi->bi_bdev),
 					      align_bi, disk_devt(mddev->gendisk),
-					      raid_bio->bi_sector);
+					      raid_bio->bi_iter.bi_sector);
 		generic_make_request(align_bi);
 		return 1;
 	} else {
@@ -4464,8 +4465,8 @@ static void make_discard_request(struct mddev *mddev, struct bio *bi)
 		/* Skip discard while reshape is happening */
 		return;
 
-	logical_sector = bi->bi_sector & ~((sector_t)STRIPE_SECTORS-1);
-	last_sector = bi->bi_sector + (bi->bi_size>>9);
+	logical_sector = bi->bi_iter.bi_sector & ~((sector_t)STRIPE_SECTORS-1);
+	last_sector = bi->bi_iter.bi_sector + (bi->bi_iter.bi_size>>9);
 
 	bi->bi_next = NULL;
 	bi->bi_phys_segments = 1; /* over-loaded to count active stripes */
@@ -4569,7 +4570,7 @@ static void make_request(struct mddev *mddev, struct bio * bi)
 		return;
 	}
 
-	logical_sector = bi->bi_sector & ~((sector_t)STRIPE_SECTORS-1);
+	logical_sector = bi->bi_iter.bi_sector & ~((sector_t)STRIPE_SECTORS-1);
 	last_sector = bio_end_sector(bi);
 	bi->bi_next = NULL;
 	bi->bi_phys_segments = 1;	/* over-loaded to count active stripes */
@@ -5053,7 +5054,8 @@ static int  retry_aligned_read(struct r5conf *conf, struct bio *raid_bio)
 	int remaining;
 	int handled = 0;
 
-	logical_sector = raid_bio->bi_sector & ~((sector_t)STRIPE_SECTORS-1);
+	logical_sector = raid_bio->bi_iter.bi_sector &
+		~((sector_t)STRIPE_SECTORS-1);
 	sector = raid5_compute_sector(conf, logical_sector,
 				      0, &dd_idx, NULL);
 	last_sector = bio_end_sector(raid_bio);
@@ -6101,6 +6103,7 @@ static int run(struct mddev *mddev)
 		blk_queue_io_min(mddev->queue, chunk_size);
 		blk_queue_io_opt(mddev->queue, chunk_size *
 				 (conf->raid_disks - conf->max_degraded));
+		mddev->queue->limits.raid_partial_stripes_expensive = 1;
 		/*
 		 * We can only discard a whole stripe. It doesn't make sense to
 		 * discard data disk but write parity disk
diff --git a/drivers/media/Kconfig b/drivers/media/Kconfig
index 8270388e2a0d..1d0758aeb8e4 100644
--- a/drivers/media/Kconfig
+++ b/drivers/media/Kconfig
@@ -172,6 +172,9 @@ comment "Media ancillary drivers (tuners, sensors, i2c, frontends)"
 config MEDIA_SUBDRV_AUTOSELECT
 	bool "Autoselect ancillary drivers (tuners, sensors, i2c, frontends)"
 	depends on MEDIA_ANALOG_TV_SUPPORT || MEDIA_DIGITAL_TV_SUPPORT || MEDIA_CAMERA_SUPPORT
+	depends on HAS_IOMEM
+	select I2C
+	select I2C_MUX
 	default y
 	help
 	  By default, a media driver auto-selects all possible ancillary
diff --git a/drivers/media/dvb-core/dvb-usb-ids.h b/drivers/media/dvb-core/dvb-usb-ids.h
index 419a2d6b4349..f19a2ccd1e4b 100644
--- a/drivers/media/dvb-core/dvb-usb-ids.h
+++ b/drivers/media/dvb-core/dvb-usb-ids.h
@@ -239,6 +239,7 @@
 #define USB_PID_AVERMEDIA_A835B_4835			0x4835
 #define USB_PID_AVERMEDIA_1867				0x1867
 #define USB_PID_AVERMEDIA_A867				0xa867
+#define USB_PID_AVERMEDIA_H335				0x0335
 #define USB_PID_AVERMEDIA_TWINSTAR			0x0825
 #define USB_PID_TECHNOTREND_CONNECT_S2400               0x3006
 #define USB_PID_TECHNOTREND_CONNECT_S2400_8KEEPROM	0x3009
@@ -317,6 +318,7 @@
 #define USB_PID_WINFAST_DTV_DONGLE_H			0x60f6
 #define USB_PID_WINFAST_DTV_DONGLE_STK7700P_2		0x6f01
 #define USB_PID_WINFAST_DTV_DONGLE_GOLD			0x6029
+#define USB_PID_WINFAST_DTV_DONGLE_MINID		0x6f0f
 #define USB_PID_GENPIX_8PSK_REV_1_COLD			0x0200
 #define USB_PID_GENPIX_8PSK_REV_1_WARM			0x0201
 #define USB_PID_GENPIX_8PSK_REV_2			0x0202
@@ -365,6 +367,7 @@
 #define USB_PID_TERRATEC_DVBS2CI_V2			0x10ac
 #define USB_PID_TECHNISAT_USB2_HDCI_V1			0x0001
 #define USB_PID_TECHNISAT_USB2_HDCI_V2			0x0002
+#define USB_PID_TECHNISAT_USB2_CABLESTAR_HDCI		0x0003
 #define USB_PID_TECHNISAT_AIRSTAR_TELESTICK_2		0x0004
 #define USB_PID_TECHNISAT_USB2_DVB_S2			0x0500
 #define USB_PID_CPYTO_REDI_PC50A			0xa803
diff --git a/drivers/media/dvb-frontends/Kconfig b/drivers/media/dvb-frontends/Kconfig
index bddbab43a2df..dd12a1ebda82 100644
--- a/drivers/media/dvb-frontends/Kconfig
+++ b/drivers/media/dvb-frontends/Kconfig
@@ -35,6 +35,13 @@ config DVB_STV6110x
 	help
 	  A Silicon tuner that supports DVB-S and DVB-S2 modes
 
+config DVB_M88DS3103
+	tristate "Montage M88DS3103"
+	depends on DVB_CORE && I2C && I2C_MUX
+	default m if !MEDIA_SUBDRV_AUTOSELECT
+	help
+	  Say Y when you want to support this frontend.
+
 comment "Multistandard (cable + terrestrial) frontends"
 	depends on DVB_CORE
 
diff --git a/drivers/media/dvb-frontends/Makefile b/drivers/media/dvb-frontends/Makefile
index f9cb43d9aed9..0c75a6aafb9d 100644
--- a/drivers/media/dvb-frontends/Makefile
+++ b/drivers/media/dvb-frontends/Makefile
@@ -85,6 +85,7 @@ obj-$(CONFIG_DVB_STV6110) += stv6110.o
 obj-$(CONFIG_DVB_STV0900) += stv0900.o
 obj-$(CONFIG_DVB_STV090x) += stv090x.o
 obj-$(CONFIG_DVB_STV6110x) += stv6110x.o
+obj-$(CONFIG_DVB_M88DS3103) += m88ds3103.o
 obj-$(CONFIG_DVB_ISL6423) += isl6423.o
 obj-$(CONFIG_DVB_EC100) += ec100.o
 obj-$(CONFIG_DVB_HD29L2) += hd29l2.o
diff --git a/drivers/media/dvb-frontends/a8293.c b/drivers/media/dvb-frontends/a8293.c
index 74fbb5d58bed..780da58132f1 100644
--- a/drivers/media/dvb-frontends/a8293.c
+++ b/drivers/media/dvb-frontends/a8293.c
@@ -96,6 +96,8 @@ static int a8293_set_voltage(struct dvb_frontend *fe,
 	if (ret)
 		goto err;
 
+	usleep_range(1500, 50000);
+
 	return ret;
 err:
 	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
diff --git a/drivers/media/dvb-frontends/cx24117.c b/drivers/media/dvb-frontends/cx24117.c
index 476b422ccf19..a6c3c9e2e897 100644
--- a/drivers/media/dvb-frontends/cx24117.c
+++ b/drivers/media/dvb-frontends/cx24117.c
@@ -135,15 +135,33 @@
 
 
 enum cmds {
-	CMD_SET_VCO     = 0x10,
-	CMD_TUNEREQUEST = 0x11,
-	CMD_MPEGCONFIG  = 0x13,
-	CMD_TUNERINIT   = 0x14,
-	CMD_LNBSEND     = 0x21, /* Formerly CMD_SEND_DISEQC */
-	CMD_LNBDCLEVEL  = 0x22,
-	CMD_SET_TONE    = 0x23,
-	CMD_UPDFWVERS   = 0x35,
-	CMD_TUNERSLEEP  = 0x36,
+	CMD_SET_VCOFREQ    = 0x10,
+	CMD_TUNEREQUEST    = 0x11,
+	CMD_GLOBAL_MPEGCFG = 0x13,
+	CMD_MPEGCFG        = 0x14,
+	CMD_TUNERINIT      = 0x15,
+	CMD_GET_SRATE      = 0x18,
+	CMD_SET_GOLDCODE   = 0x19,
+	CMD_GET_AGCACC     = 0x1a,
+	CMD_DEMODINIT      = 0x1b,
+	CMD_GETCTLACC      = 0x1c,
+
+	CMD_LNBCONFIG      = 0x20,
+	CMD_LNBSEND        = 0x21,
+	CMD_LNBDCLEVEL     = 0x22,
+	CMD_LNBPCBCONFIG   = 0x23,
+	CMD_LNBSENDTONEBST = 0x24,
+	CMD_LNBUPDREPLY    = 0x25,
+
+	CMD_SET_GPIOMODE   = 0x30,
+	CMD_SET_GPIOEN     = 0x31,
+	CMD_SET_GPIODIR    = 0x32,
+	CMD_SET_GPIOOUT    = 0x33,
+	CMD_ENABLERSCORR   = 0x34,
+	CMD_FWVERSION      = 0x35,
+	CMD_SET_SLEEPMODE  = 0x36,
+	CMD_BERCTRL        = 0x3c,
+	CMD_EVENTCTRL      = 0x3d,
 };
 
 static LIST_HEAD(hybrid_tuner_instance_list);
@@ -619,8 +637,8 @@ static int cx24117_load_firmware(struct dvb_frontend *fe,
 	cx24117_writereg(state, 0xf7, 0x0c);
 	cx24117_writereg(state, 0xe0, 0x00);
 
-	/* CMD 1B */
-	cmd.args[0] = 0x1b;
+	/* Init demodulator */
+	cmd.args[0] = CMD_DEMODINIT;
 	cmd.args[1] = 0x00;
 	cmd.args[2] = 0x01;
 	cmd.args[3] = 0x00;
@@ -629,8 +647,8 @@ static int cx24117_load_firmware(struct dvb_frontend *fe,
 	if (ret != 0)
 		goto error;
 
-	/* CMD 10 */
-	cmd.args[0] = CMD_SET_VCO;
+	/* Set VCO frequency */
+	cmd.args[0] = CMD_SET_VCOFREQ;
 	cmd.args[1] = 0x06;
 	cmd.args[2] = 0x2b;
 	cmd.args[3] = 0xd8;
@@ -648,8 +666,8 @@ static int cx24117_load_firmware(struct dvb_frontend *fe,
 	if (ret != 0)
 		goto error;
 
-	/* CMD 15 */
-	cmd.args[0] = 0x15;
+	/* Tuner init */
+	cmd.args[0] = CMD_TUNERINIT;
 	cmd.args[1] = 0x00;
 	cmd.args[2] = 0x01;
 	cmd.args[3] = 0x00;
@@ -667,8 +685,8 @@ static int cx24117_load_firmware(struct dvb_frontend *fe,
 	if (ret != 0)
 		goto error;
 
-	/* CMD 13 */
-	cmd.args[0] = CMD_MPEGCONFIG;
+	/* Global MPEG config */
+	cmd.args[0] = CMD_GLOBAL_MPEGCFG;
 	cmd.args[1] = 0x00;
 	cmd.args[2] = 0x00;
 	cmd.args[3] = 0x00;
@@ -679,9 +697,9 @@ static int cx24117_load_firmware(struct dvb_frontend *fe,
 	if (ret != 0)
 		goto error;
 
-	/* CMD 14 */
+	/* MPEG config for each demod */
 	for (i = 0; i < 2; i++) {
-		cmd.args[0] = CMD_TUNERINIT;
+		cmd.args[0] = CMD_MPEGCFG;
 		cmd.args[1] = (u8) i;
 		cmd.args[2] = 0x00;
 		cmd.args[3] = 0x05;
@@ -699,8 +717,8 @@ static int cx24117_load_firmware(struct dvb_frontend *fe,
 	cx24117_writereg(state, 0xcf, 0x00);
 	cx24117_writereg(state, 0xe5, 0x04);
 
-	/* Firmware CMD 35: Get firmware version */
-	cmd.args[0] = CMD_UPDFWVERS;
+	/* Get firmware version */
+	cmd.args[0] = CMD_FWVERSION;
 	cmd.len = 2;
 	for (i = 0; i < 4; i++) {
 		cmd.args[1] = i;
@@ -779,8 +797,8 @@ static int cx24117_read_signal_strength(struct dvb_frontend *fe,
 	u8 reg = (state->demod == 0) ?
 		CX24117_REG_SSTATUS0 : CX24117_REG_SSTATUS1;
 
-	/* Firmware CMD 1A */
-	cmd.args[0] = 0x1a;
+	/* Read AGC accumulator register */
+	cmd.args[0] = CMD_GET_AGCACC;
 	cmd.args[1] = (u8) state->demod;
 	cmd.len = 2;
 	ret = cx24117_cmd_execute(fe, &cmd);
@@ -899,22 +917,15 @@ static int cx24117_set_voltage(struct dvb_frontend *fe,
 		voltage == SEC_VOLTAGE_18 ? "SEC_VOLTAGE_18" :
 		"SEC_VOLTAGE_OFF");
 
-	/* CMD 32 */
-	cmd.args[0] = 0x32;
-	cmd.args[1] = reg;
-	cmd.args[2] = reg;
+	/* Prepare a set GPIO logic level CMD */
+	cmd.args[0] = CMD_SET_GPIOOUT;
+	cmd.args[2] = reg; /* mask */
 	cmd.len = 3;
-	ret = cx24117_cmd_execute(fe, &cmd);
-	if (ret)
-		return ret;
 
 	if ((voltage == SEC_VOLTAGE_13) ||
 	    (voltage == SEC_VOLTAGE_18)) {
-		/* CMD 33 */
-		cmd.args[0] = 0x33;
+		/* power on LNB */
 		cmd.args[1] = reg;
-		cmd.args[2] = reg;
-		cmd.len = 3;
 		ret = cx24117_cmd_execute(fe, &cmd);
 		if (ret != 0)
 			return ret;
@@ -926,22 +937,22 @@ static int cx24117_set_voltage(struct dvb_frontend *fe,
 		/* Wait for voltage/min repeat delay */
 		msleep(100);
 
-		/* CMD 22 - CMD_LNBDCLEVEL */
+		/* Set 13V/18V select pin */
 		cmd.args[0] = CMD_LNBDCLEVEL;
 		cmd.args[1] = state->demod ? 0 : 1;
 		cmd.args[2] = (voltage == SEC_VOLTAGE_18 ? 0x01 : 0x00);
 		cmd.len = 3;
+		ret = cx24117_cmd_execute(fe, &cmd);
 
 		/* Min delay time before DiSEqC send */
 		msleep(20);
 	} else {
-		cmd.args[0] = 0x33;
+		/* power off LNB */
 		cmd.args[1] = 0x00;
-		cmd.args[2] = reg;
-		cmd.len = 3;
+		ret = cx24117_cmd_execute(fe, &cmd);
 	}
 
-	return cx24117_cmd_execute(fe, &cmd);
+	return ret;
 }
 
 static int cx24117_set_tone(struct dvb_frontend *fe,
@@ -968,8 +979,7 @@ static int cx24117_set_tone(struct dvb_frontend *fe,
 	msleep(20);
 
 	/* Set the tone */
-	/* CMD 23 - CMD_SET_TONE */
-	cmd.args[0] = CMD_SET_TONE;
+	cmd.args[0] = CMD_LNBPCBCONFIG;
 	cmd.args[1] = (state->demod ? 0 : 1);
 	cmd.args[2] = 0x00;
 	cmd.args[3] = 0x00;
@@ -1166,7 +1176,7 @@ struct dvb_frontend *cx24117_attach(const struct cx24117_config *config,
 
 	switch (demod) {
 	case 0:
-		dev_err(&state->priv->i2c->dev,
+		dev_err(&i2c->dev,
 			"%s: Error attaching frontend %d\n",
 			KBUILD_MODNAME, demod);
 		goto error1;
@@ -1190,12 +1200,6 @@ struct dvb_frontend *cx24117_attach(const struct cx24117_config *config,
 	state->demod = demod - 1;
 	state->priv = priv;
 
-	/* test i2c bus for ack */
-	if (demod == 0) {
-		if (cx24117_readreg(state, 0x00) < 0)
-			goto error3;
-	}
-
 	dev_info(&state->priv->i2c->dev,
 		"%s: Attaching frontend %d\n",
 		KBUILD_MODNAME, state->demod);
@@ -1206,8 +1210,6 @@ struct dvb_frontend *cx24117_attach(const struct cx24117_config *config,
 	state->frontend.demodulator_priv = state;
 	return &state->frontend;
 
-error3:
-	kfree(state);
 error2:
 	cx24117_release_priv(priv);
 error1:
@@ -1231,8 +1233,8 @@ static int cx24117_initfe(struct dvb_frontend *fe)
 
 	mutex_lock(&state->priv->fe_lock);
 
-	/* Firmware CMD 36: Power config */
-	cmd.args[0] = CMD_TUNERSLEEP;
+	/* Set sleep mode off */
+	cmd.args[0] = CMD_SET_SLEEPMODE;
 	cmd.args[1] = (state->demod ? 1 : 0);
 	cmd.args[2] = 0;
 	cmd.len = 3;
@@ -1244,8 +1246,8 @@ static int cx24117_initfe(struct dvb_frontend *fe)
 	if (ret != 0)
 		goto exit;
 
-	/* CMD 3C */
-	cmd.args[0] = 0x3c;
+	/* Set BER control */
+	cmd.args[0] = CMD_BERCTRL;
 	cmd.args[1] = (state->demod ? 1 : 0);
 	cmd.args[2] = 0x10;
 	cmd.args[3] = 0x10;
@@ -1254,12 +1256,22 @@ static int cx24117_initfe(struct dvb_frontend *fe)
 	if (ret != 0)
 		goto exit;
 
-	/* CMD 34 */
-	cmd.args[0] = 0x34;
+	/* Set RS correction (enable/disable) */
+	cmd.args[0] = CMD_ENABLERSCORR;
 	cmd.args[1] = (state->demod ? 1 : 0);
 	cmd.args[2] = CX24117_OCC;
 	cmd.len = 3;
 	ret = cx24117_cmd_execute_nolock(fe, &cmd);
+	if (ret != 0)
+		goto exit;
+
+	/* Set GPIO direction */
+	/* Set as output - controls LNB power on/off */
+	cmd.args[0] = CMD_SET_GPIODIR;
+	cmd.args[1] = 0x30;
+	cmd.args[2] = 0x30;
+	cmd.len = 3;
+	ret = cx24117_cmd_execute_nolock(fe, &cmd);
 
 exit:
 	mutex_unlock(&state->priv->fe_lock);
@@ -1278,8 +1290,8 @@ static int cx24117_sleep(struct dvb_frontend *fe)
 	dev_dbg(&state->priv->i2c->dev, "%s() demod%d\n",
 		__func__, state->demod);
 
-	/* Firmware CMD 36: Power config */
-	cmd.args[0] = CMD_TUNERSLEEP;
+	/* Set sleep mode on */
+	cmd.args[0] = CMD_SET_SLEEPMODE;
 	cmd.args[1] = (state->demod ? 1 : 0);
 	cmd.args[2] = 1;
 	cmd.len = 3;
@@ -1558,7 +1570,8 @@ static int cx24117_get_frontend(struct dvb_frontend *fe)
 
 	u8 buf[0x1f-4];
 
-	cmd.args[0] = 0x1c;
+	/* Read current tune parameters */
+	cmd.args[0] = CMD_GETCTLACC;
 	cmd.args[1] = (u8) state->demod;
 	cmd.len = 2;
 	ret = cx24117_cmd_execute(fe, &cmd);
diff --git a/drivers/media/dvb-frontends/dib8000.c b/drivers/media/dvb-frontends/dib8000.c
index 6dbbee453ee1..1632d78a5479 100644
--- a/drivers/media/dvb-frontends/dib8000.c
+++ b/drivers/media/dvb-frontends/dib8000.c
@@ -11,6 +11,7 @@
 #include <linux/slab.h>
 #include <linux/i2c.h>
 #include <linux/mutex.h>
+#include <asm/div64.h>
 
 #include "dvb_math.h"
 
@@ -118,6 +119,12 @@ struct dib8000_state {
 	u8 longest_intlv_layer;
 	u16 output_mode;
 
+	/* for DVBv5 stats */
+	s64 init_ucb;
+	unsigned long per_jiffies_stats;
+	unsigned long ber_jiffies_stats;
+	unsigned long ber_jiffies_stats_layer[3];
+
 #ifdef DIB8000_AGC_FREEZE
 	u16 agc1_max;
 	u16 agc1_min;
@@ -157,15 +164,10 @@ static u16 dib8000_i2c_read16(struct i2c_device *i2c, u16 reg)
 	return ret;
 }
 
-static u16 dib8000_read_word(struct dib8000_state *state, u16 reg)
+static u16 __dib8000_read_word(struct dib8000_state *state, u16 reg)
 {
 	u16 ret;
 
-	if (mutex_lock_interruptible(&state->i2c_buffer_lock) < 0) {
-		dprintk("could not acquire lock");
-		return 0;
-	}
-
 	state->i2c_write_buffer[0] = reg >> 8;
 	state->i2c_write_buffer[1] = reg & 0xff;
 
@@ -183,6 +185,21 @@ static u16 dib8000_read_word(struct dib8000_state *state, u16 reg)
 		dprintk("i2c read error on %d", reg);
 
 	ret = (state->i2c_read_buffer[0] << 8) | state->i2c_read_buffer[1];
+
+	return ret;
+}
+
+static u16 dib8000_read_word(struct dib8000_state *state, u16 reg)
+{
+	u16 ret;
+
+	if (mutex_lock_interruptible(&state->i2c_buffer_lock) < 0) {
+		dprintk("could not acquire lock");
+		return 0;
+	}
+
+	ret = __dib8000_read_word(state, reg);
+
 	mutex_unlock(&state->i2c_buffer_lock);
 
 	return ret;
@@ -192,8 +209,15 @@ static u32 dib8000_read32(struct dib8000_state *state, u16 reg)
 {
 	u16 rw[2];
 
-	rw[0] = dib8000_read_word(state, reg + 0);
-	rw[1] = dib8000_read_word(state, reg + 1);
+	if (mutex_lock_interruptible(&state->i2c_buffer_lock) < 0) {
+		dprintk("could not acquire lock");
+		return 0;
+	}
+
+	rw[0] = __dib8000_read_word(state, reg + 0);
+	rw[1] = __dib8000_read_word(state, reg + 1);
+
+	mutex_unlock(&state->i2c_buffer_lock);
 
 	return ((rw[0] << 16) | (rw[1]));
 }
@@ -787,7 +811,7 @@ int dib8000_update_pll(struct dvb_frontend *fe,
 			dprintk("PLL: Update ratio (prediv: %d, ratio: %d)", state->cfg.pll->pll_prediv, ratio);
 			dib8000_write_word(state, 901, (state->cfg.pll->pll_prediv << 8) | (ratio << 0)); /* only the PLL ratio is updated. */
 		}
-}
+	}
 
 	return 0;
 }
@@ -966,6 +990,45 @@ static u16 dib8000_identify(struct i2c_device *client)
 	return value;
 }
 
+static int dib8000_read_unc_blocks(struct dvb_frontend *fe, u32 *unc);
+
+static void dib8000_reset_stats(struct dvb_frontend *fe)
+{
+	struct dib8000_state *state = fe->demodulator_priv;
+	struct dtv_frontend_properties *c = &state->fe[0]->dtv_property_cache;
+	u32 ucb;
+
+	memset(&c->strength, 0, sizeof(c->strength));
+	memset(&c->cnr, 0, sizeof(c->cnr));
+	memset(&c->post_bit_error, 0, sizeof(c->post_bit_error));
+	memset(&c->post_bit_count, 0, sizeof(c->post_bit_count));
+	memset(&c->block_error, 0, sizeof(c->block_error));
+
+	c->strength.len = 1;
+	c->cnr.len = 1;
+	c->block_error.len = 1;
+	c->block_count.len = 1;
+	c->post_bit_error.len = 1;
+	c->post_bit_count.len = 1;
+
+	c->strength.stat[0].scale = FE_SCALE_DECIBEL;
+	c->strength.stat[0].uvalue = 0;
+
+	c->cnr.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	c->block_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	c->block_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	c->post_bit_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	c->post_bit_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+
+	dib8000_read_unc_blocks(fe, &ucb);
+
+	state->init_ucb = -ucb;
+	state->ber_jiffies_stats = 0;
+	state->per_jiffies_stats = 0;
+	memset(&state->ber_jiffies_stats_layer, 0,
+	       sizeof(state->ber_jiffies_stats_layer));
+}
+
 static int dib8000_reset(struct dvb_frontend *fe)
 {
 	struct dib8000_state *state = fe->demodulator_priv;
@@ -1071,6 +1134,8 @@ static int dib8000_reset(struct dvb_frontend *fe)
 
 	dib8000_set_power_mode(state, DIB8000_POWER_INTERFACE_ONLY);
 
+	dib8000_reset_stats(fe);
+
 	return 0;
 }
 
@@ -2445,7 +2510,8 @@ static int dib8000_autosearch_start(struct dvb_frontend *fe)
 	if (state->revision == 0x8090)
 		internal = dib8000_read32(state, 23) / 1000;
 
-	if (state->autosearch_state == AS_SEARCHING_FFT) {
+	if ((state->revision >= 0x8002) &&
+	    (state->autosearch_state == AS_SEARCHING_FFT)) {
 		dib8000_write_word(state,  37, 0x0065); /* P_ctrl_pha_off_max default values */
 		dib8000_write_word(state, 116, 0x0000); /* P_ana_gain to 0 */
 
@@ -2481,7 +2547,8 @@ static int dib8000_autosearch_start(struct dvb_frontend *fe)
 		dib8000_write_word(state, 770, (dib8000_read_word(state, 770) & 0xdfff) | (1 << 13)); /* P_restart_ccg = 1 */
 		dib8000_write_word(state, 770, (dib8000_read_word(state, 770) & 0xdfff) | (0 << 13)); /* P_restart_ccg = 0 */
 		dib8000_write_word(state, 0, (dib8000_read_word(state, 0) & 0x7ff) | (0 << 15) | (1 << 13)); /* P_restart_search = 0; */
-	} else if (state->autosearch_state == AS_SEARCHING_GUARD) {
+	} else if ((state->revision >= 0x8002) &&
+		   (state->autosearch_state == AS_SEARCHING_GUARD)) {
 		c->transmission_mode = TRANSMISSION_MODE_8K;
 		c->guard_interval = GUARD_INTERVAL_1_8;
 		c->inversion = 0;
@@ -2583,7 +2650,8 @@ static int dib8000_autosearch_irq(struct dvb_frontend *fe)
 	struct dib8000_state *state = fe->demodulator_priv;
 	u16 irq_pending = dib8000_read_word(state, 1284);
 
-	if (state->autosearch_state == AS_SEARCHING_FFT) {
+	if ((state->revision >= 0x8002) &&
+	    (state->autosearch_state == AS_SEARCHING_FFT)) {
 		if (irq_pending & 0x1) {
 			dprintk("dib8000_autosearch_irq: max correlation result available");
 			return 3;
@@ -2853,6 +2921,91 @@ static int dib8090p_init_sdram(struct dib8000_state *state)
 	return 0;
 }
 
+/**
+ * is_manual_mode - Check if TMCC should be used for parameters settings
+ * @c:	struct dvb_frontend_properties
+ *
+ * By default, TMCC table should be used for parameter settings on most
+ * usercases. However, sometimes it is desirable to lock the demod to
+ * use the manual parameters.
+ *
+ * On manual mode, the current dib8000_tune state machine is very restrict:
+ * It requires that both per-layer and per-transponder parameters to be
+ * properly specified, otherwise the device won't lock.
+ *
+ * Check if all those conditions are properly satisfied before allowing
+ * the device to use the manual frequency lock mode.
+ */
+static int is_manual_mode(struct dtv_frontend_properties *c)
+{
+	int i, n_segs = 0;
+
+	/* Use auto mode on DVB-T compat mode */
+	if (c->delivery_system != SYS_ISDBT)
+		return 0;
+
+	/*
+	 * Transmission mode is only detected on auto mode, currently
+	 */
+	if (c->transmission_mode == TRANSMISSION_MODE_AUTO) {
+		dprintk("transmission mode auto");
+		return 0;
+	}
+
+	/*
+	 * Guard interval is only detected on auto mode, currently
+	 */
+	if (c->guard_interval == GUARD_INTERVAL_AUTO) {
+		dprintk("guard interval auto");
+		return 0;
+	}
+
+	/*
+	 * If no layer is enabled, assume auto mode, as at least one
+	 * layer should be enabled
+	 */
+	if (!c->isdbt_layer_enabled) {
+		dprintk("no layer modulation specified");
+		return 0;
+	}
+
+	/*
+	 * Check if the per-layer parameters aren't auto and
+	 * disable a layer if segment count is 0 or invalid.
+	 */
+	for (i = 0; i < 3; i++) {
+		if (!(c->isdbt_layer_enabled & 1 << i))
+			continue;
+
+		if ((c->layer[i].segment_count > 13) ||
+		    (c->layer[i].segment_count == 0)) {
+			c->isdbt_layer_enabled &= ~(1 << i);
+			continue;
+		}
+
+		n_segs += c->layer[i].segment_count;
+
+		if ((c->layer[i].modulation == QAM_AUTO) ||
+		    (c->layer[i].fec == FEC_AUTO)) {
+			dprintk("layer %c has either modulation or FEC auto",
+				'A' + i);
+			return 0;
+		}
+	}
+
+	/*
+	 * Userspace specified a wrong number of segments.
+	 *	fallback to auto mode.
+	 */
+	if (n_segs == 0 || n_segs > 13) {
+		dprintk("number of segments is invalid");
+		return 0;
+	}
+
+	/* Everything looks ok for manual mode */
+	return 1;
+}
+
 static int dib8000_tune(struct dvb_frontend *fe)
 {
 	struct dib8000_state *state = fe->demodulator_priv;
@@ -2878,40 +3031,19 @@ static int dib8000_tune(struct dvb_frontend *fe)
 
 	switch (*tune_state) {
 	case CT_DEMOD_START: /* 30 */
+			dib8000_reset_stats(fe);
+
 			if (state->revision == 0x8090)
 				dib8090p_init_sdram(state);
 			state->status = FE_STATUS_TUNE_PENDING;
-			if ((c->delivery_system != SYS_ISDBT) ||
-					(c->inversion == INVERSION_AUTO) ||
-					(c->transmission_mode == TRANSMISSION_MODE_AUTO) ||
-					(c->guard_interval == GUARD_INTERVAL_AUTO) ||
-					(((c->isdbt_layer_enabled & (1 << 0)) != 0) &&
-					 (c->layer[0].segment_count != 0xff) &&
-					 (c->layer[0].segment_count != 0) &&
-					 ((c->layer[0].modulation == QAM_AUTO) ||
-					  (c->layer[0].fec == FEC_AUTO))) ||
-					(((c->isdbt_layer_enabled & (1 << 1)) != 0) &&
-					 (c->layer[1].segment_count != 0xff) &&
-					 (c->layer[1].segment_count != 0) &&
-					 ((c->layer[1].modulation == QAM_AUTO) ||
-					  (c->layer[1].fec == FEC_AUTO))) ||
-					(((c->isdbt_layer_enabled & (1 << 2)) != 0) &&
-					 (c->layer[2].segment_count != 0xff) &&
-					 (c->layer[2].segment_count != 0) &&
-					 ((c->layer[2].modulation == QAM_AUTO) ||
-					  (c->layer[2].fec == FEC_AUTO))) ||
-					(((c->layer[0].segment_count == 0) ||
-					  ((c->isdbt_layer_enabled & (1 << 0)) == 0)) &&
-					 ((c->layer[1].segment_count == 0) ||
-					  ((c->isdbt_layer_enabled & (2 << 0)) == 0)) &&
-					 ((c->layer[2].segment_count == 0) || ((c->isdbt_layer_enabled & (3 << 0)) == 0))))
-				state->channel_parameters_set = 0; /* auto search */
-			else
-				state->channel_parameters_set = 1; /* channel parameters are known */
+			state->channel_parameters_set = is_manual_mode(c);
+
+			dprintk("Tuning channel on %s search mode",
+				state->channel_parameters_set ? "manual" : "auto");
 
 			dib8000_viterbi_state(state, 0); /* force chan dec in restart */
 
-			/* Layer monit */
+			/* Layer monitor */
 			dib8000_write_word(state, 285, dib8000_read_word(state, 285) & 0x60);
 
 			dib8000_set_frequency_offset(state);
@@ -3256,15 +3388,27 @@ static int dib8000_sleep(struct dvb_frontend *fe)
 	return dib8000_set_adc_state(state, DIBX000_SLOW_ADC_OFF) | dib8000_set_adc_state(state, DIBX000_ADC_OFF);
 }
 
+static int dib8000_read_status(struct dvb_frontend *fe, fe_status_t * stat);
+
 static int dib8000_get_frontend(struct dvb_frontend *fe)
 {
 	struct dib8000_state *state = fe->demodulator_priv;
 	u16 i, val = 0;
-	fe_status_t stat;
+	fe_status_t stat = 0;
 	u8 index_frontend, sub_index_frontend;
 
 	fe->dtv_property_cache.bandwidth_hz = 6000000;
 
+	/*
+	 * If called to early, get_frontend makes dib8000_tune to either
+	 * not lock or not sync. This causes dvbv5-scan/dvbv5-zap to fail.
+	 * So, let's just return if frontend 0 has not locked.
+	 */
+	dib8000_read_status(fe, &stat);
+	if (!(stat & FE_HAS_SYNC))
+		return 0;
+
+	dprintk("TMCC lock");
 	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
 		state->fe[index_frontend]->ops.read_status(state->fe[index_frontend], &stat);
 		if (stat&FE_HAS_SYNC) {
@@ -3335,9 +3479,13 @@ static int dib8000_get_frontend(struct dvb_frontend *fe)
 		fe->dtv_property_cache.layer[i].segment_count = val & 0x0F;
 		dprintk("dib8000_get_frontend : Layer %d segments = %d ", i, fe->dtv_property_cache.layer[i].segment_count);
 
-		val = dib8000_read_word(state, 499 + i);
-		fe->dtv_property_cache.layer[i].interleaving = val & 0x3;
-		dprintk("dib8000_get_frontend : Layer %d time_intlv = %d ", i, fe->dtv_property_cache.layer[i].interleaving);
+		val = dib8000_read_word(state, 499 + i) & 0x3;
+		/* Interleaving can be 0, 1, 2 or 4 */
+		if (val == 3)
+			val = 4;
+		fe->dtv_property_cache.layer[i].interleaving = val;
+		dprintk("dib8000_get_frontend : Layer %d time_intlv = %d ",
+			i, fe->dtv_property_cache.layer[i].interleaving);
 
 		val = dib8000_read_word(state, 481 + i);
 		switch (val & 0x7) {
@@ -3556,6 +3704,8 @@ static int dib8000_set_frontend(struct dvb_frontend *fe)
 	return 0;
 }
 
+static int dib8000_get_stats(struct dvb_frontend *fe, fe_status_t stat);
+
 static int dib8000_read_status(struct dvb_frontend *fe, fe_status_t * stat)
 {
 	struct dib8000_state *state = fe->demodulator_priv;
@@ -3593,6 +3743,7 @@ static int dib8000_read_status(struct dvb_frontend *fe, fe_status_t * stat)
 		if (lock & 0x01)
 			*stat |= FE_HAS_VITERBI;
 	}
+	dib8000_get_stats(fe, *stat);
 
 	return 0;
 }
@@ -3699,6 +3850,357 @@ static int dib8000_read_snr(struct dvb_frontend *fe, u16 * snr)
 	return 0;
 }
 
+struct per_layer_regs {
+	u16 lock, ber, per;
+};
+
+static const struct per_layer_regs per_layer_regs[] = {
+	{ 554, 560, 562 },
+	{ 555, 576, 578 },
+	{ 556, 581, 583 },
+};
+
+struct linear_segments {
+	unsigned x;
+	signed y;
+};
+
+/*
+ * Table to estimate signal strength in dBm.
+ * This table was empirically determinated by measuring the signal
+ * strength generated by a DTA-2111 RF generator directly connected into
+ * a dib8076 device (a PixelView PV-D231U stick), using a good quality
+ * 3 meters RC6 cable and good RC6 connectors.
+ * The real value can actually be different on other devices, depending
+ * on several factors, like if LNA is enabled or not, if diversity is
+ * enabled, type of connectors, etc.
+ * Yet, it is better to use this measure in dB than a random non-linear
+ * percentage value, especially for antenna adjustments.
+ * On my tests, the precision of the measure using this table is about
+ * 0.5 dB, with sounds reasonable enough.
+ */
+static struct linear_segments strength_to_db_table[] = {
+	{ 55953, 108500 },	/* -22.5 dBm */
+	{ 55394, 108000 },
+	{ 53834, 107000 },
+	{ 52863, 106000 },
+	{ 52239, 105000 },
+	{ 52012, 104000 },
+	{ 51803, 103000 },
+	{ 51566, 102000 },
+	{ 51356, 101000 },
+	{ 51112, 100000 },
+	{ 50869,  99000 },
+	{ 50600,  98000 },
+	{ 50363,  97000 },
+	{ 50117,  96000 },	/* -35 dBm */
+	{ 49889,  95000 },
+	{ 49680,  94000 },
+	{ 49493,  93000 },
+	{ 49302,  92000 },
+	{ 48929,  91000 },
+	{ 48416,  90000 },
+	{ 48035,  89000 },
+	{ 47593,  88000 },
+	{ 47282,  87000 },
+	{ 46953,  86000 },
+	{ 46698,  85000 },
+	{ 45617,  84000 },
+	{ 44773,  83000 },
+	{ 43845,  82000 },
+	{ 43020,  81000 },
+	{ 42010,  80000 },	/* -51 dBm */
+	{     0,      0 },
+};
+
+static u32 interpolate_value(u32 value, struct linear_segments *segments,
+			     unsigned len)
+{
+	u64 tmp64;
+	u32 dx;
+	s32 dy;
+	int i, ret;
+
+	if (value >= segments[0].x)
+		return segments[0].y;
+	if (value < segments[len-1].x)
+		return segments[len-1].y;
+
+	for (i = 1; i < len - 1; i++) {
+		/* If value is identical, no need to interpolate */
+		if (value == segments[i].x)
+			return segments[i].y;
+		if (value > segments[i].x)
+			break;
+	}
+
+	/* Linear interpolation between the two (x,y) points */
+	dy = segments[i - 1].y - segments[i].y;
+	dx = segments[i - 1].x - segments[i].x;
+
+	tmp64 = value - segments[i].x;
+	tmp64 *= dy;
+	do_div(tmp64, dx);
+	ret = segments[i].y + tmp64;
+
+	return ret;
+}
+
+static u32 dib8000_get_time_us(struct dvb_frontend *fe, int layer)
+{
+	struct dib8000_state *state = fe->demodulator_priv;
+	struct dtv_frontend_properties *c = &state->fe[0]->dtv_property_cache;
+	int ini_layer, end_layer, i;
+	u64 time_us, tmp64;
+	u32 tmp, denom;
+	int guard, rate_num, rate_denum = 1, bits_per_symbol, nsegs;
+	int interleaving = 0, fft_div;
+
+	if (layer >= 0) {
+		ini_layer = layer;
+		end_layer = layer + 1;
+	} else {
+		ini_layer = 0;
+		end_layer = 3;
+	}
+
+	switch (c->guard_interval) {
+	case GUARD_INTERVAL_1_4:
+		guard = 4;
+		break;
+	case GUARD_INTERVAL_1_8:
+		guard = 8;
+		break;
+	case GUARD_INTERVAL_1_16:
+		guard = 16;
+		break;
+	default:
+	case GUARD_INTERVAL_1_32:
+		guard = 32;
+		break;
+	}
+
+	switch (c->transmission_mode) {
+	case TRANSMISSION_MODE_2K:
+		fft_div = 4;
+		break;
+	case TRANSMISSION_MODE_4K:
+		fft_div = 2;
+		break;
+	default:
+	case TRANSMISSION_MODE_8K:
+		fft_div = 1;
+		break;
+	}
+
+	denom = 0;
+	for (i = ini_layer; i < end_layer; i++) {
+		nsegs = c->layer[i].segment_count;
+		if (nsegs == 0 || nsegs > 13)
+			continue;
+
+		switch (c->layer[i].modulation) {
+		case DQPSK:
+		case QPSK:
+			bits_per_symbol = 2;
+			break;
+		case QAM_16:
+			bits_per_symbol = 4;
+			break;
+		default:
+		case QAM_64:
+			bits_per_symbol = 6;
+			break;
+		}
+
+		switch (c->layer[i].fec) {
+		case FEC_1_2:
+			rate_num = 1;
+			rate_denum = 2;
+			break;
+		case FEC_2_3:
+			rate_num = 2;
+			rate_denum = 3;
+			break;
+		case FEC_3_4:
+			rate_num = 3;
+			rate_denum = 4;
+			break;
+		case FEC_5_6:
+			rate_num = 5;
+			rate_denum = 6;
+			break;
+		default:
+		case FEC_7_8:
+			rate_num = 7;
+			rate_denum = 8;
+			break;
+		}
+
+		interleaving = c->layer[i].interleaving;
+
+		denom += bits_per_symbol * rate_num * fft_div * nsegs * 384;
+	}
+
+	/* If all goes wrong, wait for 1s for the next stats */
+	if (!denom)
+		return 0;
+
+	/* Estimate the period for the total bit rate */
+	time_us = rate_denum * (1008 * 1562500L);
+	tmp64 = time_us;
+	do_div(tmp64, guard);
+	time_us = time_us + tmp64;
+	time_us += denom / 2;
+	do_div(time_us, denom);
+
+	tmp = 1008 * 96 * interleaving;
+	time_us += tmp + tmp / guard;
+
+	return time_us;
+}
+
+static int dib8000_get_stats(struct dvb_frontend *fe, fe_status_t stat)
+{
+	struct dib8000_state *state = fe->demodulator_priv;
+	struct dtv_frontend_properties *c = &state->fe[0]->dtv_property_cache;
+	int i;
+	int show_per_stats = 0;
+	u32 time_us = 0, snr, val;
+	u64 blocks;
+	s32 db;
+	u16 strength;
+
+	/* Get Signal strength */
+	dib8000_read_signal_strength(fe, &strength);
+	val = strength;
+	db = interpolate_value(val,
+			       strength_to_db_table,
+			       ARRAY_SIZE(strength_to_db_table)) - 131000;
+	c->strength.stat[0].svalue = db;
+
+	/* UCB/BER/CNR measures require lock */
+	if (!(stat & FE_HAS_LOCK)) {
+		c->cnr.len = 1;
+		c->block_count.len = 1;
+		c->block_error.len = 1;
+		c->post_bit_error.len = 1;
+		c->post_bit_count.len = 1;
+		c->cnr.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+		c->post_bit_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+		c->post_bit_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+		c->block_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+		c->block_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+		return 0;
+	}
+
+	/* Check if time for stats was elapsed */
+	if (time_after(jiffies, state->per_jiffies_stats)) {
+		state->per_jiffies_stats = jiffies + msecs_to_jiffies(1000);
+
+		/* Get SNR */
+		snr = dib8000_get_snr(fe);
+		for (i = 1; i < MAX_NUMBER_OF_FRONTENDS; i++) {
+			if (state->fe[i])
+				snr += dib8000_get_snr(state->fe[i]);
+		}
+		snr = snr >> 16;
+
+		if (snr) {
+			snr = 10 * intlog10(snr);
+			snr = (1000L * snr) >> 24;
+		} else {
+			snr = 0;
+		}
+		c->cnr.stat[0].svalue = snr;
+		c->cnr.stat[0].scale = FE_SCALE_DECIBEL;
+
+		/* Get UCB measures */
+		dib8000_read_unc_blocks(fe, &val);
+		if (val < state->init_ucb)
+			state->init_ucb += 0x100000000LL;
+
+		c->block_error.stat[0].scale = FE_SCALE_COUNTER;
+		c->block_error.stat[0].uvalue = val + state->init_ucb;
+
+		/* Estimate the number of packets based on bitrate */
+		if (!time_us)
+			time_us = dib8000_get_time_us(fe, -1);
+
+		if (time_us) {
+			blocks = 1250000ULL * 1000000ULL;
+			do_div(blocks, time_us * 8 * 204);
+			c->block_count.stat[0].scale = FE_SCALE_COUNTER;
+			c->block_count.stat[0].uvalue += blocks;
+		}
+
+		show_per_stats = 1;
+	}
+
+	/* Get post-BER measures */
+	if (time_after(jiffies, state->ber_jiffies_stats)) {
+		time_us = dib8000_get_time_us(fe, -1);
+		state->ber_jiffies_stats = jiffies + msecs_to_jiffies((time_us + 500) / 1000);
+
+		dprintk("Next all layers stats available in %u us.", time_us);
+
+		dib8000_read_ber(fe, &val);
+		c->post_bit_error.stat[0].scale = FE_SCALE_COUNTER;
+		c->post_bit_error.stat[0].uvalue += val;
+
+		c->post_bit_count.stat[0].scale = FE_SCALE_COUNTER;
+		c->post_bit_count.stat[0].uvalue += 100000000;
+	}
+
+	if (state->revision < 0x8002)
+		return 0;
+
+	c->block_error.len = 4;
+	c->post_bit_error.len = 4;
+	c->post_bit_count.len = 4;
+
+	for (i = 0; i < 3; i++) {
+		unsigned nsegs = c->layer[i].segment_count;
+
+		if (nsegs == 0 || nsegs > 13)
+			continue;
+
+		time_us = 0;
+
+		if (time_after(jiffies, state->ber_jiffies_stats_layer[i])) {
+			time_us = dib8000_get_time_us(fe, i);
+
+			state->ber_jiffies_stats_layer[i] = jiffies + msecs_to_jiffies((time_us + 500) / 1000);
+			dprintk("Next layer %c  stats will be available in %u us\n",
+				'A' + i, time_us);
+
+			val = dib8000_read_word(state, per_layer_regs[i].ber);
+			c->post_bit_error.stat[1 + i].scale = FE_SCALE_COUNTER;
+			c->post_bit_error.stat[1 + i].uvalue += val;
+
+			c->post_bit_count.stat[1 + i].scale = FE_SCALE_COUNTER;
+			c->post_bit_count.stat[1 + i].uvalue += 100000000;
+		}
+
+		if (show_per_stats) {
+			val = dib8000_read_word(state, per_layer_regs[i].per);
+
+			c->block_error.stat[1 + i].scale = FE_SCALE_COUNTER;
+			c->block_error.stat[1 + i].uvalue += val;
+
+			if (!time_us)
+				time_us = dib8000_get_time_us(fe, i);
+			if (time_us) {
+				blocks = 1250000ULL * 1000000ULL;
+				do_div(blocks, time_us * 8 * 204);
+				c->block_count.stat[0].scale = FE_SCALE_COUNTER;
+				c->block_count.stat[0].uvalue += blocks;
+			}
+		}
+	}
+	return 0;
+}
+
 int dib8000_set_slave_frontend(struct dvb_frontend *fe, struct dvb_frontend *fe_slave)
 {
 	struct dib8000_state *state = fe->demodulator_priv;
diff --git a/drivers/media/dvb-frontends/drxk.h b/drivers/media/dvb-frontends/drxk.h
index f22eb9f13ad5..f6cb34660327 100644
--- a/drivers/media/dvb-frontends/drxk.h
+++ b/drivers/media/dvb-frontends/drxk.h
@@ -29,7 +29,6 @@
  *				A value of 0 (default) or lower indicates that
  *				the correct number of parameters will be
  *				automatically detected.
- * @load_firmware_sync:		Force the firmware load to be synchronous.
  *
  * On the *_gpio vars, bit 0 is UIO-1, bit 1 is UIO-2 and bit 2 is
  * UIO-3.
@@ -41,7 +40,6 @@ struct drxk_config {
 	bool	parallel_ts;
 	bool	dynamic_clk;
 	bool	enable_merr_cfg;
-	bool	load_firmware_sync;
 
 	bool	antenna_dvbt;
 	u16	antenna_gpio;
diff --git a/drivers/media/dvb-frontends/drxk_hard.c b/drivers/media/dvb-frontends/drxk_hard.c
index bf29a3f0e6f0..cce94a75b2e1 100644
--- a/drivers/media/dvb-frontends/drxk_hard.c
+++ b/drivers/media/dvb-frontends/drxk_hard.c
@@ -6830,25 +6830,13 @@ struct dvb_frontend *drxk_attach(const struct drxk_config *config,
 
 	/* Load firmware and initialize DRX-K */
 	if (state->microcode_name) {
-		if (config->load_firmware_sync) {
-			const struct firmware *fw = NULL;
+		const struct firmware *fw = NULL;
 
-			status = request_firmware(&fw, state->microcode_name,
-						  state->i2c->dev.parent);
-			if (status < 0)
-				fw = NULL;
-			load_firmware_cb(fw, state);
-		} else {
-			status = request_firmware_nowait(THIS_MODULE, 1,
-					      state->microcode_name,
-					      state->i2c->dev.parent,
-					      GFP_KERNEL,
-					      state, load_firmware_cb);
-			if (status < 0) {
-				pr_err("failed to request a firmware\n");
-				return NULL;
-			}
-		}
+		status = request_firmware(&fw, state->microcode_name,
+					  state->i2c->dev.parent);
+		if (status < 0)
+			fw = NULL;
+		load_firmware_cb(fw, state);
 	} else if (init_drxk(state) < 0)
 		goto error;
 
diff --git a/drivers/media/dvb-frontends/m88ds3103.c b/drivers/media/dvb-frontends/m88ds3103.c
new file mode 100644
index 000000000000..b8a7897e7bd8
--- /dev/null
+++ b/drivers/media/dvb-frontends/m88ds3103.c
@@ -0,0 +1,1311 @@
+/*
+ * Montage M88DS3103 demodulator driver
+ *
+ * Copyright (C) 2013 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 "m88ds3103_priv.h"
+
+static struct dvb_frontend_ops m88ds3103_ops;
+
+/* write multiple registers */
+static int m88ds3103_wr_regs(struct m88ds3103_priv *priv,
+		u8 reg, const u8 *val, int len)
+{
+#define MAX_WR_LEN 32
+#define MAX_WR_XFER_LEN (MAX_WR_LEN + 1)
+	int ret;
+	u8 buf[MAX_WR_XFER_LEN];
+	struct i2c_msg msg[1] = {
+		{
+			.addr = priv->cfg->i2c_addr,
+			.flags = 0,
+			.len = 1 + len,
+			.buf = buf,
+		}
+	};
+
+	if (WARN_ON(len > MAX_WR_LEN))
+		return -EINVAL;
+
+	buf[0] = reg;
+	memcpy(&buf[1], val, len);
+
+	mutex_lock(&priv->i2c_mutex);
+	ret = i2c_transfer(priv->i2c, msg, 1);
+	mutex_unlock(&priv->i2c_mutex);
+	if (ret == 1) {
+		ret = 0;
+	} else {
+		dev_warn(&priv->i2c->dev,
+				"%s: i2c wr failed=%d reg=%02x len=%d\n",
+				KBUILD_MODNAME, ret, reg, len);
+		ret = -EREMOTEIO;
+	}
+
+	return ret;
+}
+
+/* read multiple registers */
+static int m88ds3103_rd_regs(struct m88ds3103_priv *priv,
+		u8 reg, u8 *val, int len)
+{
+#define MAX_RD_LEN 3
+#define MAX_RD_XFER_LEN (MAX_RD_LEN)
+	int ret;
+	u8 buf[MAX_RD_XFER_LEN];
+	struct i2c_msg msg[2] = {
+		{
+			.addr = priv->cfg->i2c_addr,
+			.flags = 0,
+			.len = 1,
+			.buf = &reg,
+		}, {
+			.addr = priv->cfg->i2c_addr,
+			.flags = I2C_M_RD,
+			.len = len,
+			.buf = buf,
+		}
+	};
+
+	if (WARN_ON(len > MAX_RD_LEN))
+		return -EINVAL;
+
+	mutex_lock(&priv->i2c_mutex);
+	ret = i2c_transfer(priv->i2c, msg, 2);
+	mutex_unlock(&priv->i2c_mutex);
+	if (ret == 2) {
+		memcpy(val, buf, len);
+		ret = 0;
+	} else {
+		dev_warn(&priv->i2c->dev,
+				"%s: i2c rd failed=%d reg=%02x len=%d\n",
+				KBUILD_MODNAME, ret, reg, len);
+		ret = -EREMOTEIO;
+	}
+
+	return ret;
+}
+
+/* write single register */
+static int m88ds3103_wr_reg(struct m88ds3103_priv *priv, u8 reg, u8 val)
+{
+	return m88ds3103_wr_regs(priv, reg, &val, 1);
+}
+
+/* read single register */
+static int m88ds3103_rd_reg(struct m88ds3103_priv *priv, u8 reg, u8 *val)
+{
+	return m88ds3103_rd_regs(priv, reg, val, 1);
+}
+
+/* write single register with mask */
+static int m88ds3103_wr_reg_mask(struct m88ds3103_priv *priv,
+		u8 reg, u8 val, u8 mask)
+{
+	int ret;
+	u8 u8tmp;
+
+	/* no need for read if whole reg is written */
+	if (mask != 0xff) {
+		ret = m88ds3103_rd_regs(priv, reg, &u8tmp, 1);
+		if (ret)
+			return ret;
+
+		val &= mask;
+		u8tmp &= ~mask;
+		val |= u8tmp;
+	}
+
+	return m88ds3103_wr_regs(priv, reg, &val, 1);
+}
+
+/* read single register with mask */
+static int m88ds3103_rd_reg_mask(struct m88ds3103_priv *priv,
+		u8 reg, u8 *val, u8 mask)
+{
+	int ret, i;
+	u8 u8tmp;
+
+	ret = m88ds3103_rd_regs(priv, reg, &u8tmp, 1);
+	if (ret)
+		return ret;
+
+	u8tmp &= mask;
+
+	/* find position of the first bit */
+	for (i = 0; i < 8; i++) {
+		if ((mask >> i) & 0x01)
+			break;
+	}
+	*val = u8tmp >> i;
+
+	return 0;
+}
+
+/* write reg val table using reg addr auto increment */
+static int m88ds3103_wr_reg_val_tab(struct m88ds3103_priv *priv,
+		const struct m88ds3103_reg_val *tab, int tab_len)
+{
+	int ret, i, j;
+	u8 buf[83];
+	dev_dbg(&priv->i2c->dev, "%s: tab_len=%d\n", __func__, tab_len);
+
+	if (tab_len > 83) {
+		ret = -EINVAL;
+		goto err;
+	}
+
+	for (i = 0, j = 0; i < tab_len; i++, j++) {
+		buf[j] = tab[i].val;
+
+		if (i == tab_len - 1 || tab[i].reg != tab[i + 1].reg - 1 ||
+				!((j + 1) % (priv->cfg->i2c_wr_max - 1))) {
+			ret = m88ds3103_wr_regs(priv, tab[i].reg - j, buf, j + 1);
+			if (ret)
+				goto err;
+
+			j = -1;
+		}
+	}
+
+	return 0;
+err:
+	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	return ret;
+}
+
+static int m88ds3103_read_status(struct dvb_frontend *fe, fe_status_t *status)
+{
+	struct m88ds3103_priv *priv = fe->demodulator_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	int ret;
+	u8 u8tmp;
+
+	*status = 0;
+
+	if (!priv->warm) {
+		ret = -EAGAIN;
+		goto err;
+	}
+
+	switch (c->delivery_system) {
+	case SYS_DVBS:
+		ret = m88ds3103_rd_reg_mask(priv, 0xd1, &u8tmp, 0x07);
+		if (ret)
+			goto err;
+
+		if (u8tmp == 0x07)
+			*status = FE_HAS_SIGNAL | FE_HAS_CARRIER |
+					FE_HAS_VITERBI | FE_HAS_SYNC |
+					FE_HAS_LOCK;
+		break;
+	case SYS_DVBS2:
+		ret = m88ds3103_rd_reg_mask(priv, 0x0d, &u8tmp, 0x8f);
+		if (ret)
+			goto err;
+
+		if (u8tmp == 0x8f)
+			*status = FE_HAS_SIGNAL | FE_HAS_CARRIER |
+					FE_HAS_VITERBI | FE_HAS_SYNC |
+					FE_HAS_LOCK;
+		break;
+	default:
+		dev_dbg(&priv->i2c->dev, "%s: invalid delivery_system\n",
+				__func__);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	priv->fe_status = *status;
+
+	dev_dbg(&priv->i2c->dev, "%s: lock=%02x status=%02x\n",
+			__func__, u8tmp, *status);
+
+	return 0;
+err:
+	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	return ret;
+}
+
+static int m88ds3103_set_frontend(struct dvb_frontend *fe)
+{
+	struct m88ds3103_priv *priv = fe->demodulator_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	int ret, len;
+	const struct m88ds3103_reg_val *init;
+	u8 u8tmp, u8tmp1, u8tmp2;
+	u8 buf[2];
+	u16 u16tmp, divide_ratio;
+	u32 tuner_frequency, target_mclk, ts_clk;
+	s32 s32tmp;
+	dev_dbg(&priv->i2c->dev,
+			"%s: delivery_system=%d modulation=%d frequency=%d symbol_rate=%d inversion=%d pilot=%d rolloff=%d\n",
+			__func__, c->delivery_system,
+			c->modulation, c->frequency, c->symbol_rate,
+			c->inversion, c->pilot, c->rolloff);
+
+	if (!priv->warm) {
+		ret = -EAGAIN;
+		goto err;
+	}
+
+	/* program tuner */
+	if (fe->ops.tuner_ops.set_params) {
+		ret = fe->ops.tuner_ops.set_params(fe);
+		if (ret)
+			goto err;
+	}
+
+	if (fe->ops.tuner_ops.get_frequency) {
+		ret = fe->ops.tuner_ops.get_frequency(fe, &tuner_frequency);
+		if (ret)
+			goto err;
+	}
+
+	/* reset */
+	ret = m88ds3103_wr_reg(priv, 0x07, 0x80);
+	if (ret)
+		goto err;
+
+	ret = m88ds3103_wr_reg(priv, 0x07, 0x00);
+	if (ret)
+		goto err;
+
+	ret = m88ds3103_wr_reg(priv, 0xb2, 0x01);
+	if (ret)
+		goto err;
+
+	ret = m88ds3103_wr_reg(priv, 0x00, 0x01);
+	if (ret)
+		goto err;
+
+	switch (c->delivery_system) {
+	case SYS_DVBS:
+		len = ARRAY_SIZE(m88ds3103_dvbs_init_reg_vals);
+		init = m88ds3103_dvbs_init_reg_vals;
+		target_mclk = 96000;
+		break;
+	case SYS_DVBS2:
+		len = ARRAY_SIZE(m88ds3103_dvbs2_init_reg_vals);
+		init = m88ds3103_dvbs2_init_reg_vals;
+
+		switch (priv->cfg->ts_mode) {
+		case M88DS3103_TS_SERIAL:
+		case M88DS3103_TS_SERIAL_D7:
+			if (c->symbol_rate < 18000000)
+				target_mclk = 96000;
+			else
+				target_mclk = 144000;
+			break;
+		case M88DS3103_TS_PARALLEL:
+		case M88DS3103_TS_PARALLEL_12:
+		case M88DS3103_TS_PARALLEL_16:
+		case M88DS3103_TS_PARALLEL_19_2:
+		case M88DS3103_TS_CI:
+			if (c->symbol_rate < 18000000)
+				target_mclk = 96000;
+			else if (c->symbol_rate < 28000000)
+				target_mclk = 144000;
+			else
+				target_mclk = 192000;
+			break;
+		default:
+			dev_dbg(&priv->i2c->dev, "%s: invalid ts_mode\n",
+					__func__);
+			ret = -EINVAL;
+			goto err;
+		}
+		break;
+	default:
+		dev_dbg(&priv->i2c->dev, "%s: invalid delivery_system\n",
+				__func__);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	/* program init table */
+	if (c->delivery_system != priv->delivery_system) {
+		ret = m88ds3103_wr_reg_val_tab(priv, init, len);
+		if (ret)
+			goto err;
+	}
+
+	u8tmp1 = 0; /* silence compiler warning */
+	switch (priv->cfg->ts_mode) {
+	case M88DS3103_TS_SERIAL:
+		u8tmp1 = 0x00;
+		ts_clk = 0;
+		u8tmp = 0x46;
+		break;
+	case M88DS3103_TS_SERIAL_D7:
+		u8tmp1 = 0x20;
+		ts_clk = 0;
+		u8tmp = 0x46;
+		break;
+	case M88DS3103_TS_PARALLEL:
+		ts_clk = 24000;
+		u8tmp = 0x42;
+		break;
+	case M88DS3103_TS_PARALLEL_12:
+		ts_clk = 12000;
+		u8tmp = 0x42;
+		break;
+	case M88DS3103_TS_PARALLEL_16:
+		ts_clk = 16000;
+		u8tmp = 0x42;
+		break;
+	case M88DS3103_TS_PARALLEL_19_2:
+		ts_clk = 19200;
+		u8tmp = 0x42;
+		break;
+	case M88DS3103_TS_CI:
+		ts_clk = 6000;
+		u8tmp = 0x43;
+		break;
+	default:
+		dev_dbg(&priv->i2c->dev, "%s: invalid ts_mode\n", __func__);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	/* TS mode */
+	ret = m88ds3103_wr_reg(priv, 0xfd, u8tmp);
+	if (ret)
+		goto err;
+
+	switch (priv->cfg->ts_mode) {
+	case M88DS3103_TS_SERIAL:
+	case M88DS3103_TS_SERIAL_D7:
+		ret = m88ds3103_wr_reg_mask(priv, 0x29, u8tmp1, 0x20);
+		if (ret)
+			goto err;
+	}
+
+	if (ts_clk) {
+		divide_ratio = DIV_ROUND_UP(target_mclk, ts_clk);
+		u8tmp1 = divide_ratio / 2;
+		u8tmp2 = DIV_ROUND_UP(divide_ratio, 2);
+	} else {
+		divide_ratio = 0;
+		u8tmp1 = 0;
+		u8tmp2 = 0;
+	}
+
+	dev_dbg(&priv->i2c->dev,
+			"%s: target_mclk=%d ts_clk=%d divide_ratio=%d\n",
+			__func__, target_mclk, ts_clk, divide_ratio);
+
+	u8tmp1--;
+	u8tmp2--;
+	/* u8tmp1[5:2] => fe[3:0], u8tmp1[1:0] => ea[7:6] */
+	u8tmp1 &= 0x3f;
+	/* u8tmp2[5:0] => ea[5:0] */
+	u8tmp2 &= 0x3f;
+
+	ret = m88ds3103_rd_reg(priv, 0xfe, &u8tmp);
+	if (ret)
+		goto err;
+
+	u8tmp = ((u8tmp  & 0xf0) << 0) | u8tmp1 >> 2;
+	ret = m88ds3103_wr_reg(priv, 0xfe, u8tmp);
+	if (ret)
+		goto err;
+
+	u8tmp = ((u8tmp1 & 0x03) << 6) | u8tmp2 >> 0;
+	ret = m88ds3103_wr_reg(priv, 0xea, u8tmp);
+	if (ret)
+		goto err;
+
+	switch (target_mclk) {
+	case 72000:
+		u8tmp1 = 0x00; /* 0b00 */
+		u8tmp2 = 0x03; /* 0b11 */
+		break;
+	case 96000:
+		u8tmp1 = 0x02; /* 0b10 */
+		u8tmp2 = 0x01; /* 0b01 */
+		break;
+	case 115200:
+		u8tmp1 = 0x01; /* 0b01 */
+		u8tmp2 = 0x01; /* 0b01 */
+		break;
+	case 144000:
+		u8tmp1 = 0x00; /* 0b00 */
+		u8tmp2 = 0x01; /* 0b01 */
+		break;
+	case 192000:
+		u8tmp1 = 0x03; /* 0b11 */
+		u8tmp2 = 0x00; /* 0b00 */
+		break;
+	default:
+		dev_dbg(&priv->i2c->dev, "%s: invalid target_mclk\n", __func__);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	ret = m88ds3103_wr_reg_mask(priv, 0x22, u8tmp1 << 6, 0xc0);
+	if (ret)
+		goto err;
+
+	ret = m88ds3103_wr_reg_mask(priv, 0x24, u8tmp2 << 6, 0xc0);
+	if (ret)
+		goto err;
+
+	if (c->symbol_rate <= 3000000)
+		u8tmp = 0x20;
+	else if (c->symbol_rate <= 10000000)
+		u8tmp = 0x10;
+	else
+		u8tmp = 0x06;
+
+	ret = m88ds3103_wr_reg(priv, 0xc3, 0x08);
+	if (ret)
+		goto err;
+
+	ret = m88ds3103_wr_reg(priv, 0xc8, u8tmp);
+	if (ret)
+		goto err;
+
+	ret = m88ds3103_wr_reg(priv, 0xc4, 0x08);
+	if (ret)
+		goto err;
+
+	ret = m88ds3103_wr_reg(priv, 0xc7, 0x00);
+	if (ret)
+		goto err;
+
+	u16tmp = DIV_ROUND_CLOSEST((c->symbol_rate / 1000) << 15, M88DS3103_MCLK_KHZ / 2);
+	buf[0] = (u16tmp >> 0) & 0xff;
+	buf[1] = (u16tmp >> 8) & 0xff;
+	ret = m88ds3103_wr_regs(priv, 0x61, buf, 2);
+	if (ret)
+		goto err;
+
+	ret = m88ds3103_wr_reg_mask(priv, 0x4d, priv->cfg->spec_inv << 1, 0x02);
+	if (ret)
+		goto err;
+
+	ret = m88ds3103_wr_reg_mask(priv, 0x30, priv->cfg->agc_inv << 4, 0x10);
+	if (ret)
+		goto err;
+
+	ret = m88ds3103_wr_reg(priv, 0x33, priv->cfg->agc);
+	if (ret)
+		goto err;
+
+	dev_dbg(&priv->i2c->dev, "%s: carrier offset=%d\n", __func__,
+			(tuner_frequency - c->frequency));
+
+	s32tmp = 0x10000 * (tuner_frequency - c->frequency);
+	s32tmp = DIV_ROUND_CLOSEST(s32tmp, M88DS3103_MCLK_KHZ);
+	if (s32tmp < 0)
+		s32tmp += 0x10000;
+
+	buf[0] = (s32tmp >> 0) & 0xff;
+	buf[1] = (s32tmp >> 8) & 0xff;
+	ret = m88ds3103_wr_regs(priv, 0x5e, buf, 2);
+	if (ret)
+		goto err;
+
+	ret = m88ds3103_wr_reg(priv, 0x00, 0x00);
+	if (ret)
+		goto err;
+
+	ret = m88ds3103_wr_reg(priv, 0xb2, 0x00);
+	if (ret)
+		goto err;
+
+	priv->delivery_system = c->delivery_system;
+
+	return 0;
+err:
+	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	return ret;
+}
+
+static int m88ds3103_init(struct dvb_frontend *fe)
+{
+	struct m88ds3103_priv *priv = fe->demodulator_priv;
+	int ret, len, remaining;
+	const struct firmware *fw = NULL;
+	u8 *fw_file = M88DS3103_FIRMWARE;
+	u8 u8tmp;
+	dev_dbg(&priv->i2c->dev, "%s:\n", __func__);
+
+	/* set cold state by default */
+	priv->warm = false;
+
+	/* wake up device from sleep */
+	ret = m88ds3103_wr_reg_mask(priv, 0x08, 0x01, 0x01);
+	if (ret)
+		goto err;
+
+	ret = m88ds3103_wr_reg_mask(priv, 0x04, 0x00, 0x01);
+	if (ret)
+		goto err;
+
+	ret = m88ds3103_wr_reg_mask(priv, 0x23, 0x00, 0x10);
+	if (ret)
+		goto err;
+
+	/* reset */
+	ret = m88ds3103_wr_reg(priv, 0x07, 0x60);
+	if (ret)
+		goto err;
+
+	ret = m88ds3103_wr_reg(priv, 0x07, 0x00);
+	if (ret)
+		goto err;
+
+	/* firmware status */
+	ret = m88ds3103_rd_reg(priv, 0xb9, &u8tmp);
+	if (ret)
+		goto err;
+
+	dev_dbg(&priv->i2c->dev, "%s: firmware=%02x\n", __func__, u8tmp);
+
+	if (u8tmp)
+		goto skip_fw_download;
+
+	/* cold state - try to download firmware */
+	dev_info(&priv->i2c->dev, "%s: found a '%s' in cold state\n",
+			KBUILD_MODNAME, m88ds3103_ops.info.name);
+
+	/* request the firmware, this will block and timeout */
+	ret = request_firmware(&fw, fw_file, priv->i2c->dev.parent);
+	if (ret) {
+		dev_err(&priv->i2c->dev, "%s: firmare file '%s' not found\n",
+				KBUILD_MODNAME, fw_file);
+		goto err;
+	}
+
+	dev_info(&priv->i2c->dev, "%s: downloading firmware from file '%s'\n",
+			KBUILD_MODNAME, fw_file);
+
+	ret = m88ds3103_wr_reg(priv, 0xb2, 0x01);
+	if (ret)
+		goto err;
+
+	for (remaining = fw->size; remaining > 0;
+			remaining -= (priv->cfg->i2c_wr_max - 1)) {
+		len = remaining;
+		if (len > (priv->cfg->i2c_wr_max - 1))
+			len = (priv->cfg->i2c_wr_max - 1);
+
+		ret = m88ds3103_wr_regs(priv, 0xb0,
+				&fw->data[fw->size - remaining], len);
+		if (ret) {
+			dev_err(&priv->i2c->dev,
+					"%s: firmware download failed=%d\n",
+					KBUILD_MODNAME, ret);
+			goto err;
+		}
+	}
+
+	ret = m88ds3103_wr_reg(priv, 0xb2, 0x00);
+	if (ret)
+		goto err;
+
+	release_firmware(fw);
+	fw = NULL;
+
+	ret = m88ds3103_rd_reg(priv, 0xb9, &u8tmp);
+	if (ret)
+		goto err;
+
+	if (!u8tmp) {
+		dev_info(&priv->i2c->dev, "%s: firmware did not run\n",
+				KBUILD_MODNAME);
+		ret = -EFAULT;
+		goto err;
+	}
+
+	dev_info(&priv->i2c->dev, "%s: found a '%s' in warm state\n",
+			KBUILD_MODNAME, m88ds3103_ops.info.name);
+	dev_info(&priv->i2c->dev, "%s: firmware version %X.%X\n",
+			KBUILD_MODNAME, (u8tmp >> 4) & 0xf, (u8tmp >> 0 & 0xf));
+
+skip_fw_download:
+	/* warm state */
+	priv->warm = true;
+
+	return 0;
+err:
+	if (fw)
+		release_firmware(fw);
+
+	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	return ret;
+}
+
+static int m88ds3103_sleep(struct dvb_frontend *fe)
+{
+	struct m88ds3103_priv *priv = fe->demodulator_priv;
+	int ret;
+	dev_dbg(&priv->i2c->dev, "%s:\n", __func__);
+
+	priv->delivery_system = SYS_UNDEFINED;
+
+	/* TS Hi-Z */
+	ret = m88ds3103_wr_reg_mask(priv, 0x27, 0x00, 0x01);
+	if (ret)
+		goto err;
+
+	/* sleep */
+	ret = m88ds3103_wr_reg_mask(priv, 0x08, 0x00, 0x01);
+	if (ret)
+		goto err;
+
+	ret = m88ds3103_wr_reg_mask(priv, 0x04, 0x01, 0x01);
+	if (ret)
+		goto err;
+
+	ret = m88ds3103_wr_reg_mask(priv, 0x23, 0x10, 0x10);
+	if (ret)
+		goto err;
+
+	return 0;
+err:
+	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	return ret;
+}
+
+static int m88ds3103_get_frontend(struct dvb_frontend *fe)
+{
+	struct m88ds3103_priv *priv = fe->demodulator_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	int ret;
+	u8 buf[3];
+	dev_dbg(&priv->i2c->dev, "%s:\n", __func__);
+
+	if (!priv->warm || !(priv->fe_status & FE_HAS_LOCK)) {
+		ret = -EAGAIN;
+		goto err;
+	}
+
+	switch (c->delivery_system) {
+	case SYS_DVBS:
+		ret = m88ds3103_rd_reg(priv, 0xe0, &buf[0]);
+		if (ret)
+			goto err;
+
+		ret = m88ds3103_rd_reg(priv, 0xe6, &buf[1]);
+		if (ret)
+			goto err;
+
+		switch ((buf[0] >> 2) & 0x01) {
+		case 0:
+			c->inversion = INVERSION_OFF;
+			break;
+		case 1:
+			c->inversion = INVERSION_ON;
+			break;
+		default:
+			dev_dbg(&priv->i2c->dev, "%s: invalid inversion\n",
+					__func__);
+		}
+
+		switch ((buf[1] >> 5) & 0x07) {
+		case 0:
+			c->fec_inner = FEC_7_8;
+			break;
+		case 1:
+			c->fec_inner = FEC_5_6;
+			break;
+		case 2:
+			c->fec_inner = FEC_3_4;
+			break;
+		case 3:
+			c->fec_inner = FEC_2_3;
+			break;
+		case 4:
+			c->fec_inner = FEC_1_2;
+			break;
+		default:
+			dev_dbg(&priv->i2c->dev, "%s: invalid fec_inner\n",
+					__func__);
+		}
+
+		c->modulation = QPSK;
+
+		break;
+	case SYS_DVBS2:
+		ret = m88ds3103_rd_reg(priv, 0x7e, &buf[0]);
+		if (ret)
+			goto err;
+
+		ret = m88ds3103_rd_reg(priv, 0x89, &buf[1]);
+		if (ret)
+			goto err;
+
+		ret = m88ds3103_rd_reg(priv, 0xf2, &buf[2]);
+		if (ret)
+			goto err;
+
+		switch ((buf[0] >> 0) & 0x0f) {
+		case 2:
+			c->fec_inner = FEC_2_5;
+			break;
+		case 3:
+			c->fec_inner = FEC_1_2;
+			break;
+		case 4:
+			c->fec_inner = FEC_3_5;
+			break;
+		case 5:
+			c->fec_inner = FEC_2_3;
+			break;
+		case 6:
+			c->fec_inner = FEC_3_4;
+			break;
+		case 7:
+			c->fec_inner = FEC_4_5;
+			break;
+		case 8:
+			c->fec_inner = FEC_5_6;
+			break;
+		case 9:
+			c->fec_inner = FEC_8_9;
+			break;
+		case 10:
+			c->fec_inner = FEC_9_10;
+			break;
+		default:
+			dev_dbg(&priv->i2c->dev, "%s: invalid fec_inner\n",
+					__func__);
+		}
+
+		switch ((buf[0] >> 5) & 0x01) {
+		case 0:
+			c->pilot = PILOT_OFF;
+			break;
+		case 1:
+			c->pilot = PILOT_ON;
+			break;
+		default:
+			dev_dbg(&priv->i2c->dev, "%s: invalid pilot\n",
+					__func__);
+		}
+
+		switch ((buf[0] >> 6) & 0x07) {
+		case 0:
+			c->modulation = QPSK;
+			break;
+		case 1:
+			c->modulation = PSK_8;
+			break;
+		case 2:
+			c->modulation = APSK_16;
+			break;
+		case 3:
+			c->modulation = APSK_32;
+			break;
+		default:
+			dev_dbg(&priv->i2c->dev, "%s: invalid modulation\n",
+					__func__);
+		}
+
+		switch ((buf[1] >> 7) & 0x01) {
+		case 0:
+			c->inversion = INVERSION_OFF;
+			break;
+		case 1:
+			c->inversion = INVERSION_ON;
+			break;
+		default:
+			dev_dbg(&priv->i2c->dev, "%s: invalid inversion\n",
+					__func__);
+		}
+
+		switch ((buf[2] >> 0) & 0x03) {
+		case 0:
+			c->rolloff = ROLLOFF_35;
+			break;
+		case 1:
+			c->rolloff = ROLLOFF_25;
+			break;
+		case 2:
+			c->rolloff = ROLLOFF_20;
+			break;
+		default:
+			dev_dbg(&priv->i2c->dev, "%s: invalid rolloff\n",
+					__func__);
+		}
+		break;
+	default:
+		dev_dbg(&priv->i2c->dev, "%s: invalid delivery_system\n",
+				__func__);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	ret = m88ds3103_rd_regs(priv, 0x6d, buf, 2);
+	if (ret)
+		goto err;
+
+	c->symbol_rate = 1ull * ((buf[1] << 8) | (buf[0] << 0)) *
+			M88DS3103_MCLK_KHZ * 1000 / 0x10000;
+
+	return 0;
+err:
+	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	return ret;
+}
+
+static int m88ds3103_read_snr(struct dvb_frontend *fe, u16 *snr)
+{
+	struct m88ds3103_priv *priv = fe->demodulator_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	int ret, i, tmp;
+	u8 buf[3];
+	u16 noise, signal;
+	u32 noise_tot, signal_tot;
+	dev_dbg(&priv->i2c->dev, "%s:\n", __func__);
+	/* reports SNR in resolution of 0.1 dB */
+
+	/* more iterations for more accurate estimation */
+	#define M88DS3103_SNR_ITERATIONS 3
+
+	switch (c->delivery_system) {
+	case SYS_DVBS:
+		tmp = 0;
+
+		for (i = 0; i < M88DS3103_SNR_ITERATIONS; i++) {
+			ret = m88ds3103_rd_reg(priv, 0xff, &buf[0]);
+			if (ret)
+				goto err;
+
+			tmp += buf[0];
+		}
+
+		/* use of one register limits max value to 15 dB */
+		/* SNR(X) dB = 10 * ln(X) / ln(10) dB */
+		tmp = DIV_ROUND_CLOSEST(tmp, 8 * M88DS3103_SNR_ITERATIONS);
+		if (tmp)
+			*snr = 100ul * intlog2(tmp) / intlog2(10);
+		else
+			*snr = 0;
+		break;
+	case SYS_DVBS2:
+		noise_tot = 0;
+		signal_tot = 0;
+
+		for (i = 0; i < M88DS3103_SNR_ITERATIONS; i++) {
+			ret = m88ds3103_rd_regs(priv, 0x8c, buf, 3);
+			if (ret)
+				goto err;
+
+			noise = buf[1] << 6;    /* [13:6] */
+			noise |= buf[0] & 0x3f; /*  [5:0] */
+			noise >>= 2;
+			signal = buf[2] * buf[2];
+			signal >>= 1;
+
+			noise_tot += noise;
+			signal_tot += signal;
+		}
+
+		noise = noise_tot / M88DS3103_SNR_ITERATIONS;
+		signal = signal_tot / M88DS3103_SNR_ITERATIONS;
+
+		/* SNR(X) dB = 10 * log10(X) dB */
+		if (signal > noise) {
+			tmp = signal / noise;
+			*snr = 100ul * intlog10(tmp) / (1 << 24);
+		} else {
+			*snr = 0;
+		}
+		break;
+	default:
+		dev_dbg(&priv->i2c->dev, "%s: invalid delivery_system\n",
+				__func__);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	return 0;
+err:
+	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	return ret;
+}
+
+
+static int m88ds3103_set_tone(struct dvb_frontend *fe,
+	fe_sec_tone_mode_t fe_sec_tone_mode)
+{
+	struct m88ds3103_priv *priv = fe->demodulator_priv;
+	int ret;
+	u8 u8tmp, tone, reg_a1_mask;
+	dev_dbg(&priv->i2c->dev, "%s: fe_sec_tone_mode=%d\n", __func__,
+			fe_sec_tone_mode);
+
+	if (!priv->warm) {
+		ret = -EAGAIN;
+		goto err;
+	}
+
+	switch (fe_sec_tone_mode) {
+	case SEC_TONE_ON:
+		tone = 0;
+		reg_a1_mask = 0x87;
+		break;
+	case SEC_TONE_OFF:
+		tone = 1;
+		reg_a1_mask = 0x00;
+		break;
+	default:
+		dev_dbg(&priv->i2c->dev, "%s: invalid fe_sec_tone_mode\n",
+				__func__);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	u8tmp = tone << 7 | priv->cfg->envelope_mode << 5;
+	ret = m88ds3103_wr_reg_mask(priv, 0xa2, u8tmp, 0xe0);
+	if (ret)
+		goto err;
+
+	u8tmp = 1 << 2;
+	ret = m88ds3103_wr_reg_mask(priv, 0xa1, u8tmp, reg_a1_mask);
+	if (ret)
+		goto err;
+
+	return 0;
+err:
+	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	return ret;
+}
+
+static int m88ds3103_diseqc_send_master_cmd(struct dvb_frontend *fe,
+		struct dvb_diseqc_master_cmd *diseqc_cmd)
+{
+	struct m88ds3103_priv *priv = fe->demodulator_priv;
+	int ret, i;
+	u8 u8tmp;
+	dev_dbg(&priv->i2c->dev, "%s: msg=%*ph\n", __func__,
+			diseqc_cmd->msg_len, diseqc_cmd->msg);
+
+	if (!priv->warm) {
+		ret = -EAGAIN;
+		goto err;
+	}
+
+	if (diseqc_cmd->msg_len < 3 || diseqc_cmd->msg_len > 6) {
+		ret = -EINVAL;
+		goto err;
+	}
+
+	u8tmp = priv->cfg->envelope_mode << 5;
+	ret = m88ds3103_wr_reg_mask(priv, 0xa2, u8tmp, 0xe0);
+	if (ret)
+		goto err;
+
+	ret = m88ds3103_wr_regs(priv, 0xa3, diseqc_cmd->msg,
+			diseqc_cmd->msg_len);
+	if (ret)
+		goto err;
+
+	ret = m88ds3103_wr_reg(priv, 0xa1,
+			(diseqc_cmd->msg_len - 1) << 3 | 0x07);
+	if (ret)
+		goto err;
+
+	/* DiSEqC message typical period is 54 ms */
+	usleep_range(40000, 60000);
+
+	/* wait DiSEqC TX ready */
+	for (i = 20, u8tmp = 1; i && u8tmp; i--) {
+		usleep_range(5000, 10000);
+
+		ret = m88ds3103_rd_reg_mask(priv, 0xa1, &u8tmp, 0x40);
+		if (ret)
+			goto err;
+	}
+
+	dev_dbg(&priv->i2c->dev, "%s: loop=%d\n", __func__, i);
+
+	if (i == 0) {
+		dev_dbg(&priv->i2c->dev, "%s: diseqc tx timeout\n", __func__);
+
+		ret = m88ds3103_wr_reg_mask(priv, 0xa1, 0x40, 0xc0);
+		if (ret)
+			goto err;
+	}
+
+	ret = m88ds3103_wr_reg_mask(priv, 0xa2, 0x80, 0xc0);
+	if (ret)
+		goto err;
+
+	if (i == 0) {
+		ret = -ETIMEDOUT;
+		goto err;
+	}
+
+	return 0;
+err:
+	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	return ret;
+}
+
+static int m88ds3103_diseqc_send_burst(struct dvb_frontend *fe,
+	fe_sec_mini_cmd_t fe_sec_mini_cmd)
+{
+	struct m88ds3103_priv *priv = fe->demodulator_priv;
+	int ret, i;
+	u8 u8tmp, burst;
+	dev_dbg(&priv->i2c->dev, "%s: fe_sec_mini_cmd=%d\n", __func__,
+			fe_sec_mini_cmd);
+
+	if (!priv->warm) {
+		ret = -EAGAIN;
+		goto err;
+	}
+
+	u8tmp = priv->cfg->envelope_mode << 5;
+	ret = m88ds3103_wr_reg_mask(priv, 0xa2, u8tmp, 0xe0);
+	if (ret)
+		goto err;
+
+	switch (fe_sec_mini_cmd) {
+	case SEC_MINI_A:
+		burst = 0x02;
+		break;
+	case SEC_MINI_B:
+		burst = 0x01;
+		break;
+	default:
+		dev_dbg(&priv->i2c->dev, "%s: invalid fe_sec_mini_cmd\n",
+				__func__);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	ret = m88ds3103_wr_reg(priv, 0xa1, burst);
+	if (ret)
+		goto err;
+
+	/* DiSEqC ToneBurst period is 12.5 ms */
+	usleep_range(11000, 20000);
+
+	/* wait DiSEqC TX ready */
+	for (i = 5, u8tmp = 1; i && u8tmp; i--) {
+		usleep_range(800, 2000);
+
+		ret = m88ds3103_rd_reg_mask(priv, 0xa1, &u8tmp, 0x40);
+		if (ret)
+			goto err;
+	}
+
+	dev_dbg(&priv->i2c->dev, "%s: loop=%d\n", __func__, i);
+
+	ret = m88ds3103_wr_reg_mask(priv, 0xa2, 0x80, 0xc0);
+	if (ret)
+		goto err;
+
+	if (i == 0) {
+		dev_dbg(&priv->i2c->dev, "%s: diseqc tx timeout\n", __func__);
+		ret = -ETIMEDOUT;
+		goto err;
+	}
+
+	return 0;
+err:
+	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	return ret;
+}
+
+static int m88ds3103_get_tune_settings(struct dvb_frontend *fe,
+	struct dvb_frontend_tune_settings *s)
+{
+	s->min_delay_ms = 3000;
+
+	return 0;
+}
+
+static void m88ds3103_release(struct dvb_frontend *fe)
+{
+	struct m88ds3103_priv *priv = fe->demodulator_priv;
+	i2c_del_mux_adapter(priv->i2c_adapter);
+	kfree(priv);
+}
+
+static int m88ds3103_select(struct i2c_adapter *adap, void *mux_priv, u32 chan)
+{
+	struct m88ds3103_priv *priv = mux_priv;
+	int ret;
+	struct i2c_msg gate_open_msg[1] = {
+		{
+			.addr = priv->cfg->i2c_addr,
+			.flags = 0,
+			.len = 2,
+			.buf = "\x03\x11",
+		}
+	};
+
+	mutex_lock(&priv->i2c_mutex);
+
+	/* open tuner I2C repeater for 1 xfer, closes automatically */
+	ret = __i2c_transfer(priv->i2c, gate_open_msg, 1);
+	if (ret != 1) {
+		dev_warn(&priv->i2c->dev, "%s: i2c wr failed=%d\n",
+				KBUILD_MODNAME, ret);
+		if (ret >= 0)
+			ret = -EREMOTEIO;
+
+		return ret;
+	}
+
+	return 0;
+}
+
+static int m88ds3103_deselect(struct i2c_adapter *adap, void *mux_priv,
+		u32 chan)
+{
+	struct m88ds3103_priv *priv = mux_priv;
+
+	mutex_unlock(&priv->i2c_mutex);
+
+	return 0;
+}
+
+struct dvb_frontend *m88ds3103_attach(const struct m88ds3103_config *cfg,
+		struct i2c_adapter *i2c, struct i2c_adapter **tuner_i2c_adapter)
+{
+	int ret;
+	struct m88ds3103_priv *priv;
+	u8 chip_id, u8tmp;
+
+	/* allocate memory for the internal priv */
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv) {
+		ret = -ENOMEM;
+		dev_err(&i2c->dev, "%s: kzalloc() failed\n", KBUILD_MODNAME);
+		goto err;
+	}
+
+	priv->cfg = cfg;
+	priv->i2c = i2c;
+	mutex_init(&priv->i2c_mutex);
+
+	ret = m88ds3103_rd_reg(priv, 0x01, &chip_id);
+	if (ret)
+		goto err;
+
+	dev_dbg(&priv->i2c->dev, "%s: chip_id=%02x\n", __func__, chip_id);
+
+	switch (chip_id) {
+	case 0xd0:
+		break;
+	default:
+		goto err;
+	}
+
+	switch (priv->cfg->clock_out) {
+	case M88DS3103_CLOCK_OUT_DISABLED:
+		u8tmp = 0x80;
+		break;
+	case M88DS3103_CLOCK_OUT_ENABLED:
+		u8tmp = 0x00;
+		break;
+	case M88DS3103_CLOCK_OUT_ENABLED_DIV2:
+		u8tmp = 0x10;
+		break;
+	default:
+		goto err;
+	}
+
+	ret = m88ds3103_wr_reg(priv, 0x29, u8tmp);
+	if (ret)
+		goto err;
+
+	/* sleep */
+	ret = m88ds3103_wr_reg_mask(priv, 0x08, 0x00, 0x01);
+	if (ret)
+		goto err;
+
+	ret = m88ds3103_wr_reg_mask(priv, 0x04, 0x01, 0x01);
+	if (ret)
+		goto err;
+
+	ret = m88ds3103_wr_reg_mask(priv, 0x23, 0x10, 0x10);
+	if (ret)
+		goto err;
+
+	/* create mux i2c adapter for tuner */
+	priv->i2c_adapter = i2c_add_mux_adapter(i2c, &i2c->dev, priv, 0, 0, 0,
+			m88ds3103_select, m88ds3103_deselect);
+	if (priv->i2c_adapter == NULL)
+		goto err;
+
+	*tuner_i2c_adapter = priv->i2c_adapter;
+
+	/* create dvb_frontend */
+	memcpy(&priv->fe.ops, &m88ds3103_ops, sizeof(struct dvb_frontend_ops));
+	priv->fe.demodulator_priv = priv;
+
+	return &priv->fe;
+err:
+	dev_dbg(&i2c->dev, "%s: failed=%d\n", __func__, ret);
+	kfree(priv);
+	return NULL;
+}
+EXPORT_SYMBOL(m88ds3103_attach);
+
+static struct dvb_frontend_ops m88ds3103_ops = {
+	.delsys = { SYS_DVBS, SYS_DVBS2 },
+	.info = {
+		.name = "Montage M88DS3103",
+		.frequency_min =  950000,
+		.frequency_max = 2150000,
+		.frequency_tolerance = 5000,
+		.symbol_rate_min =  1000000,
+		.symbol_rate_max = 45000000,
+		.caps = FE_CAN_INVERSION_AUTO |
+			FE_CAN_FEC_1_2 |
+			FE_CAN_FEC_2_3 |
+			FE_CAN_FEC_3_4 |
+			FE_CAN_FEC_4_5 |
+			FE_CAN_FEC_5_6 |
+			FE_CAN_FEC_6_7 |
+			FE_CAN_FEC_7_8 |
+			FE_CAN_FEC_8_9 |
+			FE_CAN_FEC_AUTO |
+			FE_CAN_QPSK |
+			FE_CAN_RECOVER |
+			FE_CAN_2G_MODULATION
+	},
+
+	.release = m88ds3103_release,
+
+	.get_tune_settings = m88ds3103_get_tune_settings,
+
+	.init = m88ds3103_init,
+	.sleep = m88ds3103_sleep,
+
+	.set_frontend = m88ds3103_set_frontend,
+	.get_frontend = m88ds3103_get_frontend,
+
+	.read_status = m88ds3103_read_status,
+	.read_snr = m88ds3103_read_snr,
+
+	.diseqc_send_master_cmd = m88ds3103_diseqc_send_master_cmd,
+	.diseqc_send_burst = m88ds3103_diseqc_send_burst,
+
+	.set_tone = m88ds3103_set_tone,
+};
+
+MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
+MODULE_DESCRIPTION("Montage M88DS3103 DVB-S/S2 demodulator driver");
+MODULE_LICENSE("GPL");
+MODULE_FIRMWARE(M88DS3103_FIRMWARE);
diff --git a/drivers/media/dvb-frontends/m88ds3103.h b/drivers/media/dvb-frontends/m88ds3103.h
new file mode 100644
index 000000000000..bbb7e3aa5675
--- /dev/null
+++ b/drivers/media/dvb-frontends/m88ds3103.h
@@ -0,0 +1,114 @@
+/*
+ * Montage M88DS3103 demodulator driver
+ *
+ * Copyright (C) 2013 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 M88DS3103_H
+#define M88DS3103_H
+
+#include <linux/dvb/frontend.h>
+
+struct m88ds3103_config {
+	/*
+	 * I2C address
+	 * Default: none, must set
+	 * 0x68, ...
+	 */
+	u8 i2c_addr;
+
+	/*
+	 * clock
+	 * Default: none, must set
+	 * 27000000
+	 */
+	u32 clock;
+
+	/*
+	 * max bytes I2C provider is asked to write at once
+	 * Default: none, must set
+	 * 33, 65, ...
+	 */
+	u16 i2c_wr_max;
+
+	/*
+	 * TS output mode
+	 * Default: M88DS3103_TS_SERIAL
+	 */
+#define M88DS3103_TS_SERIAL             0 /* TS output pin D0, normal */
+#define M88DS3103_TS_SERIAL_D7          1 /* TS output pin D7 */
+#define M88DS3103_TS_PARALLEL           2 /* 24 MHz, normal */
+#define M88DS3103_TS_PARALLEL_12        3 /* 12 MHz */
+#define M88DS3103_TS_PARALLEL_16        4 /* 16 MHz */
+#define M88DS3103_TS_PARALLEL_19_2      5 /* 19.2 MHz */
+#define M88DS3103_TS_CI                 6 /* 6 MHz */
+	u8 ts_mode;
+
+	/*
+	 * spectrum inversion
+	 * Default: 0
+	 */
+	u8 spec_inv:1;
+
+	/*
+	 * AGC polarity
+	 * Default: 0
+	 */
+	u8 agc_inv:1;
+
+	/*
+	 * clock output
+	 * Default: M88DS3103_CLOCK_OUT_DISABLED
+	 */
+#define M88DS3103_CLOCK_OUT_DISABLED        0
+#define M88DS3103_CLOCK_OUT_ENABLED         1
+#define M88DS3103_CLOCK_OUT_ENABLED_DIV2    2
+	u8 clock_out;
+
+	/*
+	 * DiSEqC envelope mode
+	 * Default: 0
+	 */
+	u8 envelope_mode:1;
+
+	/*
+	 * AGC configuration
+	 * Default: none, must set
+	 */
+	u8 agc;
+};
+
+/*
+ * Driver implements own I2C-adapter for tuner I2C access. That's since chip
+ * has I2C-gate control which closes gate automatically after I2C transfer.
+ * Using own I2C adapter we can workaround that.
+ */
+
+#if defined(CONFIG_DVB_M88DS3103) || \
+		(defined(CONFIG_DVB_M88DS3103_MODULE) && defined(MODULE))
+extern struct dvb_frontend *m88ds3103_attach(
+		const struct m88ds3103_config *config,
+		struct i2c_adapter *i2c,
+		struct i2c_adapter **tuner_i2c);
+#else
+static inline struct dvb_frontend *m88ds3103_attach(
+		const struct m88ds3103_config *config,
+		struct i2c_adapter *i2c,
+		struct i2c_adapter **tuner_i2c)
+{
+	pr_warn("%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
+#endif
+
+#endif
diff --git a/drivers/media/dvb-frontends/m88ds3103_priv.h b/drivers/media/dvb-frontends/m88ds3103_priv.h
new file mode 100644
index 000000000000..84c3c06df622
--- /dev/null
+++ b/drivers/media/dvb-frontends/m88ds3103_priv.h
@@ -0,0 +1,215 @@
+/*
+ * Montage M88DS3103 demodulator driver
+ *
+ * Copyright (C) 2013 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 M88DS3103_PRIV_H
+#define M88DS3103_PRIV_H
+
+#include "dvb_frontend.h"
+#include "m88ds3103.h"
+#include "dvb_math.h"
+#include <linux/firmware.h>
+#include <linux/i2c-mux.h>
+
+#define M88DS3103_FIRMWARE "dvb-demod-m88ds3103.fw"
+#define M88DS3103_MCLK_KHZ 96000
+
+struct m88ds3103_priv {
+	struct i2c_adapter *i2c;
+	/* mutex needed due to own tuner I2C adapter */
+	struct mutex i2c_mutex;
+	const struct m88ds3103_config *cfg;
+	struct dvb_frontend fe;
+	fe_delivery_system_t delivery_system;
+	fe_status_t fe_status;
+	bool warm; /* FW running */
+	struct i2c_adapter *i2c_adapter;
+};
+
+struct m88ds3103_reg_val {
+	u8 reg;
+	u8 val;
+};
+
+static const struct m88ds3103_reg_val m88ds3103_dvbs_init_reg_vals[] = {
+	{0x23, 0x07},
+	{0x08, 0x03},
+	{0x0c, 0x02},
+	{0x21, 0x54},
+	{0x25, 0x8a},
+	{0x27, 0x31},
+	{0x30, 0x08},
+	{0x31, 0x40},
+	{0x32, 0x32},
+	{0x35, 0xff},
+	{0x3a, 0x00},
+	{0x37, 0x10},
+	{0x38, 0x10},
+	{0x39, 0x02},
+	{0x42, 0x60},
+	{0x4a, 0x80},
+	{0x4b, 0x04},
+	{0x4d, 0x91},
+	{0x5d, 0xc8},
+	{0x50, 0x36},
+	{0x51, 0x36},
+	{0x52, 0x36},
+	{0x53, 0x36},
+	{0x56, 0x01},
+	{0x63, 0x0f},
+	{0x64, 0x30},
+	{0x65, 0x40},
+	{0x68, 0x26},
+	{0x69, 0x4c},
+	{0x70, 0x20},
+	{0x71, 0x70},
+	{0x72, 0x04},
+	{0x73, 0x00},
+	{0x70, 0x40},
+	{0x71, 0x70},
+	{0x72, 0x04},
+	{0x73, 0x00},
+	{0x70, 0x60},
+	{0x71, 0x70},
+	{0x72, 0x04},
+	{0x73, 0x00},
+	{0x70, 0x80},
+	{0x71, 0x70},
+	{0x72, 0x04},
+	{0x73, 0x00},
+	{0x70, 0xa0},
+	{0x71, 0x70},
+	{0x72, 0x04},
+	{0x73, 0x00},
+	{0x70, 0x1f},
+	{0x76, 0x38},
+	{0x77, 0xa6},
+	{0x78, 0x0c},
+	{0x79, 0x80},
+	{0x7f, 0x14},
+	{0x7c, 0x00},
+	{0xae, 0x82},
+	{0x80, 0x64},
+	{0x81, 0x66},
+	{0x82, 0x44},
+	{0x85, 0x04},
+	{0xcd, 0xf4},
+	{0x90, 0x33},
+	{0xa0, 0x44},
+	{0xc0, 0x08},
+	{0xc3, 0x10},
+	{0xc4, 0x08},
+	{0xc5, 0xf0},
+	{0xc6, 0xff},
+	{0xc7, 0x00},
+	{0xc8, 0x1a},
+	{0xc9, 0x80},
+	{0xe0, 0xf8},
+	{0xe6, 0x8b},
+	{0xd0, 0x40},
+	{0xf8, 0x20},
+	{0xfa, 0x0f},
+	{0x00, 0x00},
+	{0xbd, 0x01},
+	{0xb8, 0x00},
+};
+
+static const struct m88ds3103_reg_val m88ds3103_dvbs2_init_reg_vals[] = {
+	{0x23, 0x07},
+	{0x08, 0x07},
+	{0x0c, 0x02},
+	{0x21, 0x54},
+	{0x25, 0x8a},
+	{0x27, 0x31},
+	{0x30, 0x08},
+	{0x32, 0x32},
+	{0x35, 0xff},
+	{0x3a, 0x00},
+	{0x37, 0x10},
+	{0x38, 0x10},
+	{0x39, 0x02},
+	{0x42, 0x60},
+	{0x4a, 0x80},
+	{0x4b, 0x04},
+	{0x4d, 0x91},
+	{0x5d, 0xc8},
+	{0x50, 0x36},
+	{0x51, 0x36},
+	{0x52, 0x36},
+	{0x53, 0x36},
+	{0x56, 0x01},
+	{0x63, 0x0f},
+	{0x64, 0x10},
+	{0x65, 0x20},
+	{0x68, 0x46},
+	{0x69, 0xcd},
+	{0x70, 0x20},
+	{0x71, 0x70},
+	{0x72, 0x04},
+	{0x73, 0x00},
+	{0x70, 0x40},
+	{0x71, 0x70},
+	{0x72, 0x04},
+	{0x73, 0x00},
+	{0x70, 0x60},
+	{0x71, 0x70},
+	{0x72, 0x04},
+	{0x73, 0x00},
+	{0x70, 0x80},
+	{0x71, 0x70},
+	{0x72, 0x04},
+	{0x73, 0x00},
+	{0x70, 0xa0},
+	{0x71, 0x70},
+	{0x72, 0x04},
+	{0x73, 0x00},
+	{0x70, 0x1f},
+	{0x76, 0x38},
+	{0x77, 0xa6},
+	{0x78, 0x0c},
+	{0x79, 0x80},
+	{0x7f, 0x14},
+	{0x85, 0x08},
+	{0xcd, 0xf4},
+	{0x90, 0x33},
+	{0x86, 0x00},
+	{0x87, 0x0f},
+	{0x89, 0x00},
+	{0x8b, 0x44},
+	{0x8c, 0x66},
+	{0x9d, 0xc1},
+	{0x8a, 0x10},
+	{0xad, 0x40},
+	{0xa0, 0x44},
+	{0xc0, 0x08},
+	{0xc1, 0x10},
+	{0xc2, 0x08},
+	{0xc3, 0x10},
+	{0xc4, 0x08},
+	{0xc5, 0xf0},
+	{0xc6, 0xff},
+	{0xc7, 0x00},
+	{0xc8, 0x1a},
+	{0xc9, 0x80},
+	{0xca, 0x23},
+	{0xcb, 0x24},
+	{0xcc, 0xf4},
+	{0xce, 0x74},
+	{0x00, 0x00},
+	{0xbd, 0x01},
+	{0xb8, 0x00},
+};
+
+#endif
diff --git a/drivers/media/dvb-frontends/m88rs2000.c b/drivers/media/dvb-frontends/m88rs2000.c
index 4da5272075cb..b2351466b0da 100644
--- a/drivers/media/dvb-frontends/m88rs2000.c
+++ b/drivers/media/dvb-frontends/m88rs2000.c
@@ -110,28 +110,94 @@ static u8 m88rs2000_readreg(struct m88rs2000_state *state, u8 reg)
 	return b1[0];
 }
 
+static u32 m88rs2000_get_mclk(struct dvb_frontend *fe)
+{
+	struct m88rs2000_state *state = fe->demodulator_priv;
+	u32 mclk;
+	u8 reg;
+	/* Must not be 0x00 or 0xff */
+	reg = m88rs2000_readreg(state, 0x86);
+	if (!reg || reg == 0xff)
+		return 0;
+
+	reg /= 2;
+	reg += 1;
+
+	mclk = (u32)(reg * RS2000_FE_CRYSTAL_KHZ + 28 / 2) / 28;
+
+	return mclk;
+}
+
+static int m88rs2000_set_carrieroffset(struct dvb_frontend *fe, s16 offset)
+{
+	struct m88rs2000_state *state = fe->demodulator_priv;
+	u32 mclk;
+	s32 tmp;
+	u8 reg;
+	int ret;
+
+	mclk = m88rs2000_get_mclk(fe);
+	if (!mclk)
+		return -EINVAL;
+
+	tmp = (offset * 4096 + (s32)mclk / 2) / (s32)mclk;
+	if (tmp < 0)
+		tmp += 4096;
+
+	/* Carrier Offset */
+	ret = m88rs2000_writereg(state, 0x9c, (u8)(tmp >> 4));
+
+	reg = m88rs2000_readreg(state, 0x9d);
+	reg &= 0xf;
+	reg |= (u8)(tmp & 0xf) << 4;
+
+	ret |= m88rs2000_writereg(state, 0x9d, reg);
+
+	return ret;
+}
+
 static int m88rs2000_set_symbolrate(struct dvb_frontend *fe, u32 srate)
 {
 	struct m88rs2000_state *state = fe->demodulator_priv;
 	int ret;
-	u32 temp;
+	u64 temp;
+	u32 mclk;
 	u8 b[3];
 
 	if ((srate < 1000000) || (srate > 45000000))
 		return -EINVAL;
 
+	mclk = m88rs2000_get_mclk(fe);
+	if (!mclk)
+		return -EINVAL;
+
 	temp = srate / 1000;
-	temp *= 11831;
-	temp /= 68;
-	temp -= 3;
+	temp *= 1 << 24;
+
+	do_div(temp, mclk);
 
 	b[0] = (u8) (temp >> 16) & 0xff;
 	b[1] = (u8) (temp >> 8) & 0xff;
 	b[2] = (u8) temp & 0xff;
+
 	ret = m88rs2000_writereg(state, 0x93, b[2]);
 	ret |= m88rs2000_writereg(state, 0x94, b[1]);
 	ret |= m88rs2000_writereg(state, 0x95, b[0]);
 
+	if (srate > 10000000)
+		ret |= m88rs2000_writereg(state, 0xa0, 0x20);
+	else
+		ret |= m88rs2000_writereg(state, 0xa0, 0x60);
+
+	ret |= m88rs2000_writereg(state, 0xa1, 0xe0);
+
+	if (srate > 12000000)
+		ret |= m88rs2000_writereg(state, 0xa3, 0x20);
+	else if (srate > 2800000)
+		ret |= m88rs2000_writereg(state, 0xa3, 0x98);
+	else
+		ret |= m88rs2000_writereg(state, 0xa3, 0x90);
+
 	deb_info("m88rs2000: m88rs2000_set_symbolrate\n");
 	return ret;
 }
@@ -261,8 +327,6 @@ struct inittab m88rs2000_shutdown[] = {
 
 struct inittab fe_reset[] = {
 	{DEMOD_WRITE, 0x00, 0x01},
-	{DEMOD_WRITE, 0xf1, 0xbf},
-	{DEMOD_WRITE, 0x00, 0x01},
 	{DEMOD_WRITE, 0x20, 0x81},
 	{DEMOD_WRITE, 0x21, 0x80},
 	{DEMOD_WRITE, 0x10, 0x33},
@@ -305,9 +369,6 @@ struct inittab fe_trigger[] = {
 	{DEMOD_WRITE, 0x9b, 0x64},
 	{DEMOD_WRITE, 0x9e, 0x00},
 	{DEMOD_WRITE, 0x9f, 0xf8},
-	{DEMOD_WRITE, 0xa0, 0x20},
-	{DEMOD_WRITE, 0xa1, 0xe0},
-	{DEMOD_WRITE, 0xa3, 0x38},
 	{DEMOD_WRITE, 0x98, 0xff},
 	{DEMOD_WRITE, 0xc0, 0x0f},
 	{DEMOD_WRITE, 0x89, 0x01},
@@ -408,7 +469,7 @@ static int m88rs2000_read_status(struct dvb_frontend *fe, fe_status_t *status)
 
 	*status = 0;
 
-	if ((reg & 0x7) == 0x7) {
+	if ((reg & 0xee) == 0xee) {
 		*status = FE_HAS_CARRIER | FE_HAS_SIGNAL | FE_HAS_VITERBI
 			| FE_HAS_SYNC | FE_HAS_LOCK;
 		if (state->config->set_ts_params)
@@ -480,33 +541,38 @@ static int m88rs2000_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
 static int m88rs2000_set_fec(struct m88rs2000_state *state,
 		fe_code_rate_t fec)
 {
-	u16 fec_set;
+	u8 fec_set, reg;
+	int ret;
+
 	switch (fec) {
-	/* This is not confirmed kept for reference */
-/*	case FEC_1_2:
-		fec_set = 0x88;
+	case FEC_1_2:
+		fec_set = 0x8;
 		break;
 	case FEC_2_3:
-		fec_set = 0x68;
+		fec_set = 0x10;
 		break;
 	case FEC_3_4:
-		fec_set = 0x48;
+		fec_set = 0x20;
 		break;
 	case FEC_5_6:
-		fec_set = 0x28;
+		fec_set = 0x40;
 		break;
 	case FEC_7_8:
-		fec_set = 0x18;
-		break; */
+		fec_set = 0x80;
+		break;
 	case FEC_AUTO:
 	default:
-		fec_set = 0x08;
+		fec_set = 0x0;
 	}
-	m88rs2000_writereg(state, 0x76, fec_set);
 
-	return 0;
-}
+	reg = m88rs2000_readreg(state, 0x70);
+	reg &= 0x7;
+	ret = m88rs2000_writereg(state, 0x70, reg | fec_set);
 
+	ret |= m88rs2000_writereg(state, 0x76, 0x8);
+
+	return ret;
+}
 
 static fe_code_rate_t m88rs2000_get_fec(struct m88rs2000_state *state)
 {
@@ -515,18 +581,20 @@ static fe_code_rate_t m88rs2000_get_fec(struct m88rs2000_state *state)
 	reg = m88rs2000_readreg(state, 0x76);
 	m88rs2000_writereg(state, 0x9a, 0xb0);
 
+	reg &= 0xf0;
+	reg >>= 5;
+
 	switch (reg) {
-	case 0x88:
+	case 0x4:
 		return FEC_1_2;
-	case 0x68:
+	case 0x3:
 		return FEC_2_3;
-	case 0x48:
+	case 0x2:
 		return FEC_3_4;
-	case 0x28:
+	case 0x1:
 		return FEC_5_6;
-	case 0x18:
+	case 0x0:
 		return FEC_7_8;
-	case 0x08:
 	default:
 		break;
 	}
@@ -540,9 +608,8 @@ static int m88rs2000_set_frontend(struct dvb_frontend *fe)
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	fe_status_t status;
 	int i, ret = 0;
-	s32 tmp;
 	u32 tuner_freq;
-	u16 offset = 0;
+	s16 offset = 0;
 	u8 reg;
 
 	state->no_lock_count = 0;
@@ -567,38 +634,31 @@ static int m88rs2000_set_frontend(struct dvb_frontend *fe)
 	if (ret < 0)
 		return -ENODEV;
 
-	offset = tuner_freq - c->frequency;
-
-	/* calculate offset assuming 96000kHz*/
-	tmp = offset;
-	tmp *= 65536;
-
-	tmp = (2 * tmp + 96000) / (2 * 96000);
-	if (tmp < 0)
-		tmp += 65536;
+	offset = (s16)((s32)tuner_freq - c->frequency);
 
-	offset = tmp & 0xffff;
+	/* default mclk value 96.4285 * 2 * 1000 = 192857 */
+	if (((c->frequency % 192857) >= (192857 - 3000)) ||
+				(c->frequency % 192857) <= 3000)
+		ret = m88rs2000_writereg(state, 0x86, 0xc2);
+	else
+		ret = m88rs2000_writereg(state, 0x86, 0xc6);
 
-	ret = m88rs2000_writereg(state, 0x9a, 0x30);
-	/* Unknown usually 0xc6 sometimes 0xc1 */
-	reg = m88rs2000_readreg(state, 0x86);
-	ret |= m88rs2000_writereg(state, 0x86, reg);
-	/* Offset lower nibble always 0 */
-	ret |= m88rs2000_writereg(state, 0x9c, (offset >> 8));
-	ret |= m88rs2000_writereg(state, 0x9d, offset & 0xf0);
+	ret |= m88rs2000_set_carrieroffset(fe, offset);
+	if (ret < 0)
+		return -ENODEV;
 
+	/* Reset demod by symbol rate */
+	if (c->symbol_rate > 27500000)
+		ret = m88rs2000_writereg(state, 0xf1, 0xa4);
+	else
+		ret = m88rs2000_writereg(state, 0xf1, 0xbf);
 
-	/* Reset Demod */
-	ret = m88rs2000_tab_set(state, fe_reset);
+	ret |= m88rs2000_tab_set(state, fe_reset);
 	if (ret < 0)
 		return -ENODEV;
 
-	/* Unknown */
-	reg = m88rs2000_readreg(state, 0x70);
-	ret = m88rs2000_writereg(state, 0x70, reg);
-
 	/* Set FEC */
-	ret |= m88rs2000_set_fec(state, c->fec_inner);
+	ret = m88rs2000_set_fec(state, c->fec_inner);
 	ret |= m88rs2000_writereg(state, 0x85, 0x1);
 	ret |= m88rs2000_writereg(state, 0x8a, 0xbf);
 	ret |= m88rs2000_writereg(state, 0x8d, 0x1e);
@@ -620,7 +680,7 @@ static int m88rs2000_set_frontend(struct dvb_frontend *fe)
 
 	for (i = 0; i < 25; i++) {
 		reg = m88rs2000_readreg(state, 0x8c);
-		if ((reg & 0x7) == 0x7) {
+		if ((reg & 0xee) == 0xee) {
 			status = FE_HAS_LOCK;
 			break;
 		}
diff --git a/drivers/media/dvb-frontends/m88rs2000.h b/drivers/media/dvb-frontends/m88rs2000.h
index 14ce31e76ae6..0a50ea90736b 100644
--- a/drivers/media/dvb-frontends/m88rs2000.h
+++ b/drivers/media/dvb-frontends/m88rs2000.h
@@ -53,6 +53,8 @@ static inline struct dvb_frontend *m88rs2000_attach(
 }
 #endif /* CONFIG_DVB_M88RS2000 */
 
+#define RS2000_FE_CRYSTAL_KHZ 27000
+
 enum {
 	DEMOD_WRITE = 0x1,
 	WRITE_DELAY = 0x10,
diff --git a/drivers/media/dvb-frontends/nxt200x.c b/drivers/media/dvb-frontends/nxt200x.c
index fbca9856313a..8a8e1ecb762d 100644
--- a/drivers/media/dvb-frontends/nxt200x.c
+++ b/drivers/media/dvb-frontends/nxt200x.c
@@ -2,7 +2,7 @@
  *    Support for NXT2002 and NXT2004 - VSB/QAM
  *
  *    Copyright (C) 2005 Kirk Lapray <kirk.lapray@gmail.com>
- *    Copyright (C) 2006 Michael Krufky <mkrufky@m1k.net>
+ *    Copyright (C) 2006-2014 Michael Krufky <mkrufky@linuxtv.org>
  *    based on nxt2002 by Taylor Jacob <rtjacob@earthlink.net>
  *    and nxt2004 by Jean-Francois Thibert <jeanfrancois@sagetv.com>
  *
@@ -40,7 +40,7 @@
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 /* Max transfer size done by I2C transfer functions */
-#define MAX_XFER_SIZE  64
+#define MAX_XFER_SIZE  256
 
 #define NXT2002_DEFAULT_FIRMWARE "dvb-fe-nxt2002.fw"
 #define NXT2004_DEFAULT_FIRMWARE "dvb-fe-nxt2004.fw"
diff --git a/drivers/media/i2c/Kconfig b/drivers/media/i2c/Kconfig
index 842654d33317..4aa9c5311cc5 100644
--- a/drivers/media/i2c/Kconfig
+++ b/drivers/media/i2c/Kconfig
@@ -555,14 +555,6 @@ config VIDEO_MT9V032
 	  This is a Video4Linux2 sensor-level driver for the Micron
 	  MT9V032 752x480 CMOS sensor.
 
-config VIDEO_TCM825X
-	tristate "TCM825x camera sensor support"
-	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_INT_DEVICE
-	depends on MEDIA_CAMERA_SUPPORT
-	---help---
-	  This is a driver for the Toshiba TCM825x VGA camera sensor.
-	  It is used for example in Nokia N800.
-
 config VIDEO_SR030PC30
 	tristate "Siliconfile SR030PC30 sensor support"
 	depends on I2C && VIDEO_V4L2
@@ -594,6 +586,13 @@ config VIDEO_S5K4ECGX
           This is a V4L2 sensor-level driver for Samsung S5K4ECGX 5M
           camera sensor with an embedded SoC image signal processor.
 
+config VIDEO_S5K5BAF
+	tristate "Samsung S5K5BAF sensor support"
+	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+	---help---
+	  This is a V4L2 sensor-level driver for Samsung S5K5BAF 2M
+	  camera sensor with an embedded SoC image signal processor.
+
 source "drivers/media/i2c/smiapp/Kconfig"
 
 config VIDEO_S5C73M3
@@ -655,6 +654,18 @@ config VIDEO_UPD64083
 	  To compile this driver as a module, choose M here: the
 	  module will be called upd64083.
 
+comment "Audio/Video compression chips"
+
+config VIDEO_SAA6752HS
+	tristate "Philips SAA6752HS MPEG-2 Audio/Video Encoder"
+	depends on VIDEO_V4L2 && I2C
+	---help---
+	  Support for the Philips SAA6752HS MPEG-2 video and MPEG-audio/AC-3
+	  audio encoder with multiplexer.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called saa6752hs.
+
 comment "Miscellaneous helper chips"
 
 config VIDEO_THS7303
diff --git a/drivers/media/i2c/Makefile b/drivers/media/i2c/Makefile
index e03f1776f4f4..48888ae876fb 100644
--- a/drivers/media/i2c/Makefile
+++ b/drivers/media/i2c/Makefile
@@ -19,6 +19,7 @@ obj-$(CONFIG_VIDEO_SAA717X) += saa717x.o
 obj-$(CONFIG_VIDEO_SAA7127) += saa7127.o
 obj-$(CONFIG_VIDEO_SAA7185) += saa7185.o
 obj-$(CONFIG_VIDEO_SAA7191) += saa7191.o
+obj-$(CONFIG_VIDEO_SAA6752HS) += saa6752hs.o
 obj-$(CONFIG_VIDEO_ADV7170) += adv7170.o
 obj-$(CONFIG_VIDEO_ADV7175) += adv7175.o
 obj-$(CONFIG_VIDEO_ADV7180) += adv7180.o
@@ -57,7 +58,6 @@ obj-$(CONFIG_VIDEO_UPD64083) += upd64083.o
 obj-$(CONFIG_VIDEO_OV7640) += ov7640.o
 obj-$(CONFIG_VIDEO_OV7670) += ov7670.o
 obj-$(CONFIG_VIDEO_OV9650) += ov9650.o
-obj-$(CONFIG_VIDEO_TCM825X) += tcm825x.o
 obj-$(CONFIG_VIDEO_MT9M032) += mt9m032.o
 obj-$(CONFIG_VIDEO_MT9P031) += mt9p031.o
 obj-$(CONFIG_VIDEO_MT9T001) += mt9t001.o
@@ -67,6 +67,7 @@ obj-$(CONFIG_VIDEO_SR030PC30)	+= sr030pc30.o
 obj-$(CONFIG_VIDEO_NOON010PC30)	+= noon010pc30.o
 obj-$(CONFIG_VIDEO_S5K6AA)	+= s5k6aa.o
 obj-$(CONFIG_VIDEO_S5K4ECGX)	+= s5k4ecgx.o
+obj-$(CONFIG_VIDEO_S5K5BAF)	+= s5k5baf.o
 obj-$(CONFIG_VIDEO_S5C73M3)	+= s5c73m3/
 obj-$(CONFIG_VIDEO_ADP1653)	+= adp1653.o
 obj-$(CONFIG_VIDEO_AS3645A)	+= as3645a.o
diff --git a/drivers/media/i2c/ad9389b.c b/drivers/media/i2c/ad9389b.c
index b06a7e54ee0d..83225d6a0dd9 100644
--- a/drivers/media/i2c/ad9389b.c
+++ b/drivers/media/i2c/ad9389b.c
@@ -66,11 +66,6 @@ MODULE_LICENSE("GPL");
 **********************************************************************
 */
 
-struct i2c_reg_value {
-	u8 reg;
-	u8 value;
-};
-
 struct ad9389b_state_edid {
 	/* total number of blocks */
 	u32 blocks;
@@ -143,14 +138,14 @@ static int ad9389b_wr(struct v4l2_subdev *sd, u8 reg, u8 val)
 		if (ret == 0)
 			return 0;
 	}
-	v4l2_err(sd, "I2C Write Problem\n");
+	v4l2_err(sd, "%s: failed reg 0x%x, val 0x%x\n", __func__, reg, val);
 	return ret;
 }
 
 /* To set specific bits in the register, a clear-mask is given (to be AND-ed),
    and then the value-mask (to be OR-ed). */
 static inline void ad9389b_wr_and_or(struct v4l2_subdev *sd, u8 reg,
-						u8 clr_mask, u8 val_mask)
+				     u8 clr_mask, u8 val_mask)
 {
 	ad9389b_wr(sd, reg, (ad9389b_rd(sd, reg) & clr_mask) | val_mask);
 }
@@ -321,12 +316,12 @@ static int ad9389b_s_ctrl(struct v4l2_ctrl *ctrl)
 	struct ad9389b_state *state = get_ad9389b_state(sd);
 
 	v4l2_dbg(1, debug, sd,
-		"%s: ctrl id: %d, ctrl->val %d\n", __func__, ctrl->id, ctrl->val);
+		 "%s: ctrl id: %d, ctrl->val %d\n", __func__, ctrl->id, ctrl->val);
 
 	if (state->hdmi_mode_ctrl == ctrl) {
 		/* Set HDMI or DVI-D */
 		ad9389b_wr_and_or(sd, 0xaf, 0xfd,
-				ctrl->val == V4L2_DV_TX_MODE_HDMI ? 0x02 : 0x00);
+				  ctrl->val == V4L2_DV_TX_MODE_HDMI ? 0x02 : 0x00);
 		return 0;
 	}
 	if (state->rgb_quantization_range_ctrl == ctrl)
@@ -387,61 +382,57 @@ static int ad9389b_log_status(struct v4l2_subdev *sd)
 	v4l2_info(sd, "chip revision %d\n", state->chip_revision);
 	v4l2_info(sd, "power %s\n", state->power_on ? "on" : "off");
 	v4l2_info(sd, "%s hotplug, %s Rx Sense, %s EDID (%d block(s))\n",
-			(ad9389b_rd(sd, 0x42) & MASK_AD9389B_HPD_DETECT) ?
-							"detected" : "no",
-			(ad9389b_rd(sd, 0x42) & MASK_AD9389B_MSEN_DETECT) ?
-							"detected" : "no",
-			edid->segments ? "found" : "no", edid->blocks);
-	if (state->have_monitor) {
-		v4l2_info(sd, "%s output %s\n",
-				  (ad9389b_rd(sd, 0xaf) & 0x02) ?
-				  "HDMI" : "DVI-D",
-				  (ad9389b_rd(sd, 0xa1) & 0x3c) ?
-				  "disabled" : "enabled");
-	}
+		  (ad9389b_rd(sd, 0x42) & MASK_AD9389B_HPD_DETECT) ?
+		  "detected" : "no",
+		  (ad9389b_rd(sd, 0x42) & MASK_AD9389B_MSEN_DETECT) ?
+		  "detected" : "no",
+		  edid->segments ? "found" : "no", edid->blocks);
+	v4l2_info(sd, "%s output %s\n",
+		  (ad9389b_rd(sd, 0xaf) & 0x02) ?
+		  "HDMI" : "DVI-D",
+		  (ad9389b_rd(sd, 0xa1) & 0x3c) ?
+		  "disabled" : "enabled");
 	v4l2_info(sd, "ad9389b: %s\n", (ad9389b_rd(sd, 0xb8) & 0x40) ?
-					"encrypted" : "no encryption");
+		  "encrypted" : "no encryption");
 	v4l2_info(sd, "state: %s, error: %s, detect count: %u, msk/irq: %02x/%02x\n",
-			states[ad9389b_rd(sd, 0xc8) & 0xf],
-			errors[ad9389b_rd(sd, 0xc8) >> 4],
-			state->edid_detect_counter,
-			ad9389b_rd(sd, 0x94), ad9389b_rd(sd, 0x96));
+		  states[ad9389b_rd(sd, 0xc8) & 0xf],
+		  errors[ad9389b_rd(sd, 0xc8) >> 4],
+		  state->edid_detect_counter,
+		  ad9389b_rd(sd, 0x94), ad9389b_rd(sd, 0x96));
 	manual_gear = ad9389b_rd(sd, 0x98) & 0x80;
 	v4l2_info(sd, "ad9389b: RGB quantization: %s range\n",
-			ad9389b_rd(sd, 0x3b) & 0x01 ? "limited" : "full");
+		  ad9389b_rd(sd, 0x3b) & 0x01 ? "limited" : "full");
 	v4l2_info(sd, "ad9389b: %s gear %d\n",
 		  manual_gear ? "manual" : "automatic",
 		  manual_gear ? ((ad9389b_rd(sd, 0x98) & 0x70) >> 4) :
-				((ad9389b_rd(sd, 0x9e) & 0x0e) >> 1));
-	if (state->have_monitor) {
-		if (ad9389b_rd(sd, 0xaf) & 0x02) {
-			/* HDMI only */
-			u8 manual_cts = ad9389b_rd(sd, 0x0a) & 0x80;
-			u32 N = (ad9389b_rd(sd, 0x01) & 0xf) << 16 |
-				 ad9389b_rd(sd, 0x02) << 8 |
-				 ad9389b_rd(sd, 0x03);
-			u8 vic_detect = ad9389b_rd(sd, 0x3e) >> 2;
-			u8 vic_sent = ad9389b_rd(sd, 0x3d) & 0x3f;
-			u32 CTS;
-
-			if (manual_cts)
-				CTS = (ad9389b_rd(sd, 0x07) & 0xf) << 16 |
-				       ad9389b_rd(sd, 0x08) << 8 |
-				       ad9389b_rd(sd, 0x09);
-			else
-				CTS = (ad9389b_rd(sd, 0x04) & 0xf) << 16 |
-				       ad9389b_rd(sd, 0x05) << 8 |
-				       ad9389b_rd(sd, 0x06);
-			N = (ad9389b_rd(sd, 0x01) & 0xf) << 16 |
-			     ad9389b_rd(sd, 0x02) << 8 |
-			     ad9389b_rd(sd, 0x03);
-
-			v4l2_info(sd, "ad9389b: CTS %s mode: N %d, CTS %d\n",
-				manual_cts ? "manual" : "automatic", N, CTS);
-
-			v4l2_info(sd, "ad9389b: VIC: detected %d, sent %d\n",
-				vic_detect, vic_sent);
-		}
+		  ((ad9389b_rd(sd, 0x9e) & 0x0e) >> 1));
+	if (ad9389b_rd(sd, 0xaf) & 0x02) {
+		/* HDMI only */
+		u8 manual_cts = ad9389b_rd(sd, 0x0a) & 0x80;
+		u32 N = (ad9389b_rd(sd, 0x01) & 0xf) << 16 |
+			ad9389b_rd(sd, 0x02) << 8 |
+			ad9389b_rd(sd, 0x03);
+		u8 vic_detect = ad9389b_rd(sd, 0x3e) >> 2;
+		u8 vic_sent = ad9389b_rd(sd, 0x3d) & 0x3f;
+		u32 CTS;
+
+		if (manual_cts)
+			CTS = (ad9389b_rd(sd, 0x07) & 0xf) << 16 |
+			      ad9389b_rd(sd, 0x08) << 8 |
+			      ad9389b_rd(sd, 0x09);
+		else
+			CTS = (ad9389b_rd(sd, 0x04) & 0xf) << 16 |
+			      ad9389b_rd(sd, 0x05) << 8 |
+			      ad9389b_rd(sd, 0x06);
+		N = (ad9389b_rd(sd, 0x01) & 0xf) << 16 |
+		    ad9389b_rd(sd, 0x02) << 8 |
+		    ad9389b_rd(sd, 0x03);
+
+		v4l2_info(sd, "ad9389b: CTS %s mode: N %d, CTS %d\n",
+			  manual_cts ? "manual" : "automatic", N, CTS);
+
+		v4l2_info(sd, "ad9389b: VIC: detected %d, sent %d\n",
+			  vic_detect, vic_sent);
 	}
 	if (state->dv_timings.type == V4L2_DV_BT_656_1120)
 		v4l2_print_dv_timings(sd->name, "timings: ",
@@ -486,7 +477,7 @@ static int ad9389b_s_power(struct v4l2_subdev *sd, int on)
 	}
 	if (i > 1)
 		v4l2_dbg(1, debug, sd,
-			"needed %d retries to powerup the ad9389b\n", i);
+			 "needed %d retries to powerup the ad9389b\n", i);
 
 	/* Select chip: AD9389B */
 	ad9389b_wr_and_or(sd, 0xba, 0xef, 0x10);
@@ -556,14 +547,16 @@ static int ad9389b_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
 	irq_status = ad9389b_rd(sd, 0x96);
 	/* clear detected interrupts */
 	ad9389b_wr(sd, 0x96, irq_status);
+	/* enable interrupts */
+	ad9389b_set_isr(sd, true);
+
+	v4l2_dbg(1, debug, sd, "%s: irq_status 0x%x\n", __func__, irq_status);
 
-	if (irq_status & (MASK_AD9389B_HPD_INT | MASK_AD9389B_MSEN_INT))
+	if (irq_status & (MASK_AD9389B_HPD_INT))
 		ad9389b_check_monitor_present_status(sd);
 	if (irq_status & MASK_AD9389B_EDID_RDY_INT)
 		ad9389b_check_edid_status(sd);
 
-	/* enable interrupts */
-	ad9389b_set_isr(sd, true);
 	*handled = true;
 	return 0;
 }
@@ -599,7 +592,7 @@ static int ad9389b_get_edid(struct v4l2_subdev *sd, struct v4l2_subdev_edid *edi
 	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);
+	       128 * edid->blocks);
 	return 0;
 }
 
@@ -612,8 +605,6 @@ static const struct v4l2_subdev_pad_ops ad9389b_pad_ops = {
 /* Enable/disable ad9389b output */
 static int ad9389b_s_stream(struct v4l2_subdev *sd, int enable)
 {
-	struct ad9389b_state *state = get_ad9389b_state(sd);
-
 	v4l2_dbg(1, debug, sd, "%s: %sable\n", __func__, (enable ? "en" : "dis"));
 
 	ad9389b_wr_and_or(sd, 0xa1, ~0x3c, (enable ? 0 : 0x3c));
@@ -621,7 +612,6 @@ static int ad9389b_s_stream(struct v4l2_subdev *sd, int enable)
 		ad9389b_check_monitor_present_status(sd);
 	} else {
 		ad9389b_s_power(sd, 0);
-		state->have_monitor = false;
 	}
 	return 0;
 }
@@ -686,14 +676,14 @@ 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)
+				   struct v4l2_enum_dv_timings *timings)
 {
 	return v4l2_enum_dv_timings_cap(timings, &ad9389b_timings_cap,
 			NULL, NULL);
 }
 
 static int ad9389b_dv_timings_cap(struct v4l2_subdev *sd,
-			struct v4l2_dv_timings_cap *cap)
+				  struct v4l2_dv_timings_cap *cap)
 {
 	*cap = ad9389b_timings_cap;
 	return 0;
@@ -724,15 +714,15 @@ static int ad9389b_s_clock_freq(struct v4l2_subdev *sd, u32 freq)
 	u32 N;
 
 	switch (freq) {
-	case 32000: N = 4096; break;
-	case 44100: N = 6272; break;
-	case 48000: N = 6144; break;
-	case 88200: N = 12544; break;
-	case 96000: N = 12288; break;
+	case 32000:  N = 4096;  break;
+	case 44100:  N = 6272;  break;
+	case 48000:  N = 6144;  break;
+	case 88200:  N = 12544; break;
+	case 96000:  N = 12288; break;
 	case 176400: N = 25088; break;
 	case 192000: N = 24576; break;
 	default:
-		return -EINVAL;
+	     return -EINVAL;
 	}
 
 	/* Set N (used with CTS to regenerate the audio clock) */
@@ -748,15 +738,15 @@ static int ad9389b_s_i2s_clock_freq(struct v4l2_subdev *sd, u32 freq)
 	u32 i2s_sf;
 
 	switch (freq) {
-	case 32000: i2s_sf = 0x30; break;
-	case 44100: i2s_sf = 0x00; break;
-	case 48000: i2s_sf = 0x20; break;
-	case 88200: i2s_sf = 0x80; break;
-	case 96000: i2s_sf = 0xa0; break;
+	case 32000:  i2s_sf = 0x30; break;
+	case 44100:  i2s_sf = 0x00; break;
+	case 48000:  i2s_sf = 0x20; break;
+	case 88200:  i2s_sf = 0x80; break;
+	case 96000:  i2s_sf = 0xa0; break;
 	case 176400: i2s_sf = 0xc0; break;
 	case 192000: i2s_sf = 0xe0; break;
 	default:
-		return -EINVAL;
+	     return -EINVAL;
 	}
 
 	/* Set sampling frequency for I2S audio to 48 kHz */
@@ -800,7 +790,7 @@ static const struct v4l2_subdev_ops ad9389b_ops = {
 
 /* ----------------------------------------------------------------------- */
 static void ad9389b_dbg_dump_edid(int lvl, int debug, struct v4l2_subdev *sd,
-							int segment, u8 *buf)
+				  int segment, u8 *buf)
 {
 	int i, j;
 
@@ -826,8 +816,8 @@ static void ad9389b_dbg_dump_edid(int lvl, int debug, struct v4l2_subdev *sd,
 static void ad9389b_edid_handler(struct work_struct *work)
 {
 	struct delayed_work *dwork = to_delayed_work(work);
-	struct ad9389b_state *state = container_of(dwork,
-			struct ad9389b_state, edid_handler);
+	struct ad9389b_state *state =
+		container_of(dwork, struct ad9389b_state, edid_handler);
 	struct v4l2_subdev *sd = &state->sd;
 	struct ad9389b_edid_detect ed;
 
@@ -845,11 +835,10 @@ static void ad9389b_edid_handler(struct work_struct *work)
 		if (state->edid.read_retries) {
 			state->edid.read_retries--;
 			v4l2_dbg(1, debug, sd, "%s: edid read failed\n", __func__);
-			state->have_monitor = false;
 			ad9389b_s_power(sd, false);
 			ad9389b_s_power(sd, true);
 			queue_delayed_work(state->work_queue,
-					&state->edid_handler, EDID_DELAY);
+					   &state->edid_handler, EDID_DELAY);
 			return;
 		}
 	}
@@ -915,49 +904,35 @@ static void ad9389b_notify_monitor_detect(struct v4l2_subdev *sd)
 	v4l2_subdev_notify(sd, AD9389B_MONITOR_DETECT, (void *)&mdt);
 }
 
-static void ad9389b_check_monitor_present_status(struct v4l2_subdev *sd)
+static void ad9389b_update_monitor_present_status(struct v4l2_subdev *sd)
 {
 	struct ad9389b_state *state = get_ad9389b_state(sd);
 	/* read hotplug and rx-sense state */
 	u8 status = ad9389b_rd(sd, 0x42);
 
 	v4l2_dbg(1, debug, sd, "%s: status: 0x%x%s%s\n",
-			 __func__,
-			 status,
-			 status & MASK_AD9389B_HPD_DETECT ? ", hotplug" : "",
-			 status & MASK_AD9389B_MSEN_DETECT ? ", rx-sense" : "");
+		 __func__,
+		 status,
+		 status & MASK_AD9389B_HPD_DETECT ? ", hotplug" : "",
+		 status & MASK_AD9389B_MSEN_DETECT ? ", rx-sense" : "");
 
-	if ((status & MASK_AD9389B_HPD_DETECT) &&
-	    ((status & MASK_AD9389B_MSEN_DETECT) || state->edid.segments)) {
-		v4l2_dbg(1, debug, sd,
-				"%s: hotplug and (rx-sense or edid)\n", __func__);
-		if (!state->have_monitor) {
-			v4l2_dbg(1, debug, sd, "%s: monitor detected\n", __func__);
-			state->have_monitor = true;
-			ad9389b_set_isr(sd, true);
-			if (!ad9389b_s_power(sd, true)) {
-				v4l2_dbg(1, debug, sd,
-					"%s: monitor detected, powerup failed\n", __func__);
-				return;
-			}
-			ad9389b_setup(sd);
-			ad9389b_notify_monitor_detect(sd);
-			state->edid.read_retries = EDID_MAX_RETRIES;
-			queue_delayed_work(state->work_queue,
-					&state->edid_handler, EDID_DELAY);
-		}
-	} else if (status & MASK_AD9389B_HPD_DETECT) {
+	if (status & MASK_AD9389B_HPD_DETECT) {
 		v4l2_dbg(1, debug, sd, "%s: hotplug detected\n", __func__);
+		state->have_monitor = true;
+		if (!ad9389b_s_power(sd, true)) {
+			v4l2_dbg(1, debug, sd,
+				 "%s: monitor detected, powerup failed\n", __func__);
+			return;
+		}
+		ad9389b_setup(sd);
+		ad9389b_notify_monitor_detect(sd);
 		state->edid.read_retries = EDID_MAX_RETRIES;
 		queue_delayed_work(state->work_queue,
-				&state->edid_handler, EDID_DELAY);
+				   &state->edid_handler, EDID_DELAY);
 	} else if (!(status & MASK_AD9389B_HPD_DETECT)) {
 		v4l2_dbg(1, debug, sd, "%s: hotplug not detected\n", __func__);
-		if (state->have_monitor) {
-			v4l2_dbg(1, debug, sd, "%s: monitor not detected\n", __func__);
-			state->have_monitor = false;
-			ad9389b_notify_monitor_detect(sd);
-		}
+		state->have_monitor = false;
+		ad9389b_notify_monitor_detect(sd);
 		ad9389b_s_power(sd, false);
 		memset(&state->edid, 0, sizeof(struct ad9389b_state_edid));
 	}
@@ -966,6 +941,35 @@ static void ad9389b_check_monitor_present_status(struct v4l2_subdev *sd)
 	v4l2_ctrl_s_ctrl(state->hotplug_ctrl, ad9389b_have_hotplug(sd) ? 0x1 : 0x0);
 	v4l2_ctrl_s_ctrl(state->rx_sense_ctrl, ad9389b_have_rx_sense(sd) ? 0x1 : 0x0);
 	v4l2_ctrl_s_ctrl(state->have_edid0_ctrl, state->edid.segments ? 0x1 : 0x0);
+
+	/* update with setting from ctrls */
+	ad9389b_s_ctrl(state->rgb_quantization_range_ctrl);
+	ad9389b_s_ctrl(state->hdmi_mode_ctrl);
+}
+
+static void ad9389b_check_monitor_present_status(struct v4l2_subdev *sd)
+{
+	struct ad9389b_state *state = get_ad9389b_state(sd);
+	int retry = 0;
+
+	ad9389b_update_monitor_present_status(sd);
+
+	/*
+	 * Rapid toggling of the hotplug may leave the chip powered off,
+	 * even if we think it is on. In that case reset and power up again.
+	 */
+	while (state->power_on && (ad9389b_rd(sd, 0x41) & 0x40)) {
+		if (++retry > 5) {
+			v4l2_err(sd, "retried %d times, give up\n", retry);
+			return;
+		}
+		v4l2_dbg(1, debug, sd, "%s: reset and re-check status (%d)\n", __func__, retry);
+		ad9389b_notify_monitor_detect(sd);
+		cancel_delayed_work_sync(&state->edid_handler);
+		memset(&state->edid, 0, sizeof(struct ad9389b_state_edid));
+		ad9389b_s_power(sd, false);
+		ad9389b_update_monitor_present_status(sd);
+	}
 }
 
 static bool edid_block_verify_crc(u8 *edid_block)
@@ -978,7 +982,7 @@ static bool edid_block_verify_crc(u8 *edid_block)
 	return sum == 0;
 }
 
-static bool edid_segment_verify_crc(struct v4l2_subdev *sd, u32 segment)
+static bool edid_verify_crc(struct v4l2_subdev *sd, u32 segment)
 {
 	struct ad9389b_state *state = get_ad9389b_state(sd);
 	u32 blocks = state->edid.blocks;
@@ -992,6 +996,25 @@ static bool edid_segment_verify_crc(struct v4l2_subdev *sd, u32 segment)
 	return false;
 }
 
+static bool edid_verify_header(struct v4l2_subdev *sd, u32 segment)
+{
+	static const u8 hdmi_header[] = {
+		0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00
+	};
+	struct ad9389b_state *state = get_ad9389b_state(sd);
+	u8 *data = state->edid.data;
+	int i;
+
+	if (segment)
+		return true;
+
+	for (i = 0; i < ARRAY_SIZE(hdmi_header); i++)
+		if (data[i] != hdmi_header[i])
+			return false;
+
+	return true;
+}
+
 static bool ad9389b_check_edid_status(struct v4l2_subdev *sd)
 {
 	struct ad9389b_state *state = get_ad9389b_state(sd);
@@ -1000,7 +1023,7 @@ static bool ad9389b_check_edid_status(struct v4l2_subdev *sd)
 	u8 edidRdy = ad9389b_rd(sd, 0xc5);
 
 	v4l2_dbg(1, debug, sd, "%s: edid ready (retries: %d)\n",
-			 __func__, EDID_MAX_RETRIES - state->edid.read_retries);
+		 __func__, EDID_MAX_RETRIES - state->edid.read_retries);
 
 	if (!(edidRdy & MASK_AD9389B_EDID_RDY))
 		return false;
@@ -1013,16 +1036,16 @@ static bool ad9389b_check_edid_status(struct v4l2_subdev *sd)
 	v4l2_dbg(1, debug, sd, "%s: got segment %d\n", __func__, segment);
 	ad9389b_edid_rd(sd, 256, &state->edid.data[segment * 256]);
 	ad9389b_dbg_dump_edid(2, debug, sd, segment,
-			&state->edid.data[segment * 256]);
+			      &state->edid.data[segment * 256]);
 	if (segment == 0) {
 		state->edid.blocks = state->edid.data[0x7e] + 1;
 		v4l2_dbg(1, debug, sd, "%s: %d blocks in total\n",
-				__func__, state->edid.blocks);
+			 __func__, state->edid.blocks);
 	}
-	if (!edid_segment_verify_crc(sd, segment)) {
+	if (!edid_verify_crc(sd, segment) ||
+	    !edid_verify_header(sd, segment)) {
 		/* edid crc error, force reread of edid segment */
-		v4l2_err(sd, "%s: edid crc error\n", __func__);
-		state->have_monitor = false;
+		v4l2_err(sd, "%s: edid crc or header error\n", __func__);
 		ad9389b_s_power(sd, false);
 		ad9389b_s_power(sd, true);
 		return false;
@@ -1032,12 +1055,12 @@ static bool ad9389b_check_edid_status(struct v4l2_subdev *sd)
 	if (((state->edid.data[0x7e] >> 1) + 1) > state->edid.segments) {
 		/* Request next EDID segment */
 		v4l2_dbg(1, debug, sd, "%s: request segment %d\n",
-				__func__, state->edid.segments);
+			 __func__, state->edid.segments);
 		ad9389b_wr(sd, 0xc9, 0xf);
 		ad9389b_wr(sd, 0xc4, state->edid.segments);
 		state->edid.read_retries = EDID_MAX_RETRIES;
 		queue_delayed_work(state->work_queue,
-				&state->edid_handler, EDID_DELAY);
+				   &state->edid_handler, EDID_DELAY);
 		return false;
 	}
 
@@ -1081,7 +1104,7 @@ static int ad9389b_probe(struct i2c_client *client, const struct i2c_device_id *
 		return -EIO;
 
 	v4l_dbg(1, debug, client, "detecting ad9389b client on address 0x%x\n",
-			client->addr << 1);
+		client->addr << 1);
 
 	state = devm_kzalloc(&client->dev, sizeof(*state), GFP_KERNEL);
 	if (!state)
@@ -1140,7 +1163,7 @@ static int ad9389b_probe(struct i2c_client *client, const struct i2c_device_id *
 		goto err_entity;
 	}
 	v4l2_dbg(1, debug, sd, "reg 0x41 0x%x, chip version (reg 0x00) 0x%x\n",
-			ad9389b_rd(sd, 0x41), state->chip_revision);
+		 ad9389b_rd(sd, 0x41), state->chip_revision);
 
 	state->edid_i2c_client = i2c_new_dummy(client->adapter, (0x7e>>1));
 	if (state->edid_i2c_client == NULL) {
@@ -1163,7 +1186,7 @@ static int ad9389b_probe(struct i2c_client *client, const struct i2c_device_id *
 	ad9389b_set_isr(sd, true);
 
 	v4l2_info(sd, "%s found @ 0x%x (%s)\n", client->name,
-			  client->addr << 1, client->adapter->name);
+		  client->addr << 1, client->adapter->name);
 	return 0;
 
 err_unreg:
diff --git a/drivers/media/i2c/adv7511.c b/drivers/media/i2c/adv7511.c
index 7c8d971f1f61..ee618942cb8e 100644
--- a/drivers/media/i2c/adv7511.c
+++ b/drivers/media/i2c/adv7511.c
@@ -452,6 +452,29 @@ static int adv7511_log_status(struct v4l2_subdev *sd)
 			  errors[adv7511_rd(sd, 0xc8) >> 4], state->edid_detect_counter,
 			  adv7511_rd(sd, 0x94), adv7511_rd(sd, 0x96));
 	v4l2_info(sd, "RGB quantization: %s range\n", adv7511_rd(sd, 0x18) & 0x80 ? "limited" : "full");
+	if (adv7511_rd(sd, 0xaf) & 0x02) {
+		/* HDMI only */
+		u8 manual_cts = adv7511_rd(sd, 0x0a) & 0x80;
+		u32 N = (adv7511_rd(sd, 0x01) & 0xf) << 16 |
+			adv7511_rd(sd, 0x02) << 8 |
+			adv7511_rd(sd, 0x03);
+		u8 vic_detect = adv7511_rd(sd, 0x3e) >> 2;
+		u8 vic_sent = adv7511_rd(sd, 0x3d) & 0x3f;
+		u32 CTS;
+
+		if (manual_cts)
+			CTS = (adv7511_rd(sd, 0x07) & 0xf) << 16 |
+			      adv7511_rd(sd, 0x08) << 8 |
+			      adv7511_rd(sd, 0x09);
+		else
+			CTS = (adv7511_rd(sd, 0x04) & 0xf) << 16 |
+			      adv7511_rd(sd, 0x05) << 8 |
+			      adv7511_rd(sd, 0x06);
+		v4l2_info(sd, "CTS %s mode: N %d, CTS %d\n",
+			  manual_cts ? "manual" : "automatic", N, CTS);
+		v4l2_info(sd, "VIC: detected %d, sent %d\n",
+			  vic_detect, vic_sent);
+	}
 	if (state->dv_timings.type == V4L2_DV_BT_656_1120)
 		v4l2_print_dv_timings(sd->name, "timings: ",
 				&state->dv_timings, false);
@@ -942,26 +965,38 @@ static void adv7511_check_monitor_present_status(struct v4l2_subdev *sd)
 
 static bool edid_block_verify_crc(uint8_t *edid_block)
 {
-	int i;
 	uint8_t sum = 0;
+	int i;
 
 	for (i = 0; i < 128; i++)
-		sum += *(edid_block + i);
-	return (sum == 0);
+		sum += edid_block[i];
+	return sum == 0;
 }
 
-static bool edid_segment_verify_crc(struct v4l2_subdev *sd, u32 segment)
+static bool edid_verify_crc(struct v4l2_subdev *sd, u32 segment)
 {
 	struct adv7511_state *state = get_adv7511_state(sd);
 	u32 blocks = state->edid.blocks;
 	uint8_t *data = state->edid.data;
 
-	if (edid_block_verify_crc(&data[segment * 256])) {
-		if ((segment + 1) * 2 <= blocks)
-			return edid_block_verify_crc(&data[segment * 256 + 128]);
+	if (!edid_block_verify_crc(&data[segment * 256]))
+		return false;
+	if ((segment + 1) * 2 <= blocks)
+		return edid_block_verify_crc(&data[segment * 256 + 128]);
+	return true;
+}
+
+static bool edid_verify_header(struct v4l2_subdev *sd, u32 segment)
+{
+	static const u8 hdmi_header[] = {
+		0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00
+	};
+	struct adv7511_state *state = get_adv7511_state(sd);
+	u8 *data = state->edid.data;
+
+	if (segment != 0)
 		return true;
-	}
-	return false;
+	return !memcmp(data, hdmi_header, sizeof(hdmi_header));
 }
 
 static bool adv7511_check_edid_status(struct v4l2_subdev *sd)
@@ -990,9 +1025,10 @@ static bool adv7511_check_edid_status(struct v4l2_subdev *sd)
 			state->edid.blocks = state->edid.data[0x7e] + 1;
 			v4l2_dbg(1, debug, sd, "%s: %d blocks in total\n", __func__, state->edid.blocks);
 		}
-		if (!edid_segment_verify_crc(sd, segment)) {
+		if (!edid_verify_crc(sd, segment) ||
+		    !edid_verify_header(sd, segment)) {
 			/* edid crc error, force reread of edid segment */
-			v4l2_dbg(1, debug, sd, "%s: edid crc error\n", __func__);
+			v4l2_err(sd, "%s: edid crc or header error\n", __func__);
 			state->have_monitor = false;
 			adv7511_s_power(sd, false);
 			adv7511_s_power(sd, true);
@@ -1038,6 +1074,12 @@ static void adv7511_init_setup(struct v4l2_subdev *sd)
 
 	/* clear all interrupts */
 	adv7511_wr(sd, 0x96, 0xff);
+	/*
+	 * Stop HPD from resetting a lot of registers.
+	 * It might leave the chip in a partly un-initialized state,
+	 * in particular with regards to hotplug bounces.
+	 */
+	adv7511_wr_and_or(sd, 0xd6, 0x3f, 0xc0);
 	memset(edid, 0, sizeof(struct adv7511_state_edid));
 	state->have_monitor = false;
 	adv7511_set_isr(sd, false);
diff --git a/drivers/media/i2c/adv7604.c b/drivers/media/i2c/adv7604.c
index a324106b9f11..71c8570bd9ea 100644
--- a/drivers/media/i2c/adv7604.c
+++ b/drivers/media/i2c/adv7604.c
@@ -53,8 +53,6 @@ MODULE_LICENSE("GPL");
 /* ADV7604 system clock frequency */
 #define ADV7604_fsc (28636360)
 
-#define DIGITAL_INPUT (state->mode == ADV7604_MODE_HDMI)
-
 /*
  **********************************************************************
  *
@@ -67,17 +65,19 @@ struct adv7604_state {
 	struct v4l2_subdev sd;
 	struct media_pad pad;
 	struct v4l2_ctrl_handler hdl;
-	enum adv7604_mode mode;
+	enum adv7604_input_port selected_input;
 	struct v4l2_dv_timings timings;
-	u8 edid[256];
-	unsigned edid_blocks;
+	struct {
+		u8 edid[256];
+		u32 present;
+		unsigned blocks;
+	} edid;
+	u16 spa_port_a[2];
 	struct v4l2_fract aspect_ratio;
 	u32 rgb_quantization_range;
 	struct workqueue_struct *work_queues;
 	struct delayed_work delayed_work_enable_hotplug;
-	bool connector_hdmi;
 	bool restart_stdi_once;
-	u32 prev_input_status;
 
 	/* i2c clients */
 	struct i2c_client *i2c_avlink;
@@ -160,6 +160,7 @@ static const struct v4l2_dv_timings adv7604_timings[] = {
 	V4L2_DV_BT_DMT_1792X1344P60,
 	V4L2_DV_BT_DMT_1856X1392P60,
 	V4L2_DV_BT_DMT_1920X1200P60_RB,
+	V4L2_DV_BT_DMT_1366X768P60_RB,
 	V4L2_DV_BT_DMT_1366X768P60,
 	V4L2_DV_BT_DMT_1920X1080P60,
 	{ },
@@ -507,57 +508,31 @@ static inline int edid_read_block(struct v4l2_subdev *sd, unsigned len, u8 *val)
 	return 0;
 }
 
-static void adv7604_delayed_work_enable_hotplug(struct work_struct *work)
-{
-	struct delayed_work *dwork = to_delayed_work(work);
-	struct adv7604_state *state = container_of(dwork, struct adv7604_state,
-						delayed_work_enable_hotplug);
-	struct v4l2_subdev *sd = &state->sd;
-
-	v4l2_dbg(2, debug, sd, "%s: enable hotplug\n", __func__);
-
-	v4l2_subdev_notify(sd, ADV7604_HOTPLUG, (void *)1);
-}
-
 static inline int edid_write_block(struct v4l2_subdev *sd,
 					unsigned len, const u8 *val)
 {
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct adv7604_state *state = to_state(sd);
 	int err = 0;
 	int i;
 
 	v4l2_dbg(2, debug, sd, "%s: write EDID block (%d byte)\n", __func__, len);
 
-	v4l2_subdev_notify(sd, ADV7604_HOTPLUG, (void *)0);
-
-	/* Disables I2C access to internal EDID ram from DDC port */
-	rep_write_and_or(sd, 0x77, 0xf0, 0x0);
-
 	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);
-	if (err)
-		return err;
+	return err;
+}
 
-	/* adv7604 calculates the checksums and enables I2C access to internal
-	   EDID ram from DDC port. */
-	rep_write_and_or(sd, 0x77, 0xf0, 0x1);
+static void adv7604_delayed_work_enable_hotplug(struct work_struct *work)
+{
+	struct delayed_work *dwork = to_delayed_work(work);
+	struct adv7604_state *state = container_of(dwork, struct adv7604_state,
+						delayed_work_enable_hotplug);
+	struct v4l2_subdev *sd = &state->sd;
 
-	for (i = 0; i < 1000; i++) {
-		if (rep_read(sd, 0x7d) & 1)
-			break;
-		mdelay(1);
-	}
-	if (i == 1000) {
-		v4l_err(client, "error enabling edid\n");
-		return -EIO;
-	}
+	v4l2_dbg(2, debug, sd, "%s: enable hotplug\n", __func__);
 
-	/* enable hotplug after 100 ms */
-	queue_delayed_work(state->work_queues,
-			&state->delayed_work_enable_hotplug, HZ / 10);
-	return 0;
+	v4l2_subdev_notify(sd, ADV7604_HOTPLUG, (void *)&state->edid.present);
 }
 
 static inline int hdmi_read(struct v4l2_subdev *sd, u8 reg)
@@ -574,6 +549,11 @@ static inline int hdmi_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 	return adv_smbus_write_byte_data(state->i2c_hdmi, reg, val);
 }
 
+static inline int hdmi_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 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);
@@ -623,6 +603,26 @@ static inline int vdp_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 
 /* ----------------------------------------------------------------------- */
 
+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;
+}
+
+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;
+}
+
+/* ----------------------------------------------------------------------- */
+
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 static void adv7604_inv_register(struct v4l2_subdev *sd)
 {
@@ -696,45 +696,47 @@ static int adv7604_g_register(struct v4l2_subdev *sd,
 static int adv7604_s_register(struct v4l2_subdev *sd,
 					const struct v4l2_dbg_register *reg)
 {
+	u8 val = reg->val & 0xff;
+
 	switch (reg->reg >> 8) {
 	case 0:
-		io_write(sd, reg->reg & 0xff, reg->val & 0xff);
+		io_write(sd, reg->reg & 0xff, val);
 		break;
 	case 1:
-		avlink_write(sd, reg->reg & 0xff, reg->val & 0xff);
+		avlink_write(sd, reg->reg & 0xff, val);
 		break;
 	case 2:
-		cec_write(sd, reg->reg & 0xff, reg->val & 0xff);
+		cec_write(sd, reg->reg & 0xff, val);
 		break;
 	case 3:
-		infoframe_write(sd, reg->reg & 0xff, reg->val & 0xff);
+		infoframe_write(sd, reg->reg & 0xff, val);
 		break;
 	case 4:
-		esdp_write(sd, reg->reg & 0xff, reg->val & 0xff);
+		esdp_write(sd, reg->reg & 0xff, val);
 		break;
 	case 5:
-		dpp_write(sd, reg->reg & 0xff, reg->val & 0xff);
+		dpp_write(sd, reg->reg & 0xff, val);
 		break;
 	case 6:
-		afe_write(sd, reg->reg & 0xff, reg->val & 0xff);
+		afe_write(sd, reg->reg & 0xff, val);
 		break;
 	case 7:
-		rep_write(sd, reg->reg & 0xff, reg->val & 0xff);
+		rep_write(sd, reg->reg & 0xff, val);
 		break;
 	case 8:
-		edid_write(sd, reg->reg & 0xff, reg->val & 0xff);
+		edid_write(sd, reg->reg & 0xff, val);
 		break;
 	case 9:
-		hdmi_write(sd, reg->reg & 0xff, reg->val & 0xff);
+		hdmi_write(sd, reg->reg & 0xff, val);
 		break;
 	case 0xa:
-		test_write(sd, reg->reg & 0xff, reg->val & 0xff);
+		test_write(sd, reg->reg & 0xff, val);
 		break;
 	case 0xb:
-		cp_write(sd, reg->reg & 0xff, reg->val & 0xff);
+		cp_write(sd, reg->reg & 0xff, val);
 		break;
 	case 0xc:
-		vdp_write(sd, reg->reg & 0xff, reg->val & 0xff);
+		vdp_write(sd, reg->reg & 0xff, val);
 		break;
 	default:
 		v4l2_info(sd, "Register %03llx not supported\n", reg->reg);
@@ -748,10 +750,13 @@ static int adv7604_s_register(struct v4l2_subdev *sd,
 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);
 
-	/* port A only */
 	return v4l2_ctrl_s_ctrl(state->detect_tx_5v_ctrl,
-				((io_read(sd, 0x6f) & 0x10) >> 4));
+			((reg_io_6f & 0x10) >> 4) |
+			((reg_io_6f & 0x08) >> 2) |
+			(reg_io_6f & 0x04) |
+			((reg_io_6f & 0x02) << 2));
 }
 
 static int find_and_set_predefined_video_timings(struct v4l2_subdev *sd,
@@ -759,12 +764,11 @@ static int find_and_set_predefined_video_timings(struct v4l2_subdev *sd,
 		const struct adv7604_video_standards *predef_vid_timings,
 		const struct v4l2_dv_timings *timings)
 {
-	struct adv7604_state *state = to_state(sd);
 	int i;
 
 	for (i = 0; predef_vid_timings[i].timings.bt.width; i++) {
 		if (!v4l2_match_dv_timings(timings, &predef_vid_timings[i].timings,
-					DIGITAL_INPUT ? 250000 : 1000000))
+					is_digital_input(sd) ? 250000 : 1000000))
 			continue;
 		io_write(sd, 0x00, predef_vid_timings[i].vid_std); /* video std */
 		io_write(sd, 0x01, (predef_vid_timings[i].v_freq << 4) +
@@ -799,27 +803,22 @@ static int configure_predefined_video_timings(struct v4l2_subdev *sd,
 	cp_write(sd, 0xab, 0x00);
 	cp_write(sd, 0xac, 0x00);
 
-	switch (state->mode) {
-	case ADV7604_MODE_COMP:
-	case ADV7604_MODE_GR:
+	if (is_analog_input(sd)) {
 		err = find_and_set_predefined_video_timings(sd,
 				0x01, adv7604_prim_mode_comp, timings);
 		if (err)
 			err = find_and_set_predefined_video_timings(sd,
 					0x02, adv7604_prim_mode_gr, timings);
-		break;
-	case ADV7604_MODE_HDMI:
+	} else if (is_digital_input(sd)) {
 		err = find_and_set_predefined_video_timings(sd,
 				0x05, adv7604_prim_mode_hdmi_comp, timings);
 		if (err)
 			err = find_and_set_predefined_video_timings(sd,
 					0x06, adv7604_prim_mode_hdmi_gr, timings);
-		break;
-	default:
-		v4l2_dbg(2, debug, sd, "%s: Unknown mode %d\n",
-				__func__, state->mode);
+	} else {
+		v4l2_dbg(2, debug, sd, "%s: Unknown port %d selected\n",
+				__func__, state->selected_input);
 		err = -1;
-		break;
 	}
 
 
@@ -846,9 +845,7 @@ static void configure_custom_video_timings(struct v4l2_subdev *sd,
 
 	v4l2_dbg(2, debug, sd, "%s\n", __func__);
 
-	switch (state->mode) {
-	case ADV7604_MODE_COMP:
-	case ADV7604_MODE_GR:
+	if (is_analog_input(sd)) {
 		/* auto graphics */
 		io_write(sd, 0x00, 0x07); /* video std */
 		io_write(sd, 0x01, 0x02); /* prim mode */
@@ -858,33 +855,28 @@ static void configure_custom_video_timings(struct v4l2_subdev *sd,
 		/* 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(client, 0x16, 2, pll))
 			v4l2_err(sd, "writing to reg 0x16 and 0x17 failed\n");
-			break;
-		}
 
 		/* active video - horizontal timing */
 		cp_write(sd, 0xa2, (cp_start_sav >> 4) & 0xff);
 		cp_write(sd, 0xa3, ((cp_start_sav & 0x0f) << 4) |
-					((cp_start_eav >> 8) & 0x0f));
+				   ((cp_start_eav >> 8) & 0x0f));
 		cp_write(sd, 0xa4, cp_start_eav & 0xff);
 
 		/* active video - vertical timing */
 		cp_write(sd, 0xa5, (cp_start_vbi >> 4) & 0xff);
 		cp_write(sd, 0xa6, ((cp_start_vbi & 0xf) << 4) |
-					((cp_end_vbi >> 8) & 0xf));
+				   ((cp_end_vbi >> 8) & 0xf));
 		cp_write(sd, 0xa7, cp_end_vbi & 0xff);
-		break;
-	case ADV7604_MODE_HDMI:
+	} else if (is_digital_input(sd)) {
 		/* set default prim_mode/vid_std for HDMI
 		   according to [REF_03, c. 4.2] */
 		io_write(sd, 0x00, 0x02); /* video std */
 		io_write(sd, 0x01, 0x06); /* prim mode */
-		break;
-	default:
-		v4l2_dbg(2, debug, sd, "%s: Unknown mode %d\n",
-				__func__, state->mode);
-		break;
+	} else {
+		v4l2_dbg(2, debug, sd, "%s: Unknown port %d selected\n",
+				__func__, state->selected_input);
 	}
 
 	cp_write(sd, 0x8f, (ch1_fr_ll >> 8) & 0x7);
@@ -893,43 +885,149 @@ static void configure_custom_video_timings(struct v4l2_subdev *sd,
 	cp_write(sd, 0xac, (height & 0x0f) << 4);
 }
 
+static void adv7604_set_offset(struct v4l2_subdev *sd, bool auto_offset, u16 offset_a, u16 offset_b, u16 offset_c)
+{
+	struct adv7604_state *state = to_state(sd);
+	u8 offset_buf[4];
+
+	if (auto_offset) {
+		offset_a = 0x3ff;
+		offset_b = 0x3ff;
+		offset_c = 0x3ff;
+	}
+
+	v4l2_dbg(2, debug, sd, "%s: %s offset: a = 0x%x, b = 0x%x, c = 0x%x\n",
+			__func__, auto_offset ? "Auto" : "Manual",
+			offset_a, offset_b, offset_c);
+
+	offset_buf[0] = (cp_read(sd, 0x77) & 0xc0) | ((offset_a & 0x3f0) >> 4);
+	offset_buf[1] = ((offset_a & 0x00f) << 4) | ((offset_b & 0x3c0) >> 6);
+	offset_buf[2] = ((offset_b & 0x03f) << 2) | ((offset_c & 0x300) >> 8);
+	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))
+		v4l2_err(sd, "%s: i2c error writing to CP reg 0x77, 0x78, 0x79, 0x7a\n", __func__);
+}
+
+static void adv7604_set_gain(struct v4l2_subdev *sd, bool auto_gain, u16 gain_a, u16 gain_b, u16 gain_c)
+{
+	struct adv7604_state *state = to_state(sd);
+	u8 gain_buf[4];
+	u8 gain_man = 1;
+	u8 agc_mode_man = 1;
+
+	if (auto_gain) {
+		gain_man = 0;
+		agc_mode_man = 0;
+		gain_a = 0x100;
+		gain_b = 0x100;
+		gain_c = 0x100;
+	}
+
+	v4l2_dbg(2, debug, sd, "%s: %s gain: a = 0x%x, b = 0x%x, c = 0x%x\n",
+			__func__, auto_gain ? "Auto" : "Manual",
+			gain_a, gain_b, gain_c);
+
+	gain_buf[0] = ((gain_man << 7) | (agc_mode_man << 6) | ((gain_a & 0x3f0) >> 4));
+	gain_buf[1] = (((gain_a & 0x00f) << 4) | ((gain_b & 0x3c0) >> 6));
+	gain_buf[2] = (((gain_b & 0x03f) << 2) | ((gain_c & 0x300) >> 8));
+	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))
+		v4l2_err(sd, "%s: i2c error writing to CP reg 0x73, 0x74, 0x75, 0x76\n", __func__);
+}
+
 static void set_rgb_quantization_range(struct v4l2_subdev *sd)
 {
 	struct adv7604_state *state = to_state(sd);
+	bool rgb_output = io_read(sd, 0x02) & 0x02;
+	bool hdmi_signal = hdmi_read(sd, 0x05) & 0x80;
+
+	v4l2_dbg(2, debug, sd, "%s: RGB quantization range: %d, RGB out: %d, HDMI: %d\n",
+			__func__, state->rgb_quantization_range,
+			rgb_output, hdmi_signal);
+
+	adv7604_set_gain(sd, true, 0x0, 0x0, 0x0);
+	adv7604_set_offset(sd, true, 0x0, 0x0, 0x0);
 
 	switch (state->rgb_quantization_range) {
 	case V4L2_DV_RGB_RANGE_AUTO:
-		/* automatic */
-		if (DIGITAL_INPUT && !(hdmi_read(sd, 0x05) & 0x80)) {
-			/* receiving DVI-D signal */
+		if (state->selected_input == ADV7604_INPUT_VGA_RGB) {
+			/* Receiving analog RGB signal
+			 * Set RGB full range (0-255) */
+			io_write_and_or(sd, 0x02, 0x0f, 0x10);
+			break;
+		}
+
+		if (state->selected_input == ADV7604_INPUT_VGA_COMP) {
+			/* Receiving analog YPbPr signal
+			 * Set automode */
+			io_write_and_or(sd, 0x02, 0x0f, 0xf0);
+			break;
+		}
+
+		if (hdmi_signal) {
+			/* Receiving HDMI signal
+			 * Set automode */
+			io_write_and_or(sd, 0x02, 0x0f, 0xf0);
+			break;
+		}
 
-			/* ADV7604 selects RGB limited range regardless of
-			   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);
+		/* Receiving DVI-D signal
+		 * ADV7604 selects RGB limited range regardless of
+		 * 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);
+		} else {
+			/* RGB full range (0-255) */
+			io_write_and_or(sd, 0x02, 0x0f, 0x10);
 
+			if (is_digital_input(sd) && rgb_output) {
+				adv7604_set_offset(sd, false, 0x40, 0x40, 0x40);
 			} else {
-				/* RGB full range (0-255) */
-				io_write_and_or(sd, 0x02, 0x0f, 0x10);
+				adv7604_set_gain(sd, false, 0xe0, 0xe0, 0xe0);
+				adv7604_set_offset(sd, false, 0x70, 0x70, 0x70);
 			}
-		} else {
-			/* receiving HDMI or analog signal, set automode */
-			io_write_and_or(sd, 0x02, 0x0f, 0xf0);
 		}
 		break;
 	case V4L2_DV_RGB_RANGE_LIMITED:
+		if (state->selected_input == ADV7604_INPUT_VGA_COMP) {
+			/* YCrCb limited range (16-235) */
+			io_write_and_or(sd, 0x02, 0x0f, 0x20);
+			break;
+		}
+
 		/* RGB limited range (16-235) */
 		io_write_and_or(sd, 0x02, 0x0f, 0x00);
+
 		break;
 	case V4L2_DV_RGB_RANGE_FULL:
+		if (state->selected_input == ADV7604_INPUT_VGA_COMP) {
+			/* YCrCb full range (0-255) */
+			io_write_and_or(sd, 0x02, 0x0f, 0x60);
+			break;
+		}
+
 		/* RGB full range (0-255) */
 		io_write_and_or(sd, 0x02, 0x0f, 0x10);
+
+		if (is_analog_input(sd) || hdmi_signal)
+			break;
+
+		/* Adjust gain/offset for DVI-D signals only */
+		if (rgb_output) {
+			adv7604_set_offset(sd, false, 0x40, 0x40, 0x40);
+		} else {
+			adv7604_set_gain(sd, false, 0xe0, 0xe0, 0xe0);
+			adv7604_set_offset(sd, false, 0x70, 0x70, 0x70);
+		}
 		break;
 	}
 }
 
-
 static int adv7604_s_ctrl(struct v4l2_ctrl *ctrl)
 {
 	struct v4l2_subdev *sd = to_sd(ctrl);
@@ -983,8 +1081,9 @@ static inline bool no_power(struct v4l2_subdev *sd)
 
 static inline bool no_signal_tmds(struct v4l2_subdev *sd)
 {
-	/* TODO port B, C and D */
-	return !(io_read(sd, 0x6a) & 0x10);
+	struct adv7604_state *state = to_state(sd);
+
+	return !(io_read(sd, 0x6a) & (0x10 >> state->selected_input));
 }
 
 static inline bool no_lock_tmds(struct v4l2_subdev *sd)
@@ -1011,7 +1110,6 @@ static inline bool no_lock_stdi(struct v4l2_subdev *sd)
 
 static inline bool no_signal(struct v4l2_subdev *sd)
 {
-	struct adv7604_state *state = to_state(sd);
 	bool ret;
 
 	ret = no_power(sd);
@@ -1019,7 +1117,7 @@ static inline bool no_signal(struct v4l2_subdev *sd)
 	ret |= no_lock_stdi(sd);
 	ret |= no_lock_sspd(sd);
 
-	if (DIGITAL_INPUT) {
+	if (is_digital_input(sd)) {
 		ret |= no_lock_tmds(sd);
 		ret |= no_signal_tmds(sd);
 	}
@@ -1036,13 +1134,11 @@ static inline bool no_lock_cp(struct v4l2_subdev *sd)
 
 static int adv7604_g_input_status(struct v4l2_subdev *sd, u32 *status)
 {
-	struct adv7604_state *state = to_state(sd);
-
 	*status = 0;
 	*status |= no_power(sd) ? V4L2_IN_ST_NO_POWER : 0;
 	*status |= no_signal(sd) ? V4L2_IN_ST_NO_SIGNAL : 0;
 	if (no_lock_cp(sd))
-		*status |= DIGITAL_INPUT ? V4L2_IN_ST_NO_SYNC : V4L2_IN_ST_NO_H_LOCK;
+		*status |= is_digital_input(sd) ? V4L2_IN_ST_NO_SYNC : V4L2_IN_ST_NO_H_LOCK;
 
 	v4l2_dbg(1, debug, sd, "%s: status = 0x%x\n", __func__, *status);
 
@@ -1157,13 +1253,11 @@ 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);
-
 	cap->type = V4L2_DV_BT_656_1120;
 	cap->bt.max_width = 1920;
 	cap->bt.max_height = 1200;
 	cap->bt.min_pixelclock = 25000000;
-	if (DIGITAL_INPUT)
+	if (is_digital_input(sd))
 		cap->bt.max_pixelclock = 225000000;
 	else
 		cap->bt.max_pixelclock = 170000000;
@@ -1179,12 +1273,11 @@ static int adv7604_dv_timings_cap(struct v4l2_subdev *sd,
 static void adv7604_fill_optional_dv_timings_fields(struct v4l2_subdev *sd,
 		struct v4l2_dv_timings *timings)
 {
-	struct adv7604_state *state = to_state(sd);
 	int i;
 
 	for (i = 0; adv7604_timings[i].bt.width; i++) {
 		if (v4l2_match_dv_timings(timings, &adv7604_timings[i],
-					DIGITAL_INPUT ? 250000 : 1000000)) {
+					is_digital_input(sd) ? 250000 : 1000000)) {
 			*timings = adv7604_timings[i];
 			break;
 		}
@@ -1204,6 +1297,7 @@ static int adv7604_query_dv_timings(struct v4l2_subdev *sd,
 	memset(timings, 0, sizeof(struct v4l2_dv_timings));
 
 	if (no_signal(sd)) {
+		state->restart_stdi_once = true;
 		v4l2_dbg(1, debug, sd, "%s: no valid signal\n", __func__);
 		return -ENOLINK;
 	}
@@ -1216,7 +1310,7 @@ static int adv7604_query_dv_timings(struct v4l2_subdev *sd,
 	bt->interlaced = stdi.interlaced ?
 		V4L2_DV_INTERLACED : V4L2_DV_PROGRESSIVE;
 
-	if (DIGITAL_INPUT) {
+	if (is_digital_input(sd)) {
 		uint32_t freq;
 
 		timings->type = V4L2_DV_BT_656_1120;
@@ -1305,8 +1399,8 @@ found:
 		return -ENOLINK;
 	}
 
-	if ((!DIGITAL_INPUT && bt->pixelclock > 170000000) ||
-			(DIGITAL_INPUT && bt->pixelclock > 225000000)) {
+	if ((is_analog_input(sd) && bt->pixelclock > 170000000) ||
+			(is_digital_input(sd) && bt->pixelclock > 225000000)) {
 		v4l2_dbg(1, debug, sd, "%s: pixelclock out of range %d\n",
 				__func__, (u32)bt->pixelclock);
 		return -ERANGE;
@@ -1329,10 +1423,15 @@ static int adv7604_s_dv_timings(struct v4l2_subdev *sd,
 	if (!timings)
 		return -EINVAL;
 
+	if (v4l2_match_dv_timings(&state->timings, timings, 0)) {
+		v4l2_dbg(1, debug, sd, "%s: no change\n", __func__);
+		return 0;
+	}
+
 	bt = &timings->bt;
 
-	if ((!DIGITAL_INPUT && bt->pixelclock > 170000000) ||
-			(DIGITAL_INPUT && bt->pixelclock > 225000000)) {
+	if ((is_analog_input(sd) && bt->pixelclock > 170000000) ||
+			(is_digital_input(sd) && bt->pixelclock > 225000000)) {
 		v4l2_dbg(1, debug, sd, "%s: pixelclock out of range %d\n",
 				__func__, (u32)bt->pixelclock);
 		return -ERANGE;
@@ -1354,7 +1453,6 @@ static int adv7604_s_dv_timings(struct v4l2_subdev *sd,
 
 	set_rgb_quantization_range(sd);
 
-
 	if (debug > 1)
 		v4l2_print_dv_timings(sd->name, "adv7604_s_dv_timings: ",
 				      timings, true);
@@ -1374,30 +1472,24 @@ static void enable_input(struct v4l2_subdev *sd)
 {
 	struct adv7604_state *state = to_state(sd);
 
-	switch (state->mode) {
-	case ADV7604_MODE_COMP:
-	case ADV7604_MODE_GR:
-		/* enable */
+	if (is_analog_input(sd)) {
 		io_write(sd, 0x15, 0xb0);   /* Disable Tristate of Pins (no audio) */
-		break;
-	case ADV7604_MODE_HDMI:
-		/* enable */
-		hdmi_write(sd, 0x1a, 0x0a); /* Unmute 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 */
 		io_write(sd, 0x15, 0xa0);   /* Disable Tristate of Pins */
-		break;
-	default:
-		v4l2_dbg(2, debug, sd, "%s: Unknown mode %d\n",
-				__func__, state->mode);
-		break;
+		hdmi_write_and_or(sd, 0x1a, 0xef, 0x00); /* Unmute audio */
+	} else {
+		v4l2_dbg(2, debug, sd, "%s: Unknown port %d selected\n",
+				__func__, state->selected_input);
 	}
 }
 
 static void disable_input(struct v4l2_subdev *sd)
 {
-	/* disable */
+	hdmi_write_and_or(sd, 0x1a, 0xef, 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, 0x1a, 0x1a); /* Mute audio */
 	hdmi_write(sd, 0x01, 0x78); /* Disable HDMI clock terminators */
 }
 
@@ -1405,9 +1497,7 @@ static void select_input(struct v4l2_subdev *sd)
 {
 	struct adv7604_state *state = to_state(sd);
 
-	switch (state->mode) {
-	case ADV7604_MODE_COMP:
-	case ADV7604_MODE_GR:
+	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 */
@@ -1433,9 +1523,9 @@ static void select_input(struct v4l2_subdev *sd)
 		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 */
-		break;
+	} else if (is_digital_input(sd)) {
+		hdmi_write(sd, 0x00, state->selected_input & 0x03);
 
-	case ADV7604_MODE_HDMI:
 		/* 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 */
@@ -1461,12 +1551,9 @@ static void select_input(struct v4l2_subdev *sd)
 		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 */
-
-		break;
-	default:
-		v4l2_dbg(2, debug, sd, "%s: Unknown mode %d\n",
-				__func__, state->mode);
-		break;
+	} else {
+		v4l2_dbg(2, debug, sd, "%s: Unknown port %d selected\n",
+				__func__, state->selected_input);
 	}
 }
 
@@ -1475,9 +1562,13 @@ static int adv7604_s_routing(struct v4l2_subdev *sd,
 {
 	struct adv7604_state *state = to_state(sd);
 
-	v4l2_dbg(2, debug, sd, "%s: input %d", __func__, input);
+	v4l2_dbg(2, debug, sd, "%s: input %d, selected input %d",
+			__func__, input, state->selected_input);
+
+	if (input == state->selected_input)
+		return 0;
 
-	state->mode = input;
+	state->selected_input = input;
 
 	disable_input(sd);
 
@@ -1516,36 +1607,47 @@ static int adv7604_g_mbus_fmt(struct v4l2_subdev *sd,
 
 static int adv7604_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
 {
-	struct adv7604_state *state = to_state(sd);
-	u8 fmt_change, fmt_change_digital, tx_5v;
-	u32 input_status;
+	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);
+	u8 fmt_change_digital;
+	u8 fmt_change;
+	u8 tx_5v;
+
+	if (irq_reg_0x43)
+		io_write(sd, 0x44, irq_reg_0x43);
+	if (irq_reg_0x70)
+		io_write(sd, 0x71, irq_reg_0x70);
+	if (irq_reg_0x6b)
+		io_write(sd, 0x6c, irq_reg_0x6b);
+
+	v4l2_dbg(2, debug, sd, "%s: ", __func__);
 
 	/* format change */
-	fmt_change = io_read(sd, 0x43) & 0x98;
-	if (fmt_change)
-		io_write(sd, 0x44, fmt_change);
-	fmt_change_digital = DIGITAL_INPUT ? (io_read(sd, 0x6b) & 0xc0) : 0;
-	if (fmt_change_digital)
-		io_write(sd, 0x6c, fmt_change_digital);
+	fmt_change = irq_reg_0x43 & 0x98;
+	fmt_change_digital = is_digital_input(sd) ? (irq_reg_0x6b & 0xc0) : 0;
+
 	if (fmt_change || fmt_change_digital) {
 		v4l2_dbg(1, debug, sd,
 			"%s: fmt_change = 0x%x, fmt_change_digital = 0x%x\n",
 			__func__, fmt_change, fmt_change_digital);
 
-		adv7604_g_input_status(sd, &input_status);
-		if (input_status != state->prev_input_status) {
-			v4l2_dbg(1, debug, sd,
-				"%s: input_status = 0x%x, prev_input_status = 0x%x\n",
-				__func__, input_status, state->prev_input_status);
-			state->prev_input_status = input_status;
-			v4l2_subdev_notify(sd, ADV7604_FMT_CHANGE, NULL);
-		}
+		v4l2_subdev_notify(sd, ADV7604_FMT_CHANGE, NULL);
 
 		if (handled)
 			*handled = true;
 	}
+	/* HDMI/DVI mode */
+	if (irq_reg_0x6b & 0x01) {
+		v4l2_dbg(1, debug, sd, "%s: irq %s mode\n", __func__,
+			(io_read(sd, 0x6a) & 0x01) ? "HDMI" : "DVI");
+		set_rgb_quantization_range(sd);
+		if (handled)
+			*handled = true;
+	}
+
 	/* tx 5v detect */
-	tx_5v = io_read(sd, 0x70) & 0x10;
+	tx_5v = io_read(sd, 0x70) & 0x1e;
 	if (tx_5v) {
 		v4l2_dbg(1, debug, sd, "%s: tx_5v: 0x%x\n", __func__, tx_5v);
 		io_write(sd, 0x71, tx_5v);
@@ -1559,55 +1661,178 @@ static int adv7604_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
 static int adv7604_get_edid(struct v4l2_subdev *sd, struct v4l2_subdev_edid *edid)
 {
 	struct adv7604_state *state = to_state(sd);
+	u8 *data = NULL;
 
-	if (edid->pad != 0)
+	if (edid->pad > ADV7604_EDID_PORT_D)
 		return -EINVAL;
 	if (edid->blocks == 0)
 		return -EINVAL;
-	if (edid->start_block >= state->edid_blocks)
+	if (edid->blocks > 2)
+		return -EINVAL;
+	if (edid->start_block > 1)
 		return -EINVAL;
-	if (edid->start_block + edid->blocks > state->edid_blocks)
-		edid->blocks = state->edid_blocks - edid->start_block;
+	if (edid->start_block == 1)
+		edid->blocks = 1;
 	if (!edid->edid)
 		return -EINVAL;
-	memcpy(edid->edid + edid->start_block * 128,
-	       state->edid + edid->start_block * 128,
+
+	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:
+		if (state->edid.present & (1 << edid->pad))
+			data = state->edid.edid;
+		break;
+	default:
+		return -EINVAL;
+		break;
+	}
+	if (!data)
+		return -ENODATA;
+
+	memcpy(edid->edid,
+	       data + edid->start_block * 128,
 	       edid->blocks * 128);
 	return 0;
 }
 
+static int get_edid_spa_location(const u8 *edid)
+{
+	u8 d;
+
+	if ((edid[0x7e] != 1) ||
+	    (edid[0x80] != 0x02) ||
+	    (edid[0x81] != 0x03)) {
+		return -1;
+	}
+
+	/* search Vendor Specific Data Block (tag 3) */
+	d = edid[0x82] & 0x7f;
+	if (d > 4) {
+		int i = 0x84;
+		int end = 0x80 + d;
+
+		do {
+			u8 tag = edid[i] >> 5;
+			u8 len = edid[i] & 0x1f;
+
+			if ((tag == 3) && (len >= 5))
+				return i + 4;
+			i += len + 1;
+		} while (i < end);
+	}
+	return -1;
+}
+
 static int adv7604_set_edid(struct v4l2_subdev *sd, struct v4l2_subdev_edid *edid)
 {
 	struct adv7604_state *state = to_state(sd);
+	int spa_loc;
+	int tmp = 0;
 	int err;
+	int i;
 
-	if (edid->pad != 0)
+	if (edid->pad > ADV7604_EDID_PORT_D)
 		return -EINVAL;
 	if (edid->start_block != 0)
 		return -EINVAL;
 	if (edid->blocks == 0) {
-		/* Pull down the hotplug pin */
-		v4l2_subdev_notify(sd, ADV7604_HOTPLUG, (void *)0);
-		/* Disables I2C access to internal EDID ram from DDC port */
-		rep_write_and_or(sd, 0x77, 0xf0, 0x0);
-		state->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);
+
 		/* Fall back to a 16:9 aspect ratio */
 		state->aspect_ratio.numerator = 16;
 		state->aspect_ratio.denominator = 9;
+
+		if (!state->edid.present)
+			state->edid.blocks = 0;
+
+		v4l2_dbg(2, debug, sd, "%s: clear EDID pad %d, edid.present = 0x%x\n",
+				__func__, edid->pad, state->edid.present);
 		return 0;
 	}
-	if (edid->blocks > 2)
+	if (edid->blocks > 2) {
+		edid->blocks = 2;
 		return -E2BIG;
+	}
 	if (!edid->edid)
 		return -EINVAL;
-	memcpy(state->edid, edid->edid, 128 * edid->blocks);
-	state->edid_blocks = edid->blocks;
+
+	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);
+
+	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:
+		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:
+		rep_write(sd, 0x70, edid->edid[spa_loc]);
+		rep_write(sd, 0x71, edid->edid[spa_loc + 1]);
+		break;
+	case ADV7604_EDID_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:
+		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);
+
+	edid->edid[spa_loc] = state->spa_port_a[0];
+	edid->edid[spa_loc + 1] = state->spa_port_a[1];
+
+	memcpy(state->edid.edid, edid->edid, 128 * edid->blocks);
+	state->edid.blocks = edid->blocks;
 	state->aspect_ratio = v4l2_calc_aspect_ratio(edid->edid[0x15],
 			edid->edid[0x16]);
-	err = edid_write_block(sd, 128 * edid->blocks, state->edid);
-	if (err < 0)
-		v4l2_err(sd, "error %d writing edid\n", err);
-	return err;
+	state->edid.present |= 1 << edid->pad;
+
+	err = edid_write_block(sd, 128 * edid->blocks, state->edid.edid);
+	if (err < 0) {
+		v4l2_err(sd, "error %d writing edid pad %d\n", err, edid->pad);
+		return err;
+	}
+
+	/* 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);
+
+	for (i = 0; i < 1000; i++) {
+		if (rep_read(sd, 0x7d) & state->edid.present)
+			break;
+		mdelay(1);
+	}
+	if (i == 1000) {
+		v4l2_err(sd, "error enabling edid (0x%x)\n", state->edid.present);
+		return -EIO;
+	}
+
+
+	/* enable hotplug after 100 ms */
+	queue_delayed_work(state->work_queues,
+			&state->delayed_work_enable_hotplug, HZ / 10);
+	return 0;
 }
 
 /*********** avi info frame CEA-861-E **************/
@@ -1670,7 +1895,7 @@ static int adv7604_log_status(struct v4l2_subdev *sd)
 	char *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",
+		"xvYCC Bt.601", "xvYCC Bt.709",
 		"YCbCr Bt.601 (0-255)", "YCbCr Bt.709 (0-255)",
 		"invalid", "invalid", "invalid", "invalid", "invalid",
 		"invalid", "invalid", "automatic"
@@ -1689,16 +1914,20 @@ 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");
-	v4l2_info(sd, "Connector type: %s\n", state->connector_hdmi ?
-			"HDMI" : (DIGITAL_INPUT ? "DVI-D" : "DVI-A"));
-	v4l2_info(sd, "EDID: %s\n", ((rep_read(sd, 0x7d) & 0x01) &&
-			(rep_read(sd, 0x77) & 0x01)) ? "enabled" : "disabled ");
+	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"));
 	v4l2_info(sd, "CEC: %s\n", !!(cec_read(sd, 0x2a) & 0x01) ?
 			"enabled" : "disabled");
 
 	v4l2_info(sd, "-----Signal status-----\n");
-	v4l2_info(sd, "Cable detected (+5V power): %s\n",
-			(io_read(sd, 0x6f) & 0x10) ? "true" : "false");
+	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"));
 	v4l2_info(sd, "TMDS signal detected: %s\n",
 			no_signal_tmds(sd) ? "false" : "true");
 	v4l2_info(sd, "TMDS signal locked: %s\n",
@@ -1744,11 +1973,14 @@ static int adv7604_log_status(struct v4l2_subdev *sd)
 	v4l2_info(sd, "Color space conversion: %s\n",
 			csc_coeff_sel_rb[cp_read(sd, 0xfc) >> 4]);
 
-	if (!DIGITAL_INPUT)
+	if (!is_digital_input(sd))
 		return 0;
 
 	v4l2_info(sd, "-----%s status-----\n", is_hdmi(sd) ? "HDMI" : "DVI-D");
-	v4l2_info(sd, "HDCP encrypted content: %s\n", (hdmi_read(sd, 0x05) & 0x40) ? "true" : "false");
+	v4l2_info(sd, "Digital video port selected: %c\n",
+			(hdmi_read(sd, 0x00) & 0x03) + 'A');
+	v4l2_info(sd, "HDCP encrypted content: %s\n",
+			(hdmi_read(sd, 0x05) & 0x40) ? "true" : "false");
 	v4l2_info(sd, "HDCP keys read: %s%s\n",
 			(hdmi_read(sd, 0x04) & 0x20) ? "yes" : "no",
 			(hdmi_read(sd, 0x04) & 0x10) ? "ERROR" : "");
@@ -1894,10 +2126,16 @@ static int adv7604_core_init(struct v4l2_subdev *sd)
 					pdata->replicate_av_codes << 1 |
 					pdata->invert_cbcr << 0);
 
-	/* TODO from platform data */
 	cp_write(sd, 0x69, 0x30);   /* Enable CP CSC */
-	io_write(sd, 0x06, 0xa6);   /* positive VS and HS */
-	io_write(sd, 0x14, 0x7f);   /* Drive strength adjusted to max */
+
+	/* VS, HS polarities */
+	io_write(sd, 0x06, 0xa0 | pdata->inv_vs_pol << 2 | pdata->inv_hs_pol << 1);
+
+	/* Adjust drive strength */
+	io_write(sd, 0x14, 0x40 | pdata->dr_str_data << 4 |
+				pdata->dr_str_clk << 2 |
+				pdata->dr_str_sync);
+
 	cp_write(sd, 0xba, (pdata->hdmi_free_run_mode << 1) | 0x01); /* HDMI free run */
 	cp_write(sd, 0xf3, 0xdc); /* Low threshold to enter/exit free run mode */
 	cp_write(sd, 0xf9, 0x23); /*  STDI ch. 1 - LCVS change threshold -
@@ -1907,6 +2145,11 @@ static int adv7604_core_init(struct v4l2_subdev *sd)
 	cp_write(sd, 0xc9, 0x2d); /* use prim_mode and vid_std as free run resolution
 				     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] */
+
 	/* TODO from platform data */
 	afe_write(sd, 0xb5, 0x01);  /* Setting MCLK to 256Fs */
 
@@ -1917,8 +2160,8 @@ static int adv7604_core_init(struct v4l2_subdev *sd)
 	io_write(sd, 0x40, 0xc2); /* Configure INT1 */
 	io_write(sd, 0x41, 0xd7); /* STDI irq for any change, disable INT2 */
 	io_write(sd, 0x46, 0x98); /* Enable SSPD, STDI and CP unlocked interrupts */
-	io_write(sd, 0x6e, 0xc0); /* Enable V_LOCKED and DE_REGEN_LCK interrupts */
-	io_write(sd, 0x73, 0x10); /* Enable CABLE_DET_A_ST (+5v) interrupt */
+	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 */
 
 	return v4l2_ctrl_handler_setup(sd->ctrl_handler);
 }
@@ -1964,6 +2207,8 @@ static struct i2c_client *adv7604_dummy_client(struct v4l2_subdev *sd,
 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;
@@ -1984,19 +2229,19 @@ static int adv7604_probe(struct i2c_client *client,
 
 	/* initialize variables */
 	state->restart_stdi_once = true;
-	state->prev_input_status = ~0;
+	state->selected_input = ~0;
 
 	/* platform data */
 	if (!pdata) {
 		v4l_err(client, "No platform data!\n");
 		return -ENODEV;
 	}
-	memcpy(&state->pdata, pdata, sizeof(state->pdata));
+	state->pdata = *pdata;
+	state->timings = cea640x480;
 
 	sd = &state->sd;
 	v4l2_i2c_subdev_init(sd, client, &adv7604_ops);
 	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
-	state->connector_hdmi = pdata->connector_hdmi;
 
 	/* i2c access to adv7604? */
 	if (adv_smbus_read_byte_data_check(client, 0xfb, false) != 0x68) {
@@ -2020,7 +2265,7 @@ 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, 1, 0, 0);
+			V4L2_CID_DV_RX_POWER_PRESENT, 0, 0x0f, 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,
diff --git a/drivers/media/i2c/adv7842.c b/drivers/media/i2c/adv7842.c
index b154f36740b4..9bbd6656fb8f 100644
--- a/drivers/media/i2c/adv7842.c
+++ b/drivers/media/i2c/adv7842.c
@@ -20,10 +20,13 @@
 
 /*
  * References (c = chapter, p = page):
- * REF_01 - Analog devices, ADV7842, Register Settings Recommendations,
- *		Revision 2.5, June 2010
- * REF_02 - Analog devices, Register map documentation, Documentation of
- *		the register maps, Software manual, Rev. F, June 2010
+ * REF_01 - Analog devices, ADV7842,
+ *		Register Settings Recommendations, Rev. 1.9, April 2011
+ * REF_02 - Analog devices, Software User Guide, UG-206,
+ *		ADV7842 I2C Register Maps, Rev. 0, November 2010
+ * REF_03 - Analog devices, Hardware User Guide, UG-214,
+ *		ADV7842 Fast Switching 2:1 HDMI 1.4 Receiver with 3D-Comb
+ *		Decoder and Digitizer , Rev. 0, January 2011
  */
 
 
@@ -61,6 +64,7 @@ MODULE_LICENSE("GPL");
 */
 
 struct adv7842_state {
+	struct adv7842_platform_data pdata;
 	struct v4l2_subdev sd;
 	struct media_pad pad;
 	struct v4l2_ctrl_handler hdl;
@@ -81,7 +85,7 @@ struct adv7842_state {
 	bool is_cea_format;
 	struct workqueue_struct *work_queues;
 	struct delayed_work delayed_work_enable_hotplug;
-	bool connector_hdmi;
+	bool restart_stdi_once;
 	bool hdmi_port_a;
 
 	/* i2c clients */
@@ -491,6 +495,11 @@ static inline int hdmi_write(struct v4l2_subdev *sd, u8 reg, u8 val)
 	return adv_smbus_write_byte_data(state->i2c_hdmi, reg, val);
 }
 
+static inline int hdmi_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 val)
+{
+	return hdmi_write(sd, reg, (hdmi_read(sd, reg) & mask) | val);
+}
+
 static inline int cp_read(struct v4l2_subdev *sd, u8 reg)
 {
 	struct adv7842_state *state = to_state(sd);
@@ -532,7 +541,7 @@ static void main_reset(struct v4l2_subdev *sd)
 
 	adv_smbus_write_byte_no_check(client, 0xff, 0x80);
 
-	mdelay(2);
+	mdelay(5);
 }
 
 /* ----------------------------------------------------------------------- */
@@ -587,10 +596,10 @@ static void adv7842_delayed_work_enable_hotplug(struct work_struct *work)
 	v4l2_dbg(2, debug, sd, "%s: enable hotplug on ports: 0x%x\n",
 			__func__, present);
 
-	if (present & 0x1)
-		mask |= 0x20; /* port A */
-	if (present & 0x2)
-		mask |= 0x10; /* port B */
+	if (present & (0x04 << ADV7842_EDID_PORT_A))
+		mask |= 0x20;
+	if (present & (0x04 << ADV7842_EDID_PORT_B))
+		mask |= 0x10;
 	io_write_and_or(sd, 0x20, 0xcf, mask);
 }
 
@@ -679,14 +688,12 @@ static int edid_write_hdmi_segment(struct v4l2_subdev *sd, u8 port)
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct adv7842_state *state = to_state(sd);
 	const u8 *val = state->hdmi_edid.edid;
-	u8 cur_mask = rep_read(sd, 0x77) & 0x0c;
-	u8 mask = port == 0 ? 0x4 : 0x8;
 	int spa_loc = edid_spa_location(val);
 	int err = 0;
 	int i;
 
-	v4l2_dbg(2, debug, sd, "%s: write EDID on port %d (spa at 0x%x)\n",
-			__func__, port, spa_loc);
+	v4l2_dbg(2, debug, sd, "%s: write EDID on port %c (spa at 0x%x)\n",
+			__func__, (port == ADV7842_EDID_PORT_A) ? 'A' : 'B', spa_loc);
 
 	/* HPA disable on port A and B */
 	io_write_and_or(sd, 0x20, 0xcf, 0x00);
@@ -694,6 +701,9 @@ static int edid_write_hdmi_segment(struct v4l2_subdev *sd, u8 port)
 	/* Disable I2C access to internal EDID ram from HDMI DDC ports */
 	rep_write_and_or(sd, 0x77, 0xf3, 0x00);
 
+	if (!state->hdmi_edid.present)
+		return 0;
+
 	/* edid segment pointer '0' for HDMI ports */
 	rep_write_and_or(sd, 0x77, 0xef, 0x00);
 
@@ -703,44 +713,32 @@ static int edid_write_hdmi_segment(struct v4l2_subdev *sd, u8 port)
 	if (err)
 		return err;
 
-	if (spa_loc > 0) {
-		if (port == 0) {
-			/* port A SPA */
-			rep_write(sd, 0x72, val[spa_loc]);
-			rep_write(sd, 0x73, val[spa_loc + 1]);
-		} else {
-			/* port B SPA */
-			rep_write(sd, 0x74, val[spa_loc]);
-			rep_write(sd, 0x75, val[spa_loc + 1]);
-		}
-		rep_write(sd, 0x76, spa_loc);
+	if (spa_loc < 0)
+		spa_loc = 0xc0; /* Default value [REF_02, p. 199] */
+
+	if (port == ADV7842_EDID_PORT_A) {
+		rep_write(sd, 0x72, val[spa_loc]);
+		rep_write(sd, 0x73, val[spa_loc + 1]);
 	} else {
-		/* default register values for SPA */
-		if (port == 0) {
-			/* port A SPA */
-			rep_write(sd, 0x72, 0);
-			rep_write(sd, 0x73, 0);
-		} else {
-			/* port B SPA */
-			rep_write(sd, 0x74, 0);
-			rep_write(sd, 0x75, 0);
-		}
-		rep_write(sd, 0x76, 0xc0);
+		rep_write(sd, 0x74, val[spa_loc]);
+		rep_write(sd, 0x75, val[spa_loc + 1]);
 	}
-	rep_write_and_or(sd, 0x77, 0xbf, 0x00);
+	rep_write(sd, 0x76, spa_loc & 0xff);
+	rep_write_and_or(sd, 0x77, 0xbf, (spa_loc >> 2) & 0x40);
 
 	/* Calculates the checksums and enables I2C access to internal
 	 * EDID ram from HDMI DDC ports
 	 */
-	rep_write_and_or(sd, 0x77, 0xf3, mask | cur_mask);
+	rep_write_and_or(sd, 0x77, 0xf3, state->hdmi_edid.present);
 
 	for (i = 0; i < 1000; i++) {
-		if (rep_read(sd, 0x7d) & mask)
+		if (rep_read(sd, 0x7d) & state->hdmi_edid.present)
 			break;
 		mdelay(1);
 	}
 	if (i == 1000) {
-		v4l_err(client, "error enabling edid on port %d\n", port);
+		v4l_err(client, "error enabling edid on port %c\n",
+				(port == ADV7842_EDID_PORT_A) ? 'A' : 'B');
 		return -EIO;
 	}
 
@@ -927,7 +925,7 @@ static int configure_predefined_video_timings(struct v4l2_subdev *sd,
 	cp_write(sd, 0x27, 0x00);
 	cp_write(sd, 0x28, 0x00);
 	cp_write(sd, 0x29, 0x00);
-	cp_write(sd, 0x8f, 0x00);
+	cp_write(sd, 0x8f, 0x40);
 	cp_write(sd, 0x90, 0x00);
 	cp_write(sd, 0xa5, 0x00);
 	cp_write(sd, 0xa6, 0x00);
@@ -1033,34 +1031,60 @@ static void set_rgb_quantization_range(struct v4l2_subdev *sd)
 {
 	struct adv7842_state *state = to_state(sd);
 
+	v4l2_dbg(2, debug, sd, "%s: rgb_quantization_range = %d\n",
+		       __func__, state->rgb_quantization_range);
+
 	switch (state->rgb_quantization_range) {
 	case V4L2_DV_RGB_RANGE_AUTO:
-		/* automatic */
-		if (is_digital_input(sd) && !(hdmi_read(sd, 0x05) & 0x80)) {
-			/* receiving DVI-D signal */
-
-			/* ADV7842 selects RGB limited range regardless of
-			   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);
-
-			} else {
-				/* RGB full range (0-255) */
-				io_write_and_or(sd, 0x02, 0x0f, 0x10);
-			}
-		} else {
-			/* receiving HDMI or analog signal, set automode */
+		if (state->mode == ADV7842_MODE_RGB) {
+			/* Receiving analog RGB signal
+			 * Set RGB full range (0-255) */
+			io_write_and_or(sd, 0x02, 0x0f, 0x10);
+			break;
+		}
+
+		if (state->mode == ADV7842_MODE_COMP) {
+			/* Receiving analog YPbPr signal
+			 * Set automode */
 			io_write_and_or(sd, 0x02, 0x0f, 0xf0);
+			break;
+		}
+
+		if (hdmi_read(sd, 0x05) & 0x80) {
+			/* Receiving HDMI signal
+			 * Set automode */
+			io_write_and_or(sd, 0x02, 0x0f, 0xf0);
+			break;
+		}
+
+		/* Receiving DVI-D signal
+		 * ADV7842 selects RGB limited range regardless of
+		 * 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);
+		} else {
+			/* RGB full range (0-255) */
+			io_write_and_or(sd, 0x02, 0x0f, 0x10);
 		}
 		break;
 	case V4L2_DV_RGB_RANGE_LIMITED:
-		/* RGB limited range (16-235) */
-		io_write_and_or(sd, 0x02, 0x0f, 0x00);
+		if (state->mode == ADV7842_MODE_COMP) {
+			/* YCrCb limited range (16-235) */
+			io_write_and_or(sd, 0x02, 0x0f, 0x20);
+		} else {
+			/* RGB limited range (16-235) */
+			io_write_and_or(sd, 0x02, 0x0f, 0x00);
+		}
 		break;
 	case V4L2_DV_RGB_RANGE_FULL:
-		/* RGB full range (0-255) */
-		io_write_and_or(sd, 0x02, 0x0f, 0x10);
+		if (state->mode == ADV7842_MODE_COMP) {
+			/* YCrCb full range (0-255) */
+			io_write_and_or(sd, 0x02, 0x0f, 0x60);
+		} else {
+			/* RGB full range (0-255) */
+			io_write_and_or(sd, 0x02, 0x0f, 0x10);
+		}
 		break;
 	}
 }
@@ -1298,7 +1322,7 @@ static int adv7842_dv_timings_cap(struct v4l2_subdev *sd,
 }
 
 /* Fill the optional fields .standards and .flags in struct v4l2_dv_timings
-   if the format is listed in adv7604_timings[] */
+   if the format is listed in adv7842_timings[] */
 static void adv7842_fill_optional_dv_timings_fields(struct v4l2_subdev *sd,
 		struct v4l2_dv_timings *timings)
 {
@@ -1314,119 +1338,106 @@ static int adv7842_query_dv_timings(struct v4l2_subdev *sd,
 	struct v4l2_bt_timings *bt = &timings->bt;
 	struct stdi_readback stdi = { 0 };
 
+	v4l2_dbg(1, debug, sd, "%s:\n", __func__);
+
 	/* SDP block */
 	if (state->mode == ADV7842_MODE_SDP)
 		return -ENODATA;
 
 	/* read STDI */
 	if (read_stdi(sd, &stdi)) {
+		state->restart_stdi_once = true;
 		v4l2_dbg(1, debug, sd, "%s: no valid signal\n", __func__);
 		return -ENOLINK;
 	}
 	bt->interlaced = stdi.interlaced ?
 		V4L2_DV_INTERLACED : V4L2_DV_PROGRESSIVE;
-	bt->polarities = ((hdmi_read(sd, 0x05) & 0x10) ? V4L2_DV_VSYNC_POS_POL : 0) |
-		((hdmi_read(sd, 0x05) & 0x20) ? V4L2_DV_HSYNC_POS_POL : 0);
-	bt->vsync = stdi.lcvs;
 
 	if (is_digital_input(sd)) {
-		bool lock = hdmi_read(sd, 0x04) & 0x02;
-		bool interlaced = hdmi_read(sd, 0x0b) & 0x20;
-		unsigned w = (hdmi_read(sd, 0x07) & 0x1f) * 256 + hdmi_read(sd, 0x08);
-		unsigned h = (hdmi_read(sd, 0x09) & 0x1f) * 256 + hdmi_read(sd, 0x0a);
-		unsigned w_total = (hdmi_read(sd, 0x1e) & 0x3f) * 256 +
-			hdmi_read(sd, 0x1f);
-		unsigned h_total = ((hdmi_read(sd, 0x26) & 0x3f) * 256 +
-				    hdmi_read(sd, 0x27)) / 2;
-		unsigned freq = (((hdmi_read(sd, 0x51) << 1) +
-					(hdmi_read(sd, 0x52) >> 7)) * 1000000) +
-			((hdmi_read(sd, 0x52) & 0x7f) * 1000000) / 128;
-		int i;
+		uint32_t freq;
 
-		if (is_hdmi(sd)) {
-			/* adjust for deep color mode */
-			freq = freq * 8 / (((hdmi_read(sd, 0x0b) & 0xc0)>>6) * 2 + 8);
-		}
-
-		/* No lock? */
-		if (!lock) {
-			v4l2_dbg(1, debug, sd, "%s: no lock on TMDS signal\n", __func__);
-			return -ENOLCK;
-		}
-		/* Interlaced? */
-		if (interlaced) {
-			v4l2_dbg(1, debug, sd, "%s: interlaced video not supported\n", __func__);
-			return -ERANGE;
-		}
-
-		for (i = 0; v4l2_dv_timings_presets[i].bt.width; i++) {
-			const struct v4l2_bt_timings *bt = &v4l2_dv_timings_presets[i].bt;
-
-			if (!v4l2_valid_dv_timings(&v4l2_dv_timings_presets[i],
-						   adv7842_get_dv_timings_cap(sd),
-						   adv7842_check_dv_timings, NULL))
-				continue;
-			if (w_total != htotal(bt) || h_total != vtotal(bt))
-				continue;
+		timings->type = V4L2_DV_BT_656_1120;
 
-			if (w != bt->width || h != bt->height)
-				continue;
+		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 (abs(freq - bt->pixelclock) > 1000000)
-				continue;
-			*timings = v4l2_dv_timings_presets[i];
-			return 0;
+		if (is_hdmi(sd)) {
+			/* adjust for deep color mode */
+			freq = freq * 8 / (((hdmi_read(sd, 0x0b) & 0xc0) >> 5) + 8);
 		}
-
-		timings->type = V4L2_DV_BT_656_1120;
-
-		bt->width = w;
-		bt->height = h;
-		bt->interlaced = (hdmi_read(sd, 0x0b) & 0x20) ?
-			V4L2_DV_INTERLACED : V4L2_DV_PROGRESSIVE;
-		bt->polarities = ((hdmi_read(sd, 0x05) & 0x10) ?
-			V4L2_DV_VSYNC_POS_POL : 0) | ((hdmi_read(sd, 0x05) & 0x20) ?
-			V4L2_DV_HSYNC_POS_POL : 0);
-		bt->pixelclock = (((hdmi_read(sd, 0x51) << 1) +
-				   (hdmi_read(sd, 0x52) >> 7)) * 1000000) +
-				 ((hdmi_read(sd, 0x52) & 0x7f) * 1000000) / 128;
-		bt->hfrontporch = (hdmi_read(sd, 0x20) & 0x1f) * 256 +
+		bt->pixelclock = freq;
+		bt->hfrontporch = (hdmi_read(sd, 0x20) & 0x03) * 256 +
 			hdmi_read(sd, 0x21);
-		bt->hsync = (hdmi_read(sd, 0x22) & 0x1f) * 256 +
+		bt->hsync = (hdmi_read(sd, 0x22) & 0x03) * 256 +
 			hdmi_read(sd, 0x23);
-		bt->hbackporch = (hdmi_read(sd, 0x24) & 0x1f) * 256 +
+		bt->hbackporch = (hdmi_read(sd, 0x24) & 0x03) * 256 +
 			hdmi_read(sd, 0x25);
-		bt->vfrontporch = ((hdmi_read(sd, 0x2a) & 0x3f) * 256 +
-				   hdmi_read(sd, 0x2b)) / 2;
-		bt->il_vfrontporch = ((hdmi_read(sd, 0x2c) & 0x3f) * 256 +
-				      hdmi_read(sd, 0x2d)) / 2;
-		bt->vsync = ((hdmi_read(sd, 0x2e) & 0x3f) * 256 +
-			     hdmi_read(sd, 0x2f)) / 2;
-		bt->il_vsync = ((hdmi_read(sd, 0x30) & 0x3f) * 256 +
-				hdmi_read(sd, 0x31)) / 2;
-		bt->vbackporch = ((hdmi_read(sd, 0x32) & 0x3f) * 256 +
-				  hdmi_read(sd, 0x33)) / 2;
-		bt->il_vbackporch = ((hdmi_read(sd, 0x34) & 0x3f) * 256 +
-				     hdmi_read(sd, 0x35)) / 2;
-
-		bt->standards = 0;
-		bt->flags = 0;
-	} else {
-		/* Interlaced? */
-		if (stdi.interlaced) {
-			v4l2_dbg(1, debug, sd, "%s: interlaced video not supported\n", __func__);
-			return -ERANGE;
+		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;
+		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;
 		}
-
+		adv7842_fill_optional_dv_timings_fields(sd, timings);
+	} else {
+		/* find format
+		 * Since LCVS values are inaccurate [REF_03, p. 339-340],
+		 * stdi2dv_timings() is called with lcvs +-1 if the first attempt fails.
+		 */
+		if (!stdi2dv_timings(sd, &stdi, timings))
+			goto found;
+		stdi.lcvs += 1;
+		v4l2_dbg(1, debug, sd, "%s: lcvs + 1 = %d\n", __func__, stdi.lcvs);
+		if (!stdi2dv_timings(sd, &stdi, timings))
+			goto found;
+		stdi.lcvs -= 2;
+		v4l2_dbg(1, debug, sd, "%s: lcvs - 1 = %d\n", __func__, stdi.lcvs);
 		if (stdi2dv_timings(sd, &stdi, timings)) {
+			/*
+			 * The STDI block may measure wrong values, especially
+			 * for lcvs and lcf. If the driver can not find any
+			 * valid timing, the STDI block is restarted to measure
+			 * the video timings again. The function will return an
+			 * error, but the restart of STDI will generate a new
+			 * STDI interrupt and the format detection process will
+			 * restart.
+			 */
+			if (state->restart_stdi_once) {
+				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);
+				/* trigger STDI restart */
+				cp_write_and_or(sd, 0x86, 0xf9, 0x04);
+				/* reset to continuous mode */
+				cp_write_and_or(sd, 0x86, 0xf9, 0x02);
+				state->restart_stdi_once = false;
+				return -ENOLINK;
+			}
 			v4l2_dbg(1, debug, sd, "%s: format not supported\n", __func__);
 			return -ERANGE;
 		}
+		state->restart_stdi_once = true;
 	}
+found:
 
 	if (debug > 1)
-		v4l2_print_dv_timings(sd->name, "adv7842_query_dv_timings: ",
-				      timings, true);
+		v4l2_print_dv_timings(sd->name, "adv7842_query_dv_timings:",
+				timings, true);
 	return 0;
 }
 
@@ -1437,9 +1448,16 @@ static int adv7842_s_dv_timings(struct v4l2_subdev *sd,
 	struct v4l2_bt_timings *bt;
 	int err;
 
+	v4l2_dbg(1, debug, sd, "%s:\n", __func__);
+
 	if (state->mode == ADV7842_MODE_SDP)
 		return -ENODATA;
 
+	if (v4l2_match_dv_timings(&state->timings, timings, 0)) {
+		v4l2_dbg(1, debug, sd, "%s: no change\n", __func__);
+		return 0;
+	}
+
 	bt = &timings->bt;
 
 	if (!v4l2_valid_dv_timings(timings, adv7842_get_dv_timings_cap(sd),
@@ -1450,7 +1468,7 @@ static int adv7842_s_dv_timings(struct v4l2_subdev *sd,
 
 	state->timings = *timings;
 
-	cp_write(sd, 0x91, bt->interlaced ? 0x50 : 0x10);
+	cp_write(sd, 0x91, bt->interlaced ? 0x40 : 0x00);
 
 	/* Use prim_mode and vid_std when available */
 	err = configure_predefined_video_timings(sd, timings);
@@ -1483,18 +1501,18 @@ static int adv7842_g_dv_timings(struct v4l2_subdev *sd,
 static void enable_input(struct v4l2_subdev *sd)
 {
 	struct adv7842_state *state = to_state(sd);
+
+	set_rgb_quantization_range(sd);
 	switch (state->mode) {
 	case ADV7842_MODE_SDP:
 	case ADV7842_MODE_COMP:
 	case ADV7842_MODE_RGB:
-		/* enable */
 		io_write(sd, 0x15, 0xb0);   /* Disable Tristate of Pins (no audio) */
 		break;
 	case ADV7842_MODE_HDMI:
-		/* enable */
-		hdmi_write(sd, 0x1a, 0x0a); /* Unmute audio */
 		hdmi_write(sd, 0x01, 0x00); /* Enable HDMI clock terminators */
 		io_write(sd, 0x15, 0xa0);   /* Disable Tristate of Pins */
+		hdmi_write_and_or(sd, 0x1a, 0xef, 0x00); /* Unmute audio */
 		break;
 	default:
 		v4l2_dbg(2, debug, sd, "%s: Unknown mode %d\n",
@@ -1505,9 +1523,9 @@ static void enable_input(struct v4l2_subdev *sd)
 
 static void disable_input(struct v4l2_subdev *sd)
 {
-	/* disable */
+	hdmi_write_and_or(sd, 0x1a, 0xef, 0x10); /* Mute audio [REF_01, c. 2.2.2] */
+	msleep(16); /* 512 samples with >= 32 kHz sample rate [REF_03, c. 8.29] */
 	io_write(sd, 0x15, 0xbe);   /* Tristate all outputs from video core */
-	hdmi_write(sd, 0x1a, 0x1a); /* Mute audio */
 	hdmi_write(sd, 0x01, 0x78); /* Disable HDMI clock terminators */
 }
 
@@ -1575,9 +1593,6 @@ static void select_input(struct v4l2_subdev *sd,
 		afe_write(sd, 0x00, 0x00); /* power up ADC */
 		afe_write(sd, 0xc8, 0x00); /* phase control */
 
-		io_write(sd, 0x19, 0x83); /* LLC DLL phase */
-		io_write(sd, 0x33, 0x40); /* LLC DLL enable */
-
 		io_write(sd, 0xdd, 0x90); /* Manual 2x output clock */
 		/* script says register 0xde, which don't exist in manual */
 
@@ -1611,8 +1626,6 @@ static void select_input(struct v4l2_subdev *sd,
 		/* deinterlacer enabled and 3D comb */
 		sdp_write_and_or(sd, 0x12, 0xf6, 0x09);
 
-		sdp_write(sd, 0xdd, 0x08); /* free run auto */
-
 		break;
 
 	case ADV7842_MODE_COMP:
@@ -1627,6 +1640,13 @@ static void select_input(struct v4l2_subdev *sd,
 
 		afe_write(sd, 0x00, 0x00); /* power up ADC */
 		afe_write(sd, 0xc8, 0x00); /* phase control */
+		if (state->mode == ADV7842_MODE_COMP) {
+			/* force to YCrCb */
+			io_write_and_or(sd, 0x02, 0x0f, 0x60);
+		} else {
+			/* force to RGB */
+			io_write_and_or(sd, 0x02, 0x0f, 0x10);
+		}
 
 		/* set ADI recommended settings for digitizer */
 		/* "ADV7842 Register Settings Recommendations
@@ -1722,19 +1742,19 @@ static int adv7842_s_routing(struct v4l2_subdev *sd,
 
 	switch (input) {
 	case ADV7842_SELECT_HDMI_PORT_A:
-		/* TODO select HDMI_COMP or HDMI_GR */
 		state->mode = ADV7842_MODE_HDMI;
 		state->vid_std_select = ADV7842_HDMI_COMP_VID_STD_HD_1250P;
 		state->hdmi_port_a = true;
 		break;
 	case ADV7842_SELECT_HDMI_PORT_B:
-		/* TODO select HDMI_COMP or HDMI_GR */
 		state->mode = ADV7842_MODE_HDMI;
 		state->vid_std_select = ADV7842_HDMI_COMP_VID_STD_HD_1250P;
 		state->hdmi_port_a = false;
 		break;
 	case ADV7842_SELECT_VGA_COMP:
-		v4l2_info(sd, "%s: VGA component: todo\n", __func__);
+		state->mode = ADV7842_MODE_COMP;
+		state->vid_std_select = ADV7842_RGB_VID_STD_AUTO_GRAPH_MODE;
+		break;
 	case ADV7842_SELECT_VGA_RGB:
 		state->mode = ADV7842_MODE_RGB;
 		state->vid_std_select = ADV7842_RGB_VID_STD_AUTO_GRAPH_MODE;
@@ -1814,12 +1834,15 @@ static void adv7842_irq_enable(struct v4l2_subdev *sd, bool enable)
 		io_write(sd, 0x78, 0x03);
 		/* Enable SDP Standard Detection Change and SDP Video Detected */
 		io_write(sd, 0xa0, 0x09);
+		/* Enable HDMI_MODE interrupt */
+		io_write(sd, 0x69, 0x08);
 	} else {
 		io_write(sd, 0x46, 0x0);
 		io_write(sd, 0x5a, 0x0);
 		io_write(sd, 0x73, 0x0);
 		io_write(sd, 0x78, 0x0);
 		io_write(sd, 0xa0, 0x0);
+		io_write(sd, 0x69, 0x0);
 	}
 }
 
@@ -1827,11 +1850,9 @@ static int adv7842_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
 {
 	struct adv7842_state *state = to_state(sd);
 	u8 fmt_change_cp, fmt_change_digital, fmt_change_sdp;
-	u8 irq_status[5];
-	u8 irq_cfg = io_read(sd, 0x40);
+	u8 irq_status[6];
 
-	/* disable irq-pin output */
-	io_write(sd, 0x40, irq_cfg | 0x3);
+	adv7842_irq_enable(sd, false);
 
 	/* read status */
 	irq_status[0] = io_read(sd, 0x43);
@@ -1839,6 +1860,7 @@ static int adv7842_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
 	irq_status[2] = io_read(sd, 0x70);
 	irq_status[3] = io_read(sd, 0x75);
 	irq_status[4] = io_read(sd, 0x9d);
+	irq_status[5] = io_read(sd, 0x66);
 
 	/* and clear */
 	if (irq_status[0])
@@ -1851,10 +1873,14 @@ static int adv7842_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
 		io_write(sd, 0x76, irq_status[3]);
 	if (irq_status[4])
 		io_write(sd, 0x9e, irq_status[4]);
+	if (irq_status[5])
+		io_write(sd, 0x67, irq_status[5]);
 
-	v4l2_dbg(1, debug, sd, "%s: irq %x, %x, %x, %x, %x\n", __func__,
+	adv7842_irq_enable(sd, true);
+
+	v4l2_dbg(1, debug, sd, "%s: irq %x, %x, %x, %x, %x, %x\n", __func__,
 		 irq_status[0], irq_status[1], irq_status[2],
-		 irq_status[3], irq_status[4]);
+		 irq_status[3], irq_status[4], irq_status[5]);
 
 	/* format change CP */
 	fmt_change_cp = irq_status[0] & 0x9c;
@@ -1871,25 +1897,72 @@ static int adv7842_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
 	else
 		fmt_change_digital = 0;
 
-	/* notify */
+	/* format change */
 	if (fmt_change_cp || fmt_change_digital || fmt_change_sdp) {
 		v4l2_dbg(1, debug, sd,
 			 "%s: fmt_change_cp = 0x%x, fmt_change_digital = 0x%x, fmt_change_sdp = 0x%x\n",
 			 __func__, fmt_change_cp, fmt_change_digital,
 			 fmt_change_sdp);
 		v4l2_subdev_notify(sd, ADV7842_FMT_CHANGE, NULL);
+		if (handled)
+			*handled = true;
 	}
 
-	/* 5v cable detect */
-	if (irq_status[2])
+	/* HDMI/DVI mode */
+	if (irq_status[5] & 0x08) {
+		v4l2_dbg(1, debug, sd, "%s: irq %s mode\n", __func__,
+			 (io_read(sd, 0x65) & 0x08) ? "HDMI" : "DVI");
+		if (handled)
+			*handled = true;
+	}
+
+	/* tx 5v detect */
+	if (irq_status[2] & 0x3) {
+		v4l2_dbg(1, debug, sd, "%s: irq tx_5v\n", __func__);
 		adv7842_s_detect_tx_5v_ctrl(sd);
+		if (handled)
+			*handled = true;
+	}
+	return 0;
+}
+
+static int adv7842_get_edid(struct v4l2_subdev *sd, struct v4l2_subdev_edid *edid)
+{
+	struct adv7842_state *state = to_state(sd);
+	u8 *data = NULL;
 
-	if (handled)
-		*handled = true;
+	if (edid->pad > ADV7842_EDID_PORT_VGA)
+		return -EINVAL;
+	if (edid->blocks == 0)
+		return -EINVAL;
+	if (edid->blocks > 2)
+		return -EINVAL;
+	if (edid->start_block > 1)
+		return -EINVAL;
+	if (edid->start_block == 1)
+		edid->blocks = 1;
+	if (!edid->edid)
+		return -EINVAL;
 
-	/* re-enable irq-pin output */
-	io_write(sd, 0x40, irq_cfg);
+	switch (edid->pad) {
+	case ADV7842_EDID_PORT_A:
+	case ADV7842_EDID_PORT_B:
+		if (state->hdmi_edid.present & (0x04 << edid->pad))
+			data = state->hdmi_edid.edid;
+		break;
+	case ADV7842_EDID_PORT_VGA:
+		if (state->vga_edid.present)
+			data = state->vga_edid.edid;
+		break;
+	default:
+		return -EINVAL;
+	}
+	if (!data)
+		return -ENODATA;
 
+	memcpy(edid->edid,
+	       data + edid->start_block * 128,
+	       edid->blocks * 128);
 	return 0;
 }
 
@@ -1898,7 +1971,7 @@ static int adv7842_set_edid(struct v4l2_subdev *sd, struct v4l2_subdev_edid *e)
 	struct adv7842_state *state = to_state(sd);
 	int err = 0;
 
-	if (e->pad > 2)
+	if (e->pad > ADV7842_EDID_PORT_VGA)
 		return -EINVAL;
 	if (e->start_block != 0)
 		return -EINVAL;
@@ -1911,20 +1984,25 @@ static int adv7842_set_edid(struct v4l2_subdev *sd, struct v4l2_subdev_edid *e)
 	state->aspect_ratio = v4l2_calc_aspect_ratio(e->edid[0x15],
 			e->edid[0x16]);
 
-	if (e->pad == 2) {
+	switch (e->pad) {
+	case ADV7842_EDID_PORT_VGA:
 		memset(&state->vga_edid.edid, 0, 256);
 		state->vga_edid.present = e->blocks ? 0x1 : 0x0;
 		memcpy(&state->vga_edid.edid, e->edid, 128 * e->blocks);
 		err = edid_write_vga_segment(sd);
-	} else {
-		u32 mask = 0x1<<e->pad;
+		break;
+	case ADV7842_EDID_PORT_A:
+	case ADV7842_EDID_PORT_B:
 		memset(&state->hdmi_edid.edid, 0, 256);
 		if (e->blocks)
-			state->hdmi_edid.present |= mask;
+			state->hdmi_edid.present |= 0x04 << e->pad;
 		else
-			state->hdmi_edid.present &= ~mask;
-		memcpy(&state->hdmi_edid.edid, e->edid, 128*e->blocks);
+			state->hdmi_edid.present &= ~(0x04 << e->pad);
+		memcpy(&state->hdmi_edid.edid, e->edid, 128 * e->blocks);
 		err = edid_write_hdmi_segment(sd, e->pad);
+		break;
+	default:
+		return -EINVAL;
 	}
 	if (err < 0)
 		v4l2_err(sd, "error %d writing edid on port %d\n", err, e->pad);
@@ -2156,7 +2234,7 @@ static int adv7842_cp_log_status(struct v4l2_subdev *sd)
 	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",
+		"xvYCC Bt.601", "xvYCC Bt.709",
 		"YCbCr Bt.601 (0-255)", "YCbCr Bt.709 (0-255)",
 		"invalid", "invalid", "invalid", "invalid", "invalid",
 		"invalid", "invalid", "automatic"
@@ -2175,8 +2253,6 @@ static int adv7842_cp_log_status(struct v4l2_subdev *sd)
 
 	v4l2_info(sd, "-----Chip status-----\n");
 	v4l2_info(sd, "Chip power: %s\n", no_power(sd) ? "off" : "on");
-	v4l2_info(sd, "Connector type: %s\n", state->connector_hdmi ?
-			"HDMI" : (is_digital_input(sd) ? "DVI-D" : "DVI-A"));
 	v4l2_info(sd, "HDMI/DVI-D port selected: %s\n",
 			state->hdmi_port_a ? "A" : "B");
 	v4l2_info(sd, "EDID A %s, B %s\n",
@@ -2354,15 +2430,63 @@ static int adv7842_querystd(struct v4l2_subdev *sd, v4l2_std_id *std)
 	return 0;
 }
 
+static void adv7842_s_sdp_io(struct v4l2_subdev *sd, struct adv7842_sdp_io_sync_adjustment *s)
+{
+	if (s && s->adjust) {
+		sdp_io_write(sd, 0x94, (s->hs_beg >> 8) & 0xf);
+		sdp_io_write(sd, 0x95, s->hs_beg & 0xff);
+		sdp_io_write(sd, 0x96, (s->hs_width >> 8) & 0xf);
+		sdp_io_write(sd, 0x97, s->hs_width & 0xff);
+		sdp_io_write(sd, 0x98, (s->de_beg >> 8) & 0xf);
+		sdp_io_write(sd, 0x99, s->de_beg & 0xff);
+		sdp_io_write(sd, 0x9a, (s->de_end >> 8) & 0xf);
+		sdp_io_write(sd, 0x9b, s->de_end & 0xff);
+		sdp_io_write(sd, 0xa8, s->vs_beg_o);
+		sdp_io_write(sd, 0xa9, s->vs_beg_e);
+		sdp_io_write(sd, 0xaa, s->vs_end_o);
+		sdp_io_write(sd, 0xab, s->vs_end_e);
+		sdp_io_write(sd, 0xac, s->de_v_beg_o);
+		sdp_io_write(sd, 0xad, s->de_v_beg_e);
+		sdp_io_write(sd, 0xae, s->de_v_end_o);
+		sdp_io_write(sd, 0xaf, s->de_v_end_e);
+	} else {
+		/* set to default */
+		sdp_io_write(sd, 0x94, 0x00);
+		sdp_io_write(sd, 0x95, 0x00);
+		sdp_io_write(sd, 0x96, 0x00);
+		sdp_io_write(sd, 0x97, 0x20);
+		sdp_io_write(sd, 0x98, 0x00);
+		sdp_io_write(sd, 0x99, 0x00);
+		sdp_io_write(sd, 0x9a, 0x00);
+		sdp_io_write(sd, 0x9b, 0x00);
+		sdp_io_write(sd, 0xa8, 0x04);
+		sdp_io_write(sd, 0xa9, 0x04);
+		sdp_io_write(sd, 0xaa, 0x04);
+		sdp_io_write(sd, 0xab, 0x04);
+		sdp_io_write(sd, 0xac, 0x04);
+		sdp_io_write(sd, 0xad, 0x04);
+		sdp_io_write(sd, 0xae, 0x04);
+		sdp_io_write(sd, 0xaf, 0x04);
+	}
+}
+
 static int adv7842_s_std(struct v4l2_subdev *sd, v4l2_std_id norm)
 {
 	struct adv7842_state *state = to_state(sd);
+	struct adv7842_platform_data *pdata = &state->pdata;
 
 	v4l2_dbg(1, debug, sd, "%s:\n", __func__);
 
 	if (state->mode != ADV7842_MODE_SDP)
 		return -ENODATA;
 
+	if (norm & V4L2_STD_625_50)
+		adv7842_s_sdp_io(sd, &pdata->sdp_io_sync_625);
+	else if (norm & V4L2_STD_525_60)
+		adv7842_s_sdp_io(sd, &pdata->sdp_io_sync_525);
+	else
+		adv7842_s_sdp_io(sd, NULL);
+
 	if (norm & V4L2_STD_ALL) {
 		state->norm = norm;
 		return 0;
@@ -2385,9 +2509,10 @@ static int adv7842_g_std(struct v4l2_subdev *sd, v4l2_std_id *norm)
 
 /* ----------------------------------------------------------------------- */
 
-static int adv7842_core_init(struct v4l2_subdev *sd,
-		const struct adv7842_platform_data *pdata)
+static int adv7842_core_init(struct v4l2_subdev *sd)
 {
+	struct adv7842_state *state = to_state(sd);
+	struct adv7842_platform_data *pdata = &state->pdata;
 	hdmi_write(sd, 0x48,
 		   (pdata->disable_pwrdnb ? 0x80 : 0) |
 		   (pdata->disable_cable_det_rst ? 0x40 : 0));
@@ -2400,7 +2525,7 @@ static int adv7842_core_init(struct v4l2_subdev *sd,
 
 	/* video format */
 	io_write(sd, 0x02,
-		 pdata->inp_color_space << 4 |
+		 0xf0 |
 		 pdata->alt_gamma << 3 |
 		 pdata->op_656_range << 2 |
 		 pdata->rgb_out << 1 |
@@ -2412,13 +2537,24 @@ static int adv7842_core_init(struct v4l2_subdev *sd,
 			pdata->replicate_av_codes << 1 |
 			pdata->invert_cbcr << 0);
 
+	/* HDMI audio */
+	hdmi_write_and_or(sd, 0x1a, 0xf1, 0x08); /* Wait 1 s before unmute */
+
 	/* Drive strength */
-	io_write_and_or(sd, 0x14, 0xc0, pdata->drive_strength.data<<4 |
-			pdata->drive_strength.clock<<2 |
-			pdata->drive_strength.sync);
+	io_write_and_or(sd, 0x14, 0xc0,
+			pdata->dr_str_data << 4 |
+			pdata->dr_str_clk << 2 |
+			pdata->dr_str_sync);
 
 	/* HDMI free run */
-	cp_write(sd, 0xba, (pdata->hdmi_free_run_mode << 1) | 0x01);
+	cp_write_and_or(sd, 0xba, 0xfc, pdata->hdmi_free_run_enable |
+					(pdata->hdmi_free_run_mode << 1));
+
+	/* SPD free run */
+	sdp_write_and_or(sd, 0xdd, 0xf0, pdata->sdp_free_run_force |
+					 (pdata->sdp_free_run_cbar_en << 1) |
+					 (pdata->sdp_free_run_man_col_en << 2) |
+					 (pdata->sdp_free_run_auto << 3));
 
 	/* TODO from platform data */
 	cp_write(sd, 0x69, 0x14);   /* Enable CP CSC */
@@ -2431,18 +2567,6 @@ static int adv7842_core_init(struct v4l2_subdev *sd,
 
 	sdp_csc_coeff(sd, &pdata->sdp_csc_coeff);
 
-	if (pdata->sdp_io_sync.adjust) {
-		const struct adv7842_sdp_io_sync_adjustment *s = &pdata->sdp_io_sync;
-		sdp_io_write(sd, 0x94, (s->hs_beg>>8) & 0xf);
-		sdp_io_write(sd, 0x95, s->hs_beg & 0xff);
-		sdp_io_write(sd, 0x96, (s->hs_width>>8) & 0xf);
-		sdp_io_write(sd, 0x97, s->hs_width & 0xff);
-		sdp_io_write(sd, 0x98, (s->de_beg>>8) & 0xf);
-		sdp_io_write(sd, 0x99, s->de_beg & 0xff);
-		sdp_io_write(sd, 0x9a, (s->de_end>>8) & 0xf);
-		sdp_io_write(sd, 0x9b, s->de_end & 0xff);
-	}
-
 	/* todo, improve settings for sdram */
 	if (pdata->sd_ram_size >= 128) {
 		sdp_write(sd, 0x12, 0x0d); /* Frame TBC,3D comb enabled */
@@ -2483,12 +2607,11 @@ static int adv7842_core_init(struct v4l2_subdev *sd,
 	io_write_and_or(sd, 0x20, 0xcf, 0x00);
 
 	/* LLC */
-	/* Set phase to 16. TODO: get this from platform_data */
-	io_write(sd, 0x19, 0x90);
+	io_write(sd, 0x19, 0x80 | pdata->llc_dll_phase);
 	io_write(sd, 0x33, 0x40);
 
 	/* interrupts */
-	io_write(sd, 0x40, 0xe2); /* Configure INT1 */
+	io_write(sd, 0x40, 0xf2); /* Configure INT1 */
 
 	adv7842_irq_enable(sd, true);
 
@@ -2588,6 +2711,7 @@ static int adv7842_command_ram_test(struct v4l2_subdev *sd)
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct adv7842_state *state = to_state(sd);
 	struct adv7842_platform_data *pdata = client->dev.platform_data;
+	struct v4l2_dv_timings timings;
 	int ret = 0;
 
 	if (!pdata)
@@ -2610,7 +2734,7 @@ static int adv7842_command_ram_test(struct v4l2_subdev *sd)
 	adv7842_rewrite_i2c_addresses(sd, pdata);
 
 	/* and re-init chip and state */
-	adv7842_core_init(sd, pdata);
+	adv7842_core_init(sd);
 
 	disable_input(sd);
 
@@ -2618,11 +2742,15 @@ static int adv7842_command_ram_test(struct v4l2_subdev *sd)
 
 	enable_input(sd);
 
-	adv7842_s_dv_timings(sd, &state->timings);
-
 	edid_write_vga_segment(sd);
-	edid_write_hdmi_segment(sd, 0);
-	edid_write_hdmi_segment(sd, 1);
+	edid_write_hdmi_segment(sd, ADV7842_EDID_PORT_A);
+	edid_write_hdmi_segment(sd, ADV7842_EDID_PORT_B);
+
+	timings = state->timings;
+
+	memset(&state->timings, 0, sizeof(struct v4l2_dv_timings));
+
+	adv7842_s_dv_timings(sd, &timings);
 
 	return ret;
 }
@@ -2670,6 +2798,7 @@ 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,
 };
 
@@ -2712,8 +2841,9 @@ static const struct v4l2_ctrl_config adv7842_ctrl_free_run_color = {
 };
 
 
-static void adv7842_unregister_clients(struct adv7842_state *state)
+static void adv7842_unregister_clients(struct v4l2_subdev *sd)
 {
+	struct adv7842_state *state = to_state(sd);
 	if (state->i2c_avlink)
 		i2c_unregister_device(state->i2c_avlink);
 	if (state->i2c_cec)
@@ -2736,21 +2866,79 @@ static void adv7842_unregister_clients(struct adv7842_state *state)
 		i2c_unregister_device(state->i2c_cp);
 	if (state->i2c_vdp)
 		i2c_unregister_device(state->i2c_vdp);
+
+	state->i2c_avlink = NULL;
+	state->i2c_cec = NULL;
+	state->i2c_infoframe = NULL;
+	state->i2c_sdp_io = NULL;
+	state->i2c_sdp = NULL;
+	state->i2c_afe = NULL;
+	state->i2c_repeater = NULL;
+	state->i2c_edid = NULL;
+	state->i2c_hdmi = NULL;
+	state->i2c_cp = NULL;
+	state->i2c_vdp = NULL;
 }
 
-static struct i2c_client *adv7842_dummy_client(struct v4l2_subdev *sd,
+static struct i2c_client *adv7842_dummy_client(struct v4l2_subdev *sd, const char *desc,
 					       u8 addr, u8 io_reg)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct i2c_client *cp;
 
 	io_write(sd, io_reg, addr << 1);
-	return i2c_new_dummy(client->adapter, io_read(sd, io_reg) >> 1);
+
+	if (addr == 0) {
+		v4l2_err(sd, "no %s i2c addr configured\n", desc);
+		return NULL;
+	}
+
+	cp = i2c_new_dummy(client->adapter, io_read(sd, io_reg) >> 1);
+	if (!cp)
+		v4l2_err(sd, "register %s on i2c addr 0x%x failed\n", desc, addr);
+
+	return cp;
+}
+
+static int adv7842_register_clients(struct v4l2_subdev *sd)
+{
+	struct adv7842_state *state = to_state(sd);
+	struct adv7842_platform_data *pdata = &state->pdata;
+
+	state->i2c_avlink = adv7842_dummy_client(sd, "avlink", pdata->i2c_avlink, 0xf3);
+	state->i2c_cec = adv7842_dummy_client(sd, "cec", pdata->i2c_cec, 0xf4);
+	state->i2c_infoframe = adv7842_dummy_client(sd, "infoframe", pdata->i2c_infoframe, 0xf5);
+	state->i2c_sdp_io = adv7842_dummy_client(sd, "sdp_io", pdata->i2c_sdp_io, 0xf2);
+	state->i2c_sdp = adv7842_dummy_client(sd, "sdp", pdata->i2c_sdp, 0xf1);
+	state->i2c_afe = adv7842_dummy_client(sd, "afe", pdata->i2c_afe, 0xf8);
+	state->i2c_repeater = adv7842_dummy_client(sd, "repeater", pdata->i2c_repeater, 0xf9);
+	state->i2c_edid = adv7842_dummy_client(sd, "edid", pdata->i2c_edid, 0xfa);
+	state->i2c_hdmi = adv7842_dummy_client(sd, "hdmi", pdata->i2c_hdmi, 0xfb);
+	state->i2c_cp = adv7842_dummy_client(sd, "cp", pdata->i2c_cp, 0xfd);
+	state->i2c_vdp = adv7842_dummy_client(sd, "vdp", pdata->i2c_vdp, 0xfe);
+
+	if (!state->i2c_avlink ||
+	    !state->i2c_cec ||
+	    !state->i2c_infoframe ||
+	    !state->i2c_sdp_io ||
+	    !state->i2c_sdp ||
+	    !state->i2c_afe ||
+	    !state->i2c_repeater ||
+	    !state->i2c_edid ||
+	    !state->i2c_hdmi ||
+	    !state->i2c_cp ||
+	    !state->i2c_vdp)
+		return -1;
+
+	return 0;
 }
 
 static int adv7842_probe(struct i2c_client *client,
 			 const struct i2c_device_id *id)
 {
 	struct adv7842_state *state;
+	static const struct v4l2_dv_timings cea640x480 =
+		V4L2_DV_BT_CEA_640X480P59_94;
 	struct adv7842_platform_data *pdata = client->dev.platform_data;
 	struct v4l2_ctrl_handler *hdl;
 	struct v4l2_subdev *sd;
@@ -2775,13 +2963,17 @@ static int adv7842_probe(struct i2c_client *client,
 		return -ENOMEM;
 	}
 
+	/* platform data */
+	state->pdata = *pdata;
+	state->timings = cea640x480;
+
 	sd = &state->sd;
 	v4l2_i2c_subdev_init(sd, client, &adv7842_ops);
 	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
-	state->connector_hdmi = pdata->connector_hdmi;
 	state->mode = pdata->mode;
 
-	state->hdmi_port_a = true;
+	state->hdmi_port_a = pdata->input == ADV7842_SELECT_HDMI_PORT_A;
+	state->restart_stdi_once = true;
 
 	/* i2c access to adv7842? */
 	rev = adv_smbus_read_byte_data_check(client, 0xea, false) << 8 |
@@ -2843,21 +3035,7 @@ static int adv7842_probe(struct i2c_client *client,
 		goto err_hdl;
 	}
 
-	state->i2c_avlink = adv7842_dummy_client(sd, pdata->i2c_avlink, 0xf3);
-	state->i2c_cec = adv7842_dummy_client(sd, pdata->i2c_cec, 0xf4);
-	state->i2c_infoframe = adv7842_dummy_client(sd, pdata->i2c_infoframe, 0xf5);
-	state->i2c_sdp_io = adv7842_dummy_client(sd, pdata->i2c_sdp_io, 0xf2);
-	state->i2c_sdp = adv7842_dummy_client(sd, pdata->i2c_sdp, 0xf1);
-	state->i2c_afe = adv7842_dummy_client(sd, pdata->i2c_afe, 0xf8);
-	state->i2c_repeater = adv7842_dummy_client(sd, pdata->i2c_repeater, 0xf9);
-	state->i2c_edid = adv7842_dummy_client(sd, pdata->i2c_edid, 0xfa);
-	state->i2c_hdmi = adv7842_dummy_client(sd, pdata->i2c_hdmi, 0xfb);
-	state->i2c_cp = adv7842_dummy_client(sd, pdata->i2c_cp, 0xfd);
-	state->i2c_vdp = adv7842_dummy_client(sd, pdata->i2c_vdp, 0xfe);
-	if (!state->i2c_avlink || !state->i2c_cec || !state->i2c_infoframe ||
-	    !state->i2c_sdp_io || !state->i2c_sdp || !state->i2c_afe ||
-	    !state->i2c_repeater || !state->i2c_edid || !state->i2c_hdmi ||
-	    !state->i2c_cp || !state->i2c_vdp) {
+	if (adv7842_register_clients(sd) < 0) {
 		err = -ENOMEM;
 		v4l2_err(sd, "failed to create all i2c clients\n");
 		goto err_i2c;
@@ -2879,7 +3057,7 @@ static int adv7842_probe(struct i2c_client *client,
 	if (err)
 		goto err_work_queues;
 
-	err = adv7842_core_init(sd, pdata);
+	err = adv7842_core_init(sd);
 	if (err)
 		goto err_entity;
 
@@ -2893,7 +3071,7 @@ err_work_queues:
 	cancel_delayed_work(&state->delayed_work_enable_hotplug);
 	destroy_workqueue(state->work_queues);
 err_i2c:
-	adv7842_unregister_clients(state);
+	adv7842_unregister_clients(sd);
 err_hdl:
 	v4l2_ctrl_handler_free(hdl);
 	return err;
@@ -2912,7 +3090,7 @@ static int adv7842_remove(struct i2c_client *client)
 	destroy_workqueue(state->work_queues);
 	v4l2_device_unregister_subdev(sd);
 	media_entity_cleanup(&sd->entity);
-	adv7842_unregister_clients(to_state(sd));
+	adv7842_unregister_clients(sd);
 	v4l2_ctrl_handler_free(sd->ctrl_handler);
 	return 0;
 }
diff --git a/drivers/media/i2c/lm3560.c b/drivers/media/i2c/lm3560.c
index 3317a9ae3961..d98ca3aebe23 100644
--- a/drivers/media/i2c/lm3560.c
+++ b/drivers/media/i2c/lm3560.c
@@ -172,28 +172,28 @@ static int lm3560_flash_brt_ctrl(struct lm3560_flash *flash,
 static int lm3560_get_ctrl(struct v4l2_ctrl *ctrl, enum lm3560_led_id led_no)
 {
 	struct lm3560_flash *flash = to_lm3560_flash(ctrl, led_no);
+	int rval = -EINVAL;
 
 	mutex_lock(&flash->lock);
 
 	if (ctrl->id == V4L2_CID_FLASH_FAULT) {
-		int rval;
 		s32 fault = 0;
 		unsigned int reg_val;
 		rval = regmap_read(flash->regmap, REG_FLAG, &reg_val);
 		if (rval < 0)
-			return rval;
-		if (rval & FAULT_SHORT_CIRCUIT)
+			goto out;
+		if (reg_val & FAULT_SHORT_CIRCUIT)
 			fault |= V4L2_FLASH_FAULT_SHORT_CIRCUIT;
-		if (rval & FAULT_OVERTEMP)
+		if (reg_val & FAULT_OVERTEMP)
 			fault |= V4L2_FLASH_FAULT_OVER_TEMPERATURE;
-		if (rval & FAULT_TIMEOUT)
+		if (reg_val & FAULT_TIMEOUT)
 			fault |= V4L2_FLASH_FAULT_TIMEOUT;
 		ctrl->cur.val = fault;
-		return 0;
 	}
 
+out:
 	mutex_unlock(&flash->lock);
-	return -EINVAL;
+	return rval;
 }
 
 static int lm3560_set_ctrl(struct v4l2_ctrl *ctrl, enum lm3560_led_id led_no)
@@ -219,15 +219,19 @@ static int lm3560_set_ctrl(struct v4l2_ctrl *ctrl, enum lm3560_led_id led_no)
 		break;
 
 	case V4L2_CID_FLASH_STROBE:
-		if (flash->led_mode != V4L2_FLASH_LED_MODE_FLASH)
-			return -EBUSY;
+		if (flash->led_mode != V4L2_FLASH_LED_MODE_FLASH) {
+			rval = -EBUSY;
+			goto err_out;
+		}
 		flash->led_mode = V4L2_FLASH_LED_MODE_FLASH;
 		rval = lm3560_mode_ctrl(flash);
 		break;
 
 	case V4L2_CID_FLASH_STROBE_STOP:
-		if (flash->led_mode != V4L2_FLASH_LED_MODE_FLASH)
-			return -EBUSY;
+		if (flash->led_mode != V4L2_FLASH_LED_MODE_FLASH) {
+			rval = -EBUSY;
+			goto err_out;
+		}
 		flash->led_mode = V4L2_FLASH_LED_MODE_NONE;
 		rval = lm3560_mode_ctrl(flash);
 		break;
@@ -247,8 +251,8 @@ static int lm3560_set_ctrl(struct v4l2_ctrl *ctrl, enum lm3560_led_id led_no)
 		break;
 	}
 
-	mutex_unlock(&flash->lock);
 err_out:
+	mutex_unlock(&flash->lock);
 	return rval;
 }
 
@@ -444,14 +448,14 @@ static int lm3560_probe(struct i2c_client *client,
 	if (rval < 0)
 		return rval;
 
+	i2c_set_clientdata(client, flash);
+
 	return 0;
 }
 
 static int lm3560_remove(struct i2c_client *client)
 {
-	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
-	struct lm3560_flash *flash = container_of(subdev, struct lm3560_flash,
-						  subdev_led[LM3560_LED_MAX]);
+	struct lm3560_flash *flash = i2c_get_clientdata(client);
 	unsigned int i;
 
 	for (i = LM3560_LED0; i < LM3560_LED_MAX; i++) {
diff --git a/drivers/media/i2c/mt9m032.c b/drivers/media/i2c/mt9m032.c
index 846b15f0bf64..85ec3bacdf1c 100644
--- a/drivers/media/i2c/mt9m032.c
+++ b/drivers/media/i2c/mt9m032.c
@@ -459,13 +459,15 @@ static int mt9m032_set_pad_crop(struct v4l2_subdev *subdev,
 			  MT9M032_COLUMN_START_MAX);
 	rect.top = clamp(ALIGN(crop->rect.top, 2), MT9M032_ROW_START_MIN,
 			 MT9M032_ROW_START_MAX);
-	rect.width = clamp(ALIGN(crop->rect.width, 2), MT9M032_COLUMN_SIZE_MIN,
-			   MT9M032_COLUMN_SIZE_MAX);
-	rect.height = clamp(ALIGN(crop->rect.height, 2), MT9M032_ROW_SIZE_MIN,
-			    MT9M032_ROW_SIZE_MAX);
-
-	rect.width = min(rect.width, MT9M032_PIXEL_ARRAY_WIDTH - rect.left);
-	rect.height = min(rect.height, MT9M032_PIXEL_ARRAY_HEIGHT - rect.top);
+	rect.width = clamp_t(unsigned int, ALIGN(crop->rect.width, 2),
+			     MT9M032_COLUMN_SIZE_MIN, MT9M032_COLUMN_SIZE_MAX);
+	rect.height = clamp_t(unsigned int, ALIGN(crop->rect.height, 2),
+			      MT9M032_ROW_SIZE_MIN, MT9M032_ROW_SIZE_MAX);
+
+	rect.width = min_t(unsigned int, rect.width,
+			   MT9M032_PIXEL_ARRAY_WIDTH - rect.left);
+	rect.height = min_t(unsigned int, rect.height,
+			    MT9M032_PIXEL_ARRAY_HEIGHT - rect.top);
 
 	__crop = __mt9m032_get_pad_crop(sensor, fh, crop->which);
 
diff --git a/drivers/media/i2c/mt9p031.c b/drivers/media/i2c/mt9p031.c
index 1c2303d18bf4..e5ddf47030fd 100644
--- a/drivers/media/i2c/mt9p031.c
+++ b/drivers/media/i2c/mt9p031.c
@@ -519,11 +519,13 @@ static int mt9p031_set_format(struct v4l2_subdev *subdev,
 
 	/* Clamp the width and height to avoid dividing by zero. */
 	width = clamp_t(unsigned int, ALIGN(format->format.width, 2),
-			max(__crop->width / 7, MT9P031_WINDOW_WIDTH_MIN),
+			max_t(unsigned int, __crop->width / 7,
+			      MT9P031_WINDOW_WIDTH_MIN),
 			__crop->width);
 	height = clamp_t(unsigned int, ALIGN(format->format.height, 2),
-			max(__crop->height / 8, MT9P031_WINDOW_HEIGHT_MIN),
-			__crop->height);
+			 max_t(unsigned int, __crop->height / 8,
+			       MT9P031_WINDOW_HEIGHT_MIN),
+			 __crop->height);
 
 	hratio = DIV_ROUND_CLOSEST(__crop->width, width);
 	vratio = DIV_ROUND_CLOSEST(__crop->height, height);
@@ -565,15 +567,17 @@ static int mt9p031_set_crop(struct v4l2_subdev *subdev,
 			  MT9P031_COLUMN_START_MAX);
 	rect.top = clamp(ALIGN(crop->rect.top, 2), MT9P031_ROW_START_MIN,
 			 MT9P031_ROW_START_MAX);
-	rect.width = clamp(ALIGN(crop->rect.width, 2),
-			   MT9P031_WINDOW_WIDTH_MIN,
-			   MT9P031_WINDOW_WIDTH_MAX);
-	rect.height = clamp(ALIGN(crop->rect.height, 2),
-			    MT9P031_WINDOW_HEIGHT_MIN,
-			    MT9P031_WINDOW_HEIGHT_MAX);
-
-	rect.width = min(rect.width, MT9P031_PIXEL_ARRAY_WIDTH - rect.left);
-	rect.height = min(rect.height, MT9P031_PIXEL_ARRAY_HEIGHT - rect.top);
+	rect.width = clamp_t(unsigned int, ALIGN(crop->rect.width, 2),
+			     MT9P031_WINDOW_WIDTH_MIN,
+			     MT9P031_WINDOW_WIDTH_MAX);
+	rect.height = clamp_t(unsigned int, ALIGN(crop->rect.height, 2),
+			      MT9P031_WINDOW_HEIGHT_MIN,
+			      MT9P031_WINDOW_HEIGHT_MAX);
+
+	rect.width = min_t(unsigned int, rect.width,
+			   MT9P031_PIXEL_ARRAY_WIDTH - rect.left);
+	rect.height = min_t(unsigned int, rect.height,
+			    MT9P031_PIXEL_ARRAY_HEIGHT - rect.top);
 
 	__crop = __mt9p031_get_pad_crop(mt9p031, fh, crop->pad, crop->which);
 
diff --git a/drivers/media/i2c/mt9t001.c b/drivers/media/i2c/mt9t001.c
index 796463466ef0..d41c70eaf838 100644
--- a/drivers/media/i2c/mt9t001.c
+++ b/drivers/media/i2c/mt9t001.c
@@ -291,10 +291,12 @@ static int mt9t001_set_format(struct v4l2_subdev *subdev,
 
 	/* Clamp the width and height to avoid dividing by zero. */
 	width = clamp_t(unsigned int, ALIGN(format->format.width, 2),
-			max(__crop->width / 8, MT9T001_WINDOW_HEIGHT_MIN + 1),
+			max_t(unsigned int, __crop->width / 8,
+			      MT9T001_WINDOW_HEIGHT_MIN + 1),
 			__crop->width);
 	height = clamp_t(unsigned int, ALIGN(format->format.height, 2),
-			 max(__crop->height / 8, MT9T001_WINDOW_HEIGHT_MIN + 1),
+			 max_t(unsigned int, __crop->height / 8,
+			       MT9T001_WINDOW_HEIGHT_MIN + 1),
 			 __crop->height);
 
 	hratio = DIV_ROUND_CLOSEST(__crop->width, width);
@@ -339,15 +341,17 @@ static int mt9t001_set_crop(struct v4l2_subdev *subdev,
 	rect.top = clamp(ALIGN(crop->rect.top, 2),
 			 MT9T001_ROW_START_MIN,
 			 MT9T001_ROW_START_MAX);
-	rect.width = clamp(ALIGN(crop->rect.width, 2),
-			   MT9T001_WINDOW_WIDTH_MIN + 1,
-			   MT9T001_WINDOW_WIDTH_MAX + 1);
-	rect.height = clamp(ALIGN(crop->rect.height, 2),
-			    MT9T001_WINDOW_HEIGHT_MIN + 1,
-			    MT9T001_WINDOW_HEIGHT_MAX + 1);
-
-	rect.width = min(rect.width, MT9T001_PIXEL_ARRAY_WIDTH - rect.left);
-	rect.height = min(rect.height, MT9T001_PIXEL_ARRAY_HEIGHT - rect.top);
+	rect.width = clamp_t(unsigned int, ALIGN(crop->rect.width, 2),
+			     MT9T001_WINDOW_WIDTH_MIN + 1,
+			     MT9T001_WINDOW_WIDTH_MAX + 1);
+	rect.height = clamp_t(unsigned int, ALIGN(crop->rect.height, 2),
+			      MT9T001_WINDOW_HEIGHT_MIN + 1,
+			      MT9T001_WINDOW_HEIGHT_MAX + 1);
+
+	rect.width = min_t(unsigned int, rect.width,
+			   MT9T001_PIXEL_ARRAY_WIDTH - rect.left);
+	rect.height = min_t(unsigned int, rect.height,
+			    MT9T001_PIXEL_ARRAY_HEIGHT - rect.top);
 
 	__crop = __mt9t001_get_pad_crop(mt9t001, fh, crop->pad, crop->which);
 
diff --git a/drivers/media/i2c/mt9v032.c b/drivers/media/i2c/mt9v032.c
index 2c50effaa334..36c504b78f2c 100644
--- a/drivers/media/i2c/mt9v032.c
+++ b/drivers/media/i2c/mt9v032.c
@@ -27,14 +27,16 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-subdev.h>
 
-#define MT9V032_PIXEL_ARRAY_HEIGHT			492
-#define MT9V032_PIXEL_ARRAY_WIDTH			782
+/* The first four rows are black rows. The active area spans 753x481 pixels. */
+#define MT9V032_PIXEL_ARRAY_HEIGHT			485
+#define MT9V032_PIXEL_ARRAY_WIDTH			753
 
 #define MT9V032_SYSCLK_FREQ_DEF				26600000
 
 #define MT9V032_CHIP_VERSION				0x00
 #define		MT9V032_CHIP_ID_REV1			0x1311
 #define		MT9V032_CHIP_ID_REV3			0x1313
+#define		MT9V034_CHIP_ID_REV1			0X1324
 #define MT9V032_COLUMN_START				0x01
 #define		MT9V032_COLUMN_START_MIN		1
 #define		MT9V032_COLUMN_START_DEF		1
@@ -53,12 +55,15 @@
 #define		MT9V032_WINDOW_WIDTH_MAX		752
 #define MT9V032_HORIZONTAL_BLANKING			0x05
 #define		MT9V032_HORIZONTAL_BLANKING_MIN		43
+#define		MT9V034_HORIZONTAL_BLANKING_MIN		61
 #define		MT9V032_HORIZONTAL_BLANKING_DEF		94
 #define		MT9V032_HORIZONTAL_BLANKING_MAX		1023
 #define MT9V032_VERTICAL_BLANKING			0x06
 #define		MT9V032_VERTICAL_BLANKING_MIN		4
+#define		MT9V034_VERTICAL_BLANKING_MIN		2
 #define		MT9V032_VERTICAL_BLANKING_DEF		45
 #define		MT9V032_VERTICAL_BLANKING_MAX		3000
+#define		MT9V034_VERTICAL_BLANKING_MAX		32288
 #define MT9V032_CHIP_CONTROL				0x07
 #define		MT9V032_CHIP_CONTROL_MASTER_MODE	(1 << 3)
 #define		MT9V032_CHIP_CONTROL_DOUT_ENABLE	(1 << 7)
@@ -68,8 +73,10 @@
 #define MT9V032_SHUTTER_WIDTH_CONTROL			0x0a
 #define MT9V032_TOTAL_SHUTTER_WIDTH			0x0b
 #define		MT9V032_TOTAL_SHUTTER_WIDTH_MIN		1
+#define		MT9V034_TOTAL_SHUTTER_WIDTH_MIN		0
 #define		MT9V032_TOTAL_SHUTTER_WIDTH_DEF		480
 #define		MT9V032_TOTAL_SHUTTER_WIDTH_MAX		32767
+#define		MT9V034_TOTAL_SHUTTER_WIDTH_MAX		32765
 #define MT9V032_RESET					0x0c
 #define MT9V032_READ_MODE				0x0d
 #define		MT9V032_READ_MODE_ROW_BIN_MASK		(3 << 0)
@@ -81,6 +88,8 @@
 #define		MT9V032_READ_MODE_DARK_COLUMNS		(1 << 6)
 #define		MT9V032_READ_MODE_DARK_ROWS		(1 << 7)
 #define MT9V032_PIXEL_OPERATION_MODE			0x0f
+#define		MT9V034_PIXEL_OPERATION_MODE_HDR	(1 << 0)
+#define		MT9V034_PIXEL_OPERATION_MODE_COLOR	(1 << 1)
 #define		MT9V032_PIXEL_OPERATION_MODE_COLOR	(1 << 2)
 #define		MT9V032_PIXEL_OPERATION_MODE_HDR	(1 << 6)
 #define MT9V032_ANALOG_GAIN				0x35
@@ -96,9 +105,12 @@
 #define		MT9V032_DARK_AVG_HIGH_THRESH_MASK	(255 << 8)
 #define		MT9V032_DARK_AVG_HIGH_THRESH_SHIFT	8
 #define MT9V032_ROW_NOISE_CORR_CONTROL			0x70
+#define		MT9V034_ROW_NOISE_CORR_ENABLE		(1 << 0)
+#define		MT9V034_ROW_NOISE_CORR_USE_BLK_AVG	(1 << 1)
 #define		MT9V032_ROW_NOISE_CORR_ENABLE		(1 << 5)
 #define		MT9V032_ROW_NOISE_CORR_USE_BLK_AVG	(1 << 7)
 #define MT9V032_PIXEL_CLOCK				0x74
+#define MT9V034_PIXEL_CLOCK				0x72
 #define		MT9V032_PIXEL_CLOCK_INV_LINE		(1 << 0)
 #define		MT9V032_PIXEL_CLOCK_INV_FRAME		(1 << 1)
 #define		MT9V032_PIXEL_CLOCK_XOR_LINE		(1 << 2)
@@ -120,12 +132,88 @@
 #define		MT9V032_AGC_ENABLE			(1 << 1)
 #define MT9V032_THERMAL_INFO				0xc1
 
+enum mt9v032_model {
+	MT9V032_MODEL_V032_COLOR,
+	MT9V032_MODEL_V032_MONO,
+	MT9V032_MODEL_V034_COLOR,
+	MT9V032_MODEL_V034_MONO,
+};
+
+struct mt9v032_model_version {
+	unsigned int version;
+	const char *name;
+};
+
+struct mt9v032_model_data {
+	unsigned int min_row_time;
+	unsigned int min_hblank;
+	unsigned int min_vblank;
+	unsigned int max_vblank;
+	unsigned int min_shutter;
+	unsigned int max_shutter;
+	unsigned int pclk_reg;
+};
+
+struct mt9v032_model_info {
+	const struct mt9v032_model_data *data;
+	bool color;
+};
+
+static const struct mt9v032_model_version mt9v032_versions[] = {
+	{ MT9V032_CHIP_ID_REV1, "MT9V032 rev1/2" },
+	{ MT9V032_CHIP_ID_REV3, "MT9V032 rev3" },
+	{ MT9V034_CHIP_ID_REV1, "MT9V034 rev1" },
+};
+
+static const struct mt9v032_model_data mt9v032_model_data[] = {
+	{
+		/* MT9V032 revisions 1/2/3 */
+		.min_row_time = 660,
+		.min_hblank = MT9V032_HORIZONTAL_BLANKING_MIN,
+		.min_vblank = MT9V032_VERTICAL_BLANKING_MIN,
+		.max_vblank = MT9V032_VERTICAL_BLANKING_MAX,
+		.min_shutter = MT9V032_TOTAL_SHUTTER_WIDTH_MIN,
+		.max_shutter = MT9V032_TOTAL_SHUTTER_WIDTH_MAX,
+		.pclk_reg = MT9V032_PIXEL_CLOCK,
+	}, {
+		/* MT9V034 */
+		.min_row_time = 690,
+		.min_hblank = MT9V034_HORIZONTAL_BLANKING_MIN,
+		.min_vblank = MT9V034_VERTICAL_BLANKING_MIN,
+		.max_vblank = MT9V034_VERTICAL_BLANKING_MAX,
+		.min_shutter = MT9V034_TOTAL_SHUTTER_WIDTH_MIN,
+		.max_shutter = MT9V034_TOTAL_SHUTTER_WIDTH_MAX,
+		.pclk_reg = MT9V034_PIXEL_CLOCK,
+	},
+};
+
+static const struct mt9v032_model_info mt9v032_models[] = {
+	[MT9V032_MODEL_V032_COLOR] = {
+		.data = &mt9v032_model_data[0],
+		.color = true,
+	},
+	[MT9V032_MODEL_V032_MONO] = {
+		.data = &mt9v032_model_data[0],
+		.color = false,
+	},
+	[MT9V032_MODEL_V034_COLOR] = {
+		.data = &mt9v032_model_data[1],
+		.color = true,
+	},
+	[MT9V032_MODEL_V034_MONO] = {
+		.data = &mt9v032_model_data[1],
+		.color = false,
+	},
+};
+
 struct mt9v032 {
 	struct v4l2_subdev subdev;
 	struct media_pad pad;
 
 	struct v4l2_mbus_framefmt format;
 	struct v4l2_rect crop;
+	unsigned int hratio;
+	unsigned int vratio;
 
 	struct v4l2_ctrl_handler ctrls;
 	struct {
@@ -139,6 +227,8 @@ struct mt9v032 {
 	struct clk *clk;
 
 	struct mt9v032_platform_data *pdata;
+	const struct mt9v032_model_info *model;
+	const struct mt9v032_model_version *version;
 
 	u32 sysclk;
 	u16 chip_control;
@@ -210,12 +300,17 @@ mt9v032_update_hblank(struct mt9v032 *mt9v032)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(&mt9v032->subdev);
 	struct v4l2_rect *crop = &mt9v032->crop;
+	unsigned int min_hblank = mt9v032->model->data->min_hblank;
+	unsigned int hblank;
 
-	return mt9v032_write(client, MT9V032_HORIZONTAL_BLANKING,
-			     max_t(s32, mt9v032->hblank, 660 - crop->width));
-}
+	if (mt9v032->version->version == MT9V034_CHIP_ID_REV1)
+		min_hblank += (mt9v032->hratio - 1) * 10;
+	min_hblank = max_t(unsigned int, (int)mt9v032->model->data->min_row_time - crop->width,
+			   (int)min_hblank);
+	hblank = max_t(unsigned int, mt9v032->hblank, min_hblank);
 
-#define EXT_CLK		25000000
+	return mt9v032_write(client, MT9V032_HORIZONTAL_BLANKING, hblank);
+}
 
 static int mt9v032_power_on(struct mt9v032 *mt9v032)
 {
@@ -259,7 +354,7 @@ static int __mt9v032_set_power(struct mt9v032 *mt9v032, bool on)
 
 	/* Configure the pixel clock polarity */
 	if (mt9v032->pdata && mt9v032->pdata->clk_pol) {
-		ret = mt9v032_write(client, MT9V032_PIXEL_CLOCK,
+		ret = mt9v032_write(client, mt9v032->model->data->pclk_reg,
 				MT9V032_PIXEL_CLOCK_INV_PXL_CLK);
 		if (ret < 0)
 			return ret;
@@ -312,22 +407,20 @@ static int mt9v032_s_stream(struct v4l2_subdev *subdev, int enable)
 		       | MT9V032_CHIP_CONTROL_SEQUENTIAL;
 	struct i2c_client *client = v4l2_get_subdevdata(subdev);
 	struct mt9v032 *mt9v032 = to_mt9v032(subdev);
-	struct v4l2_mbus_framefmt *format = &mt9v032->format;
 	struct v4l2_rect *crop = &mt9v032->crop;
-	unsigned int hratio;
-	unsigned int vratio;
+	unsigned int hbin;
+	unsigned int vbin;
 	int ret;
 
 	if (!enable)
 		return mt9v032_set_chip_control(mt9v032, mode, 0);
 
 	/* Configure the window size and row/column bin */
-	hratio = DIV_ROUND_CLOSEST(crop->width, format->width);
-	vratio = DIV_ROUND_CLOSEST(crop->height, format->height);
-
+	hbin = fls(mt9v032->hratio) - 1;
+	vbin = fls(mt9v032->vratio) - 1;
 	ret = mt9v032_write(client, MT9V032_READ_MODE,
-		    (hratio - 1) << MT9V032_READ_MODE_ROW_BIN_SHIFT |
-		    (vratio - 1) << MT9V032_READ_MODE_COLUMN_BIN_SHIFT);
+			    hbin << MT9V032_READ_MODE_COLUMN_BIN_SHIFT |
+			    vbin << MT9V032_READ_MODE_ROW_BIN_SHIFT);
 	if (ret < 0)
 		return ret;
 
@@ -370,12 +463,12 @@ static int mt9v032_enum_frame_size(struct v4l2_subdev *subdev,
 				   struct v4l2_subdev_fh *fh,
 				   struct v4l2_subdev_frame_size_enum *fse)
 {
-	if (fse->index >= 8 || fse->code != V4L2_MBUS_FMT_SGRBG10_1X10)
+	if (fse->index >= 3 || fse->code != V4L2_MBUS_FMT_SGRBG10_1X10)
 		return -EINVAL;
 
-	fse->min_width = MT9V032_WINDOW_WIDTH_DEF / fse->index;
+	fse->min_width = MT9V032_WINDOW_WIDTH_DEF / (1 << fse->index);
 	fse->max_width = fse->min_width;
-	fse->min_height = MT9V032_WINDOW_HEIGHT_DEF / fse->index;
+	fse->min_height = MT9V032_WINDOW_HEIGHT_DEF / (1 << fse->index);
 	fse->max_height = fse->min_height;
 
 	return 0;
@@ -392,18 +485,30 @@ static int mt9v032_get_format(struct v4l2_subdev *subdev,
 	return 0;
 }
 
-static void mt9v032_configure_pixel_rate(struct mt9v032 *mt9v032,
-					 unsigned int hratio)
+static void mt9v032_configure_pixel_rate(struct mt9v032 *mt9v032)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(&mt9v032->subdev);
 	int ret;
 
 	ret = v4l2_ctrl_s_ctrl_int64(mt9v032->pixel_rate,
-				     mt9v032->sysclk / hratio);
+				     mt9v032->sysclk / mt9v032->hratio);
 	if (ret < 0)
 		dev_warn(&client->dev, "failed to set pixel rate (%d)\n", ret);
 }
 
+static unsigned int mt9v032_calc_ratio(unsigned int input, unsigned int output)
+{
+	/* Compute the power-of-two binning factor closest to the input size to
+	 * output size ratio. Given that the output size is bounded by input/4
+	 * and input, a generic implementation would be an ineffective luxury.
+	 */
+	if (output * 3 > input * 2)
+		return 1;
+	if (output * 3 > input)
+		return 2;
+	return 4;
+}
+
 static int mt9v032_set_format(struct v4l2_subdev *subdev,
 			      struct v4l2_subdev_fh *fh,
 			      struct v4l2_subdev_format *format)
@@ -420,22 +525,28 @@ static int mt9v032_set_format(struct v4l2_subdev *subdev,
 					format->which);
 
 	/* Clamp the width and height to avoid dividing by zero. */
-	width = clamp_t(unsigned int, ALIGN(format->format.width, 2),
-			max(__crop->width / 8, MT9V032_WINDOW_WIDTH_MIN),
-			__crop->width);
-	height = clamp_t(unsigned int, ALIGN(format->format.height, 2),
-			 max(__crop->height / 8, MT9V032_WINDOW_HEIGHT_MIN),
-			 __crop->height);
-
-	hratio = DIV_ROUND_CLOSEST(__crop->width, width);
-	vratio = DIV_ROUND_CLOSEST(__crop->height, height);
+	width = clamp(ALIGN(format->format.width, 2),
+		      max_t(unsigned int, __crop->width / 4,
+			    MT9V032_WINDOW_WIDTH_MIN),
+		      __crop->width);
+	height = clamp(ALIGN(format->format.height, 2),
+		       max_t(unsigned int, __crop->height / 4,
+			     MT9V032_WINDOW_HEIGHT_MIN),
+		       __crop->height);
+
+	hratio = mt9v032_calc_ratio(__crop->width, width);
+	vratio = mt9v032_calc_ratio(__crop->height, height);
 
 	__format = __mt9v032_get_pad_format(mt9v032, fh, format->pad,
 					    format->which);
 	__format->width = __crop->width / hratio;
 	__format->height = __crop->height / vratio;
-	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
-		mt9v032_configure_pixel_rate(mt9v032, hratio);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		mt9v032->hratio = hratio;
+		mt9v032->vratio = vratio;
+		mt9v032_configure_pixel_rate(mt9v032);
+	}
 
 	format->format = *__format;
 
@@ -471,15 +582,17 @@ static int mt9v032_set_crop(struct v4l2_subdev *subdev,
 	rect.top = clamp(ALIGN(crop->rect.top + 1, 2) - 1,
 			 MT9V032_ROW_START_MIN,
 			 MT9V032_ROW_START_MAX);
-	rect.width = clamp(ALIGN(crop->rect.width, 2),
-			   MT9V032_WINDOW_WIDTH_MIN,
-			   MT9V032_WINDOW_WIDTH_MAX);
-	rect.height = clamp(ALIGN(crop->rect.height, 2),
-			    MT9V032_WINDOW_HEIGHT_MIN,
-			    MT9V032_WINDOW_HEIGHT_MAX);
-
-	rect.width = min(rect.width, MT9V032_PIXEL_ARRAY_WIDTH - rect.left);
-	rect.height = min(rect.height, MT9V032_PIXEL_ARRAY_HEIGHT - rect.top);
+	rect.width = clamp_t(unsigned int, ALIGN(crop->rect.width, 2),
+			     MT9V032_WINDOW_WIDTH_MIN,
+			     MT9V032_WINDOW_WIDTH_MAX);
+	rect.height = clamp_t(unsigned int, ALIGN(crop->rect.height, 2),
+			      MT9V032_WINDOW_HEIGHT_MIN,
+			      MT9V032_WINDOW_HEIGHT_MAX);
+
+	rect.width = min_t(unsigned int,
+			   rect.width, MT9V032_PIXEL_ARRAY_WIDTH - rect.left);
+	rect.height = min_t(unsigned int,
+			    rect.height, MT9V032_PIXEL_ARRAY_HEIGHT - rect.top);
 
 	__crop = __mt9v032_get_pad_crop(mt9v032, fh, crop->pad, crop->which);
 
@@ -491,8 +604,11 @@ static int mt9v032_set_crop(struct v4l2_subdev *subdev,
 						    crop->which);
 		__format->width = rect.width;
 		__format->height = rect.height;
-		if (crop->which == V4L2_SUBDEV_FORMAT_ACTIVE)
-			mt9v032_configure_pixel_rate(mt9v032, 1);
+		if (crop->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+			mt9v032->hratio = 1;
+			mt9v032->vratio = 1;
+			mt9v032_configure_pixel_rate(mt9v032);
+		}
 	}
 
 	*__crop = rect;
@@ -641,7 +757,8 @@ static int mt9v032_registered(struct v4l2_subdev *subdev)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(subdev);
 	struct mt9v032 *mt9v032 = to_mt9v032(subdev);
-	s32 data;
+	unsigned int i;
+	s32 version;
 	int ret;
 
 	dev_info(&client->dev, "Probing MT9V032 at address 0x%02x\n",
@@ -654,25 +771,38 @@ static int mt9v032_registered(struct v4l2_subdev *subdev)
 	}
 
 	/* Read and check the sensor version */
-	data = mt9v032_read(client, MT9V032_CHIP_VERSION);
-	if (data != MT9V032_CHIP_ID_REV1 && data != MT9V032_CHIP_ID_REV3) {
-		dev_err(&client->dev, "MT9V032 not detected, wrong version "
-				"0x%04x\n", data);
+	version = mt9v032_read(client, MT9V032_CHIP_VERSION);
+	if (version < 0) {
+		dev_err(&client->dev, "Failed reading chip version\n");
+		return version;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(mt9v032_versions); ++i) {
+		if (mt9v032_versions[i].version == version) {
+			mt9v032->version = &mt9v032_versions[i];
+			break;
+		}
+	}
+
+	if (mt9v032->version == NULL) {
+		dev_err(&client->dev, "Unsupported chip version 0x%04x\n",
+			version);
 		return -ENODEV;
 	}
 
 	mt9v032_power_off(mt9v032);
 
-	dev_info(&client->dev, "MT9V032 detected at address 0x%02x\n",
-			client->addr);
+	dev_info(&client->dev, "%s detected at address 0x%02x\n",
+		 mt9v032->version->name, client->addr);
 
-	mt9v032_configure_pixel_rate(mt9v032, 1);
+	mt9v032_configure_pixel_rate(mt9v032);
 
 	return ret;
 }
 
 static int mt9v032_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
 {
+	struct mt9v032 *mt9v032 = to_mt9v032(subdev);
 	struct v4l2_mbus_framefmt *format;
 	struct v4l2_rect *crop;
 
@@ -683,7 +813,12 @@ static int mt9v032_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
 	crop->height = MT9V032_WINDOW_HEIGHT_DEF;
 
 	format = v4l2_subdev_get_try_format(fh, 0);
-	format->code = V4L2_MBUS_FMT_SGRBG10_1X10;
+
+	if (mt9v032->model->color)
+		format->code = V4L2_MBUS_FMT_SGRBG10_1X10;
+	else
+		format->code = V4L2_MBUS_FMT_Y10_1X10;
+
 	format->width = MT9V032_WINDOW_WIDTH_DEF;
 	format->height = MT9V032_WINDOW_HEIGHT_DEF;
 	format->field = V4L2_FIELD_NONE;
@@ -755,6 +890,7 @@ static int mt9v032_probe(struct i2c_client *client,
 
 	mutex_init(&mt9v032->power_lock);
 	mt9v032->pdata = pdata;
+	mt9v032->model = (const void *)did->driver_data;
 
 	v4l2_ctrl_handler_init(&mt9v032->ctrls, 10);
 
@@ -767,16 +903,16 @@ static int mt9v032_probe(struct i2c_client *client,
 			       V4L2_CID_EXPOSURE_AUTO, V4L2_EXPOSURE_MANUAL, 0,
 			       V4L2_EXPOSURE_AUTO);
 	v4l2_ctrl_new_std(&mt9v032->ctrls, &mt9v032_ctrl_ops,
-			  V4L2_CID_EXPOSURE, MT9V032_TOTAL_SHUTTER_WIDTH_MIN,
-			  MT9V032_TOTAL_SHUTTER_WIDTH_MAX, 1,
+			  V4L2_CID_EXPOSURE, mt9v032->model->data->min_shutter,
+			  mt9v032->model->data->max_shutter, 1,
 			  MT9V032_TOTAL_SHUTTER_WIDTH_DEF);
 	v4l2_ctrl_new_std(&mt9v032->ctrls, &mt9v032_ctrl_ops,
-			  V4L2_CID_HBLANK, MT9V032_HORIZONTAL_BLANKING_MIN,
+			  V4L2_CID_HBLANK, mt9v032->model->data->min_hblank,
 			  MT9V032_HORIZONTAL_BLANKING_MAX, 1,
 			  MT9V032_HORIZONTAL_BLANKING_DEF);
 	v4l2_ctrl_new_std(&mt9v032->ctrls, &mt9v032_ctrl_ops,
-			  V4L2_CID_VBLANK, MT9V032_VERTICAL_BLANKING_MIN,
-			  MT9V032_VERTICAL_BLANKING_MAX, 1,
+			  V4L2_CID_VBLANK, mt9v032->model->data->min_vblank,
+			  mt9v032->model->data->max_vblank, 1,
 			  MT9V032_VERTICAL_BLANKING_DEF);
 	mt9v032->test_pattern = v4l2_ctrl_new_std_menu_items(&mt9v032->ctrls,
 				&mt9v032_ctrl_ops, V4L2_CID_TEST_PATTERN,
@@ -819,12 +955,19 @@ static int mt9v032_probe(struct i2c_client *client,
 	mt9v032->crop.width = MT9V032_WINDOW_WIDTH_DEF;
 	mt9v032->crop.height = MT9V032_WINDOW_HEIGHT_DEF;
 
-	mt9v032->format.code = V4L2_MBUS_FMT_SGRBG10_1X10;
+	if (mt9v032->model->color)
+		mt9v032->format.code = V4L2_MBUS_FMT_SGRBG10_1X10;
+	else
+		mt9v032->format.code = V4L2_MBUS_FMT_Y10_1X10;
+
 	mt9v032->format.width = MT9V032_WINDOW_WIDTH_DEF;
 	mt9v032->format.height = MT9V032_WINDOW_HEIGHT_DEF;
 	mt9v032->format.field = V4L2_FIELD_NONE;
 	mt9v032->format.colorspace = V4L2_COLORSPACE_SRGB;
 
+	mt9v032->hratio = 1;
+	mt9v032->vratio = 1;
+
 	mt9v032->aec_agc = MT9V032_AEC_ENABLE | MT9V032_AGC_ENABLE;
 	mt9v032->hblank = MT9V032_HORIZONTAL_BLANKING_DEF;
 	mt9v032->sysclk = MT9V032_SYSCLK_FREQ_DEF;
@@ -855,7 +998,10 @@ static int mt9v032_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id mt9v032_id[] = {
-	{ "mt9v032", 0 },
+	{ "mt9v032", (kernel_ulong_t)&mt9v032_models[MT9V032_MODEL_V032_COLOR] },
+	{ "mt9v032m", (kernel_ulong_t)&mt9v032_models[MT9V032_MODEL_V032_MONO] },
+	{ "mt9v034", (kernel_ulong_t)&mt9v032_models[MT9V032_MODEL_V034_COLOR] },
+	{ "mt9v034m", (kernel_ulong_t)&mt9v032_models[MT9V032_MODEL_V034_MONO] },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, mt9v032_id);
diff --git a/drivers/media/i2c/s5k5baf.c b/drivers/media/i2c/s5k5baf.c
new file mode 100644
index 000000000000..77e10e0fd8d6
--- /dev/null
+++ b/drivers/media/i2c/s5k5baf.c
@@ -0,0 +1,2053 @@
+/*
+ * Driver for Samsung S5K5BAF UXGA 1/5" 2M CMOS Image Sensor
+ * with embedded SoC ISP.
+ *
+ * Copyright (C) 2013, Samsung Electronics Co., Ltd.
+ * Andrzej Hajda <a.hajda@samsung.com>
+ *
+ * Based on S5K6AA driver authored by Sylwester Nawrocki
+ * Copyright (C) 2013, 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.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/firmware.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/media.h>
+#include <linux/module.h>
+#include <linux/of_gpio.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+
+#include <media/media-entity.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-mediabus.h>
+#include <media/v4l2-of.h>
+
+static int debug;
+module_param(debug, int, 0644);
+
+#define S5K5BAF_DRIVER_NAME		"s5k5baf"
+#define S5K5BAF_DEFAULT_MCLK_FREQ	24000000U
+#define S5K5BAF_CLK_NAME		"mclk"
+
+#define S5K5BAF_FW_FILENAME		"s5k5baf-cfg.bin"
+#define S5K5BAF_FW_TAG			"SF00"
+#define S5K5BAG_FW_TAG_LEN		2
+#define S5K5BAG_FW_MAX_COUNT		16
+
+#define S5K5BAF_CIS_WIDTH		1600
+#define S5K5BAF_CIS_HEIGHT		1200
+#define S5K5BAF_WIN_WIDTH_MIN		8
+#define S5K5BAF_WIN_HEIGHT_MIN		8
+#define S5K5BAF_GAIN_RED_DEF		127
+#define S5K5BAF_GAIN_GREEN_DEF		95
+#define S5K5BAF_GAIN_BLUE_DEF		180
+/* Default number of MIPI CSI-2 data lanes used */
+#define S5K5BAF_DEF_NUM_LANES		1
+
+#define AHB_MSB_ADDR_PTR		0xfcfc
+
+/*
+ * Register interface pages (the most significant word of the address)
+ */
+#define PAGE_IF_HW			0xd000
+#define PAGE_IF_SW			0x7000
+
+/*
+ * H/W register Interface (PAGE_IF_HW)
+ */
+#define REG_SW_LOAD_COMPLETE		0x0014
+#define REG_CMDWR_PAGE			0x0028
+#define REG_CMDWR_ADDR			0x002a
+#define REG_CMDRD_PAGE			0x002c
+#define REG_CMDRD_ADDR			0x002e
+#define REG_CMD_BUF			0x0f12
+#define REG_SET_HOST_INT		0x1000
+#define REG_CLEAR_HOST_INT		0x1030
+#define REG_PATTERN_SET			0x3100
+#define REG_PATTERN_WIDTH		0x3118
+#define REG_PATTERN_HEIGHT		0x311a
+#define REG_PATTERN_PARAM		0x311c
+
+/*
+ * S/W register interface (PAGE_IF_SW)
+ */
+
+/* Firmware revision information */
+#define REG_FW_APIVER			0x012e
+#define  S5K5BAF_FW_APIVER		0x0001
+#define REG_FW_REVISION			0x0130
+#define REG_FW_SENSOR_ID		0x0152
+
+/* Initialization parameters */
+/* Master clock frequency in KHz */
+#define REG_I_INCLK_FREQ_L		0x01b8
+#define REG_I_INCLK_FREQ_H		0x01ba
+#define  MIN_MCLK_FREQ_KHZ		6000U
+#define  MAX_MCLK_FREQ_KHZ		48000U
+#define REG_I_USE_NPVI_CLOCKS		0x01c6
+#define  NPVI_CLOCKS			1
+#define REG_I_USE_NMIPI_CLOCKS		0x01c8
+#define  NMIPI_CLOCKS			1
+#define REG_I_BLOCK_INTERNAL_PLL_CALC	0x01ca
+
+/* Clock configurations, n = 0..2. REG_I_* frequency unit is 4 kHz. */
+#define REG_I_OPCLK_4KHZ(n)		((n) * 6 + 0x01cc)
+#define REG_I_MIN_OUTRATE_4KHZ(n)	((n) * 6 + 0x01ce)
+#define REG_I_MAX_OUTRATE_4KHZ(n)	((n) * 6 + 0x01d0)
+#define  SCLK_PVI_FREQ			24000
+#define  SCLK_MIPI_FREQ			48000
+#define  PCLK_MIN_FREQ			6000
+#define  PCLK_MAX_FREQ			48000
+#define REG_I_USE_REGS_API		0x01de
+#define REG_I_INIT_PARAMS_UPDATED	0x01e0
+#define REG_I_ERROR_INFO		0x01e2
+
+/* General purpose parameters */
+#define REG_USER_BRIGHTNESS		0x01e4
+#define REG_USER_CONTRAST		0x01e6
+#define REG_USER_SATURATION		0x01e8
+#define REG_USER_SHARPBLUR		0x01ea
+
+#define REG_G_SPEC_EFFECTS		0x01ee
+#define REG_G_ENABLE_PREV		0x01f0
+#define REG_G_ENABLE_PREV_CHG		0x01f2
+#define REG_G_NEW_CFG_SYNC		0x01f8
+#define REG_G_PREVREQ_IN_WIDTH		0x01fa
+#define REG_G_PREVREQ_IN_HEIGHT		0x01fc
+#define REG_G_PREVREQ_IN_XOFFS		0x01fe
+#define REG_G_PREVREQ_IN_YOFFS		0x0200
+#define REG_G_PREVZOOM_IN_WIDTH		0x020a
+#define REG_G_PREVZOOM_IN_HEIGHT	0x020c
+#define REG_G_PREVZOOM_IN_XOFFS		0x020e
+#define REG_G_PREVZOOM_IN_YOFFS		0x0210
+#define REG_G_INPUTS_CHANGE_REQ		0x021a
+#define REG_G_ACTIVE_PREV_CFG		0x021c
+#define REG_G_PREV_CFG_CHG		0x021e
+#define REG_G_PREV_OPEN_AFTER_CH	0x0220
+#define REG_G_PREV_CFG_ERROR		0x0222
+#define  CFG_ERROR_RANGE		0x0b
+#define REG_G_PREV_CFG_BYPASS_CHANGED	0x022a
+#define REG_G_ACTUAL_P_FR_TIME		0x023a
+#define REG_G_ACTUAL_P_OUT_RATE		0x023c
+#define REG_G_ACTUAL_C_FR_TIME		0x023e
+#define REG_G_ACTUAL_C_OUT_RATE		0x0240
+
+/* Preview control section. n = 0...4. */
+#define PREG(n, x)			((n) * 0x26 + x)
+#define REG_P_OUT_WIDTH(n)		PREG(n, 0x0242)
+#define REG_P_OUT_HEIGHT(n)		PREG(n, 0x0244)
+#define REG_P_FMT(n)			PREG(n, 0x0246)
+#define REG_P_MAX_OUT_RATE(n)		PREG(n, 0x0248)
+#define REG_P_MIN_OUT_RATE(n)		PREG(n, 0x024a)
+#define REG_P_PVI_MASK(n)		PREG(n, 0x024c)
+#define  PVI_MASK_MIPI			0x52
+#define REG_P_CLK_INDEX(n)		PREG(n, 0x024e)
+#define  CLK_PVI_INDEX			0
+#define  CLK_MIPI_INDEX			NPVI_CLOCKS
+#define REG_P_FR_RATE_TYPE(n)		PREG(n, 0x0250)
+#define  FR_RATE_DYNAMIC		0
+#define  FR_RATE_FIXED			1
+#define  FR_RATE_FIXED_ACCURATE		2
+#define REG_P_FR_RATE_Q_TYPE(n)		PREG(n, 0x0252)
+#define  FR_RATE_Q_DYNAMIC		0
+#define  FR_RATE_Q_BEST_FRRATE		1 /* Binning enabled */
+#define  FR_RATE_Q_BEST_QUALITY		2 /* Binning disabled */
+/* Frame period in 0.1 ms units */
+#define REG_P_MAX_FR_TIME(n)		PREG(n, 0x0254)
+#define REG_P_MIN_FR_TIME(n)		PREG(n, 0x0256)
+#define  S5K5BAF_MIN_FR_TIME		333  /* x100 us */
+#define  S5K5BAF_MAX_FR_TIME		6500 /* x100 us */
+/* The below 5 registers are for "device correction" values */
+#define REG_P_SATURATION(n)		PREG(n, 0x0258)
+#define REG_P_SHARP_BLUR(n)		PREG(n, 0x025a)
+#define REG_P_GLAMOUR(n)		PREG(n, 0x025c)
+#define REG_P_COLORTEMP(n)		PREG(n, 0x025e)
+#define REG_P_GAMMA_INDEX(n)		PREG(n, 0x0260)
+#define REG_P_PREV_MIRROR(n)		PREG(n, 0x0262)
+#define REG_P_CAP_MIRROR(n)		PREG(n, 0x0264)
+#define REG_P_CAP_ROTATION(n)		PREG(n, 0x0266)
+
+/* Extended image property controls */
+/* Exposure time in 10 us units */
+#define REG_SF_USR_EXPOSURE_L		0x03bc
+#define REG_SF_USR_EXPOSURE_H		0x03be
+#define REG_SF_USR_EXPOSURE_CHG		0x03c0
+#define REG_SF_USR_TOT_GAIN		0x03c2
+#define REG_SF_USR_TOT_GAIN_CHG		0x03c4
+#define REG_SF_RGAIN			0x03c6
+#define REG_SF_RGAIN_CHG		0x03c8
+#define REG_SF_GGAIN			0x03ca
+#define REG_SF_GGAIN_CHG		0x03cc
+#define REG_SF_BGAIN			0x03ce
+#define REG_SF_BGAIN_CHG		0x03d0
+#define REG_SF_WBGAIN_CHG		0x03d2
+#define REG_SF_FLICKER_QUANT		0x03d4
+#define REG_SF_FLICKER_QUANT_CHG	0x03d6
+
+/* Output interface (parallel/MIPI) setup */
+#define REG_OIF_EN_MIPI_LANES		0x03f2
+#define REG_OIF_EN_PACKETS		0x03f4
+#define  EN_PACKETS_CSI2		0xc3
+#define REG_OIF_CFG_CHG			0x03f6
+
+/* Auto-algorithms enable mask */
+#define REG_DBG_AUTOALG_EN		0x03f8
+#define  AALG_ALL_EN			BIT(0)
+#define  AALG_AE_EN			BIT(1)
+#define  AALG_DIVLEI_EN			BIT(2)
+#define  AALG_WB_EN			BIT(3)
+#define  AALG_USE_WB_FOR_ISP		BIT(4)
+#define  AALG_FLICKER_EN		BIT(5)
+#define  AALG_FIT_EN			BIT(6)
+#define  AALG_WRHW_EN			BIT(7)
+
+/* Pointers to color correction matrices */
+#define REG_PTR_CCM_HORIZON		0x06d0
+#define REG_PTR_CCM_INCANDESCENT	0x06d4
+#define REG_PTR_CCM_WARM_WHITE		0x06d8
+#define REG_PTR_CCM_COOL_WHITE		0x06dc
+#define REG_PTR_CCM_DL50		0x06e0
+#define REG_PTR_CCM_DL65		0x06e4
+#define REG_PTR_CCM_OUTDOOR		0x06ec
+
+#define REG_ARR_CCM(n)			(0x2800 + 36 * (n))
+
+static const char * const s5k5baf_supply_names[] = {
+	"vdda",		/* Analog power supply 2.8V (2.6V to 3.0V) */
+	"vddreg",	/* Regulator input power supply 1.8V (1.7V to 1.9V)
+			   or 2.8V (2.6V to 3.0) */
+	"vddio",	/* I/O power supply 1.8V (1.65V to 1.95V)
+			   or 2.8V (2.5V to 3.1V) */
+};
+#define S5K5BAF_NUM_SUPPLIES ARRAY_SIZE(s5k5baf_supply_names)
+
+struct s5k5baf_gpio {
+	int gpio;
+	int level;
+};
+
+enum s5k5baf_gpio_id {
+	STBY,
+	RST,
+	NUM_GPIOS,
+};
+
+#define PAD_CIS 0
+#define PAD_OUT 1
+#define NUM_CIS_PADS 1
+#define NUM_ISP_PADS 2
+
+struct s5k5baf_pixfmt {
+	enum v4l2_mbus_pixelcode code;
+	u32 colorspace;
+	/* REG_P_FMT(x) register value */
+	u16 reg_p_fmt;
+};
+
+struct s5k5baf_ctrls {
+	struct v4l2_ctrl_handler handler;
+	struct { /* Auto / manual white balance cluster */
+		struct v4l2_ctrl *awb;
+		struct v4l2_ctrl *gain_red;
+		struct v4l2_ctrl *gain_blue;
+	};
+	struct { /* Mirror cluster */
+		struct v4l2_ctrl *hflip;
+		struct v4l2_ctrl *vflip;
+	};
+	struct { /* Auto exposure / manual exposure and gain cluster */
+		struct v4l2_ctrl *auto_exp;
+		struct v4l2_ctrl *exposure;
+		struct v4l2_ctrl *gain;
+	};
+};
+
+enum {
+	S5K5BAF_FW_ID_PATCH,
+	S5K5BAF_FW_ID_CCM,
+	S5K5BAF_FW_ID_CIS,
+};
+
+struct s5k5baf_fw {
+	u16 count;
+	struct {
+		u16 id;
+		u16 offset;
+	} seq[0];
+	u16 data[0];
+};
+
+struct s5k5baf {
+	struct s5k5baf_gpio gpios[NUM_GPIOS];
+	enum v4l2_mbus_type bus_type;
+	u8 nlanes;
+	struct regulator_bulk_data supplies[S5K5BAF_NUM_SUPPLIES];
+
+	struct clk *clock;
+	u32 mclk_frequency;
+
+	struct s5k5baf_fw *fw;
+
+	struct v4l2_subdev cis_sd;
+	struct media_pad cis_pad;
+
+	struct v4l2_subdev sd;
+	struct media_pad pads[NUM_ISP_PADS];
+
+	/* protects the struct members below */
+	struct mutex lock;
+
+	int error;
+
+	struct v4l2_rect crop_sink;
+	struct v4l2_rect compose;
+	struct v4l2_rect crop_source;
+	/* index to s5k5baf_formats array */
+	int pixfmt;
+	/* actual frame interval in 100us */
+	u16 fiv;
+	/* requested frame interval in 100us */
+	u16 req_fiv;
+	/* cache for REG_DBG_AUTOALG_EN register */
+	u16 auto_alg;
+
+	struct s5k5baf_ctrls ctrls;
+
+	unsigned int streaming:1;
+	unsigned int apply_cfg:1;
+	unsigned int apply_crop:1;
+	unsigned int valid_auto_alg:1;
+	unsigned int power;
+};
+
+static const struct s5k5baf_pixfmt s5k5baf_formats[] = {
+	{ V4L2_MBUS_FMT_VYUY8_2X8,	V4L2_COLORSPACE_JPEG,	5 },
+	/* range 16-240 */
+	{ V4L2_MBUS_FMT_VYUY8_2X8,	V4L2_COLORSPACE_REC709,	6 },
+	{ V4L2_MBUS_FMT_RGB565_2X8_BE,	V4L2_COLORSPACE_JPEG,	0 },
+};
+
+static struct v4l2_rect s5k5baf_cis_rect = {
+	0, 0, S5K5BAF_CIS_WIDTH, S5K5BAF_CIS_HEIGHT
+};
+
+/* Setfile contains set of I2C command sequences. Each sequence has its ID.
+ * setfile format:
+ *	u8 magic[4];
+ *	u16 count;		number of sequences
+ *	struct {
+ *		u16 id;		sequence id
+ *		u16 offset;	sequence offset in data array
+ *	} seq[count];
+ *	u16 data[*];		array containing sequences
+ *
+ */
+static int s5k5baf_fw_parse(struct device *dev, struct s5k5baf_fw **fw,
+			    size_t count, const u16 *data)
+{
+	struct s5k5baf_fw *f;
+	u16 *d, i, *end;
+	int ret;
+
+	if (count < S5K5BAG_FW_TAG_LEN + 1) {
+		dev_err(dev, "firmware file too short (%zu)\n", count);
+		return -EINVAL;
+	}
+
+	ret = memcmp(data, S5K5BAF_FW_TAG, S5K5BAG_FW_TAG_LEN * sizeof(u16));
+	if (ret != 0) {
+		dev_err(dev, "invalid firmware magic number\n");
+		return -EINVAL;
+	}
+
+	data += S5K5BAG_FW_TAG_LEN;
+	count -= S5K5BAG_FW_TAG_LEN;
+
+	d = devm_kzalloc(dev, count * sizeof(u16), GFP_KERNEL);
+
+	for (i = 0; i < count; ++i)
+		d[i] = le16_to_cpu(data[i]);
+
+	f = (struct s5k5baf_fw *)d;
+	if (count < 1 + 2 * f->count) {
+		dev_err(dev, "invalid firmware header (count=%d size=%zu)\n",
+			f->count, 2 * (count + S5K5BAG_FW_TAG_LEN));
+		return -EINVAL;
+	}
+	end = d + count;
+	d += 1 + 2 * f->count;
+
+	for (i = 0; i < f->count; ++i) {
+		if (f->seq[i].offset + d <= end)
+			continue;
+		dev_err(dev, "invalid firmware header (seq=%d)\n", i);
+		return -EINVAL;
+	}
+
+	*fw = f;
+
+	return 0;
+}
+
+static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct s5k5baf, ctrls.handler)->sd;
+}
+
+static inline bool s5k5baf_is_cis_subdev(struct v4l2_subdev *sd)
+{
+	return sd->entity.type == MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
+}
+
+static inline struct s5k5baf *to_s5k5baf(struct v4l2_subdev *sd)
+{
+	if (s5k5baf_is_cis_subdev(sd))
+		return container_of(sd, struct s5k5baf, cis_sd);
+	else
+		return container_of(sd, struct s5k5baf, sd);
+}
+
+static u16 s5k5baf_i2c_read(struct s5k5baf *state, u16 addr)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(&state->sd);
+	__be16 w, r;
+	struct i2c_msg msg[] = {
+		{ .addr = c->addr, .flags = 0,
+		  .len = 2, .buf = (u8 *)&w },
+		{ .addr = c->addr, .flags = I2C_M_RD,
+		  .len = 2, .buf = (u8 *)&r },
+	};
+	int ret;
+
+	if (state->error)
+		return 0;
+
+	w = cpu_to_be16(addr);
+	ret = i2c_transfer(c->adapter, msg, 2);
+	r = be16_to_cpu(r);
+
+	v4l2_dbg(3, debug, c, "i2c_read: 0x%04x : 0x%04x\n", addr, r);
+
+	if (ret != 2) {
+		v4l2_err(c, "i2c_read: error during transfer (%d)\n", ret);
+		state->error = ret;
+	}
+	return r;
+}
+
+static void s5k5baf_i2c_write(struct s5k5baf *state, u16 addr, u16 val)
+{
+	u8 buf[4] = { addr >> 8, addr & 0xFF, val >> 8, val & 0xFF };
+	struct i2c_client *c = v4l2_get_subdevdata(&state->sd);
+	int ret;
+
+	if (state->error)
+		return;
+
+	ret = i2c_master_send(c, buf, 4);
+	v4l2_dbg(3, debug, c, "i2c_write: 0x%04x : 0x%04x\n", addr, val);
+
+	if (ret != 4) {
+		v4l2_err(c, "i2c_write: error during transfer (%d)\n", ret);
+		state->error = ret;
+	}
+}
+
+static u16 s5k5baf_read(struct s5k5baf *state, u16 addr)
+{
+	s5k5baf_i2c_write(state, REG_CMDRD_ADDR, addr);
+	return s5k5baf_i2c_read(state, REG_CMD_BUF);
+}
+
+static void s5k5baf_write(struct s5k5baf *state, u16 addr, u16 val)
+{
+	s5k5baf_i2c_write(state, REG_CMDWR_ADDR, addr);
+	s5k5baf_i2c_write(state, REG_CMD_BUF, val);
+}
+
+static void s5k5baf_write_arr_seq(struct s5k5baf *state, u16 addr,
+				  u16 count, const u16 *seq)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(&state->sd);
+	__be16 buf[65];
+
+	s5k5baf_i2c_write(state, REG_CMDWR_ADDR, addr);
+	if (state->error)
+		return;
+
+	v4l2_dbg(3, debug, c, "i2c_write_seq(count=%d): %*ph\n", count,
+		 min(2 * count, 64), seq);
+
+	buf[0] = __constant_cpu_to_be16(REG_CMD_BUF);
+
+	while (count > 0) {
+		int n = min_t(int, count, ARRAY_SIZE(buf) - 1);
+		int ret, i;
+
+		for (i = 1; i <= n; ++i)
+			buf[i] = cpu_to_be16(*seq++);
+
+		i *= 2;
+		ret = i2c_master_send(c, (char *)buf, i);
+		if (ret != i) {
+			v4l2_err(c, "i2c_write_seq: error during transfer (%d)\n", ret);
+			state->error = ret;
+			break;
+		}
+
+		count -= n;
+	}
+}
+
+#define s5k5baf_write_seq(state, addr, seq...) \
+	s5k5baf_write_arr_seq(state, addr, sizeof((char[]){ seq }), \
+			      (const u16 []){ seq });
+
+/* add items count at the beginning of the list */
+#define NSEQ(seq...) sizeof((char[]){ seq }), seq
+
+/*
+ * s5k5baf_write_nseq() - Writes sequences of values to sensor memory via i2c
+ * @nseq: sequence of u16 words in format:
+ *	(N, address, value[1]...value[N-1])*,0
+ * Ex.:
+ *	u16 seq[] = { NSEQ(0x4000, 1, 1), NSEQ(0x4010, 640, 480), 0 };
+ *	ret = s5k5baf_write_nseq(c, seq);
+ */
+static void s5k5baf_write_nseq(struct s5k5baf *state, const u16 *nseq)
+{
+	int count;
+
+	while ((count = *nseq++)) {
+		u16 addr = *nseq++;
+		--count;
+
+		s5k5baf_write_arr_seq(state, addr, count, nseq);
+		nseq += count;
+	}
+}
+
+static void s5k5baf_synchronize(struct s5k5baf *state, int timeout, u16 addr)
+{
+	unsigned long end = jiffies + msecs_to_jiffies(timeout);
+	u16 reg;
+
+	s5k5baf_write(state, addr, 1);
+	do {
+		reg = s5k5baf_read(state, addr);
+		if (state->error || !reg)
+			return;
+		usleep_range(5000, 10000);
+	} while (time_is_after_jiffies(end));
+
+	v4l2_err(&state->sd, "timeout on register synchronize (%#x)\n", addr);
+	state->error = -ETIMEDOUT;
+}
+
+static u16 *s5k5baf_fw_get_seq(struct s5k5baf *state, u16 seq_id)
+{
+	struct s5k5baf_fw *fw = state->fw;
+	u16 *data;
+	int i;
+
+	if (fw == NULL)
+		return NULL;
+
+	data = fw->data + 2 * fw->count;
+
+	for (i = 0; i < fw->count; ++i) {
+		if (fw->seq[i].id == seq_id)
+			return data + fw->seq[i].offset;
+	}
+
+	return NULL;
+}
+
+static void s5k5baf_hw_patch(struct s5k5baf *state)
+{
+	u16 *seq = s5k5baf_fw_get_seq(state, S5K5BAF_FW_ID_PATCH);
+
+	if (seq)
+		s5k5baf_write_nseq(state, seq);
+}
+
+static void s5k5baf_hw_set_clocks(struct s5k5baf *state)
+{
+	unsigned long mclk = state->mclk_frequency / 1000;
+	u16 status;
+	static const u16 nseq_clk_cfg[] = {
+		NSEQ(REG_I_USE_NPVI_CLOCKS,
+		  NPVI_CLOCKS, NMIPI_CLOCKS, 0,
+		  SCLK_PVI_FREQ / 4, PCLK_MIN_FREQ / 4, PCLK_MAX_FREQ / 4,
+		  SCLK_MIPI_FREQ / 4, PCLK_MIN_FREQ / 4, PCLK_MAX_FREQ / 4),
+		NSEQ(REG_I_USE_REGS_API, 1),
+		0
+	};
+
+	s5k5baf_write_seq(state, REG_I_INCLK_FREQ_L, mclk & 0xffff, mclk >> 16);
+	s5k5baf_write_nseq(state, nseq_clk_cfg);
+
+	s5k5baf_synchronize(state, 250, REG_I_INIT_PARAMS_UPDATED);
+	status = s5k5baf_read(state, REG_I_ERROR_INFO);
+	if (!state->error && status) {
+		v4l2_err(&state->sd, "error configuring PLL (%d)\n", status);
+		state->error = -EINVAL;
+	}
+}
+
+/* set custom color correction matrices for various illuminations */
+static void s5k5baf_hw_set_ccm(struct s5k5baf *state)
+{
+	u16 *seq = s5k5baf_fw_get_seq(state, S5K5BAF_FW_ID_CCM);
+
+	if (seq)
+		s5k5baf_write_nseq(state, seq);
+}
+
+/* CIS sensor tuning, based on undocumented android driver code */
+static void s5k5baf_hw_set_cis(struct s5k5baf *state)
+{
+	u16 *seq = s5k5baf_fw_get_seq(state, S5K5BAF_FW_ID_CIS);
+
+	if (!seq)
+		return;
+
+	s5k5baf_i2c_write(state, REG_CMDWR_PAGE, PAGE_IF_HW);
+	s5k5baf_write_nseq(state, seq);
+	s5k5baf_i2c_write(state, REG_CMDWR_PAGE, PAGE_IF_SW);
+}
+
+static void s5k5baf_hw_sync_cfg(struct s5k5baf *state)
+{
+	s5k5baf_write(state, REG_G_PREV_CFG_CHG, 1);
+	if (state->apply_crop) {
+		s5k5baf_write(state, REG_G_INPUTS_CHANGE_REQ, 1);
+		s5k5baf_write(state, REG_G_PREV_CFG_BYPASS_CHANGED, 1);
+	}
+	s5k5baf_synchronize(state, 500, REG_G_NEW_CFG_SYNC);
+}
+/* Set horizontal and vertical image flipping */
+static void s5k5baf_hw_set_mirror(struct s5k5baf *state)
+{
+	u16 flip = state->ctrls.vflip->val | (state->ctrls.vflip->val << 1);
+
+	s5k5baf_write(state, REG_P_PREV_MIRROR(0), flip);
+	if (state->streaming)
+		s5k5baf_hw_sync_cfg(state);
+}
+
+static void s5k5baf_hw_set_alg(struct s5k5baf *state, u16 alg, bool enable)
+{
+	u16 cur_alg, new_alg;
+
+	if (!state->valid_auto_alg)
+		cur_alg = s5k5baf_read(state, REG_DBG_AUTOALG_EN);
+	else
+		cur_alg = state->auto_alg;
+
+	new_alg = enable ? (cur_alg | alg) : (cur_alg & ~alg);
+
+	if (new_alg != cur_alg)
+		s5k5baf_write(state, REG_DBG_AUTOALG_EN, new_alg);
+
+	if (state->error)
+		return;
+
+	state->valid_auto_alg = 1;
+	state->auto_alg = new_alg;
+}
+
+/* Configure auto/manual white balance and R/G/B gains */
+static void s5k5baf_hw_set_awb(struct s5k5baf *state, int awb)
+{
+	struct s5k5baf_ctrls *ctrls = &state->ctrls;
+
+	if (!awb)
+		s5k5baf_write_seq(state, REG_SF_RGAIN,
+				  ctrls->gain_red->val, 1,
+				  S5K5BAF_GAIN_GREEN_DEF, 1,
+				  ctrls->gain_blue->val, 1,
+				  1);
+
+	s5k5baf_hw_set_alg(state, AALG_WB_EN, awb);
+}
+
+/* Program FW with exposure time, 'exposure' in us units */
+static void s5k5baf_hw_set_user_exposure(struct s5k5baf *state, int exposure)
+{
+	unsigned int time = exposure / 10;
+
+	s5k5baf_write_seq(state, REG_SF_USR_EXPOSURE_L,
+			  time & 0xffff, time >> 16, 1);
+}
+
+static void s5k5baf_hw_set_user_gain(struct s5k5baf *state, int gain)
+{
+	s5k5baf_write_seq(state, REG_SF_USR_TOT_GAIN, gain, 1);
+}
+
+/* Set auto/manual exposure and total gain */
+static void s5k5baf_hw_set_auto_exposure(struct s5k5baf *state, int value)
+{
+	if (value == V4L2_EXPOSURE_AUTO) {
+		s5k5baf_hw_set_alg(state, AALG_AE_EN | AALG_DIVLEI_EN, true);
+	} else {
+		unsigned int exp_time = state->ctrls.exposure->val;
+
+		s5k5baf_hw_set_user_exposure(state, exp_time);
+		s5k5baf_hw_set_user_gain(state, state->ctrls.gain->val);
+		s5k5baf_hw_set_alg(state, AALG_AE_EN | AALG_DIVLEI_EN, false);
+	}
+}
+
+static void s5k5baf_hw_set_anti_flicker(struct s5k5baf *state, int v)
+{
+	if (v == V4L2_CID_POWER_LINE_FREQUENCY_AUTO) {
+		s5k5baf_hw_set_alg(state, AALG_FLICKER_EN, true);
+	} else {
+		/* The V4L2_CID_LINE_FREQUENCY control values match
+		 * the register values */
+		s5k5baf_write_seq(state, REG_SF_FLICKER_QUANT, v, 1);
+		s5k5baf_hw_set_alg(state, AALG_FLICKER_EN, false);
+	}
+}
+
+static void s5k5baf_hw_set_colorfx(struct s5k5baf *state, int val)
+{
+	static const u16 colorfx[] = {
+		[V4L2_COLORFX_NONE] = 0,
+		[V4L2_COLORFX_BW] = 1,
+		[V4L2_COLORFX_NEGATIVE] = 2,
+		[V4L2_COLORFX_SEPIA] = 3,
+		[V4L2_COLORFX_SKY_BLUE] = 4,
+		[V4L2_COLORFX_SKETCH] = 5,
+	};
+
+	s5k5baf_write(state, REG_G_SPEC_EFFECTS, colorfx[val]);
+}
+
+static int s5k5baf_find_pixfmt(struct v4l2_mbus_framefmt *mf)
+{
+	int i, c = -1;
+
+	for (i = 0; i < ARRAY_SIZE(s5k5baf_formats); i++) {
+		if (mf->colorspace != s5k5baf_formats[i].colorspace)
+			continue;
+		if (mf->code == s5k5baf_formats[i].code)
+			return i;
+		if (c < 0)
+			c = i;
+	}
+	return (c < 0) ? 0 : c;
+}
+
+static int s5k5baf_clear_error(struct s5k5baf *state)
+{
+	int ret = state->error;
+
+	state->error = 0;
+	return ret;
+}
+
+static int s5k5baf_hw_set_video_bus(struct s5k5baf *state)
+{
+	u16 en_pkts;
+
+	if (state->bus_type == V4L2_MBUS_CSI2)
+		en_pkts = EN_PACKETS_CSI2;
+	else
+		en_pkts = 0;
+
+	s5k5baf_write_seq(state, REG_OIF_EN_MIPI_LANES,
+			  state->nlanes, en_pkts, 1);
+
+	return s5k5baf_clear_error(state);
+}
+
+static u16 s5k5baf_get_cfg_error(struct s5k5baf *state)
+{
+	u16 err = s5k5baf_read(state, REG_G_PREV_CFG_ERROR);
+	if (err)
+		s5k5baf_write(state, REG_G_PREV_CFG_ERROR, 0);
+	return err;
+}
+
+static void s5k5baf_hw_set_fiv(struct s5k5baf *state, u16 fiv)
+{
+	s5k5baf_write(state, REG_P_MAX_FR_TIME(0), fiv);
+	s5k5baf_hw_sync_cfg(state);
+}
+
+static void s5k5baf_hw_find_min_fiv(struct s5k5baf *state)
+{
+	u16 err, fiv;
+	int n;
+
+	fiv = s5k5baf_read(state,  REG_G_ACTUAL_P_FR_TIME);
+	if (state->error)
+		return;
+
+	for (n = 5; n > 0; --n) {
+		s5k5baf_hw_set_fiv(state, fiv);
+		err = s5k5baf_get_cfg_error(state);
+		if (state->error)
+			return;
+		switch (err) {
+		case CFG_ERROR_RANGE:
+			++fiv;
+			break;
+		case 0:
+			state->fiv = fiv;
+			v4l2_info(&state->sd,
+				  "found valid frame interval: %d00us\n", fiv);
+			return;
+		default:
+			v4l2_err(&state->sd,
+				 "error setting frame interval: %d\n", err);
+			state->error = -EINVAL;
+		}
+	};
+	v4l2_err(&state->sd, "cannot find correct frame interval\n");
+	state->error = -ERANGE;
+}
+
+static void s5k5baf_hw_validate_cfg(struct s5k5baf *state)
+{
+	u16 err;
+
+	err = s5k5baf_get_cfg_error(state);
+	if (state->error)
+		return;
+
+	switch (err) {
+	case 0:
+		state->apply_cfg = 1;
+		return;
+	case CFG_ERROR_RANGE:
+		s5k5baf_hw_find_min_fiv(state);
+		if (!state->error)
+			state->apply_cfg = 1;
+		return;
+	default:
+		v4l2_err(&state->sd,
+			 "error setting format: %d\n", err);
+		state->error = -EINVAL;
+	}
+}
+
+static void s5k5baf_rescale(struct v4l2_rect *r, const struct v4l2_rect *v,
+			    const struct v4l2_rect *n,
+			    const struct v4l2_rect *d)
+{
+	r->left = v->left * n->width / d->width;
+	r->top = v->top * n->height / d->height;
+	r->width = v->width * n->width / d->width;
+	r->height = v->height * n->height / d->height;
+}
+
+static int s5k5baf_hw_set_crop_rects(struct s5k5baf *state)
+{
+	struct v4l2_rect *p, r;
+	u16 err;
+	int ret;
+
+	p = &state->crop_sink;
+	s5k5baf_write_seq(state, REG_G_PREVREQ_IN_WIDTH, p->width, p->height,
+			  p->left, p->top);
+
+	s5k5baf_rescale(&r, &state->crop_source, &state->crop_sink,
+			&state->compose);
+	s5k5baf_write_seq(state, REG_G_PREVZOOM_IN_WIDTH, r.width, r.height,
+			  r.left, r.top);
+
+	s5k5baf_synchronize(state, 500, REG_G_INPUTS_CHANGE_REQ);
+	s5k5baf_synchronize(state, 500, REG_G_PREV_CFG_BYPASS_CHANGED);
+	err = s5k5baf_get_cfg_error(state);
+	ret = s5k5baf_clear_error(state);
+	if (ret < 0)
+		return ret;
+
+	switch (err) {
+	case 0:
+		break;
+	case CFG_ERROR_RANGE:
+		/* retry crop with frame interval set to max */
+		s5k5baf_hw_set_fiv(state, S5K5BAF_MAX_FR_TIME);
+		err = s5k5baf_get_cfg_error(state);
+		ret = s5k5baf_clear_error(state);
+		if (ret < 0)
+			return ret;
+		if (err) {
+			v4l2_err(&state->sd,
+				 "crop error on max frame interval: %d\n", err);
+			state->error = -EINVAL;
+		}
+		s5k5baf_hw_set_fiv(state, state->req_fiv);
+		s5k5baf_hw_validate_cfg(state);
+		break;
+	default:
+		v4l2_err(&state->sd, "crop error: %d\n", err);
+		return -EINVAL;
+	}
+
+	if (!state->apply_cfg)
+		return 0;
+
+	p = &state->crop_source;
+	s5k5baf_write_seq(state, REG_P_OUT_WIDTH(0), p->width, p->height);
+	s5k5baf_hw_set_fiv(state, state->req_fiv);
+	s5k5baf_hw_validate_cfg(state);
+
+	return s5k5baf_clear_error(state);
+}
+
+static void s5k5baf_hw_set_config(struct s5k5baf *state)
+{
+	u16 reg_fmt = s5k5baf_formats[state->pixfmt].reg_p_fmt;
+	struct v4l2_rect *r = &state->crop_source;
+
+	s5k5baf_write_seq(state, REG_P_OUT_WIDTH(0),
+			  r->width, r->height, reg_fmt,
+			  PCLK_MAX_FREQ >> 2, PCLK_MIN_FREQ >> 2,
+			  PVI_MASK_MIPI, CLK_MIPI_INDEX,
+			  FR_RATE_FIXED, FR_RATE_Q_DYNAMIC,
+			  state->req_fiv, S5K5BAF_MIN_FR_TIME);
+	s5k5baf_hw_sync_cfg(state);
+	s5k5baf_hw_validate_cfg(state);
+}
+
+
+static void s5k5baf_hw_set_test_pattern(struct s5k5baf *state, int id)
+{
+	s5k5baf_i2c_write(state, REG_PATTERN_WIDTH, 800);
+	s5k5baf_i2c_write(state, REG_PATTERN_HEIGHT, 511);
+	s5k5baf_i2c_write(state, REG_PATTERN_PARAM, 0);
+	s5k5baf_i2c_write(state, REG_PATTERN_SET, id);
+}
+
+static void s5k5baf_gpio_assert(struct s5k5baf *state, int id)
+{
+	struct s5k5baf_gpio *gpio = &state->gpios[id];
+
+	gpio_set_value(gpio->gpio, gpio->level);
+}
+
+static void s5k5baf_gpio_deassert(struct s5k5baf *state, int id)
+{
+	struct s5k5baf_gpio *gpio = &state->gpios[id];
+
+	gpio_set_value(gpio->gpio, !gpio->level);
+}
+
+static int s5k5baf_power_on(struct s5k5baf *state)
+{
+	int ret;
+
+	ret = regulator_bulk_enable(S5K5BAF_NUM_SUPPLIES, state->supplies);
+	if (ret < 0)
+		goto err;
+
+	ret = clk_set_rate(state->clock, state->mclk_frequency);
+	if (ret < 0)
+		goto err_reg_dis;
+
+	ret = clk_prepare_enable(state->clock);
+	if (ret < 0)
+		goto err_reg_dis;
+
+	v4l2_dbg(1, debug, &state->sd, "clock frequency: %ld\n",
+		 clk_get_rate(state->clock));
+
+	s5k5baf_gpio_deassert(state, STBY);
+	usleep_range(50, 100);
+	s5k5baf_gpio_deassert(state, RST);
+	return 0;
+
+err_reg_dis:
+	regulator_bulk_disable(S5K5BAF_NUM_SUPPLIES, state->supplies);
+err:
+	v4l2_err(&state->sd, "%s() failed (%d)\n", __func__, ret);
+	return ret;
+}
+
+static int s5k5baf_power_off(struct s5k5baf *state)
+{
+	int ret;
+
+	state->streaming = 0;
+	state->apply_cfg = 0;
+	state->apply_crop = 0;
+
+	s5k5baf_gpio_assert(state, RST);
+	s5k5baf_gpio_assert(state, STBY);
+
+	if (!IS_ERR(state->clock))
+		clk_disable_unprepare(state->clock);
+
+	ret = regulator_bulk_disable(S5K5BAF_NUM_SUPPLIES,
+					state->supplies);
+	if (ret < 0)
+		v4l2_err(&state->sd, "failed to disable regulators\n");
+
+	return 0;
+}
+
+static void s5k5baf_hw_init(struct s5k5baf *state)
+{
+	s5k5baf_i2c_write(state, AHB_MSB_ADDR_PTR, PAGE_IF_HW);
+	s5k5baf_i2c_write(state, REG_CLEAR_HOST_INT, 0);
+	s5k5baf_i2c_write(state, REG_SW_LOAD_COMPLETE, 1);
+	s5k5baf_i2c_write(state, REG_CMDRD_PAGE, PAGE_IF_SW);
+	s5k5baf_i2c_write(state, REG_CMDWR_PAGE, PAGE_IF_SW);
+}
+
+/*
+ * V4L2 subdev core and video operations
+ */
+
+static void s5k5baf_initialize_data(struct s5k5baf *state)
+{
+	state->pixfmt = 0;
+	state->req_fiv = 10000 / 15;
+	state->fiv = state->req_fiv;
+	state->valid_auto_alg = 0;
+}
+
+static int s5k5baf_load_setfile(struct s5k5baf *state)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(&state->sd);
+	const struct firmware *fw;
+	int ret;
+
+	ret = request_firmware(&fw, S5K5BAF_FW_FILENAME, &c->dev);
+	if (ret < 0) {
+		dev_warn(&c->dev, "firmware file (%s) not loaded\n",
+			 S5K5BAF_FW_FILENAME);
+		return ret;
+	}
+
+	ret = s5k5baf_fw_parse(&c->dev, &state->fw, fw->size / 2,
+			       (u16 *)fw->data);
+
+	release_firmware(fw);
+
+	return ret;
+}
+
+static int s5k5baf_set_power(struct v4l2_subdev *sd, int on)
+{
+	struct s5k5baf *state = to_s5k5baf(sd);
+	int ret = 0;
+
+	mutex_lock(&state->lock);
+
+	if (!on != state->power)
+		goto out;
+
+	if (on) {
+		if (state->fw == NULL)
+			s5k5baf_load_setfile(state);
+
+		s5k5baf_initialize_data(state);
+		ret = s5k5baf_power_on(state);
+		if (ret < 0)
+			goto out;
+
+		s5k5baf_hw_init(state);
+		s5k5baf_hw_patch(state);
+		s5k5baf_i2c_write(state, REG_SET_HOST_INT, 1);
+		s5k5baf_hw_set_clocks(state);
+
+		ret = s5k5baf_hw_set_video_bus(state);
+		if (ret < 0)
+			goto out;
+
+		s5k5baf_hw_set_cis(state);
+		s5k5baf_hw_set_ccm(state);
+
+		ret = s5k5baf_clear_error(state);
+		if (!ret)
+			state->power++;
+	} else {
+		s5k5baf_power_off(state);
+		state->power--;
+	}
+
+out:
+	mutex_unlock(&state->lock);
+
+	if (!ret && on)
+		ret = v4l2_ctrl_handler_setup(&state->ctrls.handler);
+
+	return ret;
+}
+
+static void s5k5baf_hw_set_stream(struct s5k5baf *state, int enable)
+{
+	s5k5baf_write_seq(state, REG_G_ENABLE_PREV, enable, 1);
+}
+
+static int s5k5baf_s_stream(struct v4l2_subdev *sd, int on)
+{
+	struct s5k5baf *state = to_s5k5baf(sd);
+	int ret;
+
+	mutex_lock(&state->lock);
+
+	if (state->streaming == !!on) {
+		ret = 0;
+		goto out;
+	}
+
+	if (on) {
+		s5k5baf_hw_set_config(state);
+		ret = s5k5baf_hw_set_crop_rects(state);
+		if (ret < 0)
+			goto out;
+		s5k5baf_hw_set_stream(state, 1);
+		s5k5baf_i2c_write(state, 0xb0cc, 0x000b);
+	} else {
+		s5k5baf_hw_set_stream(state, 0);
+	}
+	ret = s5k5baf_clear_error(state);
+	if (!ret)
+		state->streaming = !state->streaming;
+
+out:
+	mutex_unlock(&state->lock);
+
+	return ret;
+}
+
+static int s5k5baf_g_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *fi)
+{
+	struct s5k5baf *state = to_s5k5baf(sd);
+
+	mutex_lock(&state->lock);
+	fi->interval.numerator = state->fiv;
+	fi->interval.denominator = 10000;
+	mutex_unlock(&state->lock);
+
+	return 0;
+}
+
+static void s5k5baf_set_frame_interval(struct s5k5baf *state,
+				       struct v4l2_subdev_frame_interval *fi)
+{
+	struct v4l2_fract *i = &fi->interval;
+
+	if (fi->interval.denominator == 0)
+		state->req_fiv = S5K5BAF_MAX_FR_TIME;
+	else
+		state->req_fiv = clamp_t(u32,
+					 i->numerator * 10000 / i->denominator,
+					 S5K5BAF_MIN_FR_TIME,
+					 S5K5BAF_MAX_FR_TIME);
+
+	state->fiv = state->req_fiv;
+	if (state->apply_cfg) {
+		s5k5baf_hw_set_fiv(state, state->req_fiv);
+		s5k5baf_hw_validate_cfg(state);
+	}
+	*i = (struct v4l2_fract){ state->fiv, 10000 };
+	if (state->fiv == state->req_fiv)
+		v4l2_info(&state->sd, "frame interval changed to %d00us\n",
+			  state->fiv);
+}
+
+static int s5k5baf_s_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *fi)
+{
+	struct s5k5baf *state = to_s5k5baf(sd);
+
+	mutex_lock(&state->lock);
+	s5k5baf_set_frame_interval(state, fi);
+	mutex_unlock(&state->lock);
+	return 0;
+}
+
+/*
+ * V4L2 subdev pad level and video operations
+ */
+static int s5k5baf_enum_frame_interval(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_fh *fh,
+			      struct v4l2_subdev_frame_interval_enum *fie)
+{
+	if (fie->index > S5K5BAF_MAX_FR_TIME - S5K5BAF_MIN_FR_TIME ||
+	    fie->pad != PAD_CIS)
+		return -EINVAL;
+
+	v4l_bound_align_image(&fie->width, S5K5BAF_WIN_WIDTH_MIN,
+			      S5K5BAF_CIS_WIDTH, 1,
+			      &fie->height, S5K5BAF_WIN_HEIGHT_MIN,
+			      S5K5BAF_CIS_HEIGHT, 1, 0);
+
+	fie->interval.numerator = S5K5BAF_MIN_FR_TIME + fie->index;
+	fie->interval.denominator = 10000;
+
+	return 0;
+}
+
+static int s5k5baf_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_fh *fh,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad == PAD_CIS) {
+		if (code->index > 0)
+			return -EINVAL;
+		code->code = V4L2_MBUS_FMT_FIXED;
+		return 0;
+	}
+
+	if (code->index >= ARRAY_SIZE(s5k5baf_formats))
+		return -EINVAL;
+
+	code->code = s5k5baf_formats[code->index].code;
+	return 0;
+}
+
+static int s5k5baf_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_fh *fh,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	int i;
+
+	if (fse->index > 0)
+		return -EINVAL;
+
+	if (fse->pad == PAD_CIS) {
+		fse->code = V4L2_MBUS_FMT_FIXED;
+		fse->min_width = S5K5BAF_CIS_WIDTH;
+		fse->max_width = S5K5BAF_CIS_WIDTH;
+		fse->min_height = S5K5BAF_CIS_HEIGHT;
+		fse->max_height = S5K5BAF_CIS_HEIGHT;
+		return 0;
+	}
+
+	i = ARRAY_SIZE(s5k5baf_formats);
+	while (--i)
+		if (fse->code == s5k5baf_formats[i].code)
+			break;
+	fse->code = s5k5baf_formats[i].code;
+	fse->min_width = S5K5BAF_WIN_WIDTH_MIN;
+	fse->max_width = S5K5BAF_CIS_WIDTH;
+	fse->max_height = S5K5BAF_WIN_HEIGHT_MIN;
+	fse->min_height = S5K5BAF_CIS_HEIGHT;
+
+	return 0;
+}
+
+static void s5k5baf_try_cis_format(struct v4l2_mbus_framefmt *mf)
+{
+	mf->width = S5K5BAF_CIS_WIDTH;
+	mf->height = S5K5BAF_CIS_HEIGHT;
+	mf->code = V4L2_MBUS_FMT_FIXED;
+	mf->colorspace = V4L2_COLORSPACE_JPEG;
+	mf->field = V4L2_FIELD_NONE;
+}
+
+static int s5k5baf_try_isp_format(struct v4l2_mbus_framefmt *mf)
+{
+	int pixfmt;
+
+	v4l_bound_align_image(&mf->width, S5K5BAF_WIN_WIDTH_MIN,
+			      S5K5BAF_CIS_WIDTH, 1,
+			      &mf->height, S5K5BAF_WIN_HEIGHT_MIN,
+			      S5K5BAF_CIS_HEIGHT, 1, 0);
+
+	pixfmt = s5k5baf_find_pixfmt(mf);
+
+	mf->colorspace = s5k5baf_formats[pixfmt].colorspace;
+	mf->code = s5k5baf_formats[pixfmt].code;
+	mf->field = V4L2_FIELD_NONE;
+
+	return pixfmt;
+}
+
+static int s5k5baf_get_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct s5k5baf *state = to_s5k5baf(sd);
+	const struct s5k5baf_pixfmt *pixfmt;
+	struct v4l2_mbus_framefmt *mf;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		mf = v4l2_subdev_get_try_format(fh, fmt->pad);
+		fmt->format = *mf;
+		return 0;
+	}
+
+	mf = &fmt->format;
+	if (fmt->pad == PAD_CIS) {
+		s5k5baf_try_cis_format(mf);
+		return 0;
+	}
+	mf->field = V4L2_FIELD_NONE;
+	mutex_lock(&state->lock);
+	pixfmt = &s5k5baf_formats[state->pixfmt];
+	mf->width = state->crop_source.width;
+	mf->height = state->crop_source.height;
+	mf->code = pixfmt->code;
+	mf->colorspace = pixfmt->colorspace;
+	mutex_unlock(&state->lock);
+
+	return 0;
+}
+
+static int s5k5baf_set_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct v4l2_mbus_framefmt *mf = &fmt->format;
+	struct s5k5baf *state = to_s5k5baf(sd);
+	const struct s5k5baf_pixfmt *pixfmt;
+	int ret = 0;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		*v4l2_subdev_get_try_format(fh, fmt->pad) = *mf;
+		return 0;
+	}
+
+	if (fmt->pad == PAD_CIS) {
+		s5k5baf_try_cis_format(mf);
+		return 0;
+	}
+
+	mutex_lock(&state->lock);
+
+	if (state->streaming) {
+		mutex_unlock(&state->lock);
+		return -EBUSY;
+	}
+
+	state->pixfmt = s5k5baf_try_isp_format(mf);
+	pixfmt = &s5k5baf_formats[state->pixfmt];
+	mf->code = pixfmt->code;
+	mf->colorspace = pixfmt->colorspace;
+	mf->width = state->crop_source.width;
+	mf->height = state->crop_source.height;
+
+	mutex_unlock(&state->lock);
+	return ret;
+}
+
+enum selection_rect { R_CIS, R_CROP_SINK, R_COMPOSE, R_CROP_SOURCE, R_INVALID };
+
+static enum selection_rect s5k5baf_get_sel_rect(u32 pad, u32 target)
+{
+	switch (target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		return pad ? R_COMPOSE : R_CIS;
+	case V4L2_SEL_TGT_CROP:
+		return pad ? R_CROP_SOURCE : R_CROP_SINK;
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+		return pad ? R_INVALID : R_CROP_SINK;
+	case V4L2_SEL_TGT_COMPOSE:
+		return pad ? R_INVALID : R_COMPOSE;
+	default:
+		return R_INVALID;
+	}
+}
+
+static int s5k5baf_is_bound_target(u32 target)
+{
+	return target == V4L2_SEL_TGT_CROP_BOUNDS ||
+		target == V4L2_SEL_TGT_COMPOSE_BOUNDS;
+}
+
+static int s5k5baf_get_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_fh *fh,
+				 struct v4l2_subdev_selection *sel)
+{
+	static enum selection_rect rtype;
+	struct s5k5baf *state = to_s5k5baf(sd);
+
+	rtype = s5k5baf_get_sel_rect(sel->pad, sel->target);
+
+	switch (rtype) {
+	case R_INVALID:
+		return -EINVAL;
+	case R_CIS:
+		sel->r = s5k5baf_cis_rect;
+		return 0;
+	default:
+		break;
+	}
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
+		if (rtype == R_COMPOSE)
+			sel->r = *v4l2_subdev_get_try_compose(fh, sel->pad);
+		else
+			sel->r = *v4l2_subdev_get_try_crop(fh, sel->pad);
+		return 0;
+	}
+
+	mutex_lock(&state->lock);
+	switch (rtype) {
+	case R_CROP_SINK:
+		sel->r = state->crop_sink;
+		break;
+	case R_COMPOSE:
+		sel->r = state->compose;
+		break;
+	case R_CROP_SOURCE:
+		sel->r = state->crop_source;
+		break;
+	default:
+		break;
+	}
+	if (s5k5baf_is_bound_target(sel->target)) {
+		sel->r.left = 0;
+		sel->r.top = 0;
+	}
+	mutex_unlock(&state->lock);
+
+	return 0;
+}
+
+/* bounds range [start, start+len) to [0, max) and aligns to 2 */
+static void s5k5baf_bound_range(u32 *start, u32 *len, u32 max)
+{
+	if (*len > max)
+		*len = max;
+	if (*start + *len > max)
+		*start = max - *len;
+	*start &= ~1;
+	*len &= ~1;
+	if (*len < S5K5BAF_WIN_WIDTH_MIN)
+		*len = S5K5BAF_WIN_WIDTH_MIN;
+}
+
+static void s5k5baf_bound_rect(struct v4l2_rect *r, u32 width, u32 height)
+{
+	s5k5baf_bound_range(&r->left, &r->width, width);
+	s5k5baf_bound_range(&r->top, &r->height, height);
+}
+
+static void s5k5baf_set_rect_and_adjust(struct v4l2_rect **rects,
+					enum selection_rect first,
+					struct v4l2_rect *v)
+{
+	struct v4l2_rect *r, *br;
+	enum selection_rect i = first;
+
+	*rects[first] = *v;
+	do {
+		r = rects[i];
+		br = rects[i - 1];
+		s5k5baf_bound_rect(r, br->width, br->height);
+	} while (++i != R_INVALID);
+	*v = *rects[first];
+}
+
+static bool s5k5baf_cmp_rect(const struct v4l2_rect *r1,
+			     const struct v4l2_rect *r2)
+{
+	return !memcmp(r1, r2, sizeof(*r1));
+}
+
+static int s5k5baf_set_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_fh *fh,
+				 struct v4l2_subdev_selection *sel)
+{
+	static enum selection_rect rtype;
+	struct s5k5baf *state = to_s5k5baf(sd);
+	struct v4l2_rect **rects;
+	int ret = 0;
+
+	rtype = s5k5baf_get_sel_rect(sel->pad, sel->target);
+	if (rtype == R_INVALID || s5k5baf_is_bound_target(sel->target))
+		return -EINVAL;
+
+	/* allow only scaling on compose */
+	if (rtype == R_COMPOSE) {
+		sel->r.left = 0;
+		sel->r.top = 0;
+	}
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
+		rects = (struct v4l2_rect * []) {
+				&s5k5baf_cis_rect,
+				v4l2_subdev_get_try_crop(fh, PAD_CIS),
+				v4l2_subdev_get_try_compose(fh, PAD_CIS),
+				v4l2_subdev_get_try_crop(fh, PAD_OUT)
+			};
+		s5k5baf_set_rect_and_adjust(rects, rtype, &sel->r);
+		return 0;
+	}
+
+	rects = (struct v4l2_rect * []) {
+			&s5k5baf_cis_rect,
+			&state->crop_sink,
+			&state->compose,
+			&state->crop_source
+		};
+	mutex_lock(&state->lock);
+	if (state->streaming) {
+		/* adjust sel->r to avoid output resolution change */
+		if (rtype < R_CROP_SOURCE) {
+			if (sel->r.width < state->crop_source.width)
+				sel->r.width = state->crop_source.width;
+			if (sel->r.height < state->crop_source.height)
+				sel->r.height = state->crop_source.height;
+		} else {
+			sel->r.width = state->crop_source.width;
+			sel->r.height = state->crop_source.height;
+		}
+	}
+	s5k5baf_set_rect_and_adjust(rects, rtype, &sel->r);
+	if (!s5k5baf_cmp_rect(&state->crop_sink, &s5k5baf_cis_rect) ||
+	    !s5k5baf_cmp_rect(&state->compose, &s5k5baf_cis_rect))
+		state->apply_crop = 1;
+	if (state->streaming)
+		ret = s5k5baf_hw_set_crop_rects(state);
+	mutex_unlock(&state->lock);
+
+	return ret;
+}
+
+static const struct v4l2_subdev_pad_ops s5k5baf_cis_pad_ops = {
+	.enum_mbus_code		= s5k5baf_enum_mbus_code,
+	.enum_frame_size	= s5k5baf_enum_frame_size,
+	.get_fmt		= s5k5baf_get_fmt,
+	.set_fmt		= s5k5baf_set_fmt,
+};
+
+static const struct v4l2_subdev_pad_ops s5k5baf_pad_ops = {
+	.enum_mbus_code		= s5k5baf_enum_mbus_code,
+	.enum_frame_size	= s5k5baf_enum_frame_size,
+	.enum_frame_interval	= s5k5baf_enum_frame_interval,
+	.get_fmt		= s5k5baf_get_fmt,
+	.set_fmt		= s5k5baf_set_fmt,
+	.get_selection		= s5k5baf_get_selection,
+	.set_selection		= s5k5baf_set_selection,
+};
+
+static const struct v4l2_subdev_video_ops s5k5baf_video_ops = {
+	.g_frame_interval	= s5k5baf_g_frame_interval,
+	.s_frame_interval	= s5k5baf_s_frame_interval,
+	.s_stream		= s5k5baf_s_stream,
+};
+
+/*
+ * V4L2 subdev controls
+ */
+
+static int s5k5baf_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+	struct s5k5baf *state = to_s5k5baf(sd);
+	int ret;
+
+	v4l2_dbg(1, debug, sd, "ctrl: %s, value: %d\n", ctrl->name, ctrl->val);
+
+	mutex_lock(&state->lock);
+
+	if (state->power == 0)
+		goto unlock;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		s5k5baf_hw_set_awb(state, ctrl->val);
+		break;
+
+	case V4L2_CID_BRIGHTNESS:
+		s5k5baf_write(state, REG_USER_BRIGHTNESS, ctrl->val);
+		break;
+
+	case V4L2_CID_COLORFX:
+		s5k5baf_hw_set_colorfx(state, ctrl->val);
+		break;
+
+	case V4L2_CID_CONTRAST:
+		s5k5baf_write(state, REG_USER_CONTRAST, ctrl->val);
+		break;
+
+	case V4L2_CID_EXPOSURE_AUTO:
+		s5k5baf_hw_set_auto_exposure(state, ctrl->val);
+		break;
+
+	case V4L2_CID_HFLIP:
+		s5k5baf_hw_set_mirror(state);
+		break;
+
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		s5k5baf_hw_set_anti_flicker(state, ctrl->val);
+		break;
+
+	case V4L2_CID_SATURATION:
+		s5k5baf_write(state, REG_USER_SATURATION, ctrl->val);
+		break;
+
+	case V4L2_CID_SHARPNESS:
+		s5k5baf_write(state, REG_USER_SHARPBLUR, ctrl->val);
+		break;
+
+	case V4L2_CID_WHITE_BALANCE_TEMPERATURE:
+		s5k5baf_write(state, REG_P_COLORTEMP(0), ctrl->val);
+		if (state->apply_cfg)
+			s5k5baf_hw_sync_cfg(state);
+		break;
+
+	case V4L2_CID_TEST_PATTERN:
+		s5k5baf_hw_set_test_pattern(state, ctrl->val);
+		break;
+	}
+unlock:
+	ret = s5k5baf_clear_error(state);
+	mutex_unlock(&state->lock);
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops s5k5baf_ctrl_ops = {
+	.s_ctrl	= s5k5baf_s_ctrl,
+};
+
+static const char * const s5k5baf_test_pattern_menu[] = {
+	"Disabled",
+	"Blank",
+	"Bars",
+	"Gradients",
+	"Textile",
+	"Textile2",
+	"Squares"
+};
+
+static int s5k5baf_initialize_ctrls(struct s5k5baf *state)
+{
+	const struct v4l2_ctrl_ops *ops = &s5k5baf_ctrl_ops;
+	struct s5k5baf_ctrls *ctrls = &state->ctrls;
+	struct v4l2_ctrl_handler *hdl = &ctrls->handler;
+	int ret;
+
+	ret = v4l2_ctrl_handler_init(hdl, 16);
+	if (ret < 0) {
+		v4l2_err(&state->sd, "cannot init ctrl handler (%d)\n", ret);
+		return ret;
+	}
+
+	/* Auto white balance cluster */
+	ctrls->awb = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTO_WHITE_BALANCE,
+				       0, 1, 1, 1);
+	ctrls->gain_red = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_RED_BALANCE,
+					    0, 255, 1, S5K5BAF_GAIN_RED_DEF);
+	ctrls->gain_blue = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BLUE_BALANCE,
+					     0, 255, 1, S5K5BAF_GAIN_BLUE_DEF);
+	v4l2_ctrl_auto_cluster(3, &ctrls->awb, 0, false);
+
+	ctrls->hflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
+	ctrls->vflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_cluster(2, &ctrls->hflip);
+
+	ctrls->auto_exp = v4l2_ctrl_new_std_menu(hdl, ops,
+				V4L2_CID_EXPOSURE_AUTO,
+				V4L2_EXPOSURE_MANUAL, 0, V4L2_EXPOSURE_AUTO);
+	/* Exposure time: x 1 us */
+	ctrls->exposure = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
+					    0, 6000000U, 1, 100000U);
+	/* Total gain: 256 <=> 1x */
+	ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAIN,
+					0, 256, 1, 256);
+	v4l2_ctrl_auto_cluster(3, &ctrls->auto_exp, 0, false);
+
+	v4l2_ctrl_new_std_menu(hdl, ops, V4L2_CID_POWER_LINE_FREQUENCY,
+			       V4L2_CID_POWER_LINE_FREQUENCY_AUTO, 0,
+			       V4L2_CID_POWER_LINE_FREQUENCY_AUTO);
+
+	v4l2_ctrl_new_std_menu(hdl, ops, V4L2_CID_COLORFX,
+			       V4L2_COLORFX_SKY_BLUE, ~0x6f, V4L2_COLORFX_NONE);
+
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_WHITE_BALANCE_TEMPERATURE,
+			  0, 256, 1, 0);
+
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SATURATION, -127, 127, 1, 0);
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BRIGHTNESS, -127, 127, 1, 0);
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_CONTRAST, -127, 127, 1, 0);
+	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SHARPNESS, -127, 127, 1, 0);
+
+	v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(s5k5baf_test_pattern_menu) - 1,
+				     0, 0, s5k5baf_test_pattern_menu);
+
+	if (hdl->error) {
+		v4l2_err(&state->sd, "error creating controls (%d)\n",
+			 hdl->error);
+		ret = hdl->error;
+		v4l2_ctrl_handler_free(hdl);
+		return ret;
+	}
+
+	state->sd.ctrl_handler = hdl;
+	return 0;
+}
+
+/*
+ * V4L2 subdev internal operations
+ */
+static int s5k5baf_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_mbus_framefmt *mf;
+
+	mf = v4l2_subdev_get_try_format(fh, PAD_CIS);
+	s5k5baf_try_cis_format(mf);
+
+	if (s5k5baf_is_cis_subdev(sd))
+		return 0;
+
+	mf = v4l2_subdev_get_try_format(fh, PAD_OUT);
+	mf->colorspace = s5k5baf_formats[0].colorspace;
+	mf->code = s5k5baf_formats[0].code;
+	mf->width = s5k5baf_cis_rect.width;
+	mf->height = s5k5baf_cis_rect.height;
+	mf->field = V4L2_FIELD_NONE;
+
+	*v4l2_subdev_get_try_crop(fh, PAD_CIS) = s5k5baf_cis_rect;
+	*v4l2_subdev_get_try_compose(fh, PAD_CIS) = s5k5baf_cis_rect;
+	*v4l2_subdev_get_try_crop(fh, PAD_OUT) = s5k5baf_cis_rect;
+
+	return 0;
+}
+
+static int s5k5baf_check_fw_revision(struct s5k5baf *state)
+{
+	u16 api_ver = 0, fw_rev = 0, s_id = 0;
+	int ret;
+
+	api_ver = s5k5baf_read(state, REG_FW_APIVER);
+	fw_rev = s5k5baf_read(state, REG_FW_REVISION) & 0xff;
+	s_id = s5k5baf_read(state, REG_FW_SENSOR_ID);
+	ret = s5k5baf_clear_error(state);
+	if (ret < 0)
+		return ret;
+
+	v4l2_info(&state->sd, "FW API=%#x, revision=%#x sensor_id=%#x\n",
+		  api_ver, fw_rev, s_id);
+
+	if (api_ver != S5K5BAF_FW_APIVER) {
+		v4l2_err(&state->sd, "FW API version not supported\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int s5k5baf_registered(struct v4l2_subdev *sd)
+{
+	struct s5k5baf *state = to_s5k5baf(sd);
+	int ret;
+
+	ret = v4l2_device_register_subdev(sd->v4l2_dev, &state->cis_sd);
+	if (ret < 0)
+		v4l2_err(sd, "failed to register subdev %s\n",
+			 state->cis_sd.name);
+	else
+		ret = media_entity_create_link(&state->cis_sd.entity, PAD_CIS,
+					       &state->sd.entity, PAD_CIS,
+					       MEDIA_LNK_FL_IMMUTABLE |
+					       MEDIA_LNK_FL_ENABLED);
+	return ret;
+}
+
+static void s5k5baf_unregistered(struct v4l2_subdev *sd)
+{
+	struct s5k5baf *state = to_s5k5baf(sd);
+	v4l2_device_unregister_subdev(&state->cis_sd);
+}
+
+static const struct v4l2_subdev_ops s5k5baf_cis_subdev_ops = {
+	.pad	= &s5k5baf_cis_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops s5k5baf_cis_subdev_internal_ops = {
+	.open = s5k5baf_open,
+};
+
+static const struct v4l2_subdev_internal_ops s5k5baf_subdev_internal_ops = {
+	.registered = s5k5baf_registered,
+	.unregistered = s5k5baf_unregistered,
+	.open = s5k5baf_open,
+};
+
+static const struct v4l2_subdev_core_ops s5k5baf_core_ops = {
+	.s_power = s5k5baf_set_power,
+	.log_status = v4l2_ctrl_subdev_log_status,
+};
+
+static const struct v4l2_subdev_ops s5k5baf_subdev_ops = {
+	.core = &s5k5baf_core_ops,
+	.pad = &s5k5baf_pad_ops,
+	.video = &s5k5baf_video_ops,
+};
+
+static int s5k5baf_configure_gpios(struct s5k5baf *state)
+{
+	static const char const *name[] = { "S5K5BAF_STBY", "S5K5BAF_RST" };
+	struct i2c_client *c = v4l2_get_subdevdata(&state->sd);
+	struct s5k5baf_gpio *g = state->gpios;
+	int ret, i;
+
+	for (i = 0; i < NUM_GPIOS; ++i) {
+		int flags = GPIOF_DIR_OUT;
+		if (g[i].level)
+			flags |= GPIOF_INIT_HIGH;
+		ret = devm_gpio_request_one(&c->dev, g[i].gpio, flags, name[i]);
+		if (ret < 0) {
+			v4l2_err(c, "failed to request gpio %s\n", name[i]);
+			return ret;
+		}
+	}
+	return 0;
+}
+
+static int s5k5baf_parse_gpios(struct s5k5baf_gpio *gpios, struct device *dev)
+{
+	static const char * const names[] = {
+		"stbyn-gpios",
+		"rstn-gpios",
+	};
+	struct device_node *node = dev->of_node;
+	enum of_gpio_flags flags;
+	int ret, i;
+
+	for (i = 0; i < NUM_GPIOS; ++i) {
+		ret = of_get_named_gpio_flags(node, names[i], 0, &flags);
+		if (ret < 0) {
+			dev_err(dev, "no %s GPIO pin provided\n", names[i]);
+			return ret;
+		}
+		gpios[i].gpio = ret;
+		gpios[i].level = !(flags & OF_GPIO_ACTIVE_LOW);
+	}
+
+	return 0;
+}
+
+static int s5k5baf_parse_device_node(struct s5k5baf *state, struct device *dev)
+{
+	struct device_node *node = dev->of_node;
+	struct device_node *node_ep;
+	struct v4l2_of_endpoint ep;
+	int ret;
+
+	if (!node) {
+		dev_err(dev, "no device-tree node provided\n");
+		return -EINVAL;
+	}
+
+	ret = of_property_read_u32(node, "clock-frequency",
+				   &state->mclk_frequency);
+	if (ret < 0) {
+		state->mclk_frequency = S5K5BAF_DEFAULT_MCLK_FREQ;
+		dev_info(dev, "using default %u Hz clock frequency\n",
+			 state->mclk_frequency);
+	}
+
+	ret = s5k5baf_parse_gpios(state->gpios, dev);
+	if (ret < 0)
+		return ret;
+
+	node_ep = v4l2_of_get_next_endpoint(node, NULL);
+	if (!node_ep) {
+		dev_err(dev, "no endpoint defined at node %s\n",
+			node->full_name);
+		return -EINVAL;
+	}
+
+	v4l2_of_parse_endpoint(node_ep, &ep);
+	of_node_put(node_ep);
+	state->bus_type = ep.bus_type;
+
+	switch (state->bus_type) {
+	case V4L2_MBUS_CSI2:
+		state->nlanes = ep.bus.mipi_csi2.num_data_lanes;
+		break;
+	case V4L2_MBUS_PARALLEL:
+		break;
+	default:
+		dev_err(dev, "unsupported bus in endpoint defined at node %s\n",
+			node->full_name);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int s5k5baf_configure_subdevs(struct s5k5baf *state,
+				     struct i2c_client *c)
+{
+	struct v4l2_subdev *sd;
+	int ret;
+
+	sd = &state->cis_sd;
+	v4l2_subdev_init(sd, &s5k5baf_cis_subdev_ops);
+	sd->owner = THIS_MODULE;
+	v4l2_set_subdevdata(sd, state);
+	snprintf(sd->name, sizeof(sd->name), "S5K5BAF-CIS %d-%04x",
+		 i2c_adapter_id(c->adapter), c->addr);
+
+	sd->internal_ops = &s5k5baf_cis_subdev_internal_ops;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	state->cis_pad.flags = MEDIA_PAD_FL_SOURCE;
+	sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
+	ret = media_entity_init(&sd->entity, NUM_CIS_PADS, &state->cis_pad, 0);
+	if (ret < 0)
+		goto err;
+
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, c, &s5k5baf_subdev_ops);
+	snprintf(sd->name, sizeof(sd->name), "S5K5BAF-ISP %d-%04x",
+		 i2c_adapter_id(c->adapter), c->addr);
+
+	sd->internal_ops = &s5k5baf_subdev_internal_ops;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	state->pads[PAD_CIS].flags = MEDIA_PAD_FL_SINK;
+	state->pads[PAD_OUT].flags = MEDIA_PAD_FL_SOURCE;
+	sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV;
+	ret = media_entity_init(&sd->entity, NUM_ISP_PADS, state->pads, 0);
+
+	if (!ret)
+		return 0;
+
+	media_entity_cleanup(&state->cis_sd.entity);
+err:
+	dev_err(&c->dev, "cannot init media entity %s\n", sd->name);
+	return ret;
+}
+
+static int s5k5baf_configure_regulators(struct s5k5baf *state)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(&state->sd);
+	int ret;
+	int i;
+
+	for (i = 0; i < S5K5BAF_NUM_SUPPLIES; i++)
+		state->supplies[i].supply = s5k5baf_supply_names[i];
+
+	ret = devm_regulator_bulk_get(&c->dev, S5K5BAF_NUM_SUPPLIES,
+				      state->supplies);
+	if (ret < 0)
+		v4l2_err(c, "failed to get regulators\n");
+	return ret;
+}
+
+static int s5k5baf_probe(struct i2c_client *c,
+			const struct i2c_device_id *id)
+{
+	struct s5k5baf *state;
+	int ret;
+
+	state = devm_kzalloc(&c->dev, sizeof(*state), GFP_KERNEL);
+	if (!state)
+		return -ENOMEM;
+
+	mutex_init(&state->lock);
+	state->crop_sink = s5k5baf_cis_rect;
+	state->compose = s5k5baf_cis_rect;
+	state->crop_source = s5k5baf_cis_rect;
+
+	ret = s5k5baf_parse_device_node(state, &c->dev);
+	if (ret < 0)
+		return ret;
+
+	ret = s5k5baf_configure_subdevs(state, c);
+	if (ret < 0)
+		return ret;
+
+	ret = s5k5baf_configure_gpios(state);
+	if (ret < 0)
+		goto err_me;
+
+	ret = s5k5baf_configure_regulators(state);
+	if (ret < 0)
+		goto err_me;
+
+	state->clock = devm_clk_get(state->sd.dev, S5K5BAF_CLK_NAME);
+	if (IS_ERR(state->clock)) {
+		ret = -EPROBE_DEFER;
+		goto err_me;
+	}
+
+	ret = s5k5baf_power_on(state);
+	if (ret < 0) {
+		ret = -EPROBE_DEFER;
+		goto err_me;
+	}
+	s5k5baf_hw_init(state);
+	ret = s5k5baf_check_fw_revision(state);
+
+	s5k5baf_power_off(state);
+	if (ret < 0)
+		goto err_me;
+
+	ret = s5k5baf_initialize_ctrls(state);
+	if (ret < 0)
+		goto err_me;
+
+	ret = v4l2_async_register_subdev(&state->sd);
+	if (ret < 0)
+		goto err_ctrl;
+
+	return 0;
+
+err_ctrl:
+	v4l2_ctrl_handler_free(state->sd.ctrl_handler);
+err_me:
+	media_entity_cleanup(&state->sd.entity);
+	media_entity_cleanup(&state->cis_sd.entity);
+	return ret;
+}
+
+static int s5k5baf_remove(struct i2c_client *c)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(c);
+	struct s5k5baf *state = to_s5k5baf(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+	media_entity_cleanup(&sd->entity);
+
+	sd = &state->cis_sd;
+	v4l2_device_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+
+	return 0;
+}
+
+static const struct i2c_device_id s5k5baf_id[] = {
+	{ S5K5BAF_DRIVER_NAME, 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, s5k5baf_id);
+
+static const struct of_device_id s5k5baf_of_match[] = {
+	{ .compatible = "samsung,s5k5baf" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, s5k5baf_of_match);
+
+static struct i2c_driver s5k5baf_i2c_driver = {
+	.driver = {
+		.of_match_table = s5k5baf_of_match,
+		.name = S5K5BAF_DRIVER_NAME
+	},
+	.probe		= s5k5baf_probe,
+	.remove		= s5k5baf_remove,
+	.id_table	= s5k5baf_id,
+};
+
+module_i2c_driver(s5k5baf_i2c_driver);
+
+MODULE_DESCRIPTION("Samsung S5K5BAF(X) UXGA camera driver");
+MODULE_AUTHOR("Andrzej Hajda <a.hajda@samsung.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/saa6588.c b/drivers/media/i2c/saa6588.c
index 70bc72e795d0..2960b5a8362a 100644
--- a/drivers/media/i2c/saa6588.c
+++ b/drivers/media/i2c/saa6588.c
@@ -150,14 +150,14 @@ static inline struct saa6588 *to_saa6588(struct v4l2_subdev *sd)
 
 /* ---------------------------------------------------------------------- */
 
-static int block_to_user_buf(struct saa6588 *s, unsigned char __user *user_buf)
+static bool block_from_buf(struct saa6588 *s, unsigned char *buf)
 {
 	int i;
 
 	if (s->rd_index == s->wr_index) {
 		if (debug > 2)
 			dprintk(PREFIX "Read: buffer empty.\n");
-		return 0;
+		return false;
 	}
 
 	if (debug > 2) {
@@ -166,8 +166,7 @@ static int block_to_user_buf(struct saa6588 *s, unsigned char __user *user_buf)
 			dprintk("0x%02x ", s->buffer[i]);
 	}
 
-	if (copy_to_user(user_buf, &s->buffer[s->rd_index], 3))
-		return -EFAULT;
+	memcpy(buf, &s->buffer[s->rd_index], 3);
 
 	s->rd_index += 3;
 	if (s->rd_index >= s->buf_size)
@@ -177,22 +176,22 @@ static int block_to_user_buf(struct saa6588 *s, unsigned char __user *user_buf)
 	if (debug > 2)
 		dprintk("%d blocks total.\n", s->block_count);
 
-	return 1;
+	return true;
 }
 
 static void read_from_buf(struct saa6588 *s, struct saa6588_command *a)
 {
-	unsigned long flags;
-
 	unsigned char __user *buf_ptr = a->buffer;
-	unsigned int i;
+	unsigned char buf[3];
+	unsigned long flags;
 	unsigned int rd_blocks;
+	unsigned int i;
 
 	a->result = 0;
 	if (!a->buffer)
 		return;
 
-	while (!s->data_available_for_read) {
+	while (!a->nonblocking && !s->data_available_for_read) {
 		int ret = wait_event_interruptible(s->read_queue,
 					     s->data_available_for_read);
 		if (ret == -ERESTARTSYS) {
@@ -201,24 +200,31 @@ static void read_from_buf(struct saa6588 *s, struct saa6588_command *a)
 		}
 	}
 
-	spin_lock_irqsave(&s->lock, flags);
 	rd_blocks = a->block_count;
+	spin_lock_irqsave(&s->lock, flags);
 	if (rd_blocks > s->block_count)
 		rd_blocks = s->block_count;
+	spin_unlock_irqrestore(&s->lock, flags);
 
-	if (!rd_blocks) {
-		spin_unlock_irqrestore(&s->lock, flags);
+	if (!rd_blocks)
 		return;
-	}
 
 	for (i = 0; i < rd_blocks; i++) {
-		if (block_to_user_buf(s, buf_ptr)) {
-			buf_ptr += 3;
-			a->result++;
-		} else
+		bool got_block;
+
+		spin_lock_irqsave(&s->lock, flags);
+		got_block = block_from_buf(s, buf);
+		spin_unlock_irqrestore(&s->lock, flags);
+		if (!got_block)
 			break;
+		if (copy_to_user(buf_ptr, buf, 3)) {
+			a->result = -EFAULT;
+			return;
+		}
+		buf_ptr += 3;
+		a->result += 3;
 	}
-	a->result *= 3;
+	spin_lock_irqsave(&s->lock, flags);
 	s->data_available_for_read = (s->block_count > 0);
 	spin_unlock_irqrestore(&s->lock, flags);
 }
@@ -394,14 +400,11 @@ static long saa6588_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
 	struct saa6588_command *a = arg;
 
 	switch (cmd) {
-		/* --- open() for /dev/radio --- */
-	case SAA6588_CMD_OPEN:
-		a->result = 0;	/* return error if chip doesn't work ??? */
-		break;
 		/* --- close() for /dev/radio --- */
 	case SAA6588_CMD_CLOSE:
 		s->data_available_for_read = 1;
 		wake_up_interruptible(&s->read_queue);
+		s->data_available_for_read = 0;
 		a->result = 0;
 		break;
 		/* --- read() for /dev/radio --- */
@@ -411,9 +414,8 @@ static long saa6588_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
 		/* --- poll() for /dev/radio --- */
 	case SAA6588_CMD_POLL:
 		a->result = 0;
-		if (s->data_available_for_read) {
+		if (s->data_available_for_read)
 			a->result |= POLLIN | POLLRDNORM;
-		}
 		poll_wait(a->instance, &s->read_queue, a->event_list);
 		break;
 
diff --git a/drivers/media/pci/saa7134/saa6752hs.c b/drivers/media/i2c/saa6752hs.c
index 8ac4b1f2322d..8272c0b9c5bf 100644
--- a/drivers/media/pci/saa7134/saa6752hs.c
+++ b/drivers/media/i2c/saa6752hs.c
@@ -33,11 +33,11 @@
 #include <linux/i2c.h>
 #include <linux/types.h>
 #include <linux/videodev2.h>
+#include <linux/init.h>
+#include <linux/crc32.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-common.h>
-#include <linux/init.h>
-#include <linux/crc32.h>
 
 #define MPEG_VIDEO_TARGET_BITRATE_MAX  27000
 #define MPEG_VIDEO_MAX_BITRATE_MAX     27000
@@ -124,7 +124,7 @@ static inline struct saa6752hs_state *to_state(struct v4l2_subdev *sd)
 
 /* ---------------------------------------------------------------------- */
 
-static u8 PAT[] = {
+static const u8 PAT[] = {
 	0xc2, /* i2c register */
 	0x00, /* table number for encoder */
 
@@ -150,7 +150,7 @@ static u8 PAT[] = {
 	0x00, 0x00, 0x00, 0x00 /* CRC32 */
 };
 
-static u8 PMT[] = {
+static const u8 PMT[] = {
 	0xc2, /* i2c register */
 	0x01, /* table number for encoder */
 
@@ -179,7 +179,7 @@ static u8 PMT[] = {
 	0x00, 0x00, 0x00, 0x00 /* CRC32 */
 };
 
-static u8 PMT_AC3[] = {
+static const u8 PMT_AC3[] = {
 	0xc2, /* i2c register */
 	0x01, /* table number for encoder(1) */
 	0x47, /* sync */
@@ -212,7 +212,7 @@ static u8 PMT_AC3[] = {
 	0xED, 0xDE, 0x2D, 0xF3 /* CRC32 BE */
 };
 
-static struct saa6752hs_mpeg_params param_defaults =
+static const struct saa6752hs_mpeg_params param_defaults =
 {
 	.ts_pid_pmt      = 16,
 	.ts_pid_video    = 260,
@@ -643,13 +643,6 @@ static const struct v4l2_ctrl_ops saa6752hs_ctrl_ops = {
 
 static const struct v4l2_subdev_core_ops saa6752hs_core_ops = {
 	.init = saa6752hs_init,
-	.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,
 	.s_std = saa6752hs_s_std,
 };
 
diff --git a/drivers/media/i2c/smiapp/smiapp-core.c b/drivers/media/i2c/smiapp/smiapp-core.c
index ae66d91bf713..8741cae9c9f2 100644
--- a/drivers/media/i2c/smiapp/smiapp-core.c
+++ b/drivers/media/i2c/smiapp/smiapp-core.c
@@ -399,7 +399,6 @@ static void smiapp_update_mbus_formats(struct smiapp_sensor *sensor)
 
 	BUG_ON(max(internal_csi_format_idx, csi_format_idx) + pixel_order
 	       >= ARRAY_SIZE(smiapp_csi_data_formats));
-	BUG_ON(min(internal_csi_format_idx, csi_format_idx) < 0);
 
 	dev_dbg(&client->dev, "new pixel order %s\n",
 		pixel_order_str[pixel_order]);
@@ -2028,8 +2027,8 @@ static int smiapp_set_crop(struct v4l2_subdev *subdev,
 	sel->r.width = min(sel->r.width, src_size->width);
 	sel->r.height = min(sel->r.height, src_size->height);
 
-	sel->r.left = min(sel->r.left, src_size->width - sel->r.width);
-	sel->r.top = min(sel->r.top, src_size->height - sel->r.height);
+	sel->r.left = min_t(int, sel->r.left, src_size->width - sel->r.width);
+	sel->r.top = min_t(int, sel->r.top, src_size->height - sel->r.height);
 
 	*crops[sel->pad] = sel->r;
 
@@ -2121,8 +2120,8 @@ static int smiapp_set_selection(struct v4l2_subdev *subdev,
 
 	sel->r.left = max(0, sel->r.left & ~1);
 	sel->r.top = max(0, sel->r.top & ~1);
-	sel->r.width = max(0, SMIAPP_ALIGN_DIM(sel->r.width, sel->flags));
-	sel->r.height = max(0, SMIAPP_ALIGN_DIM(sel->r.height, sel->flags));
+	sel->r.width = SMIAPP_ALIGN_DIM(sel->r.width, sel->flags);
+	sel->r.height =	SMIAPP_ALIGN_DIM(sel->r.height, sel->flags);
 
 	sel->r.width = max_t(unsigned int,
 			     sensor->limits[SMIAPP_LIMIT_MIN_X_OUTPUT_SIZE],
diff --git a/drivers/media/i2c/soc_camera/mt9m111.c b/drivers/media/i2c/soc_camera/mt9m111.c
index 6f4056668bbc..ccf59406a172 100644
--- a/drivers/media/i2c/soc_camera/mt9m111.c
+++ b/drivers/media/i2c/soc_camera/mt9m111.c
@@ -208,8 +208,8 @@ struct mt9m111 {
 	struct mt9m111_context *ctx;
 	struct v4l2_rect rect;	/* cropping rectangle */
 	struct v4l2_clk *clk;
-	int width;		/* output */
-	int height;		/* sizes */
+	unsigned int width;	/* output */
+	unsigned int height;	/* sizes */
 	struct mutex power_lock; /* lock to protect power_count */
 	int power_count;
 	const struct mt9m111_datafmt *fmt;
diff --git a/drivers/media/i2c/tvp5150.c b/drivers/media/i2c/tvp5150.c
index 89c0b13463b7..542d2528b3f9 100644
--- a/drivers/media/i2c/tvp5150.c
+++ b/drivers/media/i2c/tvp5150.c
@@ -58,21 +58,17 @@ static int tvp5150_read(struct v4l2_subdev *sd, unsigned char addr)
 	struct i2c_client *c = v4l2_get_subdevdata(sd);
 	unsigned char buffer[1];
 	int rc;
-
-	buffer[0] = addr;
-
-	rc = i2c_master_send(c, buffer, 1);
-	if (rc < 0) {
-		v4l2_err(sd, "i2c i/o error: rc == %d (should be 1)\n", rc);
-		return rc;
-	}
-
-	msleep(10);
-
-	rc = i2c_master_recv(c, buffer, 1);
-	if (rc < 0) {
-		v4l2_err(sd, "i2c i/o error: rc == %d (should be 1)\n", rc);
-		return rc;
+	struct i2c_msg msg[] = {
+		{ .addr = c->addr, .flags = 0,
+		  .buf = &addr, .len = 1 },
+		{ .addr = c->addr, .flags = I2C_M_RD,
+		  .buf = buffer, .len = 1 }
+	};
+
+	rc = i2c_transfer(c->adapter, msg, 2);
+	if (rc < 0 || rc != 2) {
+		v4l2_err(sd, "i2c i/o error: rc == %d (should be 2)\n", rc);
+		return rc < 0 ? rc : -EIO;
 	}
 
 	v4l2_dbg(2, debug, sd, "tvp5150: read 0x%02x = 0x%02x\n", addr, buffer[0]);
@@ -867,7 +863,7 @@ static int tvp5150_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
 	struct v4l2_rect rect = a->c;
 	struct tvp5150 *decoder = to_tvp5150(sd);
 	v4l2_std_id std;
-	int hmax;
+	unsigned int hmax;
 
 	v4l2_dbg(1, debug, sd, "%s left=%d, top=%d, width=%d, height=%d\n",
 		__func__, rect.left, rect.top, rect.width, rect.height);
@@ -877,9 +873,9 @@ static int tvp5150_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
 
 	/* tvp5150 has some special limits */
 	rect.left = clamp(rect.left, 0, TVP5150_MAX_CROP_LEFT);
-	rect.width = clamp(rect.width,
-			   TVP5150_H_MAX - TVP5150_MAX_CROP_LEFT - rect.left,
-			   TVP5150_H_MAX - rect.left);
+	rect.width = clamp_t(unsigned int, rect.width,
+			     TVP5150_H_MAX - TVP5150_MAX_CROP_LEFT - rect.left,
+			     TVP5150_H_MAX - rect.left);
 	rect.top = clamp(rect.top, 0, TVP5150_MAX_CROP_TOP);
 
 	/* Calculate height based on current standard */
@@ -893,9 +889,9 @@ static int tvp5150_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
 	else
 		hmax = TVP5150_V_MAX_OTHERS;
 
-	rect.height = clamp(rect.height,
-			    hmax - TVP5150_MAX_CROP_TOP - rect.top,
-			    hmax - rect.top);
+	rect.height = clamp_t(unsigned int, rect.height,
+			      hmax - TVP5150_MAX_CROP_TOP - rect.top,
+			      hmax - rect.top);
 
 	tvp5150_write(sd, TVP5150_VERT_BLANKING_START, rect.top);
 	tvp5150_write(sd, TVP5150_VERT_BLANKING_STOP,
diff --git a/drivers/media/i2c/vs6624.c b/drivers/media/i2c/vs6624.c
index 25bdd9312fea..23f4f65fccd7 100644
--- a/drivers/media/i2c/vs6624.c
+++ b/drivers/media/i2c/vs6624.c
@@ -503,6 +503,7 @@ static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
 	return &container_of(ctrl->handler, struct vs6624, hdl)->sd;
 }
 
+#ifdef CONFIG_VIDEO_ADV_DEBUG
 static int vs6624_read(struct v4l2_subdev *sd, u16 index)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
@@ -515,6 +516,7 @@ static int vs6624_read(struct v4l2_subdev *sd, u16 index)
 
 	return buf[0];
 }
+#endif
 
 static int vs6624_write(struct v4l2_subdev *sd, u16 index,
 				u8 value)
diff --git a/drivers/media/media-entity.c b/drivers/media/media-entity.c
index 2c286c307145..37c334edc7e8 100644
--- a/drivers/media/media-entity.c
+++ b/drivers/media/media-entity.c
@@ -235,6 +235,8 @@ __must_check int media_entity_pipeline_start(struct media_entity *entity,
 	media_entity_graph_walk_start(&graph, entity);
 
 	while ((entity = media_entity_graph_walk_next(&graph))) {
+		DECLARE_BITMAP(active, entity->num_pads);
+		DECLARE_BITMAP(has_no_links, entity->num_pads);
 		unsigned int i;
 
 		entity->stream_count++;
@@ -248,21 +250,46 @@ __must_check int media_entity_pipeline_start(struct media_entity *entity,
 		if (!entity->ops || !entity->ops->link_validate)
 			continue;
 
+		bitmap_zero(active, entity->num_pads);
+		bitmap_fill(has_no_links, entity->num_pads);
+
 		for (i = 0; i < entity->num_links; i++) {
 			struct media_link *link = &entity->links[i];
-
-			/* Is this pad part of an enabled link? */
-			if (!(link->flags & MEDIA_LNK_FL_ENABLED))
-				continue;
-
-			/* Are we the sink or not? */
-			if (link->sink->entity != entity)
+			struct media_pad *pad = link->sink->entity == entity
+						? link->sink : link->source;
+
+			/* Mark that a pad is connected by a link. */
+			bitmap_clear(has_no_links, pad->index, 1);
+
+			/*
+			 * Pads that either do not need to connect or
+			 * are connected through an enabled link are
+			 * fine.
+			 */
+			if (!(pad->flags & MEDIA_PAD_FL_MUST_CONNECT) ||
+			    link->flags & MEDIA_LNK_FL_ENABLED)
+				bitmap_set(active, pad->index, 1);
+
+			/*
+			 * Link validation will only take place for
+			 * sink ends of the link that are enabled.
+			 */
+			if (link->sink != pad ||
+			    !(link->flags & MEDIA_LNK_FL_ENABLED))
 				continue;
 
 			ret = entity->ops->link_validate(link);
 			if (ret < 0 && ret != -ENOIOCTLCMD)
 				goto error;
 		}
+
+		/* Either no links or validated links are fine. */
+		bitmap_or(active, active, has_no_links, entity->num_pads);
+
+		if (!bitmap_full(active, entity->num_pads)) {
+			ret = -EPIPE;
+			goto error;
+		}
 	}
 
 	mutex_unlock(&mdev->graph_mutex);
diff --git a/drivers/media/pci/bt8xx/bttv-cards.c b/drivers/media/pci/bt8xx/bttv-cards.c
index d85cb0ace4dc..6662b495b22c 100644
--- a/drivers/media/pci/bt8xx/bttv-cards.c
+++ b/drivers/media/pci/bt8xx/bttv-cards.c
@@ -2426,7 +2426,7 @@ struct tvcard bttv_tvcards[] = {
 	},
 		/* ---- card 0x87---------------------------------- */
 	[BTTV_BOARD_DVICO_FUSIONHDTV_5_LITE] = {
-		/* Michael Krufky <mkrufky@m1k.net> */
+		/* Michael Krufky <mkrufky@linuxtv.org> */
 		.name           = "DViCO FusionHDTV 5 Lite",
 		.tuner_type     = TUNER_LG_TDVS_H06XF, /* TDVS-H064F */
 		.tuner_addr	= ADDR_UNSET,
diff --git a/drivers/media/pci/bt8xx/bttv-driver.c b/drivers/media/pci/bt8xx/bttv-driver.c
index 92a06fd85865..afcd53bfcf8e 100644
--- a/drivers/media/pci/bt8xx/bttv-driver.c
+++ b/drivers/media/pci/bt8xx/bttv-driver.c
@@ -1126,9 +1126,9 @@ bttv_crop_calc_limits(struct bttv_crop *c)
 		c->min_scaled_height = 32;
 	} else {
 		c->min_scaled_width =
-			(max(48, c->rect.width >> 4) + 3) & ~3;
+			(max_t(unsigned int, 48, c->rect.width >> 4) + 3) & ~3;
 		c->min_scaled_height =
-			max(32, c->rect.height >> 4);
+			max_t(unsigned int, 32, c->rect.height >> 4);
 	}
 
 	c->max_scaled_width  = c->rect.width & ~3;
@@ -2024,7 +2024,7 @@ limit_scaled_size_lock       (struct bttv_fh *               fh,
 		/* We cannot scale up. When the scaled image is larger
 		   than crop.rect we adjust the crop.rect as required
 		   by the V4L2 spec, hence cropcap.bounds are our limit. */
-		max_width = min(b->width, (__s32) MAX_HACTIVE);
+		max_width = min_t(unsigned int, b->width, MAX_HACTIVE);
 		max_height = b->height;
 
 		/* We cannot capture the same line as video and VBI data.
@@ -3266,7 +3266,9 @@ static ssize_t radio_read(struct file *file, char __user *data,
 	struct bttv_fh *fh = file->private_data;
 	struct bttv *btv = fh->btv;
 	struct saa6588_command cmd;
-	cmd.block_count = count/3;
+
+	cmd.block_count = count / 3;
+	cmd.nonblocking = file->f_flags & O_NONBLOCK;
 	cmd.buffer = data;
 	cmd.instance = file;
 	cmd.result = -ENODEV;
diff --git a/drivers/media/pci/bt8xx/bttv-gpio.c b/drivers/media/pci/bt8xx/bttv-gpio.c
index 922e8233fd0b..3f364b7062b9 100644
--- a/drivers/media/pci/bt8xx/bttv-gpio.c
+++ b/drivers/media/pci/bt8xx/bttv-gpio.c
@@ -98,7 +98,7 @@ int bttv_sub_add_device(struct bttv_core *core, char *name)
 
 	err = device_register(&sub->dev);
 	if (0 != err) {
-		kfree(sub);
+		put_device(&sub->dev);
 		return err;
 	}
 	pr_info("%d: add subdevice \"%s\"\n", core->nr, dev_name(&sub->dev));
diff --git a/drivers/media/pci/cx18/cx18-driver.c b/drivers/media/pci/cx18/cx18-driver.c
index c1f8cc6f14b2..716bdc57fac6 100644
--- a/drivers/media/pci/cx18/cx18-driver.c
+++ b/drivers/media/pci/cx18/cx18-driver.c
@@ -327,13 +327,16 @@ void cx18_read_eeprom(struct cx18 *cx, struct tveeprom *tv)
 	struct i2c_client *c;
 	u8 eedata[256];
 
+	memset(tv, 0, sizeof(*tv));
+
 	c = kzalloc(sizeof(*c), GFP_KERNEL);
+	if (!c)
+		return;
 
 	strlcpy(c->name, "cx18 tveeprom tmp", sizeof(c->name));
 	c->adapter = &cx->i2c_adap[0];
 	c->addr = 0xa0 >> 1;
 
-	memset(tv, 0, sizeof(*tv));
 	if (tveeprom_read(c, eedata, sizeof(eedata)))
 		goto ret;
 
diff --git a/drivers/media/pci/cx25821/cx25821-alsa.c b/drivers/media/pci/cx25821/cx25821-alsa.c
index 6e91e84d6bf9..b1e08c3e55cd 100644
--- a/drivers/media/pci/cx25821/cx25821-alsa.c
+++ b/drivers/media/pci/cx25821/cx25821-alsa.c
@@ -618,7 +618,7 @@ static int snd_cx25821_pcm(struct cx25821_audio_dev *chip, int device,
  * Only boards with eeprom and byte 1 at eeprom=1 have it
  */
 
-static DEFINE_PCI_DEVICE_TABLE(cx25821_audio_pci_tbl) = {
+static const struct pci_device_id cx25821_audio_pci_tbl[] = {
 	{0x14f1, 0x0920, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
 	{0,}
 };
diff --git a/drivers/media/pci/cx25821/cx25821-core.c b/drivers/media/pci/cx25821/cx25821-core.c
index b762c5b2ca10..e81173c41e5a 100644
--- a/drivers/media/pci/cx25821/cx25821-core.c
+++ b/drivers/media/pci/cx25821/cx25821-core.c
@@ -1361,7 +1361,7 @@ static void cx25821_finidev(struct pci_dev *pci_dev)
 	kfree(dev);
 }
 
-static DEFINE_PCI_DEVICE_TABLE(cx25821_pci_tbl) = {
+static const struct pci_device_id cx25821_pci_tbl[] = {
 	{
 		/* CX25821 Athena */
 		.vendor = 0x14f1,
diff --git a/drivers/media/pci/cx88/cx88-alsa.c b/drivers/media/pci/cx88/cx88-alsa.c
index 400eb1c42d3f..d014206e7176 100644
--- a/drivers/media/pci/cx88/cx88-alsa.c
+++ b/drivers/media/pci/cx88/cx88-alsa.c
@@ -931,9 +931,9 @@ error:
  */
 static void cx88_audio_finidev(struct pci_dev *pci)
 {
-	struct cx88_audio_dev *card = pci_get_drvdata(pci);
+	struct snd_card *card = pci_get_drvdata(pci);
 
-	snd_card_free((void *)card);
+	snd_card_free(card);
 
 	devno--;
 }
diff --git a/drivers/media/pci/saa7134/Kconfig b/drivers/media/pci/saa7134/Kconfig
index 15b90d6e9130..7883393571e5 100644
--- a/drivers/media/pci/saa7134/Kconfig
+++ b/drivers/media/pci/saa7134/Kconfig
@@ -6,6 +6,7 @@ config VIDEO_SAA7134
 	select VIDEO_TVEEPROM
 	select CRC32
 	select VIDEO_SAA6588 if MEDIA_SUBDRV_AUTOSELECT
+	select VIDEO_SAA6752HS if MEDIA_SUBDRV_AUTOSELECT
 	---help---
 	  This is a video4linux driver for Philips SAA713x based
 	  TV cards.
diff --git a/drivers/media/pci/saa7134/Makefile b/drivers/media/pci/saa7134/Makefile
index 35375480ed4d..58de9b085689 100644
--- a/drivers/media/pci/saa7134/Makefile
+++ b/drivers/media/pci/saa7134/Makefile
@@ -4,7 +4,7 @@ saa7134-y +=	saa7134-ts.o saa7134-tvaudio.o saa7134-vbi.o
 saa7134-y +=	saa7134-video.o
 saa7134-$(CONFIG_VIDEO_SAA7134_RC) += saa7134-input.o
 
-obj-$(CONFIG_VIDEO_SAA7134) +=  saa6752hs.o saa7134.o saa7134-empress.o
+obj-$(CONFIG_VIDEO_SAA7134) +=  saa7134.o saa7134-empress.o
 
 obj-$(CONFIG_VIDEO_SAA7134_ALSA) += saa7134-alsa.o
 
diff --git a/drivers/media/pci/saa7134/saa7134-cards.c b/drivers/media/pci/saa7134/saa7134-cards.c
index d45e7f6ff332..c9b2350e92c8 100644
--- a/drivers/media/pci/saa7134/saa7134-cards.c
+++ b/drivers/media/pci/saa7134/saa7134-cards.c
@@ -2590,7 +2590,7 @@ struct saa7134_board saa7134_boards[] = {
 		}},
 	},
 	[SAA7134_BOARD_AVERMEDIA_AVERTVHD_A180] = {
-		/* Michael Krufky <mkrufky@m1k.net>
+		/* Michael Krufky <mkrufky@linuxtv.org>
 		 * Uses Alps Electric TDHU2, containing NXT2004 ATSC Decoder
 		 * AFAIK, there is no analog demod, thus,
 		 * no support for analog television.
diff --git a/drivers/media/pci/saa7134/saa7134-core.c b/drivers/media/pci/saa7134/saa7134-core.c
index 27d7ee709c58..1362b4aab473 100644
--- a/drivers/media/pci/saa7134/saa7134-core.c
+++ b/drivers/media/pci/saa7134/saa7134-core.c
@@ -751,6 +751,7 @@ static int saa7134_hwfini(struct saa7134_dev *dev)
 	saa7134_input_fini(dev);
 	saa7134_vbi_fini(dev);
 	saa7134_tvaudio_fini(dev);
+	saa7134_video_fini(dev);
 	return 0;
 }
 
@@ -802,7 +803,6 @@ static struct video_device *vdev_init(struct saa7134_dev *dev,
 	*vfd = *template;
 	vfd->v4l2_dev  = &dev->v4l2_dev;
 	vfd->release = video_device_release;
-	vfd->debug   = video_debug;
 	snprintf(vfd->name, sizeof(vfd->name), "%s %s (%s)",
 		 dev->name, type, saa7134_boards[dev->board].name);
 	set_bit(V4L2_FL_USE_FH_PRIO, &vfd->flags);
@@ -1008,13 +1008,13 @@ static int saa7134_initdev(struct pci_dev *pci_dev,
 
 	/* load i2c helpers */
 	if (card_is_empress(dev)) {
-		struct v4l2_subdev *sd =
+		dev->empress_sd =
 			v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap,
 				"saa6752hs",
 				saa7134_boards[dev->board].empress_addr, NULL);
 
-		if (sd)
-			sd->grp_id = GRP_EMPRESS;
+		if (dev->empress_sd)
+			dev->empress_sd->grp_id = GRP_EMPRESS;
 	}
 
 	if (saa7134_boards[dev->board].rds_addr) {
@@ -1046,6 +1046,7 @@ static int saa7134_initdev(struct pci_dev *pci_dev,
 		printk(KERN_INFO "%s: Overlay support disabled.\n", dev->name);
 
 	dev->video_dev = vdev_init(dev,&saa7134_video_template,"video");
+	dev->video_dev->ctrl_handler = &dev->ctrl_handler;
 	err = video_register_device(dev->video_dev,VFL_TYPE_GRABBER,
 				    video_nr[dev->nr]);
 	if (err < 0) {
@@ -1057,6 +1058,7 @@ static int saa7134_initdev(struct pci_dev *pci_dev,
 	       dev->name, video_device_node_name(dev->video_dev));
 
 	dev->vbi_dev = vdev_init(dev, &saa7134_video_template, "vbi");
+	dev->vbi_dev->ctrl_handler = &dev->ctrl_handler;
 
 	err = video_register_device(dev->vbi_dev,VFL_TYPE_VBI,
 				    vbi_nr[dev->nr]);
@@ -1067,6 +1069,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;
 		err = video_register_device(dev->radio_dev,VFL_TYPE_RADIO,
 					    radio_nr[dev->nr]);
 		if (err < 0)
diff --git a/drivers/media/pci/saa7134/saa7134-empress.c b/drivers/media/pci/saa7134/saa7134-empress.c
index 3022eb2a7925..0a9047e754b9 100644
--- a/drivers/media/pci/saa7134/saa7134-empress.c
+++ b/drivers/media/pci/saa7134/saa7134-empress.c
@@ -23,12 +23,12 @@
 #include <linux/kernel.h>
 #include <linux/delay.h>
 
+#include <media/v4l2-common.h>
+#include <media/v4l2-event.h>
+
 #include "saa7134-reg.h"
 #include "saa7134.h"
 
-#include <media/saa6752hs.h>
-#include <media/v4l2-common.h>
-
 /* ------------------------------------------------------------------ */
 
 MODULE_AUTHOR("Gerd Knorr <kraxel@bytesex.org> [SuSE Labs]");
@@ -85,52 +85,54 @@ static int ts_open(struct file *file)
 {
 	struct video_device *vdev = video_devdata(file);
 	struct saa7134_dev *dev = video_drvdata(file);
-	int err;
+	struct saa7134_fh *fh;
 
-	dprintk("open dev=%s\n", video_device_node_name(vdev));
-	err = -EBUSY;
-	if (!mutex_trylock(&dev->empress_tsq.vb_lock))
-		return err;
-	if (atomic_read(&dev->empress_users))
-		goto done;
+	/* 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));
 
-	atomic_inc(&dev->empress_users);
-	file->private_data = dev;
-	err = 0;
-
-done:
-	mutex_unlock(&dev->empress_tsq.vb_lock);
-	return err;
+	return 0;
 }
 
 static int ts_release(struct file *file)
 {
-	struct saa7134_dev *dev = file->private_data;
+	struct saa7134_dev *dev = video_drvdata(file);
+	struct saa7134_fh *fh = file->private_data;
 
-	videobuf_stop(&dev->empress_tsq);
-	videobuf_mmap_free(&dev->empress_tsq);
+	if (res_check(fh, RESOURCE_EMPRESS)) {
+		videobuf_stop(&dev->empress_tsq);
+		videobuf_mmap_free(&dev->empress_tsq);
 
-	/* stop the encoder */
-	ts_reset_encoder(dev);
+		/* stop the encoder */
+		ts_reset_encoder(dev);
 
-	/* Mute audio */
-	saa_writeb(SAA7134_AUDIO_MUTE_CTRL,
-		saa_readb(SAA7134_AUDIO_MUTE_CTRL) | (1 << 6));
-
-	atomic_dec(&dev->empress_users);
+		/* 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);
 	return 0;
 }
 
 static ssize_t
 ts_read(struct file *file, char __user *data, size_t count, loff_t *ppos)
 {
-	struct saa7134_dev *dev = file->private_data;
+	struct saa7134_dev *dev = video_drvdata(file);
 
+	if (res_locked(dev, RESOURCE_EMPRESS))
+		return -EBUSY;
 	if (!dev->empress_started)
 		ts_init_encoder(dev);
 
@@ -142,68 +144,27 @@ ts_read(struct file *file, char __user *data, size_t count, loff_t *ppos)
 static unsigned int
 ts_poll(struct file *file, struct poll_table_struct *wait)
 {
-	struct saa7134_dev *dev = file->private_data;
+	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;
 
-	return videobuf_poll_stream(file, &dev->empress_tsq, wait);
+	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);
 }
 
 
 static int
 ts_mmap(struct file *file, struct vm_area_struct * vma)
 {
-	struct saa7134_dev *dev = file->private_data;
+	struct saa7134_dev *dev = video_drvdata(file);
 
 	return videobuf_mmap_mapper(&dev->empress_tsq, vma);
 }
 
-/*
- * This function is _not_ called directly, but from
- * video_generic_ioctl (and maybe others).  userspace
- * copying is done already, arg is a kernel pointer.
- */
-
-static int empress_querycap(struct file *file, void  *priv,
-					struct v4l2_capability *cap)
-{
-	struct saa7134_dev *dev = file->private_data;
-
-	strcpy(cap->driver, "saa7134");
-	strlcpy(cap->card, saa7134_boards[dev->board].name,
-		sizeof(cap->card));
-	sprintf(cap->bus_info, "PCI:%s", pci_name(dev->pci));
-	cap->capabilities =
-		V4L2_CAP_VIDEO_CAPTURE |
-		V4L2_CAP_READWRITE |
-		V4L2_CAP_STREAMING;
-	return 0;
-}
-
-static int empress_enum_input(struct file *file, void *priv,
-					struct v4l2_input *i)
-{
-	if (i->index != 0)
-		return -EINVAL;
-
-	i->type = V4L2_INPUT_TYPE_CAMERA;
-	strcpy(i->name, "CCIR656");
-
-	return 0;
-}
-
-static int empress_g_input(struct file *file, void *priv, unsigned int *i)
-{
-	*i = 0;
-	return 0;
-}
-
-static int empress_s_input(struct file *file, void *priv, unsigned int i)
-{
-	if (i != 0)
-		return -EINVAL;
-
-	return 0;
-}
-
 static int empress_enum_fmt_vid_cap(struct file *file, void  *priv,
 					struct v4l2_fmtdesc *f)
 {
@@ -219,7 +180,7 @@ static int empress_enum_fmt_vid_cap(struct file *file, void  *priv,
 static int empress_g_fmt_vid_cap(struct file *file, void *priv,
 				struct v4l2_format *f)
 {
-	struct saa7134_dev *dev = file->private_data;
+	struct saa7134_dev *dev = video_drvdata(file);
 	struct v4l2_mbus_framefmt mbus_fmt;
 
 	saa_call_all(dev, video, g_mbus_fmt, &mbus_fmt);
@@ -236,7 +197,7 @@ static int empress_g_fmt_vid_cap(struct file *file, void *priv,
 static int empress_s_fmt_vid_cap(struct file *file, void *priv,
 				struct v4l2_format *f)
 {
-	struct saa7134_dev *dev = file->private_data;
+	struct saa7134_dev *dev = video_drvdata(file);
 	struct v4l2_mbus_framefmt mbus_fmt;
 
 	v4l2_fill_mbus_format(&mbus_fmt, &f->fmt.pix, V4L2_MBUS_FMT_FIXED);
@@ -254,7 +215,7 @@ static int empress_s_fmt_vid_cap(struct file *file, void *priv,
 static int empress_try_fmt_vid_cap(struct file *file, void *priv,
 				struct v4l2_format *f)
 {
-	struct saa7134_dev *dev = file->private_data;
+	struct saa7134_dev *dev = video_drvdata(file);
 	struct v4l2_mbus_framefmt mbus_fmt;
 
 	v4l2_fill_mbus_format(&mbus_fmt, &f->fmt.pix, V4L2_MBUS_FMT_FIXED);
@@ -269,175 +230,6 @@ static int empress_try_fmt_vid_cap(struct file *file, void *priv,
 	return 0;
 }
 
-static int empress_reqbufs(struct file *file, void *priv,
-					struct v4l2_requestbuffers *p)
-{
-	struct saa7134_dev *dev = file->private_data;
-
-	return videobuf_reqbufs(&dev->empress_tsq, p);
-}
-
-static int empress_querybuf(struct file *file, void *priv,
-					struct v4l2_buffer *b)
-{
-	struct saa7134_dev *dev = file->private_data;
-
-	return videobuf_querybuf(&dev->empress_tsq, b);
-}
-
-static int empress_qbuf(struct file *file, void *priv, struct v4l2_buffer *b)
-{
-	struct saa7134_dev *dev = file->private_data;
-
-	return videobuf_qbuf(&dev->empress_tsq, b);
-}
-
-static int empress_dqbuf(struct file *file, void *priv, struct v4l2_buffer *b)
-{
-	struct saa7134_dev *dev = file->private_data;
-
-	return videobuf_dqbuf(&dev->empress_tsq, b,
-				file->f_flags & O_NONBLOCK);
-}
-
-static int empress_streamon(struct file *file, void *priv,
-					enum v4l2_buf_type type)
-{
-	struct saa7134_dev *dev = file->private_data;
-
-	return videobuf_streamon(&dev->empress_tsq);
-}
-
-static int empress_streamoff(struct file *file, void *priv,
-					enum v4l2_buf_type type)
-{
-	struct saa7134_dev *dev = file->private_data;
-
-	return videobuf_streamoff(&dev->empress_tsq);
-}
-
-static int empress_s_ext_ctrls(struct file *file, void *priv,
-			       struct v4l2_ext_controls *ctrls)
-{
-	struct saa7134_dev *dev = file->private_data;
-	int err;
-
-	/* count == 0 is abused in saa6752hs.c, so that special
-		case is handled here explicitly. */
-	if (ctrls->count == 0)
-		return 0;
-
-	if (ctrls->ctrl_class != V4L2_CTRL_CLASS_MPEG)
-		return -EINVAL;
-
-	err = saa_call_empress(dev, core, s_ext_ctrls, ctrls);
-	ts_init_encoder(dev);
-
-	return err;
-}
-
-static int empress_g_ext_ctrls(struct file *file, void *priv,
-			       struct v4l2_ext_controls *ctrls)
-{
-	struct saa7134_dev *dev = file->private_data;
-
-	if (ctrls->ctrl_class != V4L2_CTRL_CLASS_MPEG)
-		return -EINVAL;
-	return saa_call_empress(dev, core, g_ext_ctrls, ctrls);
-}
-
-static int empress_g_ctrl(struct file *file, void *priv,
-					struct v4l2_control *c)
-{
-	struct saa7134_dev *dev = file->private_data;
-
-	return saa7134_g_ctrl_internal(dev, NULL, c);
-}
-
-static int empress_s_ctrl(struct file *file, void *priv,
-					struct v4l2_control *c)
-{
-	struct saa7134_dev *dev = file->private_data;
-
-	return saa7134_s_ctrl_internal(dev, NULL, c);
-}
-
-static int empress_queryctrl(struct file *file, void *priv,
-					struct v4l2_queryctrl *c)
-{
-	/* Must be sorted from low to high control ID! */
-	static const u32 user_ctrls[] = {
-		V4L2_CID_USER_CLASS,
-		V4L2_CID_BRIGHTNESS,
-		V4L2_CID_CONTRAST,
-		V4L2_CID_SATURATION,
-		V4L2_CID_HUE,
-		V4L2_CID_AUDIO_VOLUME,
-		V4L2_CID_AUDIO_MUTE,
-		V4L2_CID_HFLIP,
-		0
-	};
-
-	/* Must be sorted from low to high control ID! */
-	static const u32 mpeg_ctrls[] = {
-		V4L2_CID_MPEG_CLASS,
-		V4L2_CID_MPEG_STREAM_TYPE,
-		V4L2_CID_MPEG_STREAM_PID_PMT,
-		V4L2_CID_MPEG_STREAM_PID_AUDIO,
-		V4L2_CID_MPEG_STREAM_PID_VIDEO,
-		V4L2_CID_MPEG_STREAM_PID_PCR,
-		V4L2_CID_MPEG_AUDIO_SAMPLING_FREQ,
-		V4L2_CID_MPEG_AUDIO_ENCODING,
-		V4L2_CID_MPEG_AUDIO_L2_BITRATE,
-		V4L2_CID_MPEG_VIDEO_ENCODING,
-		V4L2_CID_MPEG_VIDEO_ASPECT,
-		V4L2_CID_MPEG_VIDEO_BITRATE_MODE,
-		V4L2_CID_MPEG_VIDEO_BITRATE,
-		V4L2_CID_MPEG_VIDEO_BITRATE_PEAK,
-		0
-	};
-	static const u32 *ctrl_classes[] = {
-		user_ctrls,
-		mpeg_ctrls,
-		NULL
-	};
-	struct saa7134_dev *dev = file->private_data;
-
-	c->id = v4l2_ctrl_next(ctrl_classes, c->id);
-	if (c->id == 0)
-		return -EINVAL;
-	if (c->id == V4L2_CID_USER_CLASS || c->id == V4L2_CID_MPEG_CLASS)
-		return v4l2_ctrl_query_fill(c, 0, 0, 0, 0);
-	if (V4L2_CTRL_ID2CLASS(c->id) != V4L2_CTRL_CLASS_MPEG)
-		return saa7134_queryctrl(file, priv, c);
-	return saa_call_empress(dev, core, queryctrl, c);
-}
-
-static int empress_querymenu(struct file *file, void *priv,
-					struct v4l2_querymenu *c)
-{
-	struct saa7134_dev *dev = file->private_data;
-
-	if (V4L2_CTRL_ID2CLASS(c->id) != V4L2_CTRL_CLASS_MPEG)
-		return -EINVAL;
-	return saa_call_empress(dev, core, querymenu, c);
-}
-
-static int empress_s_std(struct file *file, void *priv, v4l2_std_id id)
-{
-	struct saa7134_dev *dev = file->private_data;
-
-	return saa7134_s_std_internal(dev, NULL, id);
-}
-
-static int empress_g_std(struct file *file, void *priv, v4l2_std_id *id)
-{
-	struct saa7134_dev *dev = file->private_data;
-
-	*id = dev->tvnorm->id;
-	return 0;
-}
-
 static const struct v4l2_file_operations ts_fops =
 {
 	.owner	  = THIS_MODULE,
@@ -450,28 +242,29 @@ static const struct v4l2_file_operations ts_fops =
 };
 
 static const struct v4l2_ioctl_ops ts_ioctl_ops = {
-	.vidioc_querycap		= empress_querycap,
+	.vidioc_querycap		= saa7134_querycap,
 	.vidioc_enum_fmt_vid_cap	= empress_enum_fmt_vid_cap,
 	.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			= empress_reqbufs,
-	.vidioc_querybuf		= empress_querybuf,
-	.vidioc_qbuf			= empress_qbuf,
-	.vidioc_dqbuf			= empress_dqbuf,
-	.vidioc_streamon		= empress_streamon,
-	.vidioc_streamoff		= empress_streamoff,
-	.vidioc_s_ext_ctrls		= empress_s_ext_ctrls,
-	.vidioc_g_ext_ctrls		= empress_g_ext_ctrls,
-	.vidioc_enum_input		= empress_enum_input,
-	.vidioc_g_input			= empress_g_input,
-	.vidioc_s_input			= empress_s_input,
-	.vidioc_queryctrl		= empress_queryctrl,
-	.vidioc_querymenu		= empress_querymenu,
-	.vidioc_g_ctrl			= empress_g_ctrl,
-	.vidioc_s_ctrl			= empress_s_ctrl,
-	.vidioc_s_std			= empress_s_std,
-	.vidioc_g_std			= empress_g_std,
+	.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_g_frequency		= saa7134_g_frequency,
+	.vidioc_s_frequency		= saa7134_s_frequency,
+	.vidioc_g_tuner			= saa7134_g_tuner,
+	.vidioc_s_tuner			= saa7134_s_tuner,
+	.vidioc_enum_input		= saa7134_enum_input,
+	.vidioc_g_input			= saa7134_g_input,
+	.vidioc_s_input			= saa7134_s_input,
+	.vidioc_s_std			= saa7134_s_std,
+	.vidioc_g_std			= saa7134_g_std,
+	.vidioc_log_status		= v4l2_ctrl_log_status,
+	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
 };
 
 /* ----------------------------------------------------------- */
@@ -501,9 +294,26 @@ static void empress_signal_change(struct saa7134_dev *dev)
 	schedule_work(&dev->empress_workqueue);
 }
 
+static bool empress_ctrl_filter(const struct v4l2_ctrl *ctrl)
+{
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+	case V4L2_CID_HUE:
+	case V4L2_CID_CONTRAST:
+	case V4L2_CID_SATURATION:
+	case V4L2_CID_AUDIO_MUTE:
+	case V4L2_CID_AUDIO_VOLUME:
+	case V4L2_CID_PRIVATE_INVERT:
+	case V4L2_CID_PRIVATE_AUTOMUTE:
+		return true;
+	default:
+		return false;
+	}
+}
 
 static int empress_init(struct saa7134_dev *dev)
 {
+	struct v4l2_ctrl_handler *hdl = &dev->empress_ctrl_handler;
 	int err;
 
 	dprintk("%s: %s\n",dev->name,__func__);
@@ -516,6 +326,16 @@ static int empress_init(struct saa7134_dev *dev)
 	snprintf(dev->empress_dev->name, sizeof(dev->empress_dev->name),
 		 "%s empress (%s)", dev->name,
 		 saa7134_boards[dev->board].name);
+	set_bit(V4L2_FL_USE_FH_PRIO, &dev->empress_dev->flags);
+	v4l2_ctrl_handler_init(hdl, 21);
+	v4l2_ctrl_add_handler(hdl, &dev->ctrl_handler, empress_ctrl_filter);
+	if (dev->empress_sd)
+		v4l2_ctrl_add_handler(hdl, dev->empress_sd->ctrl_handler, NULL);
+	if (hdl->error) {
+		video_device_release(dev->empress_dev);
+		return hdl->error;
+	}
+	dev->empress_dev->ctrl_handler = hdl;
 
 	INIT_WORK(&dev->empress_workqueue, empress_signal_update);
 
@@ -551,6 +371,7 @@ static int empress_fini(struct saa7134_dev *dev)
 		return 0;
 	flush_work(&dev->empress_workqueue);
 	video_unregister_device(dev->empress_dev);
+	v4l2_ctrl_handler_free(&dev->empress_ctrl_handler);
 	dev->empress_dev = NULL;
 	return 0;
 }
diff --git a/drivers/media/pci/saa7134/saa7134-vbi.c b/drivers/media/pci/saa7134/saa7134-vbi.c
index e9aa94b807f1..d4da18d049f3 100644
--- a/drivers/media/pci/saa7134/saa7134-vbi.c
+++ b/drivers/media/pci/saa7134/saa7134-vbi.c
@@ -117,8 +117,7 @@ static int buffer_prepare(struct videobuf_queue *q,
 			  struct videobuf_buffer *vb,
 			  enum v4l2_field field)
 {
-	struct saa7134_fh *fh   = q->priv_data;
-	struct saa7134_dev *dev = fh->dev;
+	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;
@@ -141,7 +140,7 @@ static int buffer_prepare(struct videobuf_queue *q,
 		buf->vb.width  = llength;
 		buf->vb.height = lines;
 		buf->vb.size   = size;
-		buf->pt        = &fh->pt_vbi;
+		buf->pt        = &dev->pt_vbi;
 
 		err = videobuf_iolock(q,&buf->vb,NULL);
 		if (err)
@@ -166,8 +165,7 @@ static int buffer_prepare(struct videobuf_queue *q,
 static int
 buffer_setup(struct videobuf_queue *q, unsigned int *count, unsigned int *size)
 {
-	struct saa7134_fh *fh   = q->priv_data;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = q->priv_data;
 	int llength,lines;
 
 	lines   = dev->tvnorm->vbi_v_stop_0 - dev->tvnorm->vbi_v_start_0 +1;
@@ -181,8 +179,7 @@ buffer_setup(struct videobuf_queue *q, unsigned int *count, unsigned int *size)
 
 static void buffer_queue(struct videobuf_queue *q, struct videobuf_buffer *vb)
 {
-	struct saa7134_fh *fh = q->priv_data;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = q->priv_data;
 	struct saa7134_buf *buf = container_of(vb,struct saa7134_buf,vb);
 
 	saa7134_buffer_queue(dev,&dev->vbi_q,buf);
diff --git a/drivers/media/pci/saa7134/saa7134-video.c b/drivers/media/pci/saa7134/saa7134-video.c
index fb60da85bc2c..eb472b5b26a0 100644
--- a/drivers/media/pci/saa7134/saa7134-video.c
+++ b/drivers/media/pci/saa7134/saa7134-video.c
@@ -27,11 +27,13 @@
 #include <linux/slab.h>
 #include <linux/sort.h>
 
-#include "saa7134-reg.h"
-#include "saa7134.h"
 #include <media/v4l2-common.h>
+#include <media/v4l2-event.h>
 #include <media/saa6588.h>
 
+#include "saa7134-reg.h"
+#include "saa7134.h"
+
 /* ------------------------------------------------------------------ */
 
 unsigned int video_debug;
@@ -369,117 +371,6 @@ static struct saa7134_tvnorm tvnorms[] = {
 };
 #define TVNORMS ARRAY_SIZE(tvnorms)
 
-#define V4L2_CID_PRIVATE_INVERT      (V4L2_CID_PRIVATE_BASE + 0)
-#define V4L2_CID_PRIVATE_Y_ODD       (V4L2_CID_PRIVATE_BASE + 1)
-#define V4L2_CID_PRIVATE_Y_EVEN      (V4L2_CID_PRIVATE_BASE + 2)
-#define V4L2_CID_PRIVATE_AUTOMUTE    (V4L2_CID_PRIVATE_BASE + 3)
-#define V4L2_CID_PRIVATE_LASTP1      (V4L2_CID_PRIVATE_BASE + 4)
-
-static const struct v4l2_queryctrl no_ctrl = {
-	.name  = "42",
-	.flags = V4L2_CTRL_FLAG_DISABLED,
-};
-static const struct v4l2_queryctrl video_ctrls[] = {
-	/* --- video --- */
-	{
-		.id            = V4L2_CID_BRIGHTNESS,
-		.name          = "Brightness",
-		.minimum       = 0,
-		.maximum       = 255,
-		.step          = 1,
-		.default_value = 128,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	},{
-		.id            = V4L2_CID_CONTRAST,
-		.name          = "Contrast",
-		.minimum       = 0,
-		.maximum       = 127,
-		.step          = 1,
-		.default_value = 68,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	},{
-		.id            = V4L2_CID_SATURATION,
-		.name          = "Saturation",
-		.minimum       = 0,
-		.maximum       = 127,
-		.step          = 1,
-		.default_value = 64,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	},{
-		.id            = V4L2_CID_HUE,
-		.name          = "Hue",
-		.minimum       = -128,
-		.maximum       = 127,
-		.step          = 1,
-		.default_value = 0,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	},{
-		.id            = V4L2_CID_HFLIP,
-		.name          = "Mirror",
-		.minimum       = 0,
-		.maximum       = 1,
-		.type          = V4L2_CTRL_TYPE_BOOLEAN,
-	},
-	/* --- audio --- */
-	{
-		.id            = V4L2_CID_AUDIO_MUTE,
-		.name          = "Mute",
-		.minimum       = 0,
-		.maximum       = 1,
-		.type          = V4L2_CTRL_TYPE_BOOLEAN,
-	},{
-		.id            = V4L2_CID_AUDIO_VOLUME,
-		.name          = "Volume",
-		.minimum       = -15,
-		.maximum       = 15,
-		.step          = 1,
-		.default_value = 0,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	},
-	/* --- private --- */
-	{
-		.id            = V4L2_CID_PRIVATE_INVERT,
-		.name          = "Invert",
-		.minimum       = 0,
-		.maximum       = 1,
-		.type          = V4L2_CTRL_TYPE_BOOLEAN,
-	},{
-		.id            = V4L2_CID_PRIVATE_Y_ODD,
-		.name          = "y offset odd field",
-		.minimum       = 0,
-		.maximum       = 128,
-		.step          = 1,
-		.default_value = 0,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	},{
-		.id            = V4L2_CID_PRIVATE_Y_EVEN,
-		.name          = "y offset even field",
-		.minimum       = 0,
-		.maximum       = 128,
-		.step          = 1,
-		.default_value = 0,
-		.type          = V4L2_CTRL_TYPE_INTEGER,
-	},{
-		.id            = V4L2_CID_PRIVATE_AUTOMUTE,
-		.name          = "automute",
-		.minimum       = 0,
-		.maximum       = 1,
-		.default_value = 1,
-		.type          = V4L2_CTRL_TYPE_BOOLEAN,
-	}
-};
-static const unsigned int CTRLS = ARRAY_SIZE(video_ctrls);
-
-static const struct v4l2_queryctrl* ctrl_by_id(unsigned int id)
-{
-	unsigned int i;
-
-	for (i = 0; i < CTRLS; i++)
-		if (video_ctrls[i].id == id)
-			return video_ctrls+i;
-	return NULL;
-}
-
 static struct saa7134_format* format_by_fourcc(unsigned int fourcc)
 {
 	unsigned int i;
@@ -514,16 +405,6 @@ static int res_get(struct saa7134_dev *dev, struct saa7134_fh *fh, unsigned int
 	return 1;
 }
 
-static int res_check(struct saa7134_fh *fh, unsigned int bit)
-{
-	return (fh->resources & bit);
-}
-
-static int res_locked(struct saa7134_dev *dev, unsigned int bit)
-{
-	return (dev->resources & bit);
-}
-
 static
 void res_free(struct saa7134_dev *dev, struct saa7134_fh *fh, unsigned int bits)
 {
@@ -868,7 +749,7 @@ static int verify_preview(struct saa7134_dev *dev, struct v4l2_window *win, bool
 	return 0;
 }
 
-static int start_preview(struct saa7134_dev *dev, struct saa7134_fh *fh)
+static int start_preview(struct saa7134_dev *dev)
 {
 	unsigned long base,control,bpl;
 	int err;
@@ -923,7 +804,7 @@ static int start_preview(struct saa7134_dev *dev, struct saa7134_fh *fh)
 	return 0;
 }
 
-static int stop_preview(struct saa7134_dev *dev, struct saa7134_fh *fh)
+static int stop_preview(struct saa7134_dev *dev)
 {
 	dev->ovenable = 0;
 	saa7134_set_dmabits(dev);
@@ -1018,8 +899,7 @@ static int buffer_prepare(struct videobuf_queue *q,
 			  struct videobuf_buffer *vb,
 			  enum v4l2_field field)
 {
-	struct saa7134_fh *fh = q->priv_data;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = q->priv_data;
 	struct saa7134_buf *buf = container_of(vb,struct saa7134_buf,vb);
 	unsigned int size;
 	int err;
@@ -1057,7 +937,7 @@ static int buffer_prepare(struct videobuf_queue *q,
 		buf->vb.size   = size;
 		buf->vb.field  = field;
 		buf->fmt       = dev->fmt;
-		buf->pt        = &fh->pt_cap;
+		buf->pt        = &dev->pt_cap;
 		dev->video_q.curr = NULL;
 
 		err = videobuf_iolock(q,&buf->vb,&dev->ovbuf);
@@ -1082,8 +962,7 @@ static int buffer_prepare(struct videobuf_queue *q,
 static int
 buffer_setup(struct videobuf_queue *q, unsigned int *count, unsigned int *size)
 {
-	struct saa7134_fh *fh = q->priv_data;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = q->priv_data;
 
 	*size = dev->fmt->depth * dev->width * dev->height >> 3;
 	if (0 == *count)
@@ -1094,10 +973,10 @@ buffer_setup(struct videobuf_queue *q, unsigned int *count, unsigned int *size)
 
 static void buffer_queue(struct videobuf_queue *q, struct videobuf_buffer *vb)
 {
-	struct saa7134_fh *fh = q->priv_data;
+	struct saa7134_dev *dev = q->priv_data;
 	struct saa7134_buf *buf = container_of(vb,struct saa7134_buf,vb);
 
-	saa7134_buffer_queue(fh->dev,&fh->dev->video_q,buf);
+	saa7134_buffer_queue(dev, &dev->video_q, buf);
 }
 
 static void buffer_release(struct videobuf_queue *q, struct videobuf_buffer *vb)
@@ -1116,133 +995,56 @@ static struct videobuf_queue_ops video_qops = {
 
 /* ------------------------------------------------------------------ */
 
-int saa7134_g_ctrl_internal(struct saa7134_dev *dev, struct saa7134_fh *fh, struct v4l2_control *c)
+static int saa7134_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	const struct v4l2_queryctrl* ctrl;
-
-	ctrl = ctrl_by_id(c->id);
-	if (NULL == ctrl)
-		return -EINVAL;
-	switch (c->id) {
-	case V4L2_CID_BRIGHTNESS:
-		c->value = dev->ctl_bright;
-		break;
-	case V4L2_CID_HUE:
-		c->value = dev->ctl_hue;
-		break;
-	case V4L2_CID_CONTRAST:
-		c->value = dev->ctl_contrast;
-		break;
-	case V4L2_CID_SATURATION:
-		c->value = dev->ctl_saturation;
-		break;
-	case V4L2_CID_AUDIO_MUTE:
-		c->value = dev->ctl_mute;
-		break;
-	case V4L2_CID_AUDIO_VOLUME:
-		c->value = dev->ctl_volume;
-		break;
-	case V4L2_CID_PRIVATE_INVERT:
-		c->value = dev->ctl_invert;
-		break;
-	case V4L2_CID_HFLIP:
-		c->value = dev->ctl_mirror;
-		break;
-	case V4L2_CID_PRIVATE_Y_EVEN:
-		c->value = dev->ctl_y_even;
-		break;
-	case V4L2_CID_PRIVATE_Y_ODD:
-		c->value = dev->ctl_y_odd;
-		break;
-	case V4L2_CID_PRIVATE_AUTOMUTE:
-		c->value = dev->ctl_automute;
-		break;
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
-EXPORT_SYMBOL_GPL(saa7134_g_ctrl_internal);
-
-static int saa7134_g_ctrl(struct file *file, void *priv, struct v4l2_control *c)
-{
-	struct saa7134_fh *fh = priv;
-
-	return saa7134_g_ctrl_internal(fh->dev, fh, c);
-}
-
-int saa7134_s_ctrl_internal(struct saa7134_dev *dev,  struct saa7134_fh *fh, struct v4l2_control *c)
-{
-	const struct v4l2_queryctrl* ctrl;
+	struct saa7134_dev *dev = container_of(ctrl->handler, struct saa7134_dev, ctrl_handler);
 	unsigned long flags;
 	int restart_overlay = 0;
-	int err;
 
-	err = -EINVAL;
-
-	mutex_lock(&dev->lock);
-
-	ctrl = ctrl_by_id(c->id);
-	if (NULL == ctrl)
-		goto error;
-
-	dprintk("set_control name=%s val=%d\n",ctrl->name,c->value);
-	switch (ctrl->type) {
-	case V4L2_CTRL_TYPE_BOOLEAN:
-	case V4L2_CTRL_TYPE_MENU:
-	case V4L2_CTRL_TYPE_INTEGER:
-		if (c->value < ctrl->minimum)
-			c->value = ctrl->minimum;
-		if (c->value > ctrl->maximum)
-			c->value = ctrl->maximum;
-		break;
-	default:
-		/* nothing */;
-	}
-	switch (c->id) {
+	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		dev->ctl_bright = c->value;
-		saa_writeb(SAA7134_DEC_LUMA_BRIGHT, dev->ctl_bright);
+		dev->ctl_bright = ctrl->val;
+		saa_writeb(SAA7134_DEC_LUMA_BRIGHT, ctrl->val);
 		break;
 	case V4L2_CID_HUE:
-		dev->ctl_hue = c->value;
-		saa_writeb(SAA7134_DEC_CHROMA_HUE, dev->ctl_hue);
+		dev->ctl_hue = ctrl->val;
+		saa_writeb(SAA7134_DEC_CHROMA_HUE, ctrl->val);
 		break;
 	case V4L2_CID_CONTRAST:
-		dev->ctl_contrast = c->value;
+		dev->ctl_contrast = ctrl->val;
 		saa_writeb(SAA7134_DEC_LUMA_CONTRAST,
 			   dev->ctl_invert ? -dev->ctl_contrast : dev->ctl_contrast);
 		break;
 	case V4L2_CID_SATURATION:
-		dev->ctl_saturation = c->value;
+		dev->ctl_saturation = ctrl->val;
 		saa_writeb(SAA7134_DEC_CHROMA_SATURATION,
 			   dev->ctl_invert ? -dev->ctl_saturation : dev->ctl_saturation);
 		break;
 	case V4L2_CID_AUDIO_MUTE:
-		dev->ctl_mute = c->value;
+		dev->ctl_mute = ctrl->val;
 		saa7134_tvaudio_setmute(dev);
 		break;
 	case V4L2_CID_AUDIO_VOLUME:
-		dev->ctl_volume = c->value;
+		dev->ctl_volume = ctrl->val;
 		saa7134_tvaudio_setvolume(dev,dev->ctl_volume);
 		break;
 	case V4L2_CID_PRIVATE_INVERT:
-		dev->ctl_invert = c->value;
+		dev->ctl_invert = ctrl->val;
 		saa_writeb(SAA7134_DEC_LUMA_CONTRAST,
 			   dev->ctl_invert ? -dev->ctl_contrast : dev->ctl_contrast);
 		saa_writeb(SAA7134_DEC_CHROMA_SATURATION,
 			   dev->ctl_invert ? -dev->ctl_saturation : dev->ctl_saturation);
 		break;
 	case V4L2_CID_HFLIP:
-		dev->ctl_mirror = c->value;
+		dev->ctl_mirror = ctrl->val;
 		restart_overlay = 1;
 		break;
 	case V4L2_CID_PRIVATE_Y_EVEN:
-		dev->ctl_y_even = c->value;
+		dev->ctl_y_even = ctrl->val;
 		restart_overlay = 1;
 		break;
 	case V4L2_CID_PRIVATE_Y_ODD:
-		dev->ctl_y_odd = c->value;
+		dev->ctl_y_odd = ctrl->val;
 		restart_overlay = 1;
 		break;
 	case V4L2_CID_PRIVATE_AUTOMUTE:
@@ -1252,7 +1054,7 @@ int saa7134_s_ctrl_internal(struct saa7134_dev *dev,  struct saa7134_fh *fh, str
 		tda9887_cfg.tuner = TUNER_TDA9887;
 		tda9887_cfg.priv = &dev->tda9887_conf;
 
-		dev->ctl_automute = c->value;
+		dev->ctl_automute = ctrl->val;
 		if (dev->tda9887_conf) {
 			if (dev->ctl_automute)
 				dev->tda9887_conf |= TDA9887_AUTOMUTE;
@@ -1264,27 +1066,15 @@ int saa7134_s_ctrl_internal(struct saa7134_dev *dev,  struct saa7134_fh *fh, str
 		break;
 	}
 	default:
-		goto error;
+		return -EINVAL;
 	}
-	if (restart_overlay && fh && res_check(fh, RESOURCE_OVERLAY)) {
-		spin_lock_irqsave(&dev->slock,flags);
-		stop_preview(dev,fh);
-		start_preview(dev,fh);
-		spin_unlock_irqrestore(&dev->slock,flags);
+	if (restart_overlay && res_locked(dev, RESOURCE_OVERLAY)) {
+		spin_lock_irqsave(&dev->slock, flags);
+		stop_preview(dev);
+		start_preview(dev);
+		spin_unlock_irqrestore(&dev->slock, flags);
 	}
-	err = 0;
-
-error:
-	mutex_unlock(&dev->lock);
-	return err;
-}
-EXPORT_SYMBOL_GPL(saa7134_s_ctrl_internal);
-
-static int saa7134_s_ctrl(struct file *file, void *f, struct v4l2_control *c)
-{
-	struct saa7134_fh *fh = f;
-
-	return saa7134_s_ctrl_internal(fh->dev, fh, c);
+	return 0;
 }
 
 /* ------------------------------------------------------------------ */
@@ -1292,15 +1082,16 @@ static int saa7134_s_ctrl(struct file *file, void *f, struct v4l2_control *c)
 static struct videobuf_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->cap;
+		q = fh->is_empress ? &dev->empress_tsq : &dev->cap;
 		break;
 	case VFL_TYPE_VBI:
-		q = &fh->vbi;
+		q = &dev->vbi;
 		break;
 	default:
 		BUG();
@@ -1311,9 +1102,10 @@ static struct videobuf_queue *saa7134_queue(struct file *file)
 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 RESOURCE_VIDEO;
+		return fh->is_empress ? RESOURCE_EMPRESS : RESOURCE_VIDEO;
 
 	if (vdev->vfl_type == VFL_TYPE_VBI)
 		return RESOURCE_VBI;
@@ -1335,22 +1127,6 @@ static int video_open(struct file *file)
 
 	v4l2_fh_init(&fh->fh, vdev);
 	file->private_data = fh;
-	fh->dev      = dev;
-
-	videobuf_queue_sg_init(&fh->cap, &video_qops,
-			    &dev->pci->dev, &dev->slock,
-			    V4L2_BUF_TYPE_VIDEO_CAPTURE,
-			    V4L2_FIELD_INTERLACED,
-			    sizeof(struct saa7134_buf),
-			    fh, NULL);
-	videobuf_queue_sg_init(&fh->vbi, &saa7134_vbi_qops,
-			    &dev->pci->dev, &dev->slock,
-			    V4L2_BUF_TYPE_VBI_CAPTURE,
-			    V4L2_FIELD_SEQ_TB,
-			    sizeof(struct saa7134_buf),
-			    fh, NULL);
-	saa7134_pgtable_alloc(dev->pci,&fh->pt_cap);
-	saa7134_pgtable_alloc(dev->pci,&fh->pt_vbi);
 
 	if (vdev->vfl_type == VFL_TYPE_RADIO) {
 		/* switch to radio mode */
@@ -1369,17 +1145,18 @@ 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(fh->dev,RESOURCE_VIDEO))
+		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(fh->dev,fh,RESOURCE_VBI))
+		if (!res_get(dev, fh, RESOURCE_VBI))
 			return -EBUSY;
 		return videobuf_read_stream(saa7134_queue(file),
 					    data, count, ppos, 1,
@@ -1394,52 +1171,59 @@ video_read(struct file *file, char __user *data, size_t count, loff_t *ppos)
 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 videobuf_poll_stream(file, &fh->vbi, wait);
+		return rc | videobuf_poll_stream(file, &dev->vbi, wait);
 
-	if (res_check(fh,RESOURCE_VIDEO)) {
-		mutex_lock(&fh->cap.vb_lock);
-		if (!list_empty(&fh->cap.stream))
-			buf = list_entry(fh->cap.stream.next, struct videobuf_buffer, stream);
+	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(&fh->cap.vb_lock);
-		if (UNSET == fh->cap.read_off) {
+		mutex_lock(&dev->cap.vb_lock);
+		if (UNSET == dev->cap.read_off) {
 			/* need to capture a new frame */
-			if (res_locked(fh->dev,RESOURCE_VIDEO))
+			if (res_locked(dev, RESOURCE_VIDEO))
 				goto err;
-			if (0 != fh->cap.ops->buf_prepare(&fh->cap,fh->cap.read_buf,fh->cap.field))
+			if (0 != dev->cap.ops->buf_prepare(&dev->cap,
+					dev->cap.read_buf, dev->cap.field))
 				goto err;
-			fh->cap.ops->buf_queue(&fh->cap,fh->cap.read_buf);
-			fh->cap.read_off = 0;
+			dev->cap.ops->buf_queue(&dev->cap, dev->cap.read_buf);
+			dev->cap.read_off = 0;
 		}
-		buf = fh->cap.read_buf;
+		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(&fh->cap.vb_lock);
+	if (buf->state == VIDEOBUF_DONE || buf->state == VIDEOBUF_ERROR)
+		rc |= POLLIN | POLLRDNORM;
+	mutex_unlock(&dev->cap.vb_lock);
 	return rc;
 
 err:
-	mutex_unlock(&fh->cap.vb_lock);
-	return POLLERR;
+	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_fh  *fh  = file->private_data;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
+	struct saa7134_fh *fh = file->private_data;
 	struct saa6588_command cmd;
 	unsigned long flags;
 
@@ -1448,26 +1232,28 @@ static int video_release(struct file *file)
 	/* turn off overlay */
 	if (res_check(fh, RESOURCE_OVERLAY)) {
 		spin_lock_irqsave(&dev->slock,flags);
-		stop_preview(dev,fh);
+		stop_preview(dev);
 		spin_unlock_irqrestore(&dev->slock,flags);
-		res_free(dev,fh,RESOURCE_OVERLAY);
+		res_free(dev, fh, RESOURCE_OVERLAY);
 	}
 
 	/* stop video capture */
 	if (res_check(fh, RESOURCE_VIDEO)) {
 		pm_qos_remove_request(&dev->qos_request);
-		videobuf_streamoff(&fh->cap);
-		res_free(dev,fh,RESOURCE_VIDEO);
+		videobuf_streamoff(&dev->cap);
+		res_free(dev, fh, RESOURCE_VIDEO);
+		videobuf_mmap_free(&dev->cap);
 	}
-	if (fh->cap.read_buf) {
-		buffer_release(&fh->cap,fh->cap.read_buf);
-		kfree(fh->cap.read_buf);
+	if (dev->cap.read_buf) {
+		buffer_release(&dev->cap, dev->cap.read_buf);
+		kfree(dev->cap.read_buf);
 	}
 
 	/* stop vbi capture */
 	if (res_check(fh, RESOURCE_VBI)) {
-		videobuf_stop(&fh->vbi);
-		res_free(dev,fh,RESOURCE_VBI);
+		videobuf_stop(&dev->vbi);
+		res_free(dev, fh, RESOURCE_VBI);
+		videobuf_mmap_free(&dev->vbi);
 	}
 
 	/* ts-capture will not work in planar mode, so turn it off Hac: 04.05*/
@@ -1480,12 +1266,6 @@ static int video_release(struct file *file)
 	if (vdev->vfl_type == VFL_TYPE_RADIO)
 		saa_call_all(dev, core, ioctl, SAA6588_CMD_CLOSE, &cmd);
 
-	/* free stuff */
-	videobuf_mmap_free(&fh->cap);
-	videobuf_mmap_free(&fh->vbi);
-	saa7134_pgtable_free(dev->pci,&fh->pt_cap);
-	saa7134_pgtable_free(dev->pci,&fh->pt_vbi);
-
 	v4l2_fh_del(&fh->fh);
 	v4l2_fh_exit(&fh->fh);
 	file->private_data = NULL;
@@ -1501,11 +1281,11 @@ static int video_mmap(struct file *file, struct vm_area_struct * vma)
 static ssize_t radio_read(struct file *file, char __user *data,
 			 size_t count, loff_t *ppos)
 {
-	struct saa7134_fh *fh = file->private_data;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
 	struct saa6588_command cmd;
 
 	cmd.block_count = count/3;
+	cmd.nonblocking = file->f_flags & O_NONBLOCK;
 	cmd.buffer = data;
 	cmd.instance = file;
 	cmd.result = -ENODEV;
@@ -1517,16 +1297,16 @@ static ssize_t radio_read(struct file *file, char __user *data,
 
 static unsigned int radio_poll(struct file *file, poll_table *wait)
 {
-	struct saa7134_fh *fh = file->private_data;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
 	struct saa6588_command cmd;
+	unsigned int rc = v4l2_ctrl_poll(file, wait);
 
 	cmd.instance = file;
 	cmd.event_list = wait;
-	cmd.result = -ENODEV;
+	cmd.result = 0;
 	saa_call_all(dev, core, ioctl, SAA6588_CMD_POLL, &cmd);
 
-	return cmd.result;
+	return rc | cmd.result;
 }
 
 /* ------------------------------------------------------------------ */
@@ -1534,8 +1314,7 @@ static unsigned int radio_poll(struct file *file, poll_table *wait)
 static int saa7134_try_get_set_fmt_vbi_cap(struct file *file, void *priv,
 						struct v4l2_format *f)
 {
-	struct saa7134_fh *fh = priv;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
 	struct saa7134_tvnorm *norm = dev->tvnorm;
 
 	memset(&f->fmt.vbi.reserved, 0, sizeof(f->fmt.vbi.reserved));
@@ -1555,12 +1334,11 @@ static int saa7134_try_get_set_fmt_vbi_cap(struct file *file, void *priv,
 static int saa7134_g_fmt_vid_cap(struct file *file, void *priv,
 				struct v4l2_format *f)
 {
-	struct saa7134_fh *fh = priv;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
 
 	f->fmt.pix.width        = dev->width;
 	f->fmt.pix.height       = dev->height;
-	f->fmt.pix.field        = fh->cap.field;
+	f->fmt.pix.field        = dev->cap.field;
 	f->fmt.pix.pixelformat  = dev->fmt->fourcc;
 	f->fmt.pix.bytesperline =
 		(f->fmt.pix.width * dev->fmt->depth) >> 3;
@@ -1574,8 +1352,7 @@ static int saa7134_g_fmt_vid_cap(struct file *file, void *priv,
 static int saa7134_g_fmt_vid_overlay(struct file *file, void *priv,
 				struct v4l2_format *f)
 {
-	struct saa7134_fh *fh = priv;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
 	struct v4l2_clip __user *clips = f->fmt.win.clips;
 	u32 clipcount = f->fmt.win.clipcount;
 	int err = 0;
@@ -1607,8 +1384,7 @@ static int saa7134_g_fmt_vid_overlay(struct file *file, void *priv,
 static int saa7134_try_fmt_vid_cap(struct file *file, void *priv,
 						struct v4l2_format *f)
 {
-	struct saa7134_fh *fh = priv;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
 	struct saa7134_format *fmt;
 	enum v4l2_field field;
 	unsigned int maxw, maxh;
@@ -1659,8 +1435,7 @@ static int saa7134_try_fmt_vid_cap(struct file *file, void *priv,
 static int saa7134_try_fmt_vid_overlay(struct file *file, void *priv,
 						struct v4l2_format *f)
 {
-	struct saa7134_fh *fh = priv;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
 
 	if (saa7134_no_overlay > 0) {
 		printk(KERN_ERR "V4L2_BUF_TYPE_VIDEO_OVERLAY: no_overlay\n");
@@ -1675,8 +1450,7 @@ static int saa7134_try_fmt_vid_overlay(struct file *file, void *priv,
 static int saa7134_s_fmt_vid_cap(struct file *file, void *priv,
 					struct v4l2_format *f)
 {
-	struct saa7134_fh *fh = priv;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
 	int err;
 
 	err = saa7134_try_fmt_vid_cap(file, priv, f);
@@ -1686,15 +1460,14 @@ static int saa7134_s_fmt_vid_cap(struct file *file, void *priv,
 	dev->fmt       = format_by_fourcc(f->fmt.pix.pixelformat);
 	dev->width     = f->fmt.pix.width;
 	dev->height    = f->fmt.pix.height;
-	fh->cap.field = f->fmt.pix.field;
+	dev->cap.field = f->fmt.pix.field;
 	return 0;
 }
 
 static int saa7134_s_fmt_vid_overlay(struct file *file, void *priv,
 					struct v4l2_format *f)
 {
-	struct saa7134_fh *fh = priv;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
 	int err;
 	unsigned long flags;
 
@@ -1719,10 +1492,10 @@ static int saa7134_s_fmt_vid_overlay(struct file *file, void *priv,
 		return -EFAULT;
 	}
 
-	if (res_check(fh, RESOURCE_OVERLAY)) {
+	if (res_check(priv, RESOURCE_OVERLAY)) {
 		spin_lock_irqsave(&dev->slock, flags);
-		stop_preview(dev, fh);
-		start_preview(dev, fh);
+		stop_preview(dev);
+		start_preview(dev);
 		spin_unlock_irqrestore(&dev->slock, flags);
 	}
 
@@ -1730,26 +1503,9 @@ static int saa7134_s_fmt_vid_overlay(struct file *file, void *priv,
 	return 0;
 }
 
-int saa7134_queryctrl(struct file *file, void *priv, struct v4l2_queryctrl *c)
-{
-	const struct v4l2_queryctrl *ctrl;
-
-	if ((c->id <  V4L2_CID_BASE ||
-	     c->id >= V4L2_CID_LASTP1) &&
-	    (c->id <  V4L2_CID_PRIVATE_BASE ||
-	     c->id >= V4L2_CID_PRIVATE_LASTP1))
-		return -EINVAL;
-	ctrl = ctrl_by_id(c->id);
-	*c = (NULL != ctrl) ? *ctrl : no_ctrl;
-	return 0;
-}
-EXPORT_SYMBOL_GPL(saa7134_queryctrl);
-
-static int saa7134_enum_input(struct file *file, void *priv,
-					struct v4l2_input *i)
+int saa7134_enum_input(struct file *file, void *priv, struct v4l2_input *i)
 {
-	struct saa7134_fh *fh = priv;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
 	unsigned int n;
 
 	n = i->index;
@@ -1769,27 +1525,27 @@ static int saa7134_enum_input(struct file *file, void *priv,
 		if (0 != (v1 & 0x40))
 			i->status |= V4L2_IN_ST_NO_H_LOCK;
 		if (0 != (v2 & 0x40))
-			i->status |= V4L2_IN_ST_NO_SYNC;
+			i->status |= V4L2_IN_ST_NO_SIGNAL;
 		if (0 != (v2 & 0x0e))
 			i->status |= V4L2_IN_ST_MACROVISION;
 	}
 	i->std = SAA7134_NORMS;
 	return 0;
 }
+EXPORT_SYMBOL_GPL(saa7134_enum_input);
 
-static int saa7134_g_input(struct file *file, void *priv, unsigned int *i)
+int saa7134_g_input(struct file *file, void *priv, unsigned int *i)
 {
-	struct saa7134_fh *fh = priv;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
 
 	*i = dev->ctl_input;
 	return 0;
 }
+EXPORT_SYMBOL_GPL(saa7134_g_input);
 
-static int saa7134_s_input(struct file *file, void *priv, unsigned int i)
+int saa7134_s_input(struct file *file, void *priv, unsigned int i)
 {
-	struct saa7134_fh *fh = priv;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
 
 	if (i >= SAA7134_INPUT_MAX)
 		return -EINVAL;
@@ -1800,13 +1556,14 @@ static int saa7134_s_input(struct file *file, void *priv, unsigned int i)
 	mutex_unlock(&dev->lock);
 	return 0;
 }
+EXPORT_SYMBOL_GPL(saa7134_s_input);
 
-static int saa7134_querycap(struct file *file, void  *priv,
+int saa7134_querycap(struct file *file, void *priv,
 					struct v4l2_capability *cap)
 {
-	struct saa7134_fh *fh = priv;
-	struct saa7134_dev *dev = fh->dev;
+	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;
@@ -1825,7 +1582,7 @@ static 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)
+	if (saa7134_no_overlay <= 0 && !fh->is_empress)
 		video_caps |= V4L2_CAP_VIDEO_OVERLAY;
 
 	vbi_caps = V4L2_CAP_VBI_CAPTURE;
@@ -1851,14 +1608,17 @@ static int saa7134_querycap(struct file *file, void  *priv,
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(saa7134_querycap);
 
-int saa7134_s_std_internal(struct saa7134_dev *dev, struct saa7134_fh *fh, v4l2_std_id id)
+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;
 	unsigned long flags;
 	unsigned int i;
 	v4l2_std_id fixup;
 
-	if (!fh && res_locked(dev, RESOURCE_OVERLAY)) {
+	if (fh->is_empress && res_locked(dev, RESOURCE_OVERLAY)) {
 		/* Don't change the std from the mpeg device
 		   if overlay is active. */
 		return -EBUSY;
@@ -1898,15 +1658,15 @@ int saa7134_s_std_internal(struct saa7134_dev *dev, struct saa7134_fh *fh, v4l2_
 	id = tvnorms[i].id;
 
 	mutex_lock(&dev->lock);
-	if (fh && res_check(fh, RESOURCE_OVERLAY)) {
+	if (!fh->is_empress && res_check(fh, RESOURCE_OVERLAY)) {
 		spin_lock_irqsave(&dev->slock, flags);
-		stop_preview(dev, fh);
+		stop_preview(dev);
 		spin_unlock_irqrestore(&dev->slock, flags);
 
 		set_tvnorm(dev, &tvnorms[i]);
 
 		spin_lock_irqsave(&dev->slock, flags);
-		start_preview(dev, fh);
+		start_preview(dev);
 		spin_unlock_irqrestore(&dev->slock, flags);
 	} else
 		set_tvnorm(dev, &tvnorms[i]);
@@ -1915,29 +1675,21 @@ int saa7134_s_std_internal(struct saa7134_dev *dev, struct saa7134_fh *fh, v4l2_
 	mutex_unlock(&dev->lock);
 	return 0;
 }
-EXPORT_SYMBOL_GPL(saa7134_s_std_internal);
+EXPORT_SYMBOL_GPL(saa7134_s_std);
 
-static 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)
 {
-	struct saa7134_fh *fh = priv;
-
-	return saa7134_s_std_internal(fh->dev, fh, id);
-}
-
-static int saa7134_g_std(struct file *file, void *priv, v4l2_std_id *id)
-{
-	struct saa7134_fh *fh = priv;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
 
 	*id = dev->tvnorm->id;
 	return 0;
 }
+EXPORT_SYMBOL_GPL(saa7134_g_std);
 
 static int saa7134_cropcap(struct file *file, void *priv,
 					struct v4l2_cropcap *cap)
 {
-	struct saa7134_fh *fh = priv;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
 
 	if (cap->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
 	    cap->type != V4L2_BUF_TYPE_VIDEO_OVERLAY)
@@ -1959,8 +1711,7 @@ static int saa7134_cropcap(struct file *file, void *priv,
 
 static int saa7134_g_crop(struct file *file, void *f, struct v4l2_crop *crop)
 {
-	struct saa7134_fh *fh = f;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
 
 	if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
 	    crop->type != V4L2_BUF_TYPE_VIDEO_OVERLAY)
@@ -1971,22 +1722,17 @@ static int saa7134_g_crop(struct file *file, void *f, struct v4l2_crop *crop)
 
 static int saa7134_s_crop(struct file *file, void *f, const struct v4l2_crop *crop)
 {
-	struct saa7134_fh *fh = f;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
 	struct v4l2_rect *b = &dev->crop_bounds;
 	struct v4l2_rect *c = &dev->crop_current;
 
 	if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
 	    crop->type != V4L2_BUF_TYPE_VIDEO_OVERLAY)
 		return -EINVAL;
-	if (crop->c.height < 0)
-		return -EINVAL;
-	if (crop->c.width < 0)
-		return -EINVAL;
 
-	if (res_locked(fh->dev, RESOURCE_OVERLAY))
+	if (res_locked(dev, RESOURCE_OVERLAY))
 		return -EBUSY;
-	if (res_locked(fh->dev, RESOURCE_VIDEO))
+	if (res_locked(dev, RESOURCE_VIDEO))
 		return -EBUSY;
 
 	*c = crop->c;
@@ -2006,11 +1752,10 @@ static int saa7134_s_crop(struct file *file, void *f, const struct v4l2_crop *cr
 	return 0;
 }
 
-static int saa7134_g_tuner(struct file *file, void *priv,
+int saa7134_g_tuner(struct file *file, void *priv,
 					struct v4l2_tuner *t)
 {
-	struct saa7134_fh *fh = priv;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
 	int n;
 
 	if (0 != t->index)
@@ -2037,12 +1782,12 @@ static int saa7134_g_tuner(struct file *file, void *priv,
 		t->signal = 0xffff;
 	return 0;
 }
+EXPORT_SYMBOL_GPL(saa7134_g_tuner);
 
-static int saa7134_s_tuner(struct file *file, void *priv,
+int saa7134_s_tuner(struct file *file, void *priv,
 					const struct v4l2_tuner *t)
 {
-	struct saa7134_fh *fh = priv;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
 	int rx, mode;
 
 	if (0 != t->index)
@@ -2058,12 +1803,12 @@ static int saa7134_s_tuner(struct file *file, void *priv,
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(saa7134_s_tuner);
 
-static int saa7134_g_frequency(struct file *file, void *priv,
+int saa7134_g_frequency(struct file *file, void *priv,
 					struct v4l2_frequency *f)
 {
-	struct saa7134_fh *fh = priv;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
 
 	if (0 != f->tuner)
 		return -EINVAL;
@@ -2072,12 +1817,12 @@ static int saa7134_g_frequency(struct file *file, void *priv,
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(saa7134_g_frequency);
 
-static int saa7134_s_frequency(struct file *file, void *priv,
+int saa7134_s_frequency(struct file *file, void *priv,
 					const struct v4l2_frequency *f)
 {
-	struct saa7134_fh *fh = priv;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
 
 	if (0 != f->tuner)
 		return -EINVAL;
@@ -2089,6 +1834,7 @@ static int saa7134_s_frequency(struct file *file, void *priv,
 	mutex_unlock(&dev->lock);
 	return 0;
 }
+EXPORT_SYMBOL_GPL(saa7134_s_frequency);
 
 static int saa7134_enum_fmt_vid_cap(struct file *file, void  *priv,
 					struct v4l2_fmtdesc *f)
@@ -2126,8 +1872,7 @@ static int saa7134_enum_fmt_vid_overlay(struct file *file, void  *priv,
 static int saa7134_g_fbuf(struct file *file, void *f,
 				struct v4l2_framebuffer *fb)
 {
-	struct saa7134_fh *fh = f;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
 
 	*fb = dev->ovbuf;
 	fb->capability = V4L2_FBUF_CAP_LIST_CLIPPING;
@@ -2138,8 +1883,7 @@ static int saa7134_g_fbuf(struct file *file, void *f,
 static int saa7134_s_fbuf(struct file *file, void *f,
 					const struct v4l2_framebuffer *fb)
 {
-	struct saa7134_fh *fh = f;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
 	struct saa7134_format *fmt;
 
 	if (!capable(CAP_SYS_ADMIN) &&
@@ -2160,10 +1904,9 @@ static int saa7134_s_fbuf(struct file *file, void *f,
 	return 0;
 }
 
-static int saa7134_overlay(struct file *file, void *f, unsigned int on)
+static int saa7134_overlay(struct file *file, void *priv, unsigned int on)
 {
-	struct saa7134_fh *fh = f;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
 	unsigned long flags;
 
 	if (on) {
@@ -2172,54 +1915,57 @@ static int saa7134_overlay(struct file *file, void *f, unsigned int on)
 			return -EINVAL;
 		}
 
-		if (!res_get(dev, fh, RESOURCE_OVERLAY))
+		if (!res_get(dev, priv, RESOURCE_OVERLAY))
 			return -EBUSY;
 		spin_lock_irqsave(&dev->slock, flags);
-		start_preview(dev, fh);
+		start_preview(dev);
 		spin_unlock_irqrestore(&dev->slock, flags);
 	}
 	if (!on) {
-		if (!res_check(fh, RESOURCE_OVERLAY))
+		if (!res_check(priv, RESOURCE_OVERLAY))
 			return -EINVAL;
 		spin_lock_irqsave(&dev->slock, flags);
-		stop_preview(dev, fh);
+		stop_preview(dev);
 		spin_unlock_irqrestore(&dev->slock, flags);
-		res_free(dev, fh, RESOURCE_OVERLAY);
+		res_free(dev, priv, RESOURCE_OVERLAY);
 	}
 	return 0;
 }
 
-static int saa7134_reqbufs(struct file *file, void *priv,
+int saa7134_reqbufs(struct file *file, void *priv,
 					struct v4l2_requestbuffers *p)
 {
 	return videobuf_reqbufs(saa7134_queue(file), p);
 }
+EXPORT_SYMBOL_GPL(saa7134_reqbufs);
 
-static int saa7134_querybuf(struct file *file, void *priv,
+int saa7134_querybuf(struct file *file, void *priv,
 					struct v4l2_buffer *b)
 {
 	return videobuf_querybuf(saa7134_queue(file), b);
 }
+EXPORT_SYMBOL_GPL(saa7134_querybuf);
 
-static int saa7134_qbuf(struct file *file, void *priv, struct v4l2_buffer *b)
+int saa7134_qbuf(struct file *file, void *priv, struct v4l2_buffer *b)
 {
 	return videobuf_qbuf(saa7134_queue(file), b);
 }
+EXPORT_SYMBOL_GPL(saa7134_qbuf);
 
-static int saa7134_dqbuf(struct file *file, void *priv, struct v4l2_buffer *b)
+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);
 
-static int saa7134_streamon(struct file *file, void *priv,
+int saa7134_streamon(struct file *file, void *priv,
 					enum v4l2_buf_type type)
 {
-	struct saa7134_fh *fh = priv;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
 	int res = saa7134_resource(file);
 
-	if (!res_get(dev, fh, res))
+	if (!res_get(dev, priv, res))
 		return -EBUSY;
 
 	/* The SAA7134 has a 1K FIFO; the datasheet suggests that when
@@ -2229,36 +1975,37 @@ static int saa7134_streamon(struct file *file, void *priv,
 	 * Unfortunately, I lack register-level documentation to check the
 	 * Linux FIFO setup and confirm the perfect value.
 	 */
-	pm_qos_add_request(&dev->qos_request,
-			   PM_QOS_CPU_DMA_LATENCY,
-			   20);
+	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);
 
-static int saa7134_streamoff(struct file *file, void *priv,
+int saa7134_streamoff(struct file *file, void *priv,
 					enum v4l2_buf_type type)
 {
+	struct saa7134_dev *dev = video_drvdata(file);
 	int err;
-	struct saa7134_fh *fh = priv;
-	struct saa7134_dev *dev = fh->dev;
 	int res = saa7134_resource(file);
 
-	pm_qos_remove_request(&dev->qos_request);
+	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, fh, res);
+	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)
 {
-	struct saa7134_fh *fh = priv;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
 
 	reg->val = saa_readb(reg->reg & 0xffffff);
 	reg->size = 1;
@@ -2268,8 +2015,7 @@ static int vidioc_g_register (struct file *file, void *priv,
 static int vidioc_s_register (struct file *file, void *priv,
 				const struct v4l2_dbg_register *reg)
 {
-	struct saa7134_fh *fh = priv;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
 
 	saa_writeb(reg->reg & 0xffffff, reg->val);
 	return 0;
@@ -2279,8 +2025,7 @@ static int vidioc_s_register (struct file *file, void *priv,
 static int radio_g_tuner(struct file *file, void *priv,
 					struct v4l2_tuner *t)
 {
-	struct saa7134_fh *fh = file->private_data;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
 
 	if (0 != t->index)
 		return -EINVAL;
@@ -2299,8 +2044,7 @@ static int radio_g_tuner(struct file *file, void *priv,
 static int radio_s_tuner(struct file *file, void *priv,
 					const struct v4l2_tuner *t)
 {
-	struct saa7134_fh *fh = file->private_data;
-	struct saa7134_dev *dev = fh->dev;
+	struct saa7134_dev *dev = video_drvdata(file);
 
 	if (0 != t->index)
 		return -EINVAL;
@@ -2309,50 +2053,6 @@ static int radio_s_tuner(struct file *file, void *priv,
 	return 0;
 }
 
-static int radio_enum_input(struct file *file, void *priv,
-					struct v4l2_input *i)
-{
-	if (i->index != 0)
-		return -EINVAL;
-
-	strcpy(i->name, "Radio");
-	i->type = V4L2_INPUT_TYPE_TUNER;
-
-	return 0;
-}
-
-static int radio_g_input(struct file *filp, void *priv, unsigned int *i)
-{
-	*i = 0;
-	return 0;
-}
-
-static int radio_s_input(struct file *filp, void *priv, unsigned int i)
-{
-	return 0;
-}
-
-static int radio_s_std(struct file *file, void *fh, v4l2_std_id norm)
-{
-	return 0;
-}
-
-static int radio_queryctrl(struct file *file, void *priv,
-					struct v4l2_queryctrl *c)
-{
-	const struct v4l2_queryctrl *ctrl;
-
-	if (c->id <  V4L2_CID_BASE ||
-	    c->id >= V4L2_CID_LASTP1)
-		return -EINVAL;
-	if (c->id == V4L2_CID_AUDIO_MUTE) {
-		ctrl = ctrl_by_id(c->id);
-		*c = *ctrl;
-	} else
-		*c = no_ctrl;
-	return 0;
-}
-
 static const struct v4l2_file_operations video_fops =
 {
 	.owner	  = THIS_MODULE,
@@ -2387,9 +2087,6 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_enum_input		= saa7134_enum_input,
 	.vidioc_g_input			= saa7134_g_input,
 	.vidioc_s_input			= saa7134_s_input,
-	.vidioc_queryctrl		= saa7134_queryctrl,
-	.vidioc_g_ctrl			= saa7134_g_ctrl,
-	.vidioc_s_ctrl			= saa7134_s_ctrl,
 	.vidioc_streamon		= saa7134_streamon,
 	.vidioc_streamoff		= saa7134_streamoff,
 	.vidioc_g_tuner			= saa7134_g_tuner,
@@ -2405,6 +2102,9 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_g_register              = vidioc_g_register,
 	.vidioc_s_register              = vidioc_s_register,
 #endif
+	.vidioc_log_status		= v4l2_ctrl_log_status,
+	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
 };
 
 static const struct v4l2_file_operations radio_fops = {
@@ -2419,16 +2119,11 @@ static const struct v4l2_file_operations radio_fops = {
 static const struct v4l2_ioctl_ops radio_ioctl_ops = {
 	.vidioc_querycap	= saa7134_querycap,
 	.vidioc_g_tuner		= radio_g_tuner,
-	.vidioc_enum_input	= radio_enum_input,
 	.vidioc_s_tuner		= radio_s_tuner,
-	.vidioc_s_input		= radio_s_input,
-	.vidioc_s_std		= radio_s_std,
-	.vidioc_queryctrl	= radio_queryctrl,
-	.vidioc_g_input		= radio_g_input,
-	.vidioc_g_ctrl		= saa7134_g_ctrl,
-	.vidioc_s_ctrl		= saa7134_s_ctrl,
 	.vidioc_g_frequency	= saa7134_g_frequency,
 	.vidioc_s_frequency	= saa7134_s_frequency,
+	.vidioc_subscribe_event	= v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 /* ----------------------------------------------------------- */
@@ -2447,8 +2142,55 @@ struct video_device saa7134_radio_template = {
 	.ioctl_ops 		= &radio_ioctl_ops,
 };
 
+static const struct v4l2_ctrl_ops saa7134_ctrl_ops = {
+	.s_ctrl = saa7134_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config saa7134_ctrl_invert = {
+	.ops = &saa7134_ctrl_ops,
+	.id = V4L2_CID_PRIVATE_INVERT,
+	.name = "Invert",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.min = 0,
+	.max = 1,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config saa7134_ctrl_y_odd = {
+	.ops = &saa7134_ctrl_ops,
+	.id = V4L2_CID_PRIVATE_Y_ODD,
+	.name = "Y Offset Odd Field",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min = 0,
+	.max = 128,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config saa7134_ctrl_y_even = {
+	.ops = &saa7134_ctrl_ops,
+	.id = V4L2_CID_PRIVATE_Y_EVEN,
+	.name = "Y Offset Even Field",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min = 0,
+	.max = 128,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config saa7134_ctrl_automute = {
+	.ops = &saa7134_ctrl_ops,
+	.id = V4L2_CID_PRIVATE_AUTOMUTE,
+	.name = "Automute",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.min = 0,
+	.max = 1,
+	.step = 1,
+	.def = 1,
+};
+
 int saa7134_video_init1(struct saa7134_dev *dev)
 {
+	struct v4l2_ctrl_handler *hdl = &dev->ctrl_handler;
+
 	/* sanitycheck insmod options */
 	if (gbuffers < 2 || gbuffers > VIDEO_MAX_FRAME)
 		gbuffers = 2;
@@ -2456,17 +2198,38 @@ int saa7134_video_init1(struct saa7134_dev *dev)
 		gbufsize = gbufsize_max;
 	gbufsize = (gbufsize + PAGE_SIZE - 1) & PAGE_MASK;
 
-	/* put some sensible defaults into the data structures ... */
-	dev->ctl_bright     = ctrl_by_id(V4L2_CID_BRIGHTNESS)->default_value;
-	dev->ctl_contrast   = ctrl_by_id(V4L2_CID_CONTRAST)->default_value;
-	dev->ctl_hue        = ctrl_by_id(V4L2_CID_HUE)->default_value;
-	dev->ctl_saturation = ctrl_by_id(V4L2_CID_SATURATION)->default_value;
-	dev->ctl_volume     = ctrl_by_id(V4L2_CID_AUDIO_VOLUME)->default_value;
-	dev->ctl_mute       = 1; // ctrl_by_id(V4L2_CID_AUDIO_MUTE)->default_value;
-	dev->ctl_invert     = ctrl_by_id(V4L2_CID_PRIVATE_INVERT)->default_value;
-	dev->ctl_automute   = ctrl_by_id(V4L2_CID_PRIVATE_AUTOMUTE)->default_value;
-
-	if (dev->tda9887_conf && dev->ctl_automute)
+	v4l2_ctrl_handler_init(hdl, 11);
+	v4l2_ctrl_new_std(hdl, &saa7134_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(hdl, &saa7134_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 127, 1, 68);
+	v4l2_ctrl_new_std(hdl, &saa7134_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 127, 1, 64);
+	v4l2_ctrl_new_std(hdl, &saa7134_ctrl_ops,
+			V4L2_CID_HUE, -128, 127, 1, 0);
+	v4l2_ctrl_new_std(hdl, &saa7134_ctrl_ops,
+			V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(hdl, &saa7134_ctrl_ops,
+			V4L2_CID_AUDIO_MUTE, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(hdl, &saa7134_ctrl_ops,
+			V4L2_CID_AUDIO_VOLUME, -15, 15, 1, 0);
+	v4l2_ctrl_new_custom(hdl, &saa7134_ctrl_invert, NULL);
+	v4l2_ctrl_new_custom(hdl, &saa7134_ctrl_y_odd, NULL);
+	v4l2_ctrl_new_custom(hdl, &saa7134_ctrl_y_even, NULL);
+	v4l2_ctrl_new_custom(hdl, &saa7134_ctrl_automute, NULL);
+	if (hdl->error)
+		return hdl->error;
+	if (card_has_radio(dev)) {
+		hdl = &dev->radio_ctrl_handler;
+		v4l2_ctrl_handler_init(hdl, 2);
+		v4l2_ctrl_add_handler(hdl, &dev->ctrl_handler,
+				v4l2_ctrl_radio_filter);
+		if (hdl->error)
+			return hdl->error;
+	}
+	dev->ctl_mute       = 1;
+
+	if (dev->tda9887_conf && saa7134_ctrl_automute.def)
 		dev->tda9887_conf |= TDA9887_AUTOMUTE;
 	dev->automute       = 0;
 
@@ -2489,9 +2252,34 @@ 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);
+
 	return 0;
 }
 
+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);
+	v4l2_ctrl_handler_free(&dev->ctrl_handler);
+	if (card_has_radio(dev))
+		v4l2_ctrl_handler_free(&dev->radio_ctrl_handler);
+}
+
 int saa7134_videoport_init(struct saa7134_dev *dev)
 {
 	/* enable video output */
@@ -2533,6 +2321,7 @@ int saa7134_video_init2(struct saa7134_dev *dev)
 	/* init video hw */
 	set_tvnorm(dev,&tvnorms[0]);
 	video_mux(dev,0);
+	v4l2_ctrl_handler_setup(&dev->ctrl_handler);
 	saa7134_tvaudio_setmute(dev);
 	saa7134_tvaudio_setvolume(dev,dev->ctl_volume);
 	return 0;
diff --git a/drivers/media/pci/saa7134/saa7134.h b/drivers/media/pci/saa7134/saa7134.h
index 8d1453a48014..2474e848f2c0 100644
--- a/drivers/media/pci/saa7134/saa7134.h
+++ b/drivers/media/pci/saa7134/saa7134.h
@@ -37,6 +37,7 @@
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-fh.h>
+#include <media/v4l2-ctrls.h>
 #include <media/tuner.h>
 #include <media/rc-core.h>
 #include <media/ir-kbd-i2c.h>
@@ -410,12 +411,18 @@ struct saa7134_board {
 #define card(dev)             (saa7134_boards[dev->board])
 #define card_in(dev,n)        (saa7134_boards[dev->board].inputs[n])
 
+#define V4L2_CID_PRIVATE_INVERT      (V4L2_CID_USER_SAA7134_BASE + 0)
+#define V4L2_CID_PRIVATE_Y_ODD       (V4L2_CID_USER_SAA7134_BASE + 1)
+#define V4L2_CID_PRIVATE_Y_EVEN      (V4L2_CID_USER_SAA7134_BASE + 2)
+#define V4L2_CID_PRIVATE_AUTOMUTE    (V4L2_CID_USER_SAA7134_BASE + 3)
+
 /* ----------------------------------------------------------- */
 /* device / file handle status                                 */
 
 #define RESOURCE_OVERLAY       1
 #define RESOURCE_VIDEO         2
 #define RESOURCE_VBI           4
+#define RESOURCE_EMPRESS       8
 
 #define INTERLACE_AUTO         0
 #define INTERLACE_ON           1
@@ -470,16 +477,8 @@ struct saa7134_dmaqueue {
 /* video filehandle status */
 struct saa7134_fh {
 	struct v4l2_fh             fh;
-	struct saa7134_dev         *dev;
+	bool			   is_empress;
 	unsigned int               resources;
-
-	/* video capture */
-	struct videobuf_queue      cap;
-	struct saa7134_pgtable     pt_cap;
-
-	/* vbi capture */
-	struct videobuf_queue      vbi;
-	struct saa7134_pgtable     pt_vbi;
 };
 
 /* dmasound dsp status */
@@ -589,7 +588,11 @@ struct saa7134_dev {
 
 	/* video+ts+vbi capture */
 	struct saa7134_dmaqueue    video_q;
+	struct videobuf_queue      cap;
+	struct saa7134_pgtable     pt_cap;
 	struct saa7134_dmaqueue    vbi_q;
+	struct videobuf_queue      vbi;
+	struct saa7134_pgtable     pt_vbi;
 	unsigned int               video_fieldcount;
 	unsigned int               vbi_fieldcount;
 	struct saa7134_format      *fmt;
@@ -599,6 +602,7 @@ struct saa7134_dev {
 	/* various v4l controls */
 	struct saa7134_tvnorm      *tvnorm;              /* video */
 	struct saa7134_tvaudio     *tvaudio;
+	struct v4l2_ctrl_handler   ctrl_handler;
 	unsigned int               ctl_input;
 	int                        ctl_bright;
 	int                        ctl_contrast;
@@ -626,6 +630,7 @@ struct saa7134_dev {
 	int                        last_carrier;
 	int                        nosignal;
 	unsigned int               insuspend;
+	struct v4l2_ctrl_handler   radio_ctrl_handler;
 
 	/* I2C keyboard data */
 	struct IR_i2c_init_data    init_data;
@@ -638,10 +643,11 @@ struct saa7134_dev {
 
 	/* SAA7134_MPEG_EMPRESS only */
 	struct video_device        *empress_dev;
+	struct v4l2_subdev	   *empress_sd;
 	struct videobuf_queue      empress_tsq;
-	atomic_t 		   empress_users;
 	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 */
@@ -699,6 +705,16 @@ struct saa7134_dev {
 	_rc;								\
 })
 
+static inline int res_check(struct saa7134_fh *fh, unsigned int bit)
+{
+	return fh->resources & bit;
+}
+
+static inline int res_locked(struct saa7134_dev *dev, unsigned int bit)
+{
+	return dev->resources & bit;
+}
+
 /* ----------------------------------------------------------- */
 /* saa7134-core.c                                              */
 
@@ -761,10 +777,31 @@ extern unsigned int video_debug;
 extern struct video_device saa7134_video_template;
 extern struct video_device saa7134_radio_template;
 
-int saa7134_s_ctrl_internal(struct saa7134_dev *dev,  struct saa7134_fh *fh, struct v4l2_control *c);
-int saa7134_g_ctrl_internal(struct saa7134_dev *dev,  struct saa7134_fh *fh, struct v4l2_control *c);
-int saa7134_queryctrl(struct file *file, void *priv, struct v4l2_queryctrl *c);
-int saa7134_s_std_internal(struct saa7134_dev *dev,  struct saa7134_fh *fh, v4l2_std_id id);
+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_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);
+int saa7134_querycap(struct file *file, void  *priv,
+					struct v4l2_capability *cap);
+int saa7134_g_tuner(struct file *file, void *priv,
+					struct v4l2_tuner *t);
+int saa7134_s_tuner(struct file *file, void *priv,
+					const struct v4l2_tuner *t);
+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);
@@ -773,6 +810,7 @@ int saa7134_video_init1(struct saa7134_dev *dev);
 int saa7134_video_init2(struct saa7134_dev *dev);
 void saa7134_irq_video_signalchange(struct saa7134_dev *dev);
 void saa7134_irq_video_done(struct saa7134_dev *dev, unsigned long status);
+void saa7134_video_fini(struct saa7134_dev *dev);
 
 
 /* ----------------------------------------------------------- */
diff --git a/drivers/media/pci/sta2x11/sta2x11_vip.c b/drivers/media/pci/sta2x11/sta2x11_vip.c
index 77edc113e485..e5cfb6cfa18d 100644
--- a/drivers/media/pci/sta2x11/sta2x11_vip.c
+++ b/drivers/media/pci/sta2x11/sta2x11_vip.c
@@ -1303,7 +1303,7 @@ static int sta2x11_vip_resume(struct pci_dev *pdev)
 
 #endif
 
-static DEFINE_PCI_DEVICE_TABLE(sta2x11_vip_pci_tbl) = {
+static const struct pci_device_id sta2x11_vip_pci_tbl[] = {
 	{PCI_DEVICE(PCI_VENDOR_ID_STMICRO, PCI_DEVICE_ID_STMICRO_VIP)},
 	{0,}
 };
diff --git a/drivers/media/platform/Kconfig b/drivers/media/platform/Kconfig
index d7f0249e4050..b2a4403940c5 100644
--- a/drivers/media/platform/Kconfig
+++ b/drivers/media/platform/Kconfig
@@ -36,7 +36,8 @@ source "drivers/media/platform/blackfin/Kconfig"
 config VIDEO_SH_VOU
 	tristate "SuperH VOU video output driver"
 	depends on MEDIA_CAMERA_SUPPORT
-	depends on VIDEO_DEV && ARCH_SHMOBILE && I2C
+	depends on VIDEO_DEV && I2C
+	depends on ARCH_SHMOBILE || COMPILE_TEST
 	select VIDEOBUF_DMA_CONTIG
 	help
 	  Support for the Video Output Unit (VOU) on SuperH SoCs.
@@ -90,13 +91,6 @@ config VIDEO_M32R_AR_M64278
 	  To compile this driver as a module, choose M here: the
 	  module will be called arv.
 
-config VIDEO_OMAP2
-	tristate "OMAP2 Camera Capture Interface driver"
-	depends on VIDEO_DEV && ARCH_OMAP2 && VIDEO_V4L2_INT_DEVICE
-	select VIDEOBUF_DMA_SG
-	---help---
-	  This is a v4l2 driver for the TI OMAP2 camera capture interface
-
 config VIDEO_OMAP3
 	tristate "OMAP 3 Camera support"
 	depends on OMAP_IOVMM && VIDEO_V4L2 && I2C && VIDEO_V4L2_SUBDEV_API && ARCH_OMAP3
diff --git a/drivers/media/platform/Makefile b/drivers/media/platform/Makefile
index 1348ba1faf92..e5269da91906 100644
--- a/drivers/media/platform/Makefile
+++ b/drivers/media/platform/Makefile
@@ -2,8 +2,6 @@
 # Makefile for the video capture/playback device drivers.
 #
 
-omap2cam-objs	:=	omap24xxcam.o omap24xxcam-dma.o
-
 obj-$(CONFIG_VIDEO_VINO) += indycam.o
 obj-$(CONFIG_VIDEO_VINO) += vino.o
 
@@ -14,7 +12,6 @@ obj-$(CONFIG_VIDEO_VIA_CAMERA) += via-camera.o
 obj-$(CONFIG_VIDEO_CAFE_CCIC) += marvell-ccic/
 obj-$(CONFIG_VIDEO_MMP_CAMERA) += marvell-ccic/
 
-obj-$(CONFIG_VIDEO_OMAP2)		+= omap2cam.o
 obj-$(CONFIG_VIDEO_OMAP3)	+= omap3isp/
 
 obj-$(CONFIG_VIDEO_VIU) += fsl-viu.o
diff --git a/drivers/media/platform/davinci/vpbe_display.c b/drivers/media/platform/davinci/vpbe_display.c
index eac472b5ae83..b02aba488826 100644
--- a/drivers/media/platform/davinci/vpbe_display.c
+++ b/drivers/media/platform/davinci/vpbe_display.c
@@ -347,7 +347,7 @@ static int vpbe_start_streaming(struct vb2_queue *vq, unsigned int count)
 	/* If buffer queue is empty, return error */
 	if (list_empty(&layer->dma_queue)) {
 		v4l2_err(&vpbe_dev->v4l2_dev, "buffer queue is empty\n");
-		return -EINVAL;
+		return -ENOBUFS;
 	}
 	/* Get the next frame from the buffer queue */
 	layer->next_frm = layer->cur_frm = list_entry(layer->dma_queue.next,
diff --git a/drivers/media/platform/davinci/vpif_capture.c b/drivers/media/platform/davinci/vpif_capture.c
index 52ac5e6c8625..735ec47601a9 100644
--- a/drivers/media/platform/davinci/vpif_capture.c
+++ b/drivers/media/platform/davinci/vpif_capture.c
@@ -277,7 +277,7 @@ static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count)
 	if (list_empty(&common->dma_queue)) {
 		spin_unlock_irqrestore(&common->irqlock, flags);
 		vpif_dbg(1, debug, "buffer queue is empty\n");
-		return -EIO;
+		return -ENOBUFS;
 	}
 
 	/* Get the next frame from the buffer queue */
diff --git a/drivers/media/platform/davinci/vpif_display.c b/drivers/media/platform/davinci/vpif_display.c
index c31bcf129a5d..9d115cdc6bdb 100644
--- a/drivers/media/platform/davinci/vpif_display.c
+++ b/drivers/media/platform/davinci/vpif_display.c
@@ -239,7 +239,7 @@ static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count)
 	if (list_empty(&common->dma_queue)) {
 		spin_unlock_irqrestore(&common->irqlock, flags);
 		vpif_err("buffer queue is empty\n");
-		return -EIO;
+		return -ENOBUFS;
 	}
 
 	/* Get the next frame from the buffer queue */
diff --git a/drivers/media/platform/exynos4-is/Kconfig b/drivers/media/platform/exynos4-is/Kconfig
index d2d3b4b61435..01ed1ecdff7e 100644
--- a/drivers/media/platform/exynos4-is/Kconfig
+++ b/drivers/media/platform/exynos4-is/Kconfig
@@ -1,7 +1,7 @@
 
 config VIDEO_SAMSUNG_EXYNOS4_IS
 	bool "Samsung S5P/EXYNOS4 SoC series Camera Subsystem driver"
-	depends on VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API && PM_RUNTIME
+	depends on VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
 	depends on (PLAT_S5P || ARCH_EXYNOS)
 	help
 	  Say Y here to enable camera host interface devices for
diff --git a/drivers/media/platform/exynos4-is/fimc-capture.c b/drivers/media/platform/exynos4-is/fimc-capture.c
index fb27ff7e1e07..8a712ca91d11 100644
--- a/drivers/media/platform/exynos4-is/fimc-capture.c
+++ b/drivers/media/platform/exynos4-is/fimc-capture.c
@@ -549,7 +549,7 @@ static int fimc_capture_release(struct file *file)
 		vc->streaming = false;
 	}
 
-	ret = vb2_fop_release(file);
+	ret = _vb2_fop_release(file, NULL);
 
 	if (close) {
 		clear_bit(ST_CAPT_BUSY, &fimc->state);
diff --git a/drivers/media/platform/exynos4-is/fimc-core.c b/drivers/media/platform/exynos4-is/fimc-core.c
index f7915695c907..da2fc86cc524 100644
--- a/drivers/media/platform/exynos4-is/fimc-core.c
+++ b/drivers/media/platform/exynos4-is/fimc-core.c
@@ -998,41 +998,46 @@ static int fimc_probe(struct platform_device *pdev)
 
 	ret = devm_request_irq(dev, res->start, fimc_irq_handler,
 			       0, dev_name(dev), fimc);
-	if (ret) {
+	if (ret < 0) {
 		dev_err(dev, "failed to install irq (%d)\n", ret);
-		goto err_clk;
+		goto err_sclk;
 	}
 
 	ret = fimc_initialize_capture_subdev(fimc);
-	if (ret)
-		goto err_clk;
+	if (ret < 0)
+		goto err_sclk;
 
 	platform_set_drvdata(pdev, fimc);
 	pm_runtime_enable(dev);
-	ret = pm_runtime_get_sync(dev);
-	if (ret < 0)
-		goto err_sd;
+
+	if (!pm_runtime_enabled(dev)) {
+		ret = clk_enable(fimc->clock[CLK_GATE]);
+		if (ret < 0)
+			goto err_sd;
+	}
+
 	/* Initialize contiguous memory allocator */
 	fimc->alloc_ctx = vb2_dma_contig_init_ctx(dev);
 	if (IS_ERR(fimc->alloc_ctx)) {
 		ret = PTR_ERR(fimc->alloc_ctx);
-		goto err_pm;
+		goto err_gclk;
 	}
 
 	dev_dbg(dev, "FIMC.%d registered successfully\n", fimc->id);
-
-	pm_runtime_put(dev);
 	return 0;
-err_pm:
-	pm_runtime_put(dev);
+
+err_gclk:
+	if (!pm_runtime_enabled(dev))
+		clk_disable(fimc->clock[CLK_GATE]);
 err_sd:
 	fimc_unregister_capture_subdev(fimc);
-err_clk:
+err_sclk:
 	clk_disable(fimc->clock[CLK_BUS]);
 	fimc_clk_put(fimc);
 	return ret;
 }
 
+#ifdef CONFIG_PM_RUNTIME
 static int fimc_runtime_resume(struct device *dev)
 {
 	struct fimc_dev *fimc =	dev_get_drvdata(dev);
@@ -1065,6 +1070,7 @@ static int fimc_runtime_suspend(struct device *dev)
 	dbg("fimc%d: state: 0x%lx", fimc->id, fimc->state);
 	return ret;
 }
+#endif
 
 #ifdef CONFIG_PM_SLEEP
 static int fimc_resume(struct device *dev)
diff --git a/drivers/media/platform/exynos4-is/fimc-core.h b/drivers/media/platform/exynos4-is/fimc-core.h
index 3d376faec777..1790fb4e32ea 100644
--- a/drivers/media/platform/exynos4-is/fimc-core.h
+++ b/drivers/media/platform/exynos4-is/fimc-core.h
@@ -481,7 +481,6 @@ struct fimc_ctrls {
  * @flags:		additional flags for image conversion
  * @state:		flags to keep track of user configuration
  * @fimc_dev:		the FIMC device this context applies to
- * @m2m_ctx:		memory-to-memory device context
  * @fh:			v4l2 file handle
  * @ctrls:		v4l2 controls structure
  */
@@ -502,7 +501,6 @@ struct fimc_ctx {
 	u32			flags;
 	u32			state;
 	struct fimc_dev		*fimc_dev;
-	struct v4l2_m2m_ctx	*m2m_ctx;
 	struct v4l2_fh		fh;
 	struct fimc_ctrls	ctrls;
 };
diff --git a/drivers/media/platform/exynos4-is/fimc-is-regs.c b/drivers/media/platform/exynos4-is/fimc-is-regs.c
index f758e2694fa3..2628733c4e10 100644
--- a/drivers/media/platform/exynos4-is/fimc-is-regs.c
+++ b/drivers/media/platform/exynos4-is/fimc-is-regs.c
@@ -33,47 +33,23 @@ void fimc_is_hw_set_intgr0_gd0(struct fimc_is *is)
 	mcuctl_write(INTGR0_INTGD(0), is, MCUCTL_REG_INTGR0);
 }
 
-int fimc_is_hw_wait_intsr0_intsd0(struct fimc_is *is)
-{
-	unsigned int timeout = 2000;
-	u32 cfg, status;
-
-	cfg = mcuctl_read(is, MCUCTL_REG_INTSR0);
-	status = INTSR0_GET_INTSD(0, cfg);
-
-	while (status) {
-		cfg = mcuctl_read(is, MCUCTL_REG_INTSR0);
-		status = INTSR0_GET_INTSD(0, cfg);
-		if (timeout == 0) {
-			dev_warn(&is->pdev->dev, "%s timeout\n",
-				 __func__);
-			return -ETIME;
-		}
-		timeout--;
-		udelay(1);
-	}
-	return 0;
-}
-
 int fimc_is_hw_wait_intmsr0_intmsd0(struct fimc_is *is)
 {
 	unsigned int timeout = 2000;
 	u32 cfg, status;
 
-	cfg = mcuctl_read(is, MCUCTL_REG_INTMSR0);
-	status = INTMSR0_GET_INTMSD(0, cfg);
-
-	while (status) {
+	do {
 		cfg = mcuctl_read(is, MCUCTL_REG_INTMSR0);
 		status = INTMSR0_GET_INTMSD(0, cfg);
-		if (timeout == 0) {
+
+		if (--timeout == 0) {
 			dev_warn(&is->pdev->dev, "%s timeout\n",
 				 __func__);
-			return -ETIME;
+			return -ETIMEDOUT;
 		}
-		timeout--;
 		udelay(1);
-	}
+	} while (status != 0);
+
 	return 0;
 }
 
diff --git a/drivers/media/platform/exynos4-is/fimc-is-regs.h b/drivers/media/platform/exynos4-is/fimc-is-regs.h
index 5fa2fda46742..1d9d4ffc6ad5 100644
--- a/drivers/media/platform/exynos4-is/fimc-is-regs.h
+++ b/drivers/media/platform/exynos4-is/fimc-is-regs.h
@@ -145,7 +145,6 @@ void fimc_is_fw_clear_irq2(struct fimc_is *is);
 int fimc_is_hw_get_params(struct fimc_is *is, unsigned int num);
 
 void fimc_is_hw_set_intgr0_gd0(struct fimc_is *is);
-int fimc_is_hw_wait_intsr0_intsd0(struct fimc_is *is);
 int fimc_is_hw_wait_intmsr0_intmsd0(struct fimc_is *is);
 void fimc_is_hw_set_sensor_num(struct fimc_is *is);
 void fimc_is_hw_stream_on(struct fimc_is *is);
diff --git a/drivers/media/platform/exynos4-is/fimc-is.c b/drivers/media/platform/exynos4-is/fimc-is.c
index 9770fa98d6a1..13a4228952e3 100644
--- a/drivers/media/platform/exynos4-is/fimc-is.c
+++ b/drivers/media/platform/exynos4-is/fimc-is.c
@@ -781,6 +781,9 @@ static int fimc_is_debugfs_create(struct fimc_is *is)
 	return is->debugfs_entry == NULL ? -EIO : 0;
 }
 
+static int fimc_is_runtime_resume(struct device *dev);
+static int fimc_is_runtime_suspend(struct device *dev);
+
 static int fimc_is_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
@@ -835,14 +838,20 @@ static int fimc_is_probe(struct platform_device *pdev)
 	}
 	pm_runtime_enable(dev);
 
+	if (!pm_runtime_enabled(dev)) {
+		ret = fimc_is_runtime_resume(dev);
+		if (ret < 0)
+			goto err_irq;
+	}
+
 	ret = pm_runtime_get_sync(dev);
 	if (ret < 0)
-		goto err_irq;
+		goto err_pm;
 
 	is->alloc_ctx = vb2_dma_contig_init_ctx(dev);
 	if (IS_ERR(is->alloc_ctx)) {
 		ret = PTR_ERR(is->alloc_ctx);
-		goto err_irq;
+		goto err_pm;
 	}
 	/*
 	 * Register FIMC-IS V4L2 subdevs to this driver. The video nodes
@@ -867,10 +876,13 @@ static int fimc_is_probe(struct platform_device *pdev)
 
 err_dfs:
 	fimc_is_debugfs_remove(is);
-err_vb:
-	vb2_dma_contig_cleanup_ctx(is->alloc_ctx);
 err_sd:
 	fimc_is_unregister_subdevs(is);
+err_vb:
+	vb2_dma_contig_cleanup_ctx(is->alloc_ctx);
+err_pm:
+	if (!pm_runtime_enabled(dev))
+		fimc_is_runtime_suspend(dev);
 err_irq:
 	free_irq(is->irq, is);
 err_clk:
@@ -919,10 +931,13 @@ static int fimc_is_suspend(struct device *dev)
 
 static int fimc_is_remove(struct platform_device *pdev)
 {
-	struct fimc_is *is = platform_get_drvdata(pdev);
+	struct device *dev = &pdev->dev;
+	struct fimc_is *is = dev_get_drvdata(dev);
 
-	pm_runtime_disable(&pdev->dev);
-	pm_runtime_set_suspended(&pdev->dev);
+	pm_runtime_disable(dev);
+	pm_runtime_set_suspended(dev);
+	if (!pm_runtime_status_suspended(dev))
+		fimc_is_runtime_suspend(dev);
 	free_irq(is->irq, is);
 	fimc_is_unregister_subdevs(is);
 	vb2_dma_contig_cleanup_ctx(is->alloc_ctx);
diff --git a/drivers/media/platform/exynos4-is/fimc-lite-reg.c b/drivers/media/platform/exynos4-is/fimc-lite-reg.c
index 72a343e3b5e8..d0dc7ee04452 100644
--- a/drivers/media/platform/exynos4-is/fimc-lite-reg.c
+++ b/drivers/media/platform/exynos4-is/fimc-lite-reg.c
@@ -133,7 +133,7 @@ void flite_hw_set_source_format(struct fimc_lite *dev, struct flite_frame *f)
 	int i = ARRAY_SIZE(src_pixfmt_map);
 	u32 cfg;
 
-	while (--i >= 0) {
+	while (--i) {
 		if (src_pixfmt_map[i][0] == pixelcode)
 			break;
 	}
@@ -240,7 +240,7 @@ static void flite_hw_set_out_order(struct fimc_lite *dev, struct flite_frame *f)
 	u32 cfg = readl(dev->regs + FLITE_REG_CIODMAFMT);
 	int i = ARRAY_SIZE(pixcode);
 
-	while (--i >= 0)
+	while (--i)
 		if (pixcode[i][0] == f->fmt->mbus_code)
 			break;
 	cfg &= ~FLITE_REG_CIODMAFMT_YCBCR_ORDER_MASK;
diff --git a/drivers/media/platform/exynos4-is/fimc-lite.c b/drivers/media/platform/exynos4-is/fimc-lite.c
index e5798f70d149..779ec3cd259d 100644
--- a/drivers/media/platform/exynos4-is/fimc-lite.c
+++ b/drivers/media/platform/exynos4-is/fimc-lite.c
@@ -546,7 +546,7 @@ static int fimc_lite_release(struct file *file)
 		mutex_unlock(&entity->parent->graph_mutex);
 	}
 
-	vb2_fop_release(file);
+	_vb2_fop_release(file, NULL);
 	pm_runtime_put(&fimc->pdev->dev);
 	clear_bit(ST_FLITE_SUSPENDED, &fimc->state);
 
@@ -1549,42 +1549,46 @@ static int fimc_lite_probe(struct platform_device *pdev)
 			       0, dev_name(dev), fimc);
 	if (ret) {
 		dev_err(dev, "Failed to install irq (%d)\n", ret);
-		goto err_clk;
+		goto err_clk_put;
 	}
 
 	/* The video node will be created within the subdev's registered() op */
 	ret = fimc_lite_create_capture_subdev(fimc);
 	if (ret)
-		goto err_clk;
+		goto err_clk_put;
 
 	platform_set_drvdata(pdev, fimc);
 	pm_runtime_enable(dev);
-	ret = pm_runtime_get_sync(dev);
-	if (ret < 0)
-		goto err_sd;
+
+	if (!pm_runtime_enabled(dev)) {
+		ret = clk_enable(fimc->clock);
+		if (ret < 0)
+			goto err_sd;
+	}
 
 	fimc->alloc_ctx = vb2_dma_contig_init_ctx(dev);
 	if (IS_ERR(fimc->alloc_ctx)) {
 		ret = PTR_ERR(fimc->alloc_ctx);
-		goto err_pm;
+		goto err_clk_dis;
 	}
 
-	pm_runtime_put(dev);
-
 	fimc_lite_set_default_config(fimc);
 
 	dev_dbg(dev, "FIMC-LITE.%d registered successfully\n",
 		fimc->index);
 	return 0;
-err_pm:
-	pm_runtime_put(dev);
+
+err_clk_dis:
+	if (!pm_runtime_enabled(dev))
+		clk_disable(fimc->clock);
 err_sd:
 	fimc_lite_unregister_capture_subdev(fimc);
-err_clk:
+err_clk_put:
 	fimc_lite_clk_put(fimc);
 	return ret;
 }
 
+#ifdef CONFIG_PM_RUNTIME
 static int fimc_lite_runtime_resume(struct device *dev)
 {
 	struct fimc_lite *fimc = dev_get_drvdata(dev);
@@ -1600,6 +1604,7 @@ static int fimc_lite_runtime_suspend(struct device *dev)
 	clk_disable(fimc->clock);
 	return 0;
 }
+#endif
 
 #ifdef CONFIG_PM_SLEEP
 static int fimc_lite_resume(struct device *dev)
diff --git a/drivers/media/platform/exynos4-is/fimc-m2m.c b/drivers/media/platform/exynos4-is/fimc-m2m.c
index 8d33b68c76ba..9da95bd14820 100644
--- a/drivers/media/platform/exynos4-is/fimc-m2m.c
+++ b/drivers/media/platform/exynos4-is/fimc-m2m.c
@@ -44,17 +44,17 @@ void fimc_m2m_job_finish(struct fimc_ctx *ctx, int vb_state)
 {
 	struct vb2_buffer *src_vb, *dst_vb;
 
-	if (!ctx || !ctx->m2m_ctx)
+	if (!ctx || !ctx->fh.m2m_ctx)
 		return;
 
-	src_vb = v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
-	dst_vb = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
+	src_vb = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+	dst_vb = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
 
 	if (src_vb && dst_vb) {
 		v4l2_m2m_buf_done(src_vb, vb_state);
 		v4l2_m2m_buf_done(dst_vb, vb_state);
 		v4l2_m2m_job_finish(ctx->fimc_dev->m2m.m2m_dev,
-				    ctx->m2m_ctx);
+				    ctx->fh.m2m_ctx);
 	}
 }
 
@@ -123,12 +123,12 @@ static void fimc_device_run(void *priv)
 		fimc_prepare_dma_offset(ctx, df);
 	}
 
-	src_vb = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
+	src_vb = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
 	ret = fimc_prepare_addr(ctx, src_vb, sf, &sf->paddr);
 	if (ret)
 		goto dma_unlock;
 
-	dst_vb = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
+	dst_vb = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
 	ret = fimc_prepare_addr(ctx, dst_vb, df, &df->paddr);
 	if (ret)
 		goto dma_unlock;
@@ -219,31 +219,15 @@ static int fimc_buf_prepare(struct vb2_buffer *vb)
 static void fimc_buf_queue(struct vb2_buffer *vb)
 {
 	struct fimc_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
-
-	dbg("ctx: %p, ctx->state: 0x%x", ctx, ctx->state);
-
-	if (ctx->m2m_ctx)
-		v4l2_m2m_buf_queue(ctx->m2m_ctx, vb);
-}
-
-static void fimc_lock(struct vb2_queue *vq)
-{
-	struct fimc_ctx *ctx = vb2_get_drv_priv(vq);
-	mutex_lock(&ctx->fimc_dev->lock);
-}
-
-static void fimc_unlock(struct vb2_queue *vq)
-{
-	struct fimc_ctx *ctx = vb2_get_drv_priv(vq);
-	mutex_unlock(&ctx->fimc_dev->lock);
+	v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vb);
 }
 
 static struct vb2_ops fimc_qops = {
 	.queue_setup	 = fimc_queue_setup,
 	.buf_prepare	 = fimc_buf_prepare,
 	.buf_queue	 = fimc_buf_queue,
-	.wait_prepare	 = fimc_unlock,
-	.wait_finish	 = fimc_lock,
+	.wait_prepare	 = vb2_ops_wait_prepare,
+	.wait_finish	 = vb2_ops_wait_finish,
 	.stop_streaming	 = stop_streaming,
 	.start_streaming = start_streaming,
 };
@@ -385,7 +369,7 @@ static int fimc_m2m_s_fmt_mplane(struct file *file, void *fh,
 	if (ret)
 		return ret;
 
-	vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
 
 	if (vb2_is_busy(vq)) {
 		v4l2_err(&fimc->m2m.vfd, "queue (%d) busy\n", f->type);
@@ -410,56 +394,6 @@ static int fimc_m2m_s_fmt_mplane(struct file *file, void *fh,
 	return 0;
 }
 
-static int fimc_m2m_reqbufs(struct file *file, void *fh,
-			    struct v4l2_requestbuffers *reqbufs)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-	return v4l2_m2m_reqbufs(file, ctx->m2m_ctx, reqbufs);
-}
-
-static int fimc_m2m_querybuf(struct file *file, void *fh,
-			     struct v4l2_buffer *buf)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-	return v4l2_m2m_querybuf(file, ctx->m2m_ctx, buf);
-}
-
-static int fimc_m2m_qbuf(struct file *file, void *fh,
-			 struct v4l2_buffer *buf)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-	return v4l2_m2m_qbuf(file, ctx->m2m_ctx, buf);
-}
-
-static int fimc_m2m_dqbuf(struct file *file, void *fh,
-			  struct v4l2_buffer *buf)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-	return v4l2_m2m_dqbuf(file, ctx->m2m_ctx, buf);
-}
-
-static int fimc_m2m_expbuf(struct file *file, void *fh,
-			    struct v4l2_exportbuffer *eb)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-	return v4l2_m2m_expbuf(file, ctx->m2m_ctx, eb);
-}
-
-
-static int fimc_m2m_streamon(struct file *file, void *fh,
-			     enum v4l2_buf_type type)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-	return v4l2_m2m_streamon(file, ctx->m2m_ctx, type);
-}
-
-static int fimc_m2m_streamoff(struct file *file, void *fh,
-			    enum v4l2_buf_type type)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(fh);
-	return v4l2_m2m_streamoff(file, ctx->m2m_ctx, type);
-}
-
 static int fimc_m2m_cropcap(struct file *file, void *fh,
 			    struct v4l2_cropcap *cr)
 {
@@ -598,13 +532,13 @@ static const struct v4l2_ioctl_ops fimc_m2m_ioctl_ops = {
 	.vidioc_try_fmt_vid_out_mplane	= fimc_m2m_try_fmt_mplane,
 	.vidioc_s_fmt_vid_cap_mplane	= fimc_m2m_s_fmt_mplane,
 	.vidioc_s_fmt_vid_out_mplane	= fimc_m2m_s_fmt_mplane,
-	.vidioc_reqbufs			= fimc_m2m_reqbufs,
-	.vidioc_querybuf		= fimc_m2m_querybuf,
-	.vidioc_qbuf			= fimc_m2m_qbuf,
-	.vidioc_dqbuf			= fimc_m2m_dqbuf,
-	.vidioc_expbuf			= fimc_m2m_expbuf,
-	.vidioc_streamon		= fimc_m2m_streamon,
-	.vidioc_streamoff		= fimc_m2m_streamoff,
+	.vidioc_reqbufs			= v4l2_m2m_ioctl_reqbufs,
+	.vidioc_querybuf		= v4l2_m2m_ioctl_querybuf,
+	.vidioc_qbuf			= v4l2_m2m_ioctl_qbuf,
+	.vidioc_dqbuf			= v4l2_m2m_ioctl_dqbuf,
+	.vidioc_expbuf			= v4l2_m2m_ioctl_expbuf,
+	.vidioc_streamon		= v4l2_m2m_ioctl_streamon,
+	.vidioc_streamoff		= v4l2_m2m_ioctl_streamoff,
 	.vidioc_g_crop			= fimc_m2m_g_crop,
 	.vidioc_s_crop			= fimc_m2m_s_crop,
 	.vidioc_cropcap			= fimc_m2m_cropcap
@@ -624,6 +558,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	src_vq->mem_ops = &vb2_dma_contig_memops;
 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	src_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	src_vq->lock = &ctx->fimc_dev->lock;
 
 	ret = vb2_queue_init(src_vq);
 	if (ret)
@@ -636,6 +571,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	dst_vq->mem_ops = &vb2_dma_contig_memops;
 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	dst_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	dst_vq->lock = &ctx->fimc_dev->lock;
 
 	return vb2_queue_init(dst_vq);
 }
@@ -708,9 +644,9 @@ static int fimc_m2m_open(struct file *file)
 	ctx->out_path = FIMC_IO_DMA;
 	ctx->scaler.enabled = 1;
 
-	ctx->m2m_ctx = v4l2_m2m_ctx_init(fimc->m2m.m2m_dev, ctx, queue_init);
-	if (IS_ERR(ctx->m2m_ctx)) {
-		ret = PTR_ERR(ctx->m2m_ctx);
+	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(fimc->m2m.m2m_dev, ctx, queue_init);
+	if (IS_ERR(ctx->fh.m2m_ctx)) {
+		ret = PTR_ERR(ctx->fh.m2m_ctx);
 		goto error_c;
 	}
 
@@ -725,7 +661,7 @@ static int fimc_m2m_open(struct file *file)
 	return 0;
 
 error_m2m_ctx:
-	v4l2_m2m_ctx_release(ctx->m2m_ctx);
+	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
 error_c:
 	fimc_ctrls_delete(ctx);
 error_fh:
@@ -747,7 +683,7 @@ static int fimc_m2m_release(struct file *file)
 
 	mutex_lock(&fimc->lock);
 
-	v4l2_m2m_ctx_release(ctx->m2m_ctx);
+	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
 	fimc_ctrls_delete(ctx);
 	v4l2_fh_del(&ctx->fh);
 	v4l2_fh_exit(&ctx->fh);
@@ -760,45 +696,13 @@ static int fimc_m2m_release(struct file *file)
 	return 0;
 }
 
-static unsigned int fimc_m2m_poll(struct file *file,
-				  struct poll_table_struct *wait)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(file->private_data);
-	struct fimc_dev *fimc = ctx->fimc_dev;
-	int ret;
-
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
-	ret = v4l2_m2m_poll(file, ctx->m2m_ctx, wait);
-	mutex_unlock(&fimc->lock);
-
-	return ret;
-}
-
-
-static int fimc_m2m_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	struct fimc_ctx *ctx = fh_to_ctx(file->private_data);
-	struct fimc_dev *fimc = ctx->fimc_dev;
-	int ret;
-
-	if (mutex_lock_interruptible(&fimc->lock))
-		return -ERESTARTSYS;
-
-	ret = v4l2_m2m_mmap(file, ctx->m2m_ctx, vma);
-	mutex_unlock(&fimc->lock);
-
-	return ret;
-}
-
 static const struct v4l2_file_operations fimc_m2m_fops = {
 	.owner		= THIS_MODULE,
 	.open		= fimc_m2m_open,
 	.release	= fimc_m2m_release,
-	.poll		= fimc_m2m_poll,
+	.poll		= v4l2_m2m_fop_poll,
 	.unlocked_ioctl	= video_ioctl2,
-	.mmap		= fimc_m2m_mmap,
+	.mmap		= v4l2_m2m_fop_mmap,
 };
 
 static struct v4l2_m2m_ops m2m_ops = {
diff --git a/drivers/media/platform/exynos4-is/mipi-csis.c b/drivers/media/platform/exynos4-is/mipi-csis.c
index 9fc2af6a0446..f3c3591fdc5d 100644
--- a/drivers/media/platform/exynos4-is/mipi-csis.c
+++ b/drivers/media/platform/exynos4-is/mipi-csis.c
@@ -91,7 +91,7 @@ MODULE_PARM_DESC(debug, "Debug level (0-2)");
 #define S5PCSIS_INTSRC_ODD_BEFORE	(1 << 29)
 #define S5PCSIS_INTSRC_ODD_AFTER	(1 << 28)
 #define S5PCSIS_INTSRC_ODD		(0x3 << 28)
-#define S5PCSIS_INTSRC_NON_IMAGE_DATA	(0xff << 28)
+#define S5PCSIS_INTSRC_NON_IMAGE_DATA	(0xf << 28)
 #define S5PCSIS_INTSRC_FRAME_START	(1 << 27)
 #define S5PCSIS_INTSRC_FRAME_END	(1 << 26)
 #define S5PCSIS_INTSRC_ERR_SOT_HS	(0xf << 12)
@@ -790,6 +790,7 @@ static int s5pcsis_parse_dt(struct platform_device *pdev,
 #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[];
 
 static int s5pcsis_probe(struct platform_device *pdev)
@@ -902,13 +903,21 @@ static int s5pcsis_probe(struct platform_device *pdev)
 	/* .. and a pointer to the subdev. */
 	platform_set_drvdata(pdev, &state->sd);
 	memcpy(state->events, s5pcsis_events, sizeof(state->events));
+
 	pm_runtime_enable(dev);
+	if (!pm_runtime_enabled(dev)) {
+		ret = s5pcsis_pm_resume(dev, true);
+		if (ret < 0)
+			goto e_m_ent;
+	}
 
 	dev_info(&pdev->dev, "lanes: %d, hs_settle: %d, wclk: %d, freq: %u\n",
 		 state->num_lanes, state->hs_settle, state->wclk_ext,
 		 state->clk_frequency);
 	return 0;
 
+e_m_ent:
+	media_entity_cleanup(&state->sd.entity);
 e_clkdis:
 	clk_disable(state->clock[CSIS_CLK_MUX]);
 e_clkput:
@@ -1014,7 +1023,7 @@ static int s5pcsis_remove(struct platform_device *pdev)
 	struct csis_state *state = sd_to_csis_state(sd);
 
 	pm_runtime_disable(&pdev->dev);
-	s5pcsis_pm_suspend(&pdev->dev, false);
+	s5pcsis_pm_suspend(&pdev->dev, true);
 	clk_disable(state->clock[CSIS_CLK_MUX]);
 	pm_runtime_set_suspended(&pdev->dev);
 	s5pcsis_clk_put(state);
diff --git a/drivers/media/platform/fsl-viu.c b/drivers/media/platform/fsl-viu.c
index 6a232239ee8c..dbf0ce38a8e7 100644
--- a/drivers/media/platform/fsl-viu.c
+++ b/drivers/media/platform/fsl-viu.c
@@ -1580,7 +1580,7 @@ static int viu_of_probe(struct platform_device *op)
 	}
 
 	/* enable VIU clock */
-	clk = devm_clk_get(&op->dev, "viu_clk");
+	clk = devm_clk_get(&op->dev, "ipg");
 	if (IS_ERR(clk)) {
 		dev_err(&op->dev, "failed to lookup the clock!\n");
 		ret = PTR_ERR(clk);
diff --git a/drivers/media/platform/m2m-deinterlace.c b/drivers/media/platform/m2m-deinterlace.c
index 65cab70fefcb..6bb86b581a34 100644
--- a/drivers/media/platform/m2m-deinterlace.c
+++ b/drivers/media/platform/m2m-deinterlace.c
@@ -918,7 +918,7 @@ static int deinterlace_open(struct file *file)
 		return ret;
 	}
 
-	ctx->xt = kzalloc(sizeof(struct dma_async_tx_descriptor) +
+	ctx->xt = kzalloc(sizeof(struct dma_interleaved_template) +
 				sizeof(struct data_chunk), GFP_KERNEL);
 	if (!ctx->xt) {
 		kfree(ctx);
diff --git a/drivers/media/platform/mem2mem_testdev.c b/drivers/media/platform/mem2mem_testdev.c
index 8df5975b700a..08e24379b794 100644
--- a/drivers/media/platform/mem2mem_testdev.c
+++ b/drivers/media/platform/mem2mem_testdev.c
@@ -177,8 +177,6 @@ struct m2mtest_ctx {
 
 	enum v4l2_colorspace	colorspace;
 
-	struct v4l2_m2m_ctx	*m2m_ctx;
-
 	/* Source and destination queue data */
 	struct m2mtest_q_data   q_data[2];
 };
@@ -342,8 +340,8 @@ static int job_ready(void *priv)
 {
 	struct m2mtest_ctx *ctx = priv;
 
-	if (v4l2_m2m_num_src_bufs_ready(ctx->m2m_ctx) < ctx->translen
-	    || v4l2_m2m_num_dst_bufs_ready(ctx->m2m_ctx) < ctx->translen) {
+	if (v4l2_m2m_num_src_bufs_ready(ctx->fh.m2m_ctx) < ctx->translen
+	    || v4l2_m2m_num_dst_bufs_ready(ctx->fh.m2m_ctx) < ctx->translen) {
 		dprintk(ctx->dev, "Not enough buffers available\n");
 		return 0;
 	}
@@ -359,21 +357,6 @@ static void job_abort(void *priv)
 	ctx->aborting = 1;
 }
 
-static void m2mtest_lock(void *priv)
-{
-	struct m2mtest_ctx *ctx = priv;
-	struct m2mtest_dev *dev = ctx->dev;
-	mutex_lock(&dev->dev_mutex);
-}
-
-static void m2mtest_unlock(void *priv)
-{
-	struct m2mtest_ctx *ctx = priv;
-	struct m2mtest_dev *dev = ctx->dev;
-	mutex_unlock(&dev->dev_mutex);
-}
-
-
 /* device_run() - prepares and starts the device
  *
  * This simulates all the immediate preparations required before starting
@@ -386,8 +369,8 @@ static void device_run(void *priv)
 	struct m2mtest_dev *dev = ctx->dev;
 	struct vb2_buffer *src_buf, *dst_buf;
 
-	src_buf = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
-	dst_buf = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
+	src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+	dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
 
 	device_process(ctx, src_buf, dst_buf);
 
@@ -409,8 +392,8 @@ static void device_isr(unsigned long priv)
 		return;
 	}
 
-	src_vb = v4l2_m2m_src_buf_remove(curr_ctx->m2m_ctx);
-	dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->m2m_ctx);
+	src_vb = v4l2_m2m_src_buf_remove(curr_ctx->fh.m2m_ctx);
+	dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->fh.m2m_ctx);
 
 	curr_ctx->num_processed++;
 
@@ -423,7 +406,7 @@ static void device_isr(unsigned long priv)
 	    || curr_ctx->aborting) {
 		dprintk(curr_ctx->dev, "Finishing transaction\n");
 		curr_ctx->num_processed = 0;
-		v4l2_m2m_job_finish(m2mtest_dev->m2m_dev, curr_ctx->m2m_ctx);
+		v4l2_m2m_job_finish(m2mtest_dev->m2m_dev, curr_ctx->fh.m2m_ctx);
 	} else {
 		device_run(curr_ctx);
 	}
@@ -491,7 +474,7 @@ static int vidioc_g_fmt(struct m2mtest_ctx *ctx, struct v4l2_format *f)
 	struct vb2_queue *vq;
 	struct m2mtest_q_data *q_data;
 
-	vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
 	if (!vq)
 		return -EINVAL;
 
@@ -594,7 +577,7 @@ static int vidioc_s_fmt(struct m2mtest_ctx *ctx, struct v4l2_format *f)
 	struct m2mtest_q_data *q_data;
 	struct vb2_queue *vq;
 
-	vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
 	if (!vq)
 		return -EINVAL;
 
@@ -648,52 +631,6 @@ static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
 	return ret;
 }
 
-static int vidioc_reqbufs(struct file *file, void *priv,
-			  struct v4l2_requestbuffers *reqbufs)
-{
-	struct m2mtest_ctx *ctx = file2ctx(file);
-
-	return v4l2_m2m_reqbufs(file, ctx->m2m_ctx, reqbufs);
-}
-
-static int vidioc_querybuf(struct file *file, void *priv,
-			   struct v4l2_buffer *buf)
-{
-	struct m2mtest_ctx *ctx = file2ctx(file);
-
-	return v4l2_m2m_querybuf(file, ctx->m2m_ctx, buf);
-}
-
-static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
-{
-	struct m2mtest_ctx *ctx = file2ctx(file);
-
-	return v4l2_m2m_qbuf(file, ctx->m2m_ctx, buf);
-}
-
-static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
-{
-	struct m2mtest_ctx *ctx = file2ctx(file);
-
-	return v4l2_m2m_dqbuf(file, ctx->m2m_ctx, buf);
-}
-
-static int vidioc_streamon(struct file *file, void *priv,
-			   enum v4l2_buf_type type)
-{
-	struct m2mtest_ctx *ctx = file2ctx(file);
-
-	return v4l2_m2m_streamon(file, ctx->m2m_ctx, type);
-}
-
-static int vidioc_streamoff(struct file *file, void *priv,
-			    enum v4l2_buf_type type)
-{
-	struct m2mtest_ctx *ctx = file2ctx(file);
-
-	return v4l2_m2m_streamoff(file, ctx->m2m_ctx, type);
-}
-
 static int m2mtest_s_ctrl(struct v4l2_ctrl *ctrl)
 {
 	struct m2mtest_ctx *ctx =
@@ -748,14 +685,14 @@ static const struct v4l2_ioctl_ops m2mtest_ioctl_ops = {
 	.vidioc_try_fmt_vid_out	= vidioc_try_fmt_vid_out,
 	.vidioc_s_fmt_vid_out	= vidioc_s_fmt_vid_out,
 
-	.vidioc_reqbufs		= vidioc_reqbufs,
-	.vidioc_querybuf	= vidioc_querybuf,
+	.vidioc_reqbufs		= v4l2_m2m_ioctl_reqbufs,
+	.vidioc_querybuf	= v4l2_m2m_ioctl_querybuf,
+	.vidioc_qbuf		= v4l2_m2m_ioctl_qbuf,
+	.vidioc_dqbuf		= v4l2_m2m_ioctl_dqbuf,
 
-	.vidioc_qbuf		= vidioc_qbuf,
-	.vidioc_dqbuf		= vidioc_dqbuf,
+	.vidioc_streamon	= v4l2_m2m_ioctl_streamon,
+	.vidioc_streamoff	= v4l2_m2m_ioctl_streamoff,
 
-	.vidioc_streamon	= vidioc_streamon,
-	.vidioc_streamoff	= vidioc_streamoff,
 	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
 	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
@@ -818,27 +755,15 @@ static int m2mtest_buf_prepare(struct vb2_buffer *vb)
 static void m2mtest_buf_queue(struct vb2_buffer *vb)
 {
 	struct m2mtest_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
-	v4l2_m2m_buf_queue(ctx->m2m_ctx, vb);
-}
-
-static void m2mtest_wait_prepare(struct vb2_queue *q)
-{
-	struct m2mtest_ctx *ctx = vb2_get_drv_priv(q);
-	m2mtest_unlock(ctx);
-}
-
-static void m2mtest_wait_finish(struct vb2_queue *q)
-{
-	struct m2mtest_ctx *ctx = vb2_get_drv_priv(q);
-	m2mtest_lock(ctx);
+	v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vb);
 }
 
 static struct vb2_ops m2mtest_qops = {
 	.queue_setup	 = m2mtest_queue_setup,
 	.buf_prepare	 = m2mtest_buf_prepare,
 	.buf_queue	 = m2mtest_buf_queue,
-	.wait_prepare	 = m2mtest_wait_prepare,
-	.wait_finish	 = m2mtest_wait_finish,
+	.wait_prepare	 = vb2_ops_wait_prepare,
+	.wait_finish	 = vb2_ops_wait_finish,
 };
 
 static int queue_init(void *priv, struct vb2_queue *src_vq, struct vb2_queue *dst_vq)
@@ -853,6 +778,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq, struct vb2_queue *ds
 	src_vq->ops = &m2mtest_qops;
 	src_vq->mem_ops = &vb2_vmalloc_memops;
 	src_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	src_vq->lock = &ctx->dev->dev_mutex;
 
 	ret = vb2_queue_init(src_vq);
 	if (ret)
@@ -865,6 +791,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq, struct vb2_queue *ds
 	dst_vq->ops = &m2mtest_qops;
 	dst_vq->mem_ops = &vb2_vmalloc_memops;
 	dst_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	dst_vq->lock = &ctx->dev->dev_mutex;
 
 	return vb2_queue_init(dst_vq);
 }
@@ -936,10 +863,10 @@ static int m2mtest_open(struct file *file)
 	ctx->q_data[V4L2_M2M_DST] = ctx->q_data[V4L2_M2M_SRC];
 	ctx->colorspace = V4L2_COLORSPACE_REC709;
 
-	ctx->m2m_ctx = v4l2_m2m_ctx_init(dev->m2m_dev, ctx, &queue_init);
+	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->m2m_dev, ctx, &queue_init);
 
-	if (IS_ERR(ctx->m2m_ctx)) {
-		rc = PTR_ERR(ctx->m2m_ctx);
+	if (IS_ERR(ctx->fh.m2m_ctx)) {
+		rc = PTR_ERR(ctx->fh.m2m_ctx);
 
 		v4l2_ctrl_handler_free(hdl);
 		kfree(ctx);
@@ -949,7 +876,8 @@ static int m2mtest_open(struct file *file)
 	v4l2_fh_add(&ctx->fh);
 	atomic_inc(&dev->num_inst);
 
-	dprintk(dev, "Created instance %p, m2m_ctx: %p\n", ctx, ctx->m2m_ctx);
+	dprintk(dev, "Created instance: %p, m2m_ctx: %p\n",
+		ctx, ctx->fh.m2m_ctx);
 
 open_unlock:
 	mutex_unlock(&dev->dev_mutex);
@@ -967,7 +895,7 @@ static int m2mtest_release(struct file *file)
 	v4l2_fh_exit(&ctx->fh);
 	v4l2_ctrl_handler_free(&ctx->hdl);
 	mutex_lock(&dev->dev_mutex);
-	v4l2_m2m_ctx_release(ctx->m2m_ctx);
+	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
 	mutex_unlock(&dev->dev_mutex);
 	kfree(ctx);
 
@@ -976,34 +904,13 @@ static int m2mtest_release(struct file *file)
 	return 0;
 }
 
-static unsigned int m2mtest_poll(struct file *file,
-				 struct poll_table_struct *wait)
-{
-	struct m2mtest_ctx *ctx = file2ctx(file);
-
-	return v4l2_m2m_poll(file, ctx->m2m_ctx, wait);
-}
-
-static int m2mtest_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	struct m2mtest_dev *dev = video_drvdata(file);
-	struct m2mtest_ctx *ctx = file2ctx(file);
-	int res;
-
-	if (mutex_lock_interruptible(&dev->dev_mutex))
-		return -ERESTARTSYS;
-	res = v4l2_m2m_mmap(file, ctx->m2m_ctx, vma);
-	mutex_unlock(&dev->dev_mutex);
-	return res;
-}
-
 static const struct v4l2_file_operations m2mtest_fops = {
 	.owner		= THIS_MODULE,
 	.open		= m2mtest_open,
 	.release	= m2mtest_release,
-	.poll		= m2mtest_poll,
+	.poll		= v4l2_m2m_fop_poll,
 	.unlocked_ioctl	= video_ioctl2,
-	.mmap		= m2mtest_mmap,
+	.mmap		= v4l2_m2m_fop_mmap,
 };
 
 static struct video_device m2mtest_videodev = {
@@ -1019,8 +926,6 @@ static struct v4l2_m2m_ops m2m_ops = {
 	.device_run	= device_run,
 	.job_ready	= job_ready,
 	.job_abort	= job_abort,
-	.lock		= m2mtest_lock,
-	.unlock		= m2mtest_unlock,
 };
 
 static int m2mtest_probe(struct platform_device *pdev)
@@ -1133,4 +1038,3 @@ static int __init m2mtest_init(void)
 
 module_init(m2mtest_init);
 module_exit(m2mtest_exit);
-
diff --git a/drivers/media/platform/omap3isp/isp.c b/drivers/media/platform/omap3isp/isp.c
index fdbdeae3900d..5807185262fe 100644
--- a/drivers/media/platform/omap3isp/isp.c
+++ b/drivers/media/platform/omap3isp/isp.c
@@ -873,15 +873,12 @@ static int isp_pipeline_enable(struct isp_pipeline *pipe,
 	unsigned long flags;
 	int ret;
 
-	/* If the preview engine crashed it might not respond to read/write
-	 * operations on the L4 bus. This would result in a bus fault and a
-	 * kernel oops. Refuse to start streaming in that case. This check must
-	 * be performed before the loop below to avoid starting entities if the
-	 * pipeline won't start anyway (those entities would then likely fail to
-	 * stop, making the problem worse).
+	/* Refuse to start streaming if an entity included in the pipeline has
+	 * crashed. This check must be performed before the loop below to avoid
+	 * starting entities if the pipeline won't start anyway (those entities
+	 * would then likely fail to stop, making the problem worse).
 	 */
-	if ((pipe->entities & isp->crashed) &
-	    (1U << isp->isp_prev.subdev.entity.id))
+	if (pipe->entities & isp->crashed)
 		return -EIO;
 
 	spin_lock_irqsave(&pipe->lock, flags);
@@ -1014,13 +1011,23 @@ static int isp_pipeline_disable(struct isp_pipeline *pipe)
 		else
 			ret = 0;
 
+		/* Handle stop failures. An entity that fails to stop can
+		 * usually just be restarted. Flag the stop failure nonetheless
+		 * to trigger an ISP reset the next time the device is released,
+		 * just in case.
+		 *
+		 * The preview engine is a special case. A failure to stop can
+		 * mean a hardware crash. When that happens the preview engine
+		 * won't respond to read/write operations on the L4 bus anymore,
+		 * resulting in a bus fault and a kernel oops next time it gets
+		 * accessed. Mark it as crashed to prevent pipelines including
+		 * it from being started.
+		 */
 		if (ret) {
 			dev_info(isp->dev, "Unable to stop %s\n", subdev->name);
-			/* If the entity failed to stopped, assume it has
-			 * crashed. Mark it as such, the ISP will be reset when
-			 * applications will release it.
-			 */
-			isp->crashed |= 1U << subdev->entity.id;
+			isp->stop_failure = true;
+			if (subdev == &isp->isp_prev.subdev)
+				isp->crashed |= 1U << subdev->entity.id;
 			failure = -ETIMEDOUT;
 		}
 	}
@@ -1057,6 +1064,23 @@ int omap3isp_pipeline_set_stream(struct isp_pipeline *pipe,
 }
 
 /*
+ * omap3isp_pipeline_cancel_stream - Cancel stream on a pipeline
+ * @pipe: ISP pipeline
+ *
+ * Cancelling a stream mark all buffers on all video nodes in the pipeline as
+ * erroneous and makes sure no new buffer can be queued. This function is called
+ * when a fatal error that prevents any further operation on the pipeline
+ * occurs.
+ */
+void omap3isp_pipeline_cancel_stream(struct isp_pipeline *pipe)
+{
+	if (pipe->input)
+		omap3isp_video_cancel_stream(pipe->input);
+	if (pipe->output)
+		omap3isp_video_cancel_stream(pipe->output);
+}
+
+/*
  * isp_pipeline_resume - Resume streaming on a pipeline
  * @pipe: ISP pipeline
  *
@@ -1208,6 +1232,7 @@ static int isp_reset(struct isp_device *isp)
 		udelay(1);
 	}
 
+	isp->stop_failure = false;
 	isp->crashed = 0;
 	return 0;
 }
@@ -1619,7 +1644,7 @@ void omap3isp_put(struct isp_device *isp)
 		/* Reset the ISP if an entity has failed to stop. This is the
 		 * only way to recover from such conditions.
 		 */
-		if (isp->crashed)
+		if (isp->crashed || isp->stop_failure)
 			isp_reset(isp);
 		isp_disable_clocks(isp);
 	}
@@ -2130,28 +2155,13 @@ static int isp_map_mem_resource(struct platform_device *pdev,
 	/* request the mem region for the camera registers */
 
 	mem = platform_get_resource(pdev, IORESOURCE_MEM, res);
-	if (!mem) {
-		dev_err(isp->dev, "no mem resource?\n");
-		return -ENODEV;
-	}
-
-	if (!devm_request_mem_region(isp->dev, mem->start, resource_size(mem),
-				     pdev->name)) {
-		dev_err(isp->dev,
-			"cannot reserve camera register I/O region\n");
-		return -ENODEV;
-	}
-	isp->mmio_base_phys[res] = mem->start;
-	isp->mmio_size[res] = resource_size(mem);
 
 	/* map the region */
-	isp->mmio_base[res] = devm_ioremap_nocache(isp->dev,
-						   isp->mmio_base_phys[res],
-						   isp->mmio_size[res]);
-	if (!isp->mmio_base[res]) {
-		dev_err(isp->dev, "cannot map camera register I/O region\n");
-		return -ENODEV;
-	}
+	isp->mmio_base[res] = devm_ioremap_resource(isp->dev, mem);
+	if (IS_ERR(isp->mmio_base[res]))
+		return PTR_ERR(isp->mmio_base[res]);
+
+	isp->mmio_base_phys[res] = mem->start;
 
 	return 0;
 }
diff --git a/drivers/media/platform/omap3isp/isp.h b/drivers/media/platform/omap3isp/isp.h
index d1e857e41731..081f5ec5a663 100644
--- a/drivers/media/platform/omap3isp/isp.h
+++ b/drivers/media/platform/omap3isp/isp.h
@@ -152,9 +152,9 @@ struct isp_xclk {
  *             regions.
  * @mmio_base_phys: Array with physical L4 bus addresses for ISP register
  *                  regions.
- * @mmio_size: Array with ISP register regions size in bytes.
  * @stat_lock: Spinlock for handling statistics
  * @isp_mutex: Mutex for serializing requests to ISP.
+ * @stop_failure: Indicates that an entity failed to stop.
  * @crashed: Bitmask of crashed entities (indexed by entity ID)
  * @has_context: Context has been saved at least once and can be restored.
  * @ref_count: Reference count for handling multiple ISP requests.
@@ -188,11 +188,11 @@ struct isp_device {
 
 	void __iomem *mmio_base[OMAP3_ISP_IOMEM_LAST];
 	unsigned long mmio_base_phys[OMAP3_ISP_IOMEM_LAST];
-	resource_size_t mmio_size[OMAP3_ISP_IOMEM_LAST];
 
 	/* ISP Obj */
 	spinlock_t stat_lock;	/* common lock for statistic drivers */
 	struct mutex isp_mutex;	/* For handling ref_count field */
+	bool stop_failure;
 	u32 crashed;
 	int has_context;
 	int ref_count;
@@ -238,6 +238,7 @@ int omap3isp_module_sync_is_stopping(wait_queue_head_t *wait,
 
 int omap3isp_pipeline_set_stream(struct isp_pipeline *pipe,
 				 enum isp_pipeline_stream_state state);
+void omap3isp_pipeline_cancel_stream(struct isp_pipeline *pipe);
 void omap3isp_configure_bridge(struct isp_device *isp,
 			       enum ccdc_input_entity input,
 			       const struct isp_parallel_platform_data *pdata,
diff --git a/drivers/media/platform/omap3isp/ispccdc.c b/drivers/media/platform/omap3isp/ispccdc.c
index 907a205da5a5..5db2c88b9ad8 100644
--- a/drivers/media/platform/omap3isp/ispccdc.c
+++ b/drivers/media/platform/omap3isp/ispccdc.c
@@ -1516,6 +1516,8 @@ static int ccdc_isr_buffer(struct isp_ccdc_device *ccdc)
 
 	if (ccdc_sbl_wait_idle(ccdc, 1000)) {
 		dev_info(isp->dev, "CCDC won't become idle!\n");
+		isp->crashed |= 1U << ccdc->subdev.entity.id;
+		omap3isp_pipeline_cancel_stream(pipe);
 		goto done;
 	}
 
@@ -2484,7 +2486,8 @@ static int ccdc_init_entities(struct isp_ccdc_device *ccdc)
 	v4l2_set_subdevdata(sd, ccdc);
 	sd->flags |= V4L2_SUBDEV_FL_HAS_EVENTS | V4L2_SUBDEV_FL_HAS_DEVNODE;
 
-	pads[CCDC_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	pads[CCDC_PAD_SINK].flags = MEDIA_PAD_FL_SINK
+				    | MEDIA_PAD_FL_MUST_CONNECT;
 	pads[CCDC_PAD_SOURCE_VP].flags = MEDIA_PAD_FL_SOURCE;
 	pads[CCDC_PAD_SOURCE_OF].flags = MEDIA_PAD_FL_SOURCE;
 
diff --git a/drivers/media/platform/omap3isp/ispccp2.c b/drivers/media/platform/omap3isp/ispccp2.c
index e71651429dda..e84fe0543e47 100644
--- a/drivers/media/platform/omap3isp/ispccp2.c
+++ b/drivers/media/platform/omap3isp/ispccp2.c
@@ -1076,7 +1076,8 @@ static int ccp2_init_entities(struct isp_ccp2_device *ccp2)
 	v4l2_set_subdevdata(sd, ccp2);
 	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 
-	pads[CCP2_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	pads[CCP2_PAD_SINK].flags = MEDIA_PAD_FL_SINK
+				    | MEDIA_PAD_FL_MUST_CONNECT;
 	pads[CCP2_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
 
 	me->ops = &ccp2_media_ops;
diff --git a/drivers/media/platform/omap3isp/ispcsi2.c b/drivers/media/platform/omap3isp/ispcsi2.c
index 6db245d84bbb..620560828a48 100644
--- a/drivers/media/platform/omap3isp/ispcsi2.c
+++ b/drivers/media/platform/omap3isp/ispcsi2.c
@@ -1245,7 +1245,8 @@ static int csi2_init_entities(struct isp_csi2_device *csi2)
 	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 
 	pads[CSI2_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
-	pads[CSI2_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	pads[CSI2_PAD_SINK].flags = MEDIA_PAD_FL_SINK
+				    | MEDIA_PAD_FL_MUST_CONNECT;
 
 	me->ops = &csi2_media_ops;
 	ret = media_entity_init(me, CSI2_PADS_NUM, pads, 0);
diff --git a/drivers/media/platform/omap3isp/isppreview.c b/drivers/media/platform/omap3isp/isppreview.c
index cd8831aebdeb..1c776c1186f1 100644
--- a/drivers/media/platform/omap3isp/isppreview.c
+++ b/drivers/media/platform/omap3isp/isppreview.c
@@ -2283,7 +2283,8 @@ static int preview_init_entities(struct isp_prev_device *prev)
 	v4l2_ctrl_handler_setup(&prev->ctrls);
 	sd->ctrl_handler = &prev->ctrls;
 
-	pads[PREV_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	pads[PREV_PAD_SINK].flags = MEDIA_PAD_FL_SINK
+				    | MEDIA_PAD_FL_MUST_CONNECT;
 	pads[PREV_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
 
 	me->ops = &preview_media_ops;
diff --git a/drivers/media/platform/omap3isp/ispqueue.c b/drivers/media/platform/omap3isp/ispqueue.c
index e15f01342058..5f0f8fab1d17 100644
--- a/drivers/media/platform/omap3isp/ispqueue.c
+++ b/drivers/media/platform/omap3isp/ispqueue.c
@@ -553,8 +553,10 @@ static void isp_video_buffer_query(struct isp_video_buffer *buf,
 	switch (buf->state) {
 	case ISP_BUF_STATE_ERROR:
 		vbuf->flags |= V4L2_BUF_FLAG_ERROR;
+		/* Fallthrough */
 	case ISP_BUF_STATE_DONE:
 		vbuf->flags |= V4L2_BUF_FLAG_DONE;
+		break;
 	case ISP_BUF_STATE_QUEUED:
 	case ISP_BUF_STATE_ACTIVE:
 		vbuf->flags |= V4L2_BUF_FLAG_QUEUED;
diff --git a/drivers/media/platform/omap3isp/ispresizer.c b/drivers/media/platform/omap3isp/ispresizer.c
index d11fb261d530..0d36b8bc9f98 100644
--- a/drivers/media/platform/omap3isp/ispresizer.c
+++ b/drivers/media/platform/omap3isp/ispresizer.c
@@ -1532,6 +1532,20 @@ static int resizer_set_format(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
 	return 0;
 }
 
+static int resizer_link_validate(struct v4l2_subdev *sd,
+				 struct media_link *link,
+				 struct v4l2_subdev_format *source_fmt,
+				 struct v4l2_subdev_format *sink_fmt)
+{
+	struct isp_res_device *res = v4l2_get_subdevdata(sd);
+	struct isp_pipeline *pipe = to_isp_pipeline(&sd->entity);
+
+	omap3isp_resizer_max_rate(res, &pipe->max_rate);
+
+	return v4l2_subdev_link_validate_default(sd, link,
+						 source_fmt, sink_fmt);
+}
+
 /*
  * resizer_init_formats - Initialize formats on all pads
  * @sd: ISP resizer V4L2 subdevice
@@ -1570,6 +1584,7 @@ static const struct v4l2_subdev_pad_ops resizer_v4l2_pad_ops = {
 	.set_fmt = resizer_set_format,
 	.get_selection = resizer_get_selection,
 	.set_selection = resizer_set_selection,
+	.link_validate = resizer_link_validate,
 };
 
 /* subdev operations */
@@ -1701,7 +1716,8 @@ static int resizer_init_entities(struct isp_res_device *res)
 	v4l2_set_subdevdata(sd, res);
 	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 
-	pads[RESZ_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	pads[RESZ_PAD_SINK].flags = MEDIA_PAD_FL_SINK
+				    | MEDIA_PAD_FL_MUST_CONNECT;
 	pads[RESZ_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
 
 	me->ops = &resizer_media_ops;
diff --git a/drivers/media/platform/omap3isp/ispstat.c b/drivers/media/platform/omap3isp/ispstat.c
index 61e17f9bd8b9..a75407c3a726 100644
--- a/drivers/media/platform/omap3isp/ispstat.c
+++ b/drivers/media/platform/omap3isp/ispstat.c
@@ -1067,7 +1067,7 @@ static int isp_stat_init_entities(struct ispstat *stat, const char *name,
 	subdev->flags |= V4L2_SUBDEV_FL_HAS_EVENTS | V4L2_SUBDEV_FL_HAS_DEVNODE;
 	v4l2_set_subdevdata(subdev, stat);
 
-	stat->pad.flags = MEDIA_PAD_FL_SINK;
+	stat->pad.flags = MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT;
 	me->ops = NULL;
 
 	return media_entity_init(me, 1, &stat->pad, 0);
diff --git a/drivers/media/platform/omap3isp/ispvideo.c b/drivers/media/platform/omap3isp/ispvideo.c
index f6304bb074f5..856fdf554035 100644
--- a/drivers/media/platform/omap3isp/ispvideo.c
+++ b/drivers/media/platform/omap3isp/ispvideo.c
@@ -278,55 +278,6 @@ static int isp_video_get_graph_data(struct isp_video *video,
 	return 0;
 }
 
-/*
- * Validate a pipeline by checking both ends of all links for format
- * discrepancies.
- *
- * Compute the minimum time per frame value as the maximum of time per frame
- * limits reported by every block in the pipeline.
- *
- * Return 0 if all formats match, or -EPIPE if at least one link is found with
- * different formats on its two ends or if the pipeline doesn't start with a
- * video source (either a subdev with no input pad, or a non-subdev entity).
- */
-static int isp_video_validate_pipeline(struct isp_pipeline *pipe)
-{
-	struct isp_device *isp = pipe->output->isp;
-	struct media_pad *pad;
-	struct v4l2_subdev *subdev;
-
-	subdev = isp_video_remote_subdev(pipe->output, NULL);
-	if (subdev == NULL)
-		return -EPIPE;
-
-	while (1) {
-		/* Retrieve the sink format */
-		pad = &subdev->entity.pads[0];
-		if (!(pad->flags & MEDIA_PAD_FL_SINK))
-			break;
-
-		/* Update the maximum frame rate */
-		if (subdev == &isp->isp_res.subdev)
-			omap3isp_resizer_max_rate(&isp->isp_res,
-						  &pipe->max_rate);
-
-		/* Retrieve the source format. Return an error if no source
-		 * entity can be found, and stop checking the pipeline if the
-		 * source entity isn't a subdev.
-		 */
-		pad = media_entity_remote_pad(pad);
-		if (pad == NULL)
-			return -EPIPE;
-
-		if (media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
-			break;
-
-		subdev = media_entity_to_v4l2_subdev(pad->entity);
-	}
-
-	return 0;
-}
-
 static int
 __isp_video_get_format(struct isp_video *video, struct v4l2_format *format)
 {
@@ -460,6 +411,15 @@ static int isp_video_buffer_prepare(struct isp_video_buffer *buf)
 	struct isp_video *video = vfh->video;
 	unsigned long addr;
 
+	/* Refuse to prepare the buffer is the video node has registered an
+	 * error. We don't need to take any lock here as the operation is
+	 * inherently racy. The authoritative check will be performed in the
+	 * queue handler, which can't return an error, this check is just a best
+	 * effort to notify userspace as early as possible.
+	 */
+	if (unlikely(video->error))
+		return -EIO;
+
 	addr = ispmmu_vmap(video->isp, buf->sglist, buf->sglen);
 	if (IS_ERR_VALUE(addr))
 		return -EIO;
@@ -496,6 +456,12 @@ static void isp_video_buffer_queue(struct isp_video_buffer *buf)
 	unsigned int empty;
 	unsigned int start;
 
+	if (unlikely(video->error)) {
+		buf->state = ISP_BUF_STATE_ERROR;
+		wake_up(&buf->wait);
+		return;
+	}
+
 	empty = list_empty(&video->dmaqueue);
 	list_add_tail(&buffer->buffer.irqlist, &video->dmaqueue);
 
@@ -618,6 +584,36 @@ struct isp_buffer *omap3isp_video_buffer_next(struct isp_video *video)
 }
 
 /*
+ * omap3isp_video_cancel_stream - Cancel stream on a video node
+ * @video: ISP video object
+ *
+ * Cancelling a stream mark all buffers on the video node as erroneous and makes
+ * sure no new buffer can be queued.
+ */
+void omap3isp_video_cancel_stream(struct isp_video *video)
+{
+	struct isp_video_queue *queue = video->queue;
+	unsigned long flags;
+
+	spin_lock_irqsave(&queue->irqlock, flags);
+
+	while (!list_empty(&video->dmaqueue)) {
+		struct isp_video_buffer *buf;
+
+		buf = list_first_entry(&video->dmaqueue,
+				       struct isp_video_buffer, irqlist);
+		list_del(&buf->irqlist);
+
+		buf->state = ISP_BUF_STATE_ERROR;
+		wake_up(&buf->wait);
+	}
+
+	video->error = true;
+
+	spin_unlock_irqrestore(&queue->irqlock, flags);
+}
+
+/*
  * omap3isp_video_resume - Perform resume operation on the buffers
  * @video: ISP video object
  * @continuous: Pipeline is in single shot mode if 0 or continuous mode otherwise
@@ -1051,11 +1047,6 @@ isp_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
 	if (ret < 0)
 		goto err_check_format;
 
-	/* Validate the pipeline and update its state. */
-	ret = isp_video_validate_pipeline(pipe);
-	if (ret < 0)
-		goto err_check_format;
-
 	pipe->error = false;
 
 	spin_lock_irqsave(&pipe->lock, flags);
@@ -1159,6 +1150,7 @@ isp_video_streamoff(struct file *file, void *fh, enum v4l2_buf_type type)
 	omap3isp_video_queue_streamoff(&vfh->queue);
 	video->queue = NULL;
 	video->streaming = 0;
+	video->error = false;
 
 	if (video->isp->pdata->set_constraints)
 		video->isp->pdata->set_constraints(video->isp, false);
@@ -1332,11 +1324,13 @@ int omap3isp_video_init(struct isp_video *video, const char *name)
 	switch (video->type) {
 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 		direction = "output";
-		video->pad.flags = MEDIA_PAD_FL_SINK;
+		video->pad.flags = MEDIA_PAD_FL_SINK
+				   | MEDIA_PAD_FL_MUST_CONNECT;
 		break;
 	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 		direction = "input";
-		video->pad.flags = MEDIA_PAD_FL_SOURCE;
+		video->pad.flags = MEDIA_PAD_FL_SOURCE
+				   | MEDIA_PAD_FL_MUST_CONNECT;
 		video->video.vfl_dir = VFL_DIR_TX;
 		break;
 
diff --git a/drivers/media/platform/omap3isp/ispvideo.h b/drivers/media/platform/omap3isp/ispvideo.h
index 1ad470ec2b9d..4e194076cc60 100644
--- a/drivers/media/platform/omap3isp/ispvideo.h
+++ b/drivers/media/platform/omap3isp/ispvideo.h
@@ -178,6 +178,7 @@ struct isp_video {
 	/* Pipeline state */
 	struct isp_pipeline pipe;
 	struct mutex stream_lock;	/* pipeline and stream states */
+	bool error;
 
 	/* Video buffers queue */
 	struct isp_video_queue *queue;
@@ -207,6 +208,7 @@ int omap3isp_video_register(struct isp_video *video,
 			    struct v4l2_device *vdev);
 void omap3isp_video_unregister(struct isp_video *video);
 struct isp_buffer *omap3isp_video_buffer_next(struct isp_video *video);
+void omap3isp_video_cancel_stream(struct isp_video *video);
 void omap3isp_video_resume(struct isp_video *video, int continuous);
 struct media_pad *omap3isp_video_remote_pad(struct isp_video *video);
 
diff --git a/drivers/media/platform/s5p-g2d/g2d.c b/drivers/media/platform/s5p-g2d/g2d.c
index 0b2948376aee..0fcf7d75e841 100644
--- a/drivers/media/platform/s5p-g2d/g2d.c
+++ b/drivers/media/platform/s5p-g2d/g2d.c
@@ -136,10 +136,9 @@ static int g2d_buf_prepare(struct vb2_buffer *vb)
 static void g2d_buf_queue(struct vb2_buffer *vb)
 {
 	struct g2d_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
-	v4l2_m2m_buf_queue(ctx->m2m_ctx, vb);
+	v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vb);
 }
 
-
 static struct vb2_ops g2d_qops = {
 	.queue_setup	= g2d_queue_setup,
 	.buf_prepare	= g2d_buf_prepare,
@@ -159,6 +158,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	src_vq->mem_ops = &vb2_dma_contig_memops;
 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	src_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	src_vq->lock = &ctx->dev->mutex;
 
 	ret = vb2_queue_init(src_vq);
 	if (ret)
@@ -171,6 +171,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	dst_vq->mem_ops = &vb2_dma_contig_memops;
 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 	dst_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	dst_vq->lock = &ctx->dev->mutex;
 
 	return vb2_queue_init(dst_vq);
 }
@@ -253,9 +254,9 @@ static int g2d_open(struct file *file)
 		kfree(ctx);
 		return -ERESTARTSYS;
 	}
-	ctx->m2m_ctx = v4l2_m2m_ctx_init(dev->m2m_dev, ctx, &queue_init);
-	if (IS_ERR(ctx->m2m_ctx)) {
-		ret = PTR_ERR(ctx->m2m_ctx);
+	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->m2m_dev, ctx, &queue_init);
+	if (IS_ERR(ctx->fh.m2m_ctx)) {
+		ret = PTR_ERR(ctx->fh.m2m_ctx);
 		mutex_unlock(&dev->mutex);
 		kfree(ctx);
 		return ret;
@@ -324,7 +325,7 @@ static int vidioc_g_fmt(struct file *file, void *prv, struct v4l2_format *f)
 	struct vb2_queue *vq;
 	struct g2d_frame *frm;
 
-	vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
 	if (!vq)
 		return -EINVAL;
 	frm = get_frame(ctx, f->type);
@@ -384,7 +385,7 @@ static int vidioc_s_fmt(struct file *file, void *prv, struct v4l2_format *f)
 	ret = vidioc_try_fmt(file, prv, f);
 	if (ret)
 		return ret;
-	vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
 	if (vb2_is_busy(vq)) {
 		v4l2_err(&dev->v4l2_dev, "queue (%d) bust\n", f->type);
 		return -EBUSY;
@@ -410,72 +411,6 @@ static int vidioc_s_fmt(struct file *file, void *prv, struct v4l2_format *f)
 	return 0;
 }
 
-static unsigned int g2d_poll(struct file *file, struct poll_table_struct *wait)
-{
-	struct g2d_ctx *ctx = fh2ctx(file->private_data);
-	struct g2d_dev *dev = ctx->dev;
-	unsigned int res;
-
-	mutex_lock(&dev->mutex);
-	res = v4l2_m2m_poll(file, ctx->m2m_ctx, wait);
-	mutex_unlock(&dev->mutex);
-	return res;
-}
-
-static int g2d_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	struct g2d_ctx *ctx = fh2ctx(file->private_data);
-	struct g2d_dev *dev = ctx->dev;
-	int ret;
-
-	if (mutex_lock_interruptible(&dev->mutex))
-		return -ERESTARTSYS;
-	ret = v4l2_m2m_mmap(file, ctx->m2m_ctx, vma);
-	mutex_unlock(&dev->mutex);
-	return ret;
-}
-
-static int vidioc_reqbufs(struct file *file, void *priv,
-			struct v4l2_requestbuffers *reqbufs)
-{
-	struct g2d_ctx *ctx = priv;
-	return v4l2_m2m_reqbufs(file, ctx->m2m_ctx, reqbufs);
-}
-
-static int vidioc_querybuf(struct file *file, void *priv,
-			struct v4l2_buffer *buf)
-{
-	struct g2d_ctx *ctx = priv;
-	return v4l2_m2m_querybuf(file, ctx->m2m_ctx, buf);
-}
-
-static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
-{
-	struct g2d_ctx *ctx = priv;
-	return v4l2_m2m_qbuf(file, ctx->m2m_ctx, buf);
-}
-
-static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
-{
-	struct g2d_ctx *ctx = priv;
-	return v4l2_m2m_dqbuf(file, ctx->m2m_ctx, buf);
-}
-
-
-static int vidioc_streamon(struct file *file, void *priv,
-					enum v4l2_buf_type type)
-{
-	struct g2d_ctx *ctx = priv;
-	return v4l2_m2m_streamon(file, ctx->m2m_ctx, type);
-}
-
-static int vidioc_streamoff(struct file *file, void *priv,
-					enum v4l2_buf_type type)
-{
-	struct g2d_ctx *ctx = priv;
-	return v4l2_m2m_streamoff(file, ctx->m2m_ctx, type);
-}
-
 static int vidioc_cropcap(struct file *file, void *priv,
 					struct v4l2_cropcap *cr)
 {
@@ -551,20 +486,6 @@ static int vidioc_s_crop(struct file *file, void *prv, const struct v4l2_crop *c
 	return 0;
 }
 
-static void g2d_lock(void *prv)
-{
-	struct g2d_ctx *ctx = prv;
-	struct g2d_dev *dev = ctx->dev;
-	mutex_lock(&dev->mutex);
-}
-
-static void g2d_unlock(void *prv)
-{
-	struct g2d_ctx *ctx = prv;
-	struct g2d_dev *dev = ctx->dev;
-	mutex_unlock(&dev->mutex);
-}
-
 static void job_abort(void *prv)
 {
 	struct g2d_ctx *ctx = prv;
@@ -589,8 +510,8 @@ static void device_run(void *prv)
 
 	dev->curr = ctx;
 
-	src = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
-	dst = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
+	src = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+	dst = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
 
 	clk_enable(dev->gate);
 	g2d_reset(dev);
@@ -631,8 +552,8 @@ static irqreturn_t g2d_isr(int irq, void *prv)
 
 	BUG_ON(ctx == NULL);
 
-	src = v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
-	dst = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
+	src = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+	dst = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
 
 	BUG_ON(src == NULL);
 	BUG_ON(dst == NULL);
@@ -642,7 +563,7 @@ static irqreturn_t g2d_isr(int irq, void *prv)
 
 	v4l2_m2m_buf_done(src, VB2_BUF_STATE_DONE);
 	v4l2_m2m_buf_done(dst, VB2_BUF_STATE_DONE);
-	v4l2_m2m_job_finish(dev->m2m_dev, ctx->m2m_ctx);
+	v4l2_m2m_job_finish(dev->m2m_dev, ctx->fh.m2m_ctx);
 
 	dev->curr = NULL;
 	wake_up(&dev->irq_queue);
@@ -653,9 +574,9 @@ static const struct v4l2_file_operations g2d_fops = {
 	.owner		= THIS_MODULE,
 	.open		= g2d_open,
 	.release	= g2d_release,
-	.poll		= g2d_poll,
+	.poll		= v4l2_m2m_fop_poll,
 	.unlocked_ioctl	= video_ioctl2,
-	.mmap		= g2d_mmap,
+	.mmap		= v4l2_m2m_fop_mmap,
 };
 
 static const struct v4l2_ioctl_ops g2d_ioctl_ops = {
@@ -671,14 +592,13 @@ static const struct v4l2_ioctl_ops g2d_ioctl_ops = {
 	.vidioc_try_fmt_vid_out		= vidioc_try_fmt,
 	.vidioc_s_fmt_vid_out		= vidioc_s_fmt,
 
-	.vidioc_reqbufs			= vidioc_reqbufs,
-	.vidioc_querybuf		= vidioc_querybuf,
-
-	.vidioc_qbuf			= vidioc_qbuf,
-	.vidioc_dqbuf			= vidioc_dqbuf,
+	.vidioc_reqbufs			= v4l2_m2m_ioctl_reqbufs,
+	.vidioc_querybuf		= v4l2_m2m_ioctl_querybuf,
+	.vidioc_qbuf			= v4l2_m2m_ioctl_qbuf,
+	.vidioc_dqbuf			= v4l2_m2m_ioctl_dqbuf,
 
-	.vidioc_streamon		= vidioc_streamon,
-	.vidioc_streamoff		= vidioc_streamoff,
+	.vidioc_streamon		= v4l2_m2m_ioctl_streamon,
+	.vidioc_streamoff		= v4l2_m2m_ioctl_streamoff,
 
 	.vidioc_g_crop			= vidioc_g_crop,
 	.vidioc_s_crop			= vidioc_s_crop,
@@ -697,8 +617,6 @@ static struct video_device g2d_videodev = {
 static struct v4l2_m2m_ops g2d_m2m_ops = {
 	.device_run	= device_run,
 	.job_abort	= job_abort,
-	.lock		= g2d_lock,
-	.unlock		= g2d_unlock,
 };
 
 static const struct of_device_id exynos_g2d_match[];
diff --git a/drivers/media/platform/s5p-g2d/g2d.h b/drivers/media/platform/s5p-g2d/g2d.h
index 300ca05ba404..b0e52ab7ecdb 100644
--- a/drivers/media/platform/s5p-g2d/g2d.h
+++ b/drivers/media/platform/s5p-g2d/g2d.h
@@ -57,7 +57,6 @@ struct g2d_frame {
 struct g2d_ctx {
 	struct v4l2_fh fh;
 	struct g2d_dev		*dev;
-	struct v4l2_m2m_ctx	*m2m_ctx;
 	struct g2d_frame	in;
 	struct g2d_frame	out;
 	struct v4l2_ctrl	*ctrl_hflip;
diff --git a/drivers/media/platform/s5p-jpeg/Makefile b/drivers/media/platform/s5p-jpeg/Makefile
index d18cb5edd2d5..a1a9169254c3 100644
--- a/drivers/media/platform/s5p-jpeg/Makefile
+++ b/drivers/media/platform/s5p-jpeg/Makefile
@@ -1,2 +1,2 @@
-s5p-jpeg-objs := jpeg-core.o
+s5p-jpeg-objs := jpeg-core.o jpeg-hw-exynos4.o jpeg-hw-s5p.o
 obj-$(CONFIG_VIDEO_SAMSUNG_S5P_JPEG) += s5p-jpeg.o
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-core.c b/drivers/media/platform/s5p-jpeg/jpeg-core.c
index 9b88a4601007..7d68d0b9966a 100644
--- a/drivers/media/platform/s5p-jpeg/jpeg-core.c
+++ b/drivers/media/platform/s5p-jpeg/jpeg-core.c
@@ -1,9 +1,10 @@
 /* linux/drivers/media/platform/s5p-jpeg/jpeg-core.c
  *
- * Copyright (c) 2011 Samsung Electronics Co., Ltd.
+ * Copyright (c) 2011-2013 Samsung Electronics Co., Ltd.
  *		http://www.samsung.com
  *
  * Author: Andrzej Pietrasiewicz <andrzej.p@samsung.com>
+ * Author: Jacek Anaszewski <j.anaszewski@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
@@ -17,6 +18,7 @@
 #include <linux/io.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/slab.h>
@@ -28,70 +30,234 @@
 #include <media/videobuf2-dma-contig.h>
 
 #include "jpeg-core.h"
-#include "jpeg-hw.h"
+#include "jpeg-hw-s5p.h"
+#include "jpeg-hw-exynos4.h"
+#include "jpeg-regs.h"
 
-static struct s5p_jpeg_fmt formats_enc[] = {
+static struct s5p_jpeg_fmt sjpeg_formats[] = {
 	{
 		.name		= "JPEG JFIF",
 		.fourcc		= V4L2_PIX_FMT_JPEG,
+		.flags		= SJPEG_FMT_FLAG_ENC_CAPTURE |
+				  SJPEG_FMT_FLAG_DEC_OUTPUT |
+				  SJPEG_FMT_FLAG_S5P |
+				  SJPEG_FMT_FLAG_EXYNOS4,
+	},
+	{
+		.name		= "YUV 4:2:2 packed, YCbYCr",
+		.fourcc		= V4L2_PIX_FMT_YUYV,
+		.depth		= 16,
 		.colplanes	= 1,
-		.types		= MEM2MEM_CAPTURE,
+		.h_align	= 4,
+		.v_align	= 3,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_S5P |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_422,
 	},
 	{
 		.name		= "YUV 4:2:2 packed, YCbYCr",
 		.fourcc		= V4L2_PIX_FMT_YUYV,
 		.depth		= 16,
 		.colplanes	= 1,
-		.types		= MEM2MEM_OUTPUT,
+		.h_align	= 1,
+		.v_align	= 0,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS4 |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_422,
+	},
+	{
+		.name		= "YUV 4:2:2 packed, YCrYCb",
+		.fourcc		= V4L2_PIX_FMT_YVYU,
+		.depth		= 16,
+		.colplanes	= 1,
+		.h_align	= 1,
+		.v_align	= 0,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS4 |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_422,
 	},
 	{
 		.name		= "RGB565",
 		.fourcc		= V4L2_PIX_FMT_RGB565,
 		.depth		= 16,
 		.colplanes	= 1,
-		.types		= MEM2MEM_OUTPUT,
+		.h_align	= 0,
+		.v_align	= 0,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS4 |
+				  SJPEG_FMT_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_444,
 	},
-};
-#define NUM_FORMATS_ENC ARRAY_SIZE(formats_enc)
-
-static struct s5p_jpeg_fmt formats_dec[] = {
 	{
-		.name		= "YUV 4:2:0 planar, YCbCr",
-		.fourcc		= V4L2_PIX_FMT_YUV420,
+		.name		= "RGB565",
+		.fourcc		= V4L2_PIX_FMT_RGB565,
+		.depth		= 16,
+		.colplanes	= 1,
+		.h_align	= 0,
+		.v_align	= 0,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_S5P |
+				  SJPEG_FMT_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_444,
+	},
+	{
+		.name		= "ARGB8888, 32 bpp",
+		.fourcc		= V4L2_PIX_FMT_RGB32,
+		.depth		= 32,
+		.colplanes	= 1,
+		.h_align	= 0,
+		.v_align	= 0,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS4 |
+				  SJPEG_FMT_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_444,
+	},
+	{
+		.name		= "YUV 4:4:4 planar, Y/CbCr",
+		.fourcc		= V4L2_PIX_FMT_NV24,
+		.depth		= 24,
+		.colplanes	= 2,
+		.h_align	= 0,
+		.v_align	= 0,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS4 |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_444,
+	},
+	{
+		.name		= "YUV 4:4:4 planar, Y/CrCb",
+		.fourcc		= V4L2_PIX_FMT_NV42,
+		.depth		= 24,
+		.colplanes	= 2,
+		.h_align	= 0,
+		.v_align	= 0,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS4 |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_444,
+	},
+	{
+		.name		= "YUV 4:2:2 planar, Y/CrCb",
+		.fourcc		= V4L2_PIX_FMT_NV61,
+		.depth		= 16,
+		.colplanes	= 2,
+		.h_align	= 1,
+		.v_align	= 0,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS4 |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_422,
+	},
+	{
+		.name		= "YUV 4:2:2 planar, Y/CbCr",
+		.fourcc		= V4L2_PIX_FMT_NV16,
+		.depth		= 16,
+		.colplanes	= 2,
+		.h_align	= 1,
+		.v_align	= 0,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS4 |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_422,
+	},
+	{
+		.name		= "YUV 4:2:0 planar, Y/CbCr",
+		.fourcc		= V4L2_PIX_FMT_NV12,
 		.depth		= 12,
-		.colplanes	= 3,
+		.colplanes	= 2,
+		.h_align	= 1,
+		.v_align	= 1,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS4 |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_420,
+	},
+	{
+		.name		= "YUV 4:2:0 planar, Y/CbCr",
+		.fourcc		= V4L2_PIX_FMT_NV12,
+		.depth		= 12,
+		.colplanes	= 2,
 		.h_align	= 4,
 		.v_align	= 4,
-		.types		= MEM2MEM_CAPTURE,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_S5P |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_420,
 	},
 	{
-		.name		= "YUV 4:2:2 packed, YCbYCr",
-		.fourcc		= V4L2_PIX_FMT_YUYV,
-		.depth		= 16,
-		.colplanes	= 1,
-		.h_align	= 4,
-		.v_align	= 3,
-		.types		= MEM2MEM_CAPTURE,
+		.name		= "YUV 4:2:0 planar, Y/CrCb",
+		.fourcc		= V4L2_PIX_FMT_NV21,
+		.depth		= 12,
+		.colplanes	= 2,
+		.h_align	= 1,
+		.v_align	= 1,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS4 |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_420,
 	},
 	{
-		.name		= "JPEG JFIF",
-		.fourcc		= V4L2_PIX_FMT_JPEG,
+		.name		= "YUV 4:2:0 contiguous 3-planar, Y/Cb/Cr",
+		.fourcc		= V4L2_PIX_FMT_YUV420,
+		.depth		= 12,
+		.colplanes	= 3,
+		.h_align	= 1,
+		.v_align	= 1,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS4 |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_420,
+	},
+	{
+		.name		= "Gray",
+		.fourcc		= V4L2_PIX_FMT_GREY,
+		.depth		= 8,
 		.colplanes	= 1,
-		.types		= MEM2MEM_OUTPUT,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS4 |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_GRAY,
 	},
 };
-#define NUM_FORMATS_DEC ARRAY_SIZE(formats_dec)
+#define SJPEG_NUM_FORMATS ARRAY_SIZE(sjpeg_formats)
 
 static const unsigned char qtbl_luminance[4][64] = {
-	{/* level 1 - high quality */
-		 8,  6,  6,  8, 12, 14, 16, 17,
-		 6,  6,  6,  8, 10, 13, 12, 15,
-		 6,  6,  7,  8, 13, 14, 18, 24,
-		 8,  8,  8, 14, 13, 19, 24, 35,
-		12, 10, 13, 13, 20, 26, 34, 39,
-		14, 13, 14, 19, 26, 34, 39, 39,
-		16, 12, 18, 24, 34, 39, 39, 39,
-		17, 15, 24, 35, 39, 39, 39, 39
+	{/*level 0 - high compression quality */
+		20, 16, 25, 39, 50, 46, 62, 68,
+		16, 18, 23, 38, 38, 53, 65, 68,
+		25, 23, 31, 38, 53, 65, 68, 68,
+		39, 38, 38, 53, 65, 68, 68, 68,
+		50, 38, 53, 65, 68, 68, 68, 68,
+		46, 53, 65, 68, 68, 68, 68, 68,
+		62, 65, 68, 68, 68, 68, 68, 68,
+		68, 68, 68, 68, 68, 68, 68, 68
+	},
+	{/* level 1 */
+		16, 11, 11, 16, 23, 27, 31, 30,
+		11, 12, 12, 15, 20, 23, 23, 30,
+		11, 12, 13, 16, 23, 26, 35, 47,
+		16, 15, 16, 23, 26, 37, 47, 64,
+		23, 20, 23, 26, 39, 51, 64, 64,
+		27, 23, 26, 37, 51, 64, 64, 64,
+		31, 23, 35, 47, 64, 64, 64, 64,
+		30, 30, 47, 64, 64, 64, 64, 64
 	},
 	{/* level 2 */
 		12,  8,  8, 12, 17, 21, 24, 23,
@@ -103,38 +269,38 @@ static const unsigned char qtbl_luminance[4][64] = {
 		24, 18, 27, 36, 51, 59, 59, 59,
 		23, 23, 36, 53, 59, 59, 59, 59
 	},
-	{/* level 3 */
-		16, 11, 11, 16, 23, 27, 31, 30,
-		11, 12, 12, 15, 20, 23, 23, 30,
-		11, 12, 13, 16, 23, 26, 35, 47,
-		16, 15, 16, 23, 26, 37, 47, 64,
-		23, 20, 23, 26, 39, 51, 64, 64,
-		27, 23, 26, 37, 51, 64, 64, 64,
-		31, 23, 35, 47, 64, 64, 64, 64,
-		30, 30, 47, 64, 64, 64, 64, 64
-	},
-	{/*level 4 - low quality */
-		20, 16, 25, 39, 50, 46, 62, 68,
-		16, 18, 23, 38, 38, 53, 65, 68,
-		25, 23, 31, 38, 53, 65, 68, 68,
-		39, 38, 38, 53, 65, 68, 68, 68,
-		50, 38, 53, 65, 68, 68, 68, 68,
-		46, 53, 65, 68, 68, 68, 68, 68,
-		62, 65, 68, 68, 68, 68, 68, 68,
-		68, 68, 68, 68, 68, 68, 68, 68
+	{/* level 3 - low compression quality */
+		 8,  6,  6,  8, 12, 14, 16, 17,
+		 6,  6,  6,  8, 10, 13, 12, 15,
+		 6,  6,  7,  8, 13, 14, 18, 24,
+		 8,  8,  8, 14, 13, 19, 24, 35,
+		12, 10, 13, 13, 20, 26, 34, 39,
+		14, 13, 14, 19, 26, 34, 39, 39,
+		16, 12, 18, 24, 34, 39, 39, 39,
+		17, 15, 24, 35, 39, 39, 39, 39
 	}
 };
 
 static const unsigned char qtbl_chrominance[4][64] = {
-	{/* level 1 - high quality */
-		 9,  8,  9, 11, 14, 17, 19, 24,
-		 8, 10,  9, 11, 14, 13, 17, 22,
-		 9,  9, 13, 14, 13, 15, 23, 26,
-		11, 11, 14, 14, 15, 20, 26, 33,
-		14, 14, 13, 15, 20, 24, 33, 39,
-		17, 13, 15, 20, 24, 32, 39, 39,
-		19, 17, 23, 26, 33, 39, 39, 39,
-		24, 22, 26, 33, 39, 39, 39, 39
+	{/*level 0 - high compression quality */
+		21, 25, 32, 38, 54, 68, 68, 68,
+		25, 28, 24, 38, 54, 68, 68, 68,
+		32, 24, 32, 43, 66, 68, 68, 68,
+		38, 38, 43, 53, 68, 68, 68, 68,
+		54, 54, 66, 68, 68, 68, 68, 68,
+		68, 68, 68, 68, 68, 68, 68, 68,
+		68, 68, 68, 68, 68, 68, 68, 68,
+		68, 68, 68, 68, 68, 68, 68, 68
+	},
+	{/* level 1 */
+		17, 15, 17, 21, 20, 26, 38, 48,
+		15, 19, 18, 17, 20, 26, 35, 43,
+		17, 18, 20, 22, 26, 30, 46, 53,
+		21, 17, 22, 28, 30, 39, 53, 64,
+		20, 20, 26, 30, 39, 48, 64, 64,
+		26, 26, 30, 39, 48, 63, 64, 64,
+		38, 35, 46, 53, 64, 64, 64, 64,
+		48, 43, 53, 64, 64, 64, 64, 64
 	},
 	{/* level 2 */
 		13, 11, 13, 16, 20, 20, 29, 37,
@@ -146,25 +312,15 @@ static const unsigned char qtbl_chrominance[4][64] = {
 		29, 26, 35, 40, 50, 59, 59, 59,
 		37, 32, 40, 50, 59, 59, 59, 59
 	},
-	{/* level 3 */
-		17, 15, 17, 21, 20, 26, 38, 48,
-		15, 19, 18, 17, 20, 26, 35, 43,
-		17, 18, 20, 22, 26, 30, 46, 53,
-		21, 17, 22, 28, 30, 39, 53, 64,
-		20, 20, 26, 30, 39, 48, 64, 64,
-		26, 26, 30, 39, 48, 63, 64, 64,
-		38, 35, 46, 53, 64, 64, 64, 64,
-		48, 43, 53, 64, 64, 64, 64, 64
-	},
-	{/*level 4 - low quality */
-		21, 25, 32, 38, 54, 68, 68, 68,
-		25, 28, 24, 38, 54, 68, 68, 68,
-		32, 24, 32, 43, 66, 68, 68, 68,
-		38, 38, 43, 53, 68, 68, 68, 68,
-		54, 54, 66, 68, 68, 68, 68, 68,
-		68, 68, 68, 68, 68, 68, 68, 68,
-		68, 68, 68, 68, 68, 68, 68, 68,
-		68, 68, 68, 68, 68, 68, 68, 68
+	{/* level 3 - low compression quality */
+		 9,  8,  9, 11, 14, 17, 19, 24,
+		 8, 10,  9, 11, 14, 13, 17, 22,
+		 9,  9, 13, 14, 13, 15, 23, 26,
+		11, 11, 14, 14, 15, 20, 26, 33,
+		14, 14, 13, 15, 20, 24, 33, 39,
+		17, 13, 15, 20, 24, 32, 39, 39,
+		19, 17, 23, 26, 33, 39, 39, 39,
+		24, 22, 26, 33, 39, 39, 39, 39
 	}
 };
 
@@ -202,6 +358,106 @@ static const unsigned char hactblg0[162] = {
 	0xf9, 0xfa
 };
 
+/*
+ * Fourcc downgrade schema lookup tables for 422 and 420
+ * chroma subsampling - fourcc on each position maps on the
+ * fourcc from the table fourcc_to_dwngrd_schema_id which allows
+ * to get the most suitable fourcc counterpart for the given
+ * downgraded subsampling property.
+ */
+static const u32 subs422_fourcc_dwngrd_schema[] = {
+	V4L2_PIX_FMT_NV16,
+	V4L2_PIX_FMT_NV61,
+};
+
+static const u32 subs420_fourcc_dwngrd_schema[] = {
+	V4L2_PIX_FMT_NV12,
+	V4L2_PIX_FMT_NV21,
+	V4L2_PIX_FMT_NV12,
+	V4L2_PIX_FMT_NV21,
+	V4L2_PIX_FMT_NV12,
+	V4L2_PIX_FMT_NV21,
+	V4L2_PIX_FMT_GREY,
+	V4L2_PIX_FMT_GREY,
+	V4L2_PIX_FMT_GREY,
+	V4L2_PIX_FMT_GREY,
+};
+
+/*
+ * Lookup table for translation of a fourcc to the position
+ * of its downgraded counterpart in the *fourcc_dwngrd_schema
+ * tables.
+ */
+static const u32 fourcc_to_dwngrd_schema_id[] = {
+	V4L2_PIX_FMT_NV24,
+	V4L2_PIX_FMT_NV42,
+	V4L2_PIX_FMT_NV16,
+	V4L2_PIX_FMT_NV61,
+	V4L2_PIX_FMT_YUYV,
+	V4L2_PIX_FMT_YVYU,
+	V4L2_PIX_FMT_NV12,
+	V4L2_PIX_FMT_NV21,
+	V4L2_PIX_FMT_YUV420,
+	V4L2_PIX_FMT_GREY,
+};
+
+static int s5p_jpeg_get_dwngrd_sch_id_by_fourcc(u32 fourcc)
+{
+	int i;
+	for (i = 0; i < ARRAY_SIZE(fourcc_to_dwngrd_schema_id); ++i) {
+		if (fourcc_to_dwngrd_schema_id[i] == fourcc)
+			return i;
+	}
+
+	return -EINVAL;
+}
+
+static int s5p_jpeg_adjust_fourcc_to_subsampling(
+					enum v4l2_jpeg_chroma_subsampling subs,
+					u32 in_fourcc,
+					u32 *out_fourcc,
+					struct s5p_jpeg_ctx *ctx)
+{
+	int dwngrd_sch_id;
+
+	if (ctx->subsampling != V4L2_JPEG_CHROMA_SUBSAMPLING_GRAY) {
+		dwngrd_sch_id =
+			s5p_jpeg_get_dwngrd_sch_id_by_fourcc(in_fourcc);
+		if (dwngrd_sch_id < 0)
+			return -EINVAL;
+	}
+
+	switch (ctx->subsampling) {
+	case V4L2_JPEG_CHROMA_SUBSAMPLING_GRAY:
+		*out_fourcc = V4L2_PIX_FMT_GREY;
+		break;
+	case V4L2_JPEG_CHROMA_SUBSAMPLING_420:
+		if (dwngrd_sch_id >
+				ARRAY_SIZE(subs420_fourcc_dwngrd_schema) - 1)
+			return -EINVAL;
+		*out_fourcc = subs420_fourcc_dwngrd_schema[dwngrd_sch_id];
+		break;
+	case V4L2_JPEG_CHROMA_SUBSAMPLING_422:
+		if (dwngrd_sch_id >
+				ARRAY_SIZE(subs422_fourcc_dwngrd_schema) - 1)
+			return -EINVAL;
+		*out_fourcc = subs422_fourcc_dwngrd_schema[dwngrd_sch_id];
+		break;
+	default:
+		*out_fourcc = V4L2_PIX_FMT_GREY;
+		break;
+	}
+
+	return 0;
+}
+
+static int exynos4x12_decoded_subsampling[] = {
+	V4L2_JPEG_CHROMA_SUBSAMPLING_GRAY,
+	V4L2_JPEG_CHROMA_SUBSAMPLING_444,
+	V4L2_JPEG_CHROMA_SUBSAMPLING_422,
+	V4L2_JPEG_CHROMA_SUBSAMPLING_420,
+};
+
 static inline struct s5p_jpeg_ctx *ctrl_to_ctx(struct v4l2_ctrl *c)
 {
 	return container_of(c->handler, struct s5p_jpeg_ctx, ctrl_handler);
@@ -212,8 +468,24 @@ static inline struct s5p_jpeg_ctx *fh_to_ctx(struct v4l2_fh *fh)
 	return container_of(fh, struct s5p_jpeg_ctx, fh);
 }
 
-static inline void jpeg_set_qtbl(void __iomem *regs, const unsigned char *qtbl,
-		   unsigned long tab, int len)
+static int s5p_jpeg_to_user_subsampling(struct s5p_jpeg_ctx *ctx)
+{
+	WARN_ON(ctx->subsampling > 3);
+
+	if (ctx->jpeg->variant->version == SJPEG_S5P) {
+		if (ctx->subsampling > 2)
+			return V4L2_JPEG_CHROMA_SUBSAMPLING_GRAY;
+		return ctx->subsampling;
+	} else {
+		if (ctx->subsampling > 2)
+			return V4L2_JPEG_CHROMA_SUBSAMPLING_420;
+		return exynos4x12_decoded_subsampling[ctx->subsampling];
+	}
+}
+
+static inline void s5p_jpeg_set_qtbl(void __iomem *regs,
+				     const unsigned char *qtbl,
+				     unsigned long tab, int len)
 {
 	int i;
 
@@ -221,22 +493,25 @@ static inline void jpeg_set_qtbl(void __iomem *regs, const unsigned char *qtbl,
 		writel((unsigned int)qtbl[i], regs + tab + (i * 0x04));
 }
 
-static inline void jpeg_set_qtbl_lum(void __iomem *regs, int quality)
+static inline void s5p_jpeg_set_qtbl_lum(void __iomem *regs, int quality)
 {
 	/* this driver fills quantisation table 0 with data for luma */
-	jpeg_set_qtbl(regs, qtbl_luminance[quality], S5P_JPG_QTBL_CONTENT(0),
-		      ARRAY_SIZE(qtbl_luminance[quality]));
+	s5p_jpeg_set_qtbl(regs, qtbl_luminance[quality],
+			  S5P_JPG_QTBL_CONTENT(0),
+			  ARRAY_SIZE(qtbl_luminance[quality]));
 }
 
-static inline void jpeg_set_qtbl_chr(void __iomem *regs, int quality)
+static inline void s5p_jpeg_set_qtbl_chr(void __iomem *regs, int quality)
 {
 	/* this driver fills quantisation table 1 with data for chroma */
-	jpeg_set_qtbl(regs, qtbl_chrominance[quality], S5P_JPG_QTBL_CONTENT(1),
-		      ARRAY_SIZE(qtbl_chrominance[quality]));
+	s5p_jpeg_set_qtbl(regs, qtbl_chrominance[quality],
+			  S5P_JPG_QTBL_CONTENT(1),
+			  ARRAY_SIZE(qtbl_chrominance[quality]));
 }
 
-static inline void jpeg_set_htbl(void __iomem *regs, const unsigned char *htbl,
-		   unsigned long tab, int len)
+static inline void s5p_jpeg_set_htbl(void __iomem *regs,
+				     const unsigned char *htbl,
+				     unsigned long tab, int len)
 {
 	int i;
 
@@ -244,28 +519,84 @@ static inline void jpeg_set_htbl(void __iomem *regs, const unsigned char *htbl,
 		writel((unsigned int)htbl[i], regs + tab + (i * 0x04));
 }
 
-static inline void jpeg_set_hdctbl(void __iomem *regs)
+static inline void s5p_jpeg_set_hdctbl(void __iomem *regs)
 {
 	/* this driver fills table 0 for this component */
-	jpeg_set_htbl(regs, hdctbl0, S5P_JPG_HDCTBL(0), ARRAY_SIZE(hdctbl0));
+	s5p_jpeg_set_htbl(regs, hdctbl0, S5P_JPG_HDCTBL(0),
+						ARRAY_SIZE(hdctbl0));
 }
 
-static inline void jpeg_set_hdctblg(void __iomem *regs)
+static inline void s5p_jpeg_set_hdctblg(void __iomem *regs)
 {
 	/* this driver fills table 0 for this component */
-	jpeg_set_htbl(regs, hdctblg0, S5P_JPG_HDCTBLG(0), ARRAY_SIZE(hdctblg0));
+	s5p_jpeg_set_htbl(regs, hdctblg0, S5P_JPG_HDCTBLG(0),
+						ARRAY_SIZE(hdctblg0));
 }
 
-static inline void jpeg_set_hactbl(void __iomem *regs)
+static inline void s5p_jpeg_set_hactbl(void __iomem *regs)
 {
 	/* this driver fills table 0 for this component */
-	jpeg_set_htbl(regs, hactbl0, S5P_JPG_HACTBL(0), ARRAY_SIZE(hactbl0));
+	s5p_jpeg_set_htbl(regs, hactbl0, S5P_JPG_HACTBL(0),
+						ARRAY_SIZE(hactbl0));
 }
 
-static inline void jpeg_set_hactblg(void __iomem *regs)
+static inline void s5p_jpeg_set_hactblg(void __iomem *regs)
 {
 	/* this driver fills table 0 for this component */
-	jpeg_set_htbl(regs, hactblg0, S5P_JPG_HACTBLG(0), ARRAY_SIZE(hactblg0));
+	s5p_jpeg_set_htbl(regs, hactblg0, S5P_JPG_HACTBLG(0),
+						ARRAY_SIZE(hactblg0));
+}
+
+static inline void exynos4_jpeg_set_tbl(void __iomem *regs,
+					const unsigned char *tbl,
+					unsigned long tab, int len)
+{
+	int i;
+	unsigned int dword;
+
+	for (i = 0; i < len; i += 4) {
+		dword = tbl[i] |
+			(tbl[i + 1] << 8) |
+			(tbl[i + 2] << 16) |
+			(tbl[i + 3] << 24);
+		writel(dword, regs + tab + i);
+	}
+}
+
+static inline void exynos4_jpeg_set_qtbl_lum(void __iomem *regs, int quality)
+{
+	/* this driver fills quantisation table 0 with data for luma */
+	exynos4_jpeg_set_tbl(regs, qtbl_luminance[quality],
+			     EXYNOS4_QTBL_CONTENT(0),
+			     ARRAY_SIZE(qtbl_luminance[quality]));
+}
+
+static inline void exynos4_jpeg_set_qtbl_chr(void __iomem *regs, int quality)
+{
+	/* this driver fills quantisation table 1 with data for chroma */
+	exynos4_jpeg_set_tbl(regs, qtbl_chrominance[quality],
+			     EXYNOS4_QTBL_CONTENT(1),
+			     ARRAY_SIZE(qtbl_chrominance[quality]));
+}
+
+void exynos4_jpeg_set_huff_tbl(void __iomem *base)
+{
+	exynos4_jpeg_set_tbl(base, hdctbl0, EXYNOS4_HUFF_TBL_HDCLL,
+							ARRAY_SIZE(hdctbl0));
+	exynos4_jpeg_set_tbl(base, hdctbl0, EXYNOS4_HUFF_TBL_HDCCL,
+							ARRAY_SIZE(hdctbl0));
+	exynos4_jpeg_set_tbl(base, hdctblg0, EXYNOS4_HUFF_TBL_HDCLV,
+							ARRAY_SIZE(hdctblg0));
+	exynos4_jpeg_set_tbl(base, hdctblg0, EXYNOS4_HUFF_TBL_HDCCV,
+							ARRAY_SIZE(hdctblg0));
+	exynos4_jpeg_set_tbl(base, hactbl0, EXYNOS4_HUFF_TBL_HACLL,
+							ARRAY_SIZE(hactbl0));
+	exynos4_jpeg_set_tbl(base, hactbl0, EXYNOS4_HUFF_TBL_HACCL,
+							ARRAY_SIZE(hactbl0));
+	exynos4_jpeg_set_tbl(base, hactblg0, EXYNOS4_HUFF_TBL_HACLV,
+							ARRAY_SIZE(hactblg0));
+	exynos4_jpeg_set_tbl(base, hactblg0, EXYNOS4_HUFF_TBL_HACCV,
+							ARRAY_SIZE(hactblg0));
 }
 
 /*
@@ -276,8 +607,8 @@ static inline void jpeg_set_hactblg(void __iomem *regs)
 
 static int queue_init(void *priv, struct vb2_queue *src_vq,
 		      struct vb2_queue *dst_vq);
-static struct s5p_jpeg_fmt *s5p_jpeg_find_format(unsigned int mode,
-						 __u32 pixelformat);
+static struct s5p_jpeg_fmt *s5p_jpeg_find_format(struct s5p_jpeg_ctx *ctx,
+				__u32 pixelformat, unsigned int fmt_type);
 static int s5p_jpeg_controls_create(struct s5p_jpeg_ctx *ctx);
 
 static int s5p_jpeg_open(struct file *file)
@@ -285,7 +616,7 @@ static int s5p_jpeg_open(struct file *file)
 	struct s5p_jpeg *jpeg = video_drvdata(file);
 	struct video_device *vfd = video_devdata(file);
 	struct s5p_jpeg_ctx *ctx;
-	struct s5p_jpeg_fmt *out_fmt;
+	struct s5p_jpeg_fmt *out_fmt, *cap_fmt;
 	int ret = 0;
 
 	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
@@ -306,24 +637,31 @@ static int s5p_jpeg_open(struct file *file)
 	ctx->jpeg = jpeg;
 	if (vfd == jpeg->vfd_encoder) {
 		ctx->mode = S5P_JPEG_ENCODE;
-		out_fmt = s5p_jpeg_find_format(ctx->mode, V4L2_PIX_FMT_RGB565);
+		out_fmt = s5p_jpeg_find_format(ctx, V4L2_PIX_FMT_RGB565,
+							FMT_TYPE_OUTPUT);
+		cap_fmt = s5p_jpeg_find_format(ctx, V4L2_PIX_FMT_JPEG,
+							FMT_TYPE_CAPTURE);
 	} else {
 		ctx->mode = S5P_JPEG_DECODE;
-		out_fmt = s5p_jpeg_find_format(ctx->mode, V4L2_PIX_FMT_JPEG);
+		out_fmt = s5p_jpeg_find_format(ctx, V4L2_PIX_FMT_JPEG,
+							FMT_TYPE_OUTPUT);
+		cap_fmt = s5p_jpeg_find_format(ctx, V4L2_PIX_FMT_YUYV,
+							FMT_TYPE_CAPTURE);
 	}
 
-	ret = s5p_jpeg_controls_create(ctx);
-	if (ret < 0)
-		goto error;
-
-	ctx->m2m_ctx = v4l2_m2m_ctx_init(jpeg->m2m_dev, ctx, queue_init);
-	if (IS_ERR(ctx->m2m_ctx)) {
-		ret = PTR_ERR(ctx->m2m_ctx);
+	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(jpeg->m2m_dev, ctx, queue_init);
+	if (IS_ERR(ctx->fh.m2m_ctx)) {
+		ret = PTR_ERR(ctx->fh.m2m_ctx);
 		goto error;
 	}
 
 	ctx->out_q.fmt = out_fmt;
-	ctx->cap_q.fmt = s5p_jpeg_find_format(ctx->mode, V4L2_PIX_FMT_YUYV);
+	ctx->cap_q.fmt = cap_fmt;
+
+	ret = s5p_jpeg_controls_create(ctx);
+	if (ret < 0)
+		goto error;
+
 	mutex_unlock(&jpeg->lock);
 	return 0;
 
@@ -342,49 +680,23 @@ static int s5p_jpeg_release(struct file *file)
 	struct s5p_jpeg_ctx *ctx = fh_to_ctx(file->private_data);
 
 	mutex_lock(&jpeg->lock);
-	v4l2_m2m_ctx_release(ctx->m2m_ctx);
-	mutex_unlock(&jpeg->lock);
+	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
 	v4l2_ctrl_handler_free(&ctx->ctrl_handler);
 	v4l2_fh_del(&ctx->fh);
 	v4l2_fh_exit(&ctx->fh);
 	kfree(ctx);
-
-	return 0;
-}
-
-static unsigned int s5p_jpeg_poll(struct file *file,
-				 struct poll_table_struct *wait)
-{
-	struct s5p_jpeg *jpeg = video_drvdata(file);
-	struct s5p_jpeg_ctx *ctx = fh_to_ctx(file->private_data);
-	unsigned int res;
-
-	mutex_lock(&jpeg->lock);
-	res = v4l2_m2m_poll(file, ctx->m2m_ctx, wait);
 	mutex_unlock(&jpeg->lock);
-	return res;
-}
-
-static int s5p_jpeg_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	struct s5p_jpeg *jpeg = video_drvdata(file);
-	struct s5p_jpeg_ctx *ctx = fh_to_ctx(file->private_data);
-	int ret;
 
-	if (mutex_lock_interruptible(&jpeg->lock))
-		return -ERESTARTSYS;
-	ret = v4l2_m2m_mmap(file, ctx->m2m_ctx, vma);
-	mutex_unlock(&jpeg->lock);
-	return ret;
+	return 0;
 }
 
 static const struct v4l2_file_operations s5p_jpeg_fops = {
 	.owner		= THIS_MODULE,
 	.open		= s5p_jpeg_open,
 	.release	= s5p_jpeg_release,
-	.poll		= s5p_jpeg_poll,
+	.poll		= v4l2_m2m_fop_poll,
 	.unlocked_ioctl	= video_ioctl2,
-	.mmap		= s5p_jpeg_mmap,
+	.mmap		= v4l2_m2m_fop_mmap,
 };
 
 /*
@@ -427,10 +739,11 @@ static void skip(struct s5p_jpeg_buffer *buf, long len)
 }
 
 static bool s5p_jpeg_parse_hdr(struct s5p_jpeg_q_data *result,
-			       unsigned long buffer, unsigned long size)
+			       unsigned long buffer, unsigned long size,
+			       struct s5p_jpeg_ctx *ctx)
 {
 	int c, components, notfound;
-	unsigned int height, width, word;
+	unsigned int height, width, word, subsampling = 0;
 	long length;
 	struct s5p_jpeg_buffer jpeg_buffer;
 
@@ -469,7 +782,15 @@ static bool s5p_jpeg_parse_hdr(struct s5p_jpeg_q_data *result,
 				break;
 			notfound = 0;
 
-			skip(&jpeg_buffer, components * 3);
+			if (components == 1) {
+				subsampling = 0x33;
+			} else {
+				skip(&jpeg_buffer, 1);
+				subsampling = get_byte(&jpeg_buffer);
+				skip(&jpeg_buffer, 1);
+			}
+
+			skip(&jpeg_buffer, components * 2);
 			break;
 
 		/* skip payload-less markers */
@@ -491,6 +812,24 @@ static bool s5p_jpeg_parse_hdr(struct s5p_jpeg_q_data *result,
 	result->w = width;
 	result->h = height;
 	result->size = components;
+
+	switch (subsampling) {
+	case 0x11:
+		ctx->subsampling = V4L2_JPEG_CHROMA_SUBSAMPLING_444;
+		break;
+	case 0x21:
+		ctx->subsampling = V4L2_JPEG_CHROMA_SUBSAMPLING_422;
+		break;
+	case 0x22:
+		ctx->subsampling = V4L2_JPEG_CHROMA_SUBSAMPLING_420;
+		break;
+	case 0x33:
+		ctx->subsampling = V4L2_JPEG_CHROMA_SUBSAMPLING_GRAY;
+		break;
+	default:
+		return false;
+	}
+
 	return !notfound;
 }
 
@@ -521,13 +860,13 @@ static int s5p_jpeg_querycap(struct file *file, void *priv,
 	return 0;
 }
 
-static int enum_fmt(struct s5p_jpeg_fmt *formats, int n,
+static int enum_fmt(struct s5p_jpeg_fmt *sjpeg_formats, int n,
 		    struct v4l2_fmtdesc *f, u32 type)
 {
 	int i, num = 0;
 
 	for (i = 0; i < n; ++i) {
-		if (formats[i].types & type) {
+		if (sjpeg_formats[i].flags & type) {
 			/* index-th format of type type found ? */
 			if (num == f->index)
 				break;
@@ -541,8 +880,8 @@ static int enum_fmt(struct s5p_jpeg_fmt *formats, int n,
 	if (i >= n)
 		return -EINVAL;
 
-	strlcpy(f->description, formats[i].name, sizeof(f->description));
-	f->pixelformat = formats[i].fourcc;
+	strlcpy(f->description, sjpeg_formats[i].name, sizeof(f->description));
+	f->pixelformat = sjpeg_formats[i].fourcc;
 
 	return 0;
 }
@@ -553,10 +892,11 @@ static int s5p_jpeg_enum_fmt_vid_cap(struct file *file, void *priv,
 	struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
 
 	if (ctx->mode == S5P_JPEG_ENCODE)
-		return enum_fmt(formats_enc, NUM_FORMATS_ENC, f,
-				MEM2MEM_CAPTURE);
+		return enum_fmt(sjpeg_formats, SJPEG_NUM_FORMATS, f,
+				SJPEG_FMT_FLAG_ENC_CAPTURE);
 
-	return enum_fmt(formats_dec, NUM_FORMATS_DEC, f, MEM2MEM_CAPTURE);
+	return enum_fmt(sjpeg_formats, SJPEG_NUM_FORMATS, f,
+					SJPEG_FMT_FLAG_DEC_CAPTURE);
 }
 
 static int s5p_jpeg_enum_fmt_vid_out(struct file *file, void *priv,
@@ -565,10 +905,11 @@ static int s5p_jpeg_enum_fmt_vid_out(struct file *file, void *priv,
 	struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
 
 	if (ctx->mode == S5P_JPEG_ENCODE)
-		return enum_fmt(formats_enc, NUM_FORMATS_ENC, f,
-				MEM2MEM_OUTPUT);
+		return enum_fmt(sjpeg_formats, SJPEG_NUM_FORMATS, f,
+				SJPEG_FMT_FLAG_ENC_OUTPUT);
 
-	return enum_fmt(formats_dec, NUM_FORMATS_DEC, f, MEM2MEM_OUTPUT);
+	return enum_fmt(sjpeg_formats, SJPEG_NUM_FORMATS, f,
+					SJPEG_FMT_FLAG_DEC_OUTPUT);
 }
 
 static struct s5p_jpeg_q_data *get_q_data(struct s5p_jpeg_ctx *ctx,
@@ -589,7 +930,7 @@ static int s5p_jpeg_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
 	struct v4l2_pix_format *pix = &f->fmt.pix;
 	struct s5p_jpeg_ctx *ct = fh_to_ctx(priv);
 
-	vq = v4l2_m2m_get_vq(ct->m2m_ctx, f->type);
+	vq = v4l2_m2m_get_vq(ct->fh.m2m_ctx, f->type);
 	if (!vq)
 		return -EINVAL;
 
@@ -615,29 +956,35 @@ static int s5p_jpeg_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
 	return 0;
 }
 
-static struct s5p_jpeg_fmt *s5p_jpeg_find_format(unsigned int mode,
-						 u32 pixelformat)
+static struct s5p_jpeg_fmt *s5p_jpeg_find_format(struct s5p_jpeg_ctx *ctx,
+				u32 pixelformat, unsigned int fmt_type)
 {
-	unsigned int k;
-	struct s5p_jpeg_fmt *formats;
-	int n;
+	unsigned int k, fmt_flag, ver_flag;
 
-	if (mode == S5P_JPEG_ENCODE) {
-		formats = formats_enc;
-		n = NUM_FORMATS_ENC;
-	} else {
-		formats = formats_dec;
-		n = NUM_FORMATS_DEC;
-	}
+	if (ctx->mode == S5P_JPEG_ENCODE)
+		fmt_flag = (fmt_type == FMT_TYPE_OUTPUT) ?
+				SJPEG_FMT_FLAG_ENC_OUTPUT :
+				SJPEG_FMT_FLAG_ENC_CAPTURE;
+	else
+		fmt_flag = (fmt_type == FMT_TYPE_OUTPUT) ?
+				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 < n; k++) {
-		struct s5p_jpeg_fmt *fmt = &formats[k];
-		if (fmt->fourcc == pixelformat)
+	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) {
 			return fmt;
+		}
 	}
 
 	return NULL;
-
 }
 
 static void jpeg_bound_align_image(u32 *w, unsigned int wmin, unsigned int wmax,
@@ -673,7 +1020,7 @@ static int vidioc_try_fmt(struct v4l2_format *f, struct s5p_jpeg_fmt *fmt,
 
 	/* V4L2 specification suggests the driver corrects the format struct
 	 * if any of the dimensions is unsupported */
-	if (q_type == MEM2MEM_OUTPUT)
+	if (q_type == FMT_TYPE_OUTPUT)
 		jpeg_bound_align_image(&pix->width, S5P_JPEG_MIN_WIDTH,
 				       S5P_JPEG_MAX_WIDTH, 0,
 				       &pix->height, S5P_JPEG_MIN_HEIGHT,
@@ -695,7 +1042,7 @@ static int vidioc_try_fmt(struct v4l2_format *f, struct s5p_jpeg_fmt *fmt,
 			bpl = pix->width; /* planar */
 
 		if (fmt->colplanes == 1 && /* packed */
-		    (bpl << 3) * fmt->depth < pix->width)
+		    (bpl << 3) / fmt->depth < pix->width)
 			bpl = (pix->width * fmt->depth) >> 3;
 
 		pix->bytesperline = bpl;
@@ -709,17 +1056,41 @@ static int s5p_jpeg_try_fmt_vid_cap(struct file *file, void *priv,
 				  struct v4l2_format *f)
 {
 	struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
 	struct s5p_jpeg_fmt *fmt;
+	int ret;
 
-	fmt = s5p_jpeg_find_format(ctx->mode, f->fmt.pix.pixelformat);
-	if (!fmt || !(fmt->types & MEM2MEM_CAPTURE)) {
+	fmt = s5p_jpeg_find_format(ctx, f->fmt.pix.pixelformat,
+						FMT_TYPE_CAPTURE);
+	if (!fmt) {
 		v4l2_err(&ctx->jpeg->v4l2_dev,
 			 "Fourcc format (0x%08x) invalid.\n",
 			 f->fmt.pix.pixelformat);
 		return -EINVAL;
 	}
 
-	return vidioc_try_fmt(f, fmt, ctx, MEM2MEM_CAPTURE);
+	/*
+	 * 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) &&
+	    (fmt->subsampling < ctx->subsampling)) {
+		ret = s5p_jpeg_adjust_fourcc_to_subsampling(ctx->subsampling,
+							    fmt->fourcc,
+							    &pix->pixelformat,
+							    ctx);
+		if (ret < 0)
+			pix->pixelformat = V4L2_PIX_FMT_GREY;
+
+		fmt = s5p_jpeg_find_format(ctx, pix->pixelformat,
+							FMT_TYPE_CAPTURE);
+	}
+
+	return vidioc_try_fmt(f, fmt, ctx, FMT_TYPE_CAPTURE);
 }
 
 static int s5p_jpeg_try_fmt_vid_out(struct file *file, void *priv,
@@ -728,15 +1099,16 @@ static int s5p_jpeg_try_fmt_vid_out(struct file *file, void *priv,
 	struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
 	struct s5p_jpeg_fmt *fmt;
 
-	fmt = s5p_jpeg_find_format(ctx->mode, f->fmt.pix.pixelformat);
-	if (!fmt || !(fmt->types & MEM2MEM_OUTPUT)) {
+	fmt = s5p_jpeg_find_format(ctx, f->fmt.pix.pixelformat,
+						FMT_TYPE_OUTPUT);
+	if (!fmt) {
 		v4l2_err(&ctx->jpeg->v4l2_dev,
 			 "Fourcc format (0x%08x) invalid.\n",
 			 f->fmt.pix.pixelformat);
 		return -EINVAL;
 	}
 
-	return vidioc_try_fmt(f, fmt, ctx, MEM2MEM_OUTPUT);
+	return vidioc_try_fmt(f, fmt, ctx, FMT_TYPE_OUTPUT);
 }
 
 static int s5p_jpeg_s_fmt(struct s5p_jpeg_ctx *ct, struct v4l2_format *f)
@@ -744,8 +1116,10 @@ static int s5p_jpeg_s_fmt(struct s5p_jpeg_ctx *ct, struct v4l2_format *f)
 	struct vb2_queue *vq;
 	struct s5p_jpeg_q_data *q_data = NULL;
 	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_ctrl *ctrl_subs;
+	unsigned int f_type;
 
-	vq = v4l2_m2m_get_vq(ct->m2m_ctx, f->type);
+	vq = v4l2_m2m_get_vq(ct->fh.m2m_ctx, f->type);
 	if (!vq)
 		return -EINVAL;
 
@@ -757,7 +1131,10 @@ static int s5p_jpeg_s_fmt(struct s5p_jpeg_ctx *ct, struct v4l2_format *f)
 		return -EBUSY;
 	}
 
-	q_data->fmt = s5p_jpeg_find_format(ct->mode, pix->pixelformat);
+	f_type = V4L2_TYPE_IS_OUTPUT(f->type) ?
+			FMT_TYPE_OUTPUT : FMT_TYPE_CAPTURE;
+
+	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)
@@ -765,6 +1142,13 @@ static int s5p_jpeg_s_fmt(struct s5p_jpeg_ctx *ct, struct v4l2_format *f)
 	else
 		q_data->size = pix->sizeimage;
 
+	if (f_type == FMT_TYPE_OUTPUT) {
+		ctrl_subs = v4l2_ctrl_find(&ct->ctrl_handler,
+					V4L2_CID_JPEG_CHROMA_SUBSAMPLING);
+		if (ctrl_subs)
+			v4l2_ctrl_s_ctrl(ctrl_subs, q_data->fmt->subsampling);
+	}
+
 	return 0;
 }
 
@@ -792,60 +1176,14 @@ static int s5p_jpeg_s_fmt_vid_out(struct file *file, void *priv,
 	return s5p_jpeg_s_fmt(fh_to_ctx(priv), f);
 }
 
-static int s5p_jpeg_reqbufs(struct file *file, void *priv,
-			  struct v4l2_requestbuffers *reqbufs)
-{
-	struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
-
-	return v4l2_m2m_reqbufs(file, ctx->m2m_ctx, reqbufs);
-}
-
-static int s5p_jpeg_querybuf(struct file *file, void *priv,
-			   struct v4l2_buffer *buf)
-{
-	struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
-
-	return v4l2_m2m_querybuf(file, ctx->m2m_ctx, buf);
-}
-
-static int s5p_jpeg_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
-{
-	struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
-
-	return v4l2_m2m_qbuf(file, ctx->m2m_ctx, buf);
-}
-
-static int s5p_jpeg_dqbuf(struct file *file, void *priv,
-			  struct v4l2_buffer *buf)
-{
-	struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
-
-	return v4l2_m2m_dqbuf(file, ctx->m2m_ctx, buf);
-}
-
-static int s5p_jpeg_streamon(struct file *file, void *priv,
-			   enum v4l2_buf_type type)
-{
-	struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
-
-	return v4l2_m2m_streamon(file, ctx->m2m_ctx, type);
-}
-
-static int s5p_jpeg_streamoff(struct file *file, void *priv,
-			    enum v4l2_buf_type type)
-{
-	struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
-
-	return v4l2_m2m_streamoff(file, ctx->m2m_ctx, type);
-}
-
 static int s5p_jpeg_g_selection(struct file *file, void *priv,
 			 struct v4l2_selection *s)
 {
 	struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
 
 	if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT &&
-	    s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+	    s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+	    ctx->jpeg->variant->version != SJPEG_S5P)
 		return -EINVAL;
 
 	/* For JPEG blob active == default == bounds */
@@ -884,12 +1222,7 @@ static int s5p_jpeg_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 	switch (ctrl->id) {
 	case V4L2_CID_JPEG_CHROMA_SUBSAMPLING:
 		spin_lock_irqsave(&jpeg->slock, flags);
-
-		WARN_ON(ctx->subsampling > S5P_SUBSAMPLING_MODE_GRAY);
-		if (ctx->subsampling > 2)
-			ctrl->val = V4L2_JPEG_CHROMA_SUBSAMPLING_GRAY;
-		else
-			ctrl->val = ctx->subsampling;
+		ctrl->val = s5p_jpeg_to_user_subsampling(ctx);
 		spin_unlock_irqrestore(&jpeg->slock, flags);
 		break;
 	}
@@ -897,6 +1230,40 @@ static int s5p_jpeg_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 	return 0;
 }
 
+static int s5p_jpeg_try_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct s5p_jpeg_ctx *ctx = ctrl_to_ctx(ctrl);
+	unsigned long flags;
+	int ret = 0;
+
+	spin_lock_irqsave(&ctx->jpeg->slock, flags);
+
+	if (ctrl->id == V4L2_CID_JPEG_CHROMA_SUBSAMPLING) {
+		if (ctx->jpeg->variant->version == SJPEG_S5P)
+			goto error_free;
+		/*
+		 * The exynos4x12 device requires input raw image fourcc
+		 * to be V4L2_PIX_FMT_GREY if gray jpeg format
+		 * is to be set.
+		 */
+		if (ctx->out_q.fmt->fourcc != V4L2_PIX_FMT_GREY &&
+		    ctrl->val == V4L2_JPEG_CHROMA_SUBSAMPLING_GRAY) {
+			ret = -EINVAL;
+			goto error_free;
+		}
+		/*
+		 * The exynos4x12 device requires resulting jpeg subsampling
+		 * not to be lower than the input raw image subsampling.
+		 */
+		if (ctx->out_q.fmt->subsampling > ctrl->val)
+			ctrl->val = ctx->out_q.fmt->subsampling;
+	}
+
+error_free:
+	spin_unlock_irqrestore(&ctx->jpeg->slock, flags);
+	return ret;
+}
+
 static int s5p_jpeg_s_ctrl(struct v4l2_ctrl *ctrl)
 {
 	struct s5p_jpeg_ctx *ctx = ctrl_to_ctx(ctrl);
@@ -906,7 +1273,7 @@ static int s5p_jpeg_s_ctrl(struct v4l2_ctrl *ctrl)
 
 	switch (ctrl->id) {
 	case V4L2_CID_JPEG_COMPRESSION_QUALITY:
-		ctx->compr_quality = S5P_JPEG_COMPR_QUAL_WORST - ctrl->val;
+		ctx->compr_quality = ctrl->val;
 		break;
 	case V4L2_CID_JPEG_RESTART_INTERVAL:
 		ctx->restart_interval = ctrl->val;
@@ -922,6 +1289,7 @@ static int s5p_jpeg_s_ctrl(struct v4l2_ctrl *ctrl)
 
 static const struct v4l2_ctrl_ops s5p_jpeg_ctrl_ops = {
 	.g_volatile_ctrl	= s5p_jpeg_g_volatile_ctrl,
+	.try_ctrl		= s5p_jpeg_try_ctrl,
 	.s_ctrl			= s5p_jpeg_s_ctrl,
 };
 
@@ -929,18 +1297,20 @@ static int s5p_jpeg_controls_create(struct s5p_jpeg_ctx *ctx)
 {
 	unsigned int mask = ~0x27; /* 444, 422, 420, GRAY */
 	struct v4l2_ctrl *ctrl;
+	int ret;
 
 	v4l2_ctrl_handler_init(&ctx->ctrl_handler, 3);
 
 	if (ctx->mode == S5P_JPEG_ENCODE) {
 		v4l2_ctrl_new_std(&ctx->ctrl_handler, &s5p_jpeg_ctrl_ops,
 				  V4L2_CID_JPEG_COMPRESSION_QUALITY,
-				  0, 3, 1, 3);
+				  0, 3, 1, S5P_JPEG_COMPR_QUAL_WORST);
 
 		v4l2_ctrl_new_std(&ctx->ctrl_handler, &s5p_jpeg_ctrl_ops,
 				  V4L2_CID_JPEG_RESTART_INTERVAL,
 				  0, 3, 0xffff, 0);
-		mask = ~0x06; /* 422, 420 */
+		if (ctx->jpeg->variant->version == SJPEG_S5P)
+			mask = ~0x06; /* 422, 420 */
 	}
 
 	ctrl = v4l2_ctrl_new_std_menu(&ctx->ctrl_handler, &s5p_jpeg_ctrl_ops,
@@ -948,13 +1318,24 @@ static int s5p_jpeg_controls_create(struct s5p_jpeg_ctx *ctx)
 				      V4L2_JPEG_CHROMA_SUBSAMPLING_GRAY, mask,
 				      V4L2_JPEG_CHROMA_SUBSAMPLING_422);
 
-	if (ctx->ctrl_handler.error)
-		return ctx->ctrl_handler.error;
+	if (ctx->ctrl_handler.error) {
+		ret = ctx->ctrl_handler.error;
+		goto error_free;
+	}
 
 	if (ctx->mode == S5P_JPEG_DECODE)
 		ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE |
 			V4L2_CTRL_FLAG_READ_ONLY;
-	return 0;
+
+	ret = v4l2_ctrl_handler_setup(&ctx->ctrl_handler);
+	if (ret < 0)
+		goto error_free;
+
+	return ret;
+
+error_free:
+	v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+	return ret;
 }
 
 static const struct v4l2_ioctl_ops s5p_jpeg_ioctl_ops = {
@@ -972,14 +1353,13 @@ static const struct v4l2_ioctl_ops s5p_jpeg_ioctl_ops = {
 	.vidioc_s_fmt_vid_cap		= s5p_jpeg_s_fmt_vid_cap,
 	.vidioc_s_fmt_vid_out		= s5p_jpeg_s_fmt_vid_out,
 
-	.vidioc_reqbufs			= s5p_jpeg_reqbufs,
-	.vidioc_querybuf		= s5p_jpeg_querybuf,
+	.vidioc_reqbufs			= v4l2_m2m_ioctl_reqbufs,
+	.vidioc_querybuf		= v4l2_m2m_ioctl_querybuf,
+	.vidioc_qbuf			= v4l2_m2m_ioctl_qbuf,
+	.vidioc_dqbuf			= v4l2_m2m_ioctl_dqbuf,
 
-	.vidioc_qbuf			= s5p_jpeg_qbuf,
-	.vidioc_dqbuf			= s5p_jpeg_dqbuf,
-
-	.vidioc_streamon		= s5p_jpeg_streamon,
-	.vidioc_streamoff		= s5p_jpeg_streamoff,
+	.vidioc_streamon		= v4l2_m2m_ioctl_streamon,
+	.vidioc_streamoff		= v4l2_m2m_ioctl_streamoff,
 
 	.vidioc_g_selection		= s5p_jpeg_g_selection,
 };
@@ -995,74 +1375,181 @@ static void s5p_jpeg_device_run(void *priv)
 	struct s5p_jpeg_ctx *ctx = priv;
 	struct s5p_jpeg *jpeg = ctx->jpeg;
 	struct vb2_buffer *src_buf, *dst_buf;
-	unsigned long src_addr, dst_addr;
+	unsigned long src_addr, dst_addr, flags;
+
+	spin_lock_irqsave(&ctx->jpeg->slock, flags);
 
-	src_buf = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
-	dst_buf = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
+	src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+	dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
 	src_addr = vb2_dma_contig_plane_dma_addr(src_buf, 0);
 	dst_addr = vb2_dma_contig_plane_dma_addr(dst_buf, 0);
 
-	jpeg_reset(jpeg->regs);
-	jpeg_poweron(jpeg->regs);
-	jpeg_proc_mode(jpeg->regs, ctx->mode);
+	s5p_jpeg_reset(jpeg->regs);
+	s5p_jpeg_poweron(jpeg->regs);
+	s5p_jpeg_proc_mode(jpeg->regs, ctx->mode);
 	if (ctx->mode == S5P_JPEG_ENCODE) {
 		if (ctx->out_q.fmt->fourcc == V4L2_PIX_FMT_RGB565)
-			jpeg_input_raw_mode(jpeg->regs, S5P_JPEG_RAW_IN_565);
+			s5p_jpeg_input_raw_mode(jpeg->regs,
+							S5P_JPEG_RAW_IN_565);
 		else
-			jpeg_input_raw_mode(jpeg->regs, S5P_JPEG_RAW_IN_422);
-		jpeg_subsampling_mode(jpeg->regs, ctx->subsampling);
-		jpeg_dri(jpeg->regs, ctx->restart_interval);
-		jpeg_x(jpeg->regs, ctx->out_q.w);
-		jpeg_y(jpeg->regs, ctx->out_q.h);
-		jpeg_imgadr(jpeg->regs, src_addr);
-		jpeg_jpgadr(jpeg->regs, dst_addr);
+			s5p_jpeg_input_raw_mode(jpeg->regs,
+							S5P_JPEG_RAW_IN_422);
+		s5p_jpeg_subsampling_mode(jpeg->regs, ctx->subsampling);
+		s5p_jpeg_dri(jpeg->regs, ctx->restart_interval);
+		s5p_jpeg_x(jpeg->regs, ctx->out_q.w);
+		s5p_jpeg_y(jpeg->regs, ctx->out_q.h);
+		s5p_jpeg_imgadr(jpeg->regs, src_addr);
+		s5p_jpeg_jpgadr(jpeg->regs, dst_addr);
 
 		/* ultimately comes from sizeimage from userspace */
-		jpeg_enc_stream_int(jpeg->regs, ctx->cap_q.size);
+		s5p_jpeg_enc_stream_int(jpeg->regs, ctx->cap_q.size);
 
 		/* JPEG RGB to YCbCr conversion matrix */
-		jpeg_coef(jpeg->regs, 1, 1, S5P_JPEG_COEF11);
-		jpeg_coef(jpeg->regs, 1, 2, S5P_JPEG_COEF12);
-		jpeg_coef(jpeg->regs, 1, 3, S5P_JPEG_COEF13);
-		jpeg_coef(jpeg->regs, 2, 1, S5P_JPEG_COEF21);
-		jpeg_coef(jpeg->regs, 2, 2, S5P_JPEG_COEF22);
-		jpeg_coef(jpeg->regs, 2, 3, S5P_JPEG_COEF23);
-		jpeg_coef(jpeg->regs, 3, 1, S5P_JPEG_COEF31);
-		jpeg_coef(jpeg->regs, 3, 2, S5P_JPEG_COEF32);
-		jpeg_coef(jpeg->regs, 3, 3, S5P_JPEG_COEF33);
+		s5p_jpeg_coef(jpeg->regs, 1, 1, S5P_JPEG_COEF11);
+		s5p_jpeg_coef(jpeg->regs, 1, 2, S5P_JPEG_COEF12);
+		s5p_jpeg_coef(jpeg->regs, 1, 3, S5P_JPEG_COEF13);
+		s5p_jpeg_coef(jpeg->regs, 2, 1, S5P_JPEG_COEF21);
+		s5p_jpeg_coef(jpeg->regs, 2, 2, S5P_JPEG_COEF22);
+		s5p_jpeg_coef(jpeg->regs, 2, 3, S5P_JPEG_COEF23);
+		s5p_jpeg_coef(jpeg->regs, 3, 1, S5P_JPEG_COEF31);
+		s5p_jpeg_coef(jpeg->regs, 3, 2, S5P_JPEG_COEF32);
+		s5p_jpeg_coef(jpeg->regs, 3, 3, S5P_JPEG_COEF33);
 
 		/*
 		 * JPEG IP allows storing 4 quantization tables
 		 * We fill table 0 for luma and table 1 for chroma
 		 */
-		jpeg_set_qtbl_lum(jpeg->regs, ctx->compr_quality);
-		jpeg_set_qtbl_chr(jpeg->regs, ctx->compr_quality);
+		s5p_jpeg_set_qtbl_lum(jpeg->regs, ctx->compr_quality);
+		s5p_jpeg_set_qtbl_chr(jpeg->regs, ctx->compr_quality);
 		/* use table 0 for Y */
-		jpeg_qtbl(jpeg->regs, 1, 0);
+		s5p_jpeg_qtbl(jpeg->regs, 1, 0);
 		/* use table 1 for Cb and Cr*/
-		jpeg_qtbl(jpeg->regs, 2, 1);
-		jpeg_qtbl(jpeg->regs, 3, 1);
+		s5p_jpeg_qtbl(jpeg->regs, 2, 1);
+		s5p_jpeg_qtbl(jpeg->regs, 3, 1);
 
 		/* Y, Cb, Cr use Huffman table 0 */
-		jpeg_htbl_ac(jpeg->regs, 1);
-		jpeg_htbl_dc(jpeg->regs, 1);
-		jpeg_htbl_ac(jpeg->regs, 2);
-		jpeg_htbl_dc(jpeg->regs, 2);
-		jpeg_htbl_ac(jpeg->regs, 3);
-		jpeg_htbl_dc(jpeg->regs, 3);
+		s5p_jpeg_htbl_ac(jpeg->regs, 1);
+		s5p_jpeg_htbl_dc(jpeg->regs, 1);
+		s5p_jpeg_htbl_ac(jpeg->regs, 2);
+		s5p_jpeg_htbl_dc(jpeg->regs, 2);
+		s5p_jpeg_htbl_ac(jpeg->regs, 3);
+		s5p_jpeg_htbl_dc(jpeg->regs, 3);
 	} else { /* S5P_JPEG_DECODE */
-		jpeg_rst_int_enable(jpeg->regs, true);
-		jpeg_data_num_int_enable(jpeg->regs, true);
-		jpeg_final_mcu_num_int_enable(jpeg->regs, true);
+		s5p_jpeg_rst_int_enable(jpeg->regs, true);
+		s5p_jpeg_data_num_int_enable(jpeg->regs, true);
+		s5p_jpeg_final_mcu_num_int_enable(jpeg->regs, true);
 		if (ctx->cap_q.fmt->fourcc == V4L2_PIX_FMT_YUYV)
-			jpeg_outform_raw(jpeg->regs, S5P_JPEG_RAW_OUT_422);
+			s5p_jpeg_outform_raw(jpeg->regs, S5P_JPEG_RAW_OUT_422);
 		else
-			jpeg_outform_raw(jpeg->regs, S5P_JPEG_RAW_OUT_420);
-		jpeg_jpgadr(jpeg->regs, src_addr);
-		jpeg_imgadr(jpeg->regs, dst_addr);
+			s5p_jpeg_outform_raw(jpeg->regs, S5P_JPEG_RAW_OUT_420);
+		s5p_jpeg_jpgadr(jpeg->regs, src_addr);
+		s5p_jpeg_imgadr(jpeg->regs, dst_addr);
 	}
 
-	jpeg_start(jpeg->regs);
+	s5p_jpeg_start(jpeg->regs);
+
+	spin_unlock_irqrestore(&ctx->jpeg->slock, flags);
+}
+
+static void exynos4_jpeg_set_img_addr(struct s5p_jpeg_ctx *ctx)
+{
+	struct s5p_jpeg *jpeg = ctx->jpeg;
+	struct s5p_jpeg_fmt *fmt;
+	struct vb2_buffer *vb;
+	struct s5p_jpeg_addr jpeg_addr;
+	u32 pix_size, padding_bytes = 0;
+
+	pix_size = ctx->cap_q.w * ctx->cap_q.h;
+
+	if (ctx->mode == S5P_JPEG_ENCODE) {
+		vb = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+		fmt = ctx->out_q.fmt;
+		if (ctx->out_q.w % 2 && fmt->h_align > 0)
+			padding_bytes = ctx->out_q.h;
+	} else {
+		fmt = ctx->cap_q.fmt;
+		vb = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+	}
+
+	jpeg_addr.y = vb2_dma_contig_plane_dma_addr(vb, 0);
+
+	if (fmt->colplanes == 2) {
+		jpeg_addr.cb = jpeg_addr.y + pix_size - padding_bytes;
+	} else if (fmt->colplanes == 3) {
+		jpeg_addr.cb = jpeg_addr.y + pix_size;
+		if (fmt->fourcc == V4L2_PIX_FMT_YUV420)
+			jpeg_addr.cr = jpeg_addr.cb + pix_size / 4;
+		else
+			jpeg_addr.cr = jpeg_addr.cb + pix_size / 2;
+	}
+
+	exynos4_jpeg_set_frame_buf_address(jpeg->regs, &jpeg_addr);
+}
+
+static void exynos4_jpeg_set_jpeg_addr(struct s5p_jpeg_ctx *ctx)
+{
+	struct s5p_jpeg *jpeg = ctx->jpeg;
+	struct vb2_buffer *vb;
+	unsigned int jpeg_addr = 0;
+
+	if (ctx->mode == S5P_JPEG_ENCODE)
+		vb = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+	else
+		vb = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+
+	jpeg_addr = vb2_dma_contig_plane_dma_addr(vb, 0);
+	exynos4_jpeg_set_stream_buf_address(jpeg->regs, jpeg_addr);
+}
+
+static void exynos4_jpeg_device_run(void *priv)
+{
+	struct s5p_jpeg_ctx *ctx = priv;
+	struct s5p_jpeg *jpeg = ctx->jpeg;
+	unsigned int bitstream_size;
+	unsigned long flags;
+
+	spin_lock_irqsave(&ctx->jpeg->slock, flags);
+
+	if (ctx->mode == S5P_JPEG_ENCODE) {
+		exynos4_jpeg_sw_reset(jpeg->regs);
+		exynos4_jpeg_set_interrupt(jpeg->regs);
+		exynos4_jpeg_set_huf_table_enable(jpeg->regs, 1);
+
+		exynos4_jpeg_set_huff_tbl(jpeg->regs);
+
+		/*
+		 * JPEG IP allows storing 4 quantization tables
+		 * We fill table 0 for luma and table 1 for chroma
+		 */
+		exynos4_jpeg_set_qtbl_lum(jpeg->regs, ctx->compr_quality);
+		exynos4_jpeg_set_qtbl_chr(jpeg->regs, ctx->compr_quality);
+
+		exynos4_jpeg_set_encode_tbl_select(jpeg->regs,
+							ctx->compr_quality);
+		exynos4_jpeg_set_stream_size(jpeg->regs, ctx->cap_q.w,
+							ctx->cap_q.h);
+
+		exynos4_jpeg_set_enc_out_fmt(jpeg->regs, ctx->subsampling);
+		exynos4_jpeg_set_img_fmt(jpeg->regs, ctx->out_q.fmt->fourcc);
+		exynos4_jpeg_set_img_addr(ctx);
+		exynos4_jpeg_set_jpeg_addr(ctx);
+		exynos4_jpeg_set_encode_hoff_cnt(jpeg->regs,
+							ctx->out_q.fmt->fourcc);
+	} else {
+		exynos4_jpeg_sw_reset(jpeg->regs);
+		exynos4_jpeg_set_interrupt(jpeg->regs);
+		exynos4_jpeg_set_img_addr(ctx);
+		exynos4_jpeg_set_jpeg_addr(ctx);
+		exynos4_jpeg_set_img_fmt(jpeg->regs, ctx->cap_q.fmt->fourcc);
+
+		bitstream_size = DIV_ROUND_UP(ctx->out_q.size, 32);
+
+		exynos4_jpeg_set_dec_bitstream_size(jpeg->regs, bitstream_size);
+	}
+
+	exynos4_jpeg_set_enc_dec_mode(jpeg->regs, ctx->mode);
+
+	spin_unlock_irqrestore(&ctx->jpeg->slock, flags);
 }
 
 static int s5p_jpeg_job_ready(void *priv)
@@ -1082,6 +1569,12 @@ static struct v4l2_m2m_ops s5p_jpeg_m2m_ops = {
 	.device_run	= s5p_jpeg_device_run,
 	.job_ready	= s5p_jpeg_job_ready,
 	.job_abort	= s5p_jpeg_job_abort,
+}
+;
+static struct v4l2_m2m_ops exynos_jpeg_m2m_ops = {
+	.device_run	= exynos4_jpeg_device_run,
+	.job_ready	= s5p_jpeg_job_ready,
+	.job_abort	= s5p_jpeg_job_abort,
 };
 
 /*
@@ -1149,7 +1642,7 @@ static void s5p_jpeg_buf_queue(struct vb2_buffer *vb)
 		ctx->hdr_parsed = s5p_jpeg_parse_hdr(&tmp,
 		     (unsigned long)vb2_plane_vaddr(vb, 0),
 		     min((unsigned long)ctx->out_q.size,
-			 vb2_get_plane_payload(vb, 0)));
+			 vb2_get_plane_payload(vb, 0)), ctx);
 		if (!ctx->hdr_parsed) {
 			vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
 			return;
@@ -1162,30 +1655,9 @@ static void s5p_jpeg_buf_queue(struct vb2_buffer *vb)
 		q_data = &ctx->cap_q;
 		q_data->w = tmp.w;
 		q_data->h = tmp.h;
-
-		jpeg_bound_align_image(&q_data->w, S5P_JPEG_MIN_WIDTH,
-				       S5P_JPEG_MAX_WIDTH, q_data->fmt->h_align,
-				       &q_data->h, S5P_JPEG_MIN_HEIGHT,
-				       S5P_JPEG_MAX_HEIGHT, q_data->fmt->v_align
-				      );
-		q_data->size = q_data->w * q_data->h * q_data->fmt->depth >> 3;
 	}
-	if (ctx->m2m_ctx)
-		v4l2_m2m_buf_queue(ctx->m2m_ctx, vb);
-}
-
-static void s5p_jpeg_wait_prepare(struct vb2_queue *vq)
-{
-	struct s5p_jpeg_ctx *ctx = vb2_get_drv_priv(vq);
 
-	mutex_unlock(&ctx->jpeg->lock);
-}
-
-static void s5p_jpeg_wait_finish(struct vb2_queue *vq)
-{
-	struct s5p_jpeg_ctx *ctx = vb2_get_drv_priv(vq);
-
-	mutex_lock(&ctx->jpeg->lock);
+	v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vb);
 }
 
 static int s5p_jpeg_start_streaming(struct vb2_queue *q, unsigned int count)
@@ -1211,8 +1683,8 @@ static struct vb2_ops s5p_jpeg_qops = {
 	.queue_setup		= s5p_jpeg_queue_setup,
 	.buf_prepare		= s5p_jpeg_buf_prepare,
 	.buf_queue		= s5p_jpeg_buf_queue,
-	.wait_prepare		= s5p_jpeg_wait_prepare,
-	.wait_finish		= s5p_jpeg_wait_finish,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
 	.start_streaming	= s5p_jpeg_start_streaming,
 	.stop_streaming		= s5p_jpeg_stop_streaming,
 };
@@ -1230,6 +1702,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	src_vq->ops = &s5p_jpeg_qops;
 	src_vq->mem_ops = &vb2_dma_contig_memops;
 	src_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	src_vq->lock = &ctx->jpeg->lock;
 
 	ret = vb2_queue_init(src_vq);
 	if (ret)
@@ -1242,6 +1715,7 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	dst_vq->ops = &s5p_jpeg_qops;
 	dst_vq->mem_ops = &vb2_dma_contig_memops;
 	dst_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	dst_vq->lock = &ctx->jpeg->lock;
 
 	return vb2_queue_init(dst_vq);
 }
@@ -1267,26 +1741,27 @@ static irqreturn_t s5p_jpeg_irq(int irq, void *dev_id)
 
 	curr_ctx = v4l2_m2m_get_curr_priv(jpeg->m2m_dev);
 
-	src_buf = v4l2_m2m_src_buf_remove(curr_ctx->m2m_ctx);
-	dst_buf = v4l2_m2m_dst_buf_remove(curr_ctx->m2m_ctx);
+	src_buf = v4l2_m2m_src_buf_remove(curr_ctx->fh.m2m_ctx);
+	dst_buf = v4l2_m2m_dst_buf_remove(curr_ctx->fh.m2m_ctx);
 
 	if (curr_ctx->mode == S5P_JPEG_ENCODE)
-		enc_jpeg_too_large = jpeg_enc_stream_stat(jpeg->regs);
-	timer_elapsed = jpeg_timer_stat(jpeg->regs);
-	op_completed = jpeg_result_stat_ok(jpeg->regs);
+		enc_jpeg_too_large = s5p_jpeg_enc_stream_stat(jpeg->regs);
+	timer_elapsed = s5p_jpeg_timer_stat(jpeg->regs);
+	op_completed = s5p_jpeg_result_stat_ok(jpeg->regs);
 	if (curr_ctx->mode == S5P_JPEG_DECODE)
-		op_completed = op_completed && jpeg_stream_stat_ok(jpeg->regs);
+		op_completed = op_completed &&
+					s5p_jpeg_stream_stat_ok(jpeg->regs);
 
 	if (enc_jpeg_too_large) {
 		state = VB2_BUF_STATE_ERROR;
-		jpeg_clear_enc_stream_stat(jpeg->regs);
+		s5p_jpeg_clear_enc_stream_stat(jpeg->regs);
 	} else if (timer_elapsed) {
 		state = VB2_BUF_STATE_ERROR;
-		jpeg_clear_timer_stat(jpeg->regs);
+		s5p_jpeg_clear_timer_stat(jpeg->regs);
 	} else if (!op_completed) {
 		state = VB2_BUF_STATE_ERROR;
 	} else {
-		payload_size = jpeg_compressed_size(jpeg->regs);
+		payload_size = s5p_jpeg_compressed_size(jpeg->regs);
 	}
 
 	dst_buf->v4l2_buf.timecode = src_buf->v4l2_buf.timecode;
@@ -1296,16 +1771,79 @@ static irqreturn_t s5p_jpeg_irq(int irq, void *dev_id)
 	if (curr_ctx->mode == S5P_JPEG_ENCODE)
 		vb2_set_plane_payload(dst_buf, 0, payload_size);
 	v4l2_m2m_buf_done(dst_buf, state);
-	v4l2_m2m_job_finish(jpeg->m2m_dev, curr_ctx->m2m_ctx);
+	v4l2_m2m_job_finish(jpeg->m2m_dev, curr_ctx->fh.m2m_ctx);
 
-	curr_ctx->subsampling = jpeg_get_subsampling_mode(jpeg->regs);
+	curr_ctx->subsampling = s5p_jpeg_get_subsampling_mode(jpeg->regs);
 	spin_unlock(&jpeg->slock);
 
-	jpeg_clear_int(jpeg->regs);
+	s5p_jpeg_clear_int(jpeg->regs);
 
 	return IRQ_HANDLED;
 }
 
+static irqreturn_t exynos4_jpeg_irq(int irq, void *priv)
+{
+	unsigned int int_status;
+	struct vb2_buffer *src_vb, *dst_vb;
+	struct s5p_jpeg *jpeg = priv;
+	struct s5p_jpeg_ctx *curr_ctx;
+	unsigned long payload_size = 0;
+
+	spin_lock(&jpeg->slock);
+
+	curr_ctx = v4l2_m2m_get_curr_priv(jpeg->m2m_dev);
+
+	src_vb = v4l2_m2m_src_buf_remove(curr_ctx->fh.m2m_ctx);
+	dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->fh.m2m_ctx);
+
+	int_status = exynos4_jpeg_get_int_status(jpeg->regs);
+
+	if (int_status) {
+		switch (int_status & 0x1f) {
+		case 0x1:
+			jpeg->irq_ret = ERR_PROT;
+			break;
+		case 0x2:
+			jpeg->irq_ret = OK_ENC_OR_DEC;
+			break;
+		case 0x4:
+			jpeg->irq_ret = ERR_DEC_INVALID_FORMAT;
+			break;
+		case 0x8:
+			jpeg->irq_ret = ERR_MULTI_SCAN;
+			break;
+		case 0x10:
+			jpeg->irq_ret = ERR_FRAME;
+			break;
+		default:
+			jpeg->irq_ret = ERR_UNKNOWN;
+			break;
+		}
+	} else {
+		jpeg->irq_ret = ERR_UNKNOWN;
+	}
+
+	if (jpeg->irq_ret == OK_ENC_OR_DEC) {
+		if (curr_ctx->mode == S5P_JPEG_ENCODE) {
+			payload_size = exynos4_jpeg_get_stream_size(jpeg->regs);
+			vb2_set_plane_payload(dst_vb, 0, payload_size);
+		}
+		v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);
+		v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);
+	} else {
+		v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_ERROR);
+		v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_ERROR);
+	}
+
+	v4l2_m2m_job_finish(jpeg->m2m_dev, curr_ctx->fh.m2m_ctx);
+	curr_ctx->subsampling = exynos4_jpeg_get_frame_fmt(jpeg->regs);
+
+	spin_unlock(&jpeg->slock);
+	return IRQ_HANDLED;
+}
+
+static void *jpeg_get_drv_data(struct platform_device *pdev);
+
 /*
  * ============================================================================
  * Driver basic infrastructure
@@ -1316,13 +1854,19 @@ 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);
+
 	mutex_init(&jpeg->lock);
 	spin_lock_init(&jpeg->slock);
 	jpeg->dev = &pdev->dev;
@@ -1341,8 +1885,8 @@ static int s5p_jpeg_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	ret = devm_request_irq(&pdev->dev, jpeg->irq, s5p_jpeg_irq, 0,
-			dev_name(&pdev->dev), jpeg);
+	ret = devm_request_irq(&pdev->dev, jpeg->irq, jpeg->variant->jpeg_irq,
+				0, dev_name(&pdev->dev), jpeg);
 	if (ret) {
 		dev_err(&pdev->dev, "cannot claim IRQ %d\n", jpeg->irq);
 		return ret;
@@ -1356,7 +1900,6 @@ static int s5p_jpeg_probe(struct platform_device *pdev)
 		return ret;
 	}
 	dev_dbg(&pdev->dev, "clock source %p\n", jpeg->clk);
-	clk_prepare_enable(jpeg->clk);
 
 	/* v4l2 device */
 	ret = v4l2_device_register(&pdev->dev, &jpeg->v4l2_dev);
@@ -1365,8 +1908,13 @@ 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(&s5p_jpeg_m2m_ops);
+	jpeg->m2m_dev = v4l2_m2m_init(samsung_jpeg_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);
@@ -1387,8 +1935,8 @@ static int s5p_jpeg_probe(struct platform_device *pdev)
 		ret = -ENOMEM;
 		goto vb2_allocator_rollback;
 	}
-	strlcpy(jpeg->vfd_encoder->name, S5P_JPEG_M2M_NAME,
-		sizeof(jpeg->vfd_encoder->name));
+	snprintf(jpeg->vfd_encoder->name, sizeof(jpeg->vfd_encoder->name),
+				"%s-enc", S5P_JPEG_M2M_NAME);
 	jpeg->vfd_encoder->fops		= &s5p_jpeg_fops;
 	jpeg->vfd_encoder->ioctl_ops	= &s5p_jpeg_ioctl_ops;
 	jpeg->vfd_encoder->minor	= -1;
@@ -1415,8 +1963,8 @@ static int s5p_jpeg_probe(struct platform_device *pdev)
 		ret = -ENOMEM;
 		goto enc_vdev_register_rollback;
 	}
-	strlcpy(jpeg->vfd_decoder->name, S5P_JPEG_M2M_NAME,
-		sizeof(jpeg->vfd_decoder->name));
+	snprintf(jpeg->vfd_decoder->name, sizeof(jpeg->vfd_decoder->name),
+				"%s-dec", S5P_JPEG_M2M_NAME);
 	jpeg->vfd_decoder->fops		= &s5p_jpeg_fops;
 	jpeg->vfd_decoder->ioctl_ops	= &s5p_jpeg_ioctl_ops;
 	jpeg->vfd_decoder->minor	= -1;
@@ -1464,7 +2012,6 @@ device_register_rollback:
 	v4l2_device_unregister(&jpeg->v4l2_dev);
 
 clk_get_rollback:
-	clk_disable_unprepare(jpeg->clk);
 	clk_put(jpeg->clk);
 
 	return ret;
@@ -1484,7 +2031,9 @@ static int s5p_jpeg_remove(struct platform_device *pdev)
 	v4l2_m2m_release(jpeg->m2m_dev);
 	v4l2_device_unregister(&jpeg->v4l2_dev);
 
-	clk_disable_unprepare(jpeg->clk);
+	if (!pm_runtime_status_suspended(&pdev->dev))
+		clk_disable_unprepare(jpeg->clk);
+
 	clk_put(jpeg->clk);
 
 	return 0;
@@ -1492,41 +2041,119 @@ static int s5p_jpeg_remove(struct platform_device *pdev)
 
 static int s5p_jpeg_runtime_suspend(struct device *dev)
 {
+	struct s5p_jpeg *jpeg = dev_get_drvdata(dev);
+
+	clk_disable_unprepare(jpeg->clk);
+
 	return 0;
 }
 
 static int s5p_jpeg_runtime_resume(struct device *dev)
 {
 	struct s5p_jpeg *jpeg = dev_get_drvdata(dev);
+	unsigned long flags;
+	int ret;
+
+	ret = clk_prepare_enable(jpeg->clk);
+	if (ret < 0)
+		return ret;
+
+	spin_lock_irqsave(&jpeg->slock, flags);
+
 	/*
 	 * JPEG IP allows storing two Huffman tables for each component
-	 * We fill table 0 for each component
+	 * We fill table 0 for each component and do this here only
+	 * for S5PC210 device as Exynos4x12 requires programming its
+	 * Huffman tables each time the encoding process is initialized.
 	 */
-	jpeg_set_hdctbl(jpeg->regs);
-	jpeg_set_hdctblg(jpeg->regs);
-	jpeg_set_hactbl(jpeg->regs);
-	jpeg_set_hactblg(jpeg->regs);
+	if (jpeg->variant->version == SJPEG_S5P) {
+		s5p_jpeg_set_hdctbl(jpeg->regs);
+		s5p_jpeg_set_hdctblg(jpeg->regs);
+		s5p_jpeg_set_hactbl(jpeg->regs);
+		s5p_jpeg_set_hactblg(jpeg->regs);
+	}
+
+	spin_unlock_irqrestore(&jpeg->slock, flags);
+
 	return 0;
 }
 
+static int s5p_jpeg_suspend(struct device *dev)
+{
+	if (pm_runtime_suspended(dev))
+		return 0;
+
+	return s5p_jpeg_runtime_suspend(dev);
+}
+
+static int s5p_jpeg_resume(struct device *dev)
+{
+	if (pm_runtime_suspended(dev))
+		return 0;
+
+	return s5p_jpeg_runtime_resume(dev);
+}
+
 static const struct dev_pm_ops s5p_jpeg_pm_ops = {
-	.runtime_suspend = s5p_jpeg_runtime_suspend,
-	.runtime_resume	 = s5p_jpeg_runtime_resume,
+	SET_SYSTEM_SLEEP_PM_OPS(s5p_jpeg_suspend, s5p_jpeg_resume)
+	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,
+};
+
+static struct s5p_jpeg_variant exynos4_jpeg_drvdata = {
+	.version	= SJPEG_EXYNOS4,
+	.jpeg_irq	= exynos4_jpeg_irq,
+};
+
+static const struct of_device_id samsung_jpeg_match[] = {
+	{
+		.compatible = "samsung,s5pv210-jpeg",
+		.data = &s5p_jpeg_drvdata,
+	}, {
+		.compatible = "samsung,exynos4210-jpeg",
+		.data = &s5p_jpeg_drvdata,
+	}, {
+		.compatible = "samsung,exynos4212-jpeg",
+		.data = &exynos4_jpeg_drvdata,
+	},
+	{},
 };
 
+MODULE_DEVICE_TABLE(of, samsung_jpeg_match);
+
+static void *jpeg_get_drv_data(struct platform_device *pdev)
+{
+	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 (match)
+		driver_data = (struct s5p_jpeg_variant *)match->data;
+
+	return driver_data;
+}
+#endif
+
 static struct platform_driver s5p_jpeg_driver = {
 	.probe = s5p_jpeg_probe,
 	.remove = s5p_jpeg_remove,
 	.driver = {
-		.owner = THIS_MODULE,
-		.name = S5P_JPEG_M2M_NAME,
-		.pm = &s5p_jpeg_pm_ops,
+		.of_match_table	= of_match_ptr(samsung_jpeg_match),
+		.owner		= THIS_MODULE,
+		.name		= S5P_JPEG_M2M_NAME,
+		.pm		= &s5p_jpeg_pm_ops,
 	},
 };
 
 module_platform_driver(s5p_jpeg_driver);
 
 MODULE_AUTHOR("Andrzej Pietrasiewicz <andrzej.p@samsung.com>");
+MODULE_AUTHOR("Jacek Anaszewski <j.anaszewski@samsung.com>");
 MODULE_DESCRIPTION("Samsung JPEG codec driver");
 MODULE_LICENSE("GPL");
-
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-core.h b/drivers/media/platform/s5p-jpeg/jpeg-core.h
index 8a4013e3aee7..f482dbf55d5f 100644
--- a/drivers/media/platform/s5p-jpeg/jpeg-core.h
+++ b/drivers/media/platform/s5p-jpeg/jpeg-core.h
@@ -13,6 +13,7 @@
 #ifndef JPEG_CORE_H_
 #define JPEG_CORE_H_
 
+#include <linux/interrupt.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-fh.h>
 #include <media/v4l2-ctrls.h>
@@ -43,8 +44,45 @@
 #define DHP				0xde
 
 /* Flags that indicate a format can be used for capture/output */
-#define MEM2MEM_CAPTURE			(1 << 0)
-#define MEM2MEM_OUTPUT			(1 << 1)
+#define SJPEG_FMT_FLAG_ENC_CAPTURE	(1 << 0)
+#define SJPEG_FMT_FLAG_ENC_OUTPUT	(1 << 1)
+#define SJPEG_FMT_FLAG_DEC_CAPTURE	(1 << 2)
+#define SJPEG_FMT_FLAG_DEC_OUTPUT	(1 << 3)
+#define SJPEG_FMT_FLAG_S5P		(1 << 4)
+#define SJPEG_FMT_FLAG_EXYNOS4		(1 << 5)
+#define SJPEG_FMT_RGB			(1 << 6)
+#define SJPEG_FMT_NON_RGB		(1 << 7)
+
+#define S5P_JPEG_ENCODE		0
+#define S5P_JPEG_DECODE		1
+
+#define FMT_TYPE_OUTPUT		0
+#define FMT_TYPE_CAPTURE	1
+
+#define SJPEG_SUBSAMPLING_444	0x11
+#define SJPEG_SUBSAMPLING_422	0x21
+#define SJPEG_SUBSAMPLING_420	0x22
+
+/* Version numbers */
+
+#define SJPEG_S5P	1
+#define SJPEG_EXYNOS4	2
+
+enum exynos4_jpeg_result {
+	OK_ENC_OR_DEC,
+	ERR_PROT,
+	ERR_DEC_INVALID_FORMAT,
+	ERR_MULTI_SCAN,
+	ERR_FRAME,
+	ERR_UNKNOWN,
+};
+
+enum  exynos4_jpeg_img_quality_level {
+	QUALITY_LEVEL_1 = 0,	/* high */
+	QUALITY_LEVEL_2,
+	QUALITY_LEVEL_3,
+	QUALITY_LEVEL_4,	/* low */
+};
 
 /**
  * struct s5p_jpeg - JPEG IP abstraction
@@ -71,9 +109,16 @@ struct s5p_jpeg {
 
 	void __iomem		*regs;
 	unsigned int		irq;
+	enum exynos4_jpeg_result irq_ret;
 	struct clk		*clk;
 	struct device		*dev;
 	void			*alloc_ctx;
+	struct s5p_jpeg_variant *variant;
+};
+
+struct s5p_jpeg_variant {
+	unsigned int	version;
+	irqreturn_t	(*jpeg_irq)(int irq, void *priv);
 };
 
 /**
@@ -84,16 +129,18 @@ struct s5p_jpeg {
  * @colplanes:	number of color planes (1 for packed formats)
  * @h_align:	horizontal alignment order (align to 2^h_align)
  * @v_align:	vertical alignment order (align to 2^v_align)
- * @types:	types of queue this format is applicable to
+ * @flags:	flags describing format applicability
  */
 struct s5p_jpeg_fmt {
 	char	*name;
 	u32	fourcc;
 	int	depth;
 	int	colplanes;
+	int	memplanes;
 	int	h_align;
 	int	v_align;
-	u32	types;
+	int	subsampling;
+	u32	flags;
 };
 
 /**
@@ -115,7 +162,6 @@ struct s5p_jpeg_q_data {
  * @jpeg:		JPEG IP device for this context
  * @mode:		compression (encode) operation or decompression (decode)
  * @compr_quality:	destination image quality in compression (encode) mode
- * @m2m_ctx:		mem2mem device context
  * @out_q:		source (output) queue information
  * @cap_fmt:		destination (capture) queue queue information
  * @hdr_parsed:		set if header has been parsed during decompression
@@ -127,7 +173,6 @@ struct s5p_jpeg_ctx {
 	unsigned short		compr_quality;
 	unsigned short		restart_interval;
 	unsigned short		subsampling;
-	struct v4l2_m2m_ctx	*m2m_ctx;
 	struct s5p_jpeg_q_data	out_q;
 	struct s5p_jpeg_q_data	cap_q;
 	struct v4l2_fh		fh;
@@ -147,4 +192,16 @@ struct s5p_jpeg_buffer {
 	unsigned long data;
 };
 
+/**
+ * struct s5p_jpeg_addr - JPEG converter physical address set for DMA
+ * @y:   luminance plane physical address
+ * @cb:  Cb plane physical address
+ * @cr:  Cr plane physical address
+ */
+struct s5p_jpeg_addr {
+	u32     y;
+	u32     cb;
+	u32     cr;
+};
+
 #endif /* JPEG_CORE_H */
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos4.c b/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos4.c
new file mode 100644
index 000000000000..da8d6a1a984f
--- /dev/null
+++ b/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos4.c
@@ -0,0 +1,279 @@
+/* Copyright (c) 2013 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com/
+ *
+ * Author: Jacek Anaszewski <j.anaszewski@samsung.com>
+ *
+ * Register interface file for JPEG driver on Exynos4x12.
+ *
+ * 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/delay.h>
+
+#include "jpeg-core.h"
+#include "jpeg-hw-exynos4.h"
+#include "jpeg-regs.h"
+
+void exynos4_jpeg_sw_reset(void __iomem *base)
+{
+	unsigned int reg;
+
+	reg = readl(base + EXYNOS4_JPEG_CNTL_REG);
+	writel(reg & ~EXYNOS4_SOFT_RESET_HI, base + EXYNOS4_JPEG_CNTL_REG);
+
+	ndelay(100000);
+
+	writel(reg | EXYNOS4_SOFT_RESET_HI, base + EXYNOS4_JPEG_CNTL_REG);
+}
+
+void exynos4_jpeg_set_enc_dec_mode(void __iomem *base, unsigned int mode)
+{
+	unsigned int reg;
+
+	reg = readl(base + EXYNOS4_JPEG_CNTL_REG);
+	/* set exynos4_jpeg mod register */
+	if (mode == S5P_JPEG_DECODE) {
+		writel((reg & EXYNOS4_ENC_DEC_MODE_MASK) |
+					EXYNOS4_DEC_MODE,
+			base + EXYNOS4_JPEG_CNTL_REG);
+	} else {/* encode */
+		writel((reg & EXYNOS4_ENC_DEC_MODE_MASK) |
+					EXYNOS4_ENC_MODE,
+			base + EXYNOS4_JPEG_CNTL_REG);
+	}
+}
+
+void exynos4_jpeg_set_img_fmt(void __iomem *base, unsigned int img_fmt)
+{
+	unsigned int reg;
+
+	reg = readl(base + EXYNOS4_IMG_FMT_REG) &
+			EXYNOS4_ENC_IN_FMT_MASK; /* clear except enc format */
+
+	switch (img_fmt) {
+	case V4L2_PIX_FMT_GREY:
+		reg = reg | EXYNOS4_ENC_GRAY_IMG | EXYNOS4_GRAY_IMG_IP;
+		break;
+	case V4L2_PIX_FMT_RGB32:
+		reg = reg | EXYNOS4_ENC_RGB_IMG |
+				EXYNOS4_RGB_IP_RGB_32BIT_IMG;
+		break;
+	case V4L2_PIX_FMT_RGB565:
+		reg = reg | EXYNOS4_ENC_RGB_IMG |
+				EXYNOS4_RGB_IP_RGB_16BIT_IMG;
+		break;
+	case V4L2_PIX_FMT_NV24:
+		reg = reg | EXYNOS4_ENC_YUV_444_IMG |
+				EXYNOS4_YUV_444_IP_YUV_444_2P_IMG |
+				EXYNOS4_SWAP_CHROMA_CBCR;
+		break;
+	case V4L2_PIX_FMT_NV42:
+		reg = reg | EXYNOS4_ENC_YUV_444_IMG |
+				EXYNOS4_YUV_444_IP_YUV_444_2P_IMG |
+				EXYNOS4_SWAP_CHROMA_CRCB;
+		break;
+	case V4L2_PIX_FMT_YUYV:
+		reg = reg | EXYNOS4_DEC_YUV_422_IMG |
+				EXYNOS4_YUV_422_IP_YUV_422_1P_IMG |
+				EXYNOS4_SWAP_CHROMA_CBCR;
+		break;
+
+	case V4L2_PIX_FMT_YVYU:
+		reg = reg | EXYNOS4_DEC_YUV_422_IMG |
+				EXYNOS4_YUV_422_IP_YUV_422_1P_IMG |
+				EXYNOS4_SWAP_CHROMA_CRCB;
+		break;
+	case V4L2_PIX_FMT_NV16:
+		reg = reg | EXYNOS4_DEC_YUV_422_IMG |
+				EXYNOS4_YUV_422_IP_YUV_422_2P_IMG |
+				EXYNOS4_SWAP_CHROMA_CBCR;
+		break;
+	case V4L2_PIX_FMT_NV61:
+		reg = reg | EXYNOS4_DEC_YUV_422_IMG |
+				EXYNOS4_YUV_422_IP_YUV_422_2P_IMG |
+				EXYNOS4_SWAP_CHROMA_CRCB;
+		break;
+	case V4L2_PIX_FMT_NV12:
+		reg = reg | EXYNOS4_DEC_YUV_420_IMG |
+				EXYNOS4_YUV_420_IP_YUV_420_2P_IMG |
+				EXYNOS4_SWAP_CHROMA_CBCR;
+		break;
+	case V4L2_PIX_FMT_NV21:
+		reg = reg | EXYNOS4_DEC_YUV_420_IMG |
+				EXYNOS4_YUV_420_IP_YUV_420_2P_IMG |
+				EXYNOS4_SWAP_CHROMA_CRCB;
+		break;
+	case V4L2_PIX_FMT_YUV420:
+		reg = reg | EXYNOS4_DEC_YUV_420_IMG |
+				EXYNOS4_YUV_420_IP_YUV_420_3P_IMG |
+				EXYNOS4_SWAP_CHROMA_CBCR;
+		break;
+	default:
+		break;
+
+	}
+
+	writel(reg, base + EXYNOS4_IMG_FMT_REG);
+}
+
+void exynos4_jpeg_set_enc_out_fmt(void __iomem *base, unsigned int out_fmt)
+{
+	unsigned int reg;
+
+	reg = readl(base + EXYNOS4_IMG_FMT_REG) &
+			~EXYNOS4_ENC_FMT_MASK; /* clear enc format */
+
+	switch (out_fmt) {
+	case V4L2_JPEG_CHROMA_SUBSAMPLING_GRAY:
+		reg = reg | EXYNOS4_ENC_FMT_GRAY;
+		break;
+
+	case V4L2_JPEG_CHROMA_SUBSAMPLING_444:
+		reg = reg | EXYNOS4_ENC_FMT_YUV_444;
+		break;
+
+	case V4L2_JPEG_CHROMA_SUBSAMPLING_422:
+		reg = reg | EXYNOS4_ENC_FMT_YUV_422;
+		break;
+
+	case V4L2_JPEG_CHROMA_SUBSAMPLING_420:
+		reg = reg | EXYNOS4_ENC_FMT_YUV_420;
+		break;
+
+	default:
+		break;
+	}
+
+	writel(reg, base + EXYNOS4_IMG_FMT_REG);
+}
+
+void exynos4_jpeg_set_interrupt(void __iomem *base)
+{
+	unsigned int reg;
+
+	reg = readl(base + EXYNOS4_INT_EN_REG) & ~EXYNOS4_INT_EN_MASK;
+	writel(EXYNOS4_INT_EN_ALL, base + EXYNOS4_INT_EN_REG);
+}
+
+unsigned int exynos4_jpeg_get_int_status(void __iomem *base)
+{
+	unsigned int	int_status;
+
+	int_status = readl(base + EXYNOS4_INT_STATUS_REG);
+
+	return int_status;
+}
+
+unsigned int exynos4_jpeg_get_fifo_status(void __iomem *base)
+{
+	unsigned int fifo_status;
+
+	fifo_status = readl(base + EXYNOS4_FIFO_STATUS_REG);
+
+	return fifo_status;
+}
+
+void exynos4_jpeg_set_huf_table_enable(void __iomem *base, int value)
+{
+	unsigned int	reg;
+
+	reg = readl(base + EXYNOS4_JPEG_CNTL_REG) & ~EXYNOS4_HUF_TBL_EN;
+
+	if (value == 1)
+		writel(reg | EXYNOS4_HUF_TBL_EN,
+					base + EXYNOS4_JPEG_CNTL_REG);
+	else
+		writel(reg | ~EXYNOS4_HUF_TBL_EN,
+					base + EXYNOS4_JPEG_CNTL_REG);
+}
+
+void exynos4_jpeg_set_sys_int_enable(void __iomem *base, int value)
+{
+	unsigned int	reg;
+
+	reg = readl(base + EXYNOS4_JPEG_CNTL_REG) & ~(EXYNOS4_SYS_INT_EN);
+
+	if (value == 1)
+		writel(EXYNOS4_SYS_INT_EN, base + EXYNOS4_JPEG_CNTL_REG);
+	else
+		writel(~EXYNOS4_SYS_INT_EN, base + EXYNOS4_JPEG_CNTL_REG);
+}
+
+void exynos4_jpeg_set_stream_buf_address(void __iomem *base,
+					 unsigned int address)
+{
+	writel(address, base + EXYNOS4_OUT_MEM_BASE_REG);
+}
+
+void exynos4_jpeg_set_stream_size(void __iomem *base,
+		unsigned int x_value, unsigned int y_value)
+{
+	writel(0x0, base + EXYNOS4_JPEG_IMG_SIZE_REG); /* clear */
+	writel(EXYNOS4_X_SIZE(x_value) | EXYNOS4_Y_SIZE(y_value),
+			base + EXYNOS4_JPEG_IMG_SIZE_REG);
+}
+
+void exynos4_jpeg_set_frame_buf_address(void __iomem *base,
+				struct s5p_jpeg_addr *exynos4_jpeg_addr)
+{
+	writel(exynos4_jpeg_addr->y, base + EXYNOS4_IMG_BA_PLANE_1_REG);
+	writel(exynos4_jpeg_addr->cb, base + EXYNOS4_IMG_BA_PLANE_2_REG);
+	writel(exynos4_jpeg_addr->cr, base + EXYNOS4_IMG_BA_PLANE_3_REG);
+}
+
+void exynos4_jpeg_set_encode_tbl_select(void __iomem *base,
+		enum exynos4_jpeg_img_quality_level level)
+{
+	unsigned int	reg;
+
+	reg = EXYNOS4_Q_TBL_COMP1_0 | EXYNOS4_Q_TBL_COMP2_1 |
+		EXYNOS4_Q_TBL_COMP3_1 |
+		EXYNOS4_HUFF_TBL_COMP1_AC_0_DC_1 |
+		EXYNOS4_HUFF_TBL_COMP2_AC_0_DC_0 |
+		EXYNOS4_HUFF_TBL_COMP3_AC_1_DC_1;
+
+	writel(reg, base + EXYNOS4_TBL_SEL_REG);
+}
+
+void exynos4_jpeg_set_encode_hoff_cnt(void __iomem *base, unsigned int fmt)
+{
+	if (fmt == V4L2_PIX_FMT_GREY)
+		writel(0xd2, base + EXYNOS4_HUFF_CNT_REG);
+	else
+		writel(0x1a2, base + EXYNOS4_HUFF_CNT_REG);
+}
+
+unsigned int exynos4_jpeg_get_stream_size(void __iomem *base)
+{
+	unsigned int size;
+
+	size = readl(base + EXYNOS4_BITSTREAM_SIZE_REG);
+	return size;
+}
+
+void exynos4_jpeg_set_dec_bitstream_size(void __iomem *base, unsigned int size)
+{
+	writel(size, base + EXYNOS4_BITSTREAM_SIZE_REG);
+}
+
+void exynos4_jpeg_get_frame_size(void __iomem *base,
+			unsigned int *width, unsigned int *height)
+{
+	*width = (readl(base + EXYNOS4_DECODE_XY_SIZE_REG) &
+				EXYNOS4_DECODED_SIZE_MASK);
+	*height = (readl(base + EXYNOS4_DECODE_XY_SIZE_REG) >> 16) &
+				EXYNOS4_DECODED_SIZE_MASK;
+}
+
+unsigned int exynos4_jpeg_get_frame_fmt(void __iomem *base)
+{
+	return readl(base + EXYNOS4_DECODE_IMG_FMT_REG) &
+				EXYNOS4_JPEG_DECODED_IMG_FMT_MASK;
+}
+
+void exynos4_jpeg_set_timer_count(void __iomem *base, unsigned int size)
+{
+	writel(size, base + EXYNOS4_INT_TIMER_COUNT_REG);
+}
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos4.h b/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos4.h
new file mode 100644
index 000000000000..c228d28a4bc7
--- /dev/null
+++ b/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos4.h
@@ -0,0 +1,42 @@
+/* Copyright (c) 2013 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com/
+ *
+ * Author: Jacek Anaszewski <j.anaszewski@samsung.com>
+ *
+ * Header file of the register interface for JPEG driver on Exynos4x12.
+ *
+ * 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 JPEG_HW_EXYNOS4_H_
+#define JPEG_HW_EXYNOS4_H_
+
+void exynos4_jpeg_sw_reset(void __iomem *base);
+void exynos4_jpeg_set_enc_dec_mode(void __iomem *base, unsigned int mode);
+void exynos4_jpeg_set_img_fmt(void __iomem *base, unsigned int img_fmt);
+void exynos4_jpeg_set_enc_out_fmt(void __iomem *base, unsigned int out_fmt);
+void exynos4_jpeg_set_enc_tbl(void __iomem *base);
+void exynos4_jpeg_set_interrupt(void __iomem *base);
+unsigned int exynos4_jpeg_get_int_status(void __iomem *base);
+void exynos4_jpeg_set_huf_table_enable(void __iomem *base, int value);
+void exynos4_jpeg_set_sys_int_enable(void __iomem *base, int value);
+void exynos4_jpeg_set_stream_buf_address(void __iomem *base,
+					 unsigned int address);
+void exynos4_jpeg_set_stream_size(void __iomem *base,
+		unsigned int x_value, unsigned int y_value);
+void exynos4_jpeg_set_frame_buf_address(void __iomem *base,
+				struct s5p_jpeg_addr *jpeg_addr);
+void exynos4_jpeg_set_encode_tbl_select(void __iomem *base,
+		enum exynos4_jpeg_img_quality_level level);
+void exynos4_jpeg_set_encode_hoff_cnt(void __iomem *base, unsigned int fmt);
+void exynos4_jpeg_set_dec_bitstream_size(void __iomem *base, unsigned int size);
+unsigned int exynos4_jpeg_get_stream_size(void __iomem *base);
+void exynos4_jpeg_get_frame_size(void __iomem *base,
+			unsigned int *width, unsigned int *height);
+unsigned int exynos4_jpeg_get_frame_fmt(void __iomem *base);
+unsigned int exynos4_jpeg_get_fifo_status(void __iomem *base);
+void exynos4_jpeg_set_timer_count(void __iomem *base, unsigned int size);
+
+#endif /* JPEG_HW_EXYNOS4_H_ */
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-hw.h b/drivers/media/platform/s5p-jpeg/jpeg-hw-s5p.c
index b47e887b6138..52407d790726 100644
--- a/drivers/media/platform/s5p-jpeg/jpeg-hw.h
+++ b/drivers/media/platform/s5p-jpeg/jpeg-hw-s5p.c
@@ -9,27 +9,15 @@
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
  */
-#ifndef JPEG_HW_H_
-#define JPEG_HW_H_
 
 #include <linux/io.h>
 #include <linux/videodev2.h>
 
-#include "jpeg-hw.h"
+#include "jpeg-core.h"
 #include "jpeg-regs.h"
+#include "jpeg-hw-s5p.h"
 
-#define S5P_JPEG_MIN_WIDTH		32
-#define S5P_JPEG_MIN_HEIGHT		32
-#define S5P_JPEG_MAX_WIDTH		8192
-#define S5P_JPEG_MAX_HEIGHT		8192
-#define S5P_JPEG_ENCODE			0
-#define S5P_JPEG_DECODE			1
-#define S5P_JPEG_RAW_IN_565		0
-#define S5P_JPEG_RAW_IN_422		1
-#define S5P_JPEG_RAW_OUT_422		0
-#define S5P_JPEG_RAW_OUT_420		1
-
-static inline void jpeg_reset(void __iomem *regs)
+void s5p_jpeg_reset(void __iomem *regs)
 {
 	unsigned long reg;
 
@@ -42,12 +30,12 @@ static inline void jpeg_reset(void __iomem *regs)
 	}
 }
 
-static inline void jpeg_poweron(void __iomem *regs)
+void s5p_jpeg_poweron(void __iomem *regs)
 {
 	writel(S5P_POWER_ON, regs + S5P_JPGCLKCON);
 }
 
-static inline void jpeg_input_raw_mode(void __iomem *regs, unsigned long mode)
+void s5p_jpeg_input_raw_mode(void __iomem *regs, unsigned long mode)
 {
 	unsigned long reg, m;
 
@@ -63,7 +51,7 @@ static inline void jpeg_input_raw_mode(void __iomem *regs, unsigned long mode)
 	writel(reg, regs + S5P_JPGCMOD);
 }
 
-static inline void jpeg_input_raw_y16(void __iomem *regs, bool y16)
+void s5p_jpeg_input_raw_y16(void __iomem *regs, bool y16)
 {
 	unsigned long reg;
 
@@ -75,7 +63,7 @@ static inline void jpeg_input_raw_y16(void __iomem *regs, bool y16)
 	writel(reg, regs + S5P_JPGCMOD);
 }
 
-static inline void jpeg_proc_mode(void __iomem *regs, unsigned long mode)
+void s5p_jpeg_proc_mode(void __iomem *regs, unsigned long mode)
 {
 	unsigned long reg, m;
 
@@ -90,7 +78,7 @@ static inline void jpeg_proc_mode(void __iomem *regs, unsigned long mode)
 	writel(reg, regs + S5P_JPGMOD);
 }
 
-static inline void jpeg_subsampling_mode(void __iomem *regs, unsigned int mode)
+void s5p_jpeg_subsampling_mode(void __iomem *regs, unsigned int mode)
 {
 	unsigned long reg, m;
 
@@ -105,12 +93,12 @@ static inline void jpeg_subsampling_mode(void __iomem *regs, unsigned int mode)
 	writel(reg, regs + S5P_JPGMOD);
 }
 
-static inline unsigned int jpeg_get_subsampling_mode(void __iomem *regs)
+unsigned int s5p_jpeg_get_subsampling_mode(void __iomem *regs)
 {
 	return readl(regs + S5P_JPGMOD) & S5P_SUBSAMPLING_MODE_MASK;
 }
 
-static inline void jpeg_dri(void __iomem *regs, unsigned int dri)
+void s5p_jpeg_dri(void __iomem *regs, unsigned int dri)
 {
 	unsigned long reg;
 
@@ -125,7 +113,7 @@ static inline void jpeg_dri(void __iomem *regs, unsigned int dri)
 	writel(reg, regs + S5P_JPGDRI_L);
 }
 
-static inline void jpeg_qtbl(void __iomem *regs, unsigned int t, unsigned int n)
+void s5p_jpeg_qtbl(void __iomem *regs, unsigned int t, unsigned int n)
 {
 	unsigned long reg;
 
@@ -135,7 +123,7 @@ static inline void jpeg_qtbl(void __iomem *regs, unsigned int t, unsigned int n)
 	writel(reg, regs + S5P_JPG_QTBL);
 }
 
-static inline void jpeg_htbl_ac(void __iomem *regs, unsigned int t)
+void s5p_jpeg_htbl_ac(void __iomem *regs, unsigned int t)
 {
 	unsigned long reg;
 
@@ -146,7 +134,7 @@ static inline void jpeg_htbl_ac(void __iomem *regs, unsigned int t)
 	writel(reg, regs + S5P_JPG_HTBL);
 }
 
-static inline void jpeg_htbl_dc(void __iomem *regs, unsigned int t)
+void s5p_jpeg_htbl_dc(void __iomem *regs, unsigned int t)
 {
 	unsigned long reg;
 
@@ -157,7 +145,7 @@ static inline void jpeg_htbl_dc(void __iomem *regs, unsigned int t)
 	writel(reg, regs + S5P_JPG_HTBL);
 }
 
-static inline void jpeg_y(void __iomem *regs, unsigned int y)
+void s5p_jpeg_y(void __iomem *regs, unsigned int y)
 {
 	unsigned long reg;
 
@@ -172,7 +160,7 @@ static inline void jpeg_y(void __iomem *regs, unsigned int y)
 	writel(reg, regs + S5P_JPGY_L);
 }
 
-static inline void jpeg_x(void __iomem *regs, unsigned int x)
+void s5p_jpeg_x(void __iomem *regs, unsigned int x)
 {
 	unsigned long reg;
 
@@ -187,7 +175,7 @@ static inline void jpeg_x(void __iomem *regs, unsigned int x)
 	writel(reg, regs + S5P_JPGX_L);
 }
 
-static inline void jpeg_rst_int_enable(void __iomem *regs, bool enable)
+void s5p_jpeg_rst_int_enable(void __iomem *regs, bool enable)
 {
 	unsigned long reg;
 
@@ -198,7 +186,7 @@ static inline void jpeg_rst_int_enable(void __iomem *regs, bool enable)
 	writel(reg, regs + S5P_JPGINTSE);
 }
 
-static inline void jpeg_data_num_int_enable(void __iomem *regs, bool enable)
+void s5p_jpeg_data_num_int_enable(void __iomem *regs, bool enable)
 {
 	unsigned long reg;
 
@@ -209,7 +197,7 @@ static inline void jpeg_data_num_int_enable(void __iomem *regs, bool enable)
 	writel(reg, regs + S5P_JPGINTSE);
 }
 
-static inline void jpeg_final_mcu_num_int_enable(void __iomem *regs, bool enbl)
+void s5p_jpeg_final_mcu_num_int_enable(void __iomem *regs, bool enbl)
 {
 	unsigned long reg;
 
@@ -220,7 +208,7 @@ static inline void jpeg_final_mcu_num_int_enable(void __iomem *regs, bool enbl)
 	writel(reg, regs + S5P_JPGINTSE);
 }
 
-static inline void jpeg_timer_enable(void __iomem *regs, unsigned long val)
+void s5p_jpeg_timer_enable(void __iomem *regs, unsigned long val)
 {
 	unsigned long reg;
 
@@ -231,7 +219,7 @@ static inline void jpeg_timer_enable(void __iomem *regs, unsigned long val)
 	writel(reg, regs + S5P_JPG_TIMER_SE);
 }
 
-static inline void jpeg_timer_disable(void __iomem *regs)
+void s5p_jpeg_timer_disable(void __iomem *regs)
 {
 	unsigned long reg;
 
@@ -240,13 +228,13 @@ static inline void jpeg_timer_disable(void __iomem *regs)
 	writel(reg, regs + S5P_JPG_TIMER_SE);
 }
 
-static inline int jpeg_timer_stat(void __iomem *regs)
+int s5p_jpeg_timer_stat(void __iomem *regs)
 {
 	return (int)((readl(regs + S5P_JPG_TIMER_ST) & S5P_TIMER_INT_STAT_MASK)
 		     >> S5P_TIMER_INT_STAT_SHIFT);
 }
 
-static inline void jpeg_clear_timer_stat(void __iomem *regs)
+void s5p_jpeg_clear_timer_stat(void __iomem *regs)
 {
 	unsigned long reg;
 
@@ -255,7 +243,7 @@ static inline void jpeg_clear_timer_stat(void __iomem *regs)
 	writel(reg, regs + S5P_JPG_TIMER_SE);
 }
 
-static inline void jpeg_enc_stream_int(void __iomem *regs, unsigned long size)
+void s5p_jpeg_enc_stream_int(void __iomem *regs, unsigned long size)
 {
 	unsigned long reg;
 
@@ -266,13 +254,13 @@ static inline void jpeg_enc_stream_int(void __iomem *regs, unsigned long size)
 	writel(reg, regs + S5P_JPG_ENC_STREAM_INTSE);
 }
 
-static inline int jpeg_enc_stream_stat(void __iomem *regs)
+int s5p_jpeg_enc_stream_stat(void __iomem *regs)
 {
 	return (int)(readl(regs + S5P_JPG_ENC_STREAM_INTST) &
 		     S5P_ENC_STREAM_INT_STAT_MASK);
 }
 
-static inline void jpeg_clear_enc_stream_stat(void __iomem *regs)
+void s5p_jpeg_clear_enc_stream_stat(void __iomem *regs)
 {
 	unsigned long reg;
 
@@ -281,7 +269,7 @@ static inline void jpeg_clear_enc_stream_stat(void __iomem *regs)
 	writel(reg, regs + S5P_JPG_ENC_STREAM_INTSE);
 }
 
-static inline void jpeg_outform_raw(void __iomem *regs, unsigned long format)
+void s5p_jpeg_outform_raw(void __iomem *regs, unsigned long format)
 {
 	unsigned long reg, f;
 
@@ -296,17 +284,17 @@ static inline void jpeg_outform_raw(void __iomem *regs, unsigned long format)
 	writel(reg, regs + S5P_JPG_OUTFORM);
 }
 
-static inline void jpeg_jpgadr(void __iomem *regs, unsigned long addr)
+void s5p_jpeg_jpgadr(void __iomem *regs, unsigned long addr)
 {
 	writel(addr, regs + S5P_JPG_JPGADR);
 }
 
-static inline void jpeg_imgadr(void __iomem *regs, unsigned long addr)
+void s5p_jpeg_imgadr(void __iomem *regs, unsigned long addr)
 {
 	writel(addr, regs + S5P_JPG_IMGADR);
 }
 
-static inline void jpeg_coef(void __iomem *regs, unsigned int i,
+void s5p_jpeg_coef(void __iomem *regs, unsigned int i,
 			     unsigned int j, unsigned int coef)
 {
 	unsigned long reg;
@@ -317,24 +305,24 @@ static inline void jpeg_coef(void __iomem *regs, unsigned int i,
 	writel(reg, regs + S5P_JPG_COEF(i));
 }
 
-static inline void jpeg_start(void __iomem *regs)
+void s5p_jpeg_start(void __iomem *regs)
 {
 	writel(1, regs + S5P_JSTART);
 }
 
-static inline int jpeg_result_stat_ok(void __iomem *regs)
+int s5p_jpeg_result_stat_ok(void __iomem *regs)
 {
 	return (int)((readl(regs + S5P_JPGINTST) & S5P_RESULT_STAT_MASK)
 		     >> S5P_RESULT_STAT_SHIFT);
 }
 
-static inline int jpeg_stream_stat_ok(void __iomem *regs)
+int s5p_jpeg_stream_stat_ok(void __iomem *regs)
 {
 	return !(int)((readl(regs + S5P_JPGINTST) & S5P_STREAM_STAT_MASK)
 		      >> S5P_STREAM_STAT_SHIFT);
 }
 
-static inline void jpeg_clear_int(void __iomem *regs)
+void s5p_jpeg_clear_int(void __iomem *regs)
 {
 	unsigned long reg;
 
@@ -343,7 +331,7 @@ static inline void jpeg_clear_int(void __iomem *regs)
 	reg = readl(regs + S5P_JPGOPR);
 }
 
-static inline unsigned int jpeg_compressed_size(void __iomem *regs)
+unsigned int s5p_jpeg_compressed_size(void __iomem *regs)
 {
 	unsigned long jpeg_size = 0;
 
@@ -353,5 +341,3 @@ static inline unsigned int jpeg_compressed_size(void __iomem *regs)
 
 	return (unsigned int)jpeg_size;
 }
-
-#endif /* JPEG_HW_H_ */
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-hw-s5p.h b/drivers/media/platform/s5p-jpeg/jpeg-hw-s5p.h
new file mode 100644
index 000000000000..c11ebe86b9c9
--- /dev/null
+++ b/drivers/media/platform/s5p-jpeg/jpeg-hw-s5p.h
@@ -0,0 +1,63 @@
+/* linux/drivers/media/platform/s5p-jpeg/jpeg-hw.h
+ *
+ * Copyright (c) 2011 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 JPEG_HW_S5P_H_
+#define JPEG_HW_S5P_H_
+
+#include <linux/io.h>
+#include <linux/videodev2.h>
+
+#include "jpeg-regs.h"
+
+#define S5P_JPEG_MIN_WIDTH		32
+#define S5P_JPEG_MIN_HEIGHT		32
+#define S5P_JPEG_MAX_WIDTH		8192
+#define S5P_JPEG_MAX_HEIGHT		8192
+#define S5P_JPEG_RAW_IN_565		0
+#define S5P_JPEG_RAW_IN_422		1
+#define S5P_JPEG_RAW_OUT_422		0
+#define S5P_JPEG_RAW_OUT_420		1
+
+void s5p_jpeg_reset(void __iomem *regs);
+void s5p_jpeg_poweron(void __iomem *regs);
+void s5p_jpeg_input_raw_mode(void __iomem *regs, unsigned long mode);
+void s5p_jpeg_input_raw_y16(void __iomem *regs, bool y16);
+void s5p_jpeg_proc_mode(void __iomem *regs, unsigned long mode);
+void s5p_jpeg_subsampling_mode(void __iomem *regs, unsigned int mode);
+unsigned int s5p_jpeg_get_subsampling_mode(void __iomem *regs);
+void s5p_jpeg_dri(void __iomem *regs, unsigned int dri);
+void s5p_jpeg_qtbl(void __iomem *regs, unsigned int t, unsigned int n);
+void s5p_jpeg_htbl_ac(void __iomem *regs, unsigned int t);
+void s5p_jpeg_htbl_dc(void __iomem *regs, unsigned int t);
+void s5p_jpeg_y(void __iomem *regs, unsigned int y);
+void s5p_jpeg_x(void __iomem *regs, unsigned int x);
+void s5p_jpeg_rst_int_enable(void __iomem *regs, bool enable);
+void s5p_jpeg_data_num_int_enable(void __iomem *regs, bool enable);
+void s5p_jpeg_final_mcu_num_int_enable(void __iomem *regs, bool enbl);
+void s5p_jpeg_timer_enable(void __iomem *regs, unsigned long val);
+void s5p_jpeg_timer_disable(void __iomem *regs);
+int s5p_jpeg_timer_stat(void __iomem *regs);
+void s5p_jpeg_clear_timer_stat(void __iomem *regs);
+void s5p_jpeg_enc_stream_int(void __iomem *regs, unsigned long size);
+int s5p_jpeg_enc_stream_stat(void __iomem *regs);
+void s5p_jpeg_clear_enc_stream_stat(void __iomem *regs);
+void s5p_jpeg_outform_raw(void __iomem *regs, unsigned long format);
+void s5p_jpeg_jpgadr(void __iomem *regs, unsigned long addr);
+void s5p_jpeg_imgadr(void __iomem *regs, unsigned long addr);
+void s5p_jpeg_coef(void __iomem *regs, unsigned int i,
+			     unsigned int j, unsigned int coef);
+void s5p_jpeg_start(void __iomem *regs);
+int s5p_jpeg_result_stat_ok(void __iomem *regs);
+int s5p_jpeg_stream_stat_ok(void __iomem *regs);
+void s5p_jpeg_clear_int(void __iomem *regs);
+unsigned int s5p_jpeg_compressed_size(void __iomem *regs);
+
+#endif /* JPEG_HW_S5P_H_ */
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-regs.h b/drivers/media/platform/s5p-jpeg/jpeg-regs.h
index 38e50815668c..33f2c7374cfd 100644
--- a/drivers/media/platform/s5p-jpeg/jpeg-regs.h
+++ b/drivers/media/platform/s5p-jpeg/jpeg-regs.h
@@ -2,10 +2,11 @@
  *
  * Register definition file for Samsung JPEG codec driver
  *
- * Copyright (c) 2011 Samsung Electronics Co., Ltd.
+ * Copyright (c) 2011-2013 Samsung Electronics Co., Ltd.
  *		http://www.samsung.com
  *
  * Author: Andrzej Pietrasiewicz <andrzej.p@samsung.com>
+ * Author: Jacek Anaszewski <j.anaszewski@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
@@ -15,6 +16,8 @@
 #ifndef JPEG_REGS_H_
 #define JPEG_REGS_H_
 
+/* Register and bit definitions for S5PC210 */
+
 /* JPEG mode register */
 #define S5P_JPGMOD			0x00
 #define S5P_PROC_MODE_MASK		(0x1 << 3)
@@ -166,5 +169,209 @@
 /* JPEG AC Huffman table register */
 #define S5P_JPG_HACTBLG(n)		(0x8c0 + (n) * 0x400)
 
+
+/* Register and bit definitions for Exynos 4x12 */
+
+/* JPEG Codec Control Registers */
+#define EXYNOS4_JPEG_CNTL_REG		0x00
+#define EXYNOS4_INT_EN_REG		0x04
+#define EXYNOS4_INT_TIMER_COUNT_REG	0x08
+#define EXYNOS4_INT_STATUS_REG		0x0c
+#define EXYNOS4_OUT_MEM_BASE_REG		0x10
+#define EXYNOS4_JPEG_IMG_SIZE_REG	0x14
+#define EXYNOS4_IMG_BA_PLANE_1_REG	0x18
+#define EXYNOS4_IMG_SO_PLANE_1_REG	0x1c
+#define EXYNOS4_IMG_PO_PLANE_1_REG	0x20
+#define EXYNOS4_IMG_BA_PLANE_2_REG	0x24
+#define EXYNOS4_IMG_SO_PLANE_2_REG	0x28
+#define EXYNOS4_IMG_PO_PLANE_2_REG	0x2c
+#define EXYNOS4_IMG_BA_PLANE_3_REG	0x30
+#define EXYNOS4_IMG_SO_PLANE_3_REG	0x34
+#define EXYNOS4_IMG_PO_PLANE_3_REG	0x38
+
+#define EXYNOS4_TBL_SEL_REG		0x3c
+
+#define EXYNOS4_IMG_FMT_REG		0x40
+
+#define EXYNOS4_BITSTREAM_SIZE_REG	0x44
+#define EXYNOS4_PADDING_REG		0x48
+#define EXYNOS4_HUFF_CNT_REG		0x4c
+#define EXYNOS4_FIFO_STATUS_REG	0x50
+#define EXYNOS4_DECODE_XY_SIZE_REG	0x54
+#define EXYNOS4_DECODE_IMG_FMT_REG	0x58
+
+#define EXYNOS4_QUAN_TBL_ENTRY_REG	0x100
+#define EXYNOS4_HUFF_TBL_ENTRY_REG	0x200
+
+
+/****************************************************************/
+/* Bit definition part						*/
+/****************************************************************/
+
+/* JPEG CNTL Register bit */
+#define EXYNOS4_ENC_DEC_MODE_MASK	(0xfffffffc << 0)
+#define EXYNOS4_DEC_MODE			(1 << 0)
+#define EXYNOS4_ENC_MODE			(1 << 1)
+#define EXYNOS4_AUTO_RST_MARKER		(1 << 2)
+#define EXYNOS4_RST_INTERVAL_SHIFT	3
+#define EXYNOS4_RST_INTERVAL(x)		(((x) & 0xffff) \
+						<< EXYNOS4_RST_INTERVAL_SHIFT)
+#define EXYNOS4_HUF_TBL_EN		(1 << 19)
+#define EXYNOS4_HOR_SCALING_SHIFT	20
+#define EXYNOS4_HOR_SCALING_MASK		(3 << EXYNOS4_HOR_SCALING_SHIFT)
+#define EXYNOS4_HOR_SCALING(x)		(((x) & 0x3) \
+						<< EXYNOS4_HOR_SCALING_SHIFT)
+#define EXYNOS4_VER_SCALING_SHIFT	22
+#define EXYNOS4_VER_SCALING_MASK		(3 << EXYNOS4_VER_SCALING_SHIFT)
+#define EXYNOS4_VER_SCALING(x)		(((x) & 0x3) \
+						<< EXYNOS4_VER_SCALING_SHIFT)
+#define EXYNOS4_PADDING			(1 << 27)
+#define EXYNOS4_SYS_INT_EN		(1 << 28)
+#define EXYNOS4_SOFT_RESET_HI		(1 << 29)
+
+/* JPEG INT Register bit */
+#define EXYNOS4_INT_EN_MASK		(0x1f << 0)
+#define EXYNOS4_PROT_ERR_INT_EN		(1 << 0)
+#define EXYNOS4_IMG_COMPLETION_INT_EN	(1 << 1)
+#define EXYNOS4_DEC_INVALID_FORMAT_EN	(1 << 2)
+#define EXYNOS4_MULTI_SCAN_ERROR_EN	(1 << 3)
+#define EXYNOS4_FRAME_ERR_EN		(1 << 4)
+#define EXYNOS4_INT_EN_ALL		(0x1f << 0)
+
+#define EXYNOS4_MOD_REG_PROC_ENC		(0 << 3)
+#define EXYNOS4_MOD_REG_PROC_DEC		(1 << 3)
+
+#define EXYNOS4_MOD_REG_SUBSAMPLE_444	(0 << 0)
+#define EXYNOS4_MOD_REG_SUBSAMPLE_422	(1 << 0)
+#define EXYNOS4_MOD_REG_SUBSAMPLE_420	(2 << 0)
+#define EXYNOS4_MOD_REG_SUBSAMPLE_GRAY	(3 << 0)
+
+
+/* JPEG IMAGE SIZE Register bit */
+#define EXYNOS4_X_SIZE_SHIFT		0
+#define EXYNOS4_X_SIZE_MASK		(0xffff << EXYNOS4_X_SIZE_SHIFT)
+#define EXYNOS4_X_SIZE(x)		(((x) & 0xffff) << EXYNOS4_X_SIZE_SHIFT)
+#define EXYNOS4_Y_SIZE_SHIFT		16
+#define EXYNOS4_Y_SIZE_MASK		(0xffff << EXYNOS4_Y_SIZE_SHIFT)
+#define EXYNOS4_Y_SIZE(x)		(((x) & 0xffff) << EXYNOS4_Y_SIZE_SHIFT)
+
+/* JPEG IMAGE FORMAT Register bit */
+#define EXYNOS4_ENC_IN_FMT_MASK		0xffff0000
+#define EXYNOS4_ENC_GRAY_IMG		(0 << 0)
+#define EXYNOS4_ENC_RGB_IMG		(1 << 0)
+#define EXYNOS4_ENC_YUV_444_IMG		(2 << 0)
+#define EXYNOS4_ENC_YUV_422_IMG		(3 << 0)
+#define EXYNOS4_ENC_YUV_440_IMG		(4 << 0)
+
+#define EXYNOS4_DEC_GRAY_IMG		(0 << 0)
+#define EXYNOS4_DEC_RGB_IMG		(1 << 0)
+#define EXYNOS4_DEC_YUV_444_IMG		(2 << 0)
+#define EXYNOS4_DEC_YUV_422_IMG		(3 << 0)
+#define EXYNOS4_DEC_YUV_420_IMG		(4 << 0)
+
+#define EXYNOS4_GRAY_IMG_IP_SHIFT	3
+#define EXYNOS4_GRAY_IMG_IP_MASK		(7 << EXYNOS4_GRAY_IMG_IP_SHIFT)
+#define EXYNOS4_GRAY_IMG_IP		(4 << EXYNOS4_GRAY_IMG_IP_SHIFT)
+
+#define EXYNOS4_RGB_IP_SHIFT		6
+#define EXYNOS4_RGB_IP_MASK		(7 << EXYNOS4_RGB_IP_SHIFT)
+#define EXYNOS4_RGB_IP_RGB_16BIT_IMG	(4 << EXYNOS4_RGB_IP_SHIFT)
+#define EXYNOS4_RGB_IP_RGB_32BIT_IMG	(5 << EXYNOS4_RGB_IP_SHIFT)
+
+#define EXYNOS4_YUV_444_IP_SHIFT			9
+#define EXYNOS4_YUV_444_IP_MASK			(7 << EXYNOS4_YUV_444_IP_SHIFT)
+#define EXYNOS4_YUV_444_IP_YUV_444_2P_IMG	(4 << EXYNOS4_YUV_444_IP_SHIFT)
+#define EXYNOS4_YUV_444_IP_YUV_444_3P_IMG	(5 << EXYNOS4_YUV_444_IP_SHIFT)
+
+#define EXYNOS4_YUV_422_IP_SHIFT			12
+#define EXYNOS4_YUV_422_IP_MASK			(7 << EXYNOS4_YUV_422_IP_SHIFT)
+#define EXYNOS4_YUV_422_IP_YUV_422_1P_IMG	(4 << EXYNOS4_YUV_422_IP_SHIFT)
+#define EXYNOS4_YUV_422_IP_YUV_422_2P_IMG	(5 << EXYNOS4_YUV_422_IP_SHIFT)
+#define EXYNOS4_YUV_422_IP_YUV_422_3P_IMG	(6 << EXYNOS4_YUV_422_IP_SHIFT)
+
+#define EXYNOS4_YUV_420_IP_SHIFT			15
+#define EXYNOS4_YUV_420_IP_MASK			(7 << EXYNOS4_YUV_420_IP_SHIFT)
+#define EXYNOS4_YUV_420_IP_YUV_420_2P_IMG	(4 << EXYNOS4_YUV_420_IP_SHIFT)
+#define EXYNOS4_YUV_420_IP_YUV_420_3P_IMG	(5 << EXYNOS4_YUV_420_IP_SHIFT)
+
+#define EXYNOS4_ENC_FMT_SHIFT			24
+#define EXYNOS4_ENC_FMT_MASK			(3 << EXYNOS4_ENC_FMT_SHIFT)
+#define EXYNOS4_ENC_FMT_GRAY			(0 << EXYNOS4_ENC_FMT_SHIFT)
+#define EXYNOS4_ENC_FMT_YUV_444			(1 << EXYNOS4_ENC_FMT_SHIFT)
+#define EXYNOS4_ENC_FMT_YUV_422			(2 << EXYNOS4_ENC_FMT_SHIFT)
+#define EXYNOS4_ENC_FMT_YUV_420			(3 << EXYNOS4_ENC_FMT_SHIFT)
+
+#define EXYNOS4_JPEG_DECODED_IMG_FMT_MASK	0x03
+
+#define EXYNOS4_SWAP_CHROMA_CRCB			(1 << 26)
+#define EXYNOS4_SWAP_CHROMA_CBCR			(0 << 26)
+
+/* JPEG HUFF count Register bit */
+#define EXYNOS4_HUFF_COUNT_MASK			0xffff
+
+/* JPEG Decoded_img_x_y_size Register bit */
+#define EXYNOS4_DECODED_SIZE_MASK		0x0000ffff
+
+/* JPEG Decoded image format Register bit */
+#define EXYNOS4_DECODED_IMG_FMT_MASK		0x3
+
+/* JPEG TBL SEL Register bit */
+#define EXYNOS4_Q_TBL_COMP1_0		(0 << 0)
+#define EXYNOS4_Q_TBL_COMP1_1		(1 << 0)
+#define EXYNOS4_Q_TBL_COMP1_2		(2 << 0)
+#define EXYNOS4_Q_TBL_COMP1_3		(3 << 0)
+
+#define EXYNOS4_Q_TBL_COMP2_0		(0 << 2)
+#define EXYNOS4_Q_TBL_COMP2_1		(1 << 2)
+#define EXYNOS4_Q_TBL_COMP2_2		(2 << 2)
+#define EXYNOS4_Q_TBL_COMP2_3		(3 << 2)
+
+#define EXYNOS4_Q_TBL_COMP3_0		(0 << 4)
+#define EXYNOS4_Q_TBL_COMP3_1		(1 << 4)
+#define EXYNOS4_Q_TBL_COMP3_2		(2 << 4)
+#define EXYNOS4_Q_TBL_COMP3_3		(3 << 4)
+
+#define EXYNOS4_HUFF_TBL_COMP1_AC_0_DC_0	(0 << 6)
+#define EXYNOS4_HUFF_TBL_COMP1_AC_0_DC_1	(1 << 6)
+#define EXYNOS4_HUFF_TBL_COMP1_AC_1_DC_0	(2 << 6)
+#define EXYNOS4_HUFF_TBL_COMP1_AC_1_DC_1	(3 << 6)
+
+#define EXYNOS4_HUFF_TBL_COMP2_AC_0_DC_0	(0 << 8)
+#define EXYNOS4_HUFF_TBL_COMP2_AC_0_DC_1	(1 << 8)
+#define EXYNOS4_HUFF_TBL_COMP2_AC_1_DC_0	(2 << 8)
+#define EXYNOS4_HUFF_TBL_COMP2_AC_1_DC_1	(3 << 8)
+
+#define EXYNOS4_HUFF_TBL_COMP3_AC_0_DC_0	(0 << 10)
+#define EXYNOS4_HUFF_TBL_COMP3_AC_0_DC_1	(1 << 10)
+#define EXYNOS4_HUFF_TBL_COMP3_AC_1_DC_0	(2 << 10)
+#define EXYNOS4_HUFF_TBL_COMP3_AC_1_DC_1	(3 << 10)
+
+/* JPEG quantizer table register */
+#define EXYNOS4_QTBL_CONTENT(n)	(0x100 + (n) * 0x40)
+
+/* JPEG DC luminance (code length) Huffman table register */
+#define EXYNOS4_HUFF_TBL_HDCLL	0x200
+
+/* JPEG DC luminance (values) Huffman table register */
+#define EXYNOS4_HUFF_TBL_HDCLV	0x210
+
+/* JPEG DC chrominance (code length) Huffman table register */
+#define EXYNOS4_HUFF_TBL_HDCCL	0x220
+
+/* JPEG DC chrominance (values) Huffman table register */
+#define EXYNOS4_HUFF_TBL_HDCCV	0x230
+
+/* JPEG AC luminance (code length) Huffman table register */
+#define EXYNOS4_HUFF_TBL_HACLL	0x240
+
+/* JPEG AC luminance (values) Huffman table register */
+#define EXYNOS4_HUFF_TBL_HACLV	0x250
+
+/* JPEG AC chrominance (code length) Huffman table register */
+#define EXYNOS4_HUFF_TBL_HACCL	0x300
+
+/* JPEG AC chrominance (values) Huffman table register */
+#define EXYNOS4_HUFF_TBL_HACCV	0x310
+
 #endif /* JPEG_REGS_H_ */
 
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc.c b/drivers/media/platform/s5p-mfc/s5p_mfc.c
index e46067a57853..e2aac592d29f 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc.c
@@ -177,21 +177,6 @@ unlock:
 		mutex_unlock(&dev->mfc_mutex);
 }
 
-static enum s5p_mfc_node_type s5p_mfc_get_node_type(struct file *file)
-{
-	struct video_device *vdev = video_devdata(file);
-
-	if (!vdev) {
-		mfc_err("failed to get video_device");
-		return MFCNODE_INVALID;
-	}
-	if (vdev->index == 0)
-		return MFCNODE_DECODER;
-	else if (vdev->index == 1)
-		return MFCNODE_ENCODER;
-	return MFCNODE_INVALID;
-}
-
 static void s5p_mfc_clear_int_flags(struct s5p_mfc_dev *dev)
 {
 	mfc_write(dev, 0, S5P_FIMV_RISC_HOST_INT);
@@ -705,6 +690,7 @@ irq_cleanup_hw:
 /* Open an MFC node */
 static int s5p_mfc_open(struct file *file)
 {
+	struct video_device *vdev = video_devdata(file);
 	struct s5p_mfc_dev *dev = video_drvdata(file);
 	struct s5p_mfc_ctx *ctx = NULL;
 	struct vb2_queue *q;
@@ -742,7 +728,7 @@ static int s5p_mfc_open(struct file *file)
 	/* Mark context as idle */
 	clear_work_bit_irqsave(ctx);
 	dev->ctx[ctx->num] = ctx;
-	if (s5p_mfc_get_node_type(file) == MFCNODE_DECODER) {
+	if (vdev == dev->vfd_dec) {
 		ctx->type = MFCINST_DECODER;
 		ctx->c_ops = get_dec_codec_ops();
 		s5p_mfc_dec_init(ctx);
@@ -752,7 +738,7 @@ static int s5p_mfc_open(struct file *file)
 			mfc_err("Failed to setup mfc controls\n");
 			goto err_ctrls_setup;
 		}
-	} else if (s5p_mfc_get_node_type(file) == MFCNODE_ENCODER) {
+	} else if (vdev == dev->vfd_enc) {
 		ctx->type = MFCINST_ENCODER;
 		ctx->c_ops = get_enc_codec_ops();
 		/* only for encoder */
@@ -797,10 +783,10 @@ static int s5p_mfc_open(struct file *file)
 	q = &ctx->vq_dst;
 	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
 	q->drv_priv = &ctx->fh;
-	if (s5p_mfc_get_node_type(file) == MFCNODE_DECODER) {
+	if (vdev == dev->vfd_dec) {
 		q->io_modes = VB2_MMAP;
 		q->ops = get_dec_queue_ops();
-	} else if (s5p_mfc_get_node_type(file) == MFCNODE_ENCODER) {
+	} else if (vdev == dev->vfd_enc) {
 		q->io_modes = VB2_MMAP | VB2_USERPTR;
 		q->ops = get_enc_queue_ops();
 	} else {
@@ -819,10 +805,10 @@ static int s5p_mfc_open(struct file *file)
 	q->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
 	q->io_modes = VB2_MMAP;
 	q->drv_priv = &ctx->fh;
-	if (s5p_mfc_get_node_type(file) == MFCNODE_DECODER) {
+	if (vdev == dev->vfd_dec) {
 		q->io_modes = VB2_MMAP;
 		q->ops = get_dec_queue_ops();
-	} else if (s5p_mfc_get_node_type(file) == MFCNODE_ENCODER) {
+	} else if (vdev == dev->vfd_enc) {
 		q->io_modes = VB2_MMAP | VB2_USERPTR;
 		q->ops = get_enc_queue_ops();
 	} else {
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_common.h b/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
index 6920b546181a..f723f1f2f578 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
@@ -115,15 +115,6 @@ enum s5p_mfc_fmt_type {
 };
 
 /**
- * enum s5p_mfc_node_type - The type of an MFC device node.
- */
-enum s5p_mfc_node_type {
-	MFCNODE_INVALID = -1,
-	MFCNODE_DECODER = 0,
-	MFCNODE_ENCODER = 1,
-};
-
-/**
  * enum s5p_mfc_inst_type - The type of an MFC instance.
  */
 enum s5p_mfc_inst_type {
@@ -422,6 +413,11 @@ struct s5p_mfc_vp8_enc_params {
 	enum v4l2_vp8_golden_frame_sel golden_frame_sel;
 	u8 hier_layer;
 	u8 hier_layer_qp[3];
+	u8 rc_min_qp;
+	u8 rc_max_qp;
+	u8 rc_frame_qp;
+	u8 rc_p_frame_qp;
+	u8 profile;
 };
 
 /**
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
index 4ff3b6cd6842..91b6e020ddf3 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
@@ -618,6 +618,46 @@ static struct mfc_control controls[] = {
 		.default_value = V4L2_CID_MPEG_VIDEO_VPX_GOLDEN_FRAME_USE_PREV,
 		.menu_skip_mask = 0,
 	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_VPX_MAX_QP,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 0,
+		.maximum = 127,
+		.step = 1,
+		.default_value = 127,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_VPX_MIN_QP,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 0,
+		.maximum = 11,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_VPX_I_FRAME_QP,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 0,
+		.maximum = 127,
+		.step = 1,
+		.default_value = 10,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_VPX_P_FRAME_QP,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 0,
+		.maximum = 127,
+		.step = 1,
+		.default_value = 10,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_VPX_PROFILE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 0,
+		.maximum = 3,
+		.step = 1,
+		.default_value = 0,
+	},
 };
 
 #define NUM_CTRLS ARRAY_SIZE(controls)
@@ -1557,6 +1597,21 @@ static int s5p_mfc_enc_s_ctrl(struct v4l2_ctrl *ctrl)
 	case V4L2_CID_MPEG_VIDEO_VPX_GOLDEN_FRAME_SEL:
 		p->codec.vp8.golden_frame_sel = ctrl->val;
 		break;
+	case V4L2_CID_MPEG_VIDEO_VPX_MIN_QP:
+		p->codec.vp8.rc_min_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_VPX_MAX_QP:
+		p->codec.vp8.rc_max_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_VPX_I_FRAME_QP:
+		p->codec.vp8.rc_frame_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_VPX_P_FRAME_QP:
+		p->codec.vp8.rc_p_frame_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_VPX_PROFILE:
+		p->codec.vp8.profile = ctrl->val;
+		break;
 	default:
 		v4l2_err(&dev->v4l2_dev, "Invalid control, id=%d, val=%d\n",
 							ctrl->id, ctrl->val);
@@ -1863,7 +1918,7 @@ static int s5p_mfc_start_streaming(struct vb2_queue *q, unsigned int count)
 		if (ctx->src_bufs_cnt < ctx->pb_count) {
 			mfc_err("Need minimum %d OUTPUT buffers\n",
 					ctx->pb_count);
-			return -EINVAL;
+			return -ENOBUFS;
 		}
 	}
 
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 461358c4a790..f6ff2dbf3a1d 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
@@ -1197,10 +1197,8 @@ static int s5p_mfc_set_enc_params_vp8(struct s5p_mfc_ctx *ctx)
 	reg |= ((p->num_b_frame & 0x3) << 16);
 	WRITEL(reg, S5P_FIMV_E_GOP_CONFIG_V6);
 
-	/* profile & level */
-	reg = 0;
-	/** profile */
-	reg |= (0x1 << 4);
+	/* profile - 0 ~ 3 */
+	reg = p_vp8->profile & 0x3;
 	WRITEL(reg, S5P_FIMV_E_PICTURE_PROFILE_V6);
 
 	/* rate control config. */
@@ -1218,6 +1216,26 @@ static int s5p_mfc_set_enc_params_vp8(struct s5p_mfc_ctx *ctx)
 		WRITEL(reg, S5P_FIMV_E_RC_FRAME_RATE_V6);
 	}
 
+	/* frame QP */
+	reg &= ~(0x7F);
+	reg |= p_vp8->rc_frame_qp & 0x7F;
+	WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+
+	/* other QPs */
+	WRITEL(0x0, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+	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);
+	}
+
+	/* 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);
+
 	/* vbv buffer size */
 	if (p->frame_skip_mode ==
 			V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
diff --git a/drivers/media/platform/s5p-tv/mixer_drv.c b/drivers/media/platform/s5p-tv/mixer_drv.c
index 51805a5e2beb..bc08b5f28e44 100644
--- a/drivers/media/platform/s5p-tv/mixer_drv.c
+++ b/drivers/media/platform/s5p-tv/mixer_drv.c
@@ -347,19 +347,41 @@ static int mxr_runtime_resume(struct device *dev)
 {
 	struct mxr_device *mdev = to_mdev(dev);
 	struct mxr_resources *res = &mdev->res;
+	int ret;
 
 	mxr_dbg(mdev, "resume - start\n");
 	mutex_lock(&mdev->mutex);
 	/* turn clocks on */
-	clk_enable(res->mixer);
-	clk_enable(res->vp);
-	clk_enable(res->sclk_mixer);
+	ret = clk_prepare_enable(res->mixer);
+	if (ret < 0) {
+		dev_err(mdev->dev, "clk_prepare_enable(mixer) failed\n");
+		goto fail;
+	}
+	ret = clk_prepare_enable(res->vp);
+	if (ret < 0) {
+		dev_err(mdev->dev, "clk_prepare_enable(vp) failed\n");
+		goto fail_mixer;
+	}
+	ret = clk_prepare_enable(res->sclk_mixer);
+	if (ret < 0) {
+		dev_err(mdev->dev, "clk_prepare_enable(sclk_mixer) failed\n");
+		goto fail_vp;
+	}
 	/* apply default configuration */
 	mxr_reg_reset(mdev);
 	mxr_dbg(mdev, "resume - finished\n");
 
 	mutex_unlock(&mdev->mutex);
 	return 0;
+
+fail_vp:
+	clk_disable_unprepare(res->vp);
+fail_mixer:
+	clk_disable_unprepare(res->mixer);
+fail:
+	mutex_unlock(&mdev->mutex);
+	dev_err(mdev->dev, "resume failed\n");
+	return ret;
 }
 
 static int mxr_runtime_suspend(struct device *dev)
@@ -369,9 +391,9 @@ static int mxr_runtime_suspend(struct device *dev)
 	mxr_dbg(mdev, "suspend - start\n");
 	mutex_lock(&mdev->mutex);
 	/* turn clocks off */
-	clk_disable(res->sclk_mixer);
-	clk_disable(res->vp);
-	clk_disable(res->mixer);
+	clk_disable_unprepare(res->sclk_mixer);
+	clk_disable_unprepare(res->vp);
+	clk_disable_unprepare(res->mixer);
 	mutex_unlock(&mdev->mutex);
 	mxr_dbg(mdev, "suspend - finished\n");
 	return 0;
diff --git a/drivers/media/platform/s5p-tv/mixer_video.c b/drivers/media/platform/s5p-tv/mixer_video.c
index 81b97db111d8..c5059ba0d733 100644
--- a/drivers/media/platform/s5p-tv/mixer_video.c
+++ b/drivers/media/platform/s5p-tv/mixer_video.c
@@ -948,7 +948,7 @@ static int start_streaming(struct vb2_queue *vq, unsigned int count)
 
 	if (count == 0) {
 		mxr_dbg(mdev, "no output buffers queued\n");
-		return -EINVAL;
+		return -ENOBUFS;
 	}
 
 	/* block any changes in output configuration */
diff --git a/drivers/media/platform/s5p-tv/sdo_drv.c b/drivers/media/platform/s5p-tv/sdo_drv.c
index 0afa90f0f6ab..5a7c3796f22e 100644
--- a/drivers/media/platform/s5p-tv/sdo_drv.c
+++ b/drivers/media/platform/s5p-tv/sdo_drv.c
@@ -55,6 +55,8 @@ struct sdo_device {
 	struct clk *dacphy;
 	/** clock for control of VPLL */
 	struct clk *fout_vpll;
+	/** vpll rate before sdo stream was on */
+	unsigned long vpll_rate;
 	/** regulator for SDO IP power */
 	struct regulator *vdac;
 	/** regulator for SDO plug detection */
@@ -193,17 +195,33 @@ static int sdo_s_power(struct v4l2_subdev *sd, int on)
 
 static int sdo_streamon(struct sdo_device *sdev)
 {
+	int ret;
+
 	/* set proper clock for Timing Generator */
-	clk_set_rate(sdev->fout_vpll, 54000000);
+	sdev->vpll_rate = clk_get_rate(sdev->fout_vpll);
+	ret = clk_set_rate(sdev->fout_vpll, 54000000);
+	if (ret < 0) {
+		dev_err(sdev->dev, "Failed to set vpll rate\n");
+		return ret;
+	}
 	dev_info(sdev->dev, "fout_vpll.rate = %lu\n",
 	clk_get_rate(sdev->fout_vpll));
 	/* enable clock in SDO */
 	sdo_write_mask(sdev, SDO_CLKCON, ~0, SDO_TVOUT_CLOCK_ON);
-	clk_enable(sdev->dacphy);
+	ret = clk_prepare_enable(sdev->dacphy);
+	if (ret < 0) {
+		dev_err(sdev->dev, "clk_prepare_enable(dacphy) failed\n");
+		goto fail;
+	}
 	/* enable DAC */
 	sdo_write_mask(sdev, SDO_DAC, ~0, SDO_POWER_ON_DAC);
 	sdo_reg_debug(sdev);
 	return 0;
+
+fail:
+	sdo_write_mask(sdev, SDO_CLKCON, 0, SDO_TVOUT_CLOCK_ON);
+	clk_set_rate(sdev->fout_vpll, sdev->vpll_rate);
+	return ret;
 }
 
 static int sdo_streamoff(struct sdo_device *sdev)
@@ -211,7 +229,7 @@ static int sdo_streamoff(struct sdo_device *sdev)
 	int tries;
 
 	sdo_write_mask(sdev, SDO_DAC, 0, SDO_POWER_ON_DAC);
-	clk_disable(sdev->dacphy);
+	clk_disable_unprepare(sdev->dacphy);
 	sdo_write_mask(sdev, SDO_CLKCON, 0, SDO_TVOUT_CLOCK_ON);
 	for (tries = 100; tries; --tries) {
 		if (sdo_read(sdev, SDO_CLKCON) & SDO_TVOUT_CLOCK_READY)
@@ -220,6 +238,7 @@ static int sdo_streamoff(struct sdo_device *sdev)
 	}
 	if (tries == 0)
 		dev_err(sdev->dev, "failed to stop streaming\n");
+	clk_set_rate(sdev->fout_vpll, sdev->vpll_rate);
 	return tries ? 0 : -EIO;
 }
 
@@ -254,7 +273,7 @@ static int sdo_runtime_suspend(struct device *dev)
 	dev_info(dev, "suspend\n");
 	regulator_disable(sdev->vdet);
 	regulator_disable(sdev->vdac);
-	clk_disable(sdev->sclk_dac);
+	clk_disable_unprepare(sdev->sclk_dac);
 	return 0;
 }
 
@@ -266,7 +285,7 @@ static int sdo_runtime_resume(struct device *dev)
 
 	dev_info(dev, "resume\n");
 
-	ret = clk_enable(sdev->sclk_dac);
+	ret = clk_prepare_enable(sdev->sclk_dac);
 	if (ret < 0)
 		return ret;
 
@@ -299,7 +318,7 @@ static int sdo_runtime_resume(struct device *dev)
 vdac_r_dis:
 	regulator_disable(sdev->vdac);
 dac_clk_dis:
-	clk_disable(sdev->sclk_dac);
+	clk_disable_unprepare(sdev->sclk_dac);
 	return ret;
 }
 
@@ -405,7 +424,11 @@ static int sdo_probe(struct platform_device *pdev)
 	}
 
 	/* enable gate for dac clock, because mixer uses it */
-	clk_enable(sdev->dac);
+	ret = clk_prepare_enable(sdev->dac);
+	if (ret < 0) {
+		dev_err(dev, "clk_prepare_enable(dac) failed\n");
+		goto fail_fout_vpll;
+	}
 
 	/* configure power management */
 	pm_runtime_enable(dev);
@@ -444,7 +467,7 @@ static int sdo_remove(struct platform_device *pdev)
 	struct sdo_device *sdev = sd_to_sdev(sd);
 
 	pm_runtime_disable(&pdev->dev);
-	clk_disable(sdev->dac);
+	clk_disable_unprepare(sdev->dac);
 	clk_put(sdev->fout_vpll);
 	clk_put(sdev->dacphy);
 	clk_put(sdev->dac);
diff --git a/drivers/media/platform/sh_vou.c b/drivers/media/platform/sh_vou.c
index 4f30341dc2ab..e5f1d4c14f2c 100644
--- a/drivers/media/platform/sh_vou.c
+++ b/drivers/media/platform/sh_vou.c
@@ -286,7 +286,7 @@ static int sh_vou_buf_prepare(struct videobuf_queue *vq,
 	vb->size = vb->height * bytes_per_line;
 	if (vb->baddr && vb->bsize < vb->size) {
 		/* User buffer too small */
-		dev_warn(vq->dev, "User buffer too small: [%u] @ %lx\n",
+		dev_warn(vq->dev, "User buffer too small: [%zu] @ %lx\n",
 			 vb->bsize, vb->baddr);
 		return -EINVAL;
 	}
@@ -302,9 +302,10 @@ static int sh_vou_buf_prepare(struct videobuf_queue *vq,
 	}
 
 	dev_dbg(vou_dev->v4l2_dev.dev,
-		"%s(): fmt #%d, %u bytes per line, phys 0x%x, type %d, state %d\n",
+		"%s(): fmt #%d, %u bytes per line, phys %pad, type %d, state %d\n",
 		__func__, vou_dev->pix_idx, bytes_per_line,
-		videobuf_to_dma_contig(vb), vb->memory, vb->state);
+		({ dma_addr_t addr = videobuf_to_dma_contig(vb); &addr; }),
+		vb->memory, vb->state);
 
 	return 0;
 }
@@ -442,7 +443,7 @@ static void sh_vou_configure_geometry(struct sh_vou_device *vou_dev,
 				      int pix_idx, int w_idx, int h_idx)
 {
 	struct sh_vou_fmt *fmt = vou_fmt + pix_idx;
-	unsigned int black_left, black_top, width_max, height_max,
+	unsigned int black_left, black_top, width_max,
 		frame_in_height, frame_out_height, frame_out_top;
 	struct v4l2_rect *rect = &vou_dev->rect;
 	struct v4l2_pix_format *pix = &vou_dev->pix;
@@ -450,10 +451,10 @@ static void sh_vou_configure_geometry(struct sh_vou_device *vou_dev,
 
 	if (vou_dev->std & V4L2_STD_525_60) {
 		width_max = 858;
-		height_max = 262;
+		/* height_max = 262; */
 	} else {
 		width_max = 864;
-		height_max = 312;
+		/* height_max = 312; */
 	}
 
 	frame_in_height = pix->height / 2;
@@ -1052,7 +1053,6 @@ static irqreturn_t sh_vou_isr(int irq, void *dev_id)
 	static unsigned long j;
 	struct videobuf_buffer *vb;
 	static int cnt;
-	static int side;
 	u32 irq_status = sh_vou_reg_a_read(vou_dev, VOUIR), masked;
 	u32 vou_status = sh_vou_reg_a_read(vou_dev, VOUSTR);
 
@@ -1080,7 +1080,7 @@ static irqreturn_t sh_vou_isr(int irq, void *dev_id)
 		irq_status, masked, vou_status, cnt);
 
 	cnt++;
-	side = vou_status & 0x10000;
+	/* side = vou_status & 0x10000; */
 
 	/* Clear only set interrupts */
 	sh_vou_reg_a_write(vou_dev, VOUIR, masked);
diff --git a/drivers/media/platform/soc_camera/atmel-isi.c b/drivers/media/platform/soc_camera/atmel-isi.c
index 104485632501..4835173d7f80 100644
--- a/drivers/media/platform/soc_camera/atmel-isi.c
+++ b/drivers/media/platform/soc_camera/atmel-isi.c
@@ -34,13 +34,6 @@
 #define MIN_FRAME_RATE			15
 #define FRAME_INTERVAL_MILLI_SEC	(1000 / MIN_FRAME_RATE)
 
-/* ISI states */
-enum {
-	ISI_STATE_IDLE = 0,
-	ISI_STATE_READY,
-	ISI_STATE_WAIT_SOF,
-};
-
 /* Frame buffer descriptor */
 struct fbd {
 	/* Physical address of the frame buffer */
@@ -75,11 +68,6 @@ struct atmel_isi {
 	void __iomem			*regs;
 
 	int				sequence;
-	/* State of the ISI module in capturing mode */
-	int				state;
-
-	/* Wait queue for waiting for SOF */
-	wait_queue_head_t		vsync_wq;
 
 	struct vb2_alloc_ctx		*alloc_ctx;
 
@@ -124,16 +112,16 @@ static int configure_geometry(struct atmel_isi *isi, u32 width,
 	case V4L2_MBUS_FMT_Y8_1X8:
 		cr = ISI_CFG2_GRAYSCALE;
 		break;
-	case V4L2_MBUS_FMT_UYVY8_2X8:
+	case V4L2_MBUS_FMT_VYUY8_2X8:
 		cr = ISI_CFG2_YCC_SWAP_MODE_3;
 		break;
-	case V4L2_MBUS_FMT_VYUY8_2X8:
+	case V4L2_MBUS_FMT_UYVY8_2X8:
 		cr = ISI_CFG2_YCC_SWAP_MODE_2;
 		break;
-	case V4L2_MBUS_FMT_YUYV8_2X8:
+	case V4L2_MBUS_FMT_YVYU8_2X8:
 		cr = ISI_CFG2_YCC_SWAP_MODE_1;
 		break;
-	case V4L2_MBUS_FMT_YVYU8_2X8:
+	case V4L2_MBUS_FMT_YUYV8_2X8:
 		cr = ISI_CFG2_YCC_SWAP_DEFAULT;
 		break;
 	/* RGB, TODO */
@@ -144,6 +132,8 @@ static int configure_geometry(struct atmel_isi *isi, u32 width,
 	isi_writel(isi, ISI_CTRL, ISI_CTRL_DIS);
 
 	cfg2 = isi_readl(isi, ISI_CFG2);
+	/* Set YCC swap mode */
+	cfg2 &= ~ISI_CFG2_YCC_SWAP_MODE_MASK;
 	cfg2 |= cr;
 	/* Set width */
 	cfg2 &= ~(ISI_CFG2_IM_HSIZE_MASK);
@@ -207,12 +197,6 @@ static irqreturn_t isi_interrupt(int irq, void *dev_id)
 		isi_writel(isi, ISI_INTDIS, ISI_CTRL_DIS);
 		ret = IRQ_HANDLED;
 	} else {
-		if ((pending & ISI_SR_VSYNC) &&
-				(isi->state == ISI_STATE_IDLE)) {
-			isi->state = ISI_STATE_READY;
-			wake_up_interruptible(&isi->vsync_wq);
-			ret = IRQ_HANDLED;
-		}
 		if (likely(pending & ISI_SR_CXFR_DONE))
 			ret = atmel_isi_handle_streaming(isi);
 	}
@@ -259,16 +243,6 @@ static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 	struct atmel_isi *isi = ici->priv;
 	unsigned long size;
-	int ret;
-
-	/* Reset ISI */
-	ret = atmel_isi_wait_status(isi, WAIT_ISI_RESET);
-	if (ret < 0) {
-		dev_err(icd->parent, "Reset ISI timed out\n");
-		return ret;
-	}
-	/* Disable all interrupts */
-	isi_writel(isi, ISI_INTDIS, ~0UL);
 
 	size = icd->sizeimage;
 
@@ -374,6 +348,7 @@ static void start_dma(struct atmel_isi *isi, struct frame_buffer *buffer)
 	isi_writel(isi, ISI_DMA_C_CTRL, ISI_DMA_CTRL_FETCH | ISI_DMA_CTRL_DONE);
 	isi_writel(isi, ISI_DMA_CHER, ISI_DMA_CHSR_C_CH);
 
+	cfg1 &= ~ISI_CFG1_FRATE_DIV_MASK;
 	/* Enable linked list */
 	cfg1 |= isi->pdata->frate | ISI_CFG1_DISCR;
 
@@ -407,43 +382,27 @@ static int start_streaming(struct vb2_queue *vq, unsigned int count)
 	struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 	struct atmel_isi *isi = ici->priv;
-
 	u32 sr = 0;
 	int ret;
 
-	spin_lock_irq(&isi->lock);
-	isi->state = ISI_STATE_IDLE;
-	/* Clear any pending SOF interrupt */
-	sr = isi_readl(isi, ISI_STATUS);
-	/* Enable VSYNC interrupt for SOF */
-	isi_writel(isi, ISI_INTEN, ISI_SR_VSYNC);
-	isi_writel(isi, ISI_CTRL, ISI_CTRL_EN);
-	spin_unlock_irq(&isi->lock);
-
-	dev_dbg(icd->parent, "Waiting for SOF\n");
-	ret = wait_event_interruptible(isi->vsync_wq,
-				       isi->state != ISI_STATE_IDLE);
-	if (ret)
-		goto err;
-
-	if (isi->state != ISI_STATE_READY) {
-		ret = -EIO;
-		goto err;
+	/* Reset ISI */
+	ret = atmel_isi_wait_status(isi, WAIT_ISI_RESET);
+	if (ret < 0) {
+		dev_err(icd->parent, "Reset ISI timed out\n");
+		return ret;
 	}
+	/* Disable all interrupts */
+	isi_writel(isi, ISI_INTDIS, ~0UL);
 
 	spin_lock_irq(&isi->lock);
-	isi->state = ISI_STATE_WAIT_SOF;
-	isi_writel(isi, ISI_INTDIS, ISI_SR_VSYNC);
+	/* Clear any pending interrupt */
+	sr = isi_readl(isi, ISI_STATUS);
+
 	if (count)
 		start_dma(isi, isi->active);
 	spin_unlock_irq(&isi->lock);
 
 	return 0;
-err:
-	isi->active = NULL;
-	isi->sequence = 0;
-	INIT_LIST_HEAD(&isi->video_buffer_list);
-	return ret;
 }
 
 /* abort streaming and wait for last buffer */
@@ -765,14 +724,16 @@ static int isi_camera_clock_start(struct soc_camera_host *ici)
 	struct atmel_isi *isi = ici->priv;
 	int ret;
 
-	ret = clk_enable(isi->pclk);
+	ret = clk_prepare_enable(isi->pclk);
 	if (ret)
 		return ret;
 
-	ret = clk_enable(isi->mck);
-	if (ret) {
-		clk_disable(isi->pclk);
-		return ret;
+	if (!IS_ERR(isi->mck)) {
+		ret = clk_prepare_enable(isi->mck);
+		if (ret) {
+			clk_disable_unprepare(isi->pclk);
+			return ret;
+		}
 	}
 
 	return 0;
@@ -783,8 +744,9 @@ static void isi_camera_clock_stop(struct soc_camera_host *ici)
 {
 	struct atmel_isi *isi = ici->priv;
 
-	clk_disable(isi->mck);
-	clk_disable(isi->pclk);
+	if (!IS_ERR(isi->mck))
+		clk_disable_unprepare(isi->mck);
+	clk_disable_unprepare(isi->pclk);
 }
 
 static unsigned int isi_camera_poll(struct file *file, poll_table *pt)
@@ -906,7 +868,6 @@ static int atmel_isi_remove(struct platform_device *pdev)
 	struct atmel_isi *isi = container_of(soc_host,
 					struct atmel_isi, soc_host);
 
-	free_irq(isi->irq, isi);
 	soc_camera_host_unregister(soc_host);
 	vb2_dma_contig_cleanup_ctx(isi->alloc_ctx);
 	dma_free_coherent(&pdev->dev,
@@ -914,13 +875,6 @@ static int atmel_isi_remove(struct platform_device *pdev)
 			isi->p_fb_descriptors,
 			isi->fb_descriptors_phys);
 
-	iounmap(isi->regs);
-	clk_unprepare(isi->mck);
-	clk_put(isi->mck);
-	clk_unprepare(isi->pclk);
-	clk_put(isi->pclk);
-	kfree(isi);
-
 	return 0;
 }
 
@@ -928,7 +882,6 @@ static int atmel_isi_probe(struct platform_device *pdev)
 {
 	unsigned int irq;
 	struct atmel_isi *isi;
-	struct clk *pclk;
 	struct resource *regs;
 	int ret, i;
 	struct device *dev = &pdev->dev;
@@ -936,64 +889,50 @@ static int atmel_isi_probe(struct platform_device *pdev)
 	struct isi_platform_data *pdata;
 
 	pdata = dev->platform_data;
-	if (!pdata || !pdata->data_width_flags || !pdata->mck_hz) {
+	if (!pdata || !pdata->data_width_flags) {
 		dev_err(&pdev->dev,
 			"No config available for Atmel ISI\n");
 		return -EINVAL;
 	}
 
-	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!regs)
-		return -ENXIO;
-
-	pclk = clk_get(&pdev->dev, "isi_clk");
-	if (IS_ERR(pclk))
-		return PTR_ERR(pclk);
-
-	ret = clk_prepare(pclk);
-	if (ret)
-		goto err_clk_prepare_pclk;
-
-	isi = kzalloc(sizeof(struct atmel_isi), GFP_KERNEL);
+	isi = devm_kzalloc(&pdev->dev, sizeof(struct atmel_isi), GFP_KERNEL);
 	if (!isi) {
-		ret = -ENOMEM;
 		dev_err(&pdev->dev, "Can't allocate interface!\n");
-		goto err_alloc_isi;
+		return -ENOMEM;
 	}
 
-	isi->pclk = pclk;
+	isi->pclk = devm_clk_get(&pdev->dev, "isi_clk");
+	if (IS_ERR(isi->pclk))
+		return PTR_ERR(isi->pclk);
+
 	isi->pdata = pdata;
 	isi->active = NULL;
 	spin_lock_init(&isi->lock);
-	init_waitqueue_head(&isi->vsync_wq);
 	INIT_LIST_HEAD(&isi->video_buffer_list);
 	INIT_LIST_HEAD(&isi->dma_desc_head);
 
-	/* Get ISI_MCK, provided by programmable clock or external clock */
-	isi->mck = clk_get(dev, "isi_mck");
-	if (IS_ERR(isi->mck)) {
-		dev_err(dev, "Failed to get isi_mck\n");
-		ret = PTR_ERR(isi->mck);
-		goto err_clk_get;
+	/* ISI_MCK is the sensor master clock. It should be handled by the
+	 * sensor driver directly, as the ISI has no use for that clock. Make
+	 * the clock optional here while platforms transition to the correct
+	 * model.
+	 */
+	isi->mck = devm_clk_get(dev, "isi_mck");
+	if (!IS_ERR(isi->mck)) {
+		/* Set ISI_MCK's frequency, it should be faster than pixel
+		 * clock.
+		 */
+		ret = clk_set_rate(isi->mck, pdata->mck_hz);
+		if (ret < 0)
+			return ret;
 	}
 
-	ret = clk_prepare(isi->mck);
-	if (ret)
-		goto err_clk_prepare_mck;
-
-	/* Set ISI_MCK's frequency, it should be faster than pixel clock */
-	ret = clk_set_rate(isi->mck, pdata->mck_hz);
-	if (ret < 0)
-		goto err_set_mck_rate;
-
 	isi->p_fb_descriptors = dma_alloc_coherent(&pdev->dev,
 				sizeof(struct fbd) * MAX_BUFFER_NUM,
 				&isi->fb_descriptors_phys,
 				GFP_KERNEL);
 	if (!isi->p_fb_descriptors) {
-		ret = -ENOMEM;
 		dev_err(&pdev->dev, "Can't allocate descriptors!\n");
-		goto err_alloc_descriptors;
+		return -ENOMEM;
 	}
 
 	for (i = 0; i < MAX_BUFFER_NUM; i++) {
@@ -1009,9 +948,10 @@ static int atmel_isi_probe(struct platform_device *pdev)
 		goto err_alloc_ctx;
 	}
 
-	isi->regs = ioremap(regs->start, resource_size(regs));
-	if (!isi->regs) {
-		ret = -ENOMEM;
+	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	isi->regs = devm_ioremap_resource(&pdev->dev, regs);
+	if (IS_ERR(isi->regs)) {
+		ret = PTR_ERR(isi->regs);
 		goto err_ioremap;
 	}
 
@@ -1028,7 +968,7 @@ static int atmel_isi_probe(struct platform_device *pdev)
 		goto err_req_irq;
 	}
 
-	ret = request_irq(irq, isi_interrupt, 0, "isi", isi);
+	ret = devm_request_irq(&pdev->dev, irq, isi_interrupt, 0, "isi", isi);
 	if (ret) {
 		dev_err(&pdev->dev, "Unable to request irq %d\n", irq);
 		goto err_req_irq;
@@ -1050,9 +990,7 @@ static int atmel_isi_probe(struct platform_device *pdev)
 	return 0;
 
 err_register_soc_camera_host:
-	free_irq(isi->irq, isi);
 err_req_irq:
-	iounmap(isi->regs);
 err_ioremap:
 	vb2_dma_contig_cleanup_ctx(isi->alloc_ctx);
 err_alloc_ctx:
@@ -1060,17 +998,6 @@ err_alloc_ctx:
 			sizeof(struct fbd) * MAX_BUFFER_NUM,
 			isi->p_fb_descriptors,
 			isi->fb_descriptors_phys);
-err_alloc_descriptors:
-err_set_mck_rate:
-	clk_unprepare(isi->mck);
-err_clk_prepare_mck:
-	clk_put(isi->mck);
-err_clk_get:
-	kfree(isi);
-err_alloc_isi:
-	clk_unprepare(pclk);
-err_clk_prepare_pclk:
-	clk_put(pclk);
 
 	return ret;
 }
diff --git a/drivers/media/platform/soc_camera/mx2_camera.c b/drivers/media/platform/soc_camera/mx2_camera.c
index 45a0276be4e5..d73abca9c6ee 100644
--- a/drivers/media/platform/soc_camera/mx2_camera.c
+++ b/drivers/media/platform/soc_camera/mx2_camera.c
@@ -659,7 +659,7 @@ static int mx2_start_streaming(struct vb2_queue *q, unsigned int count)
 	unsigned long flags;
 
 	if (count < 2)
-		return -EINVAL;
+		return -ENOBUFS;
 
 	spin_lock_irqsave(&pcdev->lock, flags);
 
diff --git a/drivers/media/platform/soc_camera/rcar_vin.c b/drivers/media/platform/soc_camera/rcar_vin.c
index 6866bb4fbebc..3b1c05a72d00 100644
--- a/drivers/media/platform/soc_camera/rcar_vin.c
+++ b/drivers/media/platform/soc_camera/rcar_vin.c
@@ -106,7 +106,7 @@
 #define VIN_MAX_HEIGHT		2048
 
 enum chip_id {
-	RCAR_H2,
+	RCAR_GEN2,
 	RCAR_H1,
 	RCAR_M1,
 	RCAR_E1,
@@ -302,7 +302,7 @@ static int rcar_vin_setup(struct rcar_vin_priv *priv)
 		dmr = 0;
 		break;
 	case V4L2_PIX_FMT_RGB32:
-		if (priv->chip == RCAR_H2 || priv->chip == RCAR_H1 ||
+		if (priv->chip == RCAR_GEN2 || priv->chip == RCAR_H1 ||
 		    priv->chip == RCAR_E1) {
 			dmr = VNDMR_EXRGB;
 			break;
@@ -1384,7 +1384,8 @@ static struct soc_camera_host_ops rcar_vin_host_ops = {
 };
 
 static struct platform_device_id rcar_vin_id_table[] = {
-	{ "r8a7790-vin",  RCAR_H2 },
+	{ "r8a7791-vin",  RCAR_GEN2 },
+	{ "r8a7790-vin",  RCAR_GEN2 },
 	{ "r8a7779-vin",  RCAR_H1 },
 	{ "r8a7778-vin",  RCAR_M1 },
 	{ "uPD35004-vin", RCAR_E1 },
diff --git a/drivers/media/platform/soc_camera/soc_scale_crop.c b/drivers/media/platform/soc_camera/soc_scale_crop.c
index cbd3a34f4f3f..8e74fb7f2a07 100644
--- a/drivers/media/platform/soc_camera/soc_scale_crop.c
+++ b/drivers/media/platform/soc_camera/soc_scale_crop.c
@@ -141,8 +141,8 @@ int soc_camera_client_s_crop(struct v4l2_subdev *sd,
 	 * Popular special case - some cameras can only handle fixed sizes like
 	 * QVGA, VGA,... Take care to avoid infinite loop.
 	 */
-	width = max(cam_rect->width, 2);
-	height = max(cam_rect->height, 2);
+	width = max_t(unsigned int, cam_rect->width, 2);
+	height = max_t(unsigned int, cam_rect->height, 2);
 
 	/*
 	 * Loop as long as sensor is not covering the requested rectangle and
diff --git a/drivers/media/platform/ti-vpe/Makefile b/drivers/media/platform/ti-vpe/Makefile
index cbf0a806ba1d..be680f839e77 100644
--- a/drivers/media/platform/ti-vpe/Makefile
+++ b/drivers/media/platform/ti-vpe/Makefile
@@ -1,5 +1,5 @@
 obj-$(CONFIG_VIDEO_TI_VPE) += ti-vpe.o
 
-ti-vpe-y := vpe.o vpdma.o
+ti-vpe-y := vpe.o sc.o csc.o vpdma.o
 
 ccflags-$(CONFIG_VIDEO_TI_VPE_DEBUG) += -DDEBUG
diff --git a/drivers/media/platform/ti-vpe/csc.c b/drivers/media/platform/ti-vpe/csc.c
new file mode 100644
index 000000000000..acfea500710e
--- /dev/null
+++ b/drivers/media/platform/ti-vpe/csc.c
@@ -0,0 +1,196 @@
+/*
+ * Color space converter library
+ *
+ * Copyright (c) 2013 Texas Instruments Inc.
+ *
+ * David Griego, <dagriego@biglakesoftware.com>
+ * Dale Farnsworth, <dale@farnsworth.org>
+ * Archit Taneja, <archit@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/err.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+
+#include "csc.h"
+
+/*
+ * 16 coefficients in the order:
+ * a0, b0, c0, a1, b1, c1, a2, b2, c2, d0, d1, d2
+ * (we may need to pass non-default values from user space later on, we might
+ * need to make the coefficient struct more easy to populate)
+ */
+struct colorspace_coeffs {
+	u16	sd[12];
+	u16	hd[12];
+};
+
+/* VIDEO_RANGE: limited range, GRAPHICS_RANGE: full range */
+#define	CSC_COEFFS_VIDEO_RANGE_Y2R	0
+#define	CSC_COEFFS_GRAPHICS_RANGE_Y2R	1
+#define	CSC_COEFFS_VIDEO_RANGE_R2Y	2
+#define	CSC_COEFFS_GRAPHICS_RANGE_R2Y	3
+
+/* default colorspace coefficients */
+static struct colorspace_coeffs colorspace_coeffs[4] = {
+	[CSC_COEFFS_VIDEO_RANGE_Y2R] = {
+		{
+			/* SDTV */
+			0x0400, 0x0000, 0x057D, 0x0400, 0x1EA7, 0x1D35,
+			0x0400, 0x06EF, 0x1FFE, 0x0D40, 0x0210, 0x0C88,
+		},
+		{
+			/* HDTV */
+			0x0400, 0x0000, 0x0629, 0x0400, 0x1F45, 0x1E2B,
+			0x0400, 0x0742, 0x0000, 0x0CEC, 0x0148, 0x0C60,
+		},
+	},
+	[CSC_COEFFS_GRAPHICS_RANGE_Y2R] = {
+		{
+			/* SDTV */
+			0x04A8, 0x1FFE, 0x0662, 0x04A8, 0x1E6F, 0x1CBF,
+			0x04A8, 0x0812, 0x1FFF, 0x0C84, 0x0220, 0x0BAC,
+		},
+		{
+			/* HDTV */
+			0x04A8, 0x0000, 0x072C, 0x04A8, 0x1F26, 0x1DDE,
+			0x04A8, 0x0873, 0x0000, 0x0C20, 0x0134, 0x0B7C,
+		},
+	},
+	[CSC_COEFFS_VIDEO_RANGE_R2Y] = {
+		{
+			/* SDTV */
+			0x0132, 0x0259, 0x0075, 0x1F50, 0x1EA5, 0x020B,
+			0x020B, 0x1E4A, 0x1FAB, 0x0000, 0x0200, 0x0200,
+		},
+		{
+			/* HDTV */
+			0x00DA, 0x02DC, 0x004A, 0x1F88, 0x1E6C, 0x020C,
+			0x020C, 0x1E24, 0x1FD0, 0x0000, 0x0200, 0x0200,
+		},
+	},
+	[CSC_COEFFS_GRAPHICS_RANGE_R2Y] = {
+		{
+			/* SDTV */
+			0x0107, 0x0204, 0x0064, 0x1F68, 0x1ED6, 0x01C2,
+			0x01C2, 0x1E87, 0x1FB7, 0x0040, 0x0200, 0x0200,
+		},
+		{
+			/* HDTV */
+			0x04A8, 0x0000, 0x072C, 0x04A8, 0x1F26, 0x1DDE,
+			0x04A8, 0x0873, 0x0000, 0x0C20, 0x0134, 0x0B7C,
+		},
+	},
+};
+
+void csc_dump_regs(struct csc_data *csc)
+{
+	struct device *dev = &csc->pdev->dev;
+
+	u32 read_reg(struct csc_data *csc, int offset)
+	{
+		return ioread32(csc->base + offset);
+	}
+
+#define DUMPREG(r) dev_dbg(dev, "%-35s %08x\n", #r, read_reg(csc, CSC_##r))
+
+	DUMPREG(CSC00);
+	DUMPREG(CSC01);
+	DUMPREG(CSC02);
+	DUMPREG(CSC03);
+	DUMPREG(CSC04);
+	DUMPREG(CSC05);
+
+#undef DUMPREG
+}
+
+void csc_set_coeff_bypass(struct csc_data *csc, u32 *csc_reg5)
+{
+	*csc_reg5 |= CSC_BYPASS;
+}
+
+/*
+ * set the color space converter coefficient shadow register values
+ */
+void csc_set_coeff(struct csc_data *csc, u32 *csc_reg0,
+		enum v4l2_colorspace src_colorspace,
+		enum v4l2_colorspace dst_colorspace)
+{
+	u32 *csc_reg5 = csc_reg0 + 5;
+	u32 *shadow_csc = csc_reg0;
+	struct colorspace_coeffs *sd_hd_coeffs;
+	u16 *coeff, *end_coeff;
+	enum v4l2_colorspace yuv_colorspace;
+	int sel = 0;
+
+	/*
+	 * support only graphics data range(full range) for now, a control ioctl
+	 * would be nice here
+	 */
+	/* Y2R */
+	if (dst_colorspace == V4L2_COLORSPACE_SRGB &&
+			(src_colorspace == V4L2_COLORSPACE_SMPTE170M ||
+			src_colorspace == V4L2_COLORSPACE_REC709)) {
+		/* Y2R */
+		sel = 1;
+		yuv_colorspace = src_colorspace;
+	} else if ((dst_colorspace == V4L2_COLORSPACE_SMPTE170M ||
+			dst_colorspace == V4L2_COLORSPACE_REC709) &&
+			src_colorspace == V4L2_COLORSPACE_SRGB) {
+		/* R2Y */
+		sel = 3;
+		yuv_colorspace = dst_colorspace;
+	} else {
+		*csc_reg5 |= CSC_BYPASS;
+		return;
+	}
+
+	sd_hd_coeffs = &colorspace_coeffs[sel];
+
+	/* select between SD or HD coefficients */
+	if (yuv_colorspace == V4L2_COLORSPACE_SMPTE170M)
+		coeff = sd_hd_coeffs->sd;
+	else
+		coeff = sd_hd_coeffs->hd;
+
+	end_coeff = coeff + 12;
+
+	for (; coeff < end_coeff; coeff += 2)
+		*shadow_csc++ = (*(coeff + 1) << 16) | *coeff;
+}
+
+struct csc_data *csc_create(struct platform_device *pdev)
+{
+	struct csc_data *csc;
+
+	dev_dbg(&pdev->dev, "csc_create\n");
+
+	csc = devm_kzalloc(&pdev->dev, sizeof(*csc), GFP_KERNEL);
+	if (!csc) {
+		dev_err(&pdev->dev, "couldn't alloc csc_data\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	csc->pdev = pdev;
+
+	csc->res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+			"vpe_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) {
+		dev_err(&pdev->dev, "failed to ioremap\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	return csc;
+}
diff --git a/drivers/media/platform/ti-vpe/csc.h b/drivers/media/platform/ti-vpe/csc.h
new file mode 100644
index 000000000000..1ad2b6dad561
--- /dev/null
+++ b/drivers/media/platform/ti-vpe/csc.h
@@ -0,0 +1,68 @@
+/*
+ * Copyright (c) 2013 Texas Instruments Inc.
+ *
+ * David Griego, <dagriego@biglakesoftware.com>
+ * Dale Farnsworth, <dale@farnsworth.org>
+ * Archit Taneja, <archit@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.
+ */
+#ifndef TI_CSC_H
+#define TI_CSC_H
+
+/* VPE color space converter regs */
+#define CSC_CSC00		0x00
+#define CSC_A0_MASK		0x1fff
+#define CSC_A0_SHIFT		0
+#define CSC_B0_MASK		0x1fff
+#define CSC_B0_SHIFT		16
+
+#define CSC_CSC01		0x04
+#define CSC_C0_MASK		0x1fff
+#define CSC_C0_SHIFT		0
+#define CSC_A1_MASK		0x1fff
+#define CSC_A1_SHIFT		16
+
+#define CSC_CSC02		0x08
+#define CSC_B1_MASK		0x1fff
+#define CSC_B1_SHIFT		0
+#define CSC_C1_MASK		0x1fff
+#define CSC_C1_SHIFT		16
+
+#define CSC_CSC03		0x0c
+#define CSC_A2_MASK		0x1fff
+#define CSC_A2_SHIFT		0
+#define CSC_B2_MASK		0x1fff
+#define CSC_B2_SHIFT		16
+
+#define CSC_CSC04		0x10
+#define CSC_C2_MASK		0x1fff
+#define CSC_C2_SHIFT		0
+#define CSC_D0_MASK		0x0fff
+#define CSC_D0_SHIFT		16
+
+#define CSC_CSC05		0x14
+#define CSC_D1_MASK		0x0fff
+#define CSC_D1_SHIFT		0
+#define CSC_D2_MASK		0x0fff
+#define CSC_D2_SHIFT		16
+
+#define CSC_BYPASS		(1 << 28)
+
+struct csc_data {
+	void __iomem		*base;
+	struct resource		*res;
+
+	struct platform_device	*pdev;
+};
+
+void csc_dump_regs(struct csc_data *csc);
+void csc_set_coeff_bypass(struct csc_data *csc, u32 *csc_reg5);
+void csc_set_coeff(struct csc_data *csc, u32 *csc_reg0,
+		enum v4l2_colorspace src_colorspace,
+		enum v4l2_colorspace dst_colorspace);
+struct csc_data *csc_create(struct platform_device *pdev);
+
+#endif
diff --git a/drivers/media/platform/ti-vpe/sc.c b/drivers/media/platform/ti-vpe/sc.c
new file mode 100644
index 000000000000..93f0af546b76
--- /dev/null
+++ b/drivers/media/platform/ti-vpe/sc.c
@@ -0,0 +1,311 @@
+/*
+ * Scaler library
+ *
+ * Copyright (c) 2013 Texas Instruments Inc.
+ *
+ * David Griego, <dagriego@biglakesoftware.com>
+ * Dale Farnsworth, <dale@farnsworth.org>
+ * Archit Taneja, <archit@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/err.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include "sc.h"
+#include "sc_coeff.h"
+
+void sc_dump_regs(struct sc_data *sc)
+{
+	struct device *dev = &sc->pdev->dev;
+
+	u32 read_reg(struct sc_data *sc, int offset)
+	{
+		return ioread32(sc->base + offset);
+	}
+
+#define DUMPREG(r) dev_dbg(dev, "%-35s %08x\n", #r, read_reg(sc, CFG_##r))
+
+	DUMPREG(SC0);
+	DUMPREG(SC1);
+	DUMPREG(SC2);
+	DUMPREG(SC3);
+	DUMPREG(SC4);
+	DUMPREG(SC5);
+	DUMPREG(SC6);
+	DUMPREG(SC8);
+	DUMPREG(SC9);
+	DUMPREG(SC10);
+	DUMPREG(SC11);
+	DUMPREG(SC12);
+	DUMPREG(SC13);
+	DUMPREG(SC17);
+	DUMPREG(SC18);
+	DUMPREG(SC19);
+	DUMPREG(SC20);
+	DUMPREG(SC21);
+	DUMPREG(SC22);
+	DUMPREG(SC23);
+	DUMPREG(SC24);
+	DUMPREG(SC25);
+
+#undef DUMPREG
+}
+
+/*
+ * set the horizontal scaler coefficients according to the ratio of output to
+ * input widths, after accounting for up to two levels of decimation
+ */
+void sc_set_hs_coeffs(struct sc_data *sc, void *addr, unsigned int src_w,
+		unsigned int dst_w)
+{
+	int sixteenths;
+	int idx;
+	int i, j;
+	u16 *coeff_h = addr;
+	const u16 *cp;
+
+	if (dst_w > src_w) {
+		idx = HS_UP_SCALE;
+	} else {
+		if ((dst_w << 1) < src_w)
+			dst_w <<= 1;	/* first level decimation */
+		if ((dst_w << 1) < src_w)
+			dst_w <<= 1;	/* second level decimation */
+
+		if (dst_w == src_w) {
+			idx = HS_LE_16_16_SCALE;
+		} else {
+			sixteenths = (dst_w << 4) / src_w;
+			if (sixteenths < 8)
+				sixteenths = 8;
+			idx = HS_LT_9_16_SCALE + sixteenths - 8;
+		}
+	}
+
+	if (idx == sc->hs_index)
+		return;
+
+	cp = scaler_hs_coeffs[idx];
+
+	for (i = 0; i < SC_NUM_PHASES * 2; i++) {
+		for (j = 0; j < SC_H_NUM_TAPS; j++)
+			*coeff_h++ = *cp++;
+		/*
+		 * for each phase, the scaler expects space for 8 coefficients
+		 * in it's memory. For the horizontal scaler, we copy the first
+		 * 7 coefficients and skip the last slot to move to the next
+		 * row to hold coefficients for the next phase
+		 */
+		coeff_h += SC_NUM_TAPS_MEM_ALIGN - SC_H_NUM_TAPS;
+	}
+
+	sc->hs_index = idx;
+
+	sc->load_coeff_h = true;
+}
+
+/*
+ * set the vertical scaler coefficients according to the ratio of output to
+ * input heights
+ */
+void sc_set_vs_coeffs(struct sc_data *sc, void *addr, unsigned int src_h,
+		unsigned int dst_h)
+{
+	int sixteenths;
+	int idx;
+	int i, j;
+	u16 *coeff_v = addr;
+	const u16 *cp;
+
+	if (dst_h > src_h) {
+		idx = VS_UP_SCALE;
+	} else if (dst_h == src_h) {
+		idx = VS_1_TO_1_SCALE;
+	} else {
+		sixteenths = (dst_h << 4) / src_h;
+		if (sixteenths < 8)
+			sixteenths = 8;
+		idx = VS_LT_9_16_SCALE + sixteenths - 8;
+	}
+
+	if (idx == sc->vs_index)
+		return;
+
+	cp = scaler_vs_coeffs[idx];
+
+	for (i = 0; i < SC_NUM_PHASES * 2; i++) {
+		for (j = 0; j < SC_V_NUM_TAPS; j++)
+			*coeff_v++ = *cp++;
+		/*
+		 * for the vertical scaler, we copy the first 5 coefficients and
+		 * skip the last 3 slots to move to the next row to hold
+		 * coefficients for the next phase
+		 */
+		coeff_v += SC_NUM_TAPS_MEM_ALIGN - SC_V_NUM_TAPS;
+	}
+
+	sc->vs_index = idx;
+	sc->load_coeff_v = true;
+}
+
+void sc_config_scaler(struct sc_data *sc, u32 *sc_reg0, u32 *sc_reg8,
+		u32 *sc_reg17, unsigned int src_w, unsigned int src_h,
+		unsigned int dst_w, unsigned int dst_h)
+{
+	struct device *dev = &sc->pdev->dev;
+	u32 val;
+	int dcm_x, dcm_shift;
+	bool use_rav;
+	unsigned long lltmp;
+	u32 lin_acc_inc, lin_acc_inc_u;
+	u32 col_acc_offset;
+	u16 factor = 0;
+	int row_acc_init_rav = 0, row_acc_init_rav_b = 0;
+	u32 row_acc_inc = 0, row_acc_offset = 0, row_acc_offset_b = 0;
+	/*
+	 * location of SC register in payload memory with respect to the first
+	 * register in the mmr address data block
+	 */
+	u32 *sc_reg9 = sc_reg8 + 1;
+	u32 *sc_reg12 = sc_reg8 + 4;
+	u32 *sc_reg13 = sc_reg8 + 5;
+	u32 *sc_reg24 = sc_reg17 + 7;
+
+	val = sc_reg0[0];
+
+	/* clear all the features(they may get enabled elsewhere later) */
+	val &= ~(CFG_SELFGEN_FID | CFG_TRIM | CFG_ENABLE_SIN2_VER_INTP |
+		CFG_INTERLACE_I | CFG_DCM_4X | CFG_DCM_2X | CFG_AUTO_HS |
+		CFG_ENABLE_EV | CFG_USE_RAV | CFG_INVT_FID | CFG_SC_BYPASS |
+		CFG_INTERLACE_O | CFG_Y_PK_EN | CFG_HP_BYPASS | CFG_LINEAR);
+
+	if (src_w == dst_w && src_h == dst_h) {
+		val |= CFG_SC_BYPASS;
+		sc_reg0[0] = val;
+		return;
+	}
+
+	/* we only support linear scaling for now */
+	val |= CFG_LINEAR;
+
+	/* configure horizontal scaler */
+
+	/* enable 2X or 4X decimation */
+	dcm_x = src_w / dst_w;
+	if (dcm_x > 4) {
+		val |= CFG_DCM_4X;
+		dcm_shift = 2;
+	} else if (dcm_x > 2) {
+		val |= CFG_DCM_2X;
+		dcm_shift = 1;
+	} else {
+		dcm_shift = 0;
+	}
+
+	lltmp = dst_w - 1;
+	lin_acc_inc = div64_u64(((u64)(src_w >> dcm_shift) - 1) << 24, lltmp);
+	lin_acc_inc_u = 0;
+	col_acc_offset = 0;
+
+	dev_dbg(dev, "hs config: src_w = %d, dst_w = %d, decimation = %s, lin_acc_inc = %08x\n",
+		src_w, dst_w, dcm_shift == 2 ? "4x" :
+		(dcm_shift == 1 ? "2x" : "none"), lin_acc_inc);
+
+	/* configure vertical scaler */
+
+	/* use RAV for vertical scaler if vertical downscaling is > 4x */
+	if (dst_h < (src_h >> 2)) {
+		use_rav = true;
+		val |= CFG_USE_RAV;
+	} else {
+		use_rav = false;
+	}
+
+	if (use_rav) {
+		/* use RAV */
+		factor = (u16) ((dst_h << 10) / src_h);
+
+		row_acc_init_rav = factor + ((1 + factor) >> 1);
+		if (row_acc_init_rav >= 1024)
+			row_acc_init_rav -= 1024;
+
+		row_acc_init_rav_b = row_acc_init_rav +
+				(1 + (row_acc_init_rav >> 1)) -
+				(1024 >> 1);
+
+		if (row_acc_init_rav_b < 0) {
+			row_acc_init_rav_b += row_acc_init_rav;
+			row_acc_init_rav *= 2;
+		}
+
+		dev_dbg(dev, "vs config(RAV): src_h = %d, dst_h = %d, factor = %d, acc_init = %08x, acc_init_b = %08x\n",
+			src_h, dst_h, factor, row_acc_init_rav,
+			row_acc_init_rav_b);
+	} else {
+		/* use polyphase */
+		row_acc_inc = ((src_h - 1) << 16) / (dst_h - 1);
+		row_acc_offset = 0;
+		row_acc_offset_b = 0;
+
+		dev_dbg(dev, "vs config(POLY): src_h = %d, dst_h = %d,row_acc_inc = %08x\n",
+			src_h, dst_h, row_acc_inc);
+	}
+
+
+	sc_reg0[0] = val;
+	sc_reg0[1] = row_acc_inc;
+	sc_reg0[2] = row_acc_offset;
+	sc_reg0[3] = row_acc_offset_b;
+
+	sc_reg0[4] = ((lin_acc_inc_u & CFG_LIN_ACC_INC_U_MASK) <<
+			CFG_LIN_ACC_INC_U_SHIFT) | (dst_w << CFG_TAR_W_SHIFT) |
+			(dst_h << CFG_TAR_H_SHIFT);
+
+	sc_reg0[5] = (src_w << CFG_SRC_W_SHIFT) | (src_h << CFG_SRC_H_SHIFT);
+
+	sc_reg0[6] = (row_acc_init_rav_b << CFG_ROW_ACC_INIT_RAV_B_SHIFT) |
+		(row_acc_init_rav << CFG_ROW_ACC_INIT_RAV_SHIFT);
+
+	*sc_reg9 = lin_acc_inc;
+
+	*sc_reg12 = col_acc_offset << CFG_COL_ACC_OFFSET_SHIFT;
+
+	*sc_reg13 = factor;
+
+	*sc_reg24 = (src_w << CFG_ORG_W_SHIFT) | (src_h << CFG_ORG_H_SHIFT);
+}
+
+struct sc_data *sc_create(struct platform_device *pdev)
+{
+	struct sc_data *sc;
+
+	dev_dbg(&pdev->dev, "sc_create\n");
+
+	sc = devm_kzalloc(&pdev->dev, sizeof(*sc), GFP_KERNEL);
+	if (!sc) {
+		dev_err(&pdev->dev, "couldn't alloc sc_data\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	sc->pdev = pdev;
+
+	sc->res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "sc");
+	if (!sc->res) {
+		dev_err(&pdev->dev, "missing platform resources data\n");
+		return ERR_PTR(-ENODEV);
+	}
+
+	sc->base = devm_ioremap_resource(&pdev->dev, sc->res);
+	if (!sc->base) {
+		dev_err(&pdev->dev, "failed to ioremap\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	return sc;
+}
diff --git a/drivers/media/platform/ti-vpe/sc.h b/drivers/media/platform/ti-vpe/sc.h
new file mode 100644
index 000000000000..60e411e05c30
--- /dev/null
+++ b/drivers/media/platform/ti-vpe/sc.h
@@ -0,0 +1,208 @@
+/*
+ * Copyright (c) 2013 Texas Instruments Inc.
+ *
+ * David Griego, <dagriego@biglakesoftware.com>
+ * Dale Farnsworth, <dale@farnsworth.org>
+ * Archit Taneja, <archit@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.
+ */
+#ifndef TI_SC_H
+#define TI_SC_H
+
+/* Scaler regs */
+#define CFG_SC0				0x0
+#define CFG_INTERLACE_O			(1 << 0)
+#define CFG_LINEAR			(1 << 1)
+#define CFG_SC_BYPASS			(1 << 2)
+#define CFG_INVT_FID			(1 << 3)
+#define CFG_USE_RAV			(1 << 4)
+#define CFG_ENABLE_EV			(1 << 5)
+#define CFG_AUTO_HS			(1 << 6)
+#define CFG_DCM_2X			(1 << 7)
+#define CFG_DCM_4X			(1 << 8)
+#define CFG_HP_BYPASS			(1 << 9)
+#define CFG_INTERLACE_I			(1 << 10)
+#define CFG_ENABLE_SIN2_VER_INTP	(1 << 11)
+#define CFG_Y_PK_EN			(1 << 14)
+#define CFG_TRIM			(1 << 15)
+#define CFG_SELFGEN_FID			(1 << 16)
+
+#define CFG_SC1				0x4
+#define CFG_ROW_ACC_INC_MASK		0x07ffffff
+#define CFG_ROW_ACC_INC_SHIFT		0
+
+#define CFG_SC2				0x08
+#define CFG_ROW_ACC_OFFSET_MASK		0x0fffffff
+#define CFG_ROW_ACC_OFFSET_SHIFT	0
+
+#define CFG_SC3				0x0c
+#define CFG_ROW_ACC_OFFSET_B_MASK	0x0fffffff
+#define CFG_ROW_ACC_OFFSET_B_SHIFT	0
+
+#define CFG_SC4				0x10
+#define CFG_TAR_H_MASK			0x07ff
+#define CFG_TAR_H_SHIFT			0
+#define CFG_TAR_W_MASK			0x07ff
+#define CFG_TAR_W_SHIFT			12
+#define CFG_LIN_ACC_INC_U_MASK		0x07
+#define CFG_LIN_ACC_INC_U_SHIFT		24
+#define CFG_NLIN_ACC_INIT_U_MASK	0x07
+#define CFG_NLIN_ACC_INIT_U_SHIFT	28
+
+#define CFG_SC5				0x14
+#define CFG_SRC_H_MASK			0x07ff
+#define CFG_SRC_H_SHIFT			0
+#define CFG_SRC_W_MASK			0x07ff
+#define CFG_SRC_W_SHIFT			12
+#define CFG_NLIN_ACC_INC_U_MASK		0x07
+#define CFG_NLIN_ACC_INC_U_SHIFT	24
+
+#define CFG_SC6				0x18
+#define CFG_ROW_ACC_INIT_RAV_MASK	0x03ff
+#define CFG_ROW_ACC_INIT_RAV_SHIFT	0
+#define CFG_ROW_ACC_INIT_RAV_B_MASK	0x03ff
+#define CFG_ROW_ACC_INIT_RAV_B_SHIFT	10
+
+#define CFG_SC8				0x20
+#define CFG_NLIN_LEFT_MASK		0x07ff
+#define CFG_NLIN_LEFT_SHIFT		0
+#define CFG_NLIN_RIGHT_MASK		0x07ff
+#define CFG_NLIN_RIGHT_SHIFT		12
+
+#define CFG_SC9				0x24
+#define CFG_LIN_ACC_INC			CFG_SC9
+
+#define CFG_SC10			0x28
+#define CFG_NLIN_ACC_INIT		CFG_SC10
+
+#define CFG_SC11			0x2c
+#define CFG_NLIN_ACC_INC		CFG_SC11
+
+#define CFG_SC12			0x30
+#define CFG_COL_ACC_OFFSET_MASK		0x01ffffff
+#define CFG_COL_ACC_OFFSET_SHIFT	0
+
+#define CFG_SC13			0x34
+#define CFG_SC_FACTOR_RAV_MASK		0xff
+#define CFG_SC_FACTOR_RAV_SHIFT		0
+#define CFG_CHROMA_INTP_THR_MASK	0x03ff
+#define CFG_CHROMA_INTP_THR_SHIFT	12
+#define CFG_DELTA_CHROMA_THR_MASK	0x0f
+#define CFG_DELTA_CHROMA_THR_SHIFT	24
+
+#define CFG_SC17			0x44
+#define CFG_EV_THR_MASK			0x03ff
+#define CFG_EV_THR_SHIFT		12
+#define CFG_DELTA_LUMA_THR_MASK		0x0f
+#define CFG_DELTA_LUMA_THR_SHIFT	24
+#define CFG_DELTA_EV_THR_MASK		0x0f
+#define CFG_DELTA_EV_THR_SHIFT		28
+
+#define CFG_SC18			0x48
+#define CFG_HS_FACTOR_MASK		0x03ff
+#define CFG_HS_FACTOR_SHIFT		0
+#define CFG_CONF_DEFAULT_MASK		0x01ff
+#define CFG_CONF_DEFAULT_SHIFT		16
+
+#define CFG_SC19			0x4c
+#define CFG_HPF_COEFF0_MASK		0xff
+#define CFG_HPF_COEFF0_SHIFT		0
+#define CFG_HPF_COEFF1_MASK		0xff
+#define CFG_HPF_COEFF1_SHIFT		8
+#define CFG_HPF_COEFF2_MASK		0xff
+#define CFG_HPF_COEFF2_SHIFT		16
+#define CFG_HPF_COEFF3_MASK		0xff
+#define CFG_HPF_COEFF3_SHIFT		23
+
+#define CFG_SC20			0x50
+#define CFG_HPF_COEFF4_MASK		0xff
+#define CFG_HPF_COEFF4_SHIFT		0
+#define CFG_HPF_COEFF5_MASK		0xff
+#define CFG_HPF_COEFF5_SHIFT		8
+#define CFG_HPF_NORM_SHIFT_MASK		0x07
+#define CFG_HPF_NORM_SHIFT_SHIFT	16
+#define CFG_NL_LIMIT_MASK		0x1ff
+#define CFG_NL_LIMIT_SHIFT		20
+
+#define CFG_SC21			0x54
+#define CFG_NL_LO_THR_MASK		0x01ff
+#define CFG_NL_LO_THR_SHIFT		0
+#define CFG_NL_LO_SLOPE_MASK		0xff
+#define CFG_NL_LO_SLOPE_SHIFT		16
+
+#define CFG_SC22			0x58
+#define CFG_NL_HI_THR_MASK		0x01ff
+#define CFG_NL_HI_THR_SHIFT		0
+#define CFG_NL_HI_SLOPE_SH_MASK		0x07
+#define CFG_NL_HI_SLOPE_SH_SHIFT	16
+
+#define CFG_SC23			0x5c
+#define CFG_GRADIENT_THR_MASK		0x07ff
+#define CFG_GRADIENT_THR_SHIFT		0
+#define CFG_GRADIENT_THR_RANGE_MASK	0x0f
+#define CFG_GRADIENT_THR_RANGE_SHIFT	12
+#define CFG_MIN_GY_THR_MASK		0xff
+#define CFG_MIN_GY_THR_SHIFT		16
+#define CFG_MIN_GY_THR_RANGE_MASK	0x0f
+#define CFG_MIN_GY_THR_RANGE_SHIFT	28
+
+#define CFG_SC24			0x60
+#define CFG_ORG_H_MASK			0x07ff
+#define CFG_ORG_H_SHIFT			0
+#define CFG_ORG_W_MASK			0x07ff
+#define CFG_ORG_W_SHIFT			16
+
+#define CFG_SC25			0x64
+#define CFG_OFF_H_MASK			0x07ff
+#define CFG_OFF_H_SHIFT			0
+#define CFG_OFF_W_MASK			0x07ff
+#define CFG_OFF_W_SHIFT			16
+
+/* number of phases supported by the polyphase scalers */
+#define SC_NUM_PHASES			32
+
+/* number of taps used by horizontal polyphase scaler */
+#define SC_H_NUM_TAPS			7
+
+/* number of taps used by vertical polyphase scaler */
+#define SC_V_NUM_TAPS			5
+
+/* number of taps expected by the scaler in it's coefficient memory */
+#define SC_NUM_TAPS_MEM_ALIGN		8
+
+/*
+ * coefficient memory size in bytes:
+ * num phases x num sets(luma and chroma) x num taps(aligned) x coeff size
+ */
+#define SC_COEF_SRAM_SIZE	(SC_NUM_PHASES * 2 * SC_NUM_TAPS_MEM_ALIGN * 2)
+
+struct sc_data {
+	void __iomem		*base;
+	struct resource		*res;
+
+	dma_addr_t		loaded_coeff_h; /* loaded h coeffs in SC */
+	dma_addr_t		loaded_coeff_v; /* loaded v coeffs in SC */
+
+	bool			load_coeff_h;	/* have new h SC coeffs */
+	bool			load_coeff_v;	/* have new v SC coeffs */
+
+	unsigned int		hs_index;	/* h SC coeffs selector */
+	unsigned int		vs_index;	/* v SC coeffs selector */
+
+	struct platform_device *pdev;
+};
+
+void sc_dump_regs(struct sc_data *sc);
+void sc_set_hs_coeffs(struct sc_data *sc, void *addr, unsigned int src_w,
+		unsigned int dst_w);
+void sc_set_vs_coeffs(struct sc_data *sc, void *addr, unsigned int src_h,
+		unsigned int dst_h);
+void sc_config_scaler(struct sc_data *sc, u32 *sc_reg0, u32 *sc_reg8,
+		u32 *sc_reg17, unsigned int src_w, unsigned int src_h,
+		unsigned int dst_w, unsigned int dst_h);
+struct sc_data *sc_create(struct platform_device *pdev);
+
+#endif
diff --git a/drivers/media/platform/ti-vpe/sc_coeff.h b/drivers/media/platform/ti-vpe/sc_coeff.h
new file mode 100644
index 000000000000..5bfa5c03aec6
--- /dev/null
+++ b/drivers/media/platform/ti-vpe/sc_coeff.h
@@ -0,0 +1,1342 @@
+/*
+ * VPE SC coefs
+ *
+ * Copyright (c) 2013 Texas Instruments Inc.
+ *
+ * David Griego, <dagriego@biglakesoftware.com>
+ * Dale Farnsworth, <dale@farnsworth.org>
+ * Archit Taneja, <archit@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.
+ */
+
+#ifndef __TI_SC_COEFF_H
+#define __TI_SC_COEFF_H
+
+/* horizontal scaler coefficients */
+enum {
+	HS_UP_SCALE = 0,
+	HS_LT_9_16_SCALE,
+	HS_LT_10_16_SCALE,
+	HS_LT_11_16_SCALE,
+	HS_LT_12_16_SCALE,
+	HS_LT_13_16_SCALE,
+	HS_LT_14_16_SCALE,
+	HS_LT_15_16_SCALE,
+	HS_LE_16_16_SCALE,
+};
+
+static const u16 scaler_hs_coeffs[13][SC_NUM_PHASES * 2 * SC_H_NUM_TAPS] = {
+	[HS_UP_SCALE] = {
+		/* Luma */
+		0x001F, 0x1F90, 0x00D2, 0x06FE, 0x00D2, 0x1F90, 0x001F,
+		0x001C, 0x1F9E, 0x009F, 0x06FB, 0x0108, 0x1F82, 0x0022,
+		0x0019, 0x1FAC, 0x006F, 0x06F3, 0x0140, 0x1F74, 0x0025,
+		0x0016, 0x1FB9, 0x0041, 0x06E7, 0x017B, 0x1F66, 0x0028,
+		0x0013, 0x1FC6, 0x0017, 0x06D6, 0x01B7, 0x1F58, 0x002B,
+		0x0010, 0x1FD3, 0x1FEF, 0x06C0, 0x01F6, 0x1F4B, 0x002D,
+		0x000E, 0x1FDF, 0x1FCB, 0x06A5, 0x0235, 0x1F3F, 0x002F,
+		0x000B, 0x1FEA, 0x1FAA, 0x0686, 0x0277, 0x1F33, 0x0031,
+		0x0009, 0x1FF5, 0x1F8C, 0x0663, 0x02B8, 0x1F28, 0x0033,
+		0x0007, 0x1FFF, 0x1F72, 0x063A, 0x02FB, 0x1F1F, 0x0034,
+		0x0005, 0x0008, 0x1F5A, 0x060F, 0x033E, 0x1F17, 0x0035,
+		0x0003, 0x0010, 0x1F46, 0x05E0, 0x0382, 0x1F10, 0x0035,
+		0x0002, 0x0017, 0x1F34, 0x05AF, 0x03C5, 0x1F0B, 0x0034,
+		0x0001, 0x001E, 0x1F26, 0x0579, 0x0407, 0x1F08, 0x0033,
+		0x0000, 0x0023, 0x1F1A, 0x0541, 0x0449, 0x1F07, 0x0032,
+		0x1FFF, 0x0028, 0x1F12, 0x0506, 0x048A, 0x1F08, 0x002F,
+		0x002C, 0x1F0C, 0x04C8, 0x04C8, 0x1F0C, 0x002C, 0x0000,
+		0x002F, 0x1F08, 0x048A, 0x0506, 0x1F12, 0x0028, 0x1FFF,
+		0x0032, 0x1F07, 0x0449, 0x0541, 0x1F1A, 0x0023, 0x0000,
+		0x0033, 0x1F08, 0x0407, 0x0579, 0x1F26, 0x001E, 0x0001,
+		0x0034, 0x1F0B, 0x03C5, 0x05AF, 0x1F34, 0x0017, 0x0002,
+		0x0035, 0x1F10, 0x0382, 0x05E0, 0x1F46, 0x0010, 0x0003,
+		0x0035, 0x1F17, 0x033E, 0x060F, 0x1F5A, 0x0008, 0x0005,
+		0x0034, 0x1F1F, 0x02FB, 0x063A, 0x1F72, 0x1FFF, 0x0007,
+		0x0033, 0x1F28, 0x02B8, 0x0663, 0x1F8C, 0x1FF5, 0x0009,
+		0x0031, 0x1F33, 0x0277, 0x0686, 0x1FAA, 0x1FEA, 0x000B,
+		0x002F, 0x1F3F, 0x0235, 0x06A5, 0x1FCB, 0x1FDF, 0x000E,
+		0x002D, 0x1F4B, 0x01F6, 0x06C0, 0x1FEF, 0x1FD3, 0x0010,
+		0x002B, 0x1F58, 0x01B7, 0x06D6, 0x0017, 0x1FC6, 0x0013,
+		0x0028, 0x1F66, 0x017B, 0x06E7, 0x0041, 0x1FB9, 0x0016,
+		0x0025, 0x1F74, 0x0140, 0x06F3, 0x006F, 0x1FAC, 0x0019,
+		0x0022, 0x1F82, 0x0108, 0x06FB, 0x009F, 0x1F9E, 0x001C,
+		/* Chroma */
+		0x001F, 0x1F90, 0x00D2, 0x06FE, 0x00D2, 0x1F90, 0x001F,
+		0x001C, 0x1F9E, 0x009F, 0x06FB, 0x0108, 0x1F82, 0x0022,
+		0x0019, 0x1FAC, 0x006F, 0x06F3, 0x0140, 0x1F74, 0x0025,
+		0x0016, 0x1FB9, 0x0041, 0x06E7, 0x017B, 0x1F66, 0x0028,
+		0x0013, 0x1FC6, 0x0017, 0x06D6, 0x01B7, 0x1F58, 0x002B,
+		0x0010, 0x1FD3, 0x1FEF, 0x06C0, 0x01F6, 0x1F4B, 0x002D,
+		0x000E, 0x1FDF, 0x1FCB, 0x06A5, 0x0235, 0x1F3F, 0x002F,
+		0x000B, 0x1FEA, 0x1FAA, 0x0686, 0x0277, 0x1F33, 0x0031,
+		0x0009, 0x1FF5, 0x1F8C, 0x0663, 0x02B8, 0x1F28, 0x0033,
+		0x0007, 0x1FFF, 0x1F72, 0x063A, 0x02FB, 0x1F1F, 0x0034,
+		0x0005, 0x0008, 0x1F5A, 0x060F, 0x033E, 0x1F17, 0x0035,
+		0x0003, 0x0010, 0x1F46, 0x05E0, 0x0382, 0x1F10, 0x0035,
+		0x0002, 0x0017, 0x1F34, 0x05AF, 0x03C5, 0x1F0B, 0x0034,
+		0x0001, 0x001E, 0x1F26, 0x0579, 0x0407, 0x1F08, 0x0033,
+		0x0000, 0x0023, 0x1F1A, 0x0541, 0x0449, 0x1F07, 0x0032,
+		0x1FFF, 0x0028, 0x1F12, 0x0506, 0x048A, 0x1F08, 0x002F,
+		0x002C, 0x1F0C, 0x04C8, 0x04C8, 0x1F0C, 0x002C, 0x0000,
+		0x002F, 0x1F08, 0x048A, 0x0506, 0x1F12, 0x0028, 0x1FFF,
+		0x0032, 0x1F07, 0x0449, 0x0541, 0x1F1A, 0x0023, 0x0000,
+		0x0033, 0x1F08, 0x0407, 0x0579, 0x1F26, 0x001E, 0x0001,
+		0x0034, 0x1F0B, 0x03C5, 0x05AF, 0x1F34, 0x0017, 0x0002,
+		0x0035, 0x1F10, 0x0382, 0x05E0, 0x1F46, 0x0010, 0x0003,
+		0x0035, 0x1F17, 0x033E, 0x060F, 0x1F5A, 0x0008, 0x0005,
+		0x0034, 0x1F1F, 0x02FB, 0x063A, 0x1F72, 0x1FFF, 0x0007,
+		0x0033, 0x1F28, 0x02B8, 0x0663, 0x1F8C, 0x1FF5, 0x0009,
+		0x0031, 0x1F33, 0x0277, 0x0686, 0x1FAA, 0x1FEA, 0x000B,
+		0x002F, 0x1F3F, 0x0235, 0x06A5, 0x1FCB, 0x1FDF, 0x000E,
+		0x002D, 0x1F4B, 0x01F6, 0x06C0, 0x1FEF, 0x1FD3, 0x0010,
+		0x002B, 0x1F58, 0x01B7, 0x06D6, 0x0017, 0x1FC6, 0x0013,
+		0x0028, 0x1F66, 0x017B, 0x06E7, 0x0041, 0x1FB9, 0x0016,
+		0x0025, 0x1F74, 0x0140, 0x06F3, 0x006F, 0x1FAC, 0x0019,
+		0x0022, 0x1F82, 0x0108, 0x06FB, 0x009F, 0x1F9E, 0x001C,
+	},
+	[HS_LT_9_16_SCALE] = {
+		/* Luma */
+		0x1FA3, 0x005E, 0x024A, 0x036A, 0x024A, 0x005E, 0x1FA3,
+		0x1FA3, 0x0052, 0x023A, 0x036A, 0x0259, 0x006A, 0x1FA4,
+		0x1FA3, 0x0046, 0x022A, 0x036A, 0x0269, 0x0076, 0x1FA4,
+		0x1FA3, 0x003B, 0x021A, 0x0368, 0x0278, 0x0083, 0x1FA5,
+		0x1FA4, 0x0031, 0x020A, 0x0365, 0x0286, 0x0090, 0x1FA6,
+		0x1FA5, 0x0026, 0x01F9, 0x0362, 0x0294, 0x009E, 0x1FA8,
+		0x1FA6, 0x001C, 0x01E8, 0x035E, 0x02A3, 0x00AB, 0x1FAA,
+		0x1FA7, 0x0013, 0x01D7, 0x035A, 0x02B0, 0x00B9, 0x1FAC,
+		0x1FA9, 0x000A, 0x01C6, 0x0354, 0x02BD, 0x00C7, 0x1FAF,
+		0x1FAA, 0x0001, 0x01B6, 0x034E, 0x02C9, 0x00D6, 0x1FB2,
+		0x1FAC, 0x1FF9, 0x01A5, 0x0347, 0x02D5, 0x00E5, 0x1FB5,
+		0x1FAE, 0x1FF1, 0x0194, 0x0340, 0x02E1, 0x00F3, 0x1FB9,
+		0x1FB0, 0x1FEA, 0x0183, 0x0338, 0x02EC, 0x0102, 0x1FBD,
+		0x1FB2, 0x1FE3, 0x0172, 0x0330, 0x02F6, 0x0112, 0x1FC1,
+		0x1FB4, 0x1FDC, 0x0161, 0x0327, 0x0301, 0x0121, 0x1FC6,
+		0x1FB7, 0x1FD6, 0x0151, 0x031D, 0x030A, 0x0130, 0x1FCB,
+		0x1FD2, 0x0136, 0x02F8, 0x02F8, 0x0136, 0x1FD2, 0x0000,
+		0x1FCB, 0x0130, 0x030A, 0x031D, 0x0151, 0x1FD6, 0x1FB7,
+		0x1FC6, 0x0121, 0x0301, 0x0327, 0x0161, 0x1FDC, 0x1FB4,
+		0x1FC1, 0x0112, 0x02F6, 0x0330, 0x0172, 0x1FE3, 0x1FB2,
+		0x1FBD, 0x0102, 0x02EC, 0x0338, 0x0183, 0x1FEA, 0x1FB0,
+		0x1FB9, 0x00F3, 0x02E1, 0x0340, 0x0194, 0x1FF1, 0x1FAE,
+		0x1FB5, 0x00E5, 0x02D5, 0x0347, 0x01A5, 0x1FF9, 0x1FAC,
+		0x1FB2, 0x00D6, 0x02C9, 0x034E, 0x01B6, 0x0001, 0x1FAA,
+		0x1FAF, 0x00C7, 0x02BD, 0x0354, 0x01C6, 0x000A, 0x1FA9,
+		0x1FAC, 0x00B9, 0x02B0, 0x035A, 0x01D7, 0x0013, 0x1FA7,
+		0x1FAA, 0x00AB, 0x02A3, 0x035E, 0x01E8, 0x001C, 0x1FA6,
+		0x1FA8, 0x009E, 0x0294, 0x0362, 0x01F9, 0x0026, 0x1FA5,
+		0x1FA6, 0x0090, 0x0286, 0x0365, 0x020A, 0x0031, 0x1FA4,
+		0x1FA5, 0x0083, 0x0278, 0x0368, 0x021A, 0x003B, 0x1FA3,
+		0x1FA4, 0x0076, 0x0269, 0x036A, 0x022A, 0x0046, 0x1FA3,
+		0x1FA4, 0x006A, 0x0259, 0x036A, 0x023A, 0x0052, 0x1FA3,
+		/* Chroma */
+		0x1FA3, 0x005E, 0x024A, 0x036A, 0x024A, 0x005E, 0x1FA3,
+		0x1FA3, 0x0052, 0x023A, 0x036A, 0x0259, 0x006A, 0x1FA4,
+		0x1FA3, 0x0046, 0x022A, 0x036A, 0x0269, 0x0076, 0x1FA4,
+		0x1FA3, 0x003B, 0x021A, 0x0368, 0x0278, 0x0083, 0x1FA5,
+		0x1FA4, 0x0031, 0x020A, 0x0365, 0x0286, 0x0090, 0x1FA6,
+		0x1FA5, 0x0026, 0x01F9, 0x0362, 0x0294, 0x009E, 0x1FA8,
+		0x1FA6, 0x001C, 0x01E8, 0x035E, 0x02A3, 0x00AB, 0x1FAA,
+		0x1FA7, 0x0013, 0x01D7, 0x035A, 0x02B0, 0x00B9, 0x1FAC,
+		0x1FA9, 0x000A, 0x01C6, 0x0354, 0x02BD, 0x00C7, 0x1FAF,
+		0x1FAA, 0x0001, 0x01B6, 0x034E, 0x02C9, 0x00D6, 0x1FB2,
+		0x1FAC, 0x1FF9, 0x01A5, 0x0347, 0x02D5, 0x00E5, 0x1FB5,
+		0x1FAE, 0x1FF1, 0x0194, 0x0340, 0x02E1, 0x00F3, 0x1FB9,
+		0x1FB0, 0x1FEA, 0x0183, 0x0338, 0x02EC, 0x0102, 0x1FBD,
+		0x1FB2, 0x1FE3, 0x0172, 0x0330, 0x02F6, 0x0112, 0x1FC1,
+		0x1FB4, 0x1FDC, 0x0161, 0x0327, 0x0301, 0x0121, 0x1FC6,
+		0x1FB7, 0x1FD6, 0x0151, 0x031D, 0x030A, 0x0130, 0x1FCB,
+		0x1FD2, 0x0136, 0x02F8, 0x02F8, 0x0136, 0x1FD2, 0x0000,
+		0x1FCB, 0x0130, 0x030A, 0x031D, 0x0151, 0x1FD6, 0x1FB7,
+		0x1FC6, 0x0121, 0x0301, 0x0327, 0x0161, 0x1FDC, 0x1FB4,
+		0x1FC1, 0x0112, 0x02F6, 0x0330, 0x0172, 0x1FE3, 0x1FB2,
+		0x1FBD, 0x0102, 0x02EC, 0x0338, 0x0183, 0x1FEA, 0x1FB0,
+		0x1FB9, 0x00F3, 0x02E1, 0x0340, 0x0194, 0x1FF1, 0x1FAE,
+		0x1FB5, 0x00E5, 0x02D5, 0x0347, 0x01A5, 0x1FF9, 0x1FAC,
+		0x1FB2, 0x00D6, 0x02C9, 0x034E, 0x01B6, 0x0001, 0x1FAA,
+		0x1FAF, 0x00C7, 0x02BD, 0x0354, 0x01C6, 0x000A, 0x1FA9,
+		0x1FAC, 0x00B9, 0x02B0, 0x035A, 0x01D7, 0x0013, 0x1FA7,
+		0x1FAA, 0x00AB, 0x02A3, 0x035E, 0x01E8, 0x001C, 0x1FA6,
+		0x1FA8, 0x009E, 0x0294, 0x0362, 0x01F9, 0x0026, 0x1FA5,
+		0x1FA6, 0x0090, 0x0286, 0x0365, 0x020A, 0x0031, 0x1FA4,
+		0x1FA5, 0x0083, 0x0278, 0x0368, 0x021A, 0x003B, 0x1FA3,
+		0x1FA4, 0x0076, 0x0269, 0x036A, 0x022A, 0x0046, 0x1FA3,
+		0x1FA4, 0x006A, 0x0259, 0x036A, 0x023A, 0x0052, 0x1FA3,
+	},
+	[HS_LT_10_16_SCALE] = {
+		/* Luma */
+		0x1F8D, 0x000C, 0x026A, 0x03FA, 0x026A, 0x000C, 0x1F8D,
+		0x1F8F, 0x0000, 0x0255, 0x03FA, 0x027F, 0x0019, 0x1F8A,
+		0x1F92, 0x1FF5, 0x023F, 0x03F8, 0x0293, 0x0027, 0x1F88,
+		0x1F95, 0x1FEA, 0x022A, 0x03F6, 0x02A7, 0x0034, 0x1F86,
+		0x1F99, 0x1FDF, 0x0213, 0x03F2, 0x02BB, 0x0043, 0x1F85,
+		0x1F9C, 0x1FD5, 0x01FE, 0x03ED, 0x02CF, 0x0052, 0x1F83,
+		0x1FA0, 0x1FCC, 0x01E8, 0x03E7, 0x02E1, 0x0061, 0x1F83,
+		0x1FA4, 0x1FC3, 0x01D2, 0x03E0, 0x02F4, 0x0071, 0x1F82,
+		0x1FA7, 0x1FBB, 0x01BC, 0x03D9, 0x0306, 0x0081, 0x1F82,
+		0x1FAB, 0x1FB4, 0x01A6, 0x03D0, 0x0317, 0x0092, 0x1F82,
+		0x1FAF, 0x1FAD, 0x0190, 0x03C7, 0x0327, 0x00A3, 0x1F83,
+		0x1FB3, 0x1FA7, 0x017A, 0x03BC, 0x0337, 0x00B5, 0x1F84,
+		0x1FB8, 0x1FA1, 0x0165, 0x03B0, 0x0346, 0x00C7, 0x1F85,
+		0x1FBC, 0x1F9C, 0x0150, 0x03A4, 0x0354, 0x00D9, 0x1F87,
+		0x1FC0, 0x1F98, 0x013A, 0x0397, 0x0361, 0x00EC, 0x1F8A,
+		0x1FC4, 0x1F93, 0x0126, 0x0389, 0x036F, 0x00FE, 0x1F8D,
+		0x1F93, 0x010A, 0x0363, 0x0363, 0x010A, 0x1F93, 0x0000,
+		0x1F8D, 0x00FE, 0x036F, 0x0389, 0x0126, 0x1F93, 0x1FC4,
+		0x1F8A, 0x00EC, 0x0361, 0x0397, 0x013A, 0x1F98, 0x1FC0,
+		0x1F87, 0x00D9, 0x0354, 0x03A4, 0x0150, 0x1F9C, 0x1FBC,
+		0x1F85, 0x00C7, 0x0346, 0x03B0, 0x0165, 0x1FA1, 0x1FB8,
+		0x1F84, 0x00B5, 0x0337, 0x03BC, 0x017A, 0x1FA7, 0x1FB3,
+		0x1F83, 0x00A3, 0x0327, 0x03C7, 0x0190, 0x1FAD, 0x1FAF,
+		0x1F82, 0x0092, 0x0317, 0x03D0, 0x01A6, 0x1FB4, 0x1FAB,
+		0x1F82, 0x0081, 0x0306, 0x03D9, 0x01BC, 0x1FBB, 0x1FA7,
+		0x1F82, 0x0071, 0x02F4, 0x03E0, 0x01D2, 0x1FC3, 0x1FA4,
+		0x1F83, 0x0061, 0x02E1, 0x03E7, 0x01E8, 0x1FCC, 0x1FA0,
+		0x1F83, 0x0052, 0x02CF, 0x03ED, 0x01FE, 0x1FD5, 0x1F9C,
+		0x1F85, 0x0043, 0x02BB, 0x03F2, 0x0213, 0x1FDF, 0x1F99,
+		0x1F86, 0x0034, 0x02A7, 0x03F6, 0x022A, 0x1FEA, 0x1F95,
+		0x1F88, 0x0027, 0x0293, 0x03F8, 0x023F, 0x1FF5, 0x1F92,
+		0x1F8A, 0x0019, 0x027F, 0x03FA, 0x0255, 0x0000, 0x1F8F,
+		/* Chroma */
+		0x1F8D, 0x000C, 0x026A, 0x03FA, 0x026A, 0x000C, 0x1F8D,
+		0x1F8F, 0x0000, 0x0255, 0x03FA, 0x027F, 0x0019, 0x1F8A,
+		0x1F92, 0x1FF5, 0x023F, 0x03F8, 0x0293, 0x0027, 0x1F88,
+		0x1F95, 0x1FEA, 0x022A, 0x03F6, 0x02A7, 0x0034, 0x1F86,
+		0x1F99, 0x1FDF, 0x0213, 0x03F2, 0x02BB, 0x0043, 0x1F85,
+		0x1F9C, 0x1FD5, 0x01FE, 0x03ED, 0x02CF, 0x0052, 0x1F83,
+		0x1FA0, 0x1FCC, 0x01E8, 0x03E7, 0x02E1, 0x0061, 0x1F83,
+		0x1FA4, 0x1FC3, 0x01D2, 0x03E0, 0x02F4, 0x0071, 0x1F82,
+		0x1FA7, 0x1FBB, 0x01BC, 0x03D9, 0x0306, 0x0081, 0x1F82,
+		0x1FAB, 0x1FB4, 0x01A6, 0x03D0, 0x0317, 0x0092, 0x1F82,
+		0x1FAF, 0x1FAD, 0x0190, 0x03C7, 0x0327, 0x00A3, 0x1F83,
+		0x1FB3, 0x1FA7, 0x017A, 0x03BC, 0x0337, 0x00B5, 0x1F84,
+		0x1FB8, 0x1FA1, 0x0165, 0x03B0, 0x0346, 0x00C7, 0x1F85,
+		0x1FBC, 0x1F9C, 0x0150, 0x03A4, 0x0354, 0x00D9, 0x1F87,
+		0x1FC0, 0x1F98, 0x013A, 0x0397, 0x0361, 0x00EC, 0x1F8A,
+		0x1FC4, 0x1F93, 0x0126, 0x0389, 0x036F, 0x00FE, 0x1F8D,
+		0x1F93, 0x010A, 0x0363, 0x0363, 0x010A, 0x1F93, 0x0000,
+		0x1F8D, 0x00FE, 0x036F, 0x0389, 0x0126, 0x1F93, 0x1FC4,
+		0x1F8A, 0x00EC, 0x0361, 0x0397, 0x013A, 0x1F98, 0x1FC0,
+		0x1F87, 0x00D9, 0x0354, 0x03A4, 0x0150, 0x1F9C, 0x1FBC,
+		0x1F85, 0x00C7, 0x0346, 0x03B0, 0x0165, 0x1FA1, 0x1FB8,
+		0x1F84, 0x00B5, 0x0337, 0x03BC, 0x017A, 0x1FA7, 0x1FB3,
+		0x1F83, 0x00A3, 0x0327, 0x03C7, 0x0190, 0x1FAD, 0x1FAF,
+		0x1F82, 0x0092, 0x0317, 0x03D0, 0x01A6, 0x1FB4, 0x1FAB,
+		0x1F82, 0x0081, 0x0306, 0x03D9, 0x01BC, 0x1FBB, 0x1FA7,
+		0x1F82, 0x0071, 0x02F4, 0x03E0, 0x01D2, 0x1FC3, 0x1FA4,
+		0x1F83, 0x0061, 0x02E1, 0x03E7, 0x01E8, 0x1FCC, 0x1FA0,
+		0x1F83, 0x0052, 0x02CF, 0x03ED, 0x01FE, 0x1FD5, 0x1F9C,
+		0x1F85, 0x0043, 0x02BB, 0x03F2, 0x0213, 0x1FDF, 0x1F99,
+		0x1F86, 0x0034, 0x02A7, 0x03F6, 0x022A, 0x1FEA, 0x1F95,
+		0x1F88, 0x0027, 0x0293, 0x03F8, 0x023F, 0x1FF5, 0x1F92,
+		0x1F8A, 0x0019, 0x027F, 0x03FA, 0x0255, 0x0000, 0x1F8F,
+	},
+	[HS_LT_11_16_SCALE] = {
+		/* Luma */
+		0x1F95, 0x1FB5, 0x0272, 0x0488, 0x0272, 0x1FB5, 0x1F95,
+		0x1F9B, 0x1FAA, 0x0257, 0x0486, 0x028D, 0x1FC1, 0x1F90,
+		0x1FA0, 0x1FA0, 0x023C, 0x0485, 0x02A8, 0x1FCD, 0x1F8A,
+		0x1FA6, 0x1F96, 0x0221, 0x0481, 0x02C2, 0x1FDB, 0x1F85,
+		0x1FAC, 0x1F8E, 0x0205, 0x047C, 0x02DC, 0x1FE9, 0x1F80,
+		0x1FB1, 0x1F86, 0x01E9, 0x0476, 0x02F6, 0x1FF8, 0x1F7C,
+		0x1FB7, 0x1F7F, 0x01CE, 0x046E, 0x030F, 0x0008, 0x1F77,
+		0x1FBD, 0x1F79, 0x01B3, 0x0465, 0x0326, 0x0019, 0x1F73,
+		0x1FC3, 0x1F73, 0x0197, 0x045B, 0x033E, 0x002A, 0x1F70,
+		0x1FC8, 0x1F6F, 0x017D, 0x044E, 0x0355, 0x003C, 0x1F6D,
+		0x1FCE, 0x1F6B, 0x0162, 0x0441, 0x036B, 0x004F, 0x1F6A,
+		0x1FD3, 0x1F68, 0x0148, 0x0433, 0x0380, 0x0063, 0x1F67,
+		0x1FD8, 0x1F65, 0x012E, 0x0424, 0x0395, 0x0077, 0x1F65,
+		0x1FDE, 0x1F63, 0x0115, 0x0413, 0x03A8, 0x008B, 0x1F64,
+		0x1FE3, 0x1F62, 0x00FC, 0x0403, 0x03BA, 0x00A0, 0x1F62,
+		0x1FE7, 0x1F62, 0x00E4, 0x03EF, 0x03CC, 0x00B6, 0x1F62,
+		0x1F63, 0x00CA, 0x03D3, 0x03D3, 0x00CA, 0x1F63, 0x0000,
+		0x1F62, 0x00B6, 0x03CC, 0x03EF, 0x00E4, 0x1F62, 0x1FE7,
+		0x1F62, 0x00A0, 0x03BA, 0x0403, 0x00FC, 0x1F62, 0x1FE3,
+		0x1F64, 0x008B, 0x03A8, 0x0413, 0x0115, 0x1F63, 0x1FDE,
+		0x1F65, 0x0077, 0x0395, 0x0424, 0x012E, 0x1F65, 0x1FD8,
+		0x1F67, 0x0063, 0x0380, 0x0433, 0x0148, 0x1F68, 0x1FD3,
+		0x1F6A, 0x004F, 0x036B, 0x0441, 0x0162, 0x1F6B, 0x1FCE,
+		0x1F6D, 0x003C, 0x0355, 0x044E, 0x017D, 0x1F6F, 0x1FC8,
+		0x1F70, 0x002A, 0x033E, 0x045B, 0x0197, 0x1F73, 0x1FC3,
+		0x1F73, 0x0019, 0x0326, 0x0465, 0x01B3, 0x1F79, 0x1FBD,
+		0x1F77, 0x0008, 0x030F, 0x046E, 0x01CE, 0x1F7F, 0x1FB7,
+		0x1F7C, 0x1FF8, 0x02F6, 0x0476, 0x01E9, 0x1F86, 0x1FB1,
+		0x1F80, 0x1FE9, 0x02DC, 0x047C, 0x0205, 0x1F8E, 0x1FAC,
+		0x1F85, 0x1FDB, 0x02C2, 0x0481, 0x0221, 0x1F96, 0x1FA6,
+		0x1F8A, 0x1FCD, 0x02A8, 0x0485, 0x023C, 0x1FA0, 0x1FA0,
+		0x1F90, 0x1FC1, 0x028D, 0x0486, 0x0257, 0x1FAA, 0x1F9B,
+		/* Chroma */
+		0x1F95, 0x1FB5, 0x0272, 0x0488, 0x0272, 0x1FB5, 0x1F95,
+		0x1F9B, 0x1FAA, 0x0257, 0x0486, 0x028D, 0x1FC1, 0x1F90,
+		0x1FA0, 0x1FA0, 0x023C, 0x0485, 0x02A8, 0x1FCD, 0x1F8A,
+		0x1FA6, 0x1F96, 0x0221, 0x0481, 0x02C2, 0x1FDB, 0x1F85,
+		0x1FAC, 0x1F8E, 0x0205, 0x047C, 0x02DC, 0x1FE9, 0x1F80,
+		0x1FB1, 0x1F86, 0x01E9, 0x0476, 0x02F6, 0x1FF8, 0x1F7C,
+		0x1FB7, 0x1F7F, 0x01CE, 0x046E, 0x030F, 0x0008, 0x1F77,
+		0x1FBD, 0x1F79, 0x01B3, 0x0465, 0x0326, 0x0019, 0x1F73,
+		0x1FC3, 0x1F73, 0x0197, 0x045B, 0x033E, 0x002A, 0x1F70,
+		0x1FC8, 0x1F6F, 0x017D, 0x044E, 0x0355, 0x003C, 0x1F6D,
+		0x1FCE, 0x1F6B, 0x0162, 0x0441, 0x036B, 0x004F, 0x1F6A,
+		0x1FD3, 0x1F68, 0x0148, 0x0433, 0x0380, 0x0063, 0x1F67,
+		0x1FD8, 0x1F65, 0x012E, 0x0424, 0x0395, 0x0077, 0x1F65,
+		0x1FDE, 0x1F63, 0x0115, 0x0413, 0x03A8, 0x008B, 0x1F64,
+		0x1FE3, 0x1F62, 0x00FC, 0x0403, 0x03BA, 0x00A0, 0x1F62,
+		0x1FE7, 0x1F62, 0x00E4, 0x03EF, 0x03CC, 0x00B6, 0x1F62,
+		0x1F63, 0x00CA, 0x03D3, 0x03D3, 0x00CA, 0x1F63, 0x0000,
+		0x1F62, 0x00B6, 0x03CC, 0x03EF, 0x00E4, 0x1F62, 0x1FE7,
+		0x1F62, 0x00A0, 0x03BA, 0x0403, 0x00FC, 0x1F62, 0x1FE3,
+		0x1F64, 0x008B, 0x03A8, 0x0413, 0x0115, 0x1F63, 0x1FDE,
+		0x1F65, 0x0077, 0x0395, 0x0424, 0x012E, 0x1F65, 0x1FD8,
+		0x1F67, 0x0063, 0x0380, 0x0433, 0x0148, 0x1F68, 0x1FD3,
+		0x1F6A, 0x004F, 0x036B, 0x0441, 0x0162, 0x1F6B, 0x1FCE,
+		0x1F6D, 0x003C, 0x0355, 0x044E, 0x017D, 0x1F6F, 0x1FC8,
+		0x1F70, 0x002A, 0x033E, 0x045B, 0x0197, 0x1F73, 0x1FC3,
+		0x1F73, 0x0019, 0x0326, 0x0465, 0x01B3, 0x1F79, 0x1FBD,
+		0x1F77, 0x0008, 0x030F, 0x046E, 0x01CE, 0x1F7F, 0x1FB7,
+		0x1F7C, 0x1FF8, 0x02F6, 0x0476, 0x01E9, 0x1F86, 0x1FB1,
+		0x1F80, 0x1FE9, 0x02DC, 0x047C, 0x0205, 0x1F8E, 0x1FAC,
+		0x1F85, 0x1FDB, 0x02C2, 0x0481, 0x0221, 0x1F96, 0x1FA6,
+		0x1F8A, 0x1FCD, 0x02A8, 0x0485, 0x023C, 0x1FA0, 0x1FA0,
+		0x1F90, 0x1FC1, 0x028D, 0x0486, 0x0257, 0x1FAA, 0x1F9B,
+	},
+	[HS_LT_12_16_SCALE] = {
+		/* Luma */
+		0x1FBB, 0x1F65, 0x025E, 0x0504, 0x025E, 0x1F65, 0x1FBB,
+		0x1FC3, 0x1F5D, 0x023C, 0x0503, 0x027F, 0x1F6E, 0x1FB4,
+		0x1FCA, 0x1F56, 0x021B, 0x0501, 0x02A0, 0x1F78, 0x1FAC,
+		0x1FD1, 0x1F50, 0x01FA, 0x04FD, 0x02C0, 0x1F83, 0x1FA5,
+		0x1FD8, 0x1F4B, 0x01D9, 0x04F6, 0x02E1, 0x1F90, 0x1F9D,
+		0x1FDF, 0x1F47, 0x01B8, 0x04EF, 0x0301, 0x1F9D, 0x1F95,
+		0x1FE6, 0x1F43, 0x0198, 0x04E5, 0x0321, 0x1FAB, 0x1F8E,
+		0x1FEC, 0x1F41, 0x0178, 0x04DA, 0x0340, 0x1FBB, 0x1F86,
+		0x1FF2, 0x1F40, 0x0159, 0x04CC, 0x035E, 0x1FCC, 0x1F7F,
+		0x1FF8, 0x1F40, 0x013A, 0x04BE, 0x037B, 0x1FDD, 0x1F78,
+		0x1FFE, 0x1F40, 0x011B, 0x04AD, 0x0398, 0x1FF0, 0x1F72,
+		0x0003, 0x1F41, 0x00FD, 0x049C, 0x03B4, 0x0004, 0x1F6B,
+		0x0008, 0x1F43, 0x00E0, 0x0489, 0x03CE, 0x0019, 0x1F65,
+		0x000D, 0x1F46, 0x00C4, 0x0474, 0x03E8, 0x002E, 0x1F5F,
+		0x0011, 0x1F49, 0x00A9, 0x045E, 0x0400, 0x0045, 0x1F5A,
+		0x0015, 0x1F4D, 0x008E, 0x0447, 0x0418, 0x005C, 0x1F55,
+		0x1F4F, 0x0076, 0x043B, 0x043B, 0x0076, 0x1F4F, 0x0000,
+		0x1F55, 0x005C, 0x0418, 0x0447, 0x008E, 0x1F4D, 0x0015,
+		0x1F5A, 0x0045, 0x0400, 0x045E, 0x00A9, 0x1F49, 0x0011,
+		0x1F5F, 0x002E, 0x03E8, 0x0474, 0x00C4, 0x1F46, 0x000D,
+		0x1F65, 0x0019, 0x03CE, 0x0489, 0x00E0, 0x1F43, 0x0008,
+		0x1F6B, 0x0004, 0x03B4, 0x049C, 0x00FD, 0x1F41, 0x0003,
+		0x1F72, 0x1FF0, 0x0398, 0x04AD, 0x011B, 0x1F40, 0x1FFE,
+		0x1F78, 0x1FDD, 0x037B, 0x04BE, 0x013A, 0x1F40, 0x1FF8,
+		0x1F7F, 0x1FCC, 0x035E, 0x04CC, 0x0159, 0x1F40, 0x1FF2,
+		0x1F86, 0x1FBB, 0x0340, 0x04DA, 0x0178, 0x1F41, 0x1FEC,
+		0x1F8E, 0x1FAB, 0x0321, 0x04E5, 0x0198, 0x1F43, 0x1FE6,
+		0x1F95, 0x1F9D, 0x0301, 0x04EF, 0x01B8, 0x1F47, 0x1FDF,
+		0x1F9D, 0x1F90, 0x02E1, 0x04F6, 0x01D9, 0x1F4B, 0x1FD8,
+		0x1FA5, 0x1F83, 0x02C0, 0x04FD, 0x01FA, 0x1F50, 0x1FD1,
+		0x1FAC, 0x1F78, 0x02A0, 0x0501, 0x021B, 0x1F56, 0x1FCA,
+		0x1FB4, 0x1F6E, 0x027F, 0x0503, 0x023C, 0x1F5D, 0x1FC3,
+		/* Chroma */
+		0x1FBB, 0x1F65, 0x025E, 0x0504, 0x025E, 0x1F65, 0x1FBB,
+		0x1FC3, 0x1F5D, 0x023C, 0x0503, 0x027F, 0x1F6E, 0x1FB4,
+		0x1FCA, 0x1F56, 0x021B, 0x0501, 0x02A0, 0x1F78, 0x1FAC,
+		0x1FD1, 0x1F50, 0x01FA, 0x04FD, 0x02C0, 0x1F83, 0x1FA5,
+		0x1FD8, 0x1F4B, 0x01D9, 0x04F6, 0x02E1, 0x1F90, 0x1F9D,
+		0x1FDF, 0x1F47, 0x01B8, 0x04EF, 0x0301, 0x1F9D, 0x1F95,
+		0x1FE6, 0x1F43, 0x0198, 0x04E5, 0x0321, 0x1FAB, 0x1F8E,
+		0x1FEC, 0x1F41, 0x0178, 0x04DA, 0x0340, 0x1FBB, 0x1F86,
+		0x1FF2, 0x1F40, 0x0159, 0x04CC, 0x035E, 0x1FCC, 0x1F7F,
+		0x1FF8, 0x1F40, 0x013A, 0x04BE, 0x037B, 0x1FDD, 0x1F78,
+		0x1FFE, 0x1F40, 0x011B, 0x04AD, 0x0398, 0x1FF0, 0x1F72,
+		0x0003, 0x1F41, 0x00FD, 0x049C, 0x03B4, 0x0004, 0x1F6B,
+		0x0008, 0x1F43, 0x00E0, 0x0489, 0x03CE, 0x0019, 0x1F65,
+		0x000D, 0x1F46, 0x00C4, 0x0474, 0x03E8, 0x002E, 0x1F5F,
+		0x0011, 0x1F49, 0x00A9, 0x045E, 0x0400, 0x0045, 0x1F5A,
+		0x0015, 0x1F4D, 0x008E, 0x0447, 0x0418, 0x005C, 0x1F55,
+		0x1F4F, 0x0076, 0x043B, 0x043B, 0x0076, 0x1F4F, 0x0000,
+		0x1F55, 0x005C, 0x0418, 0x0447, 0x008E, 0x1F4D, 0x0015,
+		0x1F5A, 0x0045, 0x0400, 0x045E, 0x00A9, 0x1F49, 0x0011,
+		0x1F5F, 0x002E, 0x03E8, 0x0474, 0x00C4, 0x1F46, 0x000D,
+		0x1F65, 0x0019, 0x03CE, 0x0489, 0x00E0, 0x1F43, 0x0008,
+		0x1F6B, 0x0004, 0x03B4, 0x049C, 0x00FD, 0x1F41, 0x0003,
+		0x1F72, 0x1FF0, 0x0398, 0x04AD, 0x011B, 0x1F40, 0x1FFE,
+		0x1F78, 0x1FDD, 0x037B, 0x04BE, 0x013A, 0x1F40, 0x1FF8,
+		0x1F7F, 0x1FCC, 0x035E, 0x04CC, 0x0159, 0x1F40, 0x1FF2,
+		0x1F86, 0x1FBB, 0x0340, 0x04DA, 0x0178, 0x1F41, 0x1FEC,
+		0x1F8E, 0x1FAB, 0x0321, 0x04E5, 0x0198, 0x1F43, 0x1FE6,
+		0x1F95, 0x1F9D, 0x0301, 0x04EF, 0x01B8, 0x1F47, 0x1FDF,
+		0x1F9D, 0x1F90, 0x02E1, 0x04F6, 0x01D9, 0x1F4B, 0x1FD8,
+		0x1FA5, 0x1F83, 0x02C0, 0x04FD, 0x01FA, 0x1F50, 0x1FD1,
+		0x1FAC, 0x1F78, 0x02A0, 0x0501, 0x021B, 0x1F56, 0x1FCA,
+		0x1FB4, 0x1F6E, 0x027F, 0x0503, 0x023C, 0x1F5D, 0x1FC3,
+	},
+	[HS_LT_13_16_SCALE] = {
+		/* Luma */
+		0x1FF4, 0x1F29, 0x022D, 0x056C, 0x022D, 0x1F29, 0x1FF4,
+		0x1FFC, 0x1F26, 0x0206, 0x056A, 0x0254, 0x1F2E, 0x1FEC,
+		0x0003, 0x1F24, 0x01E0, 0x0567, 0x027A, 0x1F34, 0x1FE4,
+		0x000A, 0x1F23, 0x01BA, 0x0561, 0x02A2, 0x1F3B, 0x1FDB,
+		0x0011, 0x1F22, 0x0194, 0x055B, 0x02C9, 0x1F43, 0x1FD2,
+		0x0017, 0x1F23, 0x016F, 0x0551, 0x02F0, 0x1F4D, 0x1FC9,
+		0x001D, 0x1F25, 0x014B, 0x0545, 0x0316, 0x1F58, 0x1FC0,
+		0x0022, 0x1F28, 0x0127, 0x0538, 0x033C, 0x1F65, 0x1FB6,
+		0x0027, 0x1F2C, 0x0104, 0x0528, 0x0361, 0x1F73, 0x1FAD,
+		0x002B, 0x1F30, 0x00E2, 0x0518, 0x0386, 0x1F82, 0x1FA3,
+		0x002F, 0x1F36, 0x00C2, 0x0504, 0x03AA, 0x1F92, 0x1F99,
+		0x0032, 0x1F3C, 0x00A2, 0x04EF, 0x03CD, 0x1FA4, 0x1F90,
+		0x0035, 0x1F42, 0x0083, 0x04D9, 0x03EF, 0x1FB8, 0x1F86,
+		0x0038, 0x1F49, 0x0065, 0x04C0, 0x0410, 0x1FCD, 0x1F7D,
+		0x003A, 0x1F51, 0x0048, 0x04A6, 0x0431, 0x1FE3, 0x1F73,
+		0x003C, 0x1F59, 0x002D, 0x048A, 0x0450, 0x1FFA, 0x1F6A,
+		0x1F5D, 0x0014, 0x048F, 0x048F, 0x0014, 0x1F5D, 0x0000,
+		0x1F6A, 0x1FFA, 0x0450, 0x048A, 0x002D, 0x1F59, 0x003C,
+		0x1F73, 0x1FE3, 0x0431, 0x04A6, 0x0048, 0x1F51, 0x003A,
+		0x1F7D, 0x1FCD, 0x0410, 0x04C0, 0x0065, 0x1F49, 0x0038,
+		0x1F86, 0x1FB8, 0x03EF, 0x04D9, 0x0083, 0x1F42, 0x0035,
+		0x1F90, 0x1FA4, 0x03CD, 0x04EF, 0x00A2, 0x1F3C, 0x0032,
+		0x1F99, 0x1F92, 0x03AA, 0x0504, 0x00C2, 0x1F36, 0x002F,
+		0x1FA3, 0x1F82, 0x0386, 0x0518, 0x00E2, 0x1F30, 0x002B,
+		0x1FAD, 0x1F73, 0x0361, 0x0528, 0x0104, 0x1F2C, 0x0027,
+		0x1FB6, 0x1F65, 0x033C, 0x0538, 0x0127, 0x1F28, 0x0022,
+		0x1FC0, 0x1F58, 0x0316, 0x0545, 0x014B, 0x1F25, 0x001D,
+		0x1FC9, 0x1F4D, 0x02F0, 0x0551, 0x016F, 0x1F23, 0x0017,
+		0x1FD2, 0x1F43, 0x02C9, 0x055B, 0x0194, 0x1F22, 0x0011,
+		0x1FDB, 0x1F3B, 0x02A2, 0x0561, 0x01BA, 0x1F23, 0x000A,
+		0x1FE4, 0x1F34, 0x027A, 0x0567, 0x01E0, 0x1F24, 0x0003,
+		0x1FEC, 0x1F2E, 0x0254, 0x056A, 0x0206, 0x1F26, 0x1FFC,
+		/* Chroma */
+		0x1FF4, 0x1F29, 0x022D, 0x056C, 0x022D, 0x1F29, 0x1FF4,
+		0x1FFC, 0x1F26, 0x0206, 0x056A, 0x0254, 0x1F2E, 0x1FEC,
+		0x0003, 0x1F24, 0x01E0, 0x0567, 0x027A, 0x1F34, 0x1FE4,
+		0x000A, 0x1F23, 0x01BA, 0x0561, 0x02A2, 0x1F3B, 0x1FDB,
+		0x0011, 0x1F22, 0x0194, 0x055B, 0x02C9, 0x1F43, 0x1FD2,
+		0x0017, 0x1F23, 0x016F, 0x0551, 0x02F0, 0x1F4D, 0x1FC9,
+		0x001D, 0x1F25, 0x014B, 0x0545, 0x0316, 0x1F58, 0x1FC0,
+		0x0022, 0x1F28, 0x0127, 0x0538, 0x033C, 0x1F65, 0x1FB6,
+		0x0027, 0x1F2C, 0x0104, 0x0528, 0x0361, 0x1F73, 0x1FAD,
+		0x002B, 0x1F30, 0x00E2, 0x0518, 0x0386, 0x1F82, 0x1FA3,
+		0x002F, 0x1F36, 0x00C2, 0x0504, 0x03AA, 0x1F92, 0x1F99,
+		0x0032, 0x1F3C, 0x00A2, 0x04EF, 0x03CD, 0x1FA4, 0x1F90,
+		0x0035, 0x1F42, 0x0083, 0x04D9, 0x03EF, 0x1FB8, 0x1F86,
+		0x0038, 0x1F49, 0x0065, 0x04C0, 0x0410, 0x1FCD, 0x1F7D,
+		0x003A, 0x1F51, 0x0048, 0x04A6, 0x0431, 0x1FE3, 0x1F73,
+		0x003C, 0x1F59, 0x002D, 0x048A, 0x0450, 0x1FFA, 0x1F6A,
+		0x1F5D, 0x0014, 0x048F, 0x048F, 0x0014, 0x1F5D, 0x0000,
+		0x1F6A, 0x1FFA, 0x0450, 0x048A, 0x002D, 0x1F59, 0x003C,
+		0x1F73, 0x1FE3, 0x0431, 0x04A6, 0x0048, 0x1F51, 0x003A,
+		0x1F7D, 0x1FCD, 0x0410, 0x04C0, 0x0065, 0x1F49, 0x0038,
+		0x1F86, 0x1FB8, 0x03EF, 0x04D9, 0x0083, 0x1F42, 0x0035,
+		0x1F90, 0x1FA4, 0x03CD, 0x04EF, 0x00A2, 0x1F3C, 0x0032,
+		0x1F99, 0x1F92, 0x03AA, 0x0504, 0x00C2, 0x1F36, 0x002F,
+		0x1FA3, 0x1F82, 0x0386, 0x0518, 0x00E2, 0x1F30, 0x002B,
+		0x1FAD, 0x1F73, 0x0361, 0x0528, 0x0104, 0x1F2C, 0x0027,
+		0x1FB6, 0x1F65, 0x033C, 0x0538, 0x0127, 0x1F28, 0x0022,
+		0x1FC0, 0x1F58, 0x0316, 0x0545, 0x014B, 0x1F25, 0x001D,
+		0x1FC9, 0x1F4D, 0x02F0, 0x0551, 0x016F, 0x1F23, 0x0017,
+		0x1FD2, 0x1F43, 0x02C9, 0x055B, 0x0194, 0x1F22, 0x0011,
+		0x1FDB, 0x1F3B, 0x02A2, 0x0561, 0x01BA, 0x1F23, 0x000A,
+		0x1FE4, 0x1F34, 0x027A, 0x0567, 0x01E0, 0x1F24, 0x0003,
+		0x1FEC, 0x1F2E, 0x0254, 0x056A, 0x0206, 0x1F26, 0x1FFC,
+	},
+	[HS_LT_14_16_SCALE] = {
+		/* Luma */
+		0x002F, 0x1F0B, 0x01E7, 0x05BE, 0x01E7, 0x1F0B, 0x002F,
+		0x0035, 0x1F0D, 0x01BC, 0x05BD, 0x0213, 0x1F0A, 0x0028,
+		0x003A, 0x1F11, 0x0191, 0x05BA, 0x023F, 0x1F0A, 0x0021,
+		0x003F, 0x1F15, 0x0167, 0x05B3, 0x026C, 0x1F0C, 0x001A,
+		0x0043, 0x1F1B, 0x013E, 0x05AA, 0x0299, 0x1F0F, 0x0012,
+		0x0046, 0x1F21, 0x0116, 0x05A1, 0x02C6, 0x1F13, 0x0009,
+		0x0049, 0x1F28, 0x00EF, 0x0593, 0x02F4, 0x1F19, 0x0000,
+		0x004C, 0x1F30, 0x00C9, 0x0584, 0x0321, 0x1F20, 0x1FF6,
+		0x004E, 0x1F39, 0x00A4, 0x0572, 0x034D, 0x1F2A, 0x1FEC,
+		0x004F, 0x1F43, 0x0080, 0x055E, 0x037A, 0x1F34, 0x1FE2,
+		0x0050, 0x1F4D, 0x005E, 0x0548, 0x03A5, 0x1F41, 0x1FD7,
+		0x0050, 0x1F57, 0x003D, 0x0531, 0x03D1, 0x1F4F, 0x1FCB,
+		0x0050, 0x1F62, 0x001E, 0x0516, 0x03FB, 0x1F5F, 0x1FC0,
+		0x004F, 0x1F6D, 0x0000, 0x04FA, 0x0425, 0x1F71, 0x1FB4,
+		0x004E, 0x1F79, 0x1FE4, 0x04DC, 0x044D, 0x1F84, 0x1FA8,
+		0x004D, 0x1F84, 0x1FCA, 0x04BC, 0x0474, 0x1F99, 0x1F9C,
+		0x1F8C, 0x1FAE, 0x04C6, 0x04C6, 0x1FAE, 0x1F8C, 0x0000,
+		0x1F9C, 0x1F99, 0x0474, 0x04BC, 0x1FCA, 0x1F84, 0x004D,
+		0x1FA8, 0x1F84, 0x044D, 0x04DC, 0x1FE4, 0x1F79, 0x004E,
+		0x1FB4, 0x1F71, 0x0425, 0x04FA, 0x0000, 0x1F6D, 0x004F,
+		0x1FC0, 0x1F5F, 0x03FB, 0x0516, 0x001E, 0x1F62, 0x0050,
+		0x1FCB, 0x1F4F, 0x03D1, 0x0531, 0x003D, 0x1F57, 0x0050,
+		0x1FD7, 0x1F41, 0x03A5, 0x0548, 0x005E, 0x1F4D, 0x0050,
+		0x1FE2, 0x1F34, 0x037A, 0x055E, 0x0080, 0x1F43, 0x004F,
+		0x1FEC, 0x1F2A, 0x034D, 0x0572, 0x00A4, 0x1F39, 0x004E,
+		0x1FF6, 0x1F20, 0x0321, 0x0584, 0x00C9, 0x1F30, 0x004C,
+		0x0000, 0x1F19, 0x02F4, 0x0593, 0x00EF, 0x1F28, 0x0049,
+		0x0009, 0x1F13, 0x02C6, 0x05A1, 0x0116, 0x1F21, 0x0046,
+		0x0012, 0x1F0F, 0x0299, 0x05AA, 0x013E, 0x1F1B, 0x0043,
+		0x001A, 0x1F0C, 0x026C, 0x05B3, 0x0167, 0x1F15, 0x003F,
+		0x0021, 0x1F0A, 0x023F, 0x05BA, 0x0191, 0x1F11, 0x003A,
+		0x0028, 0x1F0A, 0x0213, 0x05BD, 0x01BC, 0x1F0D, 0x0035,
+		/* Chroma */
+		0x002F, 0x1F0B, 0x01E7, 0x05BE, 0x01E7, 0x1F0B, 0x002F,
+		0x0035, 0x1F0D, 0x01BC, 0x05BD, 0x0213, 0x1F0A, 0x0028,
+		0x003A, 0x1F11, 0x0191, 0x05BA, 0x023F, 0x1F0A, 0x0021,
+		0x003F, 0x1F15, 0x0167, 0x05B3, 0x026C, 0x1F0C, 0x001A,
+		0x0043, 0x1F1B, 0x013E, 0x05AA, 0x0299, 0x1F0F, 0x0012,
+		0x0046, 0x1F21, 0x0116, 0x05A1, 0x02C6, 0x1F13, 0x0009,
+		0x0049, 0x1F28, 0x00EF, 0x0593, 0x02F4, 0x1F19, 0x0000,
+		0x004C, 0x1F30, 0x00C9, 0x0584, 0x0321, 0x1F20, 0x1FF6,
+		0x004E, 0x1F39, 0x00A4, 0x0572, 0x034D, 0x1F2A, 0x1FEC,
+		0x004F, 0x1F43, 0x0080, 0x055E, 0x037A, 0x1F34, 0x1FE2,
+		0x0050, 0x1F4D, 0x005E, 0x0548, 0x03A5, 0x1F41, 0x1FD7,
+		0x0050, 0x1F57, 0x003D, 0x0531, 0x03D1, 0x1F4F, 0x1FCB,
+		0x0050, 0x1F62, 0x001E, 0x0516, 0x03FB, 0x1F5F, 0x1FC0,
+		0x004F, 0x1F6D, 0x0000, 0x04FA, 0x0425, 0x1F71, 0x1FB4,
+		0x004E, 0x1F79, 0x1FE4, 0x04DC, 0x044D, 0x1F84, 0x1FA8,
+		0x004D, 0x1F84, 0x1FCA, 0x04BC, 0x0474, 0x1F99, 0x1F9C,
+		0x1F8C, 0x1FAE, 0x04C6, 0x04C6, 0x1FAE, 0x1F8C, 0x0000,
+		0x1F9C, 0x1F99, 0x0474, 0x04BC, 0x1FCA, 0x1F84, 0x004D,
+		0x1FA8, 0x1F84, 0x044D, 0x04DC, 0x1FE4, 0x1F79, 0x004E,
+		0x1FB4, 0x1F71, 0x0425, 0x04FA, 0x0000, 0x1F6D, 0x004F,
+		0x1FC0, 0x1F5F, 0x03FB, 0x0516, 0x001E, 0x1F62, 0x0050,
+		0x1FCB, 0x1F4F, 0x03D1, 0x0531, 0x003D, 0x1F57, 0x0050,
+		0x1FD7, 0x1F41, 0x03A5, 0x0548, 0x005E, 0x1F4D, 0x0050,
+		0x1FE2, 0x1F34, 0x037A, 0x055E, 0x0080, 0x1F43, 0x004F,
+		0x1FEC, 0x1F2A, 0x034D, 0x0572, 0x00A4, 0x1F39, 0x004E,
+		0x1FF6, 0x1F20, 0x0321, 0x0584, 0x00C9, 0x1F30, 0x004C,
+		0x0000, 0x1F19, 0x02F4, 0x0593, 0x00EF, 0x1F28, 0x0049,
+		0x0009, 0x1F13, 0x02C6, 0x05A1, 0x0116, 0x1F21, 0x0046,
+		0x0012, 0x1F0F, 0x0299, 0x05AA, 0x013E, 0x1F1B, 0x0043,
+		0x001A, 0x1F0C, 0x026C, 0x05B3, 0x0167, 0x1F15, 0x003F,
+		0x0021, 0x1F0A, 0x023F, 0x05BA, 0x0191, 0x1F11, 0x003A,
+		0x0028, 0x1F0A, 0x0213, 0x05BD, 0x01BC, 0x1F0D, 0x0035,
+	},
+	[HS_LT_15_16_SCALE] = {
+		/* Luma */
+		0x005B, 0x1F0A, 0x0195, 0x060C, 0x0195, 0x1F0A, 0x005B,
+		0x005D, 0x1F13, 0x0166, 0x0609, 0x01C6, 0x1F03, 0x0058,
+		0x005F, 0x1F1C, 0x0138, 0x0605, 0x01F7, 0x1EFD, 0x0054,
+		0x0060, 0x1F26, 0x010B, 0x05FF, 0x0229, 0x1EF8, 0x004F,
+		0x0060, 0x1F31, 0x00DF, 0x05F5, 0x025C, 0x1EF5, 0x004A,
+		0x0060, 0x1F3D, 0x00B5, 0x05E8, 0x028F, 0x1EF3, 0x0044,
+		0x005F, 0x1F49, 0x008C, 0x05DA, 0x02C3, 0x1EF2, 0x003D,
+		0x005E, 0x1F56, 0x0065, 0x05C7, 0x02F6, 0x1EF4, 0x0036,
+		0x005C, 0x1F63, 0x003F, 0x05B3, 0x032B, 0x1EF7, 0x002D,
+		0x0059, 0x1F71, 0x001B, 0x059D, 0x035F, 0x1EFB, 0x0024,
+		0x0057, 0x1F7F, 0x1FF9, 0x0583, 0x0392, 0x1F02, 0x001A,
+		0x0053, 0x1F8D, 0x1FD9, 0x0567, 0x03C5, 0x1F0B, 0x0010,
+		0x0050, 0x1F9B, 0x1FBB, 0x0548, 0x03F8, 0x1F15, 0x0005,
+		0x004C, 0x1FA9, 0x1F9E, 0x0528, 0x042A, 0x1F22, 0x1FF9,
+		0x0048, 0x1FB7, 0x1F84, 0x0505, 0x045A, 0x1F31, 0x1FED,
+		0x0043, 0x1FC5, 0x1F6C, 0x04E0, 0x048A, 0x1F42, 0x1FE0,
+		0x1FD1, 0x1F50, 0x04DF, 0x04DF, 0x1F50, 0x1FD1, 0x0000,
+		0x1FE0, 0x1F42, 0x048A, 0x04E0, 0x1F6C, 0x1FC5, 0x0043,
+		0x1FED, 0x1F31, 0x045A, 0x0505, 0x1F84, 0x1FB7, 0x0048,
+		0x1FF9, 0x1F22, 0x042A, 0x0528, 0x1F9E, 0x1FA9, 0x004C,
+		0x0005, 0x1F15, 0x03F8, 0x0548, 0x1FBB, 0x1F9B, 0x0050,
+		0x0010, 0x1F0B, 0x03C5, 0x0567, 0x1FD9, 0x1F8D, 0x0053,
+		0x001A, 0x1F02, 0x0392, 0x0583, 0x1FF9, 0x1F7F, 0x0057,
+		0x0024, 0x1EFB, 0x035F, 0x059D, 0x001B, 0x1F71, 0x0059,
+		0x002D, 0x1EF7, 0x032B, 0x05B3, 0x003F, 0x1F63, 0x005C,
+		0x0036, 0x1EF4, 0x02F6, 0x05C7, 0x0065, 0x1F56, 0x005E,
+		0x003D, 0x1EF2, 0x02C3, 0x05DA, 0x008C, 0x1F49, 0x005F,
+		0x0044, 0x1EF3, 0x028F, 0x05E8, 0x00B5, 0x1F3D, 0x0060,
+		0x004A, 0x1EF5, 0x025C, 0x05F5, 0x00DF, 0x1F31, 0x0060,
+		0x004F, 0x1EF8, 0x0229, 0x05FF, 0x010B, 0x1F26, 0x0060,
+		0x0054, 0x1EFD, 0x01F7, 0x0605, 0x0138, 0x1F1C, 0x005F,
+		0x0058, 0x1F03, 0x01C6, 0x0609, 0x0166, 0x1F13, 0x005D,
+		/* Chroma */
+		0x005B, 0x1F0A, 0x0195, 0x060C, 0x0195, 0x1F0A, 0x005B,
+		0x005D, 0x1F13, 0x0166, 0x0609, 0x01C6, 0x1F03, 0x0058,
+		0x005F, 0x1F1C, 0x0138, 0x0605, 0x01F7, 0x1EFD, 0x0054,
+		0x0060, 0x1F26, 0x010B, 0x05FF, 0x0229, 0x1EF8, 0x004F,
+		0x0060, 0x1F31, 0x00DF, 0x05F5, 0x025C, 0x1EF5, 0x004A,
+		0x0060, 0x1F3D, 0x00B5, 0x05E8, 0x028F, 0x1EF3, 0x0044,
+		0x005F, 0x1F49, 0x008C, 0x05DA, 0x02C3, 0x1EF2, 0x003D,
+		0x005E, 0x1F56, 0x0065, 0x05C7, 0x02F6, 0x1EF4, 0x0036,
+		0x005C, 0x1F63, 0x003F, 0x05B3, 0x032B, 0x1EF7, 0x002D,
+		0x0059, 0x1F71, 0x001B, 0x059D, 0x035F, 0x1EFB, 0x0024,
+		0x0057, 0x1F7F, 0x1FF9, 0x0583, 0x0392, 0x1F02, 0x001A,
+		0x0053, 0x1F8D, 0x1FD9, 0x0567, 0x03C5, 0x1F0B, 0x0010,
+		0x0050, 0x1F9B, 0x1FBB, 0x0548, 0x03F8, 0x1F15, 0x0005,
+		0x004C, 0x1FA9, 0x1F9E, 0x0528, 0x042A, 0x1F22, 0x1FF9,
+		0x0048, 0x1FB7, 0x1F84, 0x0505, 0x045A, 0x1F31, 0x1FED,
+		0x0043, 0x1FC5, 0x1F6C, 0x04E0, 0x048A, 0x1F42, 0x1FE0,
+		0x1FD1, 0x1F50, 0x04DF, 0x04DF, 0x1F50, 0x1FD1, 0x0000,
+		0x1FE0, 0x1F42, 0x048A, 0x04E0, 0x1F6C, 0x1FC5, 0x0043,
+		0x1FED, 0x1F31, 0x045A, 0x0505, 0x1F84, 0x1FB7, 0x0048,
+		0x1FF9, 0x1F22, 0x042A, 0x0528, 0x1F9E, 0x1FA9, 0x004C,
+		0x0005, 0x1F15, 0x03F8, 0x0548, 0x1FBB, 0x1F9B, 0x0050,
+		0x0010, 0x1F0B, 0x03C5, 0x0567, 0x1FD9, 0x1F8D, 0x0053,
+		0x001A, 0x1F02, 0x0392, 0x0583, 0x1FF9, 0x1F7F, 0x0057,
+		0x0024, 0x1EFB, 0x035F, 0x059D, 0x001B, 0x1F71, 0x0059,
+		0x002D, 0x1EF7, 0x032B, 0x05B3, 0x003F, 0x1F63, 0x005C,
+		0x0036, 0x1EF4, 0x02F6, 0x05C7, 0x0065, 0x1F56, 0x005E,
+		0x003D, 0x1EF2, 0x02C3, 0x05DA, 0x008C, 0x1F49, 0x005F,
+		0x0044, 0x1EF3, 0x028F, 0x05E8, 0x00B5, 0x1F3D, 0x0060,
+		0x004A, 0x1EF5, 0x025C, 0x05F5, 0x00DF, 0x1F31, 0x0060,
+		0x004F, 0x1EF8, 0x0229, 0x05FF, 0x010B, 0x1F26, 0x0060,
+		0x0054, 0x1EFD, 0x01F7, 0x0605, 0x0138, 0x1F1C, 0x005F,
+		0x0058, 0x1F03, 0x01C6, 0x0609, 0x0166, 0x1F13, 0x005D,
+	},
+	[HS_LE_16_16_SCALE] = {
+		/* Luma */
+		0x006E, 0x1F24, 0x013E, 0x0660, 0x013E, 0x1F24, 0x006E,
+		0x006C, 0x1F33, 0x010B, 0x065D, 0x0172, 0x1F17, 0x0070,
+		0x0069, 0x1F41, 0x00DA, 0x0659, 0x01A8, 0x1F0B, 0x0070,
+		0x0066, 0x1F51, 0x00AA, 0x0650, 0x01DF, 0x1F00, 0x0070,
+		0x0062, 0x1F61, 0x007D, 0x0644, 0x0217, 0x1EF6, 0x006F,
+		0x005E, 0x1F71, 0x0051, 0x0636, 0x0250, 0x1EED, 0x006D,
+		0x0059, 0x1F81, 0x0028, 0x0624, 0x028A, 0x1EE5, 0x006B,
+		0x0054, 0x1F91, 0x0000, 0x060F, 0x02C5, 0x1EE0, 0x0067,
+		0x004E, 0x1FA2, 0x1FDB, 0x05F6, 0x0300, 0x1EDC, 0x0063,
+		0x0049, 0x1FB2, 0x1FB8, 0x05DB, 0x033B, 0x1EDA, 0x005D,
+		0x0043, 0x1FC3, 0x1F98, 0x05BC, 0x0376, 0x1ED9, 0x0057,
+		0x003D, 0x1FD3, 0x1F7A, 0x059B, 0x03B1, 0x1EDB, 0x004F,
+		0x0036, 0x1FE2, 0x1F5E, 0x0578, 0x03EC, 0x1EDF, 0x0047,
+		0x0030, 0x1FF1, 0x1F45, 0x0551, 0x0426, 0x1EE6, 0x003D,
+		0x002A, 0x0000, 0x1F2E, 0x0528, 0x045F, 0x1EEE, 0x0033,
+		0x0023, 0x000E, 0x1F19, 0x04FD, 0x0498, 0x1EFA, 0x0027,
+		0x001B, 0x1F04, 0x04E1, 0x04E1, 0x1F04, 0x001B, 0x0000,
+		0x0027, 0x1EFA, 0x0498, 0x04FD, 0x1F19, 0x000E, 0x0023,
+		0x0033, 0x1EEE, 0x045F, 0x0528, 0x1F2E, 0x0000, 0x002A,
+		0x003D, 0x1EE6, 0x0426, 0x0551, 0x1F45, 0x1FF1, 0x0030,
+		0x0047, 0x1EDF, 0x03EC, 0x0578, 0x1F5E, 0x1FE2, 0x0036,
+		0x004F, 0x1EDB, 0x03B1, 0x059B, 0x1F7A, 0x1FD3, 0x003D,
+		0x0057, 0x1ED9, 0x0376, 0x05BC, 0x1F98, 0x1FC3, 0x0043,
+		0x005D, 0x1EDA, 0x033B, 0x05DB, 0x1FB8, 0x1FB2, 0x0049,
+		0x0063, 0x1EDC, 0x0300, 0x05F6, 0x1FDB, 0x1FA2, 0x004E,
+		0x0067, 0x1EE0, 0x02C5, 0x060F, 0x0000, 0x1F91, 0x0054,
+		0x006B, 0x1EE5, 0x028A, 0x0624, 0x0028, 0x1F81, 0x0059,
+		0x006D, 0x1EED, 0x0250, 0x0636, 0x0051, 0x1F71, 0x005E,
+		0x006F, 0x1EF6, 0x0217, 0x0644, 0x007D, 0x1F61, 0x0062,
+		0x0070, 0x1F00, 0x01DF, 0x0650, 0x00AA, 0x1F51, 0x0066,
+		0x0070, 0x1F0B, 0x01A8, 0x0659, 0x00DA, 0x1F41, 0x0069,
+		0x0070, 0x1F17, 0x0172, 0x065D, 0x010B, 0x1F33, 0x006C,
+		/* Chroma */
+		0x006E, 0x1F24, 0x013E, 0x0660, 0x013E, 0x1F24, 0x006E,
+		0x006C, 0x1F33, 0x010B, 0x065D, 0x0172, 0x1F17, 0x0070,
+		0x0069, 0x1F41, 0x00DA, 0x0659, 0x01A8, 0x1F0B, 0x0070,
+		0x0066, 0x1F51, 0x00AA, 0x0650, 0x01DF, 0x1F00, 0x0070,
+		0x0062, 0x1F61, 0x007D, 0x0644, 0x0217, 0x1EF6, 0x006F,
+		0x005E, 0x1F71, 0x0051, 0x0636, 0x0250, 0x1EED, 0x006D,
+		0x0059, 0x1F81, 0x0028, 0x0624, 0x028A, 0x1EE5, 0x006B,
+		0x0054, 0x1F91, 0x0000, 0x060F, 0x02C5, 0x1EE0, 0x0067,
+		0x004E, 0x1FA2, 0x1FDB, 0x05F6, 0x0300, 0x1EDC, 0x0063,
+		0x0049, 0x1FB2, 0x1FB8, 0x05DB, 0x033B, 0x1EDA, 0x005D,
+		0x0043, 0x1FC3, 0x1F98, 0x05BC, 0x0376, 0x1ED9, 0x0057,
+		0x003D, 0x1FD3, 0x1F7A, 0x059B, 0x03B1, 0x1EDB, 0x004F,
+		0x0036, 0x1FE2, 0x1F5E, 0x0578, 0x03EC, 0x1EDF, 0x0047,
+		0x0030, 0x1FF1, 0x1F45, 0x0551, 0x0426, 0x1EE6, 0x003D,
+		0x002A, 0x0000, 0x1F2E, 0x0528, 0x045F, 0x1EEE, 0x0033,
+		0x0023, 0x000E, 0x1F19, 0x04FD, 0x0498, 0x1EFA, 0x0027,
+		0x001B, 0x1F04, 0x04E1, 0x04E1, 0x1F04, 0x001B, 0x0000,
+		0x0027, 0x1EFA, 0x0498, 0x04FD, 0x1F19, 0x000E, 0x0023,
+		0x0033, 0x1EEE, 0x045F, 0x0528, 0x1F2E, 0x0000, 0x002A,
+		0x003D, 0x1EE6, 0x0426, 0x0551, 0x1F45, 0x1FF1, 0x0030,
+		0x0047, 0x1EDF, 0x03EC, 0x0578, 0x1F5E, 0x1FE2, 0x0036,
+		0x004F, 0x1EDB, 0x03B1, 0x059B, 0x1F7A, 0x1FD3, 0x003D,
+		0x0057, 0x1ED9, 0x0376, 0x05BC, 0x1F98, 0x1FC3, 0x0043,
+		0x005D, 0x1EDA, 0x033B, 0x05DB, 0x1FB8, 0x1FB2, 0x0049,
+		0x0063, 0x1EDC, 0x0300, 0x05F6, 0x1FDB, 0x1FA2, 0x004E,
+		0x0067, 0x1EE0, 0x02C5, 0x060F, 0x0000, 0x1F91, 0x0054,
+		0x006B, 0x1EE5, 0x028A, 0x0624, 0x0028, 0x1F81, 0x0059,
+		0x006D, 0x1EED, 0x0250, 0x0636, 0x0051, 0x1F71, 0x005E,
+		0x006F, 0x1EF6, 0x0217, 0x0644, 0x007D, 0x1F61, 0x0062,
+		0x0070, 0x1F00, 0x01DF, 0x0650, 0x00AA, 0x1F51, 0x0066,
+		0x0070, 0x1F0B, 0x01A8, 0x0659, 0x00DA, 0x1F41, 0x0069,
+		0x0070, 0x1F17, 0x0172, 0x065D, 0x010B, 0x1F33, 0x006C,
+	},
+};
+
+/* vertical scaler coefficients */
+enum {
+	VS_UP_SCALE = 0,
+	VS_LT_9_16_SCALE,
+	VS_LT_10_16_SCALE,
+	VS_LT_11_16_SCALE,
+	VS_LT_12_16_SCALE,
+	VS_LT_13_16_SCALE,
+	VS_LT_14_16_SCALE,
+	VS_LT_15_16_SCALE,
+	VS_LT_16_16_SCALE,
+	VS_1_TO_1_SCALE,
+};
+
+static const u16 scaler_vs_coeffs[15][SC_NUM_PHASES * 2 * SC_V_NUM_TAPS] = {
+	[VS_UP_SCALE] = {
+		/* Luma */
+		0x1FD1, 0x00B1, 0x06FC, 0x00B1, 0x1FD1,
+		0x1FD8, 0x0085, 0x06F9, 0x00E1, 0x1FC9,
+		0x1FDF, 0x005B, 0x06F2, 0x0114, 0x1FC0,
+		0x1FE5, 0x0035, 0x06E5, 0x014A, 0x1FB7,
+		0x1FEB, 0x0012, 0x06D3, 0x0182, 0x1FAE,
+		0x1FF1, 0x1FF3, 0x06BA, 0x01BD, 0x1FA5,
+		0x1FF5, 0x1FD7, 0x069D, 0x01FB, 0x1F9C,
+		0x1FF9, 0x1FBE, 0x067C, 0x023A, 0x1F93,
+		0x1FFD, 0x1FA8, 0x0656, 0x027B, 0x1F8A,
+		0x0000, 0x1F95, 0x062B, 0x02BF, 0x1F81,
+		0x0002, 0x1F86, 0x05FC, 0x0303, 0x1F79,
+		0x0004, 0x1F79, 0x05CA, 0x0347, 0x1F72,
+		0x0005, 0x1F6F, 0x0594, 0x038D, 0x1F6B,
+		0x0006, 0x1F67, 0x055B, 0x03D2, 0x1F66,
+		0x0007, 0x1F62, 0x051E, 0x0417, 0x1F62,
+		0x0007, 0x1F5F, 0x04DF, 0x045C, 0x1F5F,
+		0x1F5E, 0x04A2, 0x04A2, 0x1F5E, 0x0000,
+		0x1F5F, 0x045C, 0x04DF, 0x1F5F, 0x0007,
+		0x1F62, 0x0417, 0x051E, 0x1F62, 0x0007,
+		0x1F66, 0x03D2, 0x055B, 0x1F67, 0x0006,
+		0x1F6B, 0x038D, 0x0594, 0x1F6F, 0x0005,
+		0x1F72, 0x0347, 0x05CA, 0x1F79, 0x0004,
+		0x1F79, 0x0303, 0x05FC, 0x1F86, 0x0002,
+		0x1F81, 0x02BF, 0x062B, 0x1F95, 0x0000,
+		0x1F8A, 0x027B, 0x0656, 0x1FA8, 0x1FFD,
+		0x1F93, 0x023A, 0x067C, 0x1FBE, 0x1FF9,
+		0x1F9C, 0x01FB, 0x069D, 0x1FD7, 0x1FF5,
+		0x1FA5, 0x01BD, 0x06BA, 0x1FF3, 0x1FF1,
+		0x1FAE, 0x0182, 0x06D3, 0x0012, 0x1FEB,
+		0x1FB7, 0x014A, 0x06E5, 0x0035, 0x1FE5,
+		0x1FC0, 0x0114, 0x06F2, 0x005B, 0x1FDF,
+		0x1FC9, 0x00E1, 0x06F9, 0x0085, 0x1FD8,
+		/* Chroma */
+		0x1FD1, 0x00B1, 0x06FC, 0x00B1, 0x1FD1,
+		0x1FD8, 0x0085, 0x06F9, 0x00E1, 0x1FC9,
+		0x1FDF, 0x005B, 0x06F2, 0x0114, 0x1FC0,
+		0x1FE5, 0x0035, 0x06E5, 0x014A, 0x1FB7,
+		0x1FEB, 0x0012, 0x06D3, 0x0182, 0x1FAE,
+		0x1FF1, 0x1FF3, 0x06BA, 0x01BD, 0x1FA5,
+		0x1FF5, 0x1FD7, 0x069D, 0x01FB, 0x1F9C,
+		0x1FF9, 0x1FBE, 0x067C, 0x023A, 0x1F93,
+		0x1FFD, 0x1FA8, 0x0656, 0x027B, 0x1F8A,
+		0x0000, 0x1F95, 0x062B, 0x02BF, 0x1F81,
+		0x0002, 0x1F86, 0x05FC, 0x0303, 0x1F79,
+		0x0004, 0x1F79, 0x05CA, 0x0347, 0x1F72,
+		0x0005, 0x1F6F, 0x0594, 0x038D, 0x1F6B,
+		0x0006, 0x1F67, 0x055B, 0x03D2, 0x1F66,
+		0x0007, 0x1F62, 0x051E, 0x0417, 0x1F62,
+		0x0007, 0x1F5F, 0x04DF, 0x045C, 0x1F5F,
+		0x1F5E, 0x04A2, 0x04A2, 0x1F5E, 0x0000,
+		0x1F5F, 0x045C, 0x04DF, 0x1F5F, 0x0007,
+		0x1F62, 0x0417, 0x051E, 0x1F62, 0x0007,
+		0x1F66, 0x03D2, 0x055B, 0x1F67, 0x0006,
+		0x1F6B, 0x038D, 0x0594, 0x1F6F, 0x0005,
+		0x1F72, 0x0347, 0x05CA, 0x1F79, 0x0004,
+		0x1F79, 0x0303, 0x05FC, 0x1F86, 0x0002,
+		0x1F81, 0x02BF, 0x062B, 0x1F95, 0x0000,
+		0x1F8A, 0x027B, 0x0656, 0x1FA8, 0x1FFD,
+		0x1F93, 0x023A, 0x067C, 0x1FBE, 0x1FF9,
+		0x1F9C, 0x01FB, 0x069D, 0x1FD7, 0x1FF5,
+		0x1FA5, 0x01BD, 0x06BA, 0x1FF3, 0x1FF1,
+		0x1FAE, 0x0182, 0x06D3, 0x0012, 0x1FEB,
+		0x1FB7, 0x014A, 0x06E5, 0x0035, 0x1FE5,
+		0x1FC0, 0x0114, 0x06F2, 0x005B, 0x1FDF,
+		0x1FC9, 0x00E1, 0x06F9, 0x0085, 0x1FD8,
+	},
+	[VS_LT_9_16_SCALE] = {
+		/* Luma */
+		0x001C, 0x01F6, 0x03DC, 0x01F6, 0x001C,
+		0x0018, 0x01DF, 0x03DB, 0x020C, 0x0022,
+		0x0013, 0x01C9, 0x03D9, 0x0223, 0x0028,
+		0x000F, 0x01B3, 0x03D6, 0x023A, 0x002E,
+		0x000C, 0x019D, 0x03D2, 0x0250, 0x0035,
+		0x0009, 0x0188, 0x03CC, 0x0266, 0x003D,
+		0x0006, 0x0173, 0x03C5, 0x027D, 0x0045,
+		0x0004, 0x015E, 0x03BD, 0x0293, 0x004E,
+		0x0002, 0x014A, 0x03B4, 0x02A8, 0x0058,
+		0x0000, 0x0136, 0x03AA, 0x02BE, 0x0062,
+		0x1FFF, 0x0123, 0x039E, 0x02D3, 0x006D,
+		0x1FFE, 0x0110, 0x0392, 0x02E8, 0x0078,
+		0x1FFD, 0x00FE, 0x0384, 0x02FC, 0x0085,
+		0x1FFD, 0x00ED, 0x0376, 0x030F, 0x0091,
+		0x1FFC, 0x00DC, 0x0367, 0x0322, 0x009F,
+		0x1FFC, 0x00CC, 0x0357, 0x0334, 0x00AD,
+		0x00BC, 0x0344, 0x0344, 0x00BC, 0x0000,
+		0x00AD, 0x0334, 0x0357, 0x00CC, 0x1FFC,
+		0x009F, 0x0322, 0x0367, 0x00DC, 0x1FFC,
+		0x0091, 0x030F, 0x0376, 0x00ED, 0x1FFD,
+		0x0085, 0x02FC, 0x0384, 0x00FE, 0x1FFD,
+		0x0078, 0x02E8, 0x0392, 0x0110, 0x1FFE,
+		0x006D, 0x02D3, 0x039E, 0x0123, 0x1FFF,
+		0x0062, 0x02BE, 0x03AA, 0x0136, 0x0000,
+		0x0058, 0x02A8, 0x03B4, 0x014A, 0x0002,
+		0x004E, 0x0293, 0x03BD, 0x015E, 0x0004,
+		0x0045, 0x027D, 0x03C5, 0x0173, 0x0006,
+		0x003D, 0x0266, 0x03CC, 0x0188, 0x0009,
+		0x0035, 0x0250, 0x03D2, 0x019D, 0x000C,
+		0x002E, 0x023A, 0x03D6, 0x01B3, 0x000F,
+		0x0028, 0x0223, 0x03D9, 0x01C9, 0x0013,
+		0x0022, 0x020C, 0x03DB, 0x01DF, 0x0018,
+		/* Chroma */
+		0x001C, 0x01F6, 0x03DC, 0x01F6, 0x001C,
+		0x0018, 0x01DF, 0x03DB, 0x020C, 0x0022,
+		0x0013, 0x01C9, 0x03D9, 0x0223, 0x0028,
+		0x000F, 0x01B3, 0x03D6, 0x023A, 0x002E,
+		0x000C, 0x019D, 0x03D2, 0x0250, 0x0035,
+		0x0009, 0x0188, 0x03CC, 0x0266, 0x003D,
+		0x0006, 0x0173, 0x03C5, 0x027D, 0x0045,
+		0x0004, 0x015E, 0x03BD, 0x0293, 0x004E,
+		0x0002, 0x014A, 0x03B4, 0x02A8, 0x0058,
+		0x0000, 0x0136, 0x03AA, 0x02BE, 0x0062,
+		0x1FFF, 0x0123, 0x039E, 0x02D3, 0x006D,
+		0x1FFE, 0x0110, 0x0392, 0x02E8, 0x0078,
+		0x1FFD, 0x00FE, 0x0384, 0x02FC, 0x0085,
+		0x1FFD, 0x00ED, 0x0376, 0x030F, 0x0091,
+		0x1FFC, 0x00DC, 0x0367, 0x0322, 0x009F,
+		0x1FFC, 0x00CC, 0x0357, 0x0334, 0x00AD,
+		0x00BC, 0x0344, 0x0344, 0x00BC, 0x0000,
+		0x00AD, 0x0334, 0x0357, 0x00CC, 0x1FFC,
+		0x009F, 0x0322, 0x0367, 0x00DC, 0x1FFC,
+		0x0091, 0x030F, 0x0376, 0x00ED, 0x1FFD,
+		0x0085, 0x02FC, 0x0384, 0x00FE, 0x1FFD,
+		0x0078, 0x02E8, 0x0392, 0x0110, 0x1FFE,
+		0x006D, 0x02D3, 0x039E, 0x0123, 0x1FFF,
+		0x0062, 0x02BE, 0x03AA, 0x0136, 0x0000,
+		0x0058, 0x02A8, 0x03B4, 0x014A, 0x0002,
+		0x004E, 0x0293, 0x03BD, 0x015E, 0x0004,
+		0x0045, 0x027D, 0x03C5, 0x0173, 0x0006,
+		0x003D, 0x0266, 0x03CC, 0x0188, 0x0009,
+		0x0035, 0x0250, 0x03D2, 0x019D, 0x000C,
+		0x002E, 0x023A, 0x03D6, 0x01B3, 0x000F,
+		0x0028, 0x0223, 0x03D9, 0x01C9, 0x0013,
+		0x0022, 0x020C, 0x03DB, 0x01DF, 0x0018,
+	},
+	[VS_LT_10_16_SCALE] = {
+		/* Luma */
+		0x0003, 0x01E9, 0x0428, 0x01E9, 0x0003,
+		0x0000, 0x01D0, 0x0426, 0x0203, 0x0007,
+		0x1FFD, 0x01B7, 0x0424, 0x021C, 0x000C,
+		0x1FFB, 0x019E, 0x0420, 0x0236, 0x0011,
+		0x1FF9, 0x0186, 0x041A, 0x0250, 0x0017,
+		0x1FF7, 0x016E, 0x0414, 0x026A, 0x001D,
+		0x1FF6, 0x0157, 0x040B, 0x0284, 0x0024,
+		0x1FF5, 0x0140, 0x0401, 0x029E, 0x002C,
+		0x1FF4, 0x012A, 0x03F6, 0x02B7, 0x0035,
+		0x1FF4, 0x0115, 0x03E9, 0x02D0, 0x003E,
+		0x1FF4, 0x0100, 0x03DB, 0x02E9, 0x0048,
+		0x1FF4, 0x00EC, 0x03CC, 0x0301, 0x0053,
+		0x1FF4, 0x00D9, 0x03BC, 0x0318, 0x005F,
+		0x1FF5, 0x00C7, 0x03AA, 0x032F, 0x006B,
+		0x1FF6, 0x00B5, 0x0398, 0x0345, 0x0078,
+		0x1FF6, 0x00A5, 0x0384, 0x035B, 0x0086,
+		0x0094, 0x036C, 0x036C, 0x0094, 0x0000,
+		0x0086, 0x035B, 0x0384, 0x00A5, 0x1FF6,
+		0x0078, 0x0345, 0x0398, 0x00B5, 0x1FF6,
+		0x006B, 0x032F, 0x03AA, 0x00C7, 0x1FF5,
+		0x005F, 0x0318, 0x03BC, 0x00D9, 0x1FF4,
+		0x0053, 0x0301, 0x03CC, 0x00EC, 0x1FF4,
+		0x0048, 0x02E9, 0x03DB, 0x0100, 0x1FF4,
+		0x003E, 0x02D0, 0x03E9, 0x0115, 0x1FF4,
+		0x0035, 0x02B7, 0x03F6, 0x012A, 0x1FF4,
+		0x002C, 0x029E, 0x0401, 0x0140, 0x1FF5,
+		0x0024, 0x0284, 0x040B, 0x0157, 0x1FF6,
+		0x001D, 0x026A, 0x0414, 0x016E, 0x1FF7,
+		0x0017, 0x0250, 0x041A, 0x0186, 0x1FF9,
+		0x0011, 0x0236, 0x0420, 0x019E, 0x1FFB,
+		0x000C, 0x021C, 0x0424, 0x01B7, 0x1FFD,
+		0x0007, 0x0203, 0x0426, 0x01D0, 0x0000,
+		/* Chroma */
+		0x0003, 0x01E9, 0x0428, 0x01E9, 0x0003,
+		0x0000, 0x01D0, 0x0426, 0x0203, 0x0007,
+		0x1FFD, 0x01B7, 0x0424, 0x021C, 0x000C,
+		0x1FFB, 0x019E, 0x0420, 0x0236, 0x0011,
+		0x1FF9, 0x0186, 0x041A, 0x0250, 0x0017,
+		0x1FF7, 0x016E, 0x0414, 0x026A, 0x001D,
+		0x1FF6, 0x0157, 0x040B, 0x0284, 0x0024,
+		0x1FF5, 0x0140, 0x0401, 0x029E, 0x002C,
+		0x1FF4, 0x012A, 0x03F6, 0x02B7, 0x0035,
+		0x1FF4, 0x0115, 0x03E9, 0x02D0, 0x003E,
+		0x1FF4, 0x0100, 0x03DB, 0x02E9, 0x0048,
+		0x1FF4, 0x00EC, 0x03CC, 0x0301, 0x0053,
+		0x1FF4, 0x00D9, 0x03BC, 0x0318, 0x005F,
+		0x1FF5, 0x00C7, 0x03AA, 0x032F, 0x006B,
+		0x1FF6, 0x00B5, 0x0398, 0x0345, 0x0078,
+		0x1FF6, 0x00A5, 0x0384, 0x035B, 0x0086,
+		0x0094, 0x036C, 0x036C, 0x0094, 0x0000,
+		0x0086, 0x035B, 0x0384, 0x00A5, 0x1FF6,
+		0x0078, 0x0345, 0x0398, 0x00B5, 0x1FF6,
+		0x006B, 0x032F, 0x03AA, 0x00C7, 0x1FF5,
+		0x005F, 0x0318, 0x03BC, 0x00D9, 0x1FF4,
+		0x0053, 0x0301, 0x03CC, 0x00EC, 0x1FF4,
+		0x0048, 0x02E9, 0x03DB, 0x0100, 0x1FF4,
+		0x003E, 0x02D0, 0x03E9, 0x0115, 0x1FF4,
+		0x0035, 0x02B7, 0x03F6, 0x012A, 0x1FF4,
+		0x002C, 0x029E, 0x0401, 0x0140, 0x1FF5,
+		0x0024, 0x0284, 0x040B, 0x0157, 0x1FF6,
+		0x001D, 0x026A, 0x0414, 0x016E, 0x1FF7,
+		0x0017, 0x0250, 0x041A, 0x0186, 0x1FF9,
+		0x0011, 0x0236, 0x0420, 0x019E, 0x1FFB,
+		0x000C, 0x021C, 0x0424, 0x01B7, 0x1FFD,
+		0x0007, 0x0203, 0x0426, 0x01D0, 0x0000,
+	},
+	[VS_LT_11_16_SCALE] = {
+		/* Luma */
+		0x1FEC, 0x01D6, 0x047C, 0x01D6, 0x1FEC,
+		0x1FEA, 0x01BA, 0x047B, 0x01F3, 0x1FEE,
+		0x1FE9, 0x019D, 0x0478, 0x0211, 0x1FF1,
+		0x1FE8, 0x0182, 0x0473, 0x022E, 0x1FF5,
+		0x1FE8, 0x0167, 0x046C, 0x024C, 0x1FF9,
+		0x1FE8, 0x014D, 0x0464, 0x026A, 0x1FFD,
+		0x1FE8, 0x0134, 0x0459, 0x0288, 0x0003,
+		0x1FE9, 0x011B, 0x044D, 0x02A6, 0x0009,
+		0x1FE9, 0x0104, 0x0440, 0x02C3, 0x0010,
+		0x1FEA, 0x00ED, 0x0430, 0x02E1, 0x0018,
+		0x1FEB, 0x00D7, 0x0420, 0x02FD, 0x0021,
+		0x1FED, 0x00C2, 0x040D, 0x0319, 0x002B,
+		0x1FEE, 0x00AE, 0x03F9, 0x0336, 0x0035,
+		0x1FF0, 0x009C, 0x03E3, 0x0350, 0x0041,
+		0x1FF1, 0x008A, 0x03CD, 0x036B, 0x004D,
+		0x1FF3, 0x0079, 0x03B5, 0x0384, 0x005B,
+		0x0069, 0x0397, 0x0397, 0x0069, 0x0000,
+		0x005B, 0x0384, 0x03B5, 0x0079, 0x1FF3,
+		0x004D, 0x036B, 0x03CD, 0x008A, 0x1FF1,
+		0x0041, 0x0350, 0x03E3, 0x009C, 0x1FF0,
+		0x0035, 0x0336, 0x03F9, 0x00AE, 0x1FEE,
+		0x002B, 0x0319, 0x040D, 0x00C2, 0x1FED,
+		0x0021, 0x02FD, 0x0420, 0x00D7, 0x1FEB,
+		0x0018, 0x02E1, 0x0430, 0x00ED, 0x1FEA,
+		0x0010, 0x02C3, 0x0440, 0x0104, 0x1FE9,
+		0x0009, 0x02A6, 0x044D, 0x011B, 0x1FE9,
+		0x0003, 0x0288, 0x0459, 0x0134, 0x1FE8,
+		0x1FFD, 0x026A, 0x0464, 0x014D, 0x1FE8,
+		0x1FF9, 0x024C, 0x046C, 0x0167, 0x1FE8,
+		0x1FF5, 0x022E, 0x0473, 0x0182, 0x1FE8,
+		0x1FF1, 0x0211, 0x0478, 0x019D, 0x1FE9,
+		0x1FEE, 0x01F3, 0x047B, 0x01BA, 0x1FEA,
+		/* Chroma */
+		0x1FEC, 0x01D6, 0x047C, 0x01D6, 0x1FEC,
+		0x1FEA, 0x01BA, 0x047B, 0x01F3, 0x1FEE,
+		0x1FE9, 0x019D, 0x0478, 0x0211, 0x1FF1,
+		0x1FE8, 0x0182, 0x0473, 0x022E, 0x1FF5,
+		0x1FE8, 0x0167, 0x046C, 0x024C, 0x1FF9,
+		0x1FE8, 0x014D, 0x0464, 0x026A, 0x1FFD,
+		0x1FE8, 0x0134, 0x0459, 0x0288, 0x0003,
+		0x1FE9, 0x011B, 0x044D, 0x02A6, 0x0009,
+		0x1FE9, 0x0104, 0x0440, 0x02C3, 0x0010,
+		0x1FEA, 0x00ED, 0x0430, 0x02E1, 0x0018,
+		0x1FEB, 0x00D7, 0x0420, 0x02FD, 0x0021,
+		0x1FED, 0x00C2, 0x040D, 0x0319, 0x002B,
+		0x1FEE, 0x00AE, 0x03F9, 0x0336, 0x0035,
+		0x1FF0, 0x009C, 0x03E3, 0x0350, 0x0041,
+		0x1FF1, 0x008A, 0x03CD, 0x036B, 0x004D,
+		0x1FF3, 0x0079, 0x03B5, 0x0384, 0x005B,
+		0x0069, 0x0397, 0x0397, 0x0069, 0x0000,
+		0x005B, 0x0384, 0x03B5, 0x0079, 0x1FF3,
+		0x004D, 0x036B, 0x03CD, 0x008A, 0x1FF1,
+		0x0041, 0x0350, 0x03E3, 0x009C, 0x1FF0,
+		0x0035, 0x0336, 0x03F9, 0x00AE, 0x1FEE,
+		0x002B, 0x0319, 0x040D, 0x00C2, 0x1FED,
+		0x0021, 0x02FD, 0x0420, 0x00D7, 0x1FEB,
+		0x0018, 0x02E1, 0x0430, 0x00ED, 0x1FEA,
+		0x0010, 0x02C3, 0x0440, 0x0104, 0x1FE9,
+		0x0009, 0x02A6, 0x044D, 0x011B, 0x1FE9,
+		0x0003, 0x0288, 0x0459, 0x0134, 0x1FE8,
+		0x1FFD, 0x026A, 0x0464, 0x014D, 0x1FE8,
+		0x1FF9, 0x024C, 0x046C, 0x0167, 0x1FE8,
+		0x1FF5, 0x022E, 0x0473, 0x0182, 0x1FE8,
+		0x1FF1, 0x0211, 0x0478, 0x019D, 0x1FE9,
+		0x1FEE, 0x01F3, 0x047B, 0x01BA, 0x1FEA,
+	},
+	[VS_LT_12_16_SCALE] = {
+		/* Luma */
+		0x1FD8, 0x01BC, 0x04D8, 0x01BC, 0x1FD8,
+		0x1FD8, 0x019C, 0x04D8, 0x01DC, 0x1FD8,
+		0x1FD8, 0x017D, 0x04D4, 0x01FE, 0x1FD9,
+		0x1FD9, 0x015E, 0x04CF, 0x0220, 0x1FDA,
+		0x1FDB, 0x0141, 0x04C7, 0x0241, 0x1FDC,
+		0x1FDC, 0x0125, 0x04BC, 0x0264, 0x1FDF,
+		0x1FDE, 0x0109, 0x04B0, 0x0286, 0x1FE3,
+		0x1FE0, 0x00EF, 0x04A1, 0x02A9, 0x1FE7,
+		0x1FE2, 0x00D6, 0x0491, 0x02CB, 0x1FEC,
+		0x1FE4, 0x00BE, 0x047E, 0x02EE, 0x1FF2,
+		0x1FE6, 0x00A7, 0x046A, 0x030F, 0x1FFA,
+		0x1FE9, 0x0092, 0x0453, 0x0330, 0x0002,
+		0x1FEB, 0x007E, 0x043B, 0x0351, 0x000B,
+		0x1FED, 0x006B, 0x0421, 0x0372, 0x0015,
+		0x1FEF, 0x005A, 0x0406, 0x0391, 0x0020,
+		0x1FF1, 0x0049, 0x03EA, 0x03AF, 0x002D,
+		0x003A, 0x03C6, 0x03C6, 0x003A, 0x0000,
+		0x002D, 0x03AF, 0x03EA, 0x0049, 0x1FF1,
+		0x0020, 0x0391, 0x0406, 0x005A, 0x1FEF,
+		0x0015, 0x0372, 0x0421, 0x006B, 0x1FED,
+		0x000B, 0x0351, 0x043B, 0x007E, 0x1FEB,
+		0x0002, 0x0330, 0x0453, 0x0092, 0x1FE9,
+		0x1FFA, 0x030F, 0x046A, 0x00A7, 0x1FE6,
+		0x1FF2, 0x02EE, 0x047E, 0x00BE, 0x1FE4,
+		0x1FEC, 0x02CB, 0x0491, 0x00D6, 0x1FE2,
+		0x1FE7, 0x02A9, 0x04A1, 0x00EF, 0x1FE0,
+		0x1FE3, 0x0286, 0x04B0, 0x0109, 0x1FDE,
+		0x1FDF, 0x0264, 0x04BC, 0x0125, 0x1FDC,
+		0x1FDC, 0x0241, 0x04C7, 0x0141, 0x1FDB,
+		0x1FDA, 0x0220, 0x04CF, 0x015E, 0x1FD9,
+		0x1FD9, 0x01FE, 0x04D4, 0x017D, 0x1FD8,
+		0x1FD8, 0x01DC, 0x04D8, 0x019C, 0x1FD8,
+		/* Chroma */
+		0x1FD8, 0x01BC, 0x04D8, 0x01BC, 0x1FD8,
+		0x1FD8, 0x019C, 0x04D8, 0x01DC, 0x1FD8,
+		0x1FD8, 0x017D, 0x04D4, 0x01FE, 0x1FD9,
+		0x1FD9, 0x015E, 0x04CF, 0x0220, 0x1FDA,
+		0x1FDB, 0x0141, 0x04C7, 0x0241, 0x1FDC,
+		0x1FDC, 0x0125, 0x04BC, 0x0264, 0x1FDF,
+		0x1FDE, 0x0109, 0x04B0, 0x0286, 0x1FE3,
+		0x1FE0, 0x00EF, 0x04A1, 0x02A9, 0x1FE7,
+		0x1FE2, 0x00D6, 0x0491, 0x02CB, 0x1FEC,
+		0x1FE4, 0x00BE, 0x047E, 0x02EE, 0x1FF2,
+		0x1FE6, 0x00A7, 0x046A, 0x030F, 0x1FFA,
+		0x1FE9, 0x0092, 0x0453, 0x0330, 0x0002,
+		0x1FEB, 0x007E, 0x043B, 0x0351, 0x000B,
+		0x1FED, 0x006B, 0x0421, 0x0372, 0x0015,
+		0x1FEF, 0x005A, 0x0406, 0x0391, 0x0020,
+		0x1FF1, 0x0049, 0x03EA, 0x03AF, 0x002D,
+		0x003A, 0x03C6, 0x03C6, 0x003A, 0x0000,
+		0x002D, 0x03AF, 0x03EA, 0x0049, 0x1FF1,
+		0x0020, 0x0391, 0x0406, 0x005A, 0x1FEF,
+		0x0015, 0x0372, 0x0421, 0x006B, 0x1FED,
+		0x000B, 0x0351, 0x043B, 0x007E, 0x1FEB,
+		0x0002, 0x0330, 0x0453, 0x0092, 0x1FE9,
+		0x1FFA, 0x030F, 0x046A, 0x00A7, 0x1FE6,
+		0x1FF2, 0x02EE, 0x047E, 0x00BE, 0x1FE4,
+		0x1FEC, 0x02CB, 0x0491, 0x00D6, 0x1FE2,
+		0x1FE7, 0x02A9, 0x04A1, 0x00EF, 0x1FE0,
+		0x1FE3, 0x0286, 0x04B0, 0x0109, 0x1FDE,
+		0x1FDF, 0x0264, 0x04BC, 0x0125, 0x1FDC,
+		0x1FDC, 0x0241, 0x04C7, 0x0141, 0x1FDB,
+		0x1FDA, 0x0220, 0x04CF, 0x015E, 0x1FD9,
+		0x1FD9, 0x01FE, 0x04D4, 0x017D, 0x1FD8,
+		0x1FD8, 0x01DC, 0x04D8, 0x019C, 0x1FD8,
+	},
+	[VS_LT_13_16_SCALE] = {
+		/* Luma */
+		0x1FC8, 0x0199, 0x053E, 0x0199, 0x1FC8,
+		0x1FCA, 0x0175, 0x053E, 0x01BD, 0x1FC6,
+		0x1FCD, 0x0153, 0x0539, 0x01E2, 0x1FC5,
+		0x1FCF, 0x0132, 0x0532, 0x0209, 0x1FC4,
+		0x1FD2, 0x0112, 0x0529, 0x022F, 0x1FC4,
+		0x1FD5, 0x00F4, 0x051C, 0x0256, 0x1FC5,
+		0x1FD8, 0x00D7, 0x050D, 0x027E, 0x1FC6,
+		0x1FDC, 0x00BB, 0x04FB, 0x02A6, 0x1FC8,
+		0x1FDF, 0x00A1, 0x04E7, 0x02CE, 0x1FCB,
+		0x1FE2, 0x0089, 0x04D1, 0x02F5, 0x1FCF,
+		0x1FE5, 0x0072, 0x04B8, 0x031D, 0x1FD4,
+		0x1FE8, 0x005D, 0x049E, 0x0344, 0x1FD9,
+		0x1FEB, 0x0049, 0x0480, 0x036B, 0x1FE1,
+		0x1FEE, 0x0037, 0x0462, 0x0390, 0x1FE9,
+		0x1FF0, 0x0026, 0x0442, 0x03B6, 0x1FF2,
+		0x1FF2, 0x0017, 0x0420, 0x03DA, 0x1FFD,
+		0x0009, 0x03F7, 0x03F7, 0x0009, 0x0000,
+		0x1FFD, 0x03DA, 0x0420, 0x0017, 0x1FF2,
+		0x1FF2, 0x03B6, 0x0442, 0x0026, 0x1FF0,
+		0x1FE9, 0x0390, 0x0462, 0x0037, 0x1FEE,
+		0x1FE1, 0x036B, 0x0480, 0x0049, 0x1FEB,
+		0x1FD9, 0x0344, 0x049E, 0x005D, 0x1FE8,
+		0x1FD4, 0x031D, 0x04B8, 0x0072, 0x1FE5,
+		0x1FCF, 0x02F5, 0x04D1, 0x0089, 0x1FE2,
+		0x1FCB, 0x02CE, 0x04E7, 0x00A1, 0x1FDF,
+		0x1FC8, 0x02A6, 0x04FB, 0x00BB, 0x1FDC,
+		0x1FC6, 0x027E, 0x050D, 0x00D7, 0x1FD8,
+		0x1FC5, 0x0256, 0x051C, 0x00F4, 0x1FD5,
+		0x1FC4, 0x022F, 0x0529, 0x0112, 0x1FD2,
+		0x1FC4, 0x0209, 0x0532, 0x0132, 0x1FCF,
+		0x1FC5, 0x01E2, 0x0539, 0x0153, 0x1FCD,
+		0x1FC6, 0x01BD, 0x053E, 0x0175, 0x1FCA,
+		/* Chroma */
+		0x1FC8, 0x0199, 0x053E, 0x0199, 0x1FC8,
+		0x1FCA, 0x0175, 0x053E, 0x01BD, 0x1FC6,
+		0x1FCD, 0x0153, 0x0539, 0x01E2, 0x1FC5,
+		0x1FCF, 0x0132, 0x0532, 0x0209, 0x1FC4,
+		0x1FD2, 0x0112, 0x0529, 0x022F, 0x1FC4,
+		0x1FD5, 0x00F4, 0x051C, 0x0256, 0x1FC5,
+		0x1FD8, 0x00D7, 0x050D, 0x027E, 0x1FC6,
+		0x1FDC, 0x00BB, 0x04FB, 0x02A6, 0x1FC8,
+		0x1FDF, 0x00A1, 0x04E7, 0x02CE, 0x1FCB,
+		0x1FE2, 0x0089, 0x04D1, 0x02F5, 0x1FCF,
+		0x1FE5, 0x0072, 0x04B8, 0x031D, 0x1FD4,
+		0x1FE8, 0x005D, 0x049E, 0x0344, 0x1FD9,
+		0x1FEB, 0x0049, 0x0480, 0x036B, 0x1FE1,
+		0x1FEE, 0x0037, 0x0462, 0x0390, 0x1FE9,
+		0x1FF0, 0x0026, 0x0442, 0x03B6, 0x1FF2,
+		0x1FF2, 0x0017, 0x0420, 0x03DA, 0x1FFD,
+		0x0009, 0x03F7, 0x03F7, 0x0009, 0x0000,
+		0x1FFD, 0x03DA, 0x0420, 0x0017, 0x1FF2,
+		0x1FF2, 0x03B6, 0x0442, 0x0026, 0x1FF0,
+		0x1FE9, 0x0390, 0x0462, 0x0037, 0x1FEE,
+		0x1FE1, 0x036B, 0x0480, 0x0049, 0x1FEB,
+		0x1FD9, 0x0344, 0x049E, 0x005D, 0x1FE8,
+		0x1FD4, 0x031D, 0x04B8, 0x0072, 0x1FE5,
+		0x1FCF, 0x02F5, 0x04D1, 0x0089, 0x1FE2,
+		0x1FCB, 0x02CE, 0x04E7, 0x00A1, 0x1FDF,
+		0x1FC8, 0x02A6, 0x04FB, 0x00BB, 0x1FDC,
+		0x1FC6, 0x027E, 0x050D, 0x00D7, 0x1FD8,
+		0x1FC5, 0x0256, 0x051C, 0x00F4, 0x1FD5,
+		0x1FC4, 0x022F, 0x0529, 0x0112, 0x1FD2,
+		0x1FC4, 0x0209, 0x0532, 0x0132, 0x1FCF,
+		0x1FC5, 0x01E2, 0x0539, 0x0153, 0x1FCD,
+		0x1FC6, 0x01BD, 0x053E, 0x0175, 0x1FCA,
+	},
+	[VS_LT_14_16_SCALE] = {
+		/* Luma */
+		0x1FBF, 0x016C, 0x05AA, 0x016C, 0x1FBF,
+		0x1FC3, 0x0146, 0x05A8, 0x0194, 0x1FBB,
+		0x1FC7, 0x0121, 0x05A3, 0x01BD, 0x1FB8,
+		0x1FCB, 0x00FD, 0x059B, 0x01E8, 0x1FB5,
+		0x1FD0, 0x00DC, 0x058F, 0x0213, 0x1FB2,
+		0x1FD4, 0x00BC, 0x0580, 0x0240, 0x1FB0,
+		0x1FD8, 0x009E, 0x056E, 0x026D, 0x1FAF,
+		0x1FDC, 0x0082, 0x055A, 0x029A, 0x1FAE,
+		0x1FE0, 0x0067, 0x0542, 0x02C9, 0x1FAE,
+		0x1FE4, 0x004F, 0x0528, 0x02F6, 0x1FAF,
+		0x1FE8, 0x0038, 0x050A, 0x0325, 0x1FB1,
+		0x1FEB, 0x0024, 0x04EB, 0x0352, 0x1FB4,
+		0x1FEE, 0x0011, 0x04C8, 0x0380, 0x1FB9,
+		0x1FF1, 0x0000, 0x04A4, 0x03AC, 0x1FBF,
+		0x1FF4, 0x1FF1, 0x047D, 0x03D8, 0x1FC6,
+		0x1FF6, 0x1FE4, 0x0455, 0x0403, 0x1FCE,
+		0x1FD8, 0x0428, 0x0428, 0x1FD8, 0x0000,
+		0x1FCE, 0x0403, 0x0455, 0x1FE4, 0x1FF6,
+		0x1FC6, 0x03D8, 0x047D, 0x1FF1, 0x1FF4,
+		0x1FBF, 0x03AC, 0x04A4, 0x0000, 0x1FF1,
+		0x1FB9, 0x0380, 0x04C8, 0x0011, 0x1FEE,
+		0x1FB4, 0x0352, 0x04EB, 0x0024, 0x1FEB,
+		0x1FB1, 0x0325, 0x050A, 0x0038, 0x1FE8,
+		0x1FAF, 0x02F6, 0x0528, 0x004F, 0x1FE4,
+		0x1FAE, 0x02C9, 0x0542, 0x0067, 0x1FE0,
+		0x1FAE, 0x029A, 0x055A, 0x0082, 0x1FDC,
+		0x1FAF, 0x026D, 0x056E, 0x009E, 0x1FD8,
+		0x1FB0, 0x0240, 0x0580, 0x00BC, 0x1FD4,
+		0x1FB2, 0x0213, 0x058F, 0x00DC, 0x1FD0,
+		0x1FB5, 0x01E8, 0x059B, 0x00FD, 0x1FCB,
+		0x1FB8, 0x01BD, 0x05A3, 0x0121, 0x1FC7,
+		0x1FBB, 0x0194, 0x05A8, 0x0146, 0x1FC3,
+		/* Chroma */
+		0x1FBF, 0x016C, 0x05AA, 0x016C, 0x1FBF,
+		0x1FC3, 0x0146, 0x05A8, 0x0194, 0x1FBB,
+		0x1FC7, 0x0121, 0x05A3, 0x01BD, 0x1FB8,
+		0x1FCB, 0x00FD, 0x059B, 0x01E8, 0x1FB5,
+		0x1FD0, 0x00DC, 0x058F, 0x0213, 0x1FB2,
+		0x1FD4, 0x00BC, 0x0580, 0x0240, 0x1FB0,
+		0x1FD8, 0x009E, 0x056E, 0x026D, 0x1FAF,
+		0x1FDC, 0x0082, 0x055A, 0x029A, 0x1FAE,
+		0x1FE0, 0x0067, 0x0542, 0x02C9, 0x1FAE,
+		0x1FE4, 0x004F, 0x0528, 0x02F6, 0x1FAF,
+		0x1FE8, 0x0038, 0x050A, 0x0325, 0x1FB1,
+		0x1FEB, 0x0024, 0x04EB, 0x0352, 0x1FB4,
+		0x1FEE, 0x0011, 0x04C8, 0x0380, 0x1FB9,
+		0x1FF1, 0x0000, 0x04A4, 0x03AC, 0x1FBF,
+		0x1FF4, 0x1FF1, 0x047D, 0x03D8, 0x1FC6,
+		0x1FF6, 0x1FE4, 0x0455, 0x0403, 0x1FCE,
+		0x1FD8, 0x0428, 0x0428, 0x1FD8, 0x0000,
+		0x1FCE, 0x0403, 0x0455, 0x1FE4, 0x1FF6,
+		0x1FC6, 0x03D8, 0x047D, 0x1FF1, 0x1FF4,
+		0x1FBF, 0x03AC, 0x04A4, 0x0000, 0x1FF1,
+		0x1FB9, 0x0380, 0x04C8, 0x0011, 0x1FEE,
+		0x1FB4, 0x0352, 0x04EB, 0x0024, 0x1FEB,
+		0x1FB1, 0x0325, 0x050A, 0x0038, 0x1FE8,
+		0x1FAF, 0x02F6, 0x0528, 0x004F, 0x1FE4,
+		0x1FAE, 0x02C9, 0x0542, 0x0067, 0x1FE0,
+		0x1FAE, 0x029A, 0x055A, 0x0082, 0x1FDC,
+		0x1FAF, 0x026D, 0x056E, 0x009E, 0x1FD8,
+		0x1FB0, 0x0240, 0x0580, 0x00BC, 0x1FD4,
+		0x1FB2, 0x0213, 0x058F, 0x00DC, 0x1FD0,
+		0x1FB5, 0x01E8, 0x059B, 0x00FD, 0x1FCB,
+		0x1FB8, 0x01BD, 0x05A3, 0x0121, 0x1FC7,
+		0x1FBB, 0x0194, 0x05A8, 0x0146, 0x1FC3,
+	},
+	[VS_LT_15_16_SCALE] = {
+		/* Luma */
+		0x1FBD, 0x0136, 0x061A, 0x0136, 0x1FBD,
+		0x1FC3, 0x010D, 0x0617, 0x0161, 0x1FB8,
+		0x1FC9, 0x00E6, 0x0611, 0x018E, 0x1FB2,
+		0x1FCE, 0x00C1, 0x0607, 0x01BD, 0x1FAD,
+		0x1FD4, 0x009E, 0x05F9, 0x01ED, 0x1FA8,
+		0x1FD9, 0x007D, 0x05E8, 0x021F, 0x1FA3,
+		0x1FDE, 0x005E, 0x05D3, 0x0252, 0x1F9F,
+		0x1FE2, 0x0042, 0x05BC, 0x0285, 0x1F9B,
+		0x1FE7, 0x0029, 0x059F, 0x02B9, 0x1F98,
+		0x1FEA, 0x0011, 0x0580, 0x02EF, 0x1F96,
+		0x1FEE, 0x1FFC, 0x055D, 0x0324, 0x1F95,
+		0x1FF1, 0x1FE9, 0x0538, 0x0359, 0x1F95,
+		0x1FF4, 0x1FD8, 0x0510, 0x038E, 0x1F96,
+		0x1FF7, 0x1FC9, 0x04E5, 0x03C2, 0x1F99,
+		0x1FF9, 0x1FBD, 0x04B8, 0x03F5, 0x1F9D,
+		0x1FFB, 0x1FB2, 0x0489, 0x0428, 0x1FA2,
+		0x1FAA, 0x0456, 0x0456, 0x1FAA, 0x0000,
+		0x1FA2, 0x0428, 0x0489, 0x1FB2, 0x1FFB,
+		0x1F9D, 0x03F5, 0x04B8, 0x1FBD, 0x1FF9,
+		0x1F99, 0x03C2, 0x04E5, 0x1FC9, 0x1FF7,
+		0x1F96, 0x038E, 0x0510, 0x1FD8, 0x1FF4,
+		0x1F95, 0x0359, 0x0538, 0x1FE9, 0x1FF1,
+		0x1F95, 0x0324, 0x055D, 0x1FFC, 0x1FEE,
+		0x1F96, 0x02EF, 0x0580, 0x0011, 0x1FEA,
+		0x1F98, 0x02B9, 0x059F, 0x0029, 0x1FE7,
+		0x1F9B, 0x0285, 0x05BC, 0x0042, 0x1FE2,
+		0x1F9F, 0x0252, 0x05D3, 0x005E, 0x1FDE,
+		0x1FA3, 0x021F, 0x05E8, 0x007D, 0x1FD9,
+		0x1FA8, 0x01ED, 0x05F9, 0x009E, 0x1FD4,
+		0x1FAD, 0x01BD, 0x0607, 0x00C1, 0x1FCE,
+		0x1FB2, 0x018E, 0x0611, 0x00E6, 0x1FC9,
+		0x1FB8, 0x0161, 0x0617, 0x010D, 0x1FC3,
+		/* Chroma */
+		0x1FBD, 0x0136, 0x061A, 0x0136, 0x1FBD,
+		0x1FC3, 0x010D, 0x0617, 0x0161, 0x1FB8,
+		0x1FC9, 0x00E6, 0x0611, 0x018E, 0x1FB2,
+		0x1FCE, 0x00C1, 0x0607, 0x01BD, 0x1FAD,
+		0x1FD4, 0x009E, 0x05F9, 0x01ED, 0x1FA8,
+		0x1FD9, 0x007D, 0x05E8, 0x021F, 0x1FA3,
+		0x1FDE, 0x005E, 0x05D3, 0x0252, 0x1F9F,
+		0x1FE2, 0x0042, 0x05BC, 0x0285, 0x1F9B,
+		0x1FE7, 0x0029, 0x059F, 0x02B9, 0x1F98,
+		0x1FEA, 0x0011, 0x0580, 0x02EF, 0x1F96,
+		0x1FEE, 0x1FFC, 0x055D, 0x0324, 0x1F95,
+		0x1FF1, 0x1FE9, 0x0538, 0x0359, 0x1F95,
+		0x1FF4, 0x1FD8, 0x0510, 0x038E, 0x1F96,
+		0x1FF7, 0x1FC9, 0x04E5, 0x03C2, 0x1F99,
+		0x1FF9, 0x1FBD, 0x04B8, 0x03F5, 0x1F9D,
+		0x1FFB, 0x1FB2, 0x0489, 0x0428, 0x1FA2,
+		0x1FAA, 0x0456, 0x0456, 0x1FAA, 0x0000,
+		0x1FA2, 0x0428, 0x0489, 0x1FB2, 0x1FFB,
+		0x1F9D, 0x03F5, 0x04B8, 0x1FBD, 0x1FF9,
+		0x1F99, 0x03C2, 0x04E5, 0x1FC9, 0x1FF7,
+		0x1F96, 0x038E, 0x0510, 0x1FD8, 0x1FF4,
+		0x1F95, 0x0359, 0x0538, 0x1FE9, 0x1FF1,
+		0x1F95, 0x0324, 0x055D, 0x1FFC, 0x1FEE,
+		0x1F96, 0x02EF, 0x0580, 0x0011, 0x1FEA,
+		0x1F98, 0x02B9, 0x059F, 0x0029, 0x1FE7,
+		0x1F9B, 0x0285, 0x05BC, 0x0042, 0x1FE2,
+		0x1F9F, 0x0252, 0x05D3, 0x005E, 0x1FDE,
+		0x1FA3, 0x021F, 0x05E8, 0x007D, 0x1FD9,
+		0x1FA8, 0x01ED, 0x05F9, 0x009E, 0x1FD4,
+		0x1FAD, 0x01BD, 0x0607, 0x00C1, 0x1FCE,
+		0x1FB2, 0x018E, 0x0611, 0x00E6, 0x1FC9,
+		0x1FB8, 0x0161, 0x0617, 0x010D, 0x1FC3,
+	},
+	[VS_LT_16_16_SCALE] = {
+		/* Luma */
+		0x1FC3, 0x00F8, 0x068A, 0x00F8, 0x1FC3,
+		0x1FCA, 0x00CC, 0x0689, 0x0125, 0x1FBC,
+		0x1FD1, 0x00A3, 0x0681, 0x0156, 0x1FB5,
+		0x1FD7, 0x007D, 0x0676, 0x0188, 0x1FAE,
+		0x1FDD, 0x005A, 0x0666, 0x01BD, 0x1FA6,
+		0x1FE3, 0x0039, 0x0652, 0x01F3, 0x1F9F,
+		0x1FE8, 0x001B, 0x0639, 0x022C, 0x1F98,
+		0x1FEC, 0x0000, 0x061D, 0x0265, 0x1F92,
+		0x1FF0, 0x1FE8, 0x05FC, 0x02A0, 0x1F8C,
+		0x1FF4, 0x1FD2, 0x05D7, 0x02DC, 0x1F87,
+		0x1FF7, 0x1FBF, 0x05AF, 0x0319, 0x1F82,
+		0x1FFA, 0x1FAF, 0x0583, 0x0356, 0x1F7E,
+		0x1FFC, 0x1FA1, 0x0554, 0x0393, 0x1F7C,
+		0x1FFE, 0x1F95, 0x0523, 0x03CF, 0x1F7B,
+		0x0000, 0x1F8C, 0x04EE, 0x040B, 0x1F7B,
+		0x0001, 0x1F85, 0x04B8, 0x0446, 0x1F7C,
+		0x1F80, 0x0480, 0x0480, 0x1F80, 0x0000,
+		0x1F7C, 0x0446, 0x04B8, 0x1F85, 0x0001,
+		0x1F7B, 0x040B, 0x04EE, 0x1F8C, 0x0000,
+		0x1F7B, 0x03CF, 0x0523, 0x1F95, 0x1FFE,
+		0x1F7C, 0x0393, 0x0554, 0x1FA1, 0x1FFC,
+		0x1F7E, 0x0356, 0x0583, 0x1FAF, 0x1FFA,
+		0x1F82, 0x0319, 0x05AF, 0x1FBF, 0x1FF7,
+		0x1F87, 0x02DC, 0x05D7, 0x1FD2, 0x1FF4,
+		0x1F8C, 0x02A0, 0x05FC, 0x1FE8, 0x1FF0,
+		0x1F92, 0x0265, 0x061D, 0x0000, 0x1FEC,
+		0x1F98, 0x022C, 0x0639, 0x001B, 0x1FE8,
+		0x1F9F, 0x01F3, 0x0652, 0x0039, 0x1FE3,
+		0x1FA6, 0x01BD, 0x0666, 0x005A, 0x1FDD,
+		0x1FAE, 0x0188, 0x0676, 0x007D, 0x1FD7,
+		0x1FB5, 0x0156, 0x0681, 0x00A3, 0x1FD1,
+		0x1FBC, 0x0125, 0x0689, 0x00CC, 0x1FCA,
+		/* Chroma */
+		0x1FC3, 0x00F8, 0x068A, 0x00F8, 0x1FC3,
+		0x1FCA, 0x00CC, 0x0689, 0x0125, 0x1FBC,
+		0x1FD1, 0x00A3, 0x0681, 0x0156, 0x1FB5,
+		0x1FD7, 0x007D, 0x0676, 0x0188, 0x1FAE,
+		0x1FDD, 0x005A, 0x0666, 0x01BD, 0x1FA6,
+		0x1FE3, 0x0039, 0x0652, 0x01F3, 0x1F9F,
+		0x1FE8, 0x001B, 0x0639, 0x022C, 0x1F98,
+		0x1FEC, 0x0000, 0x061D, 0x0265, 0x1F92,
+		0x1FF0, 0x1FE8, 0x05FC, 0x02A0, 0x1F8C,
+		0x1FF4, 0x1FD2, 0x05D7, 0x02DC, 0x1F87,
+		0x1FF7, 0x1FBF, 0x05AF, 0x0319, 0x1F82,
+		0x1FFA, 0x1FAF, 0x0583, 0x0356, 0x1F7E,
+		0x1FFC, 0x1FA1, 0x0554, 0x0393, 0x1F7C,
+		0x1FFE, 0x1F95, 0x0523, 0x03CF, 0x1F7B,
+		0x0000, 0x1F8C, 0x04EE, 0x040B, 0x1F7B,
+		0x0001, 0x1F85, 0x04B8, 0x0446, 0x1F7C,
+		0x1F80, 0x0480, 0x0480, 0x1F80, 0x0000,
+		0x1F7C, 0x0446, 0x04B8, 0x1F85, 0x0001,
+		0x1F7B, 0x040B, 0x04EE, 0x1F8C, 0x0000,
+		0x1F7B, 0x03CF, 0x0523, 0x1F95, 0x1FFE,
+		0x1F7C, 0x0393, 0x0554, 0x1FA1, 0x1FFC,
+		0x1F7E, 0x0356, 0x0583, 0x1FAF, 0x1FFA,
+		0x1F82, 0x0319, 0x05AF, 0x1FBF, 0x1FF7,
+		0x1F87, 0x02DC, 0x05D7, 0x1FD2, 0x1FF4,
+		0x1F8C, 0x02A0, 0x05FC, 0x1FE8, 0x1FF0,
+		0x1F92, 0x0265, 0x061D, 0x0000, 0x1FEC,
+		0x1F98, 0x022C, 0x0639, 0x001B, 0x1FE8,
+		0x1F9F, 0x01F3, 0x0652, 0x0039, 0x1FE3,
+		0x1FA6, 0x01BD, 0x0666, 0x005A, 0x1FDD,
+		0x1FAE, 0x0188, 0x0676, 0x007D, 0x1FD7,
+		0x1FB5, 0x0156, 0x0681, 0x00A3, 0x1FD1,
+		0x1FBC, 0x0125, 0x0689, 0x00CC, 0x1FCA,
+	},
+	[VS_1_TO_1_SCALE] = {
+		/* Luma */
+		0x0000, 0x0000, 0x0800, 0x0000, 0x0000,
+		0x1FD8, 0x0085, 0x06F9, 0x00E1, 0x1FC9,
+		0x1FDF, 0x005B, 0x06F2, 0x0114, 0x1FC0,
+		0x1FE5, 0x0035, 0x06E5, 0x014A, 0x1FB7,
+		0x1FEB, 0x0012, 0x06D3, 0x0182, 0x1FAE,
+		0x1FF1, 0x1FF3, 0x06BA, 0x01BD, 0x1FA5,
+		0x1FF5, 0x1FD7, 0x069D, 0x01FB, 0x1F9C,
+		0x1FF9, 0x1FBE, 0x067C, 0x023A, 0x1F93,
+		0x1FFD, 0x1FA8, 0x0656, 0x027B, 0x1F8A,
+		0x0000, 0x1F95, 0x062B, 0x02BF, 0x1F81,
+		0x0002, 0x1F86, 0x05FC, 0x0303, 0x1F79,
+		0x0004, 0x1F79, 0x05CA, 0x0347, 0x1F72,
+		0x0005, 0x1F6F, 0x0594, 0x038D, 0x1F6B,
+		0x0006, 0x1F67, 0x055B, 0x03D2, 0x1F66,
+		0x0007, 0x1F62, 0x051E, 0x0417, 0x1F62,
+		0x0007, 0x1F5F, 0x04DF, 0x045C, 0x1F5F,
+		0x1F5E, 0x04A2, 0x04A2, 0x1F5E, 0x0000,
+		0x1F5F, 0x045C, 0x04DF, 0x1F5F, 0x0007,
+		0x1F62, 0x0417, 0x051E, 0x1F62, 0x0007,
+		0x1F66, 0x03D2, 0x055B, 0x1F67, 0x0006,
+		0x1F6B, 0x038D, 0x0594, 0x1F6F, 0x0005,
+		0x1F72, 0x0347, 0x05CA, 0x1F79, 0x0004,
+		0x1F79, 0x0303, 0x05FC, 0x1F86, 0x0002,
+		0x1F81, 0x02BF, 0x062B, 0x1F95, 0x0000,
+		0x1F8A, 0x027B, 0x0656, 0x1FA8, 0x1FFD,
+		0x1F93, 0x023A, 0x067C, 0x1FBE, 0x1FF9,
+		0x1F9C, 0x01FB, 0x069D, 0x1FD7, 0x1FF5,
+		0x1FA5, 0x01BD, 0x06BA, 0x1FF3, 0x1FF1,
+		0x1FAE, 0x0182, 0x06D3, 0x0012, 0x1FEB,
+		0x1FB7, 0x014A, 0x06E5, 0x0035, 0x1FE5,
+		0x1FC0, 0x0114, 0x06F2, 0x005B, 0x1FDF,
+		0x1FC9, 0x00E1, 0x06F9, 0x0085, 0x1FD8,
+		/* Chroma */
+		0x0000, 0x0000, 0x0800, 0x0000, 0x0000,
+		0x1FD8, 0x0085, 0x06F9, 0x00E1, 0x1FC9,
+		0x1FDF, 0x005B, 0x06F2, 0x0114, 0x1FC0,
+		0x1FE5, 0x0035, 0x06E5, 0x014A, 0x1FB7,
+		0x1FEB, 0x0012, 0x06D3, 0x0182, 0x1FAE,
+		0x1FF1, 0x1FF3, 0x06BA, 0x01BD, 0x1FA5,
+		0x1FF5, 0x1FD7, 0x069D, 0x01FB, 0x1F9C,
+		0x1FF9, 0x1FBE, 0x067C, 0x023A, 0x1F93,
+		0x1FFD, 0x1FA8, 0x0656, 0x027B, 0x1F8A,
+		0x0000, 0x1F95, 0x062B, 0x02BF, 0x1F81,
+		0x0002, 0x1F86, 0x05FC, 0x0303, 0x1F79,
+		0x0004, 0x1F79, 0x05CA, 0x0347, 0x1F72,
+		0x0005, 0x1F6F, 0x0594, 0x038D, 0x1F6B,
+		0x0006, 0x1F67, 0x055B, 0x03D2, 0x1F66,
+		0x0007, 0x1F62, 0x051E, 0x0417, 0x1F62,
+		0x0007, 0x1F5F, 0x04DF, 0x045C, 0x1F5F,
+		0x1F5E, 0x04A2, 0x04A2, 0x1F5E, 0x0000,
+		0x1F5F, 0x045C, 0x04DF, 0x1F5F, 0x0007,
+		0x1F62, 0x0417, 0x051E, 0x1F62, 0x0007,
+		0x1F66, 0x03D2, 0x055B, 0x1F67, 0x0006,
+		0x1F6B, 0x038D, 0x0594, 0x1F6F, 0x0005,
+		0x1F72, 0x0347, 0x05CA, 0x1F79, 0x0004,
+		0x1F79, 0x0303, 0x05FC, 0x1F86, 0x0002,
+		0x1F81, 0x02BF, 0x062B, 0x1F95, 0x0000,
+		0x1F8A, 0x027B, 0x0656, 0x1FA8, 0x1FFD,
+		0x1F93, 0x023A, 0x067C, 0x1FBE, 0x1FF9,
+		0x1F9C, 0x01FB, 0x069D, 0x1FD7, 0x1FF5,
+		0x1FA5, 0x01BD, 0x06BA, 0x1FF3, 0x1FF1,
+		0x1FAE, 0x0182, 0x06D3, 0x0012, 0x1FEB,
+		0x1FB7, 0x014A, 0x06E5, 0x0035, 0x1FE5,
+		0x1FC0, 0x0114, 0x06F2, 0x005B, 0x1FDF,
+		0x1FC9, 0x00E1, 0x06F9, 0x0085, 0x1FD8,
+	},
+};
+#endif
diff --git a/drivers/media/platform/ti-vpe/vpdma.c b/drivers/media/platform/ti-vpe/vpdma.c
index fcbe48a09cf8..e8175e7938ed 100644
--- a/drivers/media/platform/ti-vpe/vpdma.c
+++ b/drivers/media/platform/ti-vpe/vpdma.c
@@ -30,38 +30,47 @@
 
 const struct vpdma_data_format vpdma_yuv_fmts[] = {
 	[VPDMA_DATA_FMT_Y444] = {
+		.type		= VPDMA_DATA_FMT_TYPE_YUV,
 		.data_type	= DATA_TYPE_Y444,
 		.depth		= 8,
 	},
 	[VPDMA_DATA_FMT_Y422] = {
+		.type		= VPDMA_DATA_FMT_TYPE_YUV,
 		.data_type	= DATA_TYPE_Y422,
 		.depth		= 8,
 	},
 	[VPDMA_DATA_FMT_Y420] = {
+		.type		= VPDMA_DATA_FMT_TYPE_YUV,
 		.data_type	= DATA_TYPE_Y420,
 		.depth		= 8,
 	},
 	[VPDMA_DATA_FMT_C444] = {
+		.type		= VPDMA_DATA_FMT_TYPE_YUV,
 		.data_type	= DATA_TYPE_C444,
 		.depth		= 8,
 	},
 	[VPDMA_DATA_FMT_C422] = {
+		.type		= VPDMA_DATA_FMT_TYPE_YUV,
 		.data_type	= DATA_TYPE_C422,
 		.depth		= 8,
 	},
 	[VPDMA_DATA_FMT_C420] = {
+		.type		= VPDMA_DATA_FMT_TYPE_YUV,
 		.data_type	= DATA_TYPE_C420,
 		.depth		= 4,
 	},
 	[VPDMA_DATA_FMT_YC422] = {
+		.type		= VPDMA_DATA_FMT_TYPE_YUV,
 		.data_type	= DATA_TYPE_YC422,
 		.depth		= 16,
 	},
 	[VPDMA_DATA_FMT_YC444] = {
+		.type		= VPDMA_DATA_FMT_TYPE_YUV,
 		.data_type	= DATA_TYPE_YC444,
 		.depth		= 24,
 	},
 	[VPDMA_DATA_FMT_CY422] = {
+		.type		= VPDMA_DATA_FMT_TYPE_YUV,
 		.data_type	= DATA_TYPE_CY422,
 		.depth		= 16,
 	},
@@ -69,82 +78,102 @@ const struct vpdma_data_format vpdma_yuv_fmts[] = {
 
 const struct vpdma_data_format vpdma_rgb_fmts[] = {
 	[VPDMA_DATA_FMT_RGB565] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
 		.data_type	= DATA_TYPE_RGB16_565,
 		.depth		= 16,
 	},
 	[VPDMA_DATA_FMT_ARGB16_1555] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
 		.data_type	= DATA_TYPE_ARGB_1555,
 		.depth		= 16,
 	},
 	[VPDMA_DATA_FMT_ARGB16] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
 		.data_type	= DATA_TYPE_ARGB_4444,
 		.depth		= 16,
 	},
 	[VPDMA_DATA_FMT_RGBA16_5551] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
 		.data_type	= DATA_TYPE_RGBA_5551,
 		.depth		= 16,
 	},
 	[VPDMA_DATA_FMT_RGBA16] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
 		.data_type	= DATA_TYPE_RGBA_4444,
 		.depth		= 16,
 	},
 	[VPDMA_DATA_FMT_ARGB24] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
 		.data_type	= DATA_TYPE_ARGB24_6666,
 		.depth		= 24,
 	},
 	[VPDMA_DATA_FMT_RGB24] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
 		.data_type	= DATA_TYPE_RGB24_888,
 		.depth		= 24,
 	},
 	[VPDMA_DATA_FMT_ARGB32] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
 		.data_type	= DATA_TYPE_ARGB32_8888,
 		.depth		= 32,
 	},
 	[VPDMA_DATA_FMT_RGBA24] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
 		.data_type	= DATA_TYPE_RGBA24_6666,
 		.depth		= 24,
 	},
 	[VPDMA_DATA_FMT_RGBA32] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
 		.data_type	= DATA_TYPE_RGBA32_8888,
 		.depth		= 32,
 	},
 	[VPDMA_DATA_FMT_BGR565] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
 		.data_type	= DATA_TYPE_BGR16_565,
 		.depth		= 16,
 	},
 	[VPDMA_DATA_FMT_ABGR16_1555] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
 		.data_type	= DATA_TYPE_ABGR_1555,
 		.depth		= 16,
 	},
 	[VPDMA_DATA_FMT_ABGR16] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
 		.data_type	= DATA_TYPE_ABGR_4444,
 		.depth		= 16,
 	},
 	[VPDMA_DATA_FMT_BGRA16_5551] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
 		.data_type	= DATA_TYPE_BGRA_5551,
 		.depth		= 16,
 	},
 	[VPDMA_DATA_FMT_BGRA16] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
 		.data_type	= DATA_TYPE_BGRA_4444,
 		.depth		= 16,
 	},
 	[VPDMA_DATA_FMT_ABGR24] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
 		.data_type	= DATA_TYPE_ABGR24_6666,
 		.depth		= 24,
 	},
 	[VPDMA_DATA_FMT_BGR24] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
 		.data_type	= DATA_TYPE_BGR24_888,
 		.depth		= 24,
 	},
 	[VPDMA_DATA_FMT_ABGR32] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
 		.data_type	= DATA_TYPE_ABGR32_8888,
 		.depth		= 32,
 	},
 	[VPDMA_DATA_FMT_BGRA24] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
 		.data_type	= DATA_TYPE_BGRA24_6666,
 		.depth		= 24,
 	},
 	[VPDMA_DATA_FMT_BGRA32] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
 		.data_type	= DATA_TYPE_BGRA32_8888,
 		.depth		= 32,
 	},
@@ -152,6 +181,7 @@ const struct vpdma_data_format vpdma_rgb_fmts[] = {
 
 const struct vpdma_data_format vpdma_misc_fmts[] = {
 	[VPDMA_DATA_FMT_MV] = {
+		.type		= VPDMA_DATA_FMT_TYPE_MISC,
 		.data_type	= DATA_TYPE_MV,
 		.depth		= 4,
 	},
@@ -599,10 +629,11 @@ void vpdma_add_out_dtd(struct vpdma_desc_list *list, struct v4l2_rect *c_rect,
 
 	channel = next_chan = chan_info[chan].num;
 
-	if (fmt->data_type == DATA_TYPE_C420)
+	if (fmt->type == VPDMA_DATA_FMT_TYPE_YUV &&
+			fmt->data_type == DATA_TYPE_C420)
 		depth = 8;
 
-	stride = (depth * c_rect->width) >> 3;
+	stride = ALIGN((depth * c_rect->width) >> 3, VPDMA_STRIDE_ALIGN);
 	dma_addr += (c_rect->left * depth) >> 3;
 
 	dtd = list->next;
@@ -649,13 +680,14 @@ void vpdma_add_in_dtd(struct vpdma_desc_list *list, int frame_width,
 
 	channel = next_chan = chan_info[chan].num;
 
-	if (fmt->data_type == DATA_TYPE_C420) {
+	if (fmt->type == VPDMA_DATA_FMT_TYPE_YUV &&
+			fmt->data_type == DATA_TYPE_C420) {
 		height >>= 1;
 		frame_height >>= 1;
 		depth = 8;
 	}
 
-	stride = (depth * c_rect->width) >> 3;
+	stride = ALIGN((depth * c_rect->width) >> 3, VPDMA_STRIDE_ALIGN);
 	dma_addr += (c_rect->left * depth) >> 3;
 
 	dtd = list->next;
diff --git a/drivers/media/platform/ti-vpe/vpdma.h b/drivers/media/platform/ti-vpe/vpdma.h
index eaa2a71a5db9..cf40f11b3c8f 100644
--- a/drivers/media/platform/ti-vpe/vpdma.h
+++ b/drivers/media/platform/ti-vpe/vpdma.h
@@ -39,13 +39,23 @@ struct vpdma_data {
 	bool ready;
 };
 
+enum vpdma_data_format_type {
+	VPDMA_DATA_FMT_TYPE_YUV,
+	VPDMA_DATA_FMT_TYPE_RGB,
+	VPDMA_DATA_FMT_TYPE_MISC,
+};
+
 struct vpdma_data_format {
+	enum vpdma_data_format_type type;
 	int data_type;
 	u8 depth;
 };
 
 #define VPDMA_DESC_ALIGN		16	/* 16-byte descriptor alignment */
-
+#define VPDMA_STRIDE_ALIGN		16	/*
+						 * line stride of source and dest
+						 * buffers should be 16 byte aligned
+						 */
 #define VPDMA_DTD_DESC_SIZE		32	/* 8 words */
 #define VPDMA_CFD_CTD_DESC_SIZE		16	/* 4 words */
 
diff --git a/drivers/media/platform/ti-vpe/vpdma_priv.h b/drivers/media/platform/ti-vpe/vpdma_priv.h
index f0e9a8038c1b..c1a6ce1884f3 100644
--- a/drivers/media/platform/ti-vpe/vpdma_priv.h
+++ b/drivers/media/platform/ti-vpe/vpdma_priv.h
@@ -78,7 +78,7 @@
 #define DATA_TYPE_C420				0x6
 #define DATA_TYPE_YC422				0x7
 #define DATA_TYPE_YC444				0x8
-#define DATA_TYPE_CY422				0x23
+#define DATA_TYPE_CY422				0x27
 
 #define DATA_TYPE_RGB16_565			0x0
 #define DATA_TYPE_ARGB_1555			0x1
diff --git a/drivers/media/platform/ti-vpe/vpe.c b/drivers/media/platform/ti-vpe/vpe.c
index 4e58069e24ff..1296c5386231 100644
--- a/drivers/media/platform/ti-vpe/vpe.c
+++ b/drivers/media/platform/ti-vpe/vpe.c
@@ -30,6 +30,7 @@
 #include <linux/sched.h>
 #include <linux/slab.h>
 #include <linux/videodev2.h>
+#include <linux/log2.h>
 
 #include <media/v4l2-common.h>
 #include <media/v4l2-ctrls.h>
@@ -42,6 +43,8 @@
 
 #include "vpdma.h"
 #include "vpe_regs.h"
+#include "sc.h"
+#include "csc.h"
 
 #define VPE_MODULE_NAME "vpe"
 
@@ -54,10 +57,6 @@
 /* required alignments */
 #define S_ALIGN		0	/* multiple of 1 */
 #define H_ALIGN		1	/* multiple of 2 */
-#define W_ALIGN		1	/* multiple of 2 */
-
-/* multiple of 128 bits, line stride, 16 bytes */
-#define L_ALIGN		4
 
 /* flags that indicate a format can be used for capture/output */
 #define VPE_FMT_TYPE_CAPTURE	(1 << 0)
@@ -268,6 +267,38 @@ static struct vpe_fmt vpe_formats[] = {
 		.vpdma_fmt	= { &vpdma_yuv_fmts[VPDMA_DATA_FMT_CY422],
 				  },
 	},
+	{
+		.name		= "RGB888 packed",
+		.fourcc		= V4L2_PIX_FMT_RGB24,
+		.types		= VPE_FMT_TYPE_CAPTURE,
+		.coplanar	= 0,
+		.vpdma_fmt	= { &vpdma_rgb_fmts[VPDMA_DATA_FMT_RGB24],
+				  },
+	},
+	{
+		.name		= "ARGB32",
+		.fourcc		= V4L2_PIX_FMT_RGB32,
+		.types		= VPE_FMT_TYPE_CAPTURE,
+		.coplanar	= 0,
+		.vpdma_fmt	= { &vpdma_rgb_fmts[VPDMA_DATA_FMT_ARGB32],
+				  },
+	},
+	{
+		.name		= "BGR888 packed",
+		.fourcc		= V4L2_PIX_FMT_BGR24,
+		.types		= VPE_FMT_TYPE_CAPTURE,
+		.coplanar	= 0,
+		.vpdma_fmt	= { &vpdma_rgb_fmts[VPDMA_DATA_FMT_BGR24],
+				  },
+	},
+	{
+		.name		= "ABGR32",
+		.fourcc		= V4L2_PIX_FMT_BGR32,
+		.types		= VPE_FMT_TYPE_CAPTURE,
+		.coplanar	= 0,
+		.vpdma_fmt	= { &vpdma_rgb_fmts[VPDMA_DATA_FMT_ABGR32],
+				  },
+	},
 };
 
 /*
@@ -327,9 +358,12 @@ struct vpe_dev {
 
 	int			irq;
 	void __iomem		*base;
+	struct resource		*res;
 
 	struct vb2_alloc_ctx	*alloc_ctx;
 	struct vpdma_data	*vpdma;		/* vpdma data handle */
+	struct sc_data		*sc;		/* scaler data handle */
+	struct csc_data		*csc;		/* csc data handle */
 };
 
 /*
@@ -356,6 +390,8 @@ struct vpe_ctx {
 	void			*mv_buf[2];		/* virtual addrs of motion vector bufs */
 	size_t			mv_buf_size;		/* current motion vector buffer size */
 	struct vpdma_buf	mmr_adb;		/* shadow reg addr/data block */
+	struct vpdma_buf	sc_coeff_h;		/* h coeff buffer */
+	struct vpdma_buf	sc_coeff_v;		/* v coeff buffer */
 	struct vpdma_desc_list	desc_list;		/* DMA descriptor list */
 
 	bool			deinterlacing;		/* using de-interlacer */
@@ -438,14 +474,23 @@ struct vpe_mmr_adb {
 	u32			us3_regs[8];
 	struct vpdma_adb_hdr	dei_hdr;
 	u32			dei_regs[8];
-	struct vpdma_adb_hdr	sc_hdr;
-	u32			sc_regs[1];
-	u32			sc_pad[3];
+	struct vpdma_adb_hdr	sc_hdr0;
+	u32			sc_regs0[7];
+	u32			sc_pad0[1];
+	struct vpdma_adb_hdr	sc_hdr8;
+	u32			sc_regs8[6];
+	u32			sc_pad8[2];
+	struct vpdma_adb_hdr	sc_hdr17;
+	u32			sc_regs17[9];
+	u32			sc_pad17[3];
 	struct vpdma_adb_hdr	csc_hdr;
 	u32			csc_regs[6];
 	u32			csc_pad[2];
 };
 
+#define GET_OFFSET_TOP(ctx, obj, reg)	\
+	((obj)->res->start - ctx->dev->res->start + reg)
+
 #define VPE_SET_MMR_ADB_HDR(ctx, hdr, regs, offset_a)	\
 	VPDMA_SET_MMR_ADB_HDR(ctx->mmr_adb, vpe_mmr_adb, hdr, regs, offset_a)
 /*
@@ -458,8 +503,14 @@ static void init_adb_hdrs(struct vpe_ctx *ctx)
 	VPE_SET_MMR_ADB_HDR(ctx, us2_hdr, us2_regs, VPE_US2_R0);
 	VPE_SET_MMR_ADB_HDR(ctx, us3_hdr, us3_regs, VPE_US3_R0);
 	VPE_SET_MMR_ADB_HDR(ctx, dei_hdr, dei_regs, VPE_DEI_FRAME_SIZE);
-	VPE_SET_MMR_ADB_HDR(ctx, sc_hdr, sc_regs, VPE_SC_MP_SC0);
-	VPE_SET_MMR_ADB_HDR(ctx, csc_hdr, csc_regs, VPE_CSC_CSC00);
+	VPE_SET_MMR_ADB_HDR(ctx, sc_hdr0, sc_regs0,
+		GET_OFFSET_TOP(ctx, ctx->dev->sc, CFG_SC0));
+	VPE_SET_MMR_ADB_HDR(ctx, sc_hdr8, sc_regs8,
+		GET_OFFSET_TOP(ctx, ctx->dev->sc, CFG_SC8));
+	VPE_SET_MMR_ADB_HDR(ctx, sc_hdr17, sc_regs17,
+		GET_OFFSET_TOP(ctx, ctx->dev->sc, CFG_SC17));
+	VPE_SET_MMR_ADB_HDR(ctx, csc_hdr, csc_regs,
+		GET_OFFSET_TOP(ctx, ctx->dev->csc, CSC_CSC00));
 };
 
 /*
@@ -670,17 +721,20 @@ static void set_src_registers(struct vpe_ctx *ctx)
 static void set_dst_registers(struct vpe_ctx *ctx)
 {
 	struct vpe_mmr_adb *mmr_adb = ctx->mmr_adb.addr;
+	enum v4l2_colorspace clrspc = ctx->q_data[Q_DATA_DST].colorspace;
 	struct vpe_fmt *fmt = ctx->q_data[Q_DATA_DST].fmt;
 	u32 val = 0;
 
-	/* select RGB path when color space conversion is supported in future */
-	if (fmt->fourcc == V4L2_PIX_FMT_RGB24)
-		val |= VPE_RGB_OUT_SELECT | VPE_CSC_SRC_DEI_SCALER;
+	if (clrspc == V4L2_COLORSPACE_SRGB)
+		val |= VPE_RGB_OUT_SELECT;
 	else if (fmt->fourcc == V4L2_PIX_FMT_NV16)
 		val |= VPE_COLOR_SEPARATE_422;
 
-	/* The source of CHR_DS is always the scaler, whether it's used or not */
-	val |= VPE_DS_SRC_DEI_SCALER;
+	/*
+	 * the source of CHR_DS and CSC is always the scaler, irrespective of
+	 * whether it's used or not
+	 */
+	val |= VPE_DS_SRC_DEI_SCALER | VPE_CSC_SRC_DEI_SCALER;
 
 	if (fmt->fourcc != V4L2_PIX_FMT_NV12)
 		val |= VPE_DS_BYPASS;
@@ -742,28 +796,6 @@ static void set_dei_shadow_registers(struct vpe_ctx *ctx)
 	ctx->load_mmrs = true;
 }
 
-static void set_csc_coeff_bypass(struct vpe_ctx *ctx)
-{
-	struct vpe_mmr_adb *mmr_adb = ctx->mmr_adb.addr;
-	u32 *shadow_csc_reg5 = &mmr_adb->csc_regs[5];
-
-	*shadow_csc_reg5 |= VPE_CSC_BYPASS;
-
-	ctx->load_mmrs = true;
-}
-
-static void set_sc_regs_bypass(struct vpe_ctx *ctx)
-{
-	struct vpe_mmr_adb *mmr_adb = ctx->mmr_adb.addr;
-	u32 *sc_reg0 = &mmr_adb->sc_regs[0];
-	u32 val = 0;
-
-	val |= VPE_SC_BYPASS;
-	*sc_reg0 = val;
-
-	ctx->load_mmrs = true;
-}
-
 /*
  * Set the shadow registers whose values are modified when either the
  * source or destination format is changed.
@@ -772,6 +804,11 @@ static int set_srcdst_params(struct vpe_ctx *ctx)
 {
 	struct vpe_q_data *s_q_data =  &ctx->q_data[Q_DATA_SRC];
 	struct vpe_q_data *d_q_data =  &ctx->q_data[Q_DATA_DST];
+	struct vpe_mmr_adb *mmr_adb = ctx->mmr_adb.addr;
+	unsigned int src_w = s_q_data->c_rect.width;
+	unsigned int src_h = s_q_data->c_rect.height;
+	unsigned int dst_w = d_q_data->c_rect.width;
+	unsigned int dst_h = d_q_data->c_rect.height;
 	size_t mv_buf_size;
 	int ret;
 
@@ -780,12 +817,23 @@ static int set_srcdst_params(struct vpe_ctx *ctx)
 
 	if ((s_q_data->flags & Q_DATA_INTERLACED) &&
 			!(d_q_data->flags & Q_DATA_INTERLACED)) {
+		int bytes_per_line;
 		const struct vpdma_data_format *mv =
 			&vpdma_misc_fmts[VPDMA_DATA_FMT_MV];
 
+		/*
+		 * we make sure that the source image has a 16 byte aligned
+		 * stride, we need to do the same for the motion vector buffer
+		 * by aligning it's stride to the next 16 byte boundry. this
+		 * extra space will not be used by the de-interlacer, but will
+		 * ensure that vpdma operates correctly
+		 */
+		bytes_per_line = ALIGN((s_q_data->width * mv->depth) >> 3,
+					VPDMA_STRIDE_ALIGN);
+		mv_buf_size = bytes_per_line * s_q_data->height;
+
 		ctx->deinterlacing = 1;
-		mv_buf_size =
-			(s_q_data->width * s_q_data->height * mv->depth) >> 3;
+		src_h <<= 1;
 	} else {
 		ctx->deinterlacing = 0;
 		mv_buf_size = 0;
@@ -799,8 +847,16 @@ static int set_srcdst_params(struct vpe_ctx *ctx)
 
 	set_cfg_and_line_modes(ctx);
 	set_dei_regs(ctx);
-	set_csc_coeff_bypass(ctx);
-	set_sc_regs_bypass(ctx);
+
+	csc_set_coeff(ctx->dev->csc, &mmr_adb->csc_regs[0],
+		s_q_data->colorspace, d_q_data->colorspace);
+
+	sc_set_hs_coeffs(ctx->dev->sc, ctx->sc_coeff_h.addr, src_w, dst_w);
+	sc_set_vs_coeffs(ctx->dev->sc, ctx->sc_coeff_v.addr, src_h, dst_h);
+
+	sc_config_scaler(ctx->dev->sc, &mmr_adb->sc_regs0[0],
+		&mmr_adb->sc_regs8[0], &mmr_adb->sc_regs17[0],
+		src_w, src_h, dst_w, dst_h);
 
 	return 0;
 }
@@ -916,35 +972,10 @@ static void vpe_dump_regs(struct vpe_dev *dev)
 	DUMPREG(DEI_FMD_STATUS_R0);
 	DUMPREG(DEI_FMD_STATUS_R1);
 	DUMPREG(DEI_FMD_STATUS_R2);
-	DUMPREG(SC_MP_SC0);
-	DUMPREG(SC_MP_SC1);
-	DUMPREG(SC_MP_SC2);
-	DUMPREG(SC_MP_SC3);
-	DUMPREG(SC_MP_SC4);
-	DUMPREG(SC_MP_SC5);
-	DUMPREG(SC_MP_SC6);
-	DUMPREG(SC_MP_SC8);
-	DUMPREG(SC_MP_SC9);
-	DUMPREG(SC_MP_SC10);
-	DUMPREG(SC_MP_SC11);
-	DUMPREG(SC_MP_SC12);
-	DUMPREG(SC_MP_SC13);
-	DUMPREG(SC_MP_SC17);
-	DUMPREG(SC_MP_SC18);
-	DUMPREG(SC_MP_SC19);
-	DUMPREG(SC_MP_SC20);
-	DUMPREG(SC_MP_SC21);
-	DUMPREG(SC_MP_SC22);
-	DUMPREG(SC_MP_SC23);
-	DUMPREG(SC_MP_SC24);
-	DUMPREG(SC_MP_SC25);
-	DUMPREG(CSC_CSC00);
-	DUMPREG(CSC_CSC01);
-	DUMPREG(CSC_CSC02);
-	DUMPREG(CSC_CSC03);
-	DUMPREG(CSC_CSC04);
-	DUMPREG(CSC_CSC05);
 #undef DUMPREG
+
+	sc_dump_regs(dev->sc);
+	csc_dump_regs(dev->csc);
 }
 
 static void add_out_dtd(struct vpe_ctx *ctx, int port)
@@ -1053,6 +1084,7 @@ static void disable_irqs(struct vpe_ctx *ctx)
 static void device_run(void *priv)
 {
 	struct vpe_ctx *ctx = priv;
+	struct sc_data *sc = ctx->dev->sc;
 	struct vpe_q_data *d_q_data = &ctx->q_data[Q_DATA_DST];
 
 	if (ctx->deinterlacing && ctx->src_vbs[2] == NULL) {
@@ -1075,13 +1107,37 @@ static void device_run(void *priv)
 		ctx->load_mmrs = false;
 	}
 
+	if (sc->loaded_coeff_h != ctx->sc_coeff_h.dma_addr ||
+			sc->load_coeff_h) {
+		vpdma_map_desc_buf(ctx->dev->vpdma, &ctx->sc_coeff_h);
+		vpdma_add_cfd_block(&ctx->desc_list, CFD_SC_CLIENT,
+			&ctx->sc_coeff_h, 0);
+
+		sc->loaded_coeff_h = ctx->sc_coeff_h.dma_addr;
+		sc->load_coeff_h = false;
+	}
+
+	if (sc->loaded_coeff_v != ctx->sc_coeff_v.dma_addr ||
+			sc->load_coeff_v) {
+		vpdma_map_desc_buf(ctx->dev->vpdma, &ctx->sc_coeff_v);
+		vpdma_add_cfd_block(&ctx->desc_list, CFD_SC_CLIENT,
+			&ctx->sc_coeff_v, SC_COEF_SRAM_SIZE >> 4);
+
+		sc->loaded_coeff_v = ctx->sc_coeff_v.dma_addr;
+		sc->load_coeff_v = false;
+	}
+
 	/* output data descriptors */
 	if (ctx->deinterlacing)
 		add_out_dtd(ctx, VPE_PORT_MV_OUT);
 
-	add_out_dtd(ctx, VPE_PORT_LUMA_OUT);
-	if (d_q_data->fmt->coplanar)
-		add_out_dtd(ctx, VPE_PORT_CHROMA_OUT);
+	if (d_q_data->colorspace == V4L2_COLORSPACE_SRGB) {
+		add_out_dtd(ctx, VPE_PORT_RGB_OUT);
+	} else {
+		add_out_dtd(ctx, VPE_PORT_LUMA_OUT);
+		if (d_q_data->fmt->coplanar)
+			add_out_dtd(ctx, VPE_PORT_CHROMA_OUT);
+	}
 
 	/* input data descriptors */
 	if (ctx->deinterlacing) {
@@ -1117,9 +1173,16 @@ static void device_run(void *priv)
 	}
 
 	/* sync on channel control descriptors for output ports */
-	vpdma_add_sync_on_channel_ctd(&ctx->desc_list, VPE_CHAN_LUMA_OUT);
-	if (d_q_data->fmt->coplanar)
-		vpdma_add_sync_on_channel_ctd(&ctx->desc_list, VPE_CHAN_CHROMA_OUT);
+	if (d_q_data->colorspace == V4L2_COLORSPACE_SRGB) {
+		vpdma_add_sync_on_channel_ctd(&ctx->desc_list,
+			VPE_CHAN_RGB_OUT);
+	} else {
+		vpdma_add_sync_on_channel_ctd(&ctx->desc_list,
+			VPE_CHAN_LUMA_OUT);
+		if (d_q_data->fmt->coplanar)
+			vpdma_add_sync_on_channel_ctd(&ctx->desc_list,
+				VPE_CHAN_CHROMA_OUT);
+	}
 
 	if (ctx->deinterlacing)
 		vpdma_add_sync_on_channel_ctd(&ctx->desc_list, VPE_CHAN_MV_OUT);
@@ -1198,6 +1261,8 @@ static irqreturn_t vpe_irq(int irq_vpe, void *data)
 
 	vpdma_unmap_desc_buf(dev->vpdma, &ctx->desc_list.buf);
 	vpdma_unmap_desc_buf(dev->vpdma, &ctx->mmr_adb);
+	vpdma_unmap_desc_buf(dev->vpdma, &ctx->sc_coeff_h);
+	vpdma_unmap_desc_buf(dev->vpdma, &ctx->sc_coeff_v);
 
 	vpdma_reset_desc_list(&ctx->desc_list);
 
@@ -1352,7 +1417,8 @@ static int __vpe_try_fmt(struct vpe_ctx *ctx, struct v4l2_format *f,
 {
 	struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
 	struct v4l2_plane_pix_format *plane_fmt;
-	int i;
+	unsigned int w_align;
+	int i, depth, depth_bytes;
 
 	if (!fmt || !(fmt->types & type)) {
 		vpe_err(ctx->dev, "Fourcc format (0x%08x) invalid.\n",
@@ -1363,35 +1429,57 @@ static int __vpe_try_fmt(struct vpe_ctx *ctx, struct v4l2_format *f,
 	if (pix->field != V4L2_FIELD_NONE && pix->field != V4L2_FIELD_ALTERNATE)
 		pix->field = V4L2_FIELD_NONE;
 
-	v4l_bound_align_image(&pix->width, MIN_W, MAX_W, W_ALIGN,
+	depth = fmt->vpdma_fmt[VPE_LUMA]->depth;
+
+	/*
+	 * the line stride should 16 byte aligned for VPDMA to work, based on
+	 * the bytes per pixel, figure out how much the width should be aligned
+	 * to make sure line stride is 16 byte aligned
+	 */
+	depth_bytes = depth >> 3;
+
+	if (depth_bytes == 3)
+		/*
+		 * if bpp is 3(as in some RGB formats), the pixel width doesn't
+		 * really help in ensuring line stride is 16 byte aligned
+		 */
+		w_align = 4;
+	else
+		/*
+		 * for the remainder bpp(4, 2 and 1), the pixel width alignment
+		 * can ensure a line stride alignment of 16 bytes. For example,
+		 * if bpp is 2, then the line stride can be 16 byte aligned if
+		 * the width is 8 byte aligned
+		 */
+		w_align = order_base_2(VPDMA_DESC_ALIGN / depth_bytes);
+
+	v4l_bound_align_image(&pix->width, MIN_W, MAX_W, w_align,
 			      &pix->height, MIN_H, MAX_H, H_ALIGN,
 			      S_ALIGN);
 
 	pix->num_planes = fmt->coplanar ? 2 : 1;
 	pix->pixelformat = fmt->fourcc;
 
-	if (type == VPE_FMT_TYPE_CAPTURE) {
-		struct vpe_q_data *s_q_data;
-
-		/* get colorspace from the source queue */
-		s_q_data = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
-
-		pix->colorspace = s_q_data->colorspace;
-	} else {
-		if (!pix->colorspace)
-			pix->colorspace = V4L2_COLORSPACE_SMPTE240M;
+	if (!pix->colorspace) {
+		if (fmt->fourcc == V4L2_PIX_FMT_RGB24 ||
+				fmt->fourcc == V4L2_PIX_FMT_BGR24 ||
+				fmt->fourcc == V4L2_PIX_FMT_RGB32 ||
+				fmt->fourcc == V4L2_PIX_FMT_BGR32) {
+			pix->colorspace = V4L2_COLORSPACE_SRGB;
+		} else {
+			if (pix->height > 1280)	/* HD */
+				pix->colorspace = V4L2_COLORSPACE_REC709;
+			else			/* SD */
+				pix->colorspace = V4L2_COLORSPACE_SMPTE170M;
+		}
 	}
 
 	for (i = 0; i < pix->num_planes; i++) {
-		int depth;
-
 		plane_fmt = &pix->plane_fmt[i];
 		depth = fmt->vpdma_fmt[i]->depth;
 
 		if (i == VPE_LUMA)
-			plane_fmt->bytesperline =
-					round_up((pix->width * depth) >> 3,
-						1 << L_ALIGN);
+			plane_fmt->bytesperline = (pix->width * depth) >> 3;
 		else
 			plane_fmt->bytesperline = pix->width;
 
@@ -1749,6 +1837,14 @@ static int vpe_open(struct file *file)
 	if (ret != 0)
 		goto free_desc_list;
 
+	ret = vpdma_alloc_desc_buf(&ctx->sc_coeff_h, SC_COEF_SRAM_SIZE);
+	if (ret != 0)
+		goto free_mmr_adb;
+
+	ret = vpdma_alloc_desc_buf(&ctx->sc_coeff_v, SC_COEF_SRAM_SIZE);
+	if (ret != 0)
+		goto free_sc_h;
+
 	init_adb_hdrs(ctx);
 
 	v4l2_fh_init(&ctx->fh, video_devdata(file));
@@ -1770,7 +1866,7 @@ static int vpe_open(struct file *file)
 	s_q_data->height = 1080;
 	s_q_data->sizeimage[VPE_LUMA] = (s_q_data->width * s_q_data->height *
 			s_q_data->fmt->vpdma_fmt[VPE_LUMA]->depth) >> 3;
-	s_q_data->colorspace = V4L2_COLORSPACE_SMPTE240M;
+	s_q_data->colorspace = V4L2_COLORSPACE_SMPTE170M;
 	s_q_data->field = V4L2_FIELD_NONE;
 	s_q_data->c_rect.left = 0;
 	s_q_data->c_rect.top = 0;
@@ -1817,6 +1913,10 @@ static int vpe_open(struct file *file)
 exit_fh:
 	v4l2_ctrl_handler_free(hdl);
 	v4l2_fh_exit(&ctx->fh);
+	vpdma_free_desc_buf(&ctx->sc_coeff_v);
+free_sc_h:
+	vpdma_free_desc_buf(&ctx->sc_coeff_h);
+free_mmr_adb:
 	vpdma_free_desc_buf(&ctx->mmr_adb);
 free_desc_list:
 	vpdma_free_desc_list(&ctx->desc_list);
@@ -1938,12 +2038,11 @@ static int vpe_probe(struct platform_device *pdev)
 {
 	struct vpe_dev *dev;
 	struct video_device *vfd;
-	struct resource *res;
 	int ret, irq, func;
 
 	dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
-	if (IS_ERR(dev))
-		return PTR_ERR(dev);
+	if (!dev)
+		return -ENOMEM;
 
 	spin_lock_init(&dev->lock);
 
@@ -1954,16 +2053,17 @@ static int vpe_probe(struct platform_device *pdev)
 	atomic_set(&dev->num_instances, 0);
 	mutex_init(&dev->dev_mutex);
 
-	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "vpe_top");
+	dev->res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+			"vpe_top");
 	/*
 	 * HACK: we get resource info from device tree in the form of a list of
 	 * VPE sub blocks, the driver currently uses only the base of vpe_top
 	 * for register access, the driver should be changed later to access
 	 * registers based on the sub block base addresses
 	 */
-	dev->base = devm_ioremap(&pdev->dev, res->start, SZ_32K);
-	if (IS_ERR(dev->base)) {
-		ret = PTR_ERR(dev->base);
+	dev->base = devm_ioremap(&pdev->dev, dev->res->start, SZ_32K);
+	if (!dev->base) {
+		ret = -ENOMEM;
 		goto v4l2_dev_unreg;
 	}
 
@@ -2006,9 +2106,23 @@ static int vpe_probe(struct platform_device *pdev)
 
 	vpe_top_vpdma_reset(dev);
 
+	dev->sc = sc_create(pdev);
+	if (IS_ERR(dev->sc)) {
+		ret = PTR_ERR(dev->sc);
+		goto runtime_put;
+	}
+
+	dev->csc = csc_create(pdev);
+	if (IS_ERR(dev->csc)) {
+		ret = PTR_ERR(dev->csc);
+		goto runtime_put;
+	}
+
 	dev->vpdma = vpdma_create(pdev);
-	if (IS_ERR(dev->vpdma))
+	if (IS_ERR(dev->vpdma)) {
+		ret = PTR_ERR(dev->vpdma);
 		goto runtime_put;
+	}
 
 	vfd = &dev->vfd;
 	*vfd = vpe_videodev;
@@ -2081,18 +2195,7 @@ static struct platform_driver vpe_pdrv = {
 	},
 };
 
-static void __exit vpe_exit(void)
-{
-	platform_driver_unregister(&vpe_pdrv);
-}
-
-static int __init vpe_init(void)
-{
-	return platform_driver_register(&vpe_pdrv);
-}
-
-module_init(vpe_init);
-module_exit(vpe_exit);
+module_platform_driver(vpe_pdrv);
 
 MODULE_DESCRIPTION("TI VPE driver");
 MODULE_AUTHOR("Dale Farnsworth, <dale@farnsworth.org>");
diff --git a/drivers/media/platform/ti-vpe/vpe_regs.h b/drivers/media/platform/ti-vpe/vpe_regs.h
index ed214e828398..74283d79eae1 100644
--- a/drivers/media/platform/ti-vpe/vpe_regs.h
+++ b/drivers/media/platform/ti-vpe/vpe_regs.h
@@ -306,191 +306,4 @@
 #define VPE_FMD_FRAME_DIFF_MASK		0x000fffff
 #define VPE_FMD_FRAME_DIFF_SHIFT	0
 
-/* VPE scaler regs */
-#define VPE_SC_MP_SC0			0x0700
-#define VPE_INTERLACE_O			(1 << 0)
-#define VPE_LINEAR			(1 << 1)
-#define VPE_SC_BYPASS			(1 << 2)
-#define VPE_INVT_FID			(1 << 3)
-#define VPE_USE_RAV			(1 << 4)
-#define VPE_ENABLE_EV			(1 << 5)
-#define VPE_AUTO_HS			(1 << 6)
-#define VPE_DCM_2X			(1 << 7)
-#define VPE_DCM_4X			(1 << 8)
-#define VPE_HP_BYPASS			(1 << 9)
-#define VPE_INTERLACE_I			(1 << 10)
-#define VPE_ENABLE_SIN2_VER_INTP	(1 << 11)
-#define VPE_Y_PK_EN			(1 << 14)
-#define VPE_TRIM			(1 << 15)
-#define VPE_SELFGEN_FID			(1 << 16)
-
-#define VPE_SC_MP_SC1			0x0704
-#define VPE_ROW_ACC_INC_MASK		0x07ffffff
-#define VPE_ROW_ACC_INC_SHIFT		0
-
-#define VPE_SC_MP_SC2			0x0708
-#define VPE_ROW_ACC_OFFSET_MASK		0x0fffffff
-#define VPE_ROW_ACC_OFFSET_SHIFT	0
-
-#define VPE_SC_MP_SC3			0x070c
-#define VPE_ROW_ACC_OFFSET_B_MASK	0x0fffffff
-#define VPE_ROW_ACC_OFFSET_B_SHIFT	0
-
-#define VPE_SC_MP_SC4			0x0710
-#define VPE_TAR_H_MASK			0x07ff
-#define VPE_TAR_H_SHIFT			0
-#define VPE_TAR_W_MASK			0x07ff
-#define VPE_TAR_W_SHIFT			12
-#define VPE_LIN_ACC_INC_U_MASK		0x07
-#define VPE_LIN_ACC_INC_U_SHIFT		24
-#define VPE_NLIN_ACC_INIT_U_MASK	0x07
-#define VPE_NLIN_ACC_INIT_U_SHIFT	28
-
-#define VPE_SC_MP_SC5			0x0714
-#define VPE_SRC_H_MASK			0x07ff
-#define VPE_SRC_H_SHIFT			0
-#define VPE_SRC_W_MASK			0x07ff
-#define VPE_SRC_W_SHIFT			12
-#define VPE_NLIN_ACC_INC_U_MASK		0x07
-#define VPE_NLIN_ACC_INC_U_SHIFT	24
-
-#define VPE_SC_MP_SC6			0x0718
-#define VPE_ROW_ACC_INIT_RAV_MASK	0x03ff
-#define VPE_ROW_ACC_INIT_RAV_SHIFT	0
-#define VPE_ROW_ACC_INIT_RAV_B_MASK	0x03ff
-#define VPE_ROW_ACC_INIT_RAV_B_SHIFT	10
-
-#define VPE_SC_MP_SC8			0x0720
-#define VPE_NLIN_LEFT_MASK		0x07ff
-#define VPE_NLIN_LEFT_SHIFT		0
-#define VPE_NLIN_RIGHT_MASK		0x07ff
-#define VPE_NLIN_RIGHT_SHIFT		12
-
-#define VPE_SC_MP_SC9			0x0724
-#define VPE_LIN_ACC_INC			VPE_SC_MP_SC9
-
-#define VPE_SC_MP_SC10			0x0728
-#define VPE_NLIN_ACC_INIT		VPE_SC_MP_SC10
-
-#define VPE_SC_MP_SC11			0x072c
-#define VPE_NLIN_ACC_INC		VPE_SC_MP_SC11
-
-#define VPE_SC_MP_SC12			0x0730
-#define VPE_COL_ACC_OFFSET_MASK		0x01ffffff
-#define VPE_COL_ACC_OFFSET_SHIFT	0
-
-#define VPE_SC_MP_SC13			0x0734
-#define VPE_SC_FACTOR_RAV_MASK		0x03ff
-#define VPE_SC_FACTOR_RAV_SHIFT		0
-#define VPE_CHROMA_INTP_THR_MASK	0x03ff
-#define VPE_CHROMA_INTP_THR_SHIFT	12
-#define VPE_DELTA_CHROMA_THR_MASK	0x0f
-#define VPE_DELTA_CHROMA_THR_SHIFT	24
-
-#define VPE_SC_MP_SC17			0x0744
-#define VPE_EV_THR_MASK			0x03ff
-#define VPE_EV_THR_SHIFT		12
-#define VPE_DELTA_LUMA_THR_MASK		0x0f
-#define VPE_DELTA_LUMA_THR_SHIFT	24
-#define VPE_DELTA_EV_THR_MASK		0x0f
-#define VPE_DELTA_EV_THR_SHIFT		28
-
-#define VPE_SC_MP_SC18			0x0748
-#define VPE_HS_FACTOR_MASK		0x03ff
-#define VPE_HS_FACTOR_SHIFT		0
-#define VPE_CONF_DEFAULT_MASK		0x01ff
-#define VPE_CONF_DEFAULT_SHIFT		16
-
-#define VPE_SC_MP_SC19			0x074c
-#define VPE_HPF_COEFF0_MASK		0xff
-#define VPE_HPF_COEFF0_SHIFT		0
-#define VPE_HPF_COEFF1_MASK		0xff
-#define VPE_HPF_COEFF1_SHIFT		8
-#define VPE_HPF_COEFF2_MASK		0xff
-#define VPE_HPF_COEFF2_SHIFT		16
-#define VPE_HPF_COEFF3_MASK		0xff
-#define VPE_HPF_COEFF3_SHIFT		23
-
-#define VPE_SC_MP_SC20			0x0750
-#define VPE_HPF_COEFF4_MASK		0xff
-#define VPE_HPF_COEFF4_SHIFT		0
-#define VPE_HPF_COEFF5_MASK		0xff
-#define VPE_HPF_COEFF5_SHIFT		8
-#define VPE_HPF_NORM_SHIFT_MASK		0x07
-#define VPE_HPF_NORM_SHIFT_SHIFT	16
-#define VPE_NL_LIMIT_MASK		0x1ff
-#define VPE_NL_LIMIT_SHIFT		20
-
-#define VPE_SC_MP_SC21			0x0754
-#define VPE_NL_LO_THR_MASK		0x01ff
-#define VPE_NL_LO_THR_SHIFT		0
-#define VPE_NL_LO_SLOPE_MASK		0xff
-#define VPE_NL_LO_SLOPE_SHIFT		16
-
-#define VPE_SC_MP_SC22			0x0758
-#define VPE_NL_HI_THR_MASK		0x01ff
-#define VPE_NL_HI_THR_SHIFT		0
-#define VPE_NL_HI_SLOPE_SH_MASK		0x07
-#define VPE_NL_HI_SLOPE_SH_SHIFT	16
-
-#define VPE_SC_MP_SC23			0x075c
-#define VPE_GRADIENT_THR_MASK		0x07ff
-#define VPE_GRADIENT_THR_SHIFT		0
-#define VPE_GRADIENT_THR_RANGE_MASK	0x0f
-#define VPE_GRADIENT_THR_RANGE_SHIFT	12
-#define VPE_MIN_GY_THR_MASK		0xff
-#define VPE_MIN_GY_THR_SHIFT		16
-#define VPE_MIN_GY_THR_RANGE_MASK	0x0f
-#define VPE_MIN_GY_THR_RANGE_SHIFT	28
-
-#define VPE_SC_MP_SC24			0x0760
-#define VPE_ORG_H_MASK			0x07ff
-#define VPE_ORG_H_SHIFT			0
-#define VPE_ORG_W_MASK			0x07ff
-#define VPE_ORG_W_SHIFT			16
-
-#define VPE_SC_MP_SC25			0x0764
-#define VPE_OFF_H_MASK			0x07ff
-#define VPE_OFF_H_SHIFT			0
-#define VPE_OFF_W_MASK			0x07ff
-#define VPE_OFF_W_SHIFT			16
-
-/* VPE color space converter regs */
-#define VPE_CSC_CSC00			0x5700
-#define VPE_CSC_A0_MASK			0x1fff
-#define VPE_CSC_A0_SHIFT		0
-#define VPE_CSC_B0_MASK			0x1fff
-#define VPE_CSC_B0_SHIFT		16
-
-#define VPE_CSC_CSC01			0x5704
-#define VPE_CSC_C0_MASK			0x1fff
-#define VPE_CSC_C0_SHIFT		0
-#define VPE_CSC_A1_MASK			0x1fff
-#define VPE_CSC_A1_SHIFT		16
-
-#define VPE_CSC_CSC02			0x5708
-#define VPE_CSC_B1_MASK			0x1fff
-#define VPE_CSC_B1_SHIFT		0
-#define VPE_CSC_C1_MASK			0x1fff
-#define VPE_CSC_C1_SHIFT		16
-
-#define VPE_CSC_CSC03			0x570c
-#define VPE_CSC_A2_MASK			0x1fff
-#define VPE_CSC_A2_SHIFT		0
-#define VPE_CSC_B2_MASK			0x1fff
-#define VPE_CSC_B2_SHIFT		16
-
-#define VPE_CSC_CSC04			0x5710
-#define VPE_CSC_C2_MASK			0x1fff
-#define VPE_CSC_C2_SHIFT		0
-#define VPE_CSC_D0_MASK			0x0fff
-#define VPE_CSC_D0_SHIFT		16
-
-#define VPE_CSC_CSC05			0x5714
-#define VPE_CSC_D1_MASK			0x0fff
-#define VPE_CSC_D1_SHIFT		0
-#define VPE_CSC_D2_MASK			0x0fff
-#define VPE_CSC_D2_SHIFT		16
-#define VPE_CSC_BYPASS			(1 << 28)
-
 #endif
diff --git a/drivers/media/platform/vsp1/Makefile b/drivers/media/platform/vsp1/Makefile
index 4da226169e15..151cecd0ea25 100644
--- a/drivers/media/platform/vsp1/Makefile
+++ b/drivers/media/platform/vsp1/Makefile
@@ -1,5 +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_lif.o vsp1_uds.o
+vsp1-y					+= vsp1_hsit.o vsp1_lif.o vsp1_lut.o
+vsp1-y					+= 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 d6c6ecd039ff..94d1b02680c5 100644
--- a/drivers/media/platform/vsp1/vsp1.h
+++ b/drivers/media/platform/vsp1/vsp1.h
@@ -28,8 +28,11 @@ struct clk;
 struct device;
 
 struct vsp1_platform_data;
+struct vsp1_hsit;
 struct vsp1_lif;
+struct vsp1_lut;
 struct vsp1_rwpf;
+struct vsp1_sru;
 struct vsp1_uds;
 
 #define VPS1_MAX_RPF		5
@@ -47,8 +50,12 @@ struct vsp1_device {
 	struct mutex lock;
 	int ref_count;
 
+	struct vsp1_hsit *hsi;
+	struct vsp1_hsit *hst;
 	struct vsp1_lif *lif;
+	struct vsp1_lut *lut;
 	struct vsp1_rwpf *rpf[VPS1_MAX_RPF];
+	struct vsp1_sru *sru;
 	struct vsp1_uds *uds[VPS1_MAX_UDS];
 	struct vsp1_rwpf *wpf[VPS1_MAX_WPF];
 
diff --git a/drivers/media/platform/vsp1/vsp1_drv.c b/drivers/media/platform/vsp1/vsp1_drv.c
index d16bf0f41e24..0df0a994e575 100644
--- a/drivers/media/platform/vsp1/vsp1_drv.c
+++ b/drivers/media/platform/vsp1/vsp1_drv.c
@@ -20,8 +20,11 @@
 #include <linux/videodev2.h>
 
 #include "vsp1.h"
+#include "vsp1_hsit.h"
 #include "vsp1_lif.h"
+#include "vsp1_lut.h"
 #include "vsp1_rwpf.h"
+#include "vsp1_sru.h"
 #include "vsp1_uds.h"
 
 /* -----------------------------------------------------------------------------
@@ -152,6 +155,22 @@ static int vsp1_create_entities(struct vsp1_device *vsp1)
 	}
 
 	/* Instantiate all the entities. */
+	vsp1->hsi = vsp1_hsit_create(vsp1, true);
+	if (IS_ERR(vsp1->hsi)) {
+		ret = PTR_ERR(vsp1->hsi);
+		goto done;
+	}
+
+	list_add_tail(&vsp1->hsi->entity.list_dev, &vsp1->entities);
+
+	vsp1->hst = vsp1_hsit_create(vsp1, false);
+	if (IS_ERR(vsp1->hst)) {
+		ret = PTR_ERR(vsp1->hst);
+		goto done;
+	}
+
+	list_add_tail(&vsp1->hst->entity.list_dev, &vsp1->entities);
+
 	if (vsp1->pdata->features & VSP1_HAS_LIF) {
 		vsp1->lif = vsp1_lif_create(vsp1);
 		if (IS_ERR(vsp1->lif)) {
@@ -162,6 +181,16 @@ static int vsp1_create_entities(struct vsp1_device *vsp1)
 		list_add_tail(&vsp1->lif->entity.list_dev, &vsp1->entities);
 	}
 
+	if (vsp1->pdata->features & VSP1_HAS_LUT) {
+		vsp1->lut = vsp1_lut_create(vsp1);
+		if (IS_ERR(vsp1->lut)) {
+			ret = PTR_ERR(vsp1->lut);
+			goto done;
+		}
+
+		list_add_tail(&vsp1->lut->entity.list_dev, &vsp1->entities);
+	}
+
 	for (i = 0; i < vsp1->pdata->rpf_count; ++i) {
 		struct vsp1_rwpf *rpf;
 
@@ -175,6 +204,16 @@ static int vsp1_create_entities(struct vsp1_device *vsp1)
 		list_add_tail(&rpf->entity.list_dev, &vsp1->entities);
 	}
 
+	if (vsp1->pdata->features & VSP1_HAS_SRU) {
+		vsp1->sru = vsp1_sru_create(vsp1);
+		if (IS_ERR(vsp1->sru)) {
+			ret = PTR_ERR(vsp1->sru);
+			goto done;
+		}
+
+		list_add_tail(&vsp1->sru->entity.list_dev, &vsp1->entities);
+	}
+
 	for (i = 0; i < vsp1->pdata->uds_count; ++i) {
 		struct vsp1_uds *uds;
 
diff --git a/drivers/media/platform/vsp1/vsp1_entity.c b/drivers/media/platform/vsp1/vsp1_entity.c
index 9028f9d524f4..0226e47df6d9 100644
--- a/drivers/media/platform/vsp1/vsp1_entity.c
+++ b/drivers/media/platform/vsp1/vsp1_entity.c
@@ -15,6 +15,7 @@
 #include <linux/gfp.h>
 
 #include <media/media-entity.h>
+#include <media/v4l2-ctrls.h>
 #include <media/v4l2-subdev.h>
 
 #include "vsp1.h"
@@ -122,12 +123,16 @@ int vsp1_entity_init(struct vsp1_device *vsp1, struct vsp1_entity *entity,
 		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) },
@@ -177,5 +182,7 @@ int vsp1_entity_init(struct vsp1_device *vsp1, struct vsp1_entity *entity,
 
 void vsp1_entity_destroy(struct vsp1_entity *entity)
 {
+	if (entity->subdev.ctrl_handler)
+		v4l2_ctrl_handler_free(entity->subdev.ctrl_handler);
 	media_entity_cleanup(&entity->subdev.entity);
 }
diff --git a/drivers/media/platform/vsp1/vsp1_entity.h b/drivers/media/platform/vsp1/vsp1_entity.h
index c4feab2cbb81..e152798d7f38 100644
--- a/drivers/media/platform/vsp1/vsp1_entity.h
+++ b/drivers/media/platform/vsp1/vsp1_entity.h
@@ -20,8 +20,12 @@
 struct vsp1_device;
 
 enum vsp1_entity_type {
+	VSP1_ENTITY_HSI,
+	VSP1_ENTITY_HST,
 	VSP1_ENTITY_LIF,
+	VSP1_ENTITY_LUT,
 	VSP1_ENTITY_RPF,
+	VSP1_ENTITY_SRU,
 	VSP1_ENTITY_UDS,
 	VSP1_ENTITY_WPF,
 };
diff --git a/drivers/media/platform/vsp1/vsp1_hsit.c b/drivers/media/platform/vsp1/vsp1_hsit.c
new file mode 100644
index 000000000000..285485350d82
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_hsit.c
@@ -0,0 +1,222 @@
+/*
+ * vsp1_hsit.c  --  R-Car VSP1 Hue Saturation value (Inverse) Transform
+ *
+ * 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_hsit.h"
+
+#define HSIT_MIN_SIZE				4U
+#define HSIT_MAX_SIZE				8190U
+
+/* -----------------------------------------------------------------------------
+ * Device Access
+ */
+
+static inline u32 vsp1_hsit_read(struct vsp1_hsit *hsit, u32 reg)
+{
+	return vsp1_read(hsit->entity.vsp1, reg);
+}
+
+static inline void vsp1_hsit_write(struct vsp1_hsit *hsit, u32 reg, u32 data)
+{
+	vsp1_write(hsit->entity.vsp1, reg, data);
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Core Operations
+ */
+
+static int hsit_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+	struct vsp1_hsit *hsit = to_hsit(subdev);
+
+	if (!enable)
+		return 0;
+
+	if (hsit->inverse)
+		vsp1_hsit_write(hsit, VI6_HSI_CTRL, VI6_HSI_CTRL_EN);
+	else
+		vsp1_hsit_write(hsit, VI6_HST_CTRL, VI6_HST_CTRL_EN);
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Pad Operations
+ */
+
+static int hsit_enum_mbus_code(struct v4l2_subdev *subdev,
+			       struct v4l2_subdev_fh *fh,
+			       struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct vsp1_hsit *hsit = to_hsit(subdev);
+
+	if (code->index > 0)
+		return -EINVAL;
+
+	if ((code->pad == HSIT_PAD_SINK && !hsit->inverse) |
+	    (code->pad == HSIT_PAD_SOURCE && hsit->inverse))
+		code->code = V4L2_MBUS_FMT_ARGB8888_1X32;
+	else
+		code->code = V4L2_MBUS_FMT_AHSV8888_1X32;
+
+	return 0;
+}
+
+static int hsit_enum_frame_size(struct v4l2_subdev *subdev,
+				struct v4l2_subdev_fh *fh,
+				struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct v4l2_mbus_framefmt *format;
+
+	format = v4l2_subdev_get_try_format(fh, fse->pad);
+
+	if (fse->index || fse->code != format->code)
+		return -EINVAL;
+
+	if (fse->pad == HSIT_PAD_SINK) {
+		fse->min_width = HSIT_MIN_SIZE;
+		fse->max_width = HSIT_MAX_SIZE;
+		fse->min_height = HSIT_MIN_SIZE;
+		fse->max_height = HSIT_MAX_SIZE;
+	} else {
+		/* The size on the source pad are fixed and always identical to
+		 * the size on the sink pad.
+		 */
+		fse->min_width = format->width;
+		fse->max_width = format->width;
+		fse->min_height = format->height;
+		fse->max_height = format->height;
+	}
+
+	return 0;
+}
+
+static int hsit_get_format(struct v4l2_subdev *subdev,
+			   struct v4l2_subdev_fh *fh,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct vsp1_hsit *hsit = to_hsit(subdev);
+
+	fmt->format = *vsp1_entity_get_pad_format(&hsit->entity, fh, fmt->pad,
+						  fmt->which);
+
+	return 0;
+}
+
+static int hsit_set_format(struct v4l2_subdev *subdev,
+			   struct v4l2_subdev_fh *fh,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct vsp1_hsit *hsit = to_hsit(subdev);
+	struct v4l2_mbus_framefmt *format;
+
+	format = vsp1_entity_get_pad_format(&hsit->entity, fh, fmt->pad,
+					    fmt->which);
+
+	if (fmt->pad == HSIT_PAD_SOURCE) {
+		/* The HST and HSI output format code and resolution can't be
+		 * modified.
+		 */
+		fmt->format = *format;
+		return 0;
+	}
+
+	format->code = hsit->inverse ? V4L2_MBUS_FMT_AHSV8888_1X32
+		     : V4L2_MBUS_FMT_ARGB8888_1X32;
+	format->width = clamp_t(unsigned int, fmt->format.width,
+				HSIT_MIN_SIZE, HSIT_MAX_SIZE);
+	format->height = clamp_t(unsigned int, fmt->format.height,
+				 HSIT_MIN_SIZE, HSIT_MAX_SIZE);
+	format->field = V4L2_FIELD_NONE;
+	format->colorspace = V4L2_COLORSPACE_SRGB;
+
+	fmt->format = *format;
+
+	/* Propagate the format to the source pad. */
+	format = vsp1_entity_get_pad_format(&hsit->entity, fh, HSIT_PAD_SOURCE,
+					    fmt->which);
+	*format = fmt->format;
+	format->code = hsit->inverse ? V4L2_MBUS_FMT_ARGB8888_1X32
+		     : V4L2_MBUS_FMT_AHSV8888_1X32;
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Operations
+ */
+
+static struct v4l2_subdev_video_ops hsit_video_ops = {
+	.s_stream = hsit_s_stream,
+};
+
+static struct v4l2_subdev_pad_ops hsit_pad_ops = {
+	.enum_mbus_code = hsit_enum_mbus_code,
+	.enum_frame_size = hsit_enum_frame_size,
+	.get_fmt = hsit_get_format,
+	.set_fmt = hsit_set_format,
+};
+
+static struct v4l2_subdev_ops hsit_ops = {
+	.video	= &hsit_video_ops,
+	.pad    = &hsit_pad_ops,
+};
+
+/* -----------------------------------------------------------------------------
+ * Initialization and Cleanup
+ */
+
+struct vsp1_hsit *vsp1_hsit_create(struct vsp1_device *vsp1, bool inverse)
+{
+	struct v4l2_subdev *subdev;
+	struct vsp1_hsit *hsit;
+	int ret;
+
+	hsit = devm_kzalloc(vsp1->dev, sizeof(*hsit), GFP_KERNEL);
+	if (hsit == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	hsit->inverse = inverse;
+
+	if (inverse) {
+		hsit->entity.type = VSP1_ENTITY_HSI;
+		hsit->entity.id = VI6_DPR_NODE_HSI;
+	} 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)
+		return ERR_PTR(ret);
+
+	/* Initialize the V4L2 subdev. */
+	subdev = &hsit->entity.subdev;
+	v4l2_subdev_init(subdev, &hsit_ops);
+
+	subdev->entity.ops = &vsp1_media_ops;
+	subdev->internal_ops = &vsp1_subdev_internal_ops;
+	snprintf(subdev->name, sizeof(subdev->name), "%s %s",
+		 dev_name(vsp1->dev), inverse ? "hsi" : "hst");
+	v4l2_set_subdevdata(subdev, hsit);
+	subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	vsp1_entity_init_formats(subdev, NULL);
+
+	return hsit;
+}
diff --git a/drivers/media/platform/vsp1/vsp1_hsit.h b/drivers/media/platform/vsp1/vsp1_hsit.h
new file mode 100644
index 000000000000..82f1c8426900
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_hsit.h
@@ -0,0 +1,38 @@
+/*
+ * vsp1_hsit.h  --  R-Car VSP1 Hue Saturation value (Inverse) Transform
+ *
+ * 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_HSIT_H__
+#define __VSP1_HSIT_H__
+
+#include <media/media-entity.h>
+#include <media/v4l2-subdev.h>
+
+#include "vsp1_entity.h"
+
+struct vsp1_device;
+
+#define HSIT_PAD_SINK				0
+#define HSIT_PAD_SOURCE				1
+
+struct vsp1_hsit {
+	struct vsp1_entity entity;
+	bool inverse;
+};
+
+static inline struct vsp1_hsit *to_hsit(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct vsp1_hsit, entity.subdev);
+}
+
+struct vsp1_hsit *vsp1_hsit_create(struct vsp1_device *vsp1, bool inverse);
+
+#endif /* __VSP1_HSIT_H__ */
diff --git a/drivers/media/platform/vsp1/vsp1_lut.c b/drivers/media/platform/vsp1/vsp1_lut.c
new file mode 100644
index 000000000000..4e9dc7c86ef8
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_lut.c
@@ -0,0 +1,252 @@
+/*
+ * vsp1_lut.c  --  R-Car VSP1 Look-Up Table
+ *
+ * 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 <linux/vsp1.h>
+
+#include <media/v4l2-subdev.h>
+
+#include "vsp1.h"
+#include "vsp1_lut.h"
+
+#define LUT_MIN_SIZE				4U
+#define LUT_MAX_SIZE				8190U
+
+/* -----------------------------------------------------------------------------
+ * Device Access
+ */
+
+static inline u32 vsp1_lut_read(struct vsp1_lut *lut, u32 reg)
+{
+	return vsp1_read(lut->entity.vsp1, reg);
+}
+
+static inline void vsp1_lut_write(struct vsp1_lut *lut, u32 reg, u32 data)
+{
+	vsp1_write(lut->entity.vsp1, reg, data);
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Core Operations
+ */
+
+static void lut_configure(struct vsp1_lut *lut, struct vsp1_lut_config *config)
+{
+	memcpy_toio(lut->entity.vsp1->mmio + VI6_LUT_TABLE, config->lut,
+		    sizeof(config->lut));
+}
+
+static long lut_ioctl(struct v4l2_subdev *subdev, unsigned int cmd, void *arg)
+{
+	struct vsp1_lut *lut = to_lut(subdev);
+
+	switch (cmd) {
+	case VIDIOC_VSP1_LUT_CONFIG:
+		lut_configure(lut, arg);
+		return 0;
+
+	default:
+		return -ENOIOCTLCMD;
+	}
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Video Operations
+ */
+
+static int lut_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+	struct vsp1_lut *lut = to_lut(subdev);
+
+	if (!enable)
+		return 0;
+
+	vsp1_lut_write(lut, VI6_LUT_CTRL, VI6_LUT_CTRL_EN);
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Pad Operations
+ */
+
+static int lut_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_AHSV8888_1X32,
+		V4L2_MBUS_FMT_AYUV8_1X32,
+	};
+	struct v4l2_mbus_framefmt *format;
+
+	if (code->pad == LUT_PAD_SINK) {
+		if (code->index >= ARRAY_SIZE(codes))
+			return -EINVAL;
+
+		code->code = codes[code->index];
+	} else {
+		/* The LUT can't perform format conversion, the sink format is
+		 * always identical to the source format.
+		 */
+		if (code->index)
+			return -EINVAL;
+
+		format = v4l2_subdev_get_try_format(fh, LUT_PAD_SINK);
+		code->code = format->code;
+	}
+
+	return 0;
+}
+
+static int lut_enum_frame_size(struct v4l2_subdev *subdev,
+			       struct v4l2_subdev_fh *fh,
+			       struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct v4l2_mbus_framefmt *format;
+
+	format = v4l2_subdev_get_try_format(fh, fse->pad);
+
+	if (fse->index || fse->code != format->code)
+		return -EINVAL;
+
+	if (fse->pad == LUT_PAD_SINK) {
+		fse->min_width = LUT_MIN_SIZE;
+		fse->max_width = LUT_MAX_SIZE;
+		fse->min_height = LUT_MIN_SIZE;
+		fse->max_height = LUT_MAX_SIZE;
+	} else {
+		/* The size on the source pad are fixed and always identical to
+		 * the size on the sink pad.
+		 */
+		fse->min_width = format->width;
+		fse->max_width = format->width;
+		fse->min_height = format->height;
+		fse->max_height = format->height;
+	}
+
+	return 0;
+}
+
+static int lut_get_format(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct vsp1_lut *lut = to_lut(subdev);
+
+	fmt->format = *vsp1_entity_get_pad_format(&lut->entity, fh, fmt->pad,
+						  fmt->which);
+
+	return 0;
+}
+
+static int lut_set_format(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct vsp1_lut *lut = to_lut(subdev);
+	struct v4l2_mbus_framefmt *format;
+
+	/* Default to YUV if the requested format is not supported. */
+	if (fmt->format.code != V4L2_MBUS_FMT_ARGB8888_1X32 &&
+	    fmt->format.code != V4L2_MBUS_FMT_AHSV8888_1X32 &&
+	    fmt->format.code != V4L2_MBUS_FMT_AYUV8_1X32)
+		fmt->format.code = V4L2_MBUS_FMT_AYUV8_1X32;
+
+	format = vsp1_entity_get_pad_format(&lut->entity, fh, fmt->pad,
+					    fmt->which);
+
+	if (fmt->pad == LUT_PAD_SOURCE) {
+		/* The LUT output format can't be modified. */
+		fmt->format = *format;
+		return 0;
+	}
+
+	format->width = clamp_t(unsigned int, fmt->format.width,
+				LUT_MIN_SIZE, LUT_MAX_SIZE);
+	format->height = clamp_t(unsigned int, fmt->format.height,
+				 LUT_MIN_SIZE, LUT_MAX_SIZE);
+	format->field = V4L2_FIELD_NONE;
+	format->colorspace = V4L2_COLORSPACE_SRGB;
+
+	fmt->format = *format;
+
+	/* Propagate the format to the source pad. */
+	format = vsp1_entity_get_pad_format(&lut->entity, fh, LUT_PAD_SOURCE,
+					    fmt->which);
+	*format = fmt->format;
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Operations
+ */
+
+static struct v4l2_subdev_core_ops lut_core_ops = {
+	.ioctl = lut_ioctl,
+};
+
+static struct v4l2_subdev_video_ops lut_video_ops = {
+	.s_stream = lut_s_stream,
+};
+
+static struct v4l2_subdev_pad_ops lut_pad_ops = {
+	.enum_mbus_code = lut_enum_mbus_code,
+	.enum_frame_size = lut_enum_frame_size,
+	.get_fmt = lut_get_format,
+	.set_fmt = lut_set_format,
+};
+
+static struct v4l2_subdev_ops lut_ops = {
+	.core	= &lut_core_ops,
+	.video	= &lut_video_ops,
+	.pad    = &lut_pad_ops,
+};
+
+/* -----------------------------------------------------------------------------
+ * Initialization and Cleanup
+ */
+
+struct vsp1_lut *vsp1_lut_create(struct vsp1_device *vsp1)
+{
+	struct v4l2_subdev *subdev;
+	struct vsp1_lut *lut;
+	int ret;
+
+	lut = devm_kzalloc(vsp1->dev, sizeof(*lut), GFP_KERNEL);
+	if (lut == NULL)
+		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)
+		return ERR_PTR(ret);
+
+	/* Initialize the V4L2 subdev. */
+	subdev = &lut->entity.subdev;
+	v4l2_subdev_init(subdev, &lut_ops);
+
+	subdev->entity.ops = &vsp1_media_ops;
+	subdev->internal_ops = &vsp1_subdev_internal_ops;
+	snprintf(subdev->name, sizeof(subdev->name), "%s lut",
+		 dev_name(vsp1->dev));
+	v4l2_set_subdevdata(subdev, lut);
+	subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	vsp1_entity_init_formats(subdev, NULL);
+
+	return lut;
+}
diff --git a/drivers/media/platform/vsp1/vsp1_lut.h b/drivers/media/platform/vsp1/vsp1_lut.h
new file mode 100644
index 000000000000..f92ffb867350
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_lut.h
@@ -0,0 +1,38 @@
+/*
+ * vsp1_lut.h  --  R-Car VSP1 Look-Up Table
+ *
+ * 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_LUT_H__
+#define __VSP1_LUT_H__
+
+#include <media/media-entity.h>
+#include <media/v4l2-subdev.h>
+
+#include "vsp1_entity.h"
+
+struct vsp1_device;
+
+#define LUT_PAD_SINK				0
+#define LUT_PAD_SOURCE				1
+
+struct vsp1_lut {
+	struct vsp1_entity entity;
+	u32 lut[256];
+};
+
+static inline struct vsp1_lut *to_lut(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct vsp1_lut, entity.subdev);
+}
+
+struct vsp1_lut *vsp1_lut_create(struct vsp1_device *vsp1);
+
+#endif /* __VSP1_LUT_H__ */
diff --git a/drivers/media/platform/vsp1/vsp1_regs.h b/drivers/media/platform/vsp1/vsp1_regs.h
index 1d3304f1365b..28650806c20f 100644
--- a/drivers/media/platform/vsp1/vsp1_regs.h
+++ b/drivers/media/platform/vsp1/vsp1_regs.h
@@ -336,8 +336,21 @@
  */
 
 #define VI6_SRU_CTRL0			0x2200
+#define VI6_SRU_CTRL0_PARAM0_SHIFT	16
+#define VI6_SRU_CTRL0_PARAM1_SHIFT	8
+#define VI6_SRU_CTRL0_MODE_UPSCALE	(4 << 4)
+#define VI6_SRU_CTRL0_PARAM2		(1 << 3)
+#define VI6_SRU_CTRL0_PARAM3		(1 << 2)
+#define VI6_SRU_CTRL0_PARAM4		(1 << 1)
+#define VI6_SRU_CTRL0_EN		(1 << 0)
+
 #define VI6_SRU_CTRL1			0x2204
+#define VI6_SRU_CTRL1_PARAM5		0x7ff
+
 #define VI6_SRU_CTRL2			0x2208
+#define VI6_SRU_CTRL2_PARAM6_SHIFT	16
+#define VI6_SRU_CTRL2_PARAM7_SHIFT	8
+#define VI6_SRU_CTRL2_PARAM8_SHIFT	0
 
 /* -----------------------------------------------------------------------------
  * UDS Control Registers
@@ -412,6 +425,7 @@
  */
 
 #define VI6_LUT_CTRL			0x2800
+#define VI6_LUT_CTRL_EN			(1 << 0)
 
 /* -----------------------------------------------------------------------------
  * CLU Control Registers
@@ -424,12 +438,14 @@
  */
 
 #define VI6_HST_CTRL			0x2a00
+#define VI6_HST_CTRL_EN			(1 << 0)
 
 /* -----------------------------------------------------------------------------
  * HSI Control Registers
  */
 
 #define VI6_HSI_CTRL			0x2b00
+#define VI6_HSI_CTRL_EN			(1 << 0)
 
 /* -----------------------------------------------------------------------------
  * BRU Control Registers
diff --git a/drivers/media/platform/vsp1/vsp1_rpf.c b/drivers/media/platform/vsp1/vsp1_rpf.c
index 254871d3423e..bce2be5466b9 100644
--- a/drivers/media/platform/vsp1/vsp1_rpf.c
+++ b/drivers/media/platform/vsp1/vsp1_rpf.c
@@ -47,25 +47,36 @@ static int rpf_s_stream(struct v4l2_subdev *subdev, int enable)
 	struct vsp1_rwpf *rpf = to_rwpf(subdev);
 	const struct vsp1_format_info *fmtinfo = rpf->video.fmtinfo;
 	const struct v4l2_pix_format_mplane *format = &rpf->video.format;
+	const struct v4l2_rect *crop = &rpf->crop;
 	u32 pstride;
 	u32 infmt;
 
 	if (!enable)
 		return 0;
 
-	/* Source size and stride. Cropping isn't supported yet. */
+	/* Source size, stride and crop offsets.
+	 *
+	 * The crop offsets correspond to the location of the crop rectangle top
+	 * left corner in the plane buffer. Only two offsets are needed, as
+	 * planes 2 and 3 always have identical strides.
+	 */
 	vsp1_rpf_write(rpf, VI6_RPF_SRC_BSIZE,
-		       (format->width << VI6_RPF_SRC_BSIZE_BHSIZE_SHIFT) |
-		       (format->height << VI6_RPF_SRC_BSIZE_BVSIZE_SHIFT));
+		       (crop->width << VI6_RPF_SRC_BSIZE_BHSIZE_SHIFT) |
+		       (crop->height << VI6_RPF_SRC_BSIZE_BVSIZE_SHIFT));
 	vsp1_rpf_write(rpf, VI6_RPF_SRC_ESIZE,
-		       (format->width << VI6_RPF_SRC_ESIZE_EHSIZE_SHIFT) |
-		       (format->height << VI6_RPF_SRC_ESIZE_EVSIZE_SHIFT));
+		       (crop->width << VI6_RPF_SRC_ESIZE_EHSIZE_SHIFT) |
+		       (crop->height << VI6_RPF_SRC_ESIZE_EVSIZE_SHIFT));
 
+	rpf->offsets[0] = crop->top * format->plane_fmt[0].bytesperline
+			+ crop->left * fmtinfo->bpp[0] / 8;
 	pstride = format->plane_fmt[0].bytesperline
 		<< VI6_RPF_SRCM_PSTRIDE_Y_SHIFT;
-	if (format->num_planes > 1)
+	if (format->num_planes > 1) {
+		rpf->offsets[1] = crop->top * format->plane_fmt[1].bytesperline
+				+ crop->left * fmtinfo->bpp[1] / 8;
 		pstride |= format->plane_fmt[1].bytesperline
 			<< VI6_RPF_SRCM_PSTRIDE_C_SHIFT;
+	}
 
 	vsp1_rpf_write(rpf, VI6_RPF_SRCM_PSTRIDE, pstride);
 
@@ -113,6 +124,8 @@ static struct v4l2_subdev_pad_ops rpf_pad_ops = {
 	.enum_frame_size = vsp1_rwpf_enum_frame_size,
 	.get_fmt = vsp1_rwpf_get_format,
 	.set_fmt = vsp1_rwpf_set_format,
+	.get_selection = vsp1_rwpf_get_selection,
+	.set_selection = vsp1_rwpf_set_selection,
 };
 
 static struct v4l2_subdev_ops rpf_ops = {
@@ -129,11 +142,14 @@ static void rpf_vdev_queue(struct vsp1_video *video,
 {
 	struct vsp1_rwpf *rpf = container_of(video, struct vsp1_rwpf, video);
 
-	vsp1_rpf_write(rpf, VI6_RPF_SRCM_ADDR_Y, buf->addr[0]);
+	vsp1_rpf_write(rpf, VI6_RPF_SRCM_ADDR_Y,
+		       buf->addr[0] + rpf->offsets[0]);
 	if (buf->buf.num_planes > 1)
-		vsp1_rpf_write(rpf, VI6_RPF_SRCM_ADDR_C0, buf->addr[1]);
+		vsp1_rpf_write(rpf, VI6_RPF_SRCM_ADDR_C0,
+			       buf->addr[1] + rpf->offsets[1]);
 	if (buf->buf.num_planes > 2)
-		vsp1_rpf_write(rpf, VI6_RPF_SRCM_ADDR_C1, buf->addr[2]);
+		vsp1_rpf_write(rpf, VI6_RPF_SRCM_ADDR_C1,
+			       buf->addr[2] + rpf->offsets[1]);
 }
 
 static const struct vsp1_video_operations rpf_vdev_ops = {
diff --git a/drivers/media/platform/vsp1/vsp1_rwpf.c b/drivers/media/platform/vsp1/vsp1_rwpf.c
index 9752d5516ceb..782f770daee5 100644
--- a/drivers/media/platform/vsp1/vsp1_rwpf.c
+++ b/drivers/media/platform/vsp1/vsp1_rwpf.c
@@ -71,6 +71,19 @@ int vsp1_rwpf_enum_frame_size(struct v4l2_subdev *subdev,
 	return 0;
 }
 
+static struct v4l2_rect *
+vsp1_rwpf_get_crop(struct vsp1_rwpf *rwpf, struct v4l2_subdev_fh *fh, u32 which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_crop(fh, RWPF_PAD_SINK);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &rwpf->crop;
+	default:
+		return NULL;
+	}
+}
+
 int vsp1_rwpf_get_format(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh,
 			 struct v4l2_subdev_format *fmt)
 {
@@ -87,6 +100,7 @@ int vsp1_rwpf_set_format(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh,
 {
 	struct vsp1_rwpf *rwpf = to_rwpf(subdev);
 	struct v4l2_mbus_framefmt *format;
+	struct v4l2_rect *crop;
 
 	/* Default to YUV if the requested format is not supported. */
 	if (fmt->format.code != V4L2_MBUS_FMT_ARGB8888_1X32 &&
@@ -115,6 +129,13 @@ int vsp1_rwpf_set_format(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh,
 
 	fmt->format = *format;
 
+	/* Update the sink crop rectangle. */
+	crop = vsp1_rwpf_get_crop(rwpf, fh, fmt->which);
+	crop->left = 0;
+	crop->top = 0;
+	crop->width = fmt->format.width;
+	crop->height = fmt->format.height;
+
 	/* Propagate the format to the source pad. */
 	format = vsp1_entity_get_pad_format(&rwpf->entity, fh, RWPF_PAD_SOURCE,
 					    fmt->which);
@@ -122,3 +143,78 @@ int vsp1_rwpf_set_format(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh,
 
 	return 0;
 }
+
+int vsp1_rwpf_get_selection(struct v4l2_subdev *subdev,
+			    struct v4l2_subdev_fh *fh,
+			    struct v4l2_subdev_selection *sel)
+{
+	struct vsp1_rwpf *rwpf = to_rwpf(subdev);
+	struct v4l2_mbus_framefmt *format;
+
+	/* Cropping is implemented on the sink pad. */
+	if (sel->pad != RWPF_PAD_SINK)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		sel->r = *vsp1_rwpf_get_crop(rwpf, fh, sel->which);
+		break;
+
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		format = vsp1_entity_get_pad_format(&rwpf->entity, fh,
+						    RWPF_PAD_SINK, sel->which);
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = format->width;
+		sel->r.height = format->height;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int vsp1_rwpf_set_selection(struct v4l2_subdev *subdev,
+			    struct v4l2_subdev_fh *fh,
+			    struct v4l2_subdev_selection *sel)
+{
+	struct vsp1_rwpf *rwpf = to_rwpf(subdev);
+	struct v4l2_mbus_framefmt *format;
+	struct v4l2_rect *crop;
+
+	/* Cropping is implemented on the sink pad. */
+	if (sel->pad != RWPF_PAD_SINK)
+		return -EINVAL;
+
+	if (sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
+
+	/* Make sure the crop rectangle is entirely contained in the image. The
+	 * WPF top and left offsets are limited to 255.
+	 */
+	format = vsp1_entity_get_pad_format(&rwpf->entity, fh, RWPF_PAD_SINK,
+					    sel->which);
+	sel->r.left = min_t(unsigned int, sel->r.left, format->width - 2);
+	sel->r.top = min_t(unsigned int, sel->r.top, format->height - 2);
+	if (rwpf->entity.type == VSP1_ENTITY_WPF) {
+		sel->r.left = min_t(unsigned int, sel->r.left, 255);
+		sel->r.top = min_t(unsigned int, sel->r.top, 255);
+	}
+	sel->r.width = min_t(unsigned int, sel->r.width,
+			     format->width - sel->r.left);
+	sel->r.height = min_t(unsigned int, sel->r.height,
+			      format->height - sel->r.top);
+
+	crop = vsp1_rwpf_get_crop(rwpf, fh, sel->which);
+	*crop = sel->r;
+
+	/* Propagate the format to the source pad. */
+	format = vsp1_entity_get_pad_format(&rwpf->entity, fh, RWPF_PAD_SOURCE,
+					    sel->which);
+	format->width = crop->width;
+	format->height = crop->height;
+
+	return 0;
+}
diff --git a/drivers/media/platform/vsp1/vsp1_rwpf.h b/drivers/media/platform/vsp1/vsp1_rwpf.h
index c182d85f36b3..6cbdb547470b 100644
--- a/drivers/media/platform/vsp1/vsp1_rwpf.h
+++ b/drivers/media/platform/vsp1/vsp1_rwpf.h
@@ -29,6 +29,10 @@ struct vsp1_rwpf {
 
 	unsigned int max_width;
 	unsigned int max_height;
+
+	struct v4l2_rect crop;
+
+	unsigned int offsets[2];
 };
 
 static inline struct vsp1_rwpf *to_rwpf(struct v4l2_subdev *subdev)
@@ -49,5 +53,11 @@ int vsp1_rwpf_get_format(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh,
 			 struct v4l2_subdev_format *fmt);
 int vsp1_rwpf_set_format(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh,
 			 struct v4l2_subdev_format *fmt);
+int vsp1_rwpf_get_selection(struct v4l2_subdev *subdev,
+			    struct v4l2_subdev_fh *fh,
+			    struct v4l2_subdev_selection *sel);
+int vsp1_rwpf_set_selection(struct v4l2_subdev *subdev,
+			    struct v4l2_subdev_fh *fh,
+			    struct v4l2_subdev_selection *sel);
 
 #endif /* __VSP1_RWPF_H__ */
diff --git a/drivers/media/platform/vsp1/vsp1_sru.c b/drivers/media/platform/vsp1/vsp1_sru.c
new file mode 100644
index 000000000000..7ab1a0b2d656
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_sru.c
@@ -0,0 +1,356 @@
+/*
+ * vsp1_sru.c  --  R-Car VSP1 Super Resolution 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_sru.h"
+
+#define SRU_MIN_SIZE				4U
+#define SRU_MAX_SIZE				8190U
+
+/* -----------------------------------------------------------------------------
+ * Device Access
+ */
+
+static inline u32 vsp1_sru_read(struct vsp1_sru *sru, u32 reg)
+{
+	return vsp1_read(sru->entity.vsp1, reg);
+}
+
+static inline void vsp1_sru_write(struct vsp1_sru *sru, u32 reg, u32 data)
+{
+	vsp1_write(sru->entity.vsp1, reg, data);
+}
+
+/* -----------------------------------------------------------------------------
+ * Controls
+ */
+
+#define V4L2_CID_VSP1_SRU_INTENSITY		(V4L2_CID_USER_BASE + 1)
+
+static int sru_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vsp1_sru *sru =
+		container_of(ctrl->handler, struct vsp1_sru, ctrls);
+
+	switch (ctrl->id) {
+	case V4L2_CID_VSP1_SRU_INTENSITY:
+		sru->intensity = ctrl->val;
+		break;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops sru_ctrl_ops = {
+	.s_ctrl = sru_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config sru_intensity_control = {
+	.ops = &sru_ctrl_ops,
+	.id = V4L2_CID_VSP1_SRU_INTENSITY,
+	.name = "Intensity",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min = 1,
+	.max = 6,
+	.step = 1,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Core Operations
+ */
+
+struct vsp1_sru_param {
+	u32 ctrl0;
+	u32 ctrl2;
+};
+
+#define VI6_SRU_CTRL0_PARAMS(p0, p1)			\
+	(((p0) << VI6_SRU_CTRL0_PARAM0_SHIFT) |		\
+	 ((p1) << VI6_SRU_CTRL0_PARAM1_SHIFT))
+
+#define VI6_SRU_CTRL2_PARAMS(p6, p7, p8)		\
+	(((p6) << VI6_SRU_CTRL2_PARAM6_SHIFT) |		\
+	 ((p7) << VI6_SRU_CTRL2_PARAM7_SHIFT) |		\
+	 ((p8) << VI6_SRU_CTRL2_PARAM8_SHIFT))
+
+static const struct vsp1_sru_param vsp1_sru_params[] = {
+	{
+		.ctrl0 = VI6_SRU_CTRL0_PARAMS(256, 4) | VI6_SRU_CTRL0_EN,
+		.ctrl2 = VI6_SRU_CTRL2_PARAMS(24, 40, 255),
+	}, {
+		.ctrl0 = VI6_SRU_CTRL0_PARAMS(256, 4) | VI6_SRU_CTRL0_EN,
+		.ctrl2 = VI6_SRU_CTRL2_PARAMS(8, 16, 255),
+	}, {
+		.ctrl0 = VI6_SRU_CTRL0_PARAMS(384, 5) | VI6_SRU_CTRL0_EN,
+		.ctrl2 = VI6_SRU_CTRL2_PARAMS(36, 60, 255),
+	}, {
+		.ctrl0 = VI6_SRU_CTRL0_PARAMS(384, 5) | VI6_SRU_CTRL0_EN,
+		.ctrl2 = VI6_SRU_CTRL2_PARAMS(12, 27, 255),
+	}, {
+		.ctrl0 = VI6_SRU_CTRL0_PARAMS(511, 6) | VI6_SRU_CTRL0_EN,
+		.ctrl2 = VI6_SRU_CTRL2_PARAMS(48, 80, 255),
+	}, {
+		.ctrl0 = VI6_SRU_CTRL0_PARAMS(511, 6) | VI6_SRU_CTRL0_EN,
+		.ctrl2 = VI6_SRU_CTRL2_PARAMS(16, 36, 255),
+	},
+};
+
+static int sru_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+	struct vsp1_sru *sru = to_sru(subdev);
+	const struct vsp1_sru_param *param;
+	struct v4l2_mbus_framefmt *input;
+	struct v4l2_mbus_framefmt *output;
+	bool upscale;
+	u32 ctrl0;
+
+	if (!enable)
+		return 0;
+
+	input = &sru->entity.formats[SRU_PAD_SINK];
+	output = &sru->entity.formats[SRU_PAD_SOURCE];
+	upscale = input->width != output->width;
+	param = &vsp1_sru_params[sru->intensity];
+
+	if (input->code == V4L2_MBUS_FMT_ARGB8888_1X32)
+		ctrl0 = VI6_SRU_CTRL0_PARAM2 | VI6_SRU_CTRL0_PARAM3
+		      | VI6_SRU_CTRL0_PARAM4;
+	else
+		ctrl0 = VI6_SRU_CTRL0_PARAM3;
+
+	vsp1_sru_write(sru, VI6_SRU_CTRL0, param->ctrl0 | ctrl0 |
+		       (upscale ? VI6_SRU_CTRL0_MODE_UPSCALE : 0));
+	vsp1_sru_write(sru, VI6_SRU_CTRL1, VI6_SRU_CTRL1_PARAM5);
+	vsp1_sru_write(sru, VI6_SRU_CTRL2, param->ctrl2);
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Pad Operations
+ */
+
+static int sru_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 == SRU_PAD_SINK) {
+		if (code->index >= ARRAY_SIZE(codes))
+			return -EINVAL;
+
+		code->code = codes[code->index];
+	} else {
+		/* The SRU can't perform format conversion, the sink format is
+		 * always identical to the source format.
+		 */
+		if (code->index)
+			return -EINVAL;
+
+		format = v4l2_subdev_get_try_format(fh, SRU_PAD_SINK);
+		code->code = format->code;
+	}
+
+	return 0;
+}
+
+static int sru_enum_frame_size(struct v4l2_subdev *subdev,
+			       struct v4l2_subdev_fh *fh,
+			       struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct v4l2_mbus_framefmt *format;
+
+	format = v4l2_subdev_get_try_format(fh, SRU_PAD_SINK);
+
+	if (fse->index || fse->code != format->code)
+		return -EINVAL;
+
+	if (fse->pad == SRU_PAD_SINK) {
+		fse->min_width = SRU_MIN_SIZE;
+		fse->max_width = SRU_MAX_SIZE;
+		fse->min_height = SRU_MIN_SIZE;
+		fse->max_height = SRU_MAX_SIZE;
+	} else {
+		fse->min_width = format->width;
+		fse->min_height = format->height;
+		if (format->width <= SRU_MAX_SIZE / 2 &&
+		    format->height <= SRU_MAX_SIZE / 2) {
+			fse->max_width = format->width * 2;
+			fse->max_height = format->height * 2;
+		} else {
+			fse->max_width = format->width;
+			fse->max_height = format->height;
+		}
+	}
+
+	return 0;
+}
+
+static int sru_get_format(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct vsp1_sru *sru = to_sru(subdev);
+
+	fmt->format = *vsp1_entity_get_pad_format(&sru->entity, fh, fmt->pad,
+						  fmt->which);
+
+	return 0;
+}
+
+static void sru_try_format(struct vsp1_sru *sru, struct v4l2_subdev_fh *fh,
+			   unsigned int pad, struct v4l2_mbus_framefmt *fmt,
+			   enum v4l2_subdev_format_whence which)
+{
+	struct v4l2_mbus_framefmt *format;
+	unsigned int input_area;
+	unsigned int output_area;
+
+	switch (pad) {
+	case SRU_PAD_SINK:
+		/* 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;
+
+		fmt->width = clamp(fmt->width, SRU_MIN_SIZE, SRU_MAX_SIZE);
+		fmt->height = clamp(fmt->height, SRU_MIN_SIZE, SRU_MAX_SIZE);
+		break;
+
+	case SRU_PAD_SOURCE:
+		/* The SRU can't perform format conversion. */
+		format = vsp1_entity_get_pad_format(&sru->entity, fh,
+						    SRU_PAD_SINK, which);
+		fmt->code = format->code;
+
+		/* We can upscale by 2 in both direction, but not independently.
+		 * Compare the input and output rectangles areas (avoiding
+		 * integer overflows on the output): if the requested output
+		 * area is larger than 1.5^2 the input area upscale by two,
+		 * otherwise don't scale.
+		 */
+		input_area = format->width * format->height;
+		output_area = min(fmt->width, SRU_MAX_SIZE)
+			    * min(fmt->height, SRU_MAX_SIZE);
+
+		if (fmt->width <= SRU_MAX_SIZE / 2 &&
+		    fmt->height <= SRU_MAX_SIZE / 2 &&
+		    output_area > input_area * 9 / 4) {
+			fmt->width = format->width * 2;
+			fmt->height = format->height * 2;
+		} else {
+			fmt->width = format->width;
+			fmt->height = format->height;
+		}
+		break;
+	}
+
+	fmt->field = V4L2_FIELD_NONE;
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+}
+
+static int sru_set_format(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct vsp1_sru *sru = to_sru(subdev);
+	struct v4l2_mbus_framefmt *format;
+
+	sru_try_format(sru, fh, fmt->pad, &fmt->format, fmt->which);
+
+	format = vsp1_entity_get_pad_format(&sru->entity, fh, fmt->pad,
+					    fmt->which);
+	*format = fmt->format;
+
+	if (fmt->pad == SRU_PAD_SINK) {
+		/* Propagate the format to the source pad. */
+		format = vsp1_entity_get_pad_format(&sru->entity, fh,
+						    SRU_PAD_SOURCE, fmt->which);
+		*format = fmt->format;
+
+		sru_try_format(sru, fh, SRU_PAD_SOURCE, format, fmt->which);
+	}
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Operations
+ */
+
+static struct v4l2_subdev_video_ops sru_video_ops = {
+	.s_stream = sru_s_stream,
+};
+
+static struct v4l2_subdev_pad_ops sru_pad_ops = {
+	.enum_mbus_code = sru_enum_mbus_code,
+	.enum_frame_size = sru_enum_frame_size,
+	.get_fmt = sru_get_format,
+	.set_fmt = sru_set_format,
+};
+
+static struct v4l2_subdev_ops sru_ops = {
+	.video	= &sru_video_ops,
+	.pad    = &sru_pad_ops,
+};
+
+/* -----------------------------------------------------------------------------
+ * Initialization and Cleanup
+ */
+
+struct vsp1_sru *vsp1_sru_create(struct vsp1_device *vsp1)
+{
+	struct v4l2_subdev *subdev;
+	struct vsp1_sru *sru;
+	int ret;
+
+	sru = devm_kzalloc(vsp1->dev, sizeof(*sru), GFP_KERNEL);
+	if (sru == NULL)
+		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)
+		return ERR_PTR(ret);
+
+	/* Initialize the V4L2 subdev. */
+	subdev = &sru->entity.subdev;
+	v4l2_subdev_init(subdev, &sru_ops);
+
+	subdev->entity.ops = &vsp1_media_ops;
+	subdev->internal_ops = &vsp1_subdev_internal_ops;
+	snprintf(subdev->name, sizeof(subdev->name), "%s sru",
+		 dev_name(vsp1->dev));
+	v4l2_set_subdevdata(subdev, sru);
+	subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	vsp1_entity_init_formats(subdev, NULL);
+
+	/* Initialize the control handler. */
+	v4l2_ctrl_handler_init(&sru->ctrls, 1);
+	v4l2_ctrl_new_custom(&sru->ctrls, &sru_intensity_control, NULL);
+	v4l2_ctrl_handler_setup(&sru->ctrls);
+	sru->entity.subdev.ctrl_handler = &sru->ctrls;
+
+	return sru;
+}
diff --git a/drivers/media/platform/vsp1/vsp1_sru.h b/drivers/media/platform/vsp1/vsp1_sru.h
new file mode 100644
index 000000000000..381870b74780
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_sru.h
@@ -0,0 +1,41 @@
+/*
+ * vsp1_sru.h  --  R-Car VSP1 Super Resolution 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_SRU_H__
+#define __VSP1_SRU_H__
+
+#include <media/media-entity.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-subdev.h>
+
+#include "vsp1_entity.h"
+
+struct vsp1_device;
+
+#define SRU_PAD_SINK				0
+#define SRU_PAD_SOURCE				1
+
+struct vsp1_sru {
+	struct vsp1_entity entity;
+
+	struct v4l2_ctrl_handler ctrls;
+	unsigned int intensity;
+};
+
+static inline struct vsp1_sru *to_sru(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct vsp1_sru, entity.subdev);
+}
+
+struct vsp1_sru *vsp1_sru_create(struct vsp1_device *vsp1);
+
+#endif /* __VSP1_SRU_H__ */
diff --git a/drivers/media/platform/vsp1/vsp1_video.c b/drivers/media/platform/vsp1/vsp1_video.c
index 4b0ac07af662..b4687a834f85 100644
--- a/drivers/media/platform/vsp1/vsp1_video.c
+++ b/drivers/media/platform/vsp1/vsp1_video.c
@@ -488,11 +488,17 @@ static bool vsp1_pipeline_ready(struct vsp1_pipeline *pipe)
  * This function completes the current buffer by filling its sequence number,
  * time stamp and payload size, and hands it back to the videobuf core.
  *
+ * When operating in DU output mode (deep pipeline to the DU through the LIF),
+ * the VSP1 needs to constantly supply frames to the display. In that case, if
+ * no other buffer is queued, reuse the one that has just been processed instead
+ * of handing it back to the videobuf core.
+ *
  * Return the next queued buffer or NULL if the queue is empty.
  */
 static struct vsp1_video_buffer *
 vsp1_video_complete_buffer(struct vsp1_video *video)
 {
+	struct vsp1_pipeline *pipe = to_vsp1_pipeline(&video->video.entity);
 	struct vsp1_video_buffer *next = NULL;
 	struct vsp1_video_buffer *done;
 	unsigned long flags;
@@ -507,6 +513,13 @@ vsp1_video_complete_buffer(struct vsp1_video *video)
 
 	done = list_first_entry(&video->irqqueue,
 				struct vsp1_video_buffer, queue);
+
+	/* In DU output mode reuse the buffer if the list is singular. */
+	if (pipe->lif && list_is_singular(&video->irqqueue)) {
+		spin_unlock_irqrestore(&video->irqlock, flags);
+		return done;
+	}
+
 	list_del(&done->queue);
 
 	if (!list_empty(&video->irqqueue))
diff --git a/drivers/media/platform/vsp1/vsp1_wpf.c b/drivers/media/platform/vsp1/vsp1_wpf.c
index db4b85ee05fc..7baed81ff005 100644
--- a/drivers/media/platform/vsp1/vsp1_wpf.c
+++ b/drivers/media/platform/vsp1/vsp1_wpf.c
@@ -48,8 +48,7 @@ static int wpf_s_stream(struct v4l2_subdev *subdev, int enable)
 	struct vsp1_pipeline *pipe =
 		to_vsp1_pipeline(&wpf->entity.subdev.entity);
 	struct vsp1_device *vsp1 = wpf->entity.vsp1;
-	const struct v4l2_mbus_framefmt *format =
-		&wpf->entity.formats[RWPF_PAD_SOURCE];
+	const struct v4l2_rect *crop = &wpf->crop;
 	unsigned int i;
 	u32 srcrpf = 0;
 	u32 outfmt = 0;
@@ -68,7 +67,7 @@ static int wpf_s_stream(struct v4l2_subdev *subdev, int enable)
 
 	vsp1_wpf_write(wpf, VI6_WPF_SRCRPF, srcrpf);
 
-	/* Destination stride. Cropping isn't supported yet. */
+	/* Destination stride. */
 	if (!pipe->lif) {
 		struct v4l2_pix_format_mplane *format = &wpf->video.format;
 
@@ -79,10 +78,12 @@ static int wpf_s_stream(struct v4l2_subdev *subdev, int enable)
 				       format->plane_fmt[1].bytesperline);
 	}
 
-	vsp1_wpf_write(wpf, VI6_WPF_HSZCLIP,
-		       format->width << VI6_WPF_SZCLIP_SIZE_SHIFT);
-	vsp1_wpf_write(wpf, VI6_WPF_VSZCLIP,
-		       format->height << VI6_WPF_SZCLIP_SIZE_SHIFT);
+	vsp1_wpf_write(wpf, VI6_WPF_HSZCLIP, VI6_WPF_SZCLIP_EN |
+		       (crop->left << VI6_WPF_SZCLIP_OFST_SHIFT) |
+		       (crop->width << VI6_WPF_SZCLIP_SIZE_SHIFT));
+	vsp1_wpf_write(wpf, VI6_WPF_VSZCLIP, VI6_WPF_SZCLIP_EN |
+		       (crop->top << VI6_WPF_SZCLIP_OFST_SHIFT) |
+		       (crop->height << VI6_WPF_SZCLIP_SIZE_SHIFT));
 
 	/* Format */
 	if (!pipe->lif) {
@@ -130,6 +131,8 @@ static struct v4l2_subdev_pad_ops wpf_pad_ops = {
 	.enum_frame_size = vsp1_rwpf_enum_frame_size,
 	.get_fmt = vsp1_rwpf_get_format,
 	.set_fmt = vsp1_rwpf_set_format,
+	.get_selection = vsp1_rwpf_get_selection,
+	.set_selection = vsp1_rwpf_set_selection,
 };
 
 static struct v4l2_subdev_ops wpf_ops = {
diff --git a/drivers/media/radio/Kconfig b/drivers/media/radio/Kconfig
index 6ecdc39bb366..192f36f2f4aa 100644
--- a/drivers/media/radio/Kconfig
+++ b/drivers/media/radio/Kconfig
@@ -21,6 +21,12 @@ config RADIO_SI470X
 
 source "drivers/media/radio/si470x/Kconfig"
 
+config RADIO_SI4713
+	tristate "Silicon Labs Si4713 FM Radio with RDS Transmitter support"
+	depends on VIDEO_V4L2
+
+source "drivers/media/radio/si4713/Kconfig"
+
 config RADIO_SI476X
 	tristate "Silicon Laboratories Si476x I2C FM Radio"
 	depends on I2C && VIDEO_V4L2
@@ -113,29 +119,6 @@ config RADIO_SHARK2
 	  To compile this driver as a module, choose M here: the
 	  module will be called radio-shark2.
 
-config I2C_SI4713
-	tristate "I2C driver for Silicon Labs Si4713 device"
-	depends on I2C && VIDEO_V4L2
-	---help---
-	  Say Y here if you want support to Si4713 I2C device.
-	  This device driver supports only i2c bus.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called si4713.
-
-config RADIO_SI4713
-	tristate "Silicon Labs Si4713 FM Radio Transmitter support"
-	depends on I2C && VIDEO_V4L2
-	select I2C_SI4713
-	---help---
-	  Say Y here if you want support to Si4713 FM Radio Transmitter.
-	  This device can transmit audio through FM. It can transmit
-	  RDS and RBDS signals as well. This module is the v4l2 radio
-	  interface for the i2c driver of this device.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called radio-si4713.
-
 config USB_KEENE
 	tristate "Keene FM Transmitter USB support"
 	depends on USB && VIDEO_V4L2
@@ -146,6 +129,20 @@ config USB_KEENE
 	  To compile this driver as a module, choose M here: the
 	  module will be called radio-keene.
 
+config USB_RAREMONO
+	tristate "Thanko's Raremono AM/FM/SW radio support"
+	depends on USB && VIDEO_V4L2
+	---help---
+	  The 'Thanko's Raremono' device contains the Si4734 chip from Silicon Labs Inc.
+	  It is one of the very few or perhaps the only consumer USB radio device
+	  to receive the AM/FM/SW bands.
+
+	  Say Y here if you want to connect this type of AM/FM/SW receiver
+	  to your computer's USB port.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called radio-raremono.
+
 config USB_MA901
 	tristate "Masterkit MA901 USB FM radio support"
 	depends on USB && VIDEO_V4L2
diff --git a/drivers/media/radio/Makefile b/drivers/media/radio/Makefile
index 3b645601800d..120e791199b2 100644
--- a/drivers/media/radio/Makefile
+++ b/drivers/media/radio/Makefile
@@ -17,12 +17,11 @@ obj-$(CONFIG_RADIO_RTRACK) += radio-aimslab.o
 obj-$(CONFIG_RADIO_ZOLTRIX) += radio-zoltrix.o
 obj-$(CONFIG_RADIO_GEMTEK) += radio-gemtek.o
 obj-$(CONFIG_RADIO_TRUST) += radio-trust.o
-obj-$(CONFIG_I2C_SI4713) += si4713-i2c.o
-obj-$(CONFIG_RADIO_SI4713) += radio-si4713.o
 obj-$(CONFIG_RADIO_SI476X) += radio-si476x.o
 obj-$(CONFIG_RADIO_MIROPCM20) += radio-miropcm20.o
 obj-$(CONFIG_USB_DSBR) += dsbr100.o
 obj-$(CONFIG_RADIO_SI470X) += si470x/
+obj-$(CONFIG_RADIO_SI4713) += si4713/
 obj-$(CONFIG_USB_MR800) += radio-mr800.o
 obj-$(CONFIG_USB_KEENE) += radio-keene.o
 obj-$(CONFIG_USB_MA901) += radio-ma901.o
@@ -33,6 +32,7 @@ obj-$(CONFIG_RADIO_TIMBERDALE) += radio-timb.o
 obj-$(CONFIG_RADIO_WL1273) += radio-wl1273.o
 obj-$(CONFIG_RADIO_WL128X) += wl128x/
 obj-$(CONFIG_RADIO_TEA575X) += tea575x.o
+obj-$(CONFIG_USB_RAREMONO) += radio-raremono.o
 
 shark2-objs := radio-shark2.o radio-tea5777.o
 
diff --git a/drivers/media/radio/radio-raremono.c b/drivers/media/radio/radio-raremono.c
new file mode 100644
index 000000000000..7b3bdbb1be73
--- /dev/null
+++ b/drivers/media/radio/radio-raremono.c
@@ -0,0 +1,387 @@
+/*
+ * Copyright 2013 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/input.h>
+#include <linux/usb.h>
+#include <linux/hid.h>
+#include <linux/mutex.h>
+#include <linux/videodev2.h>
+#include <asm/unaligned.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+
+/*
+ * 'Thanko's Raremono' is a Japanese si4734-based AM/FM/SW USB receiver:
+ *
+ * http://www.raremono.jp/product/484.html/
+ *
+ * The USB protocol has been reversed engineered using wireshark, initially
+ * by Dinesh Ram <dinesh.ram@cern.ch> and finished by Hans Verkuil
+ * <hverkuil@xs4all.nl>.
+ *
+ * Sadly the firmware used in this product hides lots of goodies since the
+ * si4734 has more features than are supported by the firmware. Oh well...
+ */
+
+/* driver and module definitions */
+MODULE_AUTHOR("Hans Verkuil <hverkuil@xs4all.nl>");
+MODULE_DESCRIPTION("Thanko's Raremono AM/FM/SW Receiver USB driver");
+MODULE_LICENSE("GPL v2");
+
+/*
+ * The Device announces itself as Cygnal Integrated Products, Inc.
+ *
+ * The vendor and product IDs (and in fact all other lsusb information as
+ * well) are identical to the si470x Silicon Labs USB FM Radio Reference
+ * Design board, even though this card has a si4734 device. Clearly the
+ * designer of this product never bothered to change the USB IDs.
+ */
+
+/* USB Device ID List */
+static struct usb_device_id usb_raremono_device_table[] = {
+	{USB_DEVICE_AND_INTERFACE_INFO(0x10c4, 0x818a, USB_CLASS_HID, 0, 0) },
+	{ }						/* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(usb, usb_raremono_device_table);
+
+#define BUFFER_LENGTH 64
+
+/* Timeout is set to a high value, could probably be reduced. Need more tests */
+#define USB_TIMEOUT 10000
+
+/* Frequency limits in KHz */
+#define FM_FREQ_RANGE_LOW	64000
+#define FM_FREQ_RANGE_HIGH	108000
+
+#define AM_FREQ_RANGE_LOW	520
+#define AM_FREQ_RANGE_HIGH	1710
+
+#define SW_FREQ_RANGE_LOW	2300
+#define SW_FREQ_RANGE_HIGH	26100
+
+enum { BAND_FM, BAND_AM, BAND_SW };
+
+static const struct v4l2_frequency_band bands[] = {
+	/* Band FM */
+	{
+		.type = V4L2_TUNER_RADIO,
+		.index = 0,
+		.capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_STEREO |
+			      V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow   = FM_FREQ_RANGE_LOW * 16,
+		.rangehigh  = FM_FREQ_RANGE_HIGH * 16,
+		.modulation = V4L2_BAND_MODULATION_FM,
+	},
+	/* Band AM */
+	{
+		.type = V4L2_TUNER_RADIO,
+		.index = 1,
+		.capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow   = AM_FREQ_RANGE_LOW * 16,
+		.rangehigh  = AM_FREQ_RANGE_HIGH * 16,
+		.modulation = V4L2_BAND_MODULATION_AM,
+	},
+	/* Band SW */
+	{
+		.type = V4L2_TUNER_RADIO,
+		.index = 2,
+		.capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow   = SW_FREQ_RANGE_LOW * 16,
+		.rangehigh  = SW_FREQ_RANGE_HIGH * 16,
+		.modulation = V4L2_BAND_MODULATION_AM,
+	},
+};
+
+struct raremono_device {
+	struct usb_device *usbdev;
+	struct usb_interface *intf;
+	struct video_device vdev;
+	struct v4l2_device v4l2_dev;
+	struct mutex lock;
+
+	u8 *buffer;
+	u32 band;
+	unsigned curfreq;
+};
+
+static inline struct raremono_device *to_raremono_dev(struct v4l2_device *v4l2_dev)
+{
+	return container_of(v4l2_dev, struct raremono_device, v4l2_dev);
+}
+
+/* Set frequency. */
+static int raremono_cmd_main(struct raremono_device *radio, unsigned band, unsigned freq)
+{
+	unsigned band_offset;
+	int ret;
+
+	switch (band) {
+	case BAND_FM:
+		band_offset = 1;
+		freq /= 10;
+		break;
+	case BAND_AM:
+		band_offset = 0;
+		break;
+	default:
+		band_offset = 2;
+		break;
+	}
+	radio->buffer[0] = 0x04 + band_offset;
+	radio->buffer[1] = freq >> 8;
+	radio->buffer[2] = freq & 0xff;
+
+	ret = usb_control_msg(radio->usbdev, usb_sndctrlpipe(radio->usbdev, 0),
+			HID_REQ_SET_REPORT,
+			USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_OUT,
+			0x0300 + radio->buffer[0], 2,
+			radio->buffer, 3, USB_TIMEOUT);
+
+	if (ret < 0) {
+		dev_warn(radio->v4l2_dev.dev, "%s failed (%d)\n", __func__, ret);
+		return ret;
+	}
+	radio->curfreq = (band == BAND_FM) ? freq * 10 : freq;
+	return 0;
+}
+
+/* Handle unplugging the device.
+ * We call video_unregister_device in any case.
+ * The last function called in this procedure is
+ * usb_raremono_device_release.
+ */
+static void usb_raremono_disconnect(struct usb_interface *intf)
+{
+	struct raremono_device *radio = to_raremono_dev(usb_get_intfdata(intf));
+
+	dev_info(&intf->dev, "Thanko's Raremono disconnected\n");
+
+	mutex_lock(&radio->lock);
+	usb_set_intfdata(intf, NULL);
+	video_unregister_device(&radio->vdev);
+	v4l2_device_disconnect(&radio->v4l2_dev);
+	mutex_unlock(&radio->lock);
+	v4l2_device_put(&radio->v4l2_dev);
+}
+
+/*
+ * Linux Video interface
+ */
+static int vidioc_querycap(struct file *file, void *priv,
+					struct v4l2_capability *v)
+{
+	struct raremono_device *radio = video_drvdata(file);
+
+	strlcpy(v->driver, "radio-raremono", sizeof(v->driver));
+	strlcpy(v->card, "Thanko's Raremono", sizeof(v->card));
+	usb_make_path(radio->usbdev, v->bus_info, sizeof(v->bus_info));
+	v->device_caps = V4L2_CAP_TUNER | V4L2_CAP_RADIO;
+	v->capabilities = v->device_caps | V4L2_CAP_DEVICE_CAPS;
+	return 0;
+}
+
+static int vidioc_enum_freq_bands(struct file *file, void *priv,
+		struct v4l2_frequency_band *band)
+{
+	if (band->tuner != 0)
+		return -EINVAL;
+
+	if (band->index >= ARRAY_SIZE(bands))
+		return -EINVAL;
+
+	*band = bands[band->index];
+
+	return 0;
+}
+
+static int vidioc_g_tuner(struct file *file, void *priv,
+		struct v4l2_tuner *v)
+{
+	struct raremono_device *radio = video_drvdata(file);
+	int ret;
+
+	if (v->index > 0)
+		return -EINVAL;
+
+	strlcpy(v->name, "AM/FM/SW", sizeof(v->name));
+	v->capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_STEREO |
+		V4L2_TUNER_CAP_FREQ_BANDS;
+	v->rangelow = AM_FREQ_RANGE_LOW * 16;
+	v->rangehigh = FM_FREQ_RANGE_HIGH * 16;
+	v->rxsubchans = V4L2_TUNER_SUB_STEREO | V4L2_TUNER_SUB_MONO;
+	v->audmode = (radio->curfreq < FM_FREQ_RANGE_LOW) ?
+		V4L2_TUNER_MODE_MONO : V4L2_TUNER_MODE_STEREO;
+	memset(radio->buffer, 1, BUFFER_LENGTH);
+	ret = usb_control_msg(radio->usbdev, usb_rcvctrlpipe(radio->usbdev, 0),
+			1, 0xa1, 0x030d, 2, radio->buffer, BUFFER_LENGTH, USB_TIMEOUT);
+
+	if (ret < 0) {
+		dev_warn(radio->v4l2_dev.dev, "%s failed (%d)\n", __func__, ret);
+		return ret;
+	}
+	v->signal = ((radio->buffer[1] & 0xf) << 8 | radio->buffer[2]) << 4;
+	return 0;
+}
+
+static int vidioc_s_tuner(struct file *file, void *priv,
+					const struct v4l2_tuner *v)
+{
+	return v->index ? -EINVAL : 0;
+}
+
+static int vidioc_s_frequency(struct file *file, void *priv,
+				const struct v4l2_frequency *f)
+{
+	struct raremono_device *radio = video_drvdata(file);
+	u32 freq = f->frequency;
+	unsigned band;
+
+	if (f->tuner != 0 || f->type != V4L2_TUNER_RADIO)
+		return -EINVAL;
+
+	if (f->frequency >= (FM_FREQ_RANGE_LOW + SW_FREQ_RANGE_HIGH) * 8)
+		band = BAND_FM;
+	else if (f->frequency <= (AM_FREQ_RANGE_HIGH + SW_FREQ_RANGE_LOW) * 8)
+		band = BAND_AM;
+	else
+		band = BAND_SW;
+
+	freq = clamp_t(u32, f->frequency, bands[band].rangelow, bands[band].rangehigh);
+	return raremono_cmd_main(radio, band, freq / 16);
+}
+
+static int vidioc_g_frequency(struct file *file, void *priv,
+				struct v4l2_frequency *f)
+{
+	struct raremono_device *radio = video_drvdata(file);
+
+	if (f->tuner != 0)
+		return -EINVAL;
+	f->type = V4L2_TUNER_RADIO;
+	f->frequency = radio->curfreq * 16;
+	return 0;
+}
+
+/* File system interface */
+static const struct v4l2_file_operations usb_raremono_fops = {
+	.owner		= THIS_MODULE,
+	.open           = v4l2_fh_open,
+	.release        = v4l2_fh_release,
+	.unlocked_ioctl	= video_ioctl2,
+};
+
+static const struct v4l2_ioctl_ops usb_raremono_ioctl_ops = {
+	.vidioc_querycap = vidioc_querycap,
+	.vidioc_g_tuner = vidioc_g_tuner,
+	.vidioc_s_tuner = vidioc_s_tuner,
+	.vidioc_g_frequency = vidioc_g_frequency,
+	.vidioc_s_frequency = vidioc_s_frequency,
+	.vidioc_enum_freq_bands = vidioc_enum_freq_bands,
+};
+
+/* check if the device is present and register with v4l and usb if it is */
+static int usb_raremono_probe(struct usb_interface *intf,
+				const struct usb_device_id *id)
+{
+	struct raremono_device *radio;
+	int retval = 0;
+
+	radio = devm_kzalloc(&intf->dev, sizeof(struct raremono_device), GFP_KERNEL);
+	if (radio)
+		radio->buffer = devm_kmalloc(&intf->dev, BUFFER_LENGTH, GFP_KERNEL);
+
+	if (!radio || !radio->buffer)
+		return -ENOMEM;
+
+	radio->usbdev = interface_to_usbdev(intf);
+	radio->intf = intf;
+
+	/*
+	 * This device uses the same USB IDs as the si470x SiLabs reference
+	 * design. So do an additional check: attempt to read the device ID
+	 * from the si470x: the lower 12 bits are 0x0242 for the si470x. The
+	 * Raremono always returns 0x0800 (the meaning of that is unknown, but
+	 * at least it works).
+	 *
+	 * We use this check to determine which device we are dealing with.
+	 */
+	msleep(20);
+	retval = usb_control_msg(radio->usbdev,
+		usb_rcvctrlpipe(radio->usbdev, 0),
+		HID_REQ_GET_REPORT,
+		USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_IN,
+		1, 2,
+		radio->buffer, 3, 500);
+	if (retval != 3 ||
+	    (get_unaligned_be16(&radio->buffer[1]) & 0xfff) == 0x0242) {
+		dev_info(&intf->dev, "this is not Thanko's Raremono.\n");
+		return -ENODEV;
+	}
+
+	dev_info(&intf->dev, "Thanko's Raremono connected: (%04X:%04X)\n",
+			id->idVendor, id->idProduct);
+
+	retval = v4l2_device_register(&intf->dev, &radio->v4l2_dev);
+	if (retval < 0) {
+		dev_err(&intf->dev, "couldn't register v4l2_device\n");
+		return retval;
+	}
+
+	mutex_init(&radio->lock);
+
+	strlcpy(radio->vdev.name, radio->v4l2_dev.name,
+		sizeof(radio->vdev.name));
+	radio->vdev.v4l2_dev = &radio->v4l2_dev;
+	radio->vdev.fops = &usb_raremono_fops;
+	radio->vdev.ioctl_ops = &usb_raremono_ioctl_ops;
+	radio->vdev.lock = &radio->lock;
+	radio->vdev.release = video_device_release_empty;
+
+	usb_set_intfdata(intf, &radio->v4l2_dev);
+
+	video_set_drvdata(&radio->vdev, radio);
+	set_bit(V4L2_FL_USE_FH_PRIO, &radio->vdev.flags);
+
+	raremono_cmd_main(radio, BAND_FM, 95160);
+
+	retval = video_register_device(&radio->vdev, VFL_TYPE_RADIO, -1);
+	if (retval == 0) {
+		dev_info(&intf->dev, "V4L2 device registered as %s\n",
+				video_device_node_name(&radio->vdev));
+		return 0;
+	}
+	dev_err(&intf->dev, "could not register video device\n");
+	v4l2_device_unregister(&radio->v4l2_dev);
+	return retval;
+}
+
+/* USB subsystem interface */
+static struct usb_driver usb_raremono_driver = {
+	.name			= "radio-raremono",
+	.probe			= usb_raremono_probe,
+	.disconnect		= usb_raremono_disconnect,
+	.id_table		= usb_raremono_device_table,
+};
+
+module_usb_driver(usb_raremono_driver);
diff --git a/drivers/media/radio/si470x/radio-si470x-usb.c b/drivers/media/radio/si470x/radio-si470x-usb.c
index d6d4d60261d5..07ef40595efd 100644
--- a/drivers/media/radio/si470x/radio-si470x-usb.c
+++ b/drivers/media/radio/si470x/radio-si470x-usb.c
@@ -137,6 +137,8 @@ MODULE_PARM_DESC(max_rds_errors, "RDS maximum block errors: *1*");
 /* interrupt out endpoint 2 every 1 millisecond */
 #define UNUSED_REPORT		23
 
+#define MAX_REPORT_SIZE		64
+
 
 
 /**************************************************************************
@@ -208,7 +210,7 @@ MODULE_PARM_DESC(max_rds_errors, "RDS maximum block errors: *1*");
  */
 static int si470x_get_report(struct si470x_device *radio, void *buf, int size)
 {
-	unsigned char *report = (unsigned char *) buf;
+	unsigned char *report = buf;
 	int retval;
 
 	retval = usb_control_msg(radio->usbdev,
@@ -231,7 +233,7 @@ static int si470x_get_report(struct si470x_device *radio, void *buf, int size)
  */
 static int si470x_set_report(struct si470x_device *radio, void *buf, int size)
 {
-	unsigned char *report = (unsigned char *) buf;
+	unsigned char *report = buf;
 	int retval;
 
 	retval = usb_control_msg(radio->usbdev,
@@ -254,15 +256,14 @@ static int si470x_set_report(struct si470x_device *radio, void *buf, int size)
  */
 int si470x_get_register(struct si470x_device *radio, int regnr)
 {
-	unsigned char buf[REGISTER_REPORT_SIZE];
 	int retval;
 
-	buf[0] = REGISTER_REPORT(regnr);
+	radio->usb_buf[0] = REGISTER_REPORT(regnr);
 
-	retval = si470x_get_report(radio, (void *) &buf, sizeof(buf));
+	retval = si470x_get_report(radio, radio->usb_buf, REGISTER_REPORT_SIZE);
 
 	if (retval >= 0)
-		radio->registers[regnr] = get_unaligned_be16(&buf[1]);
+		radio->registers[regnr] = get_unaligned_be16(&radio->usb_buf[1]);
 
 	return (retval < 0) ? -EINVAL : 0;
 }
@@ -273,13 +274,12 @@ int si470x_get_register(struct si470x_device *radio, int regnr)
  */
 int si470x_set_register(struct si470x_device *radio, int regnr)
 {
-	unsigned char buf[REGISTER_REPORT_SIZE];
 	int retval;
 
-	buf[0] = REGISTER_REPORT(regnr);
-	put_unaligned_be16(radio->registers[regnr], &buf[1]);
+	radio->usb_buf[0] = REGISTER_REPORT(regnr);
+	put_unaligned_be16(radio->registers[regnr], &radio->usb_buf[1]);
 
-	retval = si470x_set_report(radio, (void *) &buf, sizeof(buf));
+	retval = si470x_set_report(radio, radio->usb_buf, REGISTER_REPORT_SIZE);
 
 	return (retval < 0) ? -EINVAL : 0;
 }
@@ -295,18 +295,17 @@ int si470x_set_register(struct si470x_device *radio, int regnr)
  */
 static int si470x_get_all_registers(struct si470x_device *radio)
 {
-	unsigned char buf[ENTIRE_REPORT_SIZE];
 	int retval;
 	unsigned char regnr;
 
-	buf[0] = ENTIRE_REPORT;
+	radio->usb_buf[0] = ENTIRE_REPORT;
 
-	retval = si470x_get_report(radio, (void *) &buf, sizeof(buf));
+	retval = si470x_get_report(radio, radio->usb_buf, ENTIRE_REPORT_SIZE);
 
 	if (retval >= 0)
 		for (regnr = 0; regnr < RADIO_REGISTER_NUM; regnr++)
 			radio->registers[regnr] = get_unaligned_be16(
-				&buf[regnr * RADIO_REGISTER_SIZE + 1]);
+				&radio->usb_buf[regnr * RADIO_REGISTER_SIZE + 1]);
 
 	return (retval < 0) ? -EINVAL : 0;
 }
@@ -323,14 +322,13 @@ static int si470x_get_all_registers(struct si470x_device *radio)
 static int si470x_set_led_state(struct si470x_device *radio,
 		unsigned char led_state)
 {
-	unsigned char buf[LED_REPORT_SIZE];
 	int retval;
 
-	buf[0] = LED_REPORT;
-	buf[1] = LED_COMMAND;
-	buf[2] = led_state;
+	radio->usb_buf[0] = LED_REPORT;
+	radio->usb_buf[1] = LED_COMMAND;
+	radio->usb_buf[2] = led_state;
 
-	retval = si470x_set_report(radio, (void *) &buf, sizeof(buf));
+	retval = si470x_set_report(radio, radio->usb_buf, LED_REPORT_SIZE);
 
 	return (retval < 0) ? -EINVAL : 0;
 }
@@ -346,19 +344,18 @@ static int si470x_set_led_state(struct si470x_device *radio,
  */
 static int si470x_get_scratch_page_versions(struct si470x_device *radio)
 {
-	unsigned char buf[SCRATCH_REPORT_SIZE];
 	int retval;
 
-	buf[0] = SCRATCH_REPORT;
+	radio->usb_buf[0] = SCRATCH_REPORT;
 
-	retval = si470x_get_report(radio, (void *) &buf, sizeof(buf));
+	retval = si470x_get_report(radio, radio->usb_buf, SCRATCH_REPORT_SIZE);
 
 	if (retval < 0)
 		dev_warn(&radio->intf->dev, "si470x_get_scratch: "
 			"si470x_get_report returned %d\n", retval);
 	else {
-		radio->software_version = buf[1];
-		radio->hardware_version = buf[2];
+		radio->software_version = radio->usb_buf[1];
+		radio->hardware_version = radio->usb_buf[2];
 	}
 
 	return (retval < 0) ? -EINVAL : 0;
@@ -509,6 +506,7 @@ static void si470x_usb_release(struct v4l2_device *v4l2_dev)
 	v4l2_device_unregister(&radio->v4l2_dev);
 	kfree(radio->int_in_buffer);
 	kfree(radio->buffer);
+	kfree(radio->usb_buf);
 	kfree(radio);
 }
 
@@ -593,6 +591,11 @@ static int si470x_usb_driver_probe(struct usb_interface *intf,
 		retval = -ENOMEM;
 		goto err_initial;
 	}
+	radio->usb_buf = kmalloc(MAX_REPORT_SIZE, GFP_KERNEL);
+	if (radio->usb_buf == NULL) {
+		retval = -ENOMEM;
+		goto err_radio;
+	}
 	radio->usbdev = interface_to_usbdev(intf);
 	radio->intf = intf;
 	radio->band = 1; /* Default to 76 - 108 MHz */
@@ -612,7 +615,7 @@ static int si470x_usb_driver_probe(struct usb_interface *intf,
 	if (!radio->int_in_endpoint) {
 		dev_info(&intf->dev, "could not find interrupt in endpoint\n");
 		retval = -EIO;
-		goto err_radio;
+		goto err_usbbuf;
 	}
 
 	int_end_size = le16_to_cpu(radio->int_in_endpoint->wMaxPacketSize);
@@ -621,7 +624,7 @@ static int si470x_usb_driver_probe(struct usb_interface *intf,
 	if (!radio->int_in_buffer) {
 		dev_info(&intf->dev, "could not allocate int_in_buffer");
 		retval = -ENOMEM;
-		goto err_radio;
+		goto err_usbbuf;
 	}
 
 	radio->int_in_urb = usb_alloc_urb(0, GFP_KERNEL);
@@ -632,6 +635,30 @@ static int si470x_usb_driver_probe(struct usb_interface *intf,
 	}
 
 	radio->v4l2_dev.release = si470x_usb_release;
+
+	/*
+	 * The si470x SiLabs reference design uses the same USB IDs as
+	 * 'Thanko's Raremono' si4734 based receiver. So check here which we
+	 * have: attempt to read the device ID from the si470x: the lower 12
+	 * bits should be 0x0242 for the si470x.
+	 *
+	 * We use this check to determine which device we are dealing with.
+	 */
+	if (id->idVendor == 0x10c4 && id->idProduct == 0x818a) {
+		retval = usb_control_msg(radio->usbdev,
+				usb_rcvctrlpipe(radio->usbdev, 0),
+				HID_REQ_GET_REPORT,
+				USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_IN,
+				1, 2,
+				radio->usb_buf, 3, 500);
+		if (retval != 3 ||
+		    (get_unaligned_be16(&radio->usb_buf[1]) & 0xfff) != 0x0242) {
+			dev_info(&intf->dev, "this is not a si470x device.\n");
+			retval = -ENODEV;
+			goto err_urb;
+		}
+	}
+
 	retval = v4l2_device_register(&intf->dev, &radio->v4l2_dev);
 	if (retval < 0) {
 		dev_err(&intf->dev, "couldn't register v4l2_device\n");
@@ -743,6 +770,8 @@ err_urb:
 	usb_free_urb(radio->int_in_urb);
 err_intbuffer:
 	kfree(radio->int_in_buffer);
+err_usbbuf:
+	kfree(radio->usb_buf);
 err_radio:
 	kfree(radio);
 err_initial:
diff --git a/drivers/media/radio/si470x/radio-si470x.h b/drivers/media/radio/si470x/radio-si470x.h
index 467e95575488..4b7660470e2f 100644
--- a/drivers/media/radio/si470x/radio-si470x.h
+++ b/drivers/media/radio/si470x/radio-si470x.h
@@ -167,6 +167,7 @@ struct si470x_device {
 	/* reference to USB and video device */
 	struct usb_device *usbdev;
 	struct usb_interface *intf;
+	char *usb_buf;
 
 	/* Interrupt endpoint handling */
 	char *int_in_buffer;
diff --git a/drivers/media/radio/si4713/Kconfig b/drivers/media/radio/si4713/Kconfig
new file mode 100644
index 000000000000..a7c3ba85d12b
--- /dev/null
+++ b/drivers/media/radio/si4713/Kconfig
@@ -0,0 +1,40 @@
+config USB_SI4713
+	tristate "Silicon Labs Si4713 FM Radio Transmitter support with USB"
+	depends on USB && RADIO_SI4713
+	select SI4713
+	---help---
+	  This is a driver for USB devices with the Silicon Labs SI4713
+	  chip. Currently these devices are known to work.
+	  - 10c4:8244: Silicon Labs FM Transmitter USB device.
+
+	  Say Y here if you want to connect this type of radio to your
+	  computer's USB port.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called radio-usb-si4713.
+
+config PLATFORM_SI4713
+	tristate "Silicon Labs Si4713 FM Radio Transmitter support with I2C"
+	depends on I2C && RADIO_SI4713
+	select SI4713
+	---help---
+	  This is a driver for I2C devices with the Silicon Labs SI4713
+	  chip.
+
+	  Say Y here if you want to connect this type of radio to your
+	  computer's I2C port.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called radio-platform-si4713.
+
+config I2C_SI4713
+	tristate "Silicon Labs Si4713 FM Radio Transmitter support"
+	depends on I2C && RADIO_SI4713
+	---help---
+	  Say Y here if you want support to Si4713 FM Radio Transmitter.
+	  This device can transmit audio through FM. It can transmit
+	  RDS and RBDS signals as well. This module is the v4l2 radio
+	  interface for the i2c driver of this device.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called si4713.
diff --git a/drivers/media/radio/si4713/Makefile b/drivers/media/radio/si4713/Makefile
new file mode 100644
index 000000000000..ddaaf925e883
--- /dev/null
+++ b/drivers/media/radio/si4713/Makefile
@@ -0,0 +1,7 @@
+#
+# Makefile for radios with Silicon Labs Si4713 FM Radio Transmitters
+#
+
+obj-$(CONFIG_I2C_SI4713) += si4713.o
+obj-$(CONFIG_USB_SI4713) += radio-usb-si4713.o
+obj-$(CONFIG_PLATFORM_SI4713) += radio-platform-si4713.o
diff --git a/drivers/media/radio/radio-si4713.c b/drivers/media/radio/si4713/radio-platform-si4713.c
index ba4cfc946868..ba4cfc946868 100644
--- a/drivers/media/radio/radio-si4713.c
+++ b/drivers/media/radio/si4713/radio-platform-si4713.c
diff --git a/drivers/media/radio/si4713/radio-usb-si4713.c b/drivers/media/radio/si4713/radio-usb-si4713.c
new file mode 100644
index 000000000000..779855b74bcd
--- /dev/null
+++ b/drivers/media/radio/si4713/radio-usb-si4713.c
@@ -0,0 +1,540 @@
+/*
+ * Copyright 2013 Cisco Systems, Inc. and/or its affiliates.
+ * All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+/* kernel includes */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/usb.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/input.h>
+#include <linux/mutex.h>
+#include <linux/i2c.h>
+/* V4l includes */
+#include <linux/videodev2.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
+#include <media/si4713.h>
+
+#include "si4713.h"
+
+/* driver and module definitions */
+MODULE_AUTHOR("Dinesh Ram <dinesh.ram@cern.ch>");
+MODULE_DESCRIPTION("Si4713 FM Transmitter USB driver");
+MODULE_LICENSE("GPL v2");
+
+/* The Device announces itself as Cygnal Integrated Products, Inc. */
+#define USB_SI4713_VENDOR		0x10c4
+#define USB_SI4713_PRODUCT		0x8244
+
+#define BUFFER_LENGTH			64
+#define USB_TIMEOUT			1000
+#define USB_RESP_TIMEOUT		50000
+
+/* USB Device ID List */
+static struct usb_device_id usb_si4713_usb_device_table[] = {
+	{USB_DEVICE_AND_INTERFACE_INFO(USB_SI4713_VENDOR, USB_SI4713_PRODUCT,
+							USB_CLASS_HID, 0, 0) },
+	{ }						/* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(usb, usb_si4713_usb_device_table);
+
+struct si4713_usb_device {
+	struct usb_device	*usbdev;
+	struct usb_interface	*intf;
+	struct video_device	vdev;
+	struct v4l2_device	v4l2_dev;
+	struct v4l2_subdev	*v4l2_subdev;
+	struct mutex		lock;
+	struct i2c_adapter	i2c_adapter;
+
+	u8			*buffer;
+};
+
+static inline struct si4713_usb_device *to_si4713_dev(struct v4l2_device *v4l2_dev)
+{
+	return container_of(v4l2_dev, struct si4713_usb_device, v4l2_dev);
+}
+
+static int vidioc_querycap(struct file *file, void *priv,
+					struct v4l2_capability *v)
+{
+	struct si4713_usb_device *radio = video_drvdata(file);
+
+	strlcpy(v->driver, "radio-usb-si4713", sizeof(v->driver));
+	strlcpy(v->card, "Si4713 FM Transmitter", sizeof(v->card));
+	usb_make_path(radio->usbdev, v->bus_info, sizeof(v->bus_info));
+	v->device_caps = V4L2_CAP_MODULATOR | V4L2_CAP_RDS_OUTPUT;
+	v->capabilities = v->device_caps | V4L2_CAP_DEVICE_CAPS;
+
+	return 0;
+}
+
+static int vidioc_g_modulator(struct file *file, void *priv,
+				struct v4l2_modulator *vm)
+{
+	struct si4713_usb_device *radio = video_drvdata(file);
+
+	return v4l2_subdev_call(radio->v4l2_subdev, tuner, g_modulator, vm);
+}
+
+static int vidioc_s_modulator(struct file *file, void *priv,
+				const struct v4l2_modulator *vm)
+{
+	struct si4713_usb_device *radio = video_drvdata(file);
+
+	return v4l2_subdev_call(radio->v4l2_subdev, tuner, s_modulator, vm);
+}
+
+static int vidioc_s_frequency(struct file *file, void *priv,
+				const struct v4l2_frequency *vf)
+{
+	struct si4713_usb_device *radio = video_drvdata(file);
+
+	return v4l2_subdev_call(radio->v4l2_subdev, tuner, s_frequency, vf);
+}
+
+static int vidioc_g_frequency(struct file *file, void *priv,
+				struct v4l2_frequency *vf)
+{
+	struct si4713_usb_device *radio = video_drvdata(file);
+
+	return v4l2_subdev_call(radio->v4l2_subdev, tuner, g_frequency, vf);
+}
+
+static const struct v4l2_ioctl_ops usb_si4713_ioctl_ops = {
+	.vidioc_querycap	  = vidioc_querycap,
+	.vidioc_g_modulator	  = vidioc_g_modulator,
+	.vidioc_s_modulator	  = vidioc_s_modulator,
+	.vidioc_g_frequency	  = vidioc_g_frequency,
+	.vidioc_s_frequency	  = vidioc_s_frequency,
+	.vidioc_log_status	  = v4l2_ctrl_log_status,
+	.vidioc_subscribe_event   = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+};
+
+/* File system interface */
+static const struct v4l2_file_operations usb_si4713_fops = {
+	.owner		= THIS_MODULE,
+	.open           = v4l2_fh_open,
+	.release        = v4l2_fh_release,
+	.poll           = v4l2_ctrl_poll,
+	.unlocked_ioctl	= video_ioctl2,
+};
+
+static void usb_si4713_video_device_release(struct v4l2_device *v4l2_dev)
+{
+	struct si4713_usb_device *radio = to_si4713_dev(v4l2_dev);
+	struct i2c_adapter *adapter = &radio->i2c_adapter;
+
+	i2c_del_adapter(adapter);
+	v4l2_device_unregister(&radio->v4l2_dev);
+	kfree(radio->buffer);
+	kfree(radio);
+}
+
+/*
+ * This command sequence emulates the behaviour of the Windows driver.
+ * The structure of these commands was determined by sniffing the
+ * usb traffic of the device during startup.
+ * Most likely, these commands make some queries to the device.
+ * Commands are sent to enquire parameters like the bus mode,
+ * component revision, boot mode, the device serial number etc.
+ *
+ * These commands are necessary to be sent in this order during startup.
+ * The device fails to powerup if these commands are not sent.
+ *
+ * The complete list of startup commands is given in the start_seq table below.
+ */
+static int si4713_send_startup_command(struct si4713_usb_device *radio)
+{
+	unsigned long until_jiffies = jiffies + usecs_to_jiffies(USB_RESP_TIMEOUT) + 1;
+	u8 *buffer = radio->buffer;
+	int retval;
+
+	/* send the command */
+	retval = usb_control_msg(radio->usbdev, usb_sndctrlpipe(radio->usbdev, 0),
+					0x09, 0x21, 0x033f, 0, radio->buffer,
+					BUFFER_LENGTH, USB_TIMEOUT);
+	if (retval < 0)
+		return retval;
+
+	for (;;) {
+		/* receive the response */
+		retval = usb_control_msg(radio->usbdev, usb_rcvctrlpipe(radio->usbdev, 0),
+				0x01, 0xa1, 0x033f, 0, radio->buffer,
+				BUFFER_LENGTH, USB_TIMEOUT);
+		if (retval < 0)
+			return retval;
+		if (!radio->buffer[1]) {
+			/* USB traffic sniffing showed that some commands require
+			 * additional checks. */
+			switch (buffer[1]) {
+			case 0x32:
+				if (radio->buffer[2] == 0)
+					return 0;
+				break;
+			case 0x14:
+			case 0x12:
+				if (radio->buffer[2] & SI4713_CTS)
+					return 0;
+				break;
+			case 0x06:
+				if ((radio->buffer[2] & SI4713_CTS) && radio->buffer[9] == 0x08)
+					return 0;
+				break;
+			default:
+				return 0;
+			}
+		}
+		if (time_is_before_jiffies(until_jiffies))
+			return -EIO;
+		msleep(3);
+	}
+
+	return retval;
+}
+
+struct si4713_start_seq_table {
+	int len;
+	u8 payload[8];
+};
+
+/*
+ * Some of the startup commands that could be recognized are :
+ * (0x03): Get serial number of the board (Response : CB000-00-00)
+ * (0x06, 0x03, 0x03, 0x08, 0x01, 0x0f) : Get Component revision
+ */
+static struct si4713_start_seq_table start_seq[] = {
+
+	{ 1, { 0x03 } },
+	{ 2, { 0x32, 0x7f } },
+	{ 6, { 0x06, 0x03, 0x03, 0x08, 0x01, 0x0f } },
+	{ 2, { 0x14, 0x02 } },
+	{ 2, { 0x09, 0x90 } },
+	{ 3, { 0x08, 0x90, 0xfa } },
+	{ 2, { 0x36, 0x01 } },
+	{ 2, { 0x05, 0x03 } },
+	{ 7, { 0x06, 0x00, 0x06, 0x0e, 0x01, 0x0f, 0x05 } },
+	{ 1, { 0x12 } },
+	/* Commands that are sent after pressing the 'Initialize'
+		button in the windows application */
+	{ 1, { 0x03 } },
+	{ 1, { 0x01 } },
+	{ 2, { 0x09, 0x90 } },
+	{ 3, { 0x08, 0x90, 0xfa } },
+	{ 1, { 0x34 } },
+	{ 2, { 0x35, 0x01 } },
+	{ 2, { 0x36, 0x01 } },
+	{ 2, { 0x30, 0x09 } },
+	{ 4, { 0x30, 0x06, 0x00, 0xe2 } },
+	{ 3, { 0x31, 0x01, 0x30 } },
+	{ 3, { 0x31, 0x04, 0x09 } },
+	{ 2, { 0x05, 0x02 } },
+	{ 6, { 0x06, 0x03, 0x03, 0x08, 0x01, 0x0f } },
+};
+
+static int si4713_start_seq(struct si4713_usb_device *radio)
+{
+	int retval = 0;
+	int i;
+
+	radio->buffer[0] = 0x3f;
+
+	for (i = 0; i < ARRAY_SIZE(start_seq); i++) {
+		int len = start_seq[i].len;
+		u8 *payload = start_seq[i].payload;
+
+		memcpy(radio->buffer + 1, payload, len);
+		memset(radio->buffer + len + 1, 0, BUFFER_LENGTH - 1 - len);
+		retval = si4713_send_startup_command(radio);
+	}
+
+	return retval;
+}
+
+static struct i2c_board_info si4713_board_info = {
+	I2C_BOARD_INFO("si4713", SI4713_I2C_ADDR_BUSEN_HIGH),
+};
+
+struct si4713_command_table {
+	int command_id;
+	u8 payload[8];
+};
+
+/*
+ * Structure of a command :
+ *	Byte 1 : 0x3f (always)
+ *	Byte 2 : 0x06 (send a command)
+ *	Byte 3 : Unknown
+ *	Byte 4 : Number of arguments + 1 (for the command byte)
+ *	Byte 5 : Number of response bytes
+ */
+static struct si4713_command_table command_table[] = {
+
+	{ SI4713_CMD_POWER_UP,		{ 0x00, SI4713_PWUP_NARGS + 1, SI4713_PWUP_NRESP} },
+	{ SI4713_CMD_GET_REV,		{ 0x03, 0x01, SI4713_GETREV_NRESP } },
+	{ SI4713_CMD_POWER_DOWN,	{ 0x00, 0x01, SI4713_PWDN_NRESP} },
+	{ SI4713_CMD_SET_PROPERTY,	{ 0x00, SI4713_SET_PROP_NARGS + 1, SI4713_SET_PROP_NRESP } },
+	{ SI4713_CMD_GET_PROPERTY,	{ 0x00, SI4713_GET_PROP_NARGS + 1, SI4713_GET_PROP_NRESP } },
+	{ SI4713_CMD_TX_TUNE_FREQ,	{ 0x03, SI4713_TXFREQ_NARGS + 1, SI4713_TXFREQ_NRESP } },
+	{ SI4713_CMD_TX_TUNE_POWER,	{ 0x03, SI4713_TXPWR_NARGS + 1, SI4713_TXPWR_NRESP } },
+	{ SI4713_CMD_TX_TUNE_MEASURE,	{ 0x03, SI4713_TXMEA_NARGS + 1, SI4713_TXMEA_NRESP } },
+	{ SI4713_CMD_TX_TUNE_STATUS,	{ 0x00, SI4713_TXSTATUS_NARGS + 1, SI4713_TXSTATUS_NRESP } },
+	{ SI4713_CMD_TX_ASQ_STATUS,	{ 0x03, SI4713_ASQSTATUS_NARGS + 1, SI4713_ASQSTATUS_NRESP } },
+	{ SI4713_CMD_GET_INT_STATUS,	{ 0x03, 0x01, SI4713_GET_STATUS_NRESP } },
+	{ SI4713_CMD_TX_RDS_BUFF,	{ 0x03, SI4713_RDSBUFF_NARGS + 1, SI4713_RDSBUFF_NRESP } },
+	{ SI4713_CMD_TX_RDS_PS,		{ 0x00, SI4713_RDSPS_NARGS + 1, SI4713_RDSPS_NRESP } },
+};
+
+static int send_command(struct si4713_usb_device *radio, u8 *payload, char *data, int len)
+{
+	int retval;
+
+	radio->buffer[0] = 0x3f;
+	radio->buffer[1] = 0x06;
+
+	memcpy(radio->buffer + 2, payload, 3);
+	memcpy(radio->buffer + 5, data, len);
+	memset(radio->buffer + 5 + len, 0, BUFFER_LENGTH - 5 - len);
+
+	/* send the command */
+	retval = usb_control_msg(radio->usbdev, usb_sndctrlpipe(radio->usbdev, 0),
+					0x09, 0x21, 0x033f, 0, radio->buffer,
+					BUFFER_LENGTH, USB_TIMEOUT);
+
+	return retval < 0 ? retval : 0;
+}
+
+static int si4713_i2c_read(struct si4713_usb_device *radio, char *data, int len)
+{
+	unsigned long until_jiffies = jiffies + usecs_to_jiffies(USB_RESP_TIMEOUT) + 1;
+	int retval;
+
+	/* receive the response */
+	for (;;) {
+		retval = usb_control_msg(radio->usbdev,
+					usb_rcvctrlpipe(radio->usbdev, 0),
+					0x01, 0xa1, 0x033f, 0, radio->buffer,
+					BUFFER_LENGTH, USB_TIMEOUT);
+		if (retval < 0)
+			return retval;
+
+		/*
+		 * Check that we get a valid reply back (buffer[1] == 0) and
+		 * that CTS is set before returning, otherwise we wait and try
+		 * again. The i2c driver also does the CTS check, but the timeouts
+		 * used there are much too small for this USB driver, so we wait
+		 * for it here.
+		 */
+		if (radio->buffer[1] == 0 && (radio->buffer[2] & SI4713_CTS)) {
+			memcpy(data, radio->buffer + 2, len);
+			return 0;
+		}
+		if (time_is_before_jiffies(until_jiffies)) {
+			/* Zero the status value, ensuring CTS isn't set */
+			data[0] = 0;
+			return 0;
+		}
+		msleep(3);
+	}
+}
+
+static int si4713_i2c_write(struct si4713_usb_device *radio, char *data, int len)
+{
+	int retval = -EINVAL;
+	int i;
+
+	if (len > BUFFER_LENGTH - 5)
+		return -EINVAL;
+
+	for (i = 0; i < ARRAY_SIZE(command_table); i++) {
+		if (data[0] == command_table[i].command_id)
+			retval = send_command(radio, command_table[i].payload,
+						data, len);
+	}
+
+	return retval < 0 ? retval : 0;
+}
+
+static int si4713_transfer(struct i2c_adapter *i2c_adapter,
+				struct i2c_msg *msgs, int num)
+{
+	struct si4713_usb_device *radio = i2c_get_adapdata(i2c_adapter);
+	int retval = -EINVAL;
+	int i;
+
+	if (num <= 0)
+		return 0;
+
+	for (i = 0; i < num; i++) {
+		if (msgs[i].flags & I2C_M_RD)
+			retval = si4713_i2c_read(radio, msgs[i].buf, msgs[i].len);
+		else
+			retval = si4713_i2c_write(radio, msgs[i].buf, msgs[i].len);
+		if (retval)
+			break;
+	}
+
+	return retval ? retval : num;
+}
+
+static u32 si4713_functionality(struct i2c_adapter *adapter)
+{
+	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
+}
+
+static struct i2c_algorithm si4713_algo = {
+	.master_xfer   = si4713_transfer,
+	.functionality = si4713_functionality,
+};
+
+/* This name value shows up in the sysfs filename associated
+		with this I2C adapter */
+static struct i2c_adapter si4713_i2c_adapter_template = {
+	.name   = "si4713-i2c",
+	.owner  = THIS_MODULE,
+	.algo   = &si4713_algo,
+};
+
+static int si4713_register_i2c_adapter(struct si4713_usb_device *radio)
+{
+	radio->i2c_adapter = si4713_i2c_adapter_template;
+	/* set up sysfs linkage to our parent device */
+	radio->i2c_adapter.dev.parent = &radio->usbdev->dev;
+	i2c_set_adapdata(&radio->i2c_adapter, radio);
+
+	return i2c_add_adapter(&radio->i2c_adapter);
+}
+
+/* check if the device is present and register with v4l and usb if it is */
+static int usb_si4713_probe(struct usb_interface *intf,
+				const struct usb_device_id *id)
+{
+	struct si4713_usb_device *radio;
+	struct i2c_adapter *adapter;
+	struct v4l2_subdev *sd;
+	int retval = -ENOMEM;
+
+	dev_info(&intf->dev, "Si4713 development board discovered: (%04X:%04X)\n",
+			id->idVendor, id->idProduct);
+
+	/* Initialize local device structure */
+	radio = kzalloc(sizeof(struct si4713_usb_device), GFP_KERNEL);
+	if (radio)
+		radio->buffer = kmalloc(BUFFER_LENGTH, GFP_KERNEL);
+
+	if (!radio || !radio->buffer) {
+		dev_err(&intf->dev, "kmalloc for si4713_usb_device failed\n");
+		kfree(radio);
+		return -ENOMEM;
+	}
+
+	mutex_init(&radio->lock);
+
+	radio->usbdev = interface_to_usbdev(intf);
+	radio->intf = intf;
+	usb_set_intfdata(intf, &radio->v4l2_dev);
+
+	retval = si4713_start_seq(radio);
+	if (retval < 0)
+		goto err_v4l2;
+
+	retval = v4l2_device_register(&intf->dev, &radio->v4l2_dev);
+	if (retval < 0) {
+		dev_err(&intf->dev, "couldn't register v4l2_device\n");
+		goto err_v4l2;
+	}
+
+	retval = si4713_register_i2c_adapter(radio);
+	if (retval < 0) {
+		dev_err(&intf->dev, "could not register i2c device\n");
+		goto err_i2cdev;
+	}
+
+	adapter = &radio->i2c_adapter;
+	sd = v4l2_i2c_new_subdev_board(&radio->v4l2_dev, adapter,
+					  &si4713_board_info, NULL);
+	radio->v4l2_subdev = sd;
+	if (!sd) {
+		dev_err(&intf->dev, "cannot get v4l2 subdevice\n");
+		retval = -ENODEV;
+		goto del_adapter;
+	}
+
+	radio->vdev.ctrl_handler = sd->ctrl_handler;
+	radio->v4l2_dev.release = usb_si4713_video_device_release;
+	strlcpy(radio->vdev.name, radio->v4l2_dev.name,
+		sizeof(radio->vdev.name));
+	radio->vdev.v4l2_dev = &radio->v4l2_dev;
+	radio->vdev.fops = &usb_si4713_fops;
+	radio->vdev.ioctl_ops = &usb_si4713_ioctl_ops;
+	radio->vdev.lock = &radio->lock;
+	radio->vdev.release = video_device_release_empty;
+	radio->vdev.vfl_dir = VFL_DIR_TX;
+
+	video_set_drvdata(&radio->vdev, radio);
+	set_bit(V4L2_FL_USE_FH_PRIO, &radio->vdev.flags);
+
+	retval = video_register_device(&radio->vdev, VFL_TYPE_RADIO, -1);
+	if (retval < 0) {
+		dev_err(&intf->dev, "could not register video device\n");
+		goto del_adapter;
+	}
+
+	dev_info(&intf->dev, "V4L2 device registered as %s\n",
+			video_device_node_name(&radio->vdev));
+
+	return 0;
+
+del_adapter:
+	i2c_del_adapter(adapter);
+err_i2cdev:
+	v4l2_device_unregister(&radio->v4l2_dev);
+err_v4l2:
+	kfree(radio->buffer);
+	kfree(radio);
+	return retval;
+}
+
+static void usb_si4713_disconnect(struct usb_interface *intf)
+{
+	struct si4713_usb_device *radio = to_si4713_dev(usb_get_intfdata(intf));
+
+	dev_info(&intf->dev, "Si4713 development board now disconnected\n");
+
+	mutex_lock(&radio->lock);
+	usb_set_intfdata(intf, NULL);
+	video_unregister_device(&radio->vdev);
+	v4l2_device_disconnect(&radio->v4l2_dev);
+	mutex_unlock(&radio->lock);
+	v4l2_device_put(&radio->v4l2_dev);
+}
+
+/* USB subsystem interface */
+static struct usb_driver usb_si4713_driver = {
+	.name			= "radio-usb-si4713",
+	.probe			= usb_si4713_probe,
+	.disconnect		= usb_si4713_disconnect,
+	.id_table		= usb_si4713_usb_device_table,
+};
+
+module_usb_driver(usb_si4713_driver);
diff --git a/drivers/media/radio/si4713-i2c.c b/drivers/media/radio/si4713/si4713.c
index 9ec48ccbcf0b..07d5153811e8 100644
--- a/drivers/media/radio/si4713-i2c.c
+++ b/drivers/media/radio/si4713/si4713.c
@@ -27,13 +27,12 @@
 #include <linux/i2c.h>
 #include <linux/slab.h>
 #include <linux/gpio.h>
-#include <linux/regulator/consumer.h>
 #include <linux/module.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-common.h>
 
-#include "si4713-i2c.h"
+#include "si4713.h"
 
 /* module parameters */
 static int debug;
@@ -45,23 +44,18 @@ MODULE_AUTHOR("Eduardo Valentin <eduardo.valentin@nokia.com>");
 MODULE_DESCRIPTION("I2C driver for Si4713 FM Radio Transmitter");
 MODULE_VERSION("0.0.1");
 
-static const char *si4713_supply_names[SI4713_NUM_SUPPLIES] = {
-	"vio",
-	"vdd",
-};
-
 #define DEFAULT_RDS_PI			0x00
 #define DEFAULT_RDS_PTY			0x00
 #define DEFAULT_RDS_DEVIATION		0x00C8
 #define DEFAULT_RDS_PS_REPEAT_COUNT	0x0003
 #define DEFAULT_LIMITER_RTIME		0x1392
 #define DEFAULT_LIMITER_DEV		0x102CA
-#define DEFAULT_PILOT_FREQUENCY 	0x4A38
+#define DEFAULT_PILOT_FREQUENCY		0x4A38
 #define DEFAULT_PILOT_DEVIATION		0x1A5E
 #define DEFAULT_ACOMP_ATIME		0x0000
 #define DEFAULT_ACOMP_RTIME		0xF4240L
 #define DEFAULT_ACOMP_GAIN		0x0F
-#define DEFAULT_ACOMP_THRESHOLD 	(-0x28)
+#define DEFAULT_ACOMP_THRESHOLD		(-0x28)
 #define DEFAULT_MUTE			0x01
 #define DEFAULT_POWER_LEVEL		88
 #define DEFAULT_FREQUENCY		8800
@@ -213,6 +207,7 @@ static int si4713_send_command(struct si4713_device *sdev, const u8 command,
 				u8 response[], const int respn, const int usecs)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(&sdev->sd);
+	unsigned long until_jiffies;
 	u8 data1[MAX_ARGS + 1];
 	int err;
 
@@ -228,30 +223,42 @@ static int si4713_send_command(struct si4713_device *sdev, const u8 command,
 	if (err != argn + 1) {
 		v4l2_err(&sdev->sd, "Error while sending command 0x%02x\n",
 			command);
-		return (err > 0) ? -EIO : err;
+		return err < 0 ? err : -EIO;
 	}
 
+	until_jiffies = jiffies + usecs_to_jiffies(usecs) + 1;
+
 	/* Wait response from interrupt */
-	if (!wait_for_completion_timeout(&sdev->work,
+	if (client->irq) {
+		if (!wait_for_completion_timeout(&sdev->work,
 				usecs_to_jiffies(usecs) + 1))
-		v4l2_warn(&sdev->sd,
+			v4l2_warn(&sdev->sd,
 				"(%s) Device took too much time to answer.\n",
 				__func__);
-
-	/* Then get the response */
-	err = i2c_master_recv(client, response, respn);
-	if (err != respn) {
-		v4l2_err(&sdev->sd,
-			"Error while reading response for command 0x%02x\n",
-			command);
-		return (err > 0) ? -EIO : err;
 	}
 
-	DBG_BUFFER(&sdev->sd, "Response", response, respn);
-	if (check_command_failed(response[0]))
-		return -EBUSY;
+	do {
+		err = i2c_master_recv(client, response, respn);
+		if (err != respn) {
+			v4l2_err(&sdev->sd,
+				"Error %d while reading response for command 0x%02x\n",
+				err, command);
+			return err < 0 ? err : -EIO;
+		}
 
-	return 0;
+		DBG_BUFFER(&sdev->sd, "Response", response, respn);
+		if (!check_command_failed(response[0]))
+			return 0;
+
+		if (client->irq)
+			return -EBUSY;
+		if (usecs <= 1000)
+			usleep_range(usecs, 1000);
+		else
+			usleep_range(1000, 2000);
+	} while (time_is_after_jiffies(until_jiffies));
+
+	return -EBUSY;
 }
 
 /*
@@ -265,9 +272,9 @@ static int si4713_read_property(struct si4713_device *sdev, u16 prop, u32 *pv)
 	int err;
 	u8 val[SI4713_GET_PROP_NRESP];
 	/*
-	 * 	.First byte = 0
-	 * 	.Second byte = property's MSB
-	 * 	.Third byte = property's LSB
+	 *	.First byte = 0
+	 *	.Second byte = property's MSB
+	 *	.Third byte = property's LSB
 	 */
 	const u8 args[SI4713_GET_PROP_NARGS] = {
 		0x00,
@@ -302,11 +309,11 @@ static int si4713_write_property(struct si4713_device *sdev, u16 prop, u16 val)
 	int rval;
 	u8 resp[SI4713_SET_PROP_NRESP];
 	/*
-	 * 	.First byte = 0
-	 * 	.Second byte = property's MSB
-	 * 	.Third byte = property's LSB
-	 * 	.Fourth byte = value's MSB
-	 * 	.Fifth byte = value's LSB
+	 *	.First byte = 0
+	 *	.Second byte = property's MSB
+	 *	.Third byte = property's LSB
+	 *	.Fourth byte = value's MSB
+	 *	.Fifth byte = value's LSB
 	 */
 	const u8 args[SI4713_SET_PROP_NARGS] = {
 		0x00,
@@ -344,31 +351,36 @@ static int si4713_write_property(struct si4713_device *sdev, u16 prop, u16 val)
  */
 static int si4713_powerup(struct si4713_device *sdev)
 {
+	struct i2c_client *client = v4l2_get_subdevdata(&sdev->sd);
 	int err;
 	u8 resp[SI4713_PWUP_NRESP];
 	/*
-	 * 	.First byte = Enabled interrupts and boot function
-	 * 	.Second byte = Input operation mode
+	 *	.First byte = Enabled interrupts and boot function
+	 *	.Second byte = Input operation mode
 	 */
-	const u8 args[SI4713_PWUP_NARGS] = {
-		SI4713_PWUP_CTSIEN | SI4713_PWUP_GPO2OEN | SI4713_PWUP_FUNC_TX,
+	u8 args[SI4713_PWUP_NARGS] = {
+		SI4713_PWUP_GPO2OEN | SI4713_PWUP_FUNC_TX,
 		SI4713_PWUP_OPMOD_ANALOG,
 	};
 
 	if (sdev->power_state)
 		return 0;
 
-	err = regulator_bulk_enable(ARRAY_SIZE(sdev->supplies),
-				    sdev->supplies);
-	if (err) {
-		v4l2_err(&sdev->sd, "Failed to enable supplies: %d\n", err);
-		return err;
+	if (sdev->supplies) {
+		err = regulator_bulk_enable(sdev->supplies, sdev->supply_data);
+		if (err) {
+			v4l2_err(&sdev->sd, "Failed to enable supplies: %d\n", err);
+			return err;
+		}
 	}
 	if (gpio_is_valid(sdev->gpio_reset)) {
 		udelay(50);
 		gpio_set_value(sdev->gpio_reset, 1);
 	}
 
+	if (client->irq)
+		args[0] |= SI4713_PWUP_CTSIEN;
+
 	err = si4713_send_command(sdev, SI4713_CMD_POWER_UP,
 					args, ARRAY_SIZE(args),
 					resp, ARRAY_SIZE(resp),
@@ -380,13 +392,15 @@ static int si4713_powerup(struct si4713_device *sdev)
 		v4l2_dbg(1, debug, &sdev->sd, "Device in power up mode\n");
 		sdev->power_state = POWER_ON;
 
-		err = si4713_write_property(sdev, SI4713_GPO_IEN,
+		if (client->irq)
+			err = si4713_write_property(sdev, SI4713_GPO_IEN,
 						SI4713_STC_INT | SI4713_CTS);
-	} else {
-		if (gpio_is_valid(sdev->gpio_reset))
-			gpio_set_value(sdev->gpio_reset, 0);
-		err = regulator_bulk_disable(ARRAY_SIZE(sdev->supplies),
-					     sdev->supplies);
+		return err;
+	}
+	if (gpio_is_valid(sdev->gpio_reset))
+		gpio_set_value(sdev->gpio_reset, 0);
+	if (sdev->supplies) {
+		err = regulator_bulk_disable(sdev->supplies, sdev->supply_data);
 		if (err)
 			v4l2_err(&sdev->sd,
 				 "Failed to disable supplies: %d\n", err);
@@ -418,11 +432,13 @@ static int si4713_powerdown(struct si4713_device *sdev)
 		v4l2_dbg(1, debug, &sdev->sd, "Device in reset mode\n");
 		if (gpio_is_valid(sdev->gpio_reset))
 			gpio_set_value(sdev->gpio_reset, 0);
-		err = regulator_bulk_disable(ARRAY_SIZE(sdev->supplies),
-					     sdev->supplies);
-		if (err)
-			v4l2_err(&sdev->sd,
-				 "Failed to disable supplies: %d\n", err);
+		if (sdev->supplies) {
+			err = regulator_bulk_disable(sdev->supplies,
+						     sdev->supply_data);
+			if (err)
+				v4l2_err(&sdev->sd,
+					 "Failed to disable supplies: %d\n", err);
+		}
 		sdev->power_state = POWER_OFF;
 	}
 
@@ -451,7 +467,7 @@ static int si4713_checkrev(struct si4713_device *sdev)
 		v4l2_info(&sdev->sd, "chip found @ 0x%02x (%s)\n",
 				client->addr << 1, client->adapter->name);
 	} else {
-		v4l2_err(&sdev->sd, "Invalid product number\n");
+		v4l2_err(&sdev->sd, "Invalid product number 0x%X\n", resp[1]);
 		rval = -EINVAL;
 	}
 	return rval;
@@ -465,39 +481,45 @@ static int si4713_checkrev(struct si4713_device *sdev)
  */
 static int si4713_wait_stc(struct si4713_device *sdev, const int usecs)
 {
-	int err;
+	struct i2c_client *client = v4l2_get_subdevdata(&sdev->sd);
 	u8 resp[SI4713_GET_STATUS_NRESP];
+	unsigned long start_jiffies = jiffies;
+	int err;
 
-	/* Wait response from STC interrupt */
-	if (!wait_for_completion_timeout(&sdev->work,
-			usecs_to_jiffies(usecs) + 1))
+	if (client->irq &&
+	    !wait_for_completion_timeout(&sdev->work, usecs_to_jiffies(usecs) + 1))
 		v4l2_warn(&sdev->sd,
-			"%s: device took too much time to answer (%d usec).\n",
-				__func__, usecs);
-
-	/* Clear status bits */
-	err = si4713_send_command(sdev, SI4713_CMD_GET_INT_STATUS,
-					NULL, 0,
-					resp, ARRAY_SIZE(resp),
-					DEFAULT_TIMEOUT);
-
-	if (err < 0)
-		goto exit;
-
-	v4l2_dbg(1, debug, &sdev->sd,
-			"%s: status bits: 0x%02x\n", __func__, resp[0]);
-
-	if (!(resp[0] & SI4713_STC_INT))
-		err = -EIO;
-
-exit:
-	return err;
+			"(%s) Device took too much time to answer.\n", __func__);
+
+	for (;;) {
+		/* Clear status bits */
+		err = si4713_send_command(sdev, SI4713_CMD_GET_INT_STATUS,
+				NULL, 0,
+				resp, ARRAY_SIZE(resp),
+				DEFAULT_TIMEOUT);
+		/* The USB device returns errors when it waits for the
+		 * STC bit to be set. Hence polling */
+		if (err >= 0) {
+			v4l2_dbg(1, debug, &sdev->sd,
+				"%s: status bits: 0x%02x\n", __func__, resp[0]);
+
+			if (resp[0] & SI4713_STC_INT)
+				return 0;
+		}
+		if (jiffies_to_usecs(jiffies - start_jiffies) > usecs)
+			return err < 0 ? err : -EIO;
+		/* We sleep here for 3-4 ms in order to avoid flooding the device
+		 * with USB requests. The si4713 USB driver was developed
+		 * by reverse engineering the Windows USB driver. The windows
+		 * driver also has a ~2.5 ms delay between responses. */
+		usleep_range(3000, 4000);
+	}
 }
 
 /*
  * si4713_tx_tune_freq - Sets the state of the RF carrier and sets the tuning
- * 			frequency between 76 and 108 MHz in 10 kHz units and
- * 			steps of 50 kHz.
+ *			frequency between 76 and 108 MHz in 10 kHz units and
+ *			steps of 50 kHz.
  * @sdev: si4713_device structure for the device we are communicating
  * @frequency: desired frequency (76 - 108 MHz, unit 10 KHz, step 50 kHz)
  */
@@ -506,9 +528,9 @@ static int si4713_tx_tune_freq(struct si4713_device *sdev, u16 frequency)
 	int err;
 	u8 val[SI4713_TXFREQ_NRESP];
 	/*
-	 * 	.First byte = 0
-	 * 	.Second byte = frequency's MSB
-	 * 	.Third byte = frequency's LSB
+	 *	.First byte = 0
+	 *	.Second byte = frequency's MSB
+	 *	.Third byte = frequency's LSB
 	 */
 	const u8 args[SI4713_TXFREQ_NARGS] = {
 		0x00,
@@ -535,14 +557,14 @@ static int si4713_tx_tune_freq(struct si4713_device *sdev, u16 frequency)
 }
 
 /*
- * si4713_tx_tune_power - Sets the RF voltage level between 88 and 115 dBuV in
- * 			1 dB units. A value of 0x00 indicates off. The command
- * 			also sets the antenna tuning capacitance. A value of 0
- * 			indicates autotuning, and a value of 1 - 191 indicates
- * 			a manual override, which results in a tuning
- * 			capacitance of 0.25 pF x @antcap.
+ * si4713_tx_tune_power - Sets the RF voltage level between 88 and 120 dBuV in
+ *			1 dB units. A value of 0x00 indicates off. The command
+ *			also sets the antenna tuning capacitance. A value of 0
+ *			indicates autotuning, and a value of 1 - 191 indicates
+ *			a manual override, which results in a tuning
+ *			capacitance of 0.25 pF x @antcap.
  * @sdev: si4713_device structure for the device we are communicating
- * @power: tuning power (88 - 115 dBuV, unit/step 1 dB)
+ * @power: tuning power (88 - 120 dBuV, unit/step 1 dB)
  * @antcap: value of antenna tuning capacitor (0 - 191)
  */
 static int si4713_tx_tune_power(struct si4713_device *sdev, u8 power,
@@ -551,21 +573,21 @@ static int si4713_tx_tune_power(struct si4713_device *sdev, u8 power,
 	int err;
 	u8 val[SI4713_TXPWR_NRESP];
 	/*
-	 * 	.First byte = 0
-	 * 	.Second byte = 0
-	 * 	.Third byte = power
-	 * 	.Fourth byte = antcap
+	 *	.First byte = 0
+	 *	.Second byte = 0
+	 *	.Third byte = power
+	 *	.Fourth byte = antcap
 	 */
-	const u8 args[SI4713_TXPWR_NARGS] = {
+	u8 args[SI4713_TXPWR_NARGS] = {
 		0x00,
 		0x00,
 		power,
 		antcap,
 	};
 
-	if (((power > 0) && (power < SI4713_MIN_POWER)) ||
-		power > SI4713_MAX_POWER || antcap > SI4713_MAX_ANTCAP)
-		return -EDOM;
+	/* Map power values 1-87 to MIN_POWER (88) */
+	if (power > 0 && power < SI4713_MIN_POWER)
+		args[2] = power = SI4713_MIN_POWER;
 
 	err = si4713_send_command(sdev, SI4713_CMD_TX_TUNE_POWER,
 				  args, ARRAY_SIZE(args), val,
@@ -583,12 +605,12 @@ static int si4713_tx_tune_power(struct si4713_device *sdev, u8 power,
 
 /*
  * si4713_tx_tune_measure - Enters receive mode and measures the received noise
- * 			level in units of dBuV on the selected frequency.
- * 			The Frequency must be between 76 and 108 MHz in 10 kHz
- * 			units and steps of 50 kHz. The command also sets the
- * 			antenna	tuning capacitance. A value of 0 means
- * 			autotuning, and a value of 1 to 191 indicates manual
- * 			override.
+ *			level in units of dBuV on the selected frequency.
+ *			The Frequency must be between 76 and 108 MHz in 10 kHz
+ *			units and steps of 50 kHz. The command also sets the
+ *			antenna	tuning capacitance. A value of 0 means
+ *			autotuning, and a value of 1 to 191 indicates manual
+ *			override.
  * @sdev: si4713_device structure for the device we are communicating
  * @frequency: desired frequency (76 - 108 MHz, unit 10 KHz, step 50 kHz)
  * @antcap: value of antenna tuning capacitor (0 - 191)
@@ -599,10 +621,10 @@ static int si4713_tx_tune_measure(struct si4713_device *sdev, u16 frequency,
 	int err;
 	u8 val[SI4713_TXMEA_NRESP];
 	/*
-	 * 	.First byte = 0
-	 * 	.Second byte = frequency's MSB
-	 * 	.Third byte = frequency's LSB
-	 * 	.Fourth byte = antcap
+	 *	.First byte = 0
+	 *	.Second byte = frequency's MSB
+	 *	.Third byte = frequency's LSB
+	 *	.Fourth byte = antcap
 	 */
 	const u8 args[SI4713_TXMEA_NARGS] = {
 		0x00,
@@ -632,11 +654,11 @@ static int si4713_tx_tune_measure(struct si4713_device *sdev, u16 frequency,
 
 /*
  * si4713_tx_tune_status- Returns the status of the tx_tune_freq, tx_tune_mea or
- * 			tx_tune_power commands. This command return the current
- * 			frequency, output voltage in dBuV, the antenna tunning
- * 			capacitance value and the received noise level. The
- * 			command also clears the stcint interrupt bit when the
- * 			first bit of its arguments is high.
+ *			tx_tune_power commands. This command return the current
+ *			frequency, output voltage in dBuV, the antenna tunning
+ *			capacitance value and the received noise level. The
+ *			command also clears the stcint interrupt bit when the
+ *			first bit of its arguments is high.
  * @sdev: si4713_device structure for the device we are communicating
  * @intack: 0x01 to clear the seek/tune complete interrupt status indicator.
  * @frequency: returned frequency
@@ -651,7 +673,7 @@ static int si4713_tx_tune_status(struct si4713_device *sdev, u8 intack,
 	int err;
 	u8 val[SI4713_TXSTATUS_NRESP];
 	/*
-	 * 	.First byte = intack bit
+	 *	.First byte = intack bit
 	 */
 	const u8 args[SI4713_TXSTATUS_NARGS] = {
 		intack & SI4713_INTACK_MASK,
@@ -812,8 +834,9 @@ static int si4713_set_rds_ps_name(struct si4713_device *sdev, char *ps_name)
 	return rval;
 }
 
-static int si4713_set_rds_radio_text(struct si4713_device *sdev, char *rt)
+static int si4713_set_rds_radio_text(struct si4713_device *sdev, const char *rt)
 {
+	static const char cr[RDS_RADIOTEXT_BLK_SIZE] = { RDS_CARRIAGE_RETURN, 0 };
 	int rval = 0, i;
 	u16 t_index = 0;
 	u8 b_index = 0, cr_inserted = 0;
@@ -837,7 +860,7 @@ static int si4713_set_rds_radio_text(struct si4713_device *sdev, char *rt)
 			for (i = 0; i < RDS_RADIOTEXT_BLK_SIZE; i++) {
 				if (!rt[t_index + i] ||
 				    rt[t_index + i] == RDS_CARRIAGE_RETURN) {
-					rt[t_index + i] = RDS_CARRIAGE_RETURN;
+					rt = cr;
 					cr_inserted = 1;
 					break;
 				}
@@ -1024,7 +1047,6 @@ static int si4713_initialize(struct si4713_device *sdev)
 	if (rval < 0)
 		return rval;
 
-
 	sdev->frequency = DEFAULT_FREQUENCY;
 	sdev->stereo = 1;
 	sdev->tune_rnl = DEFAULT_TUNE_RNL;
@@ -1345,7 +1367,7 @@ static int si4713_probe(struct i2c_client *client,
 	struct v4l2_ctrl_handler *hdl;
 	int rval, i;
 
-	sdev = kzalloc(sizeof *sdev, GFP_KERNEL);
+	sdev = kzalloc(sizeof(*sdev), GFP_KERNEL);
 	if (!sdev) {
 		dev_err(&client->dev, "Failed to alloc video device.\n");
 		rval = -ENOMEM;
@@ -1362,13 +1384,14 @@ static int si4713_probe(struct i2c_client *client,
 		}
 		sdev->gpio_reset = pdata->gpio_reset;
 		gpio_direction_output(sdev->gpio_reset, 0);
+		sdev->supplies = pdata->supplies;
 	}
 
-	for (i = 0; i < ARRAY_SIZE(sdev->supplies); i++)
-		sdev->supplies[i].supply = si4713_supply_names[i];
+	for (i = 0; i < sdev->supplies; i++)
+		sdev->supply_data[i].supply = pdata->supply_names[i];
 
-	rval = regulator_bulk_get(&client->dev, ARRAY_SIZE(sdev->supplies),
-				  sdev->supplies);
+	rval = regulator_bulk_get(&client->dev, sdev->supplies,
+				  sdev->supply_data);
 	if (rval) {
 		dev_err(&client->dev, "Cannot get regulators: %d\n", rval);
 		goto free_gpio;
@@ -1420,8 +1443,8 @@ static int si4713_probe(struct i2c_client *client,
 			V4L2_CID_AUDIO_COMPRESSION_GAIN, 0, MAX_ACOMP_GAIN, 1,
 			DEFAULT_ACOMP_GAIN);
 	sdev->compression_threshold = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
-			V4L2_CID_AUDIO_COMPRESSION_THRESHOLD, MIN_ACOMP_THRESHOLD,
-			MAX_ACOMP_THRESHOLD, 1,
+			V4L2_CID_AUDIO_COMPRESSION_THRESHOLD,
+			MIN_ACOMP_THRESHOLD, MAX_ACOMP_THRESHOLD, 1,
 			DEFAULT_ACOMP_THRESHOLD);
 	sdev->compression_attack_time = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
 			V4L2_CID_AUDIO_COMPRESSION_ATTACK_TIME, 0,
@@ -1443,9 +1466,11 @@ static int si4713_probe(struct i2c_client *client,
 			V4L2_CID_TUNE_PREEMPHASIS,
 			V4L2_PREEMPHASIS_75_uS, 0, V4L2_PREEMPHASIS_50_uS);
 	sdev->tune_pwr_level = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
-			V4L2_CID_TUNE_POWER_LEVEL, 0, 120, 1, DEFAULT_POWER_LEVEL);
+			V4L2_CID_TUNE_POWER_LEVEL, 0, SI4713_MAX_POWER,
+			1, DEFAULT_POWER_LEVEL);
 	sdev->tune_ant_cap = v4l2_ctrl_new_std(hdl, &si4713_ctrl_ops,
-			V4L2_CID_TUNE_ANTENNA_CAPACITOR, 0, 191, 1, 0);
+			V4L2_CID_TUNE_ANTENNA_CAPACITOR, 0, SI4713_MAX_ANTCAP,
+			1, 0);
 
 	if (hdl->error) {
 		rval = hdl->error;
@@ -1481,7 +1506,7 @@ free_irq:
 free_ctrls:
 	v4l2_ctrl_handler_free(hdl);
 put_reg:
-	regulator_bulk_free(ARRAY_SIZE(sdev->supplies), sdev->supplies);
+	regulator_bulk_free(sdev->supplies, sdev->supply_data);
 free_gpio:
 	if (gpio_is_valid(sdev->gpio_reset))
 		gpio_free(sdev->gpio_reset);
@@ -1505,7 +1530,7 @@ static int si4713_remove(struct i2c_client *client)
 
 	v4l2_device_unregister_subdev(sd);
 	v4l2_ctrl_handler_free(sd->ctrl_handler);
-	regulator_bulk_free(ARRAY_SIZE(sdev->supplies), sdev->supplies);
+	regulator_bulk_free(sdev->supplies, sdev->supply_data);
 	if (gpio_is_valid(sdev->gpio_reset))
 		gpio_free(sdev->gpio_reset);
 	kfree(sdev);
diff --git a/drivers/media/radio/si4713-i2c.h b/drivers/media/radio/si4713/si4713.h
index 25cdea26343b..4837cf6e0e1b 100644
--- a/drivers/media/radio/si4713-i2c.h
+++ b/drivers/media/radio/si4713/si4713.h
@@ -15,6 +15,7 @@
 #ifndef SI4713_I2C_H
 #define SI4713_I2C_H
 
+#include <linux/regulator/consumer.h>
 #include <media/v4l2-subdev.h>
 #include <media/v4l2-ctrls.h>
 #include <media/si4713.h>
@@ -226,7 +227,8 @@ struct si4713_device {
 		struct v4l2_ctrl *tune_ant_cap;
 	};
 	struct completion work;
-	struct regulator_bulk_data supplies[SI4713_NUM_SUPPLIES];
+	unsigned supplies;
+	struct regulator_bulk_data supply_data[SI4713_NUM_SUPPLIES];
 	int gpio_reset;
 	u32 power_state;
 	u32 rds_enabled;
diff --git a/drivers/media/radio/tea575x.c b/drivers/media/radio/tea575x.c
index cef06981b7c9..7c14060a40b8 100644
--- a/drivers/media/radio/tea575x.c
+++ b/drivers/media/radio/tea575x.c
@@ -20,12 +20,12 @@
  *
  */
 
-#include <asm/io.h>
 #include <linux/delay.h>
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/slab.h>
 #include <linux/sched.h>
+#include <asm/io.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-dev.h>
 #include <media/v4l2-fh.h>
diff --git a/drivers/media/rc/imon.c b/drivers/media/rc/imon.c
index f329485c6629..822b9f47ca72 100644
--- a/drivers/media/rc/imon.c
+++ b/drivers/media/rc/imon.c
@@ -1909,10 +1909,8 @@ static struct input_dev *imon_init_idev(struct imon_context *ictx)
 	int ret, i;
 
 	idev = input_allocate_device();
-	if (!idev) {
-		dev_err(ictx->dev, "input dev allocation failed\n");
+	if (!idev)
 		goto out;
-	}
 
 	snprintf(ictx->name_idev, sizeof(ictx->name_idev),
 		 "iMON Panel, Knob and Mouse(%04x:%04x)",
@@ -1960,10 +1958,8 @@ static struct input_dev *imon_init_touch(struct imon_context *ictx)
 	int ret;
 
 	touch = input_allocate_device();
-	if (!touch) {
-		dev_err(ictx->dev, "touchscreen input dev allocation failed\n");
+	if (!touch)
 		goto touch_alloc_failed;
-	}
 
 	snprintf(ictx->name_touch, sizeof(ictx->name_touch),
 		 "iMON USB Touchscreen (%04x:%04x)",
diff --git a/drivers/media/rc/keymaps/Makefile b/drivers/media/rc/keymaps/Makefile
index b1cde8c0422b..0b8c54919010 100644
--- a/drivers/media/rc/keymaps/Makefile
+++ b/drivers/media/rc/keymaps/Makefile
@@ -98,4 +98,5 @@ obj-$(CONFIG_RC_MAP) += rc-adstech-dvb-t-pci.o \
 			rc-videomate-s350.o \
 			rc-videomate-tv-pvr.o \
 			rc-winfast.o \
-			rc-winfast-usbii-deluxe.o
+			rc-winfast-usbii-deluxe.o \
+			rc-su3000.o
diff --git a/drivers/media/rc/keymaps/rc-su3000.c b/drivers/media/rc/keymaps/rc-su3000.c
new file mode 100644
index 000000000000..8dbd3e9bc951
--- /dev/null
+++ b/drivers/media/rc/keymaps/rc-su3000.c
@@ -0,0 +1,75 @@
+/* rc-su3000.h - Keytable for Geniatech HDStar Remote Controller
+ *
+ * Copyright (c) 2013 by Evgeny Plehov <Evgeny Plehov@ukr.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.
+ */
+
+#include <media/rc-map.h>
+#include <linux/module.h>
+
+static struct rc_map_table su3000[] = {
+	{ 0x25, KEY_POWER },	/* right-bottom Red */
+	{ 0x0a, KEY_MUTE },	/* -/-- */
+	{ 0x01, KEY_1 },
+	{ 0x02, KEY_2 },
+	{ 0x03, KEY_3 },
+	{ 0x04, KEY_4 },
+	{ 0x05, KEY_5 },
+	{ 0x06, KEY_6 },
+	{ 0x07, KEY_7 },
+	{ 0x08, KEY_8 },
+	{ 0x09, KEY_9 },
+	{ 0x00, KEY_0 },
+	{ 0x20, KEY_UP },	/* CH+ */
+	{ 0x21, KEY_DOWN },	/* CH+ */
+	{ 0x12, KEY_VOLUMEUP },	/* Brightness Up */
+	{ 0x13, KEY_VOLUMEDOWN },/* Brightness Down */
+	{ 0x1f, KEY_RECORD },
+	{ 0x17, KEY_PLAY },
+	{ 0x16, KEY_PAUSE },
+	{ 0x0b, KEY_STOP },
+	{ 0x27, KEY_FASTFORWARD },/* >> */
+	{ 0x26, KEY_REWIND },	/* << */
+	{ 0x0d, KEY_OK },	/* Mute */
+	{ 0x11, KEY_LEFT },	/* VOL- */
+	{ 0x10, KEY_RIGHT },	/* VOL+ */
+	{ 0x29, KEY_BACK },	/* button under 9 */
+	{ 0x2c, KEY_MENU },	/* TTX */
+	{ 0x2b, KEY_EPG },	/* EPG */
+	{ 0x1e, KEY_RED },	/* OSD */
+	{ 0x0e, KEY_GREEN },	/* Window */
+	{ 0x2d, KEY_YELLOW },	/* button under << */
+	{ 0x0f, KEY_BLUE },	/* bottom yellow button */
+	{ 0x14, KEY_AUDIO },	/* Snapshot */
+	{ 0x38, KEY_TV },	/* TV/Radio */
+	{ 0x0c, KEY_ESC }	/* upper Red button */
+};
+
+static struct rc_map_list su3000_map = {
+	.map = {
+		.scan    = su3000,
+		.size    = ARRAY_SIZE(su3000),
+		.rc_type = RC_TYPE_RC5,
+		.name    = RC_MAP_SU3000,
+	}
+};
+
+static int __init init_rc_map_su3000(void)
+{
+	return rc_map_register(&su3000_map);
+}
+
+static void __exit exit_rc_map_su3000(void)
+{
+	rc_map_unregister(&su3000_map);
+}
+
+module_init(init_rc_map_su3000)
+module_exit(exit_rc_map_su3000)
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Evgeny Plehov <Evgeny Plehov@ukr.net>");
diff --git a/drivers/media/rc/mceusb.c b/drivers/media/rc/mceusb.c
index 3c761014d3ce..a25bb1581e46 100644
--- a/drivers/media/rc/mceusb.c
+++ b/drivers/media/rc/mceusb.c
@@ -199,6 +199,7 @@ static bool debug;
 #define VENDOR_TIVO		0x105a
 #define VENDOR_CONEXANT		0x0572
 #define VENDOR_TWISTEDMELON	0x2596
+#define VENDOR_HAUPPAUGE	0x2040
 
 enum mceusb_model_type {
 	MCE_GEN2 = 0,		/* Most boards */
@@ -210,6 +211,7 @@ enum mceusb_model_type {
 	MULTIFUNCTION,
 	TIVO_KIT,
 	MCE_GEN2_NO_TX,
+	HAUPPAUGE_CX_HYBRID_TV,
 };
 
 struct mceusb_model {
@@ -258,6 +260,11 @@ static const struct mceusb_model mceusb_model[] = {
 		.no_tx = 1, /* tx isn't wired up at all */
 		.name = "Conexant Hybrid TV (cx231xx) MCE IR",
 	},
+	[HAUPPAUGE_CX_HYBRID_TV] = {
+		.rc_map = RC_MAP_HAUPPAUGE,
+		.no_tx = 1, /* eeprom says it has no tx */
+		.name = "Conexant Hybrid TV (cx231xx) MCE IR no TX",
+	},
 	[MULTIFUNCTION] = {
 		.mce_gen2 = 1,
 		.ir_intfnum = 2,
@@ -399,6 +406,9 @@ static struct usb_device_id mceusb_dev_table[] = {
 	{ USB_DEVICE(VENDOR_TWISTEDMELON, 0x8016) },
 	/* Twisted Melon Inc. - Manta Transceiver */
 	{ USB_DEVICE(VENDOR_TWISTEDMELON, 0x8042) },
+	/* Hauppauge WINTV-HVR-HVR 930C-HD - based on cx231xx */
+	{ USB_DEVICE(VENDOR_HAUPPAUGE, 0xb130),
+	  .driver_info = HAUPPAUGE_CX_HYBRID_TV },
 	/* Terminating entry */
 	{ }
 };
diff --git a/drivers/media/rc/rc-main.c b/drivers/media/rc/rc-main.c
index 46da365c9c84..02e2f38c9c85 100644
--- a/drivers/media/rc/rc-main.c
+++ b/drivers/media/rc/rc-main.c
@@ -22,6 +22,10 @@
 #include <linux/module.h>
 #include "rc-core-priv.h"
 
+/* Bitmap to store allocated device numbers from 0 to IRRCV_NUM_DEVICES - 1 */
+#define IRRCV_NUM_DEVICES      256
+DECLARE_BITMAP(ir_core_dev_number, IRRCV_NUM_DEVICES);
+
 /* Sizes are in bytes, 256 bytes allows for 32 entries on x64 */
 #define IR_TAB_MIN_SIZE	256
 #define IR_TAB_MAX_SIZE	8192
@@ -1065,10 +1069,9 @@ EXPORT_SYMBOL_GPL(rc_free_device);
 int rc_register_device(struct rc_dev *dev)
 {
 	static bool raw_init = false; /* raw decoders loaded? */
-	static atomic_t devno = ATOMIC_INIT(0);
 	struct rc_map *rc_map;
 	const char *path;
-	int rc;
+	int rc, devno;
 
 	if (!dev || !dev->map_name)
 		return -EINVAL;
@@ -1096,7 +1099,15 @@ int rc_register_device(struct rc_dev *dev)
 	 */
 	mutex_lock(&dev->lock);
 
-	dev->devno = (unsigned long)(atomic_inc_return(&devno) - 1);
+	do {
+		devno = find_first_zero_bit(ir_core_dev_number,
+					    IRRCV_NUM_DEVICES);
+		/* No free device slots */
+		if (devno >= IRRCV_NUM_DEVICES)
+			return -ENOMEM;
+	} while (test_and_set_bit(devno, ir_core_dev_number));
+
+	dev->devno = devno;
 	dev_set_name(&dev->dev, "rc%ld", dev->devno);
 	dev_set_drvdata(&dev->dev, dev);
 	rc = device_add(&dev->dev);
@@ -1186,6 +1197,7 @@ out_dev:
 	device_del(&dev->dev);
 out_unlock:
 	mutex_unlock(&dev->lock);
+	clear_bit(dev->devno, ir_core_dev_number);
 	return rc;
 }
 EXPORT_SYMBOL_GPL(rc_register_device);
@@ -1197,6 +1209,8 @@ void rc_unregister_device(struct rc_dev *dev)
 
 	del_timer_sync(&dev->timer_keyup);
 
+	clear_bit(dev->devno, ir_core_dev_number);
+
 	if (dev->driver_type == RC_DRIVER_IR_RAW)
 		ir_raw_event_unregister(dev);
 
diff --git a/drivers/media/rc/st_rc.c b/drivers/media/rc/st_rc.c
index 65120c2d47ad..8f0cddb9e8f2 100644
--- a/drivers/media/rc/st_rc.c
+++ b/drivers/media/rc/st_rc.c
@@ -13,6 +13,7 @@
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
+#include <linux/reset.h>
 #include <media/rc-core.h>
 #include <linux/pinctrl/consumer.h>
 
@@ -28,6 +29,7 @@ struct st_rc_device {
 	int				sample_mult;
 	int				sample_div;
 	bool				rxuhfmode;
+	struct	reset_control		*rstc;
 };
 
 /* Registers */
@@ -161,6 +163,10 @@ static void st_rc_hardware_init(struct st_rc_device *dev)
 	unsigned int rx_max_symbol_per = MAX_SYMB_TIME;
 	unsigned int rx_sampling_freq_div;
 
+	/* Enable the IP */
+	if (dev->rstc)
+		reset_control_deassert(dev->rstc);
+
 	clk_prepare_enable(dev->sys_clock);
 	baseclock = clk_get_rate(dev->sys_clock);
 
@@ -271,6 +277,11 @@ static int st_rc_probe(struct platform_device *pdev)
 	else
 		rc_dev->rx_base = rc_dev->base;
 
+
+	rc_dev->rstc = reset_control_get(dev, NULL);
+	if (IS_ERR(rc_dev->rstc))
+		rc_dev->rstc = NULL;
+
 	rc_dev->dev = dev;
 	platform_set_drvdata(pdev, rc_dev);
 	st_rc_hardware_init(rc_dev);
@@ -338,6 +349,8 @@ static int st_rc_suspend(struct device *dev)
 		writel(0x00, rc_dev->rx_base + IRB_RX_EN);
 		writel(0x00, rc_dev->rx_base + IRB_RX_INT_EN);
 		clk_disable_unprepare(rc_dev->sys_clock);
+		if (rc_dev->rstc)
+			reset_control_assert(rc_dev->rstc);
 	}
 
 	return 0;
diff --git a/drivers/media/tuners/Kconfig b/drivers/media/tuners/Kconfig
index 15665debc572..ba2e365296cf 100644
--- a/drivers/media/tuners/Kconfig
+++ b/drivers/media/tuners/Kconfig
@@ -215,6 +215,13 @@ config MEDIA_TUNER_FC2580
 	help
 	  FCI FC2580 silicon tuner driver.
 
+config MEDIA_TUNER_M88TS2022
+	tristate "Montage M88TS2022 silicon tuner"
+	depends on MEDIA_SUPPORT && I2C
+	default m if !MEDIA_SUBDRV_AUTOSELECT
+	help
+	  Montage M88TS2022 silicon tuner driver.
+
 config MEDIA_TUNER_TUA9001
 	tristate "Infineon TUA 9001 silicon tuner"
 	depends on MEDIA_SUPPORT && I2C
diff --git a/drivers/media/tuners/Makefile b/drivers/media/tuners/Makefile
index 308f108eadba..efe82a904b12 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_M88TS2022) += m88ts2022.o
 obj-$(CONFIG_MEDIA_TUNER_FC0011) += fc0011.o
 obj-$(CONFIG_MEDIA_TUNER_FC0012) += fc0012.o
 obj-$(CONFIG_MEDIA_TUNER_FC0013) += fc0013.o
diff --git a/drivers/media/tuners/e4000.c b/drivers/media/tuners/e4000.c
index 72971a8d3c37..40c1da707d15 100644
--- a/drivers/media/tuners/e4000.c
+++ b/drivers/media/tuners/e4000.c
@@ -243,8 +243,10 @@ static int e4000_set_params(struct dvb_frontend *fe)
 			break;
 	}
 
-	if (i == ARRAY_SIZE(e4000_pll_lut))
+	if (i == ARRAY_SIZE(e4000_pll_lut)) {
+		ret = -EINVAL;
 		goto err;
+	}
 
 	/*
 	 * Note: Currently f_vco overflows when c->frequency is 1 073 741 824 Hz
@@ -271,8 +273,10 @@ static int e4000_set_params(struct dvb_frontend *fe)
 			break;
 	}
 
-	if (i == ARRAY_SIZE(e400_lna_filter_lut))
+	if (i == ARRAY_SIZE(e400_lna_filter_lut)) {
+		ret = -EINVAL;
 		goto err;
+	}
 
 	ret = e4000_wr_reg(priv, 0x10, e400_lna_filter_lut[i].val);
 	if (ret < 0)
@@ -284,8 +288,10 @@ static int e4000_set_params(struct dvb_frontend *fe)
 			break;
 	}
 
-	if (i == ARRAY_SIZE(e4000_if_filter_lut))
+	if (i == ARRAY_SIZE(e4000_if_filter_lut)) {
+		ret = -EINVAL;
 		goto err;
+	}
 
 	buf[0] = e4000_if_filter_lut[i].reg11_val;
 	buf[1] = e4000_if_filter_lut[i].reg12_val;
@@ -300,8 +306,10 @@ static int e4000_set_params(struct dvb_frontend *fe)
 			break;
 	}
 
-	if (i == ARRAY_SIZE(e4000_band_lut))
+	if (i == ARRAY_SIZE(e4000_band_lut)) {
+		ret = -EINVAL;
 		goto err;
+	}
 
 	ret = e4000_wr_reg(priv, 0x07, e4000_band_lut[i].reg07_val);
 	if (ret < 0)
diff --git a/drivers/media/tuners/m88ts2022.c b/drivers/media/tuners/m88ts2022.c
new file mode 100644
index 000000000000..40c42dec721b
--- /dev/null
+++ b/drivers/media/tuners/m88ts2022.c
@@ -0,0 +1,674 @@
+/*
+ * Montage M88TS2022 silicon tuner driver
+ *
+ * Copyright (C) 2013 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.
+ *
+ * Some calculations are taken from existing TS2020 driver.
+ */
+
+#include "m88ts2022_priv.h"
+
+/* write multiple registers */
+static int m88ts2022_wr_regs(struct m88ts2022_priv *priv,
+		u8 reg, const u8 *val, int len)
+{
+#define MAX_WR_LEN 3
+#define MAX_WR_XFER_LEN (MAX_WR_LEN + 1)
+	int ret;
+	u8 buf[MAX_WR_XFER_LEN];
+	struct i2c_msg msg[1] = {
+		{
+			.addr = priv->client->addr,
+			.flags = 0,
+			.len = 1 + len,
+			.buf = buf,
+		}
+	};
+
+	if (WARN_ON(len > MAX_WR_LEN))
+		return -EINVAL;
+
+	buf[0] = reg;
+	memcpy(&buf[1], val, len);
+
+	ret = i2c_transfer(priv->client->adapter, msg, 1);
+	if (ret == 1) {
+		ret = 0;
+	} else {
+		dev_warn(&priv->client->dev,
+				"%s: i2c wr failed=%d reg=%02x len=%d\n",
+				KBUILD_MODNAME, ret, reg, len);
+		ret = -EREMOTEIO;
+	}
+
+	return ret;
+}
+
+/* read multiple registers */
+static int m88ts2022_rd_regs(struct m88ts2022_priv *priv, u8 reg,
+		u8 *val, int len)
+{
+#define MAX_RD_LEN 1
+#define MAX_RD_XFER_LEN (MAX_RD_LEN)
+	int ret;
+	u8 buf[MAX_RD_XFER_LEN];
+	struct i2c_msg msg[2] = {
+		{
+			.addr = priv->client->addr,
+			.flags = 0,
+			.len = 1,
+			.buf = &reg,
+		}, {
+			.addr = priv->client->addr,
+			.flags = I2C_M_RD,
+			.len = len,
+			.buf = buf,
+		}
+	};
+
+	if (WARN_ON(len > MAX_RD_LEN))
+		return -EINVAL;
+
+	ret = i2c_transfer(priv->client->adapter, msg, 2);
+	if (ret == 2) {
+		memcpy(val, buf, len);
+		ret = 0;
+	} else {
+		dev_warn(&priv->client->dev,
+				"%s: i2c rd failed=%d reg=%02x len=%d\n",
+				KBUILD_MODNAME, ret, reg, len);
+		ret = -EREMOTEIO;
+	}
+
+	return ret;
+}
+
+/* write single register */
+static int m88ts2022_wr_reg(struct m88ts2022_priv *priv, u8 reg, u8 val)
+{
+	return m88ts2022_wr_regs(priv, reg, &val, 1);
+}
+
+/* read single register */
+static int m88ts2022_rd_reg(struct m88ts2022_priv *priv, u8 reg, u8 *val)
+{
+	return m88ts2022_rd_regs(priv, reg, val, 1);
+}
+
+/* write single register with mask */
+static int m88ts2022_wr_reg_mask(struct m88ts2022_priv *priv,
+		u8 reg, u8 val, u8 mask)
+{
+	int ret;
+	u8 u8tmp;
+
+	/* no need for read if whole reg is written */
+	if (mask != 0xff) {
+		ret = m88ts2022_rd_regs(priv, reg, &u8tmp, 1);
+		if (ret)
+			return ret;
+
+		val &= mask;
+		u8tmp &= ~mask;
+		val |= u8tmp;
+	}
+
+	return m88ts2022_wr_regs(priv, reg, &val, 1);
+}
+
+static int m88ts2022_cmd(struct dvb_frontend *fe,
+		int op, int sleep, u8 reg, u8 mask, u8 val, u8 *reg_val)
+{
+	struct m88ts2022_priv *priv = fe->tuner_priv;
+	int ret, i;
+	u8 u8tmp;
+	struct m88ts2022_reg_val reg_vals[] = {
+		{0x51, 0x1f - op},
+		{0x51, 0x1f},
+		{0x50, 0x00 + op},
+		{0x50, 0x00},
+	};
+
+	for (i = 0; i < 2; i++) {
+		dev_dbg(&priv->client->dev,
+				"%s: i=%d op=%02x reg=%02x mask=%02x val=%02x\n",
+				__func__, i, op, reg, mask, val);
+
+		for (i = 0; i < ARRAY_SIZE(reg_vals); i++) {
+			ret = m88ts2022_wr_reg(priv, reg_vals[i].reg,
+					reg_vals[i].val);
+			if (ret)
+				goto err;
+		}
+
+		usleep_range(sleep * 1000, sleep * 10000);
+
+		ret = m88ts2022_rd_reg(priv, reg, &u8tmp);
+		if (ret)
+			goto err;
+
+		if ((u8tmp & mask) != val)
+			break;
+	}
+
+	if (reg_val)
+		*reg_val = u8tmp;
+err:
+	return ret;
+}
+
+static int m88ts2022_set_params(struct dvb_frontend *fe)
+{
+	struct m88ts2022_priv *priv = fe->tuner_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	int ret;
+	unsigned int frequency_khz, frequency_offset_khz, f_3db_hz;
+	unsigned int f_ref_khz, f_vco_khz, div_ref, div_out, pll_n, gdiv28;
+	u8 buf[3], u8tmp, cap_code, lpf_gm, lpf_mxdiv, div_max, div_min;
+	u16 u16tmp;
+	dev_dbg(&priv->client->dev,
+			"%s: frequency=%d symbol_rate=%d rolloff=%d\n",
+			__func__, c->frequency, c->symbol_rate, c->rolloff);
+	/*
+	 * Integer-N PLL synthesizer
+	 * kHz is used for all calculations to keep calculations within 32-bit
+	 */
+	f_ref_khz = DIV_ROUND_CLOSEST(priv->cfg.clock, 1000);
+	div_ref = DIV_ROUND_CLOSEST(f_ref_khz, 2000);
+
+	if (c->symbol_rate < 5000000)
+		frequency_offset_khz = 3000; /* 3 MHz */
+	else
+		frequency_offset_khz = 0;
+
+	frequency_khz = c->frequency + frequency_offset_khz;
+
+	if (frequency_khz < 1103000) {
+		div_out = 4;
+		u8tmp = 0x1b;
+	} else {
+		div_out = 2;
+		u8tmp = 0x0b;
+	}
+
+	buf[0] = u8tmp;
+	buf[1] = 0x40;
+	ret = m88ts2022_wr_regs(priv, 0x10, buf, 2);
+	if (ret)
+		goto err;
+
+	f_vco_khz = frequency_khz * div_out;
+	pll_n = f_vco_khz * div_ref / f_ref_khz;
+	pll_n += pll_n % 2;
+	priv->frequency_khz = pll_n * f_ref_khz / div_ref / div_out;
+
+	if (pll_n < 4095)
+		u16tmp = pll_n - 1024;
+	else if (pll_n < 6143)
+		u16tmp = pll_n + 1024;
+	else
+		u16tmp = pll_n + 3072;
+
+	buf[0] = (u16tmp >> 8) & 0x3f;
+	buf[1] = (u16tmp >> 0) & 0xff;
+	buf[2] = div_ref - 8;
+	ret = m88ts2022_wr_regs(priv, 0x01, buf, 3);
+	if (ret)
+		goto err;
+
+	dev_dbg(&priv->client->dev,
+			"%s: frequency=%u offset=%d f_vco_khz=%u pll_n=%u div_ref=%u div_out=%u\n",
+			__func__, priv->frequency_khz,
+			priv->frequency_khz - c->frequency, f_vco_khz, pll_n,
+			div_ref, div_out);
+
+	ret = m88ts2022_cmd(fe, 0x10, 5, 0x15, 0x40, 0x00, NULL);
+	if (ret)
+		goto err;
+
+	ret = m88ts2022_rd_reg(priv, 0x14, &u8tmp);
+	if (ret)
+		goto err;
+
+	u8tmp &= 0x7f;
+	if (u8tmp < 64) {
+		ret = m88ts2022_wr_reg_mask(priv, 0x10, 0x80, 0x80);
+		if (ret)
+			goto err;
+
+		ret = m88ts2022_wr_reg(priv, 0x11, 0x6f);
+		if (ret)
+			goto err;
+
+		ret = m88ts2022_cmd(fe, 0x10, 5, 0x15, 0x40, 0x00, NULL);
+		if (ret)
+			goto err;
+	}
+
+	ret = m88ts2022_rd_reg(priv, 0x14, &u8tmp);
+	if (ret)
+		goto err;
+
+	u8tmp &= 0x1f;
+	if (u8tmp > 19) {
+		ret = m88ts2022_wr_reg_mask(priv, 0x10, 0x00, 0x02);
+		if (ret)
+			goto err;
+	}
+
+	ret = m88ts2022_cmd(fe, 0x08, 5, 0x3c, 0xff, 0x00, NULL);
+	if (ret)
+		goto err;
+
+	ret = m88ts2022_wr_reg(priv, 0x25, 0x00);
+	if (ret)
+		goto err;
+
+	ret = m88ts2022_wr_reg(priv, 0x27, 0x70);
+	if (ret)
+		goto err;
+
+	ret = m88ts2022_wr_reg(priv, 0x41, 0x09);
+	if (ret)
+		goto err;
+
+	ret = m88ts2022_wr_reg(priv, 0x08, 0x0b);
+	if (ret)
+		goto err;
+
+	/* filters */
+	gdiv28 = DIV_ROUND_CLOSEST(f_ref_khz * 1694U, 1000000U);
+
+	ret = m88ts2022_wr_reg(priv, 0x04, gdiv28);
+	if (ret)
+		goto err;
+
+	ret = m88ts2022_cmd(fe, 0x04, 2, 0x26, 0xff, 0x00, &u8tmp);
+	if (ret)
+		goto err;
+
+	cap_code = u8tmp & 0x3f;
+
+	ret = m88ts2022_wr_reg(priv, 0x41, 0x0d);
+	if (ret)
+		goto err;
+
+	ret = m88ts2022_cmd(fe, 0x04, 2, 0x26, 0xff, 0x00, &u8tmp);
+	if (ret)
+		goto err;
+
+	u8tmp &= 0x3f;
+	cap_code = (cap_code + u8tmp) / 2;
+	gdiv28 = gdiv28 * 207 / (cap_code * 2 + 151);
+	div_max = gdiv28 * 135 / 100;
+	div_min = gdiv28 * 78 / 100;
+	div_max = clamp_val(div_max, 0U, 63U);
+
+	f_3db_hz = c->symbol_rate * 135UL / 200UL;
+	f_3db_hz +=  2000000U + (frequency_offset_khz * 1000U);
+	f_3db_hz = clamp(f_3db_hz, 7000000U, 40000000U);
+
+#define LPF_COEFF 3200U
+	lpf_gm = DIV_ROUND_CLOSEST(f_3db_hz * gdiv28, LPF_COEFF * f_ref_khz);
+	lpf_gm = clamp_val(lpf_gm, 1U, 23U);
+
+	lpf_mxdiv = DIV_ROUND_CLOSEST(lpf_gm * LPF_COEFF * f_ref_khz, f_3db_hz);
+	if (lpf_mxdiv < div_min)
+		lpf_mxdiv = DIV_ROUND_CLOSEST(++lpf_gm * LPF_COEFF * f_ref_khz, f_3db_hz);
+	lpf_mxdiv = clamp_val(lpf_mxdiv, 0U, div_max);
+
+	ret = m88ts2022_wr_reg(priv, 0x04, lpf_mxdiv);
+	if (ret)
+		goto err;
+
+	ret = m88ts2022_wr_reg(priv, 0x06, lpf_gm);
+	if (ret)
+		goto err;
+
+	ret = m88ts2022_cmd(fe, 0x04, 2, 0x26, 0xff, 0x00, &u8tmp);
+	if (ret)
+		goto err;
+
+	cap_code = u8tmp & 0x3f;
+
+	ret = m88ts2022_wr_reg(priv, 0x41, 0x09);
+	if (ret)
+		goto err;
+
+	ret = m88ts2022_cmd(fe, 0x04, 2, 0x26, 0xff, 0x00, &u8tmp);
+	if (ret)
+		goto err;
+
+	u8tmp &= 0x3f;
+	cap_code = (cap_code + u8tmp) / 2;
+
+	u8tmp = cap_code | 0x80;
+	ret = m88ts2022_wr_reg(priv, 0x25, u8tmp);
+	if (ret)
+		goto err;
+
+	ret = m88ts2022_wr_reg(priv, 0x27, 0x30);
+	if (ret)
+		goto err;
+
+	ret = m88ts2022_wr_reg(priv, 0x08, 0x09);
+	if (ret)
+		goto err;
+
+	ret = m88ts2022_cmd(fe, 0x01, 20, 0x21, 0xff, 0x00, NULL);
+	if (ret)
+		goto err;
+err:
+	if (ret)
+		dev_dbg(&priv->client->dev, "%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int m88ts2022_init(struct dvb_frontend *fe)
+{
+	struct m88ts2022_priv *priv = fe->tuner_priv;
+	int ret, i;
+	u8 u8tmp;
+	static const struct m88ts2022_reg_val reg_vals[] = {
+		{0x7d, 0x9d},
+		{0x7c, 0x9a},
+		{0x7a, 0x76},
+		{0x3b, 0x01},
+		{0x63, 0x88},
+		{0x61, 0x85},
+		{0x22, 0x30},
+		{0x30, 0x40},
+		{0x20, 0x23},
+		{0x24, 0x02},
+		{0x12, 0xa0},
+	};
+	dev_dbg(&priv->client->dev, "%s:\n", __func__);
+
+	ret = m88ts2022_wr_reg(priv, 0x00, 0x01);
+	if (ret)
+		goto err;
+
+	ret = m88ts2022_wr_reg(priv, 0x00, 0x03);
+	if (ret)
+		goto err;
+
+	switch (priv->cfg.clock_out) {
+	case M88TS2022_CLOCK_OUT_DISABLED:
+		u8tmp = 0x60;
+		break;
+	case M88TS2022_CLOCK_OUT_ENABLED:
+		u8tmp = 0x70;
+		ret = m88ts2022_wr_reg(priv, 0x05, priv->cfg.clock_out_div);
+		if (ret)
+			goto err;
+		break;
+	case M88TS2022_CLOCK_OUT_ENABLED_XTALOUT:
+		u8tmp = 0x6c;
+		break;
+	default:
+		goto err;
+	}
+
+	ret = m88ts2022_wr_reg(priv, 0x42, u8tmp);
+	if (ret)
+		goto err;
+
+	if (priv->cfg.loop_through)
+		u8tmp = 0xec;
+	else
+		u8tmp = 0x6c;
+
+	ret = m88ts2022_wr_reg(priv, 0x62, u8tmp);
+	if (ret)
+		goto err;
+
+	for (i = 0; i < ARRAY_SIZE(reg_vals); i++) {
+		ret = m88ts2022_wr_reg(priv, reg_vals[i].reg, reg_vals[i].val);
+		if (ret)
+			goto err;
+	}
+err:
+	if (ret)
+		dev_dbg(&priv->client->dev, "%s: failed=%d\n", __func__, ret);
+	return ret;
+}
+
+static int m88ts2022_sleep(struct dvb_frontend *fe)
+{
+	struct m88ts2022_priv *priv = fe->tuner_priv;
+	int ret;
+	dev_dbg(&priv->client->dev, "%s:\n", __func__);
+
+	ret = m88ts2022_wr_reg(priv, 0x00, 0x00);
+	if (ret)
+		goto err;
+err:
+	if (ret)
+		dev_dbg(&priv->client->dev, "%s: failed=%d\n", __func__, ret);
+	return ret;
+}
+
+static int m88ts2022_get_frequency(struct dvb_frontend *fe, u32 *frequency)
+{
+	struct m88ts2022_priv *priv = fe->tuner_priv;
+	dev_dbg(&priv->client->dev, "%s:\n", __func__);
+
+	*frequency = priv->frequency_khz;
+	return 0;
+}
+
+static int m88ts2022_get_if_frequency(struct dvb_frontend *fe, u32 *frequency)
+{
+	struct m88ts2022_priv *priv = fe->tuner_priv;
+	dev_dbg(&priv->client->dev, "%s:\n", __func__);
+
+	*frequency = 0; /* Zero-IF */
+	return 0;
+}
+
+static int m88ts2022_get_rf_strength(struct dvb_frontend *fe, u16 *strength)
+{
+	struct m88ts2022_priv *priv = fe->tuner_priv;
+	int ret;
+	u8 u8tmp;
+	u16 gain, u16tmp;
+	unsigned int gain1, gain2, gain3;
+
+	ret = m88ts2022_rd_reg(priv, 0x3d, &u8tmp);
+	if (ret)
+		goto err;
+
+	gain1 = (u8tmp >> 0) & 0x1f;
+	gain1 = clamp(gain1, 0U, 15U);
+
+	ret = m88ts2022_rd_reg(priv, 0x21, &u8tmp);
+	if (ret)
+		goto err;
+
+	gain2 = (u8tmp >> 0) & 0x1f;
+	gain2 = clamp(gain2, 2U, 16U);
+
+	ret = m88ts2022_rd_reg(priv, 0x66, &u8tmp);
+	if (ret)
+		goto err;
+
+	gain3 = (u8tmp >> 3) & 0x07;
+	gain3 = clamp(gain3, 0U, 6U);
+
+	gain = gain1 * 265 + gain2 * 338 + gain3 * 285;
+
+	/* scale value to 0x0000-0xffff */
+	u16tmp = (0xffff - gain);
+	u16tmp = clamp_val(u16tmp, 59000U, 61500U);
+
+	*strength = (u16tmp - 59000) * 0xffff / (61500 - 59000);
+err:
+	if (ret)
+		dev_dbg(&priv->client->dev, "%s: failed=%d\n", __func__, ret);
+	return ret;
+}
+
+static const struct dvb_tuner_ops m88ts2022_tuner_ops = {
+	.info = {
+		.name          = "Montage M88TS2022",
+		.frequency_min = 950000,
+		.frequency_max = 2150000,
+	},
+
+	.init = m88ts2022_init,
+	.sleep = m88ts2022_sleep,
+	.set_params = m88ts2022_set_params,
+
+	.get_frequency = m88ts2022_get_frequency,
+	.get_if_frequency = m88ts2022_get_if_frequency,
+	.get_rf_strength = m88ts2022_get_rf_strength,
+};
+
+static int m88ts2022_probe(struct i2c_client *client,
+		const struct i2c_device_id *id)
+{
+	struct m88ts2022_config *cfg = client->dev.platform_data;
+	struct dvb_frontend *fe = cfg->fe;
+	struct m88ts2022_priv *priv;
+	int ret;
+	u8 chip_id, u8tmp;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv) {
+		ret = -ENOMEM;
+		dev_err(&client->dev, "%s: kzalloc() failed\n", KBUILD_MODNAME);
+		goto err;
+	}
+
+	memcpy(&priv->cfg, cfg, sizeof(struct m88ts2022_config));
+	priv->client = client;
+
+	/* check if the tuner is there */
+	ret = m88ts2022_rd_reg(priv, 0x00, &u8tmp);
+	if (ret)
+		goto err;
+
+	if ((u8tmp & 0x03) == 0x00) {
+		ret = m88ts2022_wr_reg(priv, 0x00, 0x01);
+		if (ret < 0)
+			goto err;
+
+		usleep_range(2000, 50000);
+	}
+
+	ret = m88ts2022_wr_reg(priv, 0x00, 0x03);
+	if (ret)
+		goto err;
+
+	usleep_range(2000, 50000);
+
+	ret = m88ts2022_rd_reg(priv, 0x00, &chip_id);
+	if (ret)
+		goto err;
+
+	dev_dbg(&priv->client->dev, "%s: chip_id=%02x\n", __func__, chip_id);
+
+	switch (chip_id) {
+	case 0xc3:
+	case 0x83:
+		break;
+	default:
+		goto err;
+	}
+
+	switch (priv->cfg.clock_out) {
+	case M88TS2022_CLOCK_OUT_DISABLED:
+		u8tmp = 0x60;
+		break;
+	case M88TS2022_CLOCK_OUT_ENABLED:
+		u8tmp = 0x70;
+		ret = m88ts2022_wr_reg(priv, 0x05, priv->cfg.clock_out_div);
+		if (ret)
+			goto err;
+		break;
+	case M88TS2022_CLOCK_OUT_ENABLED_XTALOUT:
+		u8tmp = 0x6c;
+		break;
+	default:
+		goto err;
+	}
+
+	ret = m88ts2022_wr_reg(priv, 0x42, u8tmp);
+	if (ret)
+		goto err;
+
+	if (priv->cfg.loop_through)
+		u8tmp = 0xec;
+	else
+		u8tmp = 0x6c;
+
+	ret = m88ts2022_wr_reg(priv, 0x62, u8tmp);
+	if (ret)
+		goto err;
+
+	/* sleep */
+	ret = m88ts2022_wr_reg(priv, 0x00, 0x00);
+	if (ret)
+		goto err;
+
+	dev_info(&priv->client->dev,
+			"%s: Montage M88TS2022 successfully identified\n",
+			KBUILD_MODNAME);
+
+	fe->tuner_priv = priv;
+	memcpy(&fe->ops.tuner_ops, &m88ts2022_tuner_ops,
+			sizeof(struct dvb_tuner_ops));
+
+	i2c_set_clientdata(client, priv);
+	return 0;
+err:
+	dev_dbg(&client->dev, "%s: failed=%d\n", __func__, ret);
+	kfree(priv);
+	return ret;
+}
+
+static int m88ts2022_remove(struct i2c_client *client)
+{
+	struct m88ts2022_priv *priv = i2c_get_clientdata(client);
+	struct dvb_frontend *fe = priv->cfg.fe;
+	dev_dbg(&client->dev, "%s:\n", __func__);
+
+	memset(&fe->ops.tuner_ops, 0, sizeof(struct dvb_tuner_ops));
+	fe->tuner_priv = NULL;
+	kfree(priv);
+
+	return 0;
+}
+
+static const struct i2c_device_id m88ts2022_id[] = {
+	{"m88ts2022", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, m88ts2022_id);
+
+static struct i2c_driver m88ts2022_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "m88ts2022",
+	},
+	.probe		= m88ts2022_probe,
+	.remove		= m88ts2022_remove,
+	.id_table	= m88ts2022_id,
+};
+
+module_i2c_driver(m88ts2022_driver);
+
+MODULE_DESCRIPTION("Montage M88TS2022 silicon tuner driver");
+MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/tuners/m88ts2022.h b/drivers/media/tuners/m88ts2022.h
new file mode 100644
index 000000000000..659fa1b1633a
--- /dev/null
+++ b/drivers/media/tuners/m88ts2022.h
@@ -0,0 +1,54 @@
+/*
+ * Montage M88TS2022 silicon tuner driver
+ *
+ * Copyright (C) 2013 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 M88TS2022_H
+#define M88TS2022_H
+
+#include "dvb_frontend.h"
+
+struct m88ts2022_config {
+	/*
+	 * clock
+	 * 16000000 - 32000000
+	 */
+	u32 clock;
+
+	/*
+	 * RF loop-through
+	 */
+	u8 loop_through:1;
+
+	/*
+	 * clock output
+	 */
+#define M88TS2022_CLOCK_OUT_DISABLED        0
+#define M88TS2022_CLOCK_OUT_ENABLED         1
+#define M88TS2022_CLOCK_OUT_ENABLED_XTALOUT 2
+	u8 clock_out:2;
+
+	/*
+	 * clock output divider
+	 * 1 - 31
+	 */
+	u8 clock_out_div:5;
+
+	/*
+	 * pointer to DVB frontend
+	 */
+	struct dvb_frontend *fe;
+};
+
+#endif
diff --git a/drivers/media/tuners/m88ts2022_priv.h b/drivers/media/tuners/m88ts2022_priv.h
new file mode 100644
index 000000000000..0363dd866a2d
--- /dev/null
+++ b/drivers/media/tuners/m88ts2022_priv.h
@@ -0,0 +1,34 @@
+/*
+ * Montage M88TS2022 silicon tuner driver
+ *
+ * Copyright (C) 2013 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 M88TS2022_PRIV_H
+#define M88TS2022_PRIV_H
+
+#include "m88ts2022.h"
+
+struct m88ts2022_priv {
+	struct m88ts2022_config cfg;
+	struct i2c_client *client;
+	struct dvb_frontend *fe;
+	u32 frequency_khz;
+};
+
+struct m88ts2022_reg_val {
+	u8 reg;
+	u8 val;
+};
+
+#endif
diff --git a/drivers/media/tuners/tuner-xc2028.c b/drivers/media/tuners/tuner-xc2028.c
index 4be5cf808a40..cca508d4aafb 100644
--- a/drivers/media/tuners/tuner-xc2028.c
+++ b/drivers/media/tuners/tuner-xc2028.c
@@ -134,15 +134,6 @@ struct xc2028_data {
 	_rc;								\
 })
 
-#define i2c_rcv(priv, buf, size) ({					\
-	int _rc;							\
-	_rc = tuner_i2c_xfer_recv(&priv->i2c_props, buf, size);		\
-	if (size != _rc)						\
-		tuner_err("i2c input error: rc = %d (should be %d)\n",	\
-			   _rc, (int)size); 				\
-	_rc;								\
-})
-
 #define i2c_send_recv(priv, obuf, osize, ibuf, isize) ({		\
 	int _rc;							\
 	_rc = tuner_i2c_xfer_send_recv(&priv->i2c_props, obuf, osize,	\
@@ -276,6 +267,7 @@ static int check_device_status(struct xc2028_data *priv)
 	case XC2028_WAITING_FIRMWARE:
 		return -EAGAIN;
 	case XC2028_ACTIVE:
+		return 1;
 	case XC2028_SLEEP:
 		return 0;
 	case XC2028_NODEV:
@@ -718,6 +710,8 @@ static int load_scode(struct dvb_frontend *fe, unsigned int type,
 	return 0;
 }
 
+static int xc2028_sleep(struct dvb_frontend *fe);
+
 static int check_firmware(struct dvb_frontend *fe, unsigned int type,
 			  v4l2_std_id std, __u16 int_freq)
 {
@@ -890,7 +884,7 @@ read_not_reliable:
 	return 0;
 
 fail:
-	priv->state = XC2028_SLEEP;
+	priv->state = XC2028_NO_FIRMWARE;
 
 	memset(&priv->cur_fw, 0, sizeof(priv->cur_fw));
 	if (retry_count < 8) {
@@ -900,6 +894,9 @@ fail:
 		goto retry;
 	}
 
+	/* Firmware didn't load. Put the device to sleep */
+	xc2028_sleep(fe);
+
 	if (rc == -ENOENT)
 		rc = -EINVAL;
 	return rc;
@@ -917,6 +914,12 @@ static int xc2028_signal(struct dvb_frontend *fe, u16 *strength)
 	if (rc < 0)
 		return rc;
 
+	/* If the device is sleeping, no channel is tuned */
+	if (!rc) {
+		*strength = 0;
+		return 0;
+	}
+
 	mutex_lock(&priv->lock);
 
 	/* Sync Lock Indicator */
@@ -964,6 +967,12 @@ static int xc2028_get_afc(struct dvb_frontend *fe, s32 *afc)
 	if (rc < 0)
 		return rc;
 
+	/* If the device is sleeping, no channel is tuned */
+	if (!rc) {
+		*afc = 0;
+		return 0;
+	}
+
 	mutex_lock(&priv->lock);
 
 	/* Sync Lock Indicator */
@@ -1281,6 +1290,10 @@ static int xc2028_sleep(struct dvb_frontend *fe)
 	if (rc < 0)
 		return rc;
 
+	/* Device is already in sleep mode */
+	if (!rc)
+		return 0;
+
 	/* Avoid firmware reload on slow devices or if PM disabled */
 	if (no_poweroff || priv->ctrl.disable_power_mgmt)
 		return 0;
@@ -1298,7 +1311,8 @@ static int xc2028_sleep(struct dvb_frontend *fe)
 	else
 		rc = send_seq(priv, {0x80, XREG_POWER_DOWN, 0x00, 0x00});
 
-	priv->state = XC2028_SLEEP;
+	if (rc >= 0)
+		priv->state = XC2028_SLEEP;
 
 	mutex_unlock(&priv->lock);
 
@@ -1366,7 +1380,7 @@ static void load_firmware_cb(const struct firmware *fw,
 
 	if (rc < 0)
 		return;
-	priv->state = XC2028_SLEEP;
+	priv->state = XC2028_ACTIVE;
 }
 
 static int xc2028_set_config(struct dvb_frontend *fe, void *priv_cfg)
diff --git a/drivers/media/usb/Kconfig b/drivers/media/usb/Kconfig
index cfe8056b91aa..39d824e2bb69 100644
--- a/drivers/media/usb/Kconfig
+++ b/drivers/media/usb/Kconfig
@@ -17,7 +17,6 @@ source "drivers/media/usb/cpia2/Kconfig"
 source "drivers/media/usb/zr364xx/Kconfig"
 source "drivers/media/usb/stkwebcam/Kconfig"
 source "drivers/media/usb/s2255/Kconfig"
-source "drivers/media/usb/sn9c102/Kconfig"
 source "drivers/media/usb/usbtv/Kconfig"
 endif
 
diff --git a/drivers/media/usb/Makefile b/drivers/media/usb/Makefile
index 0935f47497a6..7ac4b143dce8 100644
--- a/drivers/media/usb/Makefile
+++ b/drivers/media/usb/Makefile
@@ -10,7 +10,6 @@ obj-$(CONFIG_USB_VIDEO_CLASS)	+= uvc/
 obj-$(CONFIG_USB_GSPCA)         += gspca/
 obj-$(CONFIG_USB_PWC)           += pwc/
 obj-$(CONFIG_VIDEO_CPIA2) += cpia2/
-obj-$(CONFIG_USB_SN9C102)       += sn9c102/
 obj-$(CONFIG_VIDEO_AU0828) += au0828/
 obj-$(CONFIG_VIDEO_HDPVR)	+= hdpvr/
 obj-$(CONFIG_VIDEO_PVRUSB2) += pvrusb2/
diff --git a/drivers/media/usb/au0828/au0828-core.c b/drivers/media/usb/au0828/au0828-core.c
index bd9d19a73efd..ab45a6f9dcc9 100644
--- a/drivers/media/usb/au0828/au0828-core.c
+++ b/drivers/media/usb/au0828/au0828-core.c
@@ -173,9 +173,8 @@ static int au0828_usb_probe(struct usb_interface *interface,
 	const struct usb_device_id *id)
 {
 	int ifnum;
-#ifdef CONFIG_VIDEO_AU0828_V4L2
-	int retval;
-#endif
+	int retval = 0;
+
 	struct au0828_dev *dev;
 	struct usb_device *usbdev = interface_to_usbdev(interface);
 
@@ -257,7 +256,11 @@ static int au0828_usb_probe(struct usb_interface *interface,
 #endif
 
 	/* Digital TV */
-	au0828_dvb_register(dev);
+	retval = au0828_dvb_register(dev);
+	if (retval)
+		pr_err("%s() au0282_dev_register failed\n",
+		       __func__);
+
 
 	/* Store the pointer to the au0828_dev so it can be accessed in
 	   au0828_usb_disconnect */
@@ -268,7 +271,7 @@ static int au0828_usb_probe(struct usb_interface *interface,
 
 	mutex_unlock(&dev->lock);
 
-	return 0;
+	return retval;
 }
 
 static struct usb_driver au0828_usb_driver = {
diff --git a/drivers/media/usb/au0828/au0828-dvb.c b/drivers/media/usb/au0828/au0828-dvb.c
index 9a6f15613a38..4ae8b1074649 100644
--- a/drivers/media/usb/au0828/au0828-dvb.c
+++ b/drivers/media/usb/au0828/au0828-dvb.c
@@ -33,6 +33,10 @@
 #include "mxl5007t.h"
 #include "tda18271.h"
 
+static int preallocate_big_buffers;
+module_param_named(preallocate_big_buffers, preallocate_big_buffers, int, 0644);
+MODULE_PARM_DESC(preallocate_big_buffers, "Preallocate the larger transfer buffers at module load time");
+
 DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
 
 #define _AU0828_BULKPIPE 0x83
@@ -153,9 +157,13 @@ static int stop_urb_transfer(struct au0828_dev *dev)
 
 	dev->urb_streaming = 0;
 	for (i = 0; i < URB_COUNT; i++) {
-		usb_kill_urb(dev->urbs[i]);
-		kfree(dev->urbs[i]->transfer_buffer);
-		usb_free_urb(dev->urbs[i]);
+		if (dev->urbs[i]) {
+			usb_kill_urb(dev->urbs[i]);
+			if (!preallocate_big_buffers)
+				kfree(dev->urbs[i]->transfer_buffer);
+
+			usb_free_urb(dev->urbs[i]);
+		}
 	}
 
 	return 0;
@@ -181,10 +189,18 @@ static int start_urb_transfer(struct au0828_dev *dev)
 
 		purb = dev->urbs[i];
 
-		purb->transfer_buffer = kzalloc(URB_BUFSIZE, GFP_KERNEL);
+		if (preallocate_big_buffers)
+			purb->transfer_buffer = dev->dig_transfer_buffer[i];
+		else
+			purb->transfer_buffer = kzalloc(URB_BUFSIZE,
+					GFP_KERNEL);
+
 		if (!purb->transfer_buffer) {
 			usb_free_urb(purb);
 			dev->urbs[i] = NULL;
+			printk(KERN_ERR
+			       "%s: failed big buffer allocation, err = %d\n",
+			       __func__, ret);
 			goto err;
 		}
 
@@ -217,6 +233,27 @@ err:
 	return ret;
 }
 
+static void au0828_start_transport(struct au0828_dev *dev)
+{
+	au0828_write(dev, 0x608, 0x90);
+	au0828_write(dev, 0x609, 0x72);
+	au0828_write(dev, 0x60a, 0x71);
+	au0828_write(dev, 0x60b, 0x01);
+
+}
+
+static void au0828_stop_transport(struct au0828_dev *dev, int full_stop)
+{
+	if (full_stop) {
+		au0828_write(dev, 0x608, 0x00);
+		au0828_write(dev, 0x609, 0x00);
+		au0828_write(dev, 0x60a, 0x00);
+	}
+	au0828_write(dev, 0x60b, 0x00);
+}
+
+
+
 static int au0828_dvb_start_feed(struct dvb_demux_feed *feed)
 {
 	struct dvb_demux *demux = feed->demux;
@@ -231,13 +268,17 @@ static int au0828_dvb_start_feed(struct dvb_demux_feed *feed)
 
 	if (dvb) {
 		mutex_lock(&dvb->lock);
+		dvb->start_count++;
+		dprintk(1, "%s(), start_count: %d, stop_count: %d\n", __func__,
+			dvb->start_count, dvb->stop_count);
 		if (dvb->feeding++ == 0) {
 			/* Start transport */
-			au0828_write(dev, 0x608, 0x90);
-			au0828_write(dev, 0x609, 0x72);
-			au0828_write(dev, 0x60a, 0x71);
-			au0828_write(dev, 0x60b, 0x01);
+			au0828_start_transport(dev);
 			ret = start_urb_transfer(dev);
+			if (ret < 0) {
+				au0828_stop_transport(dev, 0);
+				dvb->feeding--;	/* We ran out of memory... */
+			}
 		}
 		mutex_unlock(&dvb->lock);
 	}
@@ -256,10 +297,16 @@ static int au0828_dvb_stop_feed(struct dvb_demux_feed *feed)
 
 	if (dvb) {
 		mutex_lock(&dvb->lock);
-		if (--dvb->feeding == 0) {
-			/* Stop transport */
-			ret = stop_urb_transfer(dev);
-			au0828_write(dev, 0x60b, 0x00);
+		dvb->stop_count++;
+		dprintk(1, "%s(), start_count: %d, stop_count: %d\n", __func__,
+			dvb->start_count, dvb->stop_count);
+		if (dvb->feeding > 0) {
+			dvb->feeding--;
+			if (dvb->feeding == 0) {
+				/* Stop transport */
+				ret = stop_urb_transfer(dev);
+				au0828_stop_transport(dev, 0);
+			}
 		}
 		mutex_unlock(&dvb->lock);
 	}
@@ -282,16 +329,10 @@ static void au0828_restart_dvb_streaming(struct work_struct *work)
 
 	/* Stop transport */
 	stop_urb_transfer(dev);
-	au0828_write(dev, 0x608, 0x00);
-	au0828_write(dev, 0x609, 0x00);
-	au0828_write(dev, 0x60a, 0x00);
-	au0828_write(dev, 0x60b, 0x00);
+	au0828_stop_transport(dev, 1);
 
 	/* Start transport */
-	au0828_write(dev, 0x608, 0x90);
-	au0828_write(dev, 0x609, 0x72);
-	au0828_write(dev, 0x60a, 0x71);
-	au0828_write(dev, 0x60b, 0x01);
+	au0828_start_transport(dev);
 	start_urb_transfer(dev);
 
 	mutex_unlock(&dvb->lock);
@@ -304,6 +345,23 @@ static int dvb_register(struct au0828_dev *dev)
 
 	dprintk(1, "%s()\n", __func__);
 
+	if (preallocate_big_buffers) {
+		int i;
+		for (i = 0; i < URB_COUNT; i++) {
+			dev->dig_transfer_buffer[i] = kzalloc(URB_BUFSIZE,
+					GFP_KERNEL);
+
+			if (!dev->dig_transfer_buffer[i]) {
+				result = -ENOMEM;
+
+				printk(KERN_ERR
+				       "%s: failed buffer allocation (errno = %d)\n",
+				       DRIVER_NAME, result);
+				goto fail_adapter;
+			}
+		}
+	}
+
 	INIT_WORK(&dev->restart_streaming, au0828_restart_dvb_streaming);
 
 	/* register adapter */
@@ -375,6 +433,9 @@ static int dvb_register(struct au0828_dev *dev)
 
 	/* register network adapter */
 	dvb_net_init(&dvb->adapter, &dvb->net, &dvb->demux.dmx);
+
+	dvb->start_count = 0;
+	dvb->stop_count = 0;
 	return 0;
 
 fail_fe_conn:
@@ -391,6 +452,13 @@ fail_frontend:
 	dvb_frontend_detach(dvb->frontend);
 	dvb_unregister_adapter(&dvb->adapter);
 fail_adapter:
+
+	if (preallocate_big_buffers) {
+		int i;
+		for (i = 0; i < URB_COUNT; i++)
+			kfree(dev->dig_transfer_buffer[i]);
+	}
+
 	return result;
 }
 
@@ -411,6 +479,14 @@ void au0828_dvb_unregister(struct au0828_dev *dev)
 	dvb_unregister_frontend(dvb->frontend);
 	dvb_frontend_detach(dvb->frontend);
 	dvb_unregister_adapter(&dvb->adapter);
+
+	if (preallocate_big_buffers) {
+		int i;
+		for (i = 0; i < URB_COUNT; i++)
+			kfree(dev->dig_transfer_buffer[i]);
+	}
+
+
 }
 
 /* All the DVB attach calls go here, this function get's modified
diff --git a/drivers/media/usb/au0828/au0828.h b/drivers/media/usb/au0828/au0828.h
index ef1f57f22be7..5439772c1551 100644
--- a/drivers/media/usb/au0828/au0828.h
+++ b/drivers/media/usb/au0828/au0828.h
@@ -102,6 +102,8 @@ struct au0828_dvb {
 	struct dmx_frontend fe_mem;
 	struct dvb_net net;
 	int feeding;
+	int start_count;
+	int stop_count;
 };
 
 enum au0828_stream_state {
@@ -260,6 +262,10 @@ struct au0828_dev {
 	/* USB / URB Related */
 	int		urb_streaming;
 	struct urb	*urbs[URB_COUNT];
+
+	/* Preallocated transfer digital transfer buffers */
+
+	char *dig_transfer_buffer[URB_COUNT];
 };
 
 /* ----------------------------------------------------------- */
diff --git a/drivers/media/usb/cx231xx/Kconfig b/drivers/media/usb/cx231xx/Kconfig
index 86feeeaf61c2..f14c5e89a567 100644
--- a/drivers/media/usb/cx231xx/Kconfig
+++ b/drivers/media/usb/cx231xx/Kconfig
@@ -45,6 +45,8 @@ config VIDEO_CX231XX_DVB
 	select MEDIA_TUNER_XC5000 if MEDIA_SUBDRV_AUTOSELECT
 	select MEDIA_TUNER_TDA18271 if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_MB86A20S if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_LGDT3305 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_TDA18271C2DD if MEDIA_SUBDRV_AUTOSELECT
 
 	---help---
 	  This adds support for DVB cards based on the
diff --git a/drivers/media/usb/cx231xx/cx231xx-cards.c b/drivers/media/usb/cx231xx/cx231xx-cards.c
index 528cce958a82..2ee03e4ddd86 100644
--- a/drivers/media/usb/cx231xx/cx231xx-cards.c
+++ b/drivers/media/usb/cx231xx/cx231xx-cards.c
@@ -709,6 +709,8 @@ const unsigned int cx231xx_bcount = ARRAY_SIZE(cx231xx_boards);
 
 /* table of devices that work with this driver */
 struct usb_device_id cx231xx_id_table[] = {
+	{USB_DEVICE(0x1D19, 0x6109),
+	.driver_info = CX231XX_BOARD_PV_XCAPTURE_USB},
 	{USB_DEVICE(0x0572, 0x5A3C),
 	 .driver_info = CX231XX_BOARD_UNKNOWN},
 	{USB_DEVICE(0x0572, 0x58A2),
diff --git a/drivers/media/usb/cx231xx/cx231xx-i2c.c b/drivers/media/usb/cx231xx/cx231xx-i2c.c
index 96a5a0965399..7c0f797f1057 100644
--- a/drivers/media/usb/cx231xx/cx231xx-i2c.c
+++ b/drivers/media/usb/cx231xx/cx231xx-i2c.c
@@ -371,9 +371,9 @@ static int cx231xx_i2c_xfer(struct i2c_adapter *i2c_adap,
 	mutex_lock(&dev->i2c_lock);
 	for (i = 0; i < num; i++) {
 
-		addr = msgs[i].addr >> 1;
+		addr = msgs[i].addr;
 
-		dprintk2(2, "%s %s addr=%x len=%d:",
+		dprintk2(2, "%s %s addr=0x%x len=%d:",
 			 (msgs[i].flags & I2C_M_RD) ? "read" : "write",
 			 i == num - 1 ? "stop" : "nonstop", addr, msgs[i].len);
 		if (!msgs[i].len) {
@@ -390,32 +390,41 @@ static int cx231xx_i2c_xfer(struct i2c_adapter *i2c_adap,
 			rc = cx231xx_i2c_recv_bytes(i2c_adap, &msgs[i]);
 			if (i2c_debug >= 2) {
 				for (byte = 0; byte < msgs[i].len; byte++)
-					printk(" %02x", msgs[i].buf[byte]);
+					printk(KERN_CONT " %02x", msgs[i].buf[byte]);
 			}
 		} else if (i + 1 < num && (msgs[i + 1].flags & I2C_M_RD) &&
 			   msgs[i].addr == msgs[i + 1].addr
 			   && (msgs[i].len <= 2) && (bus->nr < 3)) {
+			/* write bytes */
+			if (i2c_debug >= 2) {
+				for (byte = 0; byte < msgs[i].len; byte++)
+					printk(KERN_CONT " %02x", msgs[i].buf[byte]);
+				printk(KERN_CONT "\n");
+			}
 			/* read bytes */
+			dprintk2(2, "plus %s %s addr=0x%x len=%d:",
+				(msgs[i+1].flags & I2C_M_RD) ? "read" : "write",
+				i+1 == num - 1 ? "stop" : "nonstop", addr, msgs[i+1].len);
 			rc = cx231xx_i2c_recv_bytes_with_saddr(i2c_adap,
 							       &msgs[i],
 							       &msgs[i + 1]);
 			if (i2c_debug >= 2) {
-				for (byte = 0; byte < msgs[i].len; byte++)
-					printk(" %02x", msgs[i].buf[byte]);
+				for (byte = 0; byte < msgs[i+1].len; byte++)
+					printk(KERN_CONT " %02x", msgs[i+1].buf[byte]);
 			}
 			i++;
 		} else {
 			/* write bytes */
 			if (i2c_debug >= 2) {
 				for (byte = 0; byte < msgs[i].len; byte++)
-					printk(" %02x", msgs[i].buf[byte]);
+					printk(KERN_CONT " %02x", msgs[i].buf[byte]);
 			}
 			rc = cx231xx_i2c_send_bytes(i2c_adap, &msgs[i]);
 		}
 		if (rc < 0)
 			goto err;
 		if (i2c_debug >= 2)
-			printk("\n");
+			printk(KERN_CONT "\n");
 	}
 	mutex_unlock(&dev->i2c_lock);
 	return num;
diff --git a/drivers/media/usb/dvb-usb-v2/af9035.c b/drivers/media/usb/dvb-usb-v2/af9035.c
index 8f9b2cea88f0..8ede8ea762e6 100644
--- a/drivers/media/usb/dvb-usb-v2/af9035.c
+++ b/drivers/media/usb/dvb-usb-v2/af9035.c
@@ -1539,6 +1539,8 @@ static const struct usb_device_id af9035_id_table[] = {
 		&af9035_props, "TerraTec Cinergy T Stick Dual RC (rev. 2)", NULL) },
 	{ DVB_USB_DEVICE(USB_VID_LEADTEK, 0x6a05,
 		&af9035_props, "Leadtek WinFast DTV Dongle Dual", NULL) },
+	{ DVB_USB_DEVICE(USB_VID_HAUPPAUGE, 0xf900,
+		&af9035_props, "Hauppauge WinTV-MiniStick 2", NULL) },
 	{ }
 };
 MODULE_DEVICE_TABLE(usb, af9035_id_table);
diff --git a/drivers/media/usb/dvb-usb-v2/anysee.c b/drivers/media/usb/dvb-usb-v2/anysee.c
index 90cfa35ef6e6..eeab79bdd2aa 100644
--- a/drivers/media/usb/dvb-usb-v2/anysee.c
+++ b/drivers/media/usb/dvb-usb-v2/anysee.c
@@ -442,6 +442,7 @@ static struct cxd2820r_config anysee_cxd2820r_config = {
  * IOD[0] ZL10353 1=enabled
  * IOE[0] tuner 0=enabled
  * tuner is behind ZL10353 I2C-gate
+ * tuner is behind TDA10023 I2C-gate
  *
  * E7 TC VID=1c73 PID=861f HW=18 FW=0.7 AMTCI=0.5 "anysee-E7TC(LP)"
  * PCB: 508TC (rev0.6)
@@ -956,7 +957,7 @@ static int anysee_tuner_attach(struct dvb_usb_adapter *adap)
 
 		if (fe && adap->fe[1]) {
 			/* attach tuner for 2nd FE */
-			fe = dvb_attach(dvb_pll_attach, adap->fe[0],
+			fe = dvb_attach(dvb_pll_attach, adap->fe[1],
 					(0xc0 >> 1), &d->i2c_adap,
 					DVB_PLL_SAMSUNG_DTOS403IH102A);
 		}
diff --git a/drivers/media/usb/dvb-usb-v2/az6007.c b/drivers/media/usb/dvb-usb-v2/az6007.c
index 44c64ef361bf..c1051c347744 100644
--- a/drivers/media/usb/dvb-usb-v2/az6007.c
+++ b/drivers/media/usb/dvb-usb-v2/az6007.c
@@ -68,6 +68,19 @@ static struct drxk_config terratec_h7_drxk = {
 	.microcode_name = "dvb-usb-terratec-h7-drxk.fw",
 };
 
+static struct drxk_config cablestar_hdci_drxk = {
+	.adr = 0x29,
+	.parallel_ts = true,
+	.dynamic_clk = true,
+	.single_master = true,
+	.enable_merr_cfg = true,
+	.no_i2c_bridge = false,
+	.chunk_size = 64,
+	.mpeg_out_clk_strength = 0x02,
+	.qam_demod_parameter_count = 2,
+	.microcode_name = "dvb-usb-technisat-cablestar-hdci-drxk.fw",
+};
+
 static int drxk_gate_ctrl(struct dvb_frontend *fe, int enable)
 {
 	struct az6007_device_state *st = fe_to_priv(fe);
@@ -630,6 +643,27 @@ static int az6007_frontend_attach(struct dvb_usb_adapter *adap)
 	return 0;
 }
 
+static int az6007_cablestar_hdci_frontend_attach(struct dvb_usb_adapter *adap)
+{
+	struct az6007_device_state *st = adap_to_priv(adap);
+	struct dvb_usb_device *d = adap_to_d(adap);
+
+	pr_debug("attaching demod drxk\n");
+
+	adap->fe[0] = dvb_attach(drxk_attach, &cablestar_hdci_drxk,
+				 &d->i2c_adap);
+	if (!adap->fe[0])
+		return -EINVAL;
+
+	adap->fe[0]->sec_priv = adap;
+	st->gate_ctrl = adap->fe[0]->ops.i2c_gate_ctrl;
+	adap->fe[0]->ops.i2c_gate_ctrl = drxk_gate_ctrl;
+
+	az6007_ci_init(adap);
+
+	return 0;
+}
+
 static int az6007_tuner_attach(struct dvb_usb_adapter *adap)
 {
 	struct dvb_usb_device *d = adap_to_d(adap);
@@ -868,6 +902,29 @@ static struct dvb_usb_device_properties az6007_props = {
 	}
 };
 
+static struct dvb_usb_device_properties az6007_cablestar_hdci_props = {
+	.driver_name         = KBUILD_MODNAME,
+	.owner               = THIS_MODULE,
+	.firmware            = AZ6007_FIRMWARE,
+
+	.adapter_nr          = adapter_nr,
+	.size_of_priv        = sizeof(struct az6007_device_state),
+	.i2c_algo            = &az6007_i2c_algo,
+	.tuner_attach        = az6007_tuner_attach,
+	.frontend_attach     = az6007_cablestar_hdci_frontend_attach,
+	.streaming_ctrl      = az6007_streaming_ctrl,
+/* ditch get_rc_config as it can't work (TS35 remote, I believe it's rc5) */
+	.get_rc_config       = NULL,
+	.read_mac_address    = az6007_read_mac_addr,
+	.download_firmware   = az6007_download_firmware,
+	.identify_state	     = az6007_identify_state,
+	.power_ctrl          = az6007_power_ctrl,
+	.num_adapters        = 1,
+	.adapter             = {
+		{ .stream = DVB_USB_STREAM_BULK(0x02, 10, 4096), }
+	}
+};
+
 static struct usb_device_id az6007_usb_table[] = {
 	{DVB_USB_DEVICE(USB_VID_AZUREWAVE, USB_PID_AZUREWAVE_6007,
 		&az6007_props, "Azurewave 6007", RC_MAP_EMPTY)},
@@ -875,6 +932,8 @@ static struct usb_device_id az6007_usb_table[] = {
 		&az6007_props, "Terratec H7", RC_MAP_NEC_TERRATEC_CINERGY_XS)},
 	{DVB_USB_DEVICE(USB_VID_TERRATEC, USB_PID_TERRATEC_H7_2,
 		&az6007_props, "Terratec H7", RC_MAP_NEC_TERRATEC_CINERGY_XS)},
+	{DVB_USB_DEVICE(USB_VID_TECHNISAT, USB_PID_TECHNISAT_USB2_CABLESTAR_HDCI,
+		&az6007_cablestar_hdci_props, "Technisat CableStar Combo HD CI", RC_MAP_EMPTY)},
 	{0},
 };
 
diff --git a/drivers/media/usb/dvb-usb-v2/ec168.c b/drivers/media/usb/dvb-usb-v2/ec168.c
index 5c68f3918bc8..0c2b377704ff 100644
--- a/drivers/media/usb/dvb-usb-v2/ec168.c
+++ b/drivers/media/usb/dvb-usb-v2/ec168.c
@@ -170,7 +170,7 @@ static int ec168_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 
 error:
 	mutex_unlock(&d->i2c_mutex);
-	return i;
+	return ret;
 }
 
 static u32 ec168_i2c_func(struct i2c_adapter *adapter)
diff --git a/drivers/media/usb/dvb-usb-v2/it913x.c b/drivers/media/usb/dvb-usb-v2/it913x.c
index 1cb6899cf797..fe95a586dd5d 100644
--- a/drivers/media/usb/dvb-usb-v2/it913x.c
+++ b/drivers/media/usb/dvb-usb-v2/it913x.c
@@ -799,6 +799,9 @@ static const struct usb_device_id it913x_id_table[] = {
 	{ DVB_USB_DEVICE(USB_VID_KWORLD_2, USB_PID_CTVDIGDUAL_V2,
 		&it913x_properties, "Digital Dual TV Receiver CTVDIGDUAL_V2",
 			RC_MAP_IT913X_V1) },
+	{ DVB_USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_H335,
+		&it913x_properties, "Avermedia H335",
+			RC_MAP_IT913X_V2) },
 	{}		/* Terminating entry */
 };
 
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.c b/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.c
index d83df4bb72d3..0a98d04c53e4 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.c
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.c
@@ -1,7 +1,7 @@
 /*
  *  mxl111sf-demod.c - driver for the MaxLinear MXL111SF DVB-T demodulator
  *
- *  Copyright (C) 2010 Michael Krufky <mkrufky@kernellabs.com>
+ *  Copyright (C) 2010-2014 Michael Krufky <mkrufky@linuxtv.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
@@ -601,7 +601,7 @@ struct dvb_frontend *mxl111sf_demod_attach(struct mxl111sf_state *mxl_state,
 EXPORT_SYMBOL_GPL(mxl111sf_demod_attach);
 
 MODULE_DESCRIPTION("MaxLinear MxL111SF DVB-T demodulator driver");
-MODULE_AUTHOR("Michael Krufky <mkrufky@kernellabs.com>");
+MODULE_AUTHOR("Michael Krufky <mkrufky@linuxtv.org>");
 MODULE_LICENSE("GPL");
 MODULE_VERSION("0.1");
 
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.h b/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.h
index 3f3f8bfd190b..2d4530f5be54 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.h
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.h
@@ -1,7 +1,7 @@
 /*
  *  mxl111sf-demod.h - driver for the MaxLinear MXL111SF DVB-T demodulator
  *
- *  Copyright (C) 2010 Michael Krufky <mkrufky@kernellabs.com>
+ *  Copyright (C) 2010-2014 Michael Krufky <mkrufky@linuxtv.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
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf-gpio.c b/drivers/media/usb/dvb-usb-v2/mxl111sf-gpio.c
index e4121cb8f5ef..a619410adde4 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf-gpio.c
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf-gpio.c
@@ -1,7 +1,7 @@
 /*
  *  mxl111sf-gpio.c - driver for the MaxLinear MXL111SF
  *
- *  Copyright (C) 2010 Michael Krufky <mkrufky@kernellabs.com>
+ *  Copyright (C) 2010-2014 Michael Krufky <mkrufky@linuxtv.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
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf-gpio.h b/drivers/media/usb/dvb-usb-v2/mxl111sf-gpio.h
index 0220f54299a5..b85a5772d771 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf-gpio.h
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf-gpio.h
@@ -1,7 +1,7 @@
 /*
  *  mxl111sf-gpio.h - driver for the MaxLinear MXL111SF
  *
- *  Copyright (C) 2010 Michael Krufky <mkrufky@kernellabs.com>
+ *  Copyright (C) 2010-2014 Michael Krufky <mkrufky@linuxtv.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
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf-i2c.c b/drivers/media/usb/dvb-usb-v2/mxl111sf-i2c.c
index 34434557ef65..a101d06eb143 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf-i2c.c
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf-i2c.c
@@ -1,7 +1,7 @@
 /*
  *  mxl111sf-i2c.c - driver for the MaxLinear MXL111SF
  *
- *  Copyright (C) 2010 Michael Krufky <mkrufky@kernellabs.com>
+ *  Copyright (C) 2010-2014 Michael Krufky <mkrufky@linuxtv.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
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf-i2c.h b/drivers/media/usb/dvb-usb-v2/mxl111sf-i2c.h
index a57a45ffb9e4..465762145ad2 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf-i2c.h
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf-i2c.h
@@ -1,7 +1,7 @@
 /*
  *  mxl111sf-i2c.h - driver for the MaxLinear MXL111SF
  *
- *  Copyright (C) 2010 Michael Krufky <mkrufky@kernellabs.com>
+ *  Copyright (C) 2010-2014 Michael Krufky <mkrufky@linuxtv.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
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf-phy.c b/drivers/media/usb/dvb-usb-v2/mxl111sf-phy.c
index b741b3a7a325..f6b348024bec 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf-phy.c
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf-phy.c
@@ -1,7 +1,7 @@
 /*
  *  mxl111sf-phy.c - driver for the MaxLinear MXL111SF
  *
- *  Copyright (C) 2010 Michael Krufky <mkrufky@kernellabs.com>
+ *  Copyright (C) 2010-2014 Michael Krufky <mkrufky@linuxtv.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
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf-phy.h b/drivers/media/usb/dvb-usb-v2/mxl111sf-phy.h
index f0756071d347..0643738de7de 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf-phy.h
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf-phy.h
@@ -1,7 +1,7 @@
 /*
  *  mxl111sf-phy.h - driver for the MaxLinear MXL111SF
  *
- *  Copyright (C) 2010 Michael Krufky <mkrufky@kernellabs.com>
+ *  Copyright (C) 2010-2014 Michael Krufky <mkrufky@linuxtv.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
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf-reg.h b/drivers/media/usb/dvb-usb-v2/mxl111sf-reg.h
index 17831b0fb9db..89bf115e927e 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf-reg.h
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf-reg.h
@@ -1,7 +1,7 @@
 /*
  *  mxl111sf-reg.h - driver for the MaxLinear MXL111SF
  *
- *  Copyright (C) 2010 Michael Krufky <mkrufky@kernellabs.com>
+ *  Copyright (C) 2010-2014 Michael Krufky <mkrufky@linuxtv.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
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf-tuner.c b/drivers/media/usb/dvb-usb-v2/mxl111sf-tuner.c
index 879c529640f7..a8d2c7053674 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf-tuner.c
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf-tuner.c
@@ -1,7 +1,7 @@
 /*
  *  mxl111sf-tuner.c - driver for the MaxLinear MXL111SF CMOS tuner
  *
- *  Copyright (C) 2010 Michael Krufky <mkrufky@kernellabs.com>
+ *  Copyright (C) 2010-2014 Michael Krufky <mkrufky@linuxtv.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
@@ -512,7 +512,7 @@ struct dvb_frontend *mxl111sf_tuner_attach(struct dvb_frontend *fe,
 EXPORT_SYMBOL_GPL(mxl111sf_tuner_attach);
 
 MODULE_DESCRIPTION("MaxLinear MxL111SF CMOS tuner driver");
-MODULE_AUTHOR("Michael Krufky <mkrufky@kernellabs.com>");
+MODULE_AUTHOR("Michael Krufky <mkrufky@linuxtv.org>");
 MODULE_LICENSE("GPL");
 MODULE_VERSION("0.1");
 
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf-tuner.h b/drivers/media/usb/dvb-usb-v2/mxl111sf-tuner.h
index 90f583e5d6a6..2046db22519e 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf-tuner.h
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf-tuner.h
@@ -1,7 +1,7 @@
 /*
  *  mxl111sf-tuner.h - driver for the MaxLinear MXL111SF CMOS tuner
  *
- *  Copyright (C) 2010 Michael Krufky <mkrufky@kernellabs.com>
+ *  Copyright (C) 2010-2014 Michael Krufky <mkrufky@linuxtv.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
@@ -68,7 +68,7 @@ struct dvb_frontend *mxl111sf_tuner_attach(struct dvb_frontend *fe,
 #else
 static inline
 struct dvb_frontend *mxl111sf_tuner_attach(struct dvb_frontend *fe,
-					   struct mxl111sf_state *mxl_state
+					   struct mxl111sf_state *mxl_state,
 					   struct mxl111sf_tuner_config *cfg)
 {
 	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf.c b/drivers/media/usb/dvb-usb-v2/mxl111sf.c
index 08240e498451..c7304fa8ab73 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf.c
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2010 Michael Krufky (mkrufky@kernellabs.com)
+ * Copyright (C) 2010-2014 Michael Krufky (mkrufky@linuxtv.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
@@ -105,7 +105,7 @@ int mxl111sf_read_reg(struct mxl111sf_state *state, u8 addr, u8 *data)
 		ret = -EINVAL;
 	}
 
-	pr_debug("R: (0x%02x, 0x%02x)\n", addr, *data);
+	pr_debug("R: (0x%02x, 0x%02x)\n", addr, buf[1]);
 fail:
 	return ret;
 }
@@ -1421,7 +1421,7 @@ static struct usb_driver mxl111sf_usb_driver = {
 
 module_usb_driver(mxl111sf_usb_driver);
 
-MODULE_AUTHOR("Michael Krufky <mkrufky@kernellabs.com>");
+MODULE_AUTHOR("Michael Krufky <mkrufky@linuxtv.org>");
 MODULE_DESCRIPTION("Driver for MaxLinear MxL111SF");
 MODULE_VERSION("1.0");
 MODULE_LICENSE("GPL");
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf.h b/drivers/media/usb/dvb-usb-v2/mxl111sf.h
index 9816de86e48c..8516c011b7cc 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf.h
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2010 Michael Krufky (mkrufky@kernellabs.com)
+ * Copyright (C) 2010-2014 Michael Krufky (mkrufky@linuxtv.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
diff --git a/drivers/media/usb/dvb-usb-v2/rtl28xxu.c b/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
index ecca03667f98..fda5c64ba0e8 100644
--- a/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
+++ b/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
@@ -1407,6 +1407,8 @@ static const struct usb_device_id rtl28xxu_id_table[] = {
 		&rtl2832u_props, "Dexatek DK DVB-T Dongle", NULL) },
 	{ DVB_USB_DEVICE(USB_VID_LEADTEK, 0x6680,
 		&rtl2832u_props, "DigitalNow Quad DVB-T Receiver", NULL) },
+	{ DVB_USB_DEVICE(USB_VID_LEADTEK, USB_PID_WINFAST_DTV_DONGLE_MINID,
+		&rtl2832u_props, "Leadtek Winfast DTV Dongle Mini D", NULL) },
 	{ DVB_USB_DEVICE(USB_VID_TERRATEC, 0x00d3,
 		&rtl2832u_props, "TerraTec Cinergy T Stick RC (Rev. 3)", NULL) },
 	{ DVB_USB_DEVICE(USB_VID_DEXATEK, 0x1102,
diff --git a/drivers/media/usb/dvb-usb/cxusb.c b/drivers/media/usb/dvb-usb/cxusb.c
index 20e345d9fe8f..a1c641e18362 100644
--- a/drivers/media/usb/dvb-usb/cxusb.c
+++ b/drivers/media/usb/dvb-usb/cxusb.c
@@ -149,6 +149,7 @@ static int cxusb_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 			  int num)
 {
 	struct dvb_usb_device *d = i2c_get_adapdata(adap);
+	int ret;
 	int i;
 
 	if (mutex_lock_interruptible(&d->i2c_mutex) < 0)
@@ -173,7 +174,8 @@ static int cxusb_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 			if (1 + msg[i].len > sizeof(ibuf)) {
 				warn("i2c rd: len=%d is too big!\n",
 				     msg[i].len);
-				return -EOPNOTSUPP;
+				ret = -EOPNOTSUPP;
+				goto unlock;
 			}
 			obuf[0] = 0;
 			obuf[1] = msg[i].len;
@@ -193,12 +195,14 @@ static int cxusb_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 			if (3 + msg[i].len > sizeof(obuf)) {
 				warn("i2c wr: len=%d is too big!\n",
 				     msg[i].len);
-				return -EOPNOTSUPP;
+				ret = -EOPNOTSUPP;
+				goto unlock;
 			}
 			if (1 + msg[i + 1].len > sizeof(ibuf)) {
 				warn("i2c rd: len=%d is too big!\n",
 				     msg[i + 1].len);
-				return -EOPNOTSUPP;
+				ret = -EOPNOTSUPP;
+				goto unlock;
 			}
 			obuf[0] = msg[i].len;
 			obuf[1] = msg[i+1].len;
@@ -223,7 +227,8 @@ static int cxusb_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 			if (2 + msg[i].len > sizeof(obuf)) {
 				warn("i2c wr: len=%d is too big!\n",
 				     msg[i].len);
-				return -EOPNOTSUPP;
+				ret = -EOPNOTSUPP;
+				goto unlock;
 			}
 			obuf[0] = msg[i].addr;
 			obuf[1] = msg[i].len;
@@ -237,8 +242,14 @@ static int cxusb_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 		}
 	}
 
+	if (i == num)
+		ret = num;
+	else
+		ret = -EREMOTEIO;
+
+unlock:
 	mutex_unlock(&d->i2c_mutex);
-	return i == num ? num : -EREMOTEIO;
+	return ret;
 }
 
 static u32 cxusb_i2c_func(struct i2c_adapter *adapter)
diff --git a/drivers/media/usb/dvb-usb/dw2102.c b/drivers/media/usb/dvb-usb/dw2102.c
index c1a63b2a6baa..ae0f56a32e4d 100644
--- a/drivers/media/usb/dvb-usb/dw2102.c
+++ b/drivers/media/usb/dvb-usb/dw2102.c
@@ -2,7 +2,7 @@
  *	DVBWorld DVB-S 2101, 2102, DVB-S2 2104, DVB-C 3101,
  *	TeVii S600, S630, S650, S660, S480, S421, S632
  *	Prof 1100, 7500,
- *	Geniatech SU3000 Cards
+ *	Geniatech SU3000, T220 Cards
  * Copyright (C) 2008-2012 Igor M. Liplianin (liplianin@me.by)
  *
  *	This program is free software; you can redistribute it and/or modify it
@@ -29,6 +29,8 @@
 #include "stb6100.h"
 #include "stb6100_proc.h"
 #include "m88rs2000.h"
+#include "tda18271.h"
+#include "cxd2820r.h"
 
 /* Max transfer size done by I2C transfer functions */
 #define MAX_XFER_SIZE  64
@@ -110,11 +112,6 @@
 		"Please see linux/Documentation/dvb/ for more details " \
 		"on firmware-problems."
 
-struct rc_map_dvb_usb_table_table {
-	struct rc_map_table *rc_keys;
-	int rc_keys_size;
-};
-
 struct su3000_state {
 	u8 initialized;
 };
@@ -129,12 +126,6 @@ module_param_named(debug, dvb_usb_dw2102_debug, int, 0644);
 MODULE_PARM_DESC(debug, "set debugging level (1=info 2=xfer 4=rc(or-able))."
 						DVB_USB_DEBUG_STATUS);
 
-/* keymaps */
-static int ir_keymap;
-module_param_named(keymap, ir_keymap, int, 0644);
-MODULE_PARM_DESC(keymap, "set keymap 0=default 1=dvbworld 2=tevii 3=tbs  ..."
-			" 256=none");
-
 /* demod probe */
 static int demod_probe = 1;
 module_param_named(demod, demod_probe, int, 0644);
@@ -301,6 +292,7 @@ static int dw2102_serit_i2c_transfer(struct i2c_adapter *adap,
 static int dw2102_earda_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[], int num)
 {
 	struct dvb_usb_device *d = i2c_get_adapdata(adap);
+	int ret;
 
 	if (!d)
 		return -ENODEV;
@@ -316,7 +308,8 @@ static int dw2102_earda_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg ms
 		if (2 + msg[1].len > sizeof(ibuf)) {
 			warn("i2c rd: len=%d is too big!\n",
 			     msg[1].len);
-			return -EOPNOTSUPP;
+			ret = -EOPNOTSUPP;
+			goto unlock;
 		}
 
 		obuf[0] = msg[0].addr << 1;
@@ -340,7 +333,8 @@ static int dw2102_earda_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg ms
 			if (2 + msg[0].len > sizeof(obuf)) {
 				warn("i2c wr: len=%d is too big!\n",
 				     msg[1].len);
-				return -EOPNOTSUPP;
+				ret = -EOPNOTSUPP;
+				goto unlock;
 			}
 
 			obuf[0] = msg[0].addr << 1;
@@ -357,7 +351,8 @@ static int dw2102_earda_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg ms
 			if (2 + msg[0].len > sizeof(obuf)) {
 				warn("i2c wr: len=%d is too big!\n",
 				     msg[1].len);
-				return -EOPNOTSUPP;
+				ret = -EOPNOTSUPP;
+				goto unlock;
 			}
 
 			obuf[0] = msg[0].addr << 1;
@@ -386,15 +381,17 @@ static int dw2102_earda_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg ms
 
 		break;
 	}
+	ret = num;
 
+unlock:
 	mutex_unlock(&d->i2c_mutex);
-	return num;
+	return ret;
 }
 
 static int dw2104_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[], int num)
 {
 	struct dvb_usb_device *d = i2c_get_adapdata(adap);
-	int len, i, j;
+	int len, i, j, ret;
 
 	if (!d)
 		return -ENODEV;
@@ -430,7 +427,8 @@ static int dw2104_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[], i
 				if (2 + msg[j].len > sizeof(ibuf)) {
 					warn("i2c rd: len=%d is too big!\n",
 					     msg[j].len);
-					return -EOPNOTSUPP;
+					ret = -EOPNOTSUPP;
+					goto unlock;
 				}
 
 				dw210x_op_rw(d->udev, 0xc3,
@@ -466,7 +464,8 @@ static int dw2104_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[], i
 				if (2 + msg[j].len > sizeof(obuf)) {
 					warn("i2c wr: len=%d is too big!\n",
 					     msg[j].len);
-					return -EOPNOTSUPP;
+					ret = -EOPNOTSUPP;
+					goto unlock;
 				}
 
 				obuf[0] = msg[j].addr << 1;
@@ -481,15 +480,18 @@ static int dw2104_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[], i
 		}
 
 	}
+	ret = num;
 
+unlock:
 	mutex_unlock(&d->i2c_mutex);
-	return num;
+	return ret;
 }
 
 static int dw3101_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 								int num)
 {
 	struct dvb_usb_device *d = i2c_get_adapdata(adap);
+	int ret;
 	int i;
 
 	if (!d)
@@ -506,7 +508,8 @@ static int dw3101_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 		if (2 + msg[1].len > sizeof(ibuf)) {
 			warn("i2c rd: len=%d is too big!\n",
 			     msg[1].len);
-			return -EOPNOTSUPP;
+			ret = -EOPNOTSUPP;
+			goto unlock;
 		}
 		obuf[0] = msg[0].addr << 1;
 		obuf[1] = msg[0].len;
@@ -530,7 +533,8 @@ static int dw3101_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 			if (2 + msg[0].len > sizeof(obuf)) {
 				warn("i2c wr: len=%d is too big!\n",
 				     msg[0].len);
-				return -EOPNOTSUPP;
+				ret = -EOPNOTSUPP;
+				goto unlock;
 			}
 			obuf[0] = msg[0].addr << 1;
 			obuf[1] = msg[0].len;
@@ -556,9 +560,11 @@ static int dw3101_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 				msg[i].flags == 0 ? ">>>" : "<<<");
 		debug_dump(msg[i].buf, msg[i].len, deb_xfer);
 	}
+	ret = num;
 
+unlock:
 	mutex_unlock(&d->i2c_mutex);
-	return num;
+	return ret;
 }
 
 static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
@@ -566,7 +572,7 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 {
 	struct dvb_usb_device *d = i2c_get_adapdata(adap);
 	struct usb_device *udev;
-	int len, i, j;
+	int len, i, j, ret;
 
 	if (!d)
 		return -ENODEV;
@@ -618,7 +624,8 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 				if (msg[j].len > sizeof(ibuf)) {
 					warn("i2c rd: len=%d is too big!\n",
 					     msg[j].len);
-					return -EOPNOTSUPP;
+					ret = -EOPNOTSUPP;
+					goto unlock;
 				}
 
 				dw210x_op_rw(d->udev, 0x91, 0, 0,
@@ -652,7 +659,8 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 				if (2 + msg[j].len > sizeof(obuf)) {
 					warn("i2c wr: len=%d is too big!\n",
 					     msg[j].len);
-					return -EOPNOTSUPP;
+					ret = -EOPNOTSUPP;
+					goto unlock;
 				}
 
 				obuf[0] = msg[j + 1].len;
@@ -671,7 +679,8 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 				if (2 + msg[j].len > sizeof(obuf)) {
 					warn("i2c wr: len=%d is too big!\n",
 					     msg[j].len);
-					return -EOPNOTSUPP;
+					ret = -EOPNOTSUPP;
+					goto unlock;
 				}
 				obuf[0] = msg[j].len + 1;
 				obuf[1] = (msg[j].addr << 1);
@@ -685,9 +694,11 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 		}
 		}
 	}
+	ret = num;
 
+unlock:
 	mutex_unlock(&d->i2c_mutex);
-	return num;
+	return ret;
 }
 
 static int su3000_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
@@ -1095,6 +1106,16 @@ static struct ds3000_config su3000_ds3000_config = {
 	.set_lock_led = dw210x_led_ctrl,
 };
 
+static struct cxd2820r_config cxd2820r_config = {
+	.i2c_address = 0x6c, /* (0xd8 >> 1) */
+	.ts_mode = 0x38,
+};
+
+static struct tda18271_config tda18271_config = {
+	.output_opt = TDA18271_OUTPUT_LT_OFF,
+	.gate = TDA18271_GATE_DIGITAL,
+};
+
 static u8 m88rs2000_inittab[] = {
 	DEMOD_WRITE, 0x9a, 0x30,
 	DEMOD_WRITE, 0x00, 0x01,
@@ -1364,6 +1385,49 @@ static int su3000_frontend_attach(struct dvb_usb_adapter *d)
 	return -EIO;
 }
 
+static int t220_frontend_attach(struct dvb_usb_adapter *d)
+{
+	u8 obuf[3] = { 0xe, 0x80, 0 };
+	u8 ibuf[] = { 0 };
+
+	if (dvb_usb_generic_rw(d->dev, obuf, 3, ibuf, 1, 0) < 0)
+		err("command 0x0e transfer failed.");
+
+	obuf[0] = 0xe;
+	obuf[1] = 0x83;
+	obuf[2] = 0;
+
+	if (dvb_usb_generic_rw(d->dev, obuf, 3, ibuf, 1, 0) < 0)
+		err("command 0x0e transfer failed.");
+
+	msleep(100);
+
+	obuf[0] = 0xe;
+	obuf[1] = 0x80;
+	obuf[2] = 1;
+
+	if (dvb_usb_generic_rw(d->dev, obuf, 3, ibuf, 1, 0) < 0)
+		err("command 0x0e transfer failed.");
+
+	obuf[0] = 0x51;
+
+	if (dvb_usb_generic_rw(d->dev, obuf, 1, ibuf, 1, 0) < 0)
+		err("command 0x51 transfer failed.");
+
+	d->fe_adap[0].fe = dvb_attach(cxd2820r_attach, &cxd2820r_config,
+					&d->dev->i2c_adap, NULL);
+	if (d->fe_adap[0].fe != NULL) {
+		if (dvb_attach(tda18271_attach, d->fe_adap[0].fe, 0x60,
+					&d->dev->i2c_adap, &tda18271_config)) {
+			info("Attached TDA18271HD/CXD2820R!\n");
+			return 0;
+		}
+	}
+
+	info("Failed to attach TDA18271HD/CXD2820R!\n");
+	return -EIO;
+}
+
 static int m88rs2000_frontend_attach(struct dvb_usb_adapter *d)
 {
 	u8 obuf[] = { 0x51 };
@@ -1404,174 +1468,29 @@ static int dw3101_tuner_attach(struct dvb_usb_adapter *adap)
 	return 0;
 }
 
-static struct rc_map_table rc_map_dw210x_table[] = {
-	{ 0xf80a, KEY_POWER2 },		/*power*/
-	{ 0xf80c, KEY_MUTE },		/*mute*/
-	{ 0xf811, KEY_1 },
-	{ 0xf812, KEY_2 },
-	{ 0xf813, KEY_3 },
-	{ 0xf814, KEY_4 },
-	{ 0xf815, KEY_5 },
-	{ 0xf816, KEY_6 },
-	{ 0xf817, KEY_7 },
-	{ 0xf818, KEY_8 },
-	{ 0xf819, KEY_9 },
-	{ 0xf810, KEY_0 },
-	{ 0xf81c, KEY_CHANNELUP },	/*ch+*/
-	{ 0xf80f, KEY_CHANNELDOWN },	/*ch-*/
-	{ 0xf81a, KEY_VOLUMEUP },	/*vol+*/
-	{ 0xf80e, KEY_VOLUMEDOWN },	/*vol-*/
-	{ 0xf804, KEY_RECORD },		/*rec*/
-	{ 0xf809, KEY_FAVORITES },	/*fav*/
-	{ 0xf808, KEY_REWIND },		/*rewind*/
-	{ 0xf807, KEY_FASTFORWARD },	/*fast*/
-	{ 0xf80b, KEY_PAUSE },		/*pause*/
-	{ 0xf802, KEY_ESC },		/*cancel*/
-	{ 0xf803, KEY_TAB },		/*tab*/
-	{ 0xf800, KEY_UP },		/*up*/
-	{ 0xf81f, KEY_OK },		/*ok*/
-	{ 0xf801, KEY_DOWN },		/*down*/
-	{ 0xf805, KEY_CAMERA },		/*cap*/
-	{ 0xf806, KEY_STOP },		/*stop*/
-	{ 0xf840, KEY_ZOOM },		/*full*/
-	{ 0xf81e, KEY_TV },		/*tvmode*/
-	{ 0xf81b, KEY_LAST },		/*recall*/
-};
-
-static struct rc_map_table rc_map_tevii_table[] = {
-	{ 0xf80a, KEY_POWER },
-	{ 0xf80c, KEY_MUTE },
-	{ 0xf811, KEY_1 },
-	{ 0xf812, KEY_2 },
-	{ 0xf813, KEY_3 },
-	{ 0xf814, KEY_4 },
-	{ 0xf815, KEY_5 },
-	{ 0xf816, KEY_6 },
-	{ 0xf817, KEY_7 },
-	{ 0xf818, KEY_8 },
-	{ 0xf819, KEY_9 },
-	{ 0xf810, KEY_0 },
-	{ 0xf81c, KEY_MENU },
-	{ 0xf80f, KEY_VOLUMEDOWN },
-	{ 0xf81a, KEY_LAST },
-	{ 0xf80e, KEY_OPEN },
-	{ 0xf804, KEY_RECORD },
-	{ 0xf809, KEY_VOLUMEUP },
-	{ 0xf808, KEY_CHANNELUP },
-	{ 0xf807, KEY_PVR },
-	{ 0xf80b, KEY_TIME },
-	{ 0xf802, KEY_RIGHT },
-	{ 0xf803, KEY_LEFT },
-	{ 0xf800, KEY_UP },
-	{ 0xf81f, KEY_OK },
-	{ 0xf801, KEY_DOWN },
-	{ 0xf805, KEY_TUNER },
-	{ 0xf806, KEY_CHANNELDOWN },
-	{ 0xf840, KEY_PLAYPAUSE },
-	{ 0xf81e, KEY_REWIND },
-	{ 0xf81b, KEY_FAVORITES },
-	{ 0xf81d, KEY_BACK },
-	{ 0xf84d, KEY_FASTFORWARD },
-	{ 0xf844, KEY_EPG },
-	{ 0xf84c, KEY_INFO },
-	{ 0xf841, KEY_AB },
-	{ 0xf843, KEY_AUDIO },
-	{ 0xf845, KEY_SUBTITLE },
-	{ 0xf84a, KEY_LIST },
-	{ 0xf846, KEY_F1 },
-	{ 0xf847, KEY_F2 },
-	{ 0xf85e, KEY_F3 },
-	{ 0xf85c, KEY_F4 },
-	{ 0xf852, KEY_F5 },
-	{ 0xf85a, KEY_F6 },
-	{ 0xf856, KEY_MODE },
-	{ 0xf858, KEY_SWITCHVIDEOMODE },
-};
-
-static struct rc_map_table rc_map_tbs_table[] = {
-	{ 0xf884, KEY_POWER },
-	{ 0xf894, KEY_MUTE },
-	{ 0xf887, KEY_1 },
-	{ 0xf886, KEY_2 },
-	{ 0xf885, KEY_3 },
-	{ 0xf88b, KEY_4 },
-	{ 0xf88a, KEY_5 },
-	{ 0xf889, KEY_6 },
-	{ 0xf88f, KEY_7 },
-	{ 0xf88e, KEY_8 },
-	{ 0xf88d, KEY_9 },
-	{ 0xf892, KEY_0 },
-	{ 0xf896, KEY_CHANNELUP },
-	{ 0xf891, KEY_CHANNELDOWN },
-	{ 0xf893, KEY_VOLUMEUP },
-	{ 0xf88c, KEY_VOLUMEDOWN },
-	{ 0xf883, KEY_RECORD },
-	{ 0xf898, KEY_PAUSE  },
-	{ 0xf899, KEY_OK },
-	{ 0xf89a, KEY_SHUFFLE },
-	{ 0xf881, KEY_UP },
-	{ 0xf890, KEY_LEFT },
-	{ 0xf882, KEY_RIGHT },
-	{ 0xf888, KEY_DOWN },
-	{ 0xf895, KEY_FAVORITES },
-	{ 0xf897, KEY_SUBTITLE },
-	{ 0xf89d, KEY_ZOOM },
-	{ 0xf89f, KEY_EXIT },
-	{ 0xf89e, KEY_MENU },
-	{ 0xf89c, KEY_EPG },
-	{ 0xf880, KEY_PREVIOUS },
-	{ 0xf89b, KEY_MODE }
-};
+static int dw2102_rc_query(struct dvb_usb_device *d)
+{
+	u8 key[2];
+	struct i2c_msg msg = {
+		.addr = DW2102_RC_QUERY,
+		.flags = I2C_M_RD,
+		.buf = key,
+		.len = 2
+	};
 
-static struct rc_map_table rc_map_su3000_table[] = {
-	{ 0x25, KEY_POWER },	/* right-bottom Red */
-	{ 0x0a, KEY_MUTE },	/* -/-- */
-	{ 0x01, KEY_1 },
-	{ 0x02, KEY_2 },
-	{ 0x03, KEY_3 },
-	{ 0x04, KEY_4 },
-	{ 0x05, KEY_5 },
-	{ 0x06, KEY_6 },
-	{ 0x07, KEY_7 },
-	{ 0x08, KEY_8 },
-	{ 0x09, KEY_9 },
-	{ 0x00, KEY_0 },
-	{ 0x20, KEY_UP },	/* CH+ */
-	{ 0x21, KEY_DOWN },	/* CH+ */
-	{ 0x12, KEY_VOLUMEUP },	/* Brightness Up */
-	{ 0x13, KEY_VOLUMEDOWN },/* Brightness Down */
-	{ 0x1f, KEY_RECORD },
-	{ 0x17, KEY_PLAY },
-	{ 0x16, KEY_PAUSE },
-	{ 0x0b, KEY_STOP },
-	{ 0x27, KEY_FASTFORWARD },/* >> */
-	{ 0x26, KEY_REWIND },	/* << */
-	{ 0x0d, KEY_OK },	/* Mute */
-	{ 0x11, KEY_LEFT },	/* VOL- */
-	{ 0x10, KEY_RIGHT },	/* VOL+ */
-	{ 0x29, KEY_BACK },	/* button under 9 */
-	{ 0x2c, KEY_MENU },	/* TTX */
-	{ 0x2b, KEY_EPG },	/* EPG */
-	{ 0x1e, KEY_RED },	/* OSD */
-	{ 0x0e, KEY_GREEN },	/* Window */
-	{ 0x2d, KEY_YELLOW },	/* button under << */
-	{ 0x0f, KEY_BLUE },	/* bottom yellow button */
-	{ 0x14, KEY_AUDIO },	/* Snapshot */
-	{ 0x38, KEY_TV },	/* TV/Radio */
-	{ 0x0c, KEY_ESC }	/* upper Red button */
-};
+	if (d->props.i2c_algo->master_xfer(&d->i2c_adap, &msg, 1) == 1) {
+		if (msg.buf[0] != 0xff) {
+			deb_rc("%s: rc code: %x, %x\n",
+					__func__, key[0], key[1]);
+			rc_keydown(d->rc_dev, key[0], 1);
+		}
+	}
 
-static struct rc_map_dvb_usb_table_table keys_tables[] = {
-	{ rc_map_dw210x_table, ARRAY_SIZE(rc_map_dw210x_table) },
-	{ rc_map_tevii_table, ARRAY_SIZE(rc_map_tevii_table) },
-	{ rc_map_tbs_table, ARRAY_SIZE(rc_map_tbs_table) },
-	{ rc_map_su3000_table, ARRAY_SIZE(rc_map_su3000_table) },
-};
+	return 0;
+}
 
-static int dw2102_rc_query(struct dvb_usb_device *d, u32 *event, int *state)
+static int prof_rc_query(struct dvb_usb_device *d)
 {
-	struct rc_map_table *keymap = d->props.rc.legacy.rc_map_table;
-	int keymap_size = d->props.rc.legacy.rc_map_size;
 	u8 key[2];
 	struct i2c_msg msg = {
 		.addr = DW2102_RC_QUERY,
@@ -1579,32 +1498,34 @@ static int dw2102_rc_query(struct dvb_usb_device *d, u32 *event, int *state)
 		.buf = key,
 		.len = 2
 	};
-	int i;
-	/* override keymap */
-	if ((ir_keymap > 0) && (ir_keymap <= ARRAY_SIZE(keys_tables))) {
-		keymap = keys_tables[ir_keymap - 1].rc_keys ;
-		keymap_size = keys_tables[ir_keymap - 1].rc_keys_size;
-	} else if (ir_keymap > ARRAY_SIZE(keys_tables))
-		return 0; /* none */
-
-	*state = REMOTE_NO_KEY_PRESSED;
-	if (d->props.i2c_algo->master_xfer(&d->i2c_adap, &msg, 1) == 1) {
-		for (i = 0; i < keymap_size ; i++) {
-			if (rc5_data(&keymap[i]) == msg.buf[0]) {
-				*state = REMOTE_KEY_PRESSED;
-				*event = keymap[i].keycode;
-				break;
-			}
 
+	if (d->props.i2c_algo->master_xfer(&d->i2c_adap, &msg, 1) == 1) {
+		if (msg.buf[0] != 0xff) {
+			deb_rc("%s: rc code: %x, %x\n",
+					__func__, key[0], key[1]);
+			rc_keydown(d->rc_dev, key[0]^0xff, 1);
 		}
+	}
 
-		if ((*state) == REMOTE_KEY_PRESSED)
-			deb_rc("%s: found rc key: %x, %x, event: %x\n",
-					__func__, key[0], key[1], (*event));
-		else if (key[0] != 0xff)
-			deb_rc("%s: unknown rc key: %x, %x\n",
-					__func__, key[0], key[1]);
+	return 0;
+}
 
+static int su3000_rc_query(struct dvb_usb_device *d)
+{
+	u8 key[2];
+	struct i2c_msg msg = {
+		.addr = DW2102_RC_QUERY,
+		.flags = I2C_M_RD,
+		.buf = key,
+		.len = 2
+	};
+
+	if (d->props.i2c_algo->master_xfer(&d->i2c_adap, &msg, 1) == 1) {
+		if (msg.buf[0] != 0xff) {
+			deb_rc("%s: rc code: %x, %x\n",
+					__func__, key[0], key[1]);
+			rc_keydown(d->rc_dev, key[1] << 8 | key[0], 1);
+		}
 	}
 
 	return 0;
@@ -1630,6 +1551,7 @@ enum dw2102_table_entry {
 	TEVII_S632,
 	TERRATEC_CINERGY_S2_R2,
 	GOTVIEW_SAT_HD,
+	GENIATECH_T220,
 };
 
 static struct usb_device_id dw2102_table[] = {
@@ -1652,6 +1574,7 @@ static struct usb_device_id dw2102_table[] = {
 	[TEVII_S632] = {USB_DEVICE(0x9022, USB_PID_TEVII_S632)},
 	[TERRATEC_CINERGY_S2_R2] = {USB_DEVICE(USB_VID_TERRATEC, 0x00b0)},
 	[GOTVIEW_SAT_HD] = {USB_DEVICE(0x1FE1, USB_PID_GOTVIEW_SAT_HD)},
+	[GENIATECH_T220] = {USB_DEVICE(0x1f4d, 0xD220)},
 	{ }
 };
 
@@ -1711,9 +1634,7 @@ static int dw2102_load_firmware(struct usb_device *dev,
 		/* init registers */
 		switch (dev->descriptor.idProduct) {
 		case USB_PID_TEVII_S650:
-			dw2104_properties.rc.legacy.rc_map_table = rc_map_tevii_table;
-			dw2104_properties.rc.legacy.rc_map_size =
-					ARRAY_SIZE(rc_map_tevii_table);
+			dw2104_properties.rc.core.rc_codes = RC_MAP_TEVII_NEC;
 		case USB_PID_DW2104:
 			reset = 1;
 			dw210x_op_rw(dev, 0xc4, 0x0000, 0, &reset, 1,
@@ -1777,10 +1698,11 @@ static struct dvb_usb_device_properties dw2102_properties = {
 
 	.i2c_algo = &dw2102_serit_i2c_algo,
 
-	.rc.legacy = {
-		.rc_map_table = rc_map_dw210x_table,
-		.rc_map_size = ARRAY_SIZE(rc_map_dw210x_table),
+	.rc.core = {
 		.rc_interval = 150,
+		.rc_codes = RC_MAP_DM1105_NEC,
+		.module_name = "dw2102",
+		.allowed_protos   = RC_BIT_NEC,
 		.rc_query = dw2102_rc_query,
 	},
 
@@ -1831,10 +1753,11 @@ static struct dvb_usb_device_properties dw2104_properties = {
 	.no_reconnect = 1,
 
 	.i2c_algo = &dw2104_i2c_algo,
-	.rc.legacy = {
-		.rc_map_table = rc_map_dw210x_table,
-		.rc_map_size = ARRAY_SIZE(rc_map_dw210x_table),
+	.rc.core = {
 		.rc_interval = 150,
+		.rc_codes = RC_MAP_DM1105_NEC,
+		.module_name = "dw2102",
+		.allowed_protos   = RC_BIT_NEC,
 		.rc_query = dw2102_rc_query,
 	},
 
@@ -1881,10 +1804,11 @@ static struct dvb_usb_device_properties dw3101_properties = {
 	.no_reconnect = 1,
 
 	.i2c_algo = &dw3101_i2c_algo,
-	.rc.legacy = {
-		.rc_map_table = rc_map_dw210x_table,
-		.rc_map_size = ARRAY_SIZE(rc_map_dw210x_table),
+	.rc.core = {
 		.rc_interval = 150,
+		.rc_codes = RC_MAP_DM1105_NEC,
+		.module_name = "dw2102",
+		.allowed_protos   = RC_BIT_NEC,
 		.rc_query = dw2102_rc_query,
 	},
 
@@ -1929,10 +1853,11 @@ static struct dvb_usb_device_properties s6x0_properties = {
 	.no_reconnect = 1,
 
 	.i2c_algo = &s6x0_i2c_algo,
-	.rc.legacy = {
-		.rc_map_table = rc_map_tevii_table,
-		.rc_map_size = ARRAY_SIZE(rc_map_tevii_table),
+	.rc.core = {
 		.rc_interval = 150,
+		.rc_codes = RC_MAP_TEVII_NEC,
+		.module_name = "dw2102",
+		.allowed_protos   = RC_BIT_NEC,
 		.rc_query = dw2102_rc_query,
 	},
 
@@ -2022,11 +1947,12 @@ static struct dvb_usb_device_properties su3000_properties = {
 	.identify_state	= su3000_identify_state,
 	.i2c_algo = &su3000_i2c_algo,
 
-	.rc.legacy = {
-		.rc_map_table = rc_map_su3000_table,
-		.rc_map_size = ARRAY_SIZE(rc_map_su3000_table),
+	.rc.core = {
 		.rc_interval = 150,
-		.rc_query = dw2102_rc_query,
+		.rc_codes = RC_MAP_SU3000,
+		.module_name = "dw2102",
+		.allowed_protos   = RC_BIT_RC5,
+		.rc_query = su3000_rc_query,
 	},
 
 	.read_mac_address = su3000_read_mac_address,
@@ -2077,6 +2003,55 @@ static struct dvb_usb_device_properties su3000_properties = {
 	}
 };
 
+static struct dvb_usb_device_properties t220_properties = {
+	.caps = DVB_USB_IS_AN_I2C_ADAPTER,
+	.usb_ctrl = DEVICE_SPECIFIC,
+	.size_of_priv = sizeof(struct su3000_state),
+	.power_ctrl = su3000_power_ctrl,
+	.num_adapters = 1,
+	.identify_state	= su3000_identify_state,
+	.i2c_algo = &su3000_i2c_algo,
+
+	.rc.core = {
+		.rc_interval = 150,
+		.rc_codes = RC_MAP_SU3000,
+		.module_name = "dw2102",
+		.allowed_protos   = RC_BIT_RC5,
+		.rc_query = su3000_rc_query,
+	},
+
+	.read_mac_address = su3000_read_mac_address,
+
+	.generic_bulk_ctrl_endpoint = 0x01,
+
+	.adapter = {
+		{
+		.num_frontends = 1,
+		.fe = { {
+			.streaming_ctrl   = su3000_streaming_ctrl,
+			.frontend_attach  = t220_frontend_attach,
+			.stream = {
+				.type = USB_BULK,
+				.count = 8,
+				.endpoint = 0x82,
+				.u = {
+					.bulk = {
+						.buffersize = 4096,
+					}
+				}
+			}
+		} },
+		}
+	},
+	.num_device_descs = 1,
+	.devices = {
+		{ "Geniatech T220 DVB-T/T2 USB2.0",
+			{ &dw2102_table[GENIATECH_T220], NULL },
+			{ NULL },
+		},
+	}
+};
+
 static int dw2102_probe(struct usb_interface *intf,
 		const struct usb_device_id *id)
 {
@@ -2088,8 +2063,8 @@ static int dw2102_probe(struct usb_interface *intf,
 	/* fill only different fields */
 	p1100->firmware = P1100_FIRMWARE;
 	p1100->devices[0] = d1100;
-	p1100->rc.legacy.rc_map_table = rc_map_tbs_table;
-	p1100->rc.legacy.rc_map_size = ARRAY_SIZE(rc_map_tbs_table);
+	p1100->rc.core.rc_query = prof_rc_query;
+	p1100->rc.core.rc_codes = RC_MAP_TBS_NEC;
 	p1100->adapter->fe[0].frontend_attach = stv0288_frontend_attach;
 
 	s660 = kmemdup(&s6x0_properties,
@@ -2114,8 +2089,8 @@ static int dw2102_probe(struct usb_interface *intf,
 	}
 	p7500->firmware = P7500_FIRMWARE;
 	p7500->devices[0] = d7500;
-	p7500->rc.legacy.rc_map_table = rc_map_tbs_table;
-	p7500->rc.legacy.rc_map_size = ARRAY_SIZE(rc_map_tbs_table);
+	p7500->rc.core.rc_query = prof_rc_query;
+	p7500->rc.core.rc_codes = RC_MAP_TBS_NEC;
 	p7500->adapter->fe[0].frontend_attach = prof_7500_frontend_attach;
 
 
@@ -2149,7 +2124,9 @@ static int dw2102_probe(struct usb_interface *intf,
 	    0 == dvb_usb_device_init(intf, s421,
 			THIS_MODULE, NULL, adapter_nr) ||
 	    0 == dvb_usb_device_init(intf, &su3000_properties,
-				     THIS_MODULE, NULL, adapter_nr))
+			 THIS_MODULE, NULL, adapter_nr) ||
+	    0 == dvb_usb_device_init(intf, &t220_properties,
+			 THIS_MODULE, NULL, adapter_nr))
 		return 0;
 
 	return -ENODEV;
@@ -2169,7 +2146,7 @@ MODULE_DESCRIPTION("Driver for DVBWorld DVB-S 2101, 2102, DVB-S2 2104,"
 			" DVB-C 3101 USB2.0,"
 			" TeVii S600, S630, S650, S660, S480, S421, S632"
 			" Prof 1100, 7500 USB2.0,"
-			" Geniatech SU3000 devices");
+			" Geniatech SU3000, T220 devices");
 MODULE_VERSION("0.1");
 MODULE_LICENSE("GPL");
 MODULE_FIRMWARE(DW2101_FIRMWARE);
diff --git a/drivers/media/usb/em28xx/Kconfig b/drivers/media/usb/em28xx/Kconfig
index ca5ee6aceb62..a1fccf3096de 100644
--- a/drivers/media/usb/em28xx/Kconfig
+++ b/drivers/media/usb/em28xx/Kconfig
@@ -1,8 +1,12 @@
 config VIDEO_EM28XX
-	tristate "Empia EM28xx USB video capture support"
+	tristate "Empia EM28xx USB devices support"
 	depends on VIDEO_DEV && I2C
 	select VIDEO_TUNER
 	select VIDEO_TVEEPROM
+
+config VIDEO_EM28XX_V4L2
+	tristate "Empia EM28xx analog TV, video capture and/or webcam support"
+	depends on VIDEO_EM28XX
 	select VIDEOBUF2_VMALLOC
 	select VIDEO_SAA711X if MEDIA_SUBDRV_AUTOSELECT
 	select VIDEO_TVP5150 if MEDIA_SUBDRV_AUTOSELECT
@@ -49,6 +53,8 @@ config VIDEO_EM28XX_DVB
 	select DVB_MB86A20S if MEDIA_SUBDRV_AUTOSELECT
 	select MEDIA_TUNER_QT1010 if MEDIA_SUBDRV_AUTOSELECT
 	select MEDIA_TUNER_TDA18271 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_M88DS3103 if MEDIA_SUBDRV_AUTOSELECT
+	select MEDIA_TUNER_M88TS2022 if MEDIA_SUBDRV_AUTOSELECT
 	---help---
 	  This adds support for DVB cards based on the
 	  Empiatech em28xx chips.
diff --git a/drivers/media/usb/em28xx/Makefile b/drivers/media/usb/em28xx/Makefile
index ad6d48557940..3f850d5063d0 100644
--- a/drivers/media/usb/em28xx/Makefile
+++ b/drivers/media/usb/em28xx/Makefile
@@ -1,10 +1,11 @@
-em28xx-y +=	em28xx-video.o em28xx-i2c.o em28xx-cards.o
-em28xx-y +=	em28xx-core.o  em28xx-vbi.o em28xx-camera.o
+em28xx-y +=	em28xx-core.o em28xx-i2c.o em28xx-cards.o em28xx-camera.o
 
+em28xx-v4l-objs := em28xx-video.o em28xx-vbi.o
 em28xx-alsa-objs := em28xx-audio.o
 em28xx-rc-objs := em28xx-input.o
 
 obj-$(CONFIG_VIDEO_EM28XX) += em28xx.o
+obj-$(CONFIG_VIDEO_EM28XX_V4L2) += em28xx-v4l.o
 obj-$(CONFIG_VIDEO_EM28XX_ALSA) += em28xx-alsa.o
 obj-$(CONFIG_VIDEO_EM28XX_DVB) += em28xx-dvb.o
 obj-$(CONFIG_VIDEO_EM28XX_RC) += em28xx-rc.o
diff --git a/drivers/media/usb/em28xx/em28xx-audio.c b/drivers/media/usb/em28xx/em28xx-audio.c
index 2fdb66ee44ab..05e9bd11a3ff 100644
--- a/drivers/media/usb/em28xx/em28xx-audio.c
+++ b/drivers/media/usb/em28xx/em28xx-audio.c
@@ -3,7 +3,7 @@
  *
  *  Copyright (C) 2006 Markus Rechberger <mrechberger@gmail.com>
  *
- *  Copyright (C) 2007-2011 Mauro Carvalho Chehab <mchehab@redhat.com>
+ *  Copyright (C) 2007-2014 Mauro Carvalho Chehab
  *	- Port to work with the in-kernel driver
  *	- Cleanups, fixes, alsa-controls, etc.
  *
@@ -50,6 +50,9 @@ static int debug;
 module_param(debug, int, 0644);
 MODULE_PARM_DESC(debug, "activates debug info");
 
+#define EM28XX_MAX_AUDIO_BUFS		5
+#define EM28XX_MIN_AUDIO_PACKETS	64
+
 #define dprintk(fmt, arg...) do {					\
 	    if (debug)							\
 		printk(KERN_INFO "em28xx-audio %s: " fmt,		\
@@ -63,17 +66,13 @@ static int em28xx_deinit_isoc_audio(struct em28xx *dev)
 	int i;
 
 	dprintk("Stopping isoc\n");
-	for (i = 0; i < EM28XX_AUDIO_BUFS; i++) {
+	for (i = 0; i < dev->adev.num_urb; i++) {
+		struct urb *urb = dev->adev.urb[i];
+
 		if (!irqs_disabled())
-			usb_kill_urb(dev->adev.urb[i]);
+			usb_kill_urb(urb);
 		else
-			usb_unlink_urb(dev->adev.urb[i]);
-
-		usb_free_urb(dev->adev.urb[i]);
-		dev->adev.urb[i] = NULL;
-
-		kfree(dev->adev.transfer_buffer[i]);
-		dev->adev.transfer_buffer[i] = NULL;
+			usb_unlink_urb(urb);
 	}
 
 	return 0;
@@ -91,6 +90,12 @@ static void em28xx_audio_isocirq(struct urb *urb)
 	struct snd_pcm_substream *substream;
 	struct snd_pcm_runtime   *runtime;
 
+	if (dev->disconnected) {
+		dprintk("device disconnected while streaming. URB status=%d.\n", urb->status);
+		atomic_set(&dev->stream_started, 0);
+		return;
+	}
+
 	switch (urb->status) {
 	case 0:             /* success */
 	case -ETIMEDOUT:    /* NAK */
@@ -158,63 +163,27 @@ static void em28xx_audio_isocirq(struct urb *urb)
 	urb->status = 0;
 
 	status = usb_submit_urb(urb, GFP_ATOMIC);
-	if (status < 0) {
+	if (status < 0)
 		em28xx_errdev("resubmit of audio urb failed (error=%i)\n",
 			      status);
-	}
 	return;
 }
 
 static int em28xx_init_audio_isoc(struct em28xx *dev)
 {
 	int       i, errCode;
-	const int sb_size = EM28XX_NUM_AUDIO_PACKETS *
-			    EM28XX_AUDIO_MAX_PACKET_SIZE;
 
 	dprintk("Starting isoc transfers\n");
 
-	for (i = 0; i < EM28XX_AUDIO_BUFS; i++) {
-		struct urb *urb;
-		int j, k;
-
-		dev->adev.transfer_buffer[i] = kmalloc(sb_size, GFP_ATOMIC);
-		if (!dev->adev.transfer_buffer[i])
-			return -ENOMEM;
-
-		memset(dev->adev.transfer_buffer[i], 0x80, sb_size);
-		urb = usb_alloc_urb(EM28XX_NUM_AUDIO_PACKETS, GFP_ATOMIC);
-		if (!urb) {
-			em28xx_errdev("usb_alloc_urb failed!\n");
-			for (j = 0; j < i; j++) {
-				usb_free_urb(dev->adev.urb[j]);
-				kfree(dev->adev.transfer_buffer[j]);
-			}
-			return -ENOMEM;
-		}
-
-		urb->dev = dev->udev;
-		urb->context = dev;
-		urb->pipe = usb_rcvisocpipe(dev->udev, EM28XX_EP_AUDIO);
-		urb->transfer_flags = URB_ISO_ASAP;
-		urb->transfer_buffer = dev->adev.transfer_buffer[i];
-		urb->interval = 1;
-		urb->complete = em28xx_audio_isocirq;
-		urb->number_of_packets = EM28XX_NUM_AUDIO_PACKETS;
-		urb->transfer_buffer_length = sb_size;
-
-		for (j = k = 0; j < EM28XX_NUM_AUDIO_PACKETS;
-			     j++, k += EM28XX_AUDIO_MAX_PACKET_SIZE) {
-			urb->iso_frame_desc[j].offset = k;
-			urb->iso_frame_desc[j].length =
-			    EM28XX_AUDIO_MAX_PACKET_SIZE;
-		}
-		dev->adev.urb[i] = urb;
-	}
+	/* Start streaming */
+	for (i = 0; i < dev->adev.num_urb; i++) {
+		memset(dev->adev.transfer_buffer[i], 0x80,
+		       dev->adev.urb[i]->transfer_buffer_length);
 
-	for (i = 0; i < EM28XX_AUDIO_BUFS; i++) {
 		errCode = usb_submit_urb(dev->adev.urb[i], GFP_ATOMIC);
 		if (errCode) {
-			em28xx_errdev("submit of audio urb failed\n");
+			em28xx_errdev("submit of audio urb failed (error=%i)\n",
+				      errCode);
 			em28xx_deinit_isoc_audio(dev);
 			atomic_set(&dev->stream_started, 0);
 			return errCode;
@@ -255,15 +224,26 @@ static struct snd_pcm_hardware snd_em28xx_hw_capture = {
 
 	.formats = SNDRV_PCM_FMTBIT_S16_LE,
 
-	.rates = SNDRV_PCM_RATE_CONTINUOUS | SNDRV_PCM_RATE_KNOT,
+	.rates = SNDRV_PCM_RATE_48000,
 
 	.rate_min = 48000,
 	.rate_max = 48000,
 	.channels_min = 2,
 	.channels_max = 2,
 	.buffer_bytes_max = 62720 * 8,	/* just about the value in usbaudio.c */
-	.period_bytes_min = 64,		/* 12544/2, */
-	.period_bytes_max = 12544,
+
+
+	/*
+	 * The period is 12.288 bytes. Allow a 10% of variation along its
+	 * value, in order to avoid overruns/underruns due to some clock
+	 * drift.
+	 *
+	 * FIXME: This period assumes 64 packets, and a 48000 PCM rate.
+	 * Calculate it dynamically.
+	 */
+	.period_bytes_min = 11059,
+	.period_bytes_max = 13516,
+
 	.periods_min = 2,
 	.periods_max = 98,		/* 12544, */
 };
@@ -274,28 +254,48 @@ static int snd_em28xx_capture_open(struct snd_pcm_substream *substream)
 	struct snd_pcm_runtime *runtime = substream->runtime;
 	int ret = 0;
 
-	dprintk("opening device and trying to acquire exclusive lock\n");
-
 	if (!dev) {
 		em28xx_err("BUG: em28xx can't find device struct."
 				" Can't proceed with open\n");
 		return -ENODEV;
 	}
 
+	if (dev->disconnected)
+		return -ENODEV;
+
+	dprintk("opening device and trying to acquire exclusive lock\n");
+
 	runtime->hw = snd_em28xx_hw_capture;
-	if ((dev->alt == 0 || dev->audio_ifnum) && dev->adev.users == 0) {
-		if (dev->audio_ifnum)
+	if ((dev->alt == 0 || dev->is_audio_only) && dev->adev.users == 0) {
+		int nonblock = !!(substream->f_flags & O_NONBLOCK);
+
+		if (nonblock) {
+			if (!mutex_trylock(&dev->lock))
+				return -EAGAIN;
+		} else
+			mutex_lock(&dev->lock);
+		if (dev->is_audio_only)
+			/* vendor audio is on a separate interface */
 			dev->alt = 1;
 		else
+			/* vendor audio is on the same interface as video */
 			dev->alt = 7;
+			/*
+			 * FIXME: The intention seems to be to select the alt
+			 * setting with the largest wMaxPacketSize for the video
+			 * endpoint.
+			 * At least dev->alt should be used instead, but we
+			 * should probably not touch it at all if it is
+			 * already >0, because wMaxPacketSize of the audio
+			 * endpoints seems to be the same for all.
+			 */
 
 		dprintk("changing alternate number on interface %d to %d\n",
-			dev->audio_ifnum, dev->alt);
-		usb_set_interface(dev->udev, dev->audio_ifnum, dev->alt);
+			dev->ifnum, dev->alt);
+		usb_set_interface(dev->udev, dev->ifnum, dev->alt);
 
 		/* Sets volume, mute, etc */
 		dev->mute = 0;
-		mutex_lock(&dev->lock);
 		ret = em28xx_audio_analog_set(dev);
 		if (ret < 0)
 			goto err;
@@ -304,7 +304,12 @@ static int snd_em28xx_capture_open(struct snd_pcm_substream *substream)
 		mutex_unlock(&dev->lock);
 	}
 
+	/* Dynamically adjust the period size */
 	snd_pcm_hw_constraint_integer(runtime, SNDRV_PCM_HW_PARAM_PERIODS);
+	snd_pcm_hw_constraint_minmax(runtime, SNDRV_PCM_HW_PARAM_PERIOD_BYTES,
+				     dev->adev.period * 95 / 100,
+				     dev->adev.period * 105 / 100);
+
 	dev->adev.capture_pcm_substream = substream;
 
 	return 0;
@@ -344,6 +349,10 @@ static int snd_em28xx_hw_capture_params(struct snd_pcm_substream *substream,
 					struct snd_pcm_hw_params *hw_params)
 {
 	int ret;
+	struct em28xx *dev = snd_pcm_substream_chip(substream);
+
+	if (dev->disconnected)
+		return -ENODEV;
 
 	dprintk("Setting capture parameters\n");
 
@@ -383,6 +392,9 @@ static int snd_em28xx_prepare(struct snd_pcm_substream *substream)
 {
 	struct em28xx *dev = snd_pcm_substream_chip(substream);
 
+	if (dev->disconnected)
+		return -ENODEV;
+
 	dev->adev.hwptr_done_capture = 0;
 	dev->adev.capture_transfer_done = 0;
 
@@ -408,6 +420,9 @@ static int snd_em28xx_capture_trigger(struct snd_pcm_substream *substream,
 	struct em28xx *dev = snd_pcm_substream_chip(substream);
 	int retval = 0;
 
+	if (dev->disconnected)
+		return -ENODEV;
+
 	switch (cmd) {
 	case SNDRV_PCM_TRIGGER_PAUSE_RELEASE: /* fall through */
 	case SNDRV_PCM_TRIGGER_RESUME: /* fall through */
@@ -434,6 +449,9 @@ static snd_pcm_uframes_t snd_em28xx_capture_pointer(struct snd_pcm_substream
 	snd_pcm_uframes_t hwptr_done;
 
 	dev = snd_pcm_substream_chip(substream);
+	if (dev->disconnected)
+		return SNDRV_PCM_POS_XRUN;
+
 	spin_lock_irqsave(&dev->adev.slock, flags);
 	hwptr_done = dev->adev.hwptr_done_capture;
 	spin_unlock_irqrestore(&dev->adev.slock, flags);
@@ -455,6 +473,11 @@ static struct page *snd_pcm_get_vmalloc_page(struct snd_pcm_substream *subs,
 static int em28xx_vol_info(struct snd_kcontrol *kcontrol,
 				struct snd_ctl_elem_info *info)
 {
+	struct em28xx *dev = snd_kcontrol_chip(kcontrol);
+
+	if (dev->disconnected)
+		return -ENODEV;
+
 	info->type = SNDRV_CTL_ELEM_TYPE_INTEGER;
 	info->count = 2;
 	info->value.integer.min = 0;
@@ -467,11 +490,22 @@ static int em28xx_vol_put(struct snd_kcontrol *kcontrol,
 			       struct snd_ctl_elem_value *value)
 {
 	struct em28xx *dev = snd_kcontrol_chip(kcontrol);
+	struct snd_pcm_substream *substream = dev->adev.capture_pcm_substream;
 	u16 val = (0x1f - (value->value.integer.value[0] & 0x1f)) |
 		  (0x1f - (value->value.integer.value[1] & 0x1f)) << 8;
+	int nonblock = 0;
 	int rc;
 
-	mutex_lock(&dev->lock);
+	if (dev->disconnected)
+		return -ENODEV;
+
+	if (substream)
+		nonblock = !!(substream->f_flags & O_NONBLOCK);
+	if (nonblock) {
+		if (!mutex_trylock(&dev->lock))
+			return -EAGAIN;
+	} else
+		mutex_lock(&dev->lock);
 	rc = em28xx_read_ac97(dev, kcontrol->private_value);
 	if (rc < 0)
 		goto err;
@@ -496,9 +530,20 @@ static int em28xx_vol_get(struct snd_kcontrol *kcontrol,
 			       struct snd_ctl_elem_value *value)
 {
 	struct em28xx *dev = snd_kcontrol_chip(kcontrol);
+	struct snd_pcm_substream *substream = dev->adev.capture_pcm_substream;
+	int nonblock = 0;
 	int val;
 
-	mutex_lock(&dev->lock);
+	if (dev->disconnected)
+		return -ENODEV;
+
+	if (substream)
+		nonblock = !!(substream->f_flags & O_NONBLOCK);
+	if (nonblock) {
+		if (!mutex_trylock(&dev->lock))
+			return -EAGAIN;
+	} else
+		mutex_lock(&dev->lock);
 	val = em28xx_read_ac97(dev, kcontrol->private_value);
 	mutex_unlock(&dev->lock);
 	if (val < 0)
@@ -520,9 +565,20 @@ static int em28xx_vol_put_mute(struct snd_kcontrol *kcontrol,
 {
 	struct em28xx *dev = snd_kcontrol_chip(kcontrol);
 	u16 val = value->value.integer.value[0];
+	struct snd_pcm_substream *substream = dev->adev.capture_pcm_substream;
+	int nonblock = 0;
 	int rc;
 
-	mutex_lock(&dev->lock);
+	if (dev->disconnected)
+		return -ENODEV;
+
+	if (substream)
+		nonblock = !!(substream->f_flags & O_NONBLOCK);
+	if (nonblock) {
+		if (!mutex_trylock(&dev->lock))
+			return -EAGAIN;
+	} else
+		mutex_lock(&dev->lock);
 	rc = em28xx_read_ac97(dev, kcontrol->private_value);
 	if (rc < 0)
 		goto err;
@@ -550,9 +606,20 @@ static int em28xx_vol_get_mute(struct snd_kcontrol *kcontrol,
 			       struct snd_ctl_elem_value *value)
 {
 	struct em28xx *dev = snd_kcontrol_chip(kcontrol);
+	struct snd_pcm_substream *substream = dev->adev.capture_pcm_substream;
+	int nonblock = 0;
 	int val;
 
-	mutex_lock(&dev->lock);
+	if (dev->disconnected)
+		return -ENODEV;
+
+	if (substream)
+		nonblock = !!(substream->f_flags & O_NONBLOCK);
+	if (nonblock) {
+		if (!mutex_trylock(&dev->lock))
+			return -EAGAIN;
+	} else
+		mutex_lock(&dev->lock);
 	val = em28xx_read_ac97(dev, kcontrol->private_value);
 	mutex_unlock(&dev->lock);
 	if (val < 0)
@@ -634,25 +701,204 @@ static struct snd_pcm_ops snd_em28xx_pcm_capture = {
 	.page      = snd_pcm_get_vmalloc_page,
 };
 
+static void em28xx_audio_free_urb(struct em28xx *dev)
+{
+	int i;
+
+	for (i = 0; i < dev->adev.num_urb; i++) {
+		struct urb *urb = dev->adev.urb[i];
+
+		if (!urb)
+			continue;
+
+		usb_free_coherent(dev->udev, urb->transfer_buffer_length,
+				  dev->adev.transfer_buffer[i],
+				  urb->transfer_dma);
+
+		usb_free_urb(urb);
+	}
+	kfree(dev->adev.urb);
+	kfree(dev->adev.transfer_buffer);
+	dev->adev.num_urb = 0;
+}
+
+/* high bandwidth multiplier, as encoded in highspeed endpoint descriptors */
+static int em28xx_audio_ep_packet_size(struct usb_device *udev,
+					struct usb_endpoint_descriptor *e)
+{
+	int size = le16_to_cpu(e->wMaxPacketSize);
+
+	if (udev->speed == USB_SPEED_HIGH)
+		return (size & 0x7ff) *  (1 + (((size) >> 11) & 0x03));
+
+	return size & 0x7ff;
+}
+
+static int em28xx_audio_urb_init(struct em28xx *dev)
+{
+	struct usb_interface *intf;
+	struct usb_endpoint_descriptor *e, *ep = NULL;
+	int                 i, ep_size, interval, num_urb, npackets;
+	int		    urb_size, bytes_per_transfer;
+	u8 alt;
+
+	if (dev->ifnum)
+		alt = 1;
+	else
+		alt = 7;
+
+	intf = usb_ifnum_to_if(dev->udev, dev->ifnum);
+
+	if (intf->num_altsetting <= alt) {
+		em28xx_errdev("alt %d doesn't exist on interface %d\n",
+			      dev->ifnum, alt);
+		return -ENODEV;
+	}
+
+	for (i = 0; i < intf->altsetting[alt].desc.bNumEndpoints; i++) {
+		e = &intf->altsetting[alt].endpoint[i].desc;
+		if (!usb_endpoint_dir_in(e))
+			continue;
+		if (e->bEndpointAddress == EM28XX_EP_AUDIO) {
+			ep = e;
+			break;
+		}
+	}
+
+	if (!ep) {
+		em28xx_errdev("Couldn't find an audio endpoint");
+		return -ENODEV;
+	}
+
+	ep_size = em28xx_audio_ep_packet_size(dev->udev, ep);
+	interval = 1 << (ep->bInterval - 1);
+
+	em28xx_info("Endpoint 0x%02x %s on intf %d alt %d interval = %d, size %d\n",
+		     EM28XX_EP_AUDIO, usb_speed_string(dev->udev->speed),
+		     dev->ifnum, alt,
+		     interval,
+		     ep_size);
+
+	/* Calculate the number and size of URBs to better fit the audio samples */
+
+	/*
+	 * Estimate the number of bytes per DMA transfer.
+	 *
+	 * This is given by the bit rate (for now, only 48000 Hz) multiplied
+	 * by 2 channels and 2 bytes/sample divided by the number of microframe
+	 * intervals and by the microframe rate (125 us)
+	 */
+	bytes_per_transfer = DIV_ROUND_UP(48000 * 2 * 2, 125 * interval);
+
+	/*
+	 * Estimate the number of transfer URBs. Don't let it go past the
+	 * maximum number of URBs that is known to be supported by the device.
+	 */
+	num_urb = DIV_ROUND_UP(bytes_per_transfer, ep_size);
+	if (num_urb > EM28XX_MAX_AUDIO_BUFS)
+		num_urb = EM28XX_MAX_AUDIO_BUFS;
+
+	/*
+	 * Now that we know the number of bytes per transfer and the number of
+	 * URBs, estimate the typical size of an URB, in order to adjust the
+	 * minimal number of packets.
+	 */
+	urb_size = bytes_per_transfer / num_urb;
+
+	/*
+	 * Now, calculate the amount of audio packets to be filled on each
+	 * URB. In order to preserve the old behaviour, use a minimal
+	 * threshold for this value.
+	 */
+	npackets = EM28XX_MIN_AUDIO_PACKETS;
+	if (urb_size > ep_size * npackets)
+		npackets = DIV_ROUND_UP(urb_size, ep_size);
+
+	em28xx_info("Number of URBs: %d, with %d packets and %d size",
+		    num_urb, npackets, urb_size);
+
+	/* Estimate the bytes per period */
+	dev->adev.period = urb_size * npackets;
+
+	/* Allocate space to store the number of URBs to be used */
+
+	dev->adev.transfer_buffer = kcalloc(num_urb,
+					    sizeof(*dev->adev.transfer_buffer),
+					    GFP_ATOMIC);
+	if (!dev->adev.transfer_buffer) {
+		return -ENOMEM;
+	}
+
+	dev->adev.urb = kcalloc(num_urb, sizeof(*dev->adev.urb), GFP_ATOMIC);
+	if (!dev->adev.urb) {
+		kfree(dev->adev.transfer_buffer);
+		return -ENOMEM;
+	}
+
+	/* Alloc memory for each URB and for each transfer buffer */
+	dev->adev.num_urb = num_urb;
+	for (i = 0; i < num_urb; i++) {
+		struct urb *urb;
+		int j, k;
+		void *buf;
+
+		urb = usb_alloc_urb(npackets, GFP_ATOMIC);
+		if (!urb) {
+			em28xx_errdev("usb_alloc_urb failed!\n");
+			em28xx_audio_free_urb(dev);
+			return -ENOMEM;
+		}
+		dev->adev.urb[i] = urb;
+
+		buf = usb_alloc_coherent(dev->udev, npackets * ep_size, GFP_ATOMIC,
+					 &urb->transfer_dma);
+		if (!buf) {
+			em28xx_errdev("usb_alloc_coherent failed!\n");
+			em28xx_audio_free_urb(dev);
+			return -ENOMEM;
+		}
+		dev->adev.transfer_buffer[i] = buf;
+
+		urb->dev = dev->udev;
+		urb->context = dev;
+		urb->pipe = usb_rcvisocpipe(dev->udev, EM28XX_EP_AUDIO);
+		urb->transfer_flags = URB_ISO_ASAP | URB_NO_TRANSFER_DMA_MAP;
+		urb->transfer_buffer = buf;
+		urb->interval = interval;
+		urb->complete = em28xx_audio_isocirq;
+		urb->number_of_packets = npackets;
+		urb->transfer_buffer_length = ep_size * npackets;
+
+		for (j = k = 0; j < npackets; j++, k += ep_size) {
+			urb->iso_frame_desc[j].offset = k;
+			urb->iso_frame_desc[j].length = ep_size;
+		}
+	}
+
+	return 0;
+}
+
 static int em28xx_audio_init(struct em28xx *dev)
 {
 	struct em28xx_audio *adev = &dev->adev;
 	struct snd_pcm      *pcm;
 	struct snd_card     *card;
 	static int          devnr;
-	int                 err;
+	int		    err;
 
-	if (!dev->has_alsa_audio || dev->audio_ifnum < 0) {
+	if (!dev->has_alsa_audio) {
 		/* This device does not support the extension (in this case
 		   the device is expecting the snd-usb-audio module or
 		   doesn't have analog audio support at all) */
 		return 0;
 	}
 
-	printk(KERN_INFO "em28xx-audio.c: probing for em28xx Audio Vendor Class\n");
+	em28xx_info("Binding audio extension\n");
+
 	printk(KERN_INFO "em28xx-audio.c: Copyright (C) 2006 Markus "
 			 "Rechberger\n");
-	printk(KERN_INFO "em28xx-audio.c: Copyright (C) 2007-2011 Mauro Carvalho Chehab\n");
+	printk(KERN_INFO
+	       "em28xx-audio.c: Copyright (C) 2007-2014 Mauro Carvalho Chehab\n");
 
 	err = snd_card_create(index[devnr], "Em28xx Audio", THIS_MODULE, 0,
 			      &card);
@@ -660,11 +906,12 @@ static int em28xx_audio_init(struct em28xx *dev)
 		return err;
 
 	spin_lock_init(&adev->slock);
+	adev->sndcard = card;
+	adev->udev = dev->udev;
+
 	err = snd_pcm_new(card, "Em28xx Audio", 0, 0, 1, &pcm);
-	if (err < 0) {
-		snd_card_free(card);
-		return err;
-	}
+	if (err < 0)
+		goto card_free;
 
 	snd_pcm_set_ops(pcm, SNDRV_PCM_STREAM_CAPTURE, &snd_em28xx_pcm_capture);
 	pcm->info_flags = 0;
@@ -694,15 +941,25 @@ static int em28xx_audio_init(struct em28xx *dev)
 		em28xx_cvol_new(card, dev, "Surround", AC97_SURROUND_MASTER);
 	}
 
+	err = em28xx_audio_urb_init(dev);
+	if (err)
+		goto card_free;
+
 	err = snd_card_register(card);
-	if (err < 0) {
-		snd_card_free(card);
-		return err;
-	}
-	adev->sndcard = card;
-	adev->udev = dev->udev;
+	if (err < 0)
+		goto urb_free;
 
+	em28xx_info("Audio extension successfully initialized\n");
 	return 0;
+
+urb_free:
+	em28xx_audio_free_urb(dev);
+
+card_free:
+	snd_card_free(card);
+	adev->sndcard = NULL;
+
+	return err;
 }
 
 static int em28xx_audio_fini(struct em28xx *dev)
@@ -717,7 +974,14 @@ static int em28xx_audio_fini(struct em28xx *dev)
 		return 0;
 	}
 
+	em28xx_info("Closing audio extension");
+
 	if (dev->adev.sndcard) {
+		snd_card_disconnect(dev->adev.sndcard);
+		flush_work(&dev->wq_trigger);
+
+		em28xx_audio_free_urb(dev);
+
 		snd_card_free(dev->adev.sndcard);
 		dev->adev.sndcard = NULL;
 	}
@@ -745,7 +1009,8 @@ static void __exit em28xx_alsa_unregister(void)
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Markus Rechberger <mrechberger@gmail.com>");
 MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@redhat.com>");
-MODULE_DESCRIPTION("Em28xx Audio driver");
+MODULE_DESCRIPTION(DRIVER_DESC " - audio interface");
+MODULE_VERSION(EM28XX_VERSION);
 
 module_init(em28xx_alsa_register);
 module_exit(em28xx_alsa_unregister);
diff --git a/drivers/media/usb/em28xx/em28xx-camera.c b/drivers/media/usb/em28xx/em28xx-camera.c
index d666741797d4..c29f5c4e7b40 100644
--- a/drivers/media/usb/em28xx/em28xx-camera.c
+++ b/drivers/media/usb/em28xx/em28xx-camera.c
@@ -454,3 +454,4 @@ int em28xx_init_camera(struct em28xx *dev)
 
 	return ret;
 }
+EXPORT_SYMBOL_GPL(em28xx_init_camera);
diff --git a/drivers/media/usb/em28xx/em28xx-cards.c b/drivers/media/usb/em28xx/em28xx-cards.c
index a5196697627f..4d97a76cc3b0 100644
--- a/drivers/media/usb/em28xx/em28xx-cards.c
+++ b/drivers/media/usb/em28xx/em28xx-cards.c
@@ -36,7 +36,6 @@
 #include <media/tvaudio.h>
 #include <media/i2c-addr.h>
 #include <media/tveeprom.h>
-#include <media/v4l2-clk.h>
 #include <media/v4l2-common.h>
 
 #include "em28xx.h"
@@ -67,7 +66,7 @@ MODULE_PARM_DESC(usb_xfer_mode,
 
 
 /* Bitmask marking allocated devices from 0 to EM28XX_MAXBOARDS - 1 */
-static unsigned long em28xx_devused;
+DECLARE_BITMAP(em28xx_devused, EM28XX_MAXBOARDS);
 
 struct em28xx_hash_table {
 	unsigned long hash;
@@ -356,6 +355,28 @@ static struct em28xx_reg_seq c3tech_digital_duo_digital[] = {
 	{	-1,			-1,	-1,	-1},
 };
 
+/*
+ * 2013:0258 PCTV DVB-S2 Stick (461e)
+ * GPIO 0 = POWER_ON
+ * GPIO 1 = BOOST
+ * GPIO 2 = VUV_LNB (red LED)
+ * GPIO 3 = #EXT_12V
+ * GPIO 4 = INT_DEM
+ * GPIO 5 = INT_LNB
+ * GPIO 6 = #RESET_DEM
+ * GPIO 7 = P07_LED (green LED)
+ */
+static struct em28xx_reg_seq pctv_461e[] = {
+	{EM2874_R80_GPIO_P0_CTRL,      0x7f, 0xff,    0},
+	{0x0d,                 0xff, 0xff,    0},
+	{EM2874_R80_GPIO_P0_CTRL,      0x3f, 0xff,  100}, /* reset demod */
+	{EM2874_R80_GPIO_P0_CTRL,      0x7f, 0xff,  200}, /* reset demod */
+	{0x0d,                 0x42, 0xff,    0},
+	{EM2874_R80_GPIO_P0_CTRL,      0xeb, 0xff,    0},
+	{EM2874_R5F_TS_ENABLE, 0x84, 0x84,    0}, /* parallel? | null discard */
+	{                  -1,   -1,   -1,   -1},
+};
+
 #if 0
 static struct em28xx_reg_seq hauppauge_930c_gpio[] = {
 	{EM2874_R80_GPIO_P0_CTRL,	0x6f,	0xff,	10},
@@ -412,6 +433,70 @@ static struct em28xx_reg_seq pctv_520e[] = {
 	{	-1,			-1,	-1,	-1},
 };
 
+/* 1ae7:9003/9004 SpeedLink Vicious And Devine Laplace webcam
+ * reg 0x80/0x84:
+ * GPIO_0: capturing LED, 0=on, 1=off
+ * GPIO_2: AV mute button, 0=pressed, 1=unpressed
+ * GPIO 3: illumination button, 0=pressed, 1=unpressed
+ * GPIO_6: illumination/flash LED, 0=on, 1=off
+ * reg 0x81/0x85:
+ * GPIO_7: snapshot button, 0=pressed, 1=unpressed
+ */
+static struct em28xx_reg_seq speedlink_vad_laplace_reg_seq[] = {
+	{EM2820_R08_GPIO_CTRL,		0xf7,	0xff,	10},
+	{EM2874_R80_GPIO_P0_CTRL,	0xff,	0xb2,	10},
+	{	-1,			-1,	-1,	-1},
+};
+
+/*
+ *  Button definitions
+ */
+static struct em28xx_button std_snapshot_button[] = {
+	{
+		.role         = EM28XX_BUTTON_SNAPSHOT,
+		.reg_r        = EM28XX_R0C_USBSUSP,
+		.reg_clearing = EM28XX_R0C_USBSUSP,
+		.mask         = EM28XX_R0C_USBSUSP_SNAPSHOT,
+		.inverted     = 0,
+	},
+	{-1, 0, 0, 0, 0},
+};
+
+static struct em28xx_button speedlink_vad_laplace_buttons[] = {
+	{
+		.role     = EM28XX_BUTTON_SNAPSHOT,
+		.reg_r    = EM2874_R85_GPIO_P1_STATE,
+		.mask     = 0x80,
+		.inverted = 1,
+	},
+	{
+		.role     = EM28XX_BUTTON_ILLUMINATION,
+		.reg_r    = EM2874_R84_GPIO_P0_STATE,
+		.mask     = 0x08,
+		.inverted = 1,
+	},
+	{-1, 0, 0, 0, 0},
+};
+
+/*
+ *  LED definitions
+ */
+static struct em28xx_led speedlink_vad_laplace_leds[] = {
+	{
+		.role      = EM28XX_LED_ANALOG_CAPTURING,
+		.gpio_reg  = EM2874_R80_GPIO_P0_CTRL,
+		.gpio_mask = 0x01,
+		.inverted  = 1,
+	},
+	{
+		.role      = EM28XX_LED_ILLUMINATION,
+		.gpio_reg  = EM2874_R80_GPIO_P0_CTRL,
+		.gpio_mask = 0x40,
+		.inverted  = 1,
+	},
+	{-1, 0, 0, 0},
+};
+
 /*
  *  Board definitions
  */
@@ -1391,7 +1476,7 @@ struct em28xx_board em28xx_boards[] = {
 	},
 	[EM2820_BOARD_PROLINK_PLAYTV_USB2] = {
 		.name         = "SIIG AVTuner-PVR / Pixelview Prolink PlayTV USB 2.0",
-		.has_snapshot_button = 1,
+		.buttons = std_snapshot_button,
 		.tda9887_conf = TDA9887_PRESENT,
 		.tuner_type   = TUNER_YMEC_TVF_5533MF,
 		.decoder      = EM28XX_SAA711X,
@@ -1413,7 +1498,7 @@ struct em28xx_board em28xx_boards[] = {
 	},
 	[EM2860_BOARD_SAA711X_REFERENCE_DESIGN] = {
 		.name                = "EM2860/SAA711X Reference Design",
-		.has_snapshot_button = 1,
+		.buttons = std_snapshot_button,
 		.tuner_type          = TUNER_ABSENT,
 		.decoder             = EM28XX_SAA711X,
 		.input               = { {
@@ -2020,7 +2105,7 @@ struct em28xx_board em28xx_boards[] = {
 	},
 	/* 1b80:e1cc Delock 61959
 	 * Empia EM2874B + Micronas DRX 3913KA2 + NXP TDA18271HDC2
-         * mostly the same as MaxMedia UB-425-TC but different remote */
+	 * mostly the same as MaxMedia UB-425-TC but different remote */
 	[EM2874_BOARD_DELOCK_61959] = {
 		.name          = "Delock 61959",
 		.tuner_type    = TUNER_ABSENT,
@@ -2043,7 +2128,38 @@ struct em28xx_board em28xx_boards[] = {
 		.tuner_gpio	= default_tuner_gpio,
 		.def_i2c_bus	= 1,
 	},
+	/* 1ae7:9003/9004 SpeedLink Vicious And Devine Laplace webcam
+	 * Empia EM2765 + OmniVision OV2640 */
+	[EM2765_BOARD_SPEEDLINK_VAD_LAPLACE] = {
+		.name         = "SpeedLink Vicious And Devine Laplace webcam",
+		.xclk         = EM28XX_XCLK_FREQUENCY_24MHZ,
+		.i2c_speed    = EM28XX_I2C_CLK_WAIT_ENABLE |
+				EM28XX_I2C_FREQ_100_KHZ,
+		.def_i2c_bus  = 1,
+		.tuner_type   = TUNER_ABSENT,
+		.is_webcam    = 1,
+		.input        = { {
+			.type     = EM28XX_VMUX_COMPOSITE1,
+			.amux     = EM28XX_AMUX_VIDEO,
+			.gpio     = speedlink_vad_laplace_reg_seq,
+		} },
+		.buttons = speedlink_vad_laplace_buttons,
+		.leds = speedlink_vad_laplace_leds,
+	},
+	/* 2013:0258 PCTV DVB-S2 Stick (461e)
+	 * Empia EM28178, Montage M88DS3103, Montage M88TS2022, Allegro A8293 */
+	[EM28178_BOARD_PCTV_461E] = {
+		.def_i2c_bus   = 1,
+		.i2c_speed     = EM28XX_I2C_CLK_WAIT_ENABLE | EM28XX_I2C_FREQ_400_KHZ,
+		.name          = "PCTV DVB-S2 Stick (461e)",
+		.tuner_type    = TUNER_ABSENT,
+		.tuner_gpio    = pctv_461e,
+		.has_dvb       = 1,
+		.ir_codes      = RC_MAP_PINNACLE_PCTV_HD,
+	},
 };
+EXPORT_SYMBOL_GPL(em28xx_boards);
+
 const unsigned int em28xx_bcount = ARRAY_SIZE(em28xx_boards);
 
 /* table of devices that work with this driver */
@@ -2208,6 +2324,12 @@ struct usb_device_id em28xx_id_table[] = {
 			.driver_info = EM2884_BOARD_PCTV_520E },
 	{ USB_DEVICE(0x1b80, 0xe1cc),
 			.driver_info = EM2874_BOARD_DELOCK_61959 },
+	{ USB_DEVICE(0x1ae7, 0x9003),
+			.driver_info = EM2765_BOARD_SPEEDLINK_VAD_LAPLACE },
+	{ USB_DEVICE(0x1ae7, 0x9004),
+			.driver_info = EM2765_BOARD_SPEEDLINK_VAD_LAPLACE },
+	{ USB_DEVICE(0x2013, 0x0258),
+			.driver_info = EM28178_BOARD_PCTV_461E },
 	{ },
 };
 MODULE_DEVICE_TABLE(usb, em28xx_id_table);
@@ -2239,24 +2361,6 @@ static struct em28xx_hash_table em28xx_i2c_hash[] = {
 };
 /* NOTE: introduce a separate hash table for devices with 16 bit eeproms */
 
-/* I2C possible address to saa7115, tvp5150, msp3400, tvaudio */
-static unsigned short saa711x_addrs[] = {
-	0x4a >> 1, 0x48 >> 1,   /* SAA7111, SAA7111A and SAA7113 */
-	0x42 >> 1, 0x40 >> 1,   /* SAA7114, SAA7115 and SAA7118 */
-	I2C_CLIENT_END };
-
-static unsigned short tvp5150_addrs[] = {
-	0xb8 >> 1,
-	0xba >> 1,
-	I2C_CLIENT_END
-};
-
-static unsigned short msp3400_addrs[] = {
-	0x80 >> 1,
-	0x88 >> 1,
-	I2C_CLIENT_END
-};
-
 int em28xx_tuner_callback(void *ptr, int component, int command, int arg)
 {
 	struct em28xx_i2c_bus *i2c_bus = ptr;
@@ -2408,113 +2512,6 @@ static void em28xx_pre_card_setup(struct em28xx *dev)
 	em28xx_set_mode(dev, EM28XX_SUSPEND);
 }
 
-static void em28xx_setup_xc3028(struct em28xx *dev, struct xc2028_ctrl *ctl)
-{
-	memset(ctl, 0, sizeof(*ctl));
-
-	ctl->fname   = XC2028_DEFAULT_FIRMWARE;
-	ctl->max_len = 64;
-	ctl->mts = em28xx_boards[dev->model].mts_firmware;
-
-	switch (dev->model) {
-	case EM2880_BOARD_EMPIRE_DUAL_TV:
-	case EM2880_BOARD_HAUPPAUGE_WINTV_HVR_900:
-	case EM2882_BOARD_TERRATEC_HYBRID_XS:
-		ctl->demod = XC3028_FE_ZARLINK456;
-		break;
-	case EM2880_BOARD_TERRATEC_HYBRID_XS:
-	case EM2880_BOARD_TERRATEC_HYBRID_XS_FR:
-	case EM2881_BOARD_PINNACLE_HYBRID_PRO:
-		ctl->demod = XC3028_FE_ZARLINK456;
-		break;
-	case EM2880_BOARD_HAUPPAUGE_WINTV_HVR_900_R2:
-	case EM2882_BOARD_PINNACLE_HYBRID_PRO_330E:
-		ctl->demod = XC3028_FE_DEFAULT;
-		break;
-	case EM2880_BOARD_AMD_ATI_TV_WONDER_HD_600:
-		ctl->demod = XC3028_FE_DEFAULT;
-		ctl->fname = XC3028L_DEFAULT_FIRMWARE;
-		break;
-	case EM2883_BOARD_HAUPPAUGE_WINTV_HVR_850:
-	case EM2883_BOARD_HAUPPAUGE_WINTV_HVR_950:
-	case EM2880_BOARD_PINNACLE_PCTV_HD_PRO:
-		/* FIXME: Better to specify the needed IF */
-		ctl->demod = XC3028_FE_DEFAULT;
-		break;
-	case EM2883_BOARD_KWORLD_HYBRID_330U:
-	case EM2882_BOARD_DIKOM_DK300:
-	case EM2882_BOARD_KWORLD_VS_DVBT:
-		ctl->demod = XC3028_FE_CHINA;
-		ctl->fname = XC2028_DEFAULT_FIRMWARE;
-		break;
-	case EM2882_BOARD_EVGA_INDTUBE:
-		ctl->demod = XC3028_FE_CHINA;
-		ctl->fname = XC3028L_DEFAULT_FIRMWARE;
-		break;
-	default:
-		ctl->demod = XC3028_FE_OREN538;
-	}
-}
-
-static void em28xx_tuner_setup(struct em28xx *dev)
-{
-	struct tuner_setup           tun_setup;
-	struct v4l2_frequency        f;
-
-	if (dev->tuner_type == TUNER_ABSENT)
-		return;
-
-	memset(&tun_setup, 0, sizeof(tun_setup));
-
-	tun_setup.mode_mask = T_ANALOG_TV | T_RADIO;
-	tun_setup.tuner_callback = em28xx_tuner_callback;
-
-	if (dev->board.radio.type) {
-		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);
-	}
-
-	if ((dev->tuner_type != TUNER_ABSENT) && (dev->tuner_type)) {
-		tun_setup.type   = dev->tuner_type;
-		tun_setup.addr   = dev->tuner_addr;
-
-		v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_type_addr, &tun_setup);
-	}
-
-	if (dev->tda9887_conf) {
-		struct v4l2_priv_tun_config tda9887_cfg;
-
-		tda9887_cfg.tuner = TUNER_TDA9887;
-		tda9887_cfg.priv = &dev->tda9887_conf;
-
-		v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_config, &tda9887_cfg);
-	}
-
-	if (dev->tuner_type == TUNER_XC2028) {
-		struct v4l2_priv_tun_config  xc2028_cfg;
-		struct xc2028_ctrl           ctl;
-
-		memset(&xc2028_cfg, 0, sizeof(xc2028_cfg));
-		memset(&ctl, 0, sizeof(ctl));
-
-		em28xx_setup_xc3028(dev, &ctl);
-
-		xc2028_cfg.tuner = TUNER_XC2028;
-		xc2028_cfg.priv  = &ctl;
-
-		v4l2_device_call_all(&dev->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);
-}
-
 static int em28xx_hint_board(struct em28xx *dev)
 {
 	int i;
@@ -2768,57 +2765,56 @@ static void em28xx_card_setup(struct em28xx *dev)
 	/* Allow override tuner type by a module parameter */
 	if (tuner >= 0)
 		dev->tuner_type = tuner;
+}
 
-	/* 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);
-
-	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);
-
-	if (dev->board.decoder == EM28XX_TVP5150)
-		v4l2_i2c_new_subdev(&dev->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);
-
-	if (dev->board.tuner_type != TUNER_ABSENT) {
-		int has_demod = (dev->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);
-
-		if (has_demod)
-			v4l2_i2c_new_subdev(&dev->v4l2_dev,
-				&dev->i2c_adap[dev->def_i2c_bus], "tuner",
-				0, v4l2_i2c_tuner_addrs(ADDRS_DEMOD));
-		if (dev->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,
-				&dev->i2c_adap[dev->def_i2c_bus], "tuner",
-				0, v4l2_i2c_tuner_addrs(type));
-
-			if (sd)
-				dev->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);
-		}
-	}
+void em28xx_setup_xc3028(struct em28xx *dev, struct xc2028_ctrl *ctl)
+{
+	memset(ctl, 0, sizeof(*ctl));
 
-	em28xx_tuner_setup(dev);
+	ctl->fname   = XC2028_DEFAULT_FIRMWARE;
+	ctl->max_len = 64;
+	ctl->mts = em28xx_boards[dev->model].mts_firmware;
 
-	em28xx_init_camera(dev);
+	switch (dev->model) {
+	case EM2880_BOARD_EMPIRE_DUAL_TV:
+	case EM2880_BOARD_HAUPPAUGE_WINTV_HVR_900:
+	case EM2882_BOARD_TERRATEC_HYBRID_XS:
+		ctl->demod = XC3028_FE_ZARLINK456;
+		break;
+	case EM2880_BOARD_TERRATEC_HYBRID_XS:
+	case EM2880_BOARD_TERRATEC_HYBRID_XS_FR:
+	case EM2881_BOARD_PINNACLE_HYBRID_PRO:
+		ctl->demod = XC3028_FE_ZARLINK456;
+		break;
+	case EM2880_BOARD_HAUPPAUGE_WINTV_HVR_900_R2:
+	case EM2882_BOARD_PINNACLE_HYBRID_PRO_330E:
+		ctl->demod = XC3028_FE_DEFAULT;
+		break;
+	case EM2880_BOARD_AMD_ATI_TV_WONDER_HD_600:
+		ctl->demod = XC3028_FE_DEFAULT;
+		ctl->fname = XC3028L_DEFAULT_FIRMWARE;
+		break;
+	case EM2883_BOARD_HAUPPAUGE_WINTV_HVR_850:
+	case EM2883_BOARD_HAUPPAUGE_WINTV_HVR_950:
+	case EM2880_BOARD_PINNACLE_PCTV_HD_PRO:
+		/* FIXME: Better to specify the needed IF */
+		ctl->demod = XC3028_FE_DEFAULT;
+		break;
+	case EM2883_BOARD_KWORLD_HYBRID_330U:
+	case EM2882_BOARD_DIKOM_DK300:
+	case EM2882_BOARD_KWORLD_VS_DVBT:
+		ctl->demod = XC3028_FE_CHINA;
+		ctl->fname = XC2028_DEFAULT_FIRMWARE;
+		break;
+	case EM2882_BOARD_EVGA_INDTUBE:
+		ctl->demod = XC3028_FE_CHINA;
+		ctl->fname = XC3028L_DEFAULT_FIRMWARE;
+		break;
+	default:
+		ctl->demod = XC3028_FE_OREN538;
+	}
 }
-
+EXPORT_SYMBOL_GPL(em28xx_setup_xc3028);
 
 static void request_module_async(struct work_struct *work)
 {
@@ -2831,17 +2827,30 @@ static void request_module_async(struct work_struct *work)
 	 * can be initialised right now. Otherwise, the module init
 	 * code will do it.
 	 */
+
+	/*
+	 * Devicdes with an audio-only interface also have a V4L/DVB/RC
+	 * interface. Don't register extensions twice on those devices.
+	 */
+	if (dev->is_audio_only) {
+#if defined(CONFIG_MODULES) && defined(MODULE)
+		request_module("em28xx-alsa");
+#endif
+		return;
+	}
+
 	em28xx_init_extension(dev);
 
 #if defined(CONFIG_MODULES) && defined(MODULE)
+	if (dev->has_video)
+		request_module("em28xx-v4l");
 	if (dev->has_audio_class)
 		request_module("snd-usb-audio");
 	else if (dev->has_alsa_audio)
 		request_module("em28xx-alsa");
-
 	if (dev->board.has_dvb)
 		request_module("em28xx-dvb");
-	if (dev->board.has_snapshot_button ||
+	if (dev->board.buttons ||
 	    ((dev->board.ir_codes || dev->board.has_ir_i2c) && !disable_ir))
 		request_module("em28xx-rc");
 #endif /* CONFIG_MODULES */
@@ -2867,23 +2876,20 @@ void em28xx_release_resources(struct em28xx *dev)
 {
 	/*FIXME: I2C IR should be disconnected */
 
-	em28xx_release_analog_resources(dev);
+	mutex_lock(&dev->lock);
 
 	if (dev->def_i2c_bus)
 		em28xx_i2c_unregister(dev, 1);
 	em28xx_i2c_unregister(dev, 0);
-	if (dev->clk)
-		v4l2_clk_unregister_fixed(dev->clk);
-
-	v4l2_ctrl_handler_free(&dev->ctrl_handler);
-
-	v4l2_device_unregister(&dev->v4l2_dev);
 
 	usb_put_dev(dev->udev);
 
 	/* Mark device as unused */
-	clear_bit(dev->devno, &em28xx_devused);
+	clear_bit(dev->devno, em28xx_devused);
+
+	mutex_unlock(&dev->lock);
 };
+EXPORT_SYMBOL_GPL(em28xx_release_resources);
 
 /*
  * em28xx_init_dev()
@@ -2893,7 +2899,6 @@ static int em28xx_init_dev(struct em28xx *dev, struct usb_device *udev,
 			   struct usb_interface *interface,
 			   int minor)
 {
-	struct v4l2_ctrl_handler *hdl = &dev->ctrl_handler;
 	int retval;
 	static const char *default_chip_name = "em28xx";
 	const char *chip_name = default_chip_name;
@@ -2968,6 +2973,11 @@ static int em28xx_init_dev(struct em28xx *dev, struct usb_device *udev,
 			dev->wait_after_write = 0;
 			dev->eeprom_addrwidth_16bit = 1;
 			break;
+		case CHIP_ID_EM28178:
+			chip_name = "em28178";
+			dev->wait_after_write = 0;
+			dev->eeprom_addrwidth_16bit = 1;
+			break;
 		case CHIP_ID_EM2883:
 			chip_name = "em2882/3";
 			dev->wait_after_write = 0;
@@ -2983,6 +2993,16 @@ static int em28xx_init_dev(struct em28xx *dev, struct usb_device *udev,
 		}
 	}
 
+	if (dev->chip_id == CHIP_ID_EM2870 ||
+	    dev->chip_id == CHIP_ID_EM2874 ||
+	    dev->chip_id == CHIP_ID_EM28174 ||
+	    dev->chip_id == CHIP_ID_EM28178) {
+		/* Digital only device - don't load any alsa module */
+		dev->audio_mode.has_audio = false;
+		dev->has_audio_class = false;
+		dev->has_alsa_audio = false;
+	}
+
 	if (chip_name != default_chip_name)
 		printk(KERN_INFO DRIVER_NAME
 		       ": chip ID is %s\n", chip_name);
@@ -3015,15 +3035,6 @@ static int em28xx_init_dev(struct em28xx *dev, struct usb_device *udev,
 		}
 	}
 
-	retval = v4l2_device_register(&interface->dev, &dev->v4l2_dev);
-	if (retval < 0) {
-		em28xx_errdev("Call to v4l2_device_register() failed!\n");
-		return retval;
-	}
-
-	v4l2_ctrl_handler_init(hdl, 8);
-	dev->v4l2_dev.ctrl_handler = hdl;
-
 	rt_mutex_init(&dev->i2c_bus_lock);
 
 	/* register i2c bus 0 */
@@ -3034,7 +3045,7 @@ static int em28xx_init_dev(struct em28xx *dev, struct usb_device *udev,
 	if (retval < 0) {
 		em28xx_errdev("%s: em28xx_i2c_register bus 0 - error [%d]!\n",
 			__func__, retval);
-		goto unregister_dev;
+		return retval;
 	}
 
 	/* register i2c bus 1 */
@@ -3048,88 +3059,17 @@ static int em28xx_init_dev(struct em28xx *dev, struct usb_device *udev,
 		if (retval < 0) {
 			em28xx_errdev("%s: em28xx_i2c_register bus 1 - error [%d]!\n",
 				__func__, retval);
-			goto unregister_dev;
-		}
-	}
-
-	/*
-	 * Default format, used for tvp5150 or saa711x output formats
-	 */
-	dev->vinmode = 0x10;
-	dev->vinctl  = EM28XX_VINCTRL_INTERLACED |
-		       EM28XX_VINCTRL_CCIR656_ENABLE;
 
-	/* Do board specific init and eeprom reading */
-	em28xx_card_setup(dev);
+			em28xx_i2c_unregister(dev, 0);
 
-	/* Configure audio */
-	retval = em28xx_audio_setup(dev);
-	if (retval < 0) {
-		em28xx_errdev("%s: Error while setting audio - error [%d]!\n",
-			__func__, retval);
-		goto fail;
-	}
-	if (dev->audio_mode.ac97 != EM28XX_NO_AC97) {
-		v4l2_ctrl_new_std(hdl, &em28xx_ctrl_ops,
-			V4L2_CID_AUDIO_MUTE, 0, 1, 1, 1);
-		v4l2_ctrl_new_std(hdl, &em28xx_ctrl_ops,
-			V4L2_CID_AUDIO_VOLUME, 0, 0x1f, 1, 0x1f);
-	} else {
-		/* install the em28xx notify callback */
-		v4l2_ctrl_notify(v4l2_ctrl_find(hdl, V4L2_CID_AUDIO_MUTE),
-				em28xx_ctrl_notify, dev);
-		v4l2_ctrl_notify(v4l2_ctrl_find(hdl, V4L2_CID_AUDIO_VOLUME),
-				em28xx_ctrl_notify, dev);
-	}
-
-	/* wake i2c devices */
-	em28xx_wake_i2c(dev);
-
-	/* init video dma queues */
-	INIT_LIST_HEAD(&dev->vidq.active);
-	INIT_LIST_HEAD(&dev->vbiq.active);
-
-	if (dev->board.has_msp34xx) {
-		/* Send a reset to other chips via gpio */
-		retval = em28xx_write_reg(dev, EM2820_R08_GPIO_CTRL, 0xf7);
-		if (retval < 0) {
-			em28xx_errdev("%s: em28xx_write_reg - "
-				      "msp34xx(1) failed! error [%d]\n",
-				      __func__, retval);
-			goto fail;
-		}
-		msleep(3);
-
-		retval = em28xx_write_reg(dev, EM2820_R08_GPIO_CTRL, 0xff);
-		if (retval < 0) {
-			em28xx_errdev("%s: em28xx_write_reg - "
-				      "msp34xx(2) failed! error [%d]\n",
-				      __func__, retval);
-			goto fail;
+			return retval;
 		}
-		msleep(3);
-	}
-
-	retval = em28xx_register_analog_devices(dev);
-	if (retval < 0) {
-		goto fail;
 	}
 
-	/* Save some power by putting tuner to sleep */
-	v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_power, 0);
+	/* Do board specific init and eeprom reading */
+	em28xx_card_setup(dev);
 
 	return 0;
-
-fail:
-	if (dev->def_i2c_bus)
-		em28xx_i2c_unregister(dev, 1);
-	em28xx_i2c_unregister(dev, 0);
-	v4l2_ctrl_handler_free(&dev->ctrl_handler);
-
-unregister_dev:
-	v4l2_device_unregister(&dev->v4l2_dev);
-
-	return retval;
 }
 
 /* high bandwidth multiplier, as encoded in highspeed endpoint descriptors */
@@ -3154,7 +3094,7 @@ static int em28xx_usb_probe(struct usb_interface *interface,
 
 	/* Check to see next free device and mark as used */
 	do {
-		nr = find_first_zero_bit(&em28xx_devused, EM28XX_MAXBOARDS);
+		nr = find_first_zero_bit(em28xx_devused, EM28XX_MAXBOARDS);
 		if (nr >= EM28XX_MAXBOARDS) {
 			/* No free device slots */
 			printk(DRIVER_NAME ": Supports only %i em28xx boards.\n",
@@ -3162,7 +3102,7 @@ static int em28xx_usb_probe(struct usb_interface *interface,
 			retval = -ENOMEM;
 			goto err_no_slot;
 		}
-	} while (test_and_set_bit(nr, &em28xx_devused));
+	} while (test_and_set_bit(nr, em28xx_devused));
 
 	/* Don't register audio interfaces */
 	if (interface->altsetting[0].desc.bInterfaceClass == USB_CLASS_AUDIO) {
@@ -3332,7 +3272,9 @@ static int em28xx_usb_probe(struct usb_interface *interface,
 	dev->alt   = -1;
 	dev->is_audio_only = has_audio && !(has_video || has_dvb);
 	dev->has_alsa_audio = has_audio;
-	dev->audio_ifnum = ifnum;
+	dev->audio_mode.has_audio = has_audio;
+	dev->has_video = has_video;
+	dev->ifnum = ifnum;
 
 	/* Checks if audio is provided by some interface */
 	for (i = 0; i < udev->config->desc.bNumInterfaces; i++) {
@@ -3369,15 +3311,11 @@ static int em28xx_usb_probe(struct usb_interface *interface,
 	/* save our data pointer in this interface device */
 	usb_set_intfdata(interface, dev);
 
-	/* initialize videobuf2 stuff */
-	em28xx_vb2_setup(dev);
-
 	/* allocate device struct */
 	mutex_init(&dev->lock);
-	mutex_lock(&dev->lock);
 	retval = em28xx_init_dev(dev, udev, interface, nr);
 	if (retval) {
-		goto unlock_and_free;
+		goto err_free;
 	}
 
 	if (usb_xfer_mode < 0) {
@@ -3402,26 +3340,6 @@ static int em28xx_usb_probe(struct usb_interface *interface,
 
 		em28xx_info("dvb set to %s mode.\n",
 			    dev->dvb_xfer_bulk ? "bulk" : "isoc");
-
-		/* pre-allocate DVB usb transfer buffers */
-		if (dev->dvb_xfer_bulk) {
-			retval = em28xx_alloc_urbs(dev, EM28XX_DIGITAL_MODE,
-					    dev->dvb_xfer_bulk,
-					    EM28XX_DVB_NUM_BUFS,
-					    512,
-					    EM28XX_DVB_BULK_PACKET_MULTIPLIER);
-		} else {
-			retval = em28xx_alloc_urbs(dev, EM28XX_DIGITAL_MODE,
-					    dev->dvb_xfer_bulk,
-					    EM28XX_DVB_NUM_BUFS,
-					    dev->dvb_max_pkt_size_isoc,
-					    EM28XX_DVB_NUM_ISOC_PACKETS);
-		}
-		if (retval) {
-			printk(DRIVER_NAME
-			       ": Failed to pre-allocate USB transfer buffers for DVB.\n");
-			goto unlock_and_free;
-		}
 	}
 
 	request_modules(dev);
@@ -3429,19 +3347,15 @@ static int em28xx_usb_probe(struct usb_interface *interface,
 	/* Should be the last thing to do, to avoid newer udev's to
 	   open the device before fully initializing it
 	 */
-	mutex_unlock(&dev->lock);
 
 	return 0;
 
-unlock_and_free:
-	mutex_unlock(&dev->lock);
-
 err_free:
 	kfree(dev->alt_max_pkt_size_isoc);
 	kfree(dev);
 
 err:
-	clear_bit(nr, &em28xx_devused);
+	clear_bit(nr, em28xx_devused);
 
 err_no_slot:
 	usb_put_dev(udev);
@@ -3465,36 +3379,13 @@ static void em28xx_usb_disconnect(struct usb_interface *interface)
 
 	dev->disconnected = 1;
 
-	if (dev->is_audio_only) {
-		mutex_lock(&dev->lock);
-		em28xx_close_extension(dev);
-		mutex_unlock(&dev->lock);
-		return;
-	}
-
-	em28xx_info("disconnecting %s\n", dev->vdev->name);
+	em28xx_info("Disconnecting %s\n", dev->name);
 
 	flush_request_modules(dev);
 
-	mutex_lock(&dev->lock);
-
-	v4l2_device_disconnect(&dev->v4l2_dev);
-
-	if (dev->users) {
-		em28xx_warn("device %s is open! Deregistration and memory deallocation are deferred on close.\n",
-			    video_device_node_name(dev->vdev));
-
-		em28xx_uninit_usb_xfer(dev, EM28XX_ANALOG_MODE);
-		em28xx_uninit_usb_xfer(dev, EM28XX_DIGITAL_MODE);
-	}
-
 	em28xx_close_extension(dev);
-	/* NOTE: must be called BEFORE the resources are released */
-
-	if (!dev->users)
-		em28xx_release_resources(dev);
 
-	mutex_unlock(&dev->lock);
+	em28xx_release_resources(dev);
 
 	if (!dev->users) {
 		kfree(dev->alt_max_pkt_size_isoc);
diff --git a/drivers/media/usb/em28xx/em28xx-core.c b/drivers/media/usb/em28xx/em28xx-core.c
index fc157af5234a..898fb9bd88a2 100644
--- a/drivers/media/usb/em28xx/em28xx-core.c
+++ b/drivers/media/usb/em28xx/em28xx-core.c
@@ -23,6 +23,7 @@
  */
 
 #include <linux/init.h>
+#include <linux/jiffies.h>
 #include <linux/list.h>
 #include <linux/module.h>
 #include <linux/slab.h>
@@ -33,6 +34,16 @@
 
 #include "em28xx.h"
 
+#define DRIVER_AUTHOR "Ludovico Cavedon <cavedon@sssup.it>, " \
+		      "Markus Rechberger <mrechberger@gmail.com>, " \
+		      "Mauro Carvalho Chehab <mchehab@infradead.org>, " \
+		      "Sascha Sommer <saschasommer@freenet.de>"
+
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL");
+MODULE_VERSION(EM28XX_VERSION);
+
 /* #define ENABLE_DEBUG_ISOC_FRAMES */
 
 static unsigned int core_debug;
@@ -53,14 +64,6 @@ MODULE_PARM_DESC(reg_debug, "enable debug messages [URB reg]");
 		printk(KERN_INFO "%s %s :"fmt, \
 			 dev->name, __func__ , ##arg); } while (0)
 
-static int alt;
-module_param(alt, int, 0644);
-MODULE_PARM_DESC(alt, "alternate setting to use for video endpoint");
-
-static unsigned int disable_vbi;
-module_param(disable_vbi, int, 0644);
-MODULE_PARM_DESC(disable_vbi, "disable vbi support");
-
 /* FIXME */
 #define em28xx_isocdbg(fmt, arg...) do {\
 	if (core_debug) \
@@ -226,21 +229,42 @@ int em28xx_write_reg_bits(struct em28xx *dev, u16 reg, u8 val,
 EXPORT_SYMBOL_GPL(em28xx_write_reg_bits);
 
 /*
+ * em28xx_toggle_reg_bits()
+ * toggles/inverts the bits (specified by bitmask) of a register
+ */
+int em28xx_toggle_reg_bits(struct em28xx *dev, u16 reg, u8 bitmask)
+{
+	int oldval;
+	u8 newval;
+
+	oldval = em28xx_read_reg(dev, reg);
+	if (oldval < 0)
+		return oldval;
+
+	newval = (~oldval & bitmask) | (oldval & ~bitmask);
+
+	return em28xx_write_reg(dev, reg, newval);
+}
+EXPORT_SYMBOL_GPL(em28xx_toggle_reg_bits);
+
+/*
  * em28xx_is_ac97_ready()
  * Checks if ac97 is ready
  */
 static int em28xx_is_ac97_ready(struct em28xx *dev)
 {
-	int ret, i;
+	unsigned long timeout = jiffies + msecs_to_jiffies(EM28XX_AC97_XFER_TIMEOUT);
+	int ret;
 
 	/* Wait up to 50 ms for AC97 command to complete */
-	for (i = 0; i < 10; i++, msleep(5)) {
+	while (time_is_after_jiffies(timeout)) {
 		ret = em28xx_read_reg(dev, EM28XX_R43_AC97BUSY);
 		if (ret < 0)
 			return ret;
 
 		if (!(ret & 0x01))
 			return 0;
+		msleep(5);
 	}
 
 	em28xx_warn("AC97 command still being executed: not handled properly!\n");
@@ -482,16 +506,8 @@ int em28xx_audio_setup(struct em28xx *dev)
 	int vid1, vid2, feat, cfg;
 	u32 vid;
 
-	if (dev->chip_id == CHIP_ID_EM2870 || dev->chip_id == CHIP_ID_EM2874
-		|| dev->chip_id == CHIP_ID_EM28174) {
-		/* Digital only device - don't load any alsa module */
-		dev->audio_mode.has_audio = false;
-		dev->has_audio_class = false;
-		dev->has_alsa_audio = false;
+	if (!dev->audio_mode.has_audio)
 		return 0;
-	}
-
-	dev->audio_mode.has_audio = true;
 
 	/* See how this device is configured */
 	cfg = em28xx_read_reg(dev, EM28XX_R00_CHIPCFG);
@@ -504,17 +520,19 @@ int em28xx_audio_setup(struct em28xx *dev)
 		dev->has_alsa_audio = false;
 		dev->audio_mode.has_audio = false;
 		return 0;
-	} else if ((cfg & EM28XX_CHIPCFG_AUDIOMASK) ==
-		   EM28XX_CHIPCFG_I2S_3_SAMPRATES) {
-		em28xx_info("I2S Audio (3 sample rates)\n");
-		dev->audio_mode.i2s_3rates = 1;
-	} else if ((cfg & EM28XX_CHIPCFG_AUDIOMASK) ==
-		   EM28XX_CHIPCFG_I2S_5_SAMPRATES) {
-		em28xx_info("I2S Audio (5 sample rates)\n");
-		dev->audio_mode.i2s_5rates = 1;
-	}
-
-	if ((cfg & EM28XX_CHIPCFG_AUDIOMASK) != EM28XX_CHIPCFG_AC97) {
+	} else if ((cfg & EM28XX_CHIPCFG_AUDIOMASK) != EM28XX_CHIPCFG_AC97) {
+		if (dev->chip_id < CHIP_ID_EM2860 &&
+	            (cfg & EM28XX_CHIPCFG_AUDIOMASK) ==
+		    EM2820_CHIPCFG_I2S_1_SAMPRATE)
+			dev->audio_mode.i2s_samplerates = 1;
+		else if (dev->chip_id >= CHIP_ID_EM2860 &&
+			 (cfg & EM28XX_CHIPCFG_AUDIOMASK) ==
+			 EM2860_CHIPCFG_I2S_5_SAMPRATES)
+			dev->audio_mode.i2s_samplerates = 5;
+		else
+			dev->audio_mode.i2s_samplerates = 3;
+		em28xx_info("I2S Audio (%d sample rate(s))\n",
+					       dev->audio_mode.i2s_samplerates);
 		/* Skip the code that does AC97 vendor detection */
 		dev->audio_mode.ac97 = EM28XX_NO_AC97;
 		goto init_audio;
@@ -582,23 +600,21 @@ init_audio:
 }
 EXPORT_SYMBOL_GPL(em28xx_audio_setup);
 
-int em28xx_colorlevels_set_default(struct em28xx *dev)
+const struct em28xx_led *em28xx_find_led(struct em28xx *dev,
+					 enum em28xx_led_role role)
 {
-	em28xx_write_reg(dev, EM28XX_R20_YGAIN, CONTRAST_DEFAULT);
-	em28xx_write_reg(dev, EM28XX_R21_YOFFSET, BRIGHTNESS_DEFAULT);
-	em28xx_write_reg(dev, EM28XX_R22_UVGAIN, SATURATION_DEFAULT);
-	em28xx_write_reg(dev, EM28XX_R23_UOFFSET, BLUE_BALANCE_DEFAULT);
-	em28xx_write_reg(dev, EM28XX_R24_VOFFSET, RED_BALANCE_DEFAULT);
-	em28xx_write_reg(dev, EM28XX_R25_SHARPNESS, SHARPNESS_DEFAULT);
-
-	em28xx_write_reg(dev, EM28XX_R14_GAMMA, 0x20);
-	em28xx_write_reg(dev, EM28XX_R15_RGAIN, 0x20);
-	em28xx_write_reg(dev, EM28XX_R16_GGAIN, 0x20);
-	em28xx_write_reg(dev, EM28XX_R17_BGAIN, 0x20);
-	em28xx_write_reg(dev, EM28XX_R18_ROFFSET, 0x00);
-	em28xx_write_reg(dev, EM28XX_R19_GOFFSET, 0x00);
-	return em28xx_write_reg(dev, EM28XX_R1A_BOFFSET, 0x00);
+	if (dev->board.leds) {
+		u8 k = 0;
+		while (dev->board.leds[k].role >= 0 &&
+			       dev->board.leds[k].role < EM28XX_NUM_LED_ROLES) {
+			if (dev->board.leds[k].role == role)
+				return &dev->board.leds[k];
+			k++;
+		}
+	}
+	return NULL;
 }
+EXPORT_SYMBOL_GPL(em28xx_find_led);
 
 int em28xx_capture_start(struct em28xx *dev, int start)
 {
@@ -606,271 +622,57 @@ int em28xx_capture_start(struct em28xx *dev, int start)
 
 	if (dev->chip_id == CHIP_ID_EM2874 ||
 	    dev->chip_id == CHIP_ID_EM2884 ||
-	    dev->chip_id == CHIP_ID_EM28174) {
+	    dev->chip_id == CHIP_ID_EM28174 ||
+	    dev->chip_id == CHIP_ID_EM28178) {
 		/* The Transport Stream Enable Register moved in em2874 */
-		if (!start) {
-			rc = em28xx_write_reg_bits(dev, EM2874_R5F_TS_ENABLE,
-						   0x00,
-						   EM2874_TS1_CAPTURE_ENABLE);
-			return rc;
-		}
-
-		/* Enable Transport Stream */
 		rc = em28xx_write_reg_bits(dev, EM2874_R5F_TS_ENABLE,
-					   EM2874_TS1_CAPTURE_ENABLE,
+					   start ?
+					       EM2874_TS1_CAPTURE_ENABLE : 0x00,
 					   EM2874_TS1_CAPTURE_ENABLE);
-		return rc;
-	}
-
-
-	/* FIXME: which is the best order? */
-	/* video registers are sampled by VREF */
-	rc = em28xx_write_reg_bits(dev, EM28XX_R0C_USBSUSP,
-				   start ? 0x10 : 0x00, 0x10);
-	if (rc < 0)
-		return rc;
-
-	if (!start) {
-		/* disable video capture */
-		rc = em28xx_write_reg(dev, EM28XX_R12_VINENABLE, 0x27);
-		return rc;
-	}
-
-	if (dev->board.is_webcam)
-		rc = em28xx_write_reg(dev, 0x13, 0x0c);
-
-	/* enable video capture */
-	rc = em28xx_write_reg(dev, 0x48, 0x00);
-
-	if (dev->mode == EM28XX_ANALOG_MODE)
-		rc = em28xx_write_reg(dev, EM28XX_R12_VINENABLE, 0x67);
-	else
-		rc = em28xx_write_reg(dev, EM28XX_R12_VINENABLE, 0x37);
-
-	msleep(6);
-
-	return rc;
-}
-
-int em28xx_vbi_supported(struct em28xx *dev)
-{
-	/* Modprobe option to manually disable */
-	if (disable_vbi == 1)
-		return 0;
-
-	if (dev->board.is_webcam)
-		return 0;
-
-	/* FIXME: check subdevices for VBI support */
-
-	if (dev->chip_id == CHIP_ID_EM2860 ||
-	    dev->chip_id == CHIP_ID_EM2883)
-		return 1;
-
-	/* Version of em28xx that does not support VBI */
-	return 0;
-}
-
-int em28xx_set_outfmt(struct em28xx *dev)
-{
-	int ret;
-	u8 fmt, vinctrl;
-
-	fmt = dev->format->reg;
-	if (!dev->is_em25xx)
-		fmt |= 0x20;
-	/*
-	 * NOTE: it's not clear if this is really needed !
-	 * The datasheets say bit 5 is a reserved bit and devices seem to work
-	 * fine without it. But the Windows driver sets it for em2710/50+em28xx
-	 * devices and we've always been setting it, too.
-	 *
-	 * em2765 (em25xx, em276x/7x/8x) devices do NOT work with this bit set,
-	 * it's likely used for an additional (compressed ?) format there.
-	 */
-	ret = em28xx_write_reg(dev, EM28XX_R27_OUTFMT, fmt);
-	if (ret < 0)
-		return ret;
-
-	ret = em28xx_write_reg(dev, EM28XX_R10_VINMODE, dev->vinmode);
-	if (ret < 0)
-		return ret;
-
-	vinctrl = dev->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) {
-			/* NTSC */
-			em28xx_write_reg(dev, EM28XX_R35_VBI_START_V, 0x09);
-		} else if (dev->norm & V4L2_STD_625_50) {
-			/* PAL */
-			em28xx_write_reg(dev, EM28XX_R35_VBI_START_V, 0x07);
-		}
-	}
-
-	return em28xx_write_reg(dev, EM28XX_R11_VINCTRL, vinctrl);
-}
-
-static int em28xx_accumulator_set(struct em28xx *dev, u8 xmin, u8 xmax,
-				  u8 ymin, u8 ymax)
-{
-	em28xx_coredbg("em28xx Scale: (%d,%d)-(%d,%d)\n",
-			xmin, ymin, xmax, ymax);
-
-	em28xx_write_regs(dev, EM28XX_R28_XMIN, &xmin, 1);
-	em28xx_write_regs(dev, EM28XX_R29_XMAX, &xmax, 1);
-	em28xx_write_regs(dev, EM28XX_R2A_YMIN, &ymin, 1);
-	return em28xx_write_regs(dev, EM28XX_R2B_YMAX, &ymax, 1);
-}
-
-static void em28xx_capture_area_set(struct em28xx *dev, u8 hstart, u8 vstart,
-				   u16 width, u16 height)
-{
-	u8 cwidth = width >> 2;
-	u8 cheight = height >> 2;
-	u8 overflow = (height >> 9 & 0x02) | (width >> 10 & 0x01);
-	/* NOTE: size limit: 2047x1023 = 2MPix */
-
-	em28xx_coredbg("capture area set to (%d,%d): %dx%d\n",
-		       hstart, vstart,
-		       ((overflow & 2) << 9 | cwidth << 2),
-		       ((overflow & 1) << 10 | cheight << 2));
-
-	em28xx_write_regs(dev, EM28XX_R1C_HSTART, &hstart, 1);
-	em28xx_write_regs(dev, EM28XX_R1D_VSTART, &vstart, 1);
-	em28xx_write_regs(dev, EM28XX_R1E_CWIDTH, &cwidth, 1);
-	em28xx_write_regs(dev, EM28XX_R1F_CHEIGHT, &cheight, 1);
-	em28xx_write_regs(dev, EM28XX_R1B_OFLOW, &overflow, 1);
-
-	/* FIXME: function/meaning of these registers ? */
-	/* FIXME: align width+height to multiples of 4 ?! */
-	if (dev->is_em25xx) {
-		em28xx_write_reg(dev, 0x34, width >> 4);
-		em28xx_write_reg(dev, 0x35, height >> 4);
-	}
-}
-
-static int em28xx_scaler_set(struct em28xx *dev, u16 h, u16 v)
-{
-	u8 mode;
-	/* the em2800 scaler only supports scaling down to 50% */
-
-	if (dev->board.is_em2800) {
-		mode = (v ? 0x20 : 0x00) | (h ? 0x10 : 0x00);
 	} else {
-		u8 buf[2];
-
-		buf[0] = h;
-		buf[1] = h >> 8;
-		em28xx_write_regs(dev, EM28XX_R30_HSCALELOW, (char *)buf, 2);
-
-		buf[0] = v;
-		buf[1] = v >> 8;
-		em28xx_write_regs(dev, EM28XX_R32_VSCALELOW, (char *)buf, 2);
-		/* it seems that both H and V scalers must be active
-		   to work correctly */
-		mode = (h || v) ? 0x30 : 0x00;
-	}
-	return em28xx_write_reg_bits(dev, EM28XX_R26_COMPR, mode, 0x30);
-}
-
-/* FIXME: this only function read values from dev */
-int em28xx_resolution_set(struct em28xx *dev)
-{
-	int width, height;
-	width = norm_maxw(dev);
-	height = norm_maxh(dev);
-
-	/* Properly setup VBI */
-	dev->vbi_width = 720;
-	if (dev->norm & V4L2_STD_525_60)
-		dev->vbi_height = 12;
-	else
-		dev->vbi_height = 18;
-
-	em28xx_set_outfmt(dev);
+		/* FIXME: which is the best order? */
+		/* video registers are sampled by VREF */
+		rc = em28xx_write_reg_bits(dev, EM28XX_R0C_USBSUSP,
+					   start ? 0x10 : 0x00, 0x10);
+		if (rc < 0)
+			return rc;
 
-	em28xx_accumulator_set(dev, 1, (width - 4) >> 2, 1, (height - 4) >> 2);
+		if (start) {
+			if (dev->board.is_webcam)
+				rc = em28xx_write_reg(dev, 0x13, 0x0c);
 
-	/* If we don't set the start position to 2 in VBI mode, we end up
-	   with line 20/21 being YUYV encoded instead of being in 8-bit
-	   greyscale.  The core of the issue is that line 21 (and line 23 for
-	   PAL WSS) are inside of active video region, and as a result they
-	   get the pixelformatting associated with that area.  So by cropping
-	   it out, we end up with the same format as the rest of the VBI
-	   region */
-	if (em28xx_vbi_supported(dev) == 1)
-		em28xx_capture_area_set(dev, 0, 2, width, height);
-	else
-		em28xx_capture_area_set(dev, 0, 0, width, height);
+			/* Enable video capture */
+			rc = em28xx_write_reg(dev, 0x48, 0x00);
 
-	return em28xx_scaler_set(dev, dev->hscale, dev->vscale);
-}
+			if (dev->mode == EM28XX_ANALOG_MODE)
+				rc = em28xx_write_reg(dev,
+						    EM28XX_R12_VINENABLE, 0x67);
+			else
+				rc = em28xx_write_reg(dev,
+						    EM28XX_R12_VINENABLE, 0x37);
 
-/* Set USB alternate setting for analog video */
-int em28xx_set_alternate(struct em28xx *dev)
-{
-	int errCode;
-	int i;
-	unsigned int min_pkt_size = dev->width * 2 + 4;
-
-	/* NOTE: for isoc transfers, only alt settings > 0 are allowed
-		 bulk transfers seem to work only with alt=0 ! */
-	dev->alt = 0;
-	if ((alt > 0) && (alt < dev->num_alt)) {
-		em28xx_coredbg("alternate forced to %d\n", dev->alt);
-		dev->alt = alt;
-		goto set_alt;
+			msleep(6);
+		} else {
+			/* disable video capture */
+			rc = em28xx_write_reg(dev, EM28XX_R12_VINENABLE, 0x27);
+		}
 	}
-	if (dev->analog_xfer_bulk)
-		goto set_alt;
 
-	/* When image size is bigger than a certain value,
-	   the frame size should be increased, otherwise, only
-	   green screen will be received.
-	 */
-	if (dev->width * 2 * dev->height > 720 * 240 * 2)
-		min_pkt_size *= 2;
+	if (rc < 0)
+		return rc;
 
-	for (i = 0; i < dev->num_alt; i++) {
-		/* stop when the selected alt setting offers enough bandwidth */
-		if (dev->alt_max_pkt_size_isoc[i] >= min_pkt_size) {
-			dev->alt = i;
-			break;
-		/* otherwise make sure that we end up with the maximum bandwidth
-		   because the min_pkt_size equation might be wrong...
-		*/
-		} else if (dev->alt_max_pkt_size_isoc[i] >
-			   dev->alt_max_pkt_size_isoc[dev->alt])
-			dev->alt = i;
+	/* Switch (explicitly controlled) analog capturing LED on/off */
+	if (dev->mode == EM28XX_ANALOG_MODE) {
+		const struct em28xx_led *led;
+		led = em28xx_find_led(dev, EM28XX_LED_ANALOG_CAPTURING);
+		if (led)
+			em28xx_write_reg_bits(dev, led->gpio_reg,
+					      (!start ^ led->inverted) ?
+					      ~led->gpio_mask : led->gpio_mask,
+					      led->gpio_mask);
 	}
 
-set_alt:
-	/* NOTE: for bulk transfers, we need to call usb_set_interface()
-	 * even if the previous settings were the same. Otherwise streaming
-	 * fails with all urbs having status = -EOVERFLOW ! */
-	if (dev->analog_xfer_bulk) {
-		dev->max_pkt_size = 512; /* USB 2.0 spec */
-		dev->packet_multiplier = EM28XX_BULK_PACKET_MULTIPLIER;
-	} else { /* isoc */
-		em28xx_coredbg("minimum isoc packet size: %u (alt=%d)\n",
-			       min_pkt_size, dev->alt);
-		dev->max_pkt_size =
-				  dev->alt_max_pkt_size_isoc[dev->alt];
-		dev->packet_multiplier = EM28XX_NUM_ISOC_PACKETS;
-	}
-	em28xx_coredbg("setting alternate %d with wMaxPacketSize=%u\n",
-		       dev->alt, dev->max_pkt_size);
-	errCode = usb_set_interface(dev->udev, 0, dev->alt);
-	if (errCode < 0) {
-		em28xx_errdev("cannot change alternate number to %d (error=%i)\n",
-			      dev->alt, errCode);
-		return errCode;
-	}
-	return 0;
+	return rc;
 }
 
 int em28xx_gpio_set(struct em28xx *dev, struct em28xx_reg_seq *gpio)
@@ -1238,18 +1040,6 @@ int em28xx_init_usb_xfer(struct em28xx *dev, enum em28xx_mode mode,
 EXPORT_SYMBOL_GPL(em28xx_init_usb_xfer);
 
 /*
- * em28xx_wake_i2c()
- * configure i2c attached devices
- */
-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,
-			INPUT(dev->ctl_input)->vmux, 0, 0);
-	v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_stream, 0);
-}
-
-/*
  * Device control list
  */
 
@@ -1272,7 +1062,7 @@ int em28xx_register_extension(struct em28xx_ops *ops)
 		ops->init(dev);
 	}
 	mutex_unlock(&em28xx_devlist_mutex);
-	printk(KERN_INFO "Em28xx: Initialized (%s) extension\n", ops->name);
+	printk(KERN_INFO "em28xx: Registered (%s) extension\n", ops->name);
 	return 0;
 }
 EXPORT_SYMBOL(em28xx_register_extension);
diff --git a/drivers/media/usb/em28xx/em28xx-dvb.c b/drivers/media/usb/em28xx/em28xx-dvb.c
index 344042bb845c..a0a669e81362 100644
--- a/drivers/media/usb/em28xx/em28xx-dvb.c
+++ b/drivers/media/usb/em28xx/em28xx-dvb.c
@@ -51,10 +51,14 @@
 #include "a8293.h"
 #include "qt1010.h"
 #include "mb86a20s.h"
+#include "m88ds3103.h"
+#include "m88ts2022.h"
 
-MODULE_DESCRIPTION("driver for em28xx based DVB cards");
 MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@infradead.org>");
 MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION(DRIVER_DESC " - digital TV interface");
+MODULE_VERSION(EM28XX_VERSION);
+
 
 static unsigned int debug;
 module_param(debug, int, 0644);
@@ -87,6 +91,7 @@ struct em28xx_dvb {
 	struct semaphore      pll_mutex;
 	bool			dont_attach_fe1;
 	int			lna_gpio;
+	struct i2c_client	*i2c_client_tuner;
 };
 
 
@@ -198,7 +203,7 @@ static int em28xx_start_streaming(struct em28xx_dvb *dvb)
 		dvb_alt = dev->dvb_alt_isoc;
 	}
 
-	usb_set_interface(dev->udev, 0, dvb_alt);
+	usb_set_interface(dev->udev, dev->ifnum, dvb_alt);
 	rc = em28xx_set_mode(dev, EM28XX_DIGITAL_MODE);
 	if (rc < 0)
 		return rc;
@@ -271,7 +276,7 @@ static int em28xx_stop_feed(struct dvb_demux_feed *feed)
 static int em28xx_dvb_bus_ctrl(struct dvb_frontend *fe, int acquire)
 {
 	struct em28xx_i2c_bus *i2c_bus = fe->dvb->priv;
-        struct em28xx *dev = i2c_bus->dev;
+	struct em28xx *dev = i2c_bus->dev;
 
 	if (acquire)
 		return em28xx_set_mode(dev, EM28XX_DIGITAL_MODE);
@@ -370,7 +375,6 @@ static struct drxk_config terratec_h5_drxk = {
 	.no_i2c_bridge = 1,
 	.microcode_name = "dvb-usb-terratec-h5-drxk.fw",
 	.qam_demod_parameter_count = 2,
-	.load_firmware_sync = true,
 };
 
 static struct drxk_config hauppauge_930c_drxk = {
@@ -380,7 +384,6 @@ static struct drxk_config hauppauge_930c_drxk = {
 	.microcode_name = "dvb-usb-hauppauge-hvr930c-drxk.fw",
 	.chunk_size = 56,
 	.qam_demod_parameter_count = 2,
-	.load_firmware_sync = true,
 };
 
 static struct drxk_config terratec_htc_stick_drxk = {
@@ -394,7 +397,6 @@ static struct drxk_config terratec_htc_stick_drxk = {
 	.antenna_dvbt = true,
 	/* The windows driver uses the same. This will disable LNA. */
 	.antenna_gpio = 0x6,
-	.load_firmware_sync = true,
 };
 
 static struct drxk_config maxmedia_ub425_tc_drxk = {
@@ -403,7 +405,6 @@ static struct drxk_config maxmedia_ub425_tc_drxk = {
 	.no_i2c_bridge = 1,
 	.microcode_name = "dvb-demod-drxk-01.fw",
 	.chunk_size = 62,
-	.load_firmware_sync = true,
 	.qam_demod_parameter_count = 2,
 };
 
@@ -415,7 +416,6 @@ static struct drxk_config pctv_520e_drxk = {
 	.chunk_size = 58,
 	.antenna_dvbt = true, /* disable LNA */
 	.antenna_gpio = (1 << 2), /* disable LNA */
-	.load_firmware_sync = true,
 };
 
 static int drxk_gate_ctrl(struct dvb_frontend *fe, int enable)
@@ -808,6 +808,14 @@ static struct tda18271_config c3tech_duo_tda18271_config = {
 	.small_i2c = TDA18271_03_BYTE_CHUNK_INIT,
 };
 
+static const struct m88ds3103_config pctv_461e_m88ds3103_config = {
+	.i2c_addr = 0x68,
+	.clock = 27000000,
+	.i2c_wr_max = 33,
+	.clock_out = 0,
+	.ts_mode = M88DS3103_TS_PARALLEL_16,
+	.agc = 0x99,
+};
 
 /* ------------------------------------------------------------------ */
 
@@ -815,11 +823,16 @@ static int em28xx_attach_xc3028(u8 addr, struct em28xx *dev)
 {
 	struct dvb_frontend *fe;
 	struct xc2028_config cfg;
+	struct xc2028_ctrl ctl;
 
 	memset(&cfg, 0, sizeof(cfg));
 	cfg.i2c_adap  = &dev->i2c_adap[dev->def_i2c_bus];
 	cfg.i2c_addr  = addr;
 
+	memset(&ctl, 0, sizeof(ctl));
+	em28xx_setup_xc3028(dev, &ctl);
+	cfg.ctrl  = &ctl;
+
 	if (!dev->dvb->fe[0]) {
 		em28xx_errdev("/2: dvb frontend not attached. "
 				"Can't attach xc3028\n");
@@ -979,12 +992,18 @@ static int em28xx_dvb_init(struct em28xx *dev)
 	int result = 0, mfe_shared = 0;
 	struct em28xx_dvb *dvb;
 
+	if (dev->is_audio_only) {
+		/* Shouldn't initialize IR for this interface */
+		return 0;
+	}
+
 	if (!dev->board.has_dvb) {
 		/* This device does not support the extension */
-		printk(KERN_INFO "em28xx_dvb: This device does not support the extension\n");
 		return 0;
 	}
 
+	em28xx_info("Binding DVB extension\n");
+
 	dvb = kzalloc(sizeof(struct em28xx_dvb), GFP_KERNEL);
 
 	if (dvb == NULL) {
@@ -994,6 +1013,27 @@ static int em28xx_dvb_init(struct em28xx *dev)
 	dev->dvb = dvb;
 	dvb->fe[0] = dvb->fe[1] = NULL;
 
+	/* pre-allocate DVB usb transfer buffers */
+	if (dev->dvb_xfer_bulk) {
+		result = em28xx_alloc_urbs(dev, EM28XX_DIGITAL_MODE,
+					   dev->dvb_xfer_bulk,
+					   EM28XX_DVB_NUM_BUFS,
+					   512,
+					   EM28XX_DVB_BULK_PACKET_MULTIPLIER);
+	} else {
+		result = em28xx_alloc_urbs(dev, EM28XX_DIGITAL_MODE,
+					   dev->dvb_xfer_bulk,
+					   EM28XX_DVB_NUM_BUFS,
+					   dev->dvb_max_pkt_size_isoc,
+					   EM28XX_DVB_NUM_ISOC_PACKETS);
+	}
+	if (result) {
+		em28xx_errdev("em28xx_dvb: failed to pre-allocate USB transfer buffers for DVB.\n");
+		kfree(dvb);
+		dev->dvb = NULL;
+		return result;
+	}
+
 	mutex_lock(&dev->lock);
 	em28xx_set_mode(dev, EM28XX_DIGITAL_MODE);
 	/* init frontend */
@@ -1330,6 +1370,48 @@ static int em28xx_dvb_init(struct em28xx *dev)
 			goto out_free;
 		}
 		break;
+	case EM28178_BOARD_PCTV_461E:
+		{
+			/* demod I2C adapter */
+			struct i2c_adapter *i2c_adapter;
+			struct i2c_board_info info;
+			struct m88ts2022_config m88ts2022_config = {
+				.clock = 27000000,
+			};
+			memset(&info, 0, sizeof(struct i2c_board_info));
+
+			/* attach demod */
+			dvb->fe[0] = dvb_attach(m88ds3103_attach,
+					&pctv_461e_m88ds3103_config,
+					&dev->i2c_adap[dev->def_i2c_bus],
+					&i2c_adapter);
+			if (dvb->fe[0] == NULL) {
+				result = -ENODEV;
+				goto out_free;
+			}
+
+			/* attach tuner */
+			m88ts2022_config.fe = dvb->fe[0];
+			strlcpy(info.type, "m88ts2022", I2C_NAME_SIZE);
+			info.addr = 0x60;
+			info.platform_data = &m88ts2022_config;
+			request_module("m88ts2022");
+			dvb->i2c_client_tuner = i2c_new_device(i2c_adapter, &info);
+
+			/* delegate signal strength measurement to tuner */
+			dvb->fe[0]->ops.read_signal_strength =
+					dvb->fe[0]->ops.tuner_ops.get_rf_strength;
+
+			/* attach SEC */
+			if (!dvb_attach(a8293_attach, dvb->fe[0],
+					&dev->i2c_adap[dev->def_i2c_bus],
+					&em28xx_a8293_config)) {
+				dvb_frontend_detach(dvb->fe[0]);
+				result = -ENODEV;
+				goto out_free;
+			}
+		}
+		break;
 	default:
 		em28xx_errdev("/2: The frontend of your DVB/ATSC card"
 				" isn't supported yet\n");
@@ -1354,7 +1436,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 	/* MFE lock */
 	dvb->adapter.mfe_shared = mfe_shared;
 
-	em28xx_info("Successfully loaded em28xx-dvb\n");
+	em28xx_info("DVB extension successfully initialized\n");
 ret:
 	em28xx_set_mode(dev, EM28XX_SUSPEND);
 	mutex_unlock(&dev->lock);
@@ -1375,14 +1457,23 @@ static inline void prevent_sleep(struct dvb_frontend_ops *ops)
 
 static int em28xx_dvb_fini(struct em28xx *dev)
 {
+	if (dev->is_audio_only) {
+		/* Shouldn't initialize IR for this interface */
+		return 0;
+	}
+
 	if (!dev->board.has_dvb) {
 		/* This device does not support the extension */
 		return 0;
 	}
 
+	em28xx_info("Closing DVB extension");
+
 	if (dev->dvb) {
 		struct em28xx_dvb *dvb = dev->dvb;
 
+		em28xx_uninit_usb_xfer(dev, EM28XX_DIGITAL_MODE);
+
 		if (dev->disconnected) {
 			/* We cannot tell the device to sleep
 			 * once it has been unplugged. */
@@ -1392,6 +1483,7 @@ static int em28xx_dvb_fini(struct em28xx *dev)
 				prevent_sleep(&dvb->fe[1]->ops);
 		}
 
+		i2c_release_client(dvb->i2c_client_tuner);
 		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 c4ff9739a7ae..7e1724076ac4 100644
--- a/drivers/media/usb/em28xx/em28xx-i2c.c
+++ b/drivers/media/usb/em28xx/em28xx-i2c.c
@@ -26,6 +26,7 @@
 #include <linux/kernel.h>
 #include <linux/usb.h>
 #include <linux/i2c.h>
+#include <linux/jiffies.h>
 
 #include "em28xx.h"
 #include "tuner-xc2028.h"
@@ -40,7 +41,7 @@ MODULE_PARM_DESC(i2c_scan, "scan i2c bus at insmod time");
 
 static unsigned int i2c_debug;
 module_param(i2c_debug, int, 0644);
-MODULE_PARM_DESC(i2c_debug, "enable debug messages [i2c]");
+MODULE_PARM_DESC(i2c_debug, "i2c debug message level (1: normal debug, 2: show I2C transfers)");
 
 /*
  * em2800_i2c_send_bytes()
@@ -48,8 +49,8 @@ MODULE_PARM_DESC(i2c_debug, "enable debug messages [i2c]");
  */
 static int em2800_i2c_send_bytes(struct em28xx *dev, u8 addr, u8 *buf, u16 len)
 {
+	unsigned long timeout = jiffies + msecs_to_jiffies(EM28XX_I2C_XFER_TIMEOUT);
 	int ret;
-	int write_timeout;
 	u8 b2[6];
 
 	if (len < 1 || len > 4)
@@ -74,22 +75,26 @@ static int em2800_i2c_send_bytes(struct em28xx *dev, u8 addr, u8 *buf, u16 len)
 		return (ret < 0) ? ret : -EIO;
 	}
 	/* wait for completion */
-	for (write_timeout = EM2800_I2C_XFER_TIMEOUT; write_timeout > 0;
-	     write_timeout -= 5) {
+	while (time_is_after_jiffies(timeout)) {
 		ret = dev->em28xx_read_reg(dev, 0x05);
-		if (ret == 0x80 + len - 1) {
+		if (ret == 0x80 + len - 1)
 			return len;
-		} else if (ret == 0x94 + len - 1) {
-			return -ENODEV;
-		} else if (ret < 0) {
+		if (ret == 0x94 + len - 1) {
+			if (i2c_debug == 1)
+				em28xx_warn("R05 returned 0x%02x: I2C timeout",
+					    ret);
+			return -ENXIO;
+		}
+		if (ret < 0) {
 			em28xx_warn("failed to get i2c transfer status from bridge register (error=%i)\n",
 				    ret);
 			return ret;
 		}
 		msleep(5);
 	}
-	em28xx_warn("write to i2c device at 0x%x timed out\n", addr);
-	return -EIO;
+	if (i2c_debug)
+		em28xx_warn("write to i2c device at 0x%x timed out\n", addr);
+	return -ETIMEDOUT;
 }
 
 /*
@@ -98,9 +103,9 @@ static int em2800_i2c_send_bytes(struct em28xx *dev, u8 addr, u8 *buf, u16 len)
  */
 static int em2800_i2c_recv_bytes(struct em28xx *dev, u8 addr, u8 *buf, u16 len)
 {
+	unsigned long timeout = jiffies + msecs_to_jiffies(EM28XX_I2C_XFER_TIMEOUT);
 	u8 buf2[4];
 	int ret;
-	int read_timeout;
 	int i;
 
 	if (len < 1 || len > 4)
@@ -117,22 +122,28 @@ static int em2800_i2c_recv_bytes(struct em28xx *dev, u8 addr, u8 *buf, u16 len)
 	}
 
 	/* wait for completion */
-	for (read_timeout = EM2800_I2C_XFER_TIMEOUT; read_timeout > 0;
-	     read_timeout -= 5) {
+	while (time_is_after_jiffies(timeout)) {
 		ret = dev->em28xx_read_reg(dev, 0x05);
-		if (ret == 0x84 + len - 1) {
+		if (ret == 0x84 + len - 1)
 			break;
-		} else if (ret == 0x94 + len - 1) {
-			return -ENODEV;
-		} else if (ret < 0) {
+		if (ret == 0x94 + len - 1) {
+			if (i2c_debug == 1)
+				em28xx_warn("R05 returned 0x%02x: I2C timeout",
+					    ret);
+			return -ENXIO;
+		}
+		if (ret < 0) {
 			em28xx_warn("failed to get i2c transfer status from bridge register (error=%i)\n",
 				    ret);
 			return ret;
 		}
 		msleep(5);
 	}
-	if (ret != 0x84 + len - 1)
-		em28xx_warn("read from i2c device at 0x%x timed out\n", addr);
+	if (ret != 0x84 + len - 1) {
+		if (i2c_debug)
+			em28xx_warn("read from i2c device at 0x%x timed out\n",
+				    addr);
+	}
 
 	/* get the received message */
 	ret = dev->em28xx_read_reg_req_len(dev, 0x00, 4-len, buf2, len);
@@ -168,7 +179,8 @@ static int em2800_i2c_check_for_device(struct em28xx *dev, u8 addr)
 static int em28xx_i2c_send_bytes(struct em28xx *dev, u16 addr, u8 *buf,
 				 u16 len, int stop)
 {
-	int write_timeout, ret;
+	unsigned long timeout = jiffies + msecs_to_jiffies(EM28XX_I2C_XFER_TIMEOUT);
+	int ret;
 
 	if (len < 1 || len > 64)
 		return -EOPNOTSUPP;
@@ -191,16 +203,19 @@ static int em28xx_i2c_send_bytes(struct em28xx *dev, u16 addr, u8 *buf,
 		}
 	}
 
-	/* Check success of the i2c operation */
-	for (write_timeout = EM2800_I2C_XFER_TIMEOUT; write_timeout > 0;
-	     write_timeout -= 5) {
+	/* wait for completion */
+	while (time_is_after_jiffies(timeout)) {
 		ret = dev->em28xx_read_reg(dev, 0x05);
-		if (ret == 0) { /* success */
+		if (ret == 0) /* success */
 			return len;
-		} else if (ret == 0x10) {
-			return -ENODEV;
-		} else if (ret < 0) {
-			em28xx_warn("failed to read i2c transfer status from bridge (error=%i)\n",
+		if (ret == 0x10) {
+			if (i2c_debug == 1)
+				em28xx_warn("I2C transfer timeout on writing to addr 0x%02x",
+					    addr);
+			return -ENXIO;
+		}
+		if (ret < 0) {
+			em28xx_warn("failed to get i2c transfer status from bridge register (error=%i)\n",
 				    ret);
 			return ret;
 		}
@@ -211,8 +226,10 @@ static int em28xx_i2c_send_bytes(struct em28xx *dev, u16 addr, u8 *buf,
 		 * (even with high payload) ...
 		 */
 	}
-	em28xx_warn("write to i2c device at 0x%x timed out\n", addr);
-	return -EIO;
+	if (i2c_debug)
+		em28xx_warn("write to i2c device at 0x%x timed out (status=%i)\n",
+			    addr, ret);
+	return -ETIMEDOUT;
 }
 
 /*
@@ -242,26 +259,28 @@ static int em28xx_i2c_recv_bytes(struct em28xx *dev, u16 addr, u8 *buf, u16 len)
 	 * bytes if we are on bus B AND there was no write attempt to the
 	 * specified slave address before AND no device is present at the
 	 * requested slave address.
-	 * Anyway, the next check will fail with -ENODEV in this case, so avoid
+	 * Anyway, the next check will fail with -ENXIO in this case, so avoid
 	 * spamming the system log on device probing and do nothing here.
 	 */
 
 	/* Check success of the i2c operation */
 	ret = dev->em28xx_read_reg(dev, 0x05);
+	if (ret == 0) /* success */
+		return len;
 	if (ret < 0) {
-		em28xx_warn("failed to read i2c transfer status from bridge (error=%i)\n",
+		em28xx_warn("failed to get i2c transfer status from bridge register (error=%i)\n",
 			    ret);
 		return ret;
 	}
-	if (ret > 0) {
-		if (ret == 0x10) {
-			return -ENODEV;
-		} else {
-			em28xx_warn("unknown i2c error (status=%i)\n", ret);
-			return -EIO;
-		}
+	if (ret == 0x10) {
+		if (i2c_debug == 1)
+			em28xx_warn("I2C transfer timeout on writing to addr 0x%02x",
+				    addr);
+		return -ENXIO;
 	}
-	return len;
+
+	em28xx_warn("unknown i2c error (status=%i)\n", ret);
+	return -ETIMEDOUT;
 }
 
 /*
@@ -316,8 +335,12 @@ static int em25xx_bus_B_send_bytes(struct em28xx *dev, u16 addr, u8 *buf,
 	 */
 	if (!ret)
 		return len;
-	else if (ret > 0)
-		return -ENODEV;
+	else if (ret > 0) {
+		if (i2c_debug == 1)
+			em28xx_warn("Bus B R08 returned 0x%02x: I2C timeout",
+				    ret);
+		return -ENXIO;
+	}
 
 	return ret;
 	/*
@@ -355,7 +378,7 @@ static int em25xx_bus_B_recv_bytes(struct em28xx *dev, u16 addr, u8 *buf,
 	 * bytes if we are on bus B AND there was no write attempt to the
 	 * specified slave address before AND no device is present at the
 	 * requested slave address.
-	 * Anyway, the next check will fail with -ENODEV in this case, so avoid
+	 * Anyway, the next check will fail with -ENXIO in this case, so avoid
 	 * spamming the system log on device probing and do nothing here.
 	 */
 
@@ -367,8 +390,12 @@ static int em25xx_bus_B_recv_bytes(struct em28xx *dev, u16 addr, u8 *buf,
 	 */
 	if (!ret)
 		return len;
-	else if (ret > 0)
-		return -ENODEV;
+	else if (ret > 0) {
+		if (i2c_debug == 1)
+			em28xx_warn("Bus B R08 returned 0x%02x: I2C timeout",
+				    ret);
+		return -ENXIO;
+	}
 
 	return ret;
 	/*
@@ -409,10 +436,6 @@ static inline int i2c_check_for_device(struct em28xx_i2c_bus *i2c_bus, u16 addr)
 		rc = em2800_i2c_check_for_device(dev, addr);
 	else if (i2c_bus->algo_type == EM28XX_I2C_ALGO_EM25XX_BUS_B)
 		rc = em25xx_bus_B_check_for_device(dev, addr);
-	if (rc == -ENODEV) {
-		if (i2c_debug)
-			printk(" no device\n");
-	}
 	return rc;
 }
 
@@ -421,7 +444,7 @@ static inline int i2c_recv_bytes(struct em28xx_i2c_bus *i2c_bus,
 {
 	struct em28xx *dev = i2c_bus->dev;
 	u16 addr = msg.addr << 1;
-	int byte, rc = -EOPNOTSUPP;
+	int rc = -EOPNOTSUPP;
 
 	if (i2c_bus->algo_type == EM28XX_I2C_ALGO_EM28XX)
 		rc = em28xx_i2c_recv_bytes(dev, addr, msg.buf, msg.len);
@@ -429,10 +452,6 @@ static inline int i2c_recv_bytes(struct em28xx_i2c_bus *i2c_bus,
 		rc = em2800_i2c_recv_bytes(dev, addr, msg.buf, msg.len);
 	else if (i2c_bus->algo_type == EM28XX_I2C_ALGO_EM25XX_BUS_B)
 		rc = em25xx_bus_B_recv_bytes(dev, addr, msg.buf, msg.len);
-	if (i2c_debug) {
-		for (byte = 0; byte < msg.len; byte++)
-			printk(" %02x", msg.buf[byte]);
-	}
 	return rc;
 }
 
@@ -441,12 +460,8 @@ static inline int i2c_send_bytes(struct em28xx_i2c_bus *i2c_bus,
 {
 	struct em28xx *dev = i2c_bus->dev;
 	u16 addr = msg.addr << 1;
-	int byte, rc = -EOPNOTSUPP;
+	int rc = -EOPNOTSUPP;
 
-	if (i2c_debug) {
-		for (byte = 0; byte < msg.len; byte++)
-			printk(" %02x", msg.buf[byte]);
-	}
 	if (i2c_bus->algo_type == EM28XX_I2C_ALGO_EM28XX)
 		rc = em28xx_i2c_send_bytes(dev, addr, msg.buf, msg.len, stop);
 	else if (i2c_bus->algo_type == EM28XX_I2C_ALGO_EM2800)
@@ -491,33 +506,53 @@ static int em28xx_i2c_xfer(struct i2c_adapter *i2c_adap,
 	}
 	for (i = 0; i < num; i++) {
 		addr = msgs[i].addr << 1;
-		if (i2c_debug)
+		if (i2c_debug > 1)
 			printk(KERN_DEBUG "%s at %s: %s %s addr=%02x len=%d:",
 			       dev->name, __func__ ,
 			       (msgs[i].flags & I2C_M_RD) ? "read" : "write",
 			       i == num - 1 ? "stop" : "nonstop",
 			       addr, msgs[i].len);
-		if (!msgs[i].len) { /* no len: check only for device presence */
+		if (!msgs[i].len) {
+			/*
+			 * no len: check only for device presence
+			 * This code is only called during device probe.
+			 */
 			rc = i2c_check_for_device(i2c_bus, addr);
-			if (rc == -ENODEV) {
+			if (rc < 0) {
+				if (rc == -ENXIO) {
+					if (i2c_debug > 1)
+						printk(KERN_CONT " no device\n");
+					rc = -ENODEV;
+				} else {
+					if (i2c_debug > 1)
+						printk(KERN_CONT " ERROR: %i\n", rc);
+				}
 				rt_mutex_unlock(&dev->i2c_bus_lock);
 				return rc;
 			}
 		} else if (msgs[i].flags & I2C_M_RD) {
 			/* read bytes */
 			rc = i2c_recv_bytes(i2c_bus, msgs[i]);
+
+			if (i2c_debug > 1 && rc >= 0)
+				printk(KERN_CONT " %*ph",
+				       msgs[i].len, msgs[i].buf);
 		} else {
+			if (i2c_debug > 1)
+				printk(KERN_CONT " %*ph",
+				       msgs[i].len, msgs[i].buf);
+
 			/* write bytes */
 			rc = i2c_send_bytes(i2c_bus, msgs[i], i == num - 1);
 		}
 		if (rc < 0) {
-			if (i2c_debug)
-				printk(" ERROR: %i\n", rc);
+			if (i2c_debug > 1)
+				printk(KERN_CONT " ERROR: %i\n", rc);
 			rt_mutex_unlock(&dev->i2c_bus_lock);
 			return rc;
 		}
-		if (i2c_debug)
-			printk("\n");
+		if (i2c_debug > 1)
+			printk(KERN_CONT "\n");
 	}
 
 	rt_mutex_unlock(&dev->i2c_bus_lock);
@@ -600,7 +635,7 @@ static int em28xx_i2c_eeprom(struct em28xx *dev, unsigned bus,
 	 * calculation and returned device dataset. Simplifies the code a lot,
 	 * but we might have to deal with multiple sizes in the future !
 	 */
-	int i, err;
+	int err;
 	struct em28xx_eeprom *dev_config;
 	u8 buf, *data;
 
@@ -631,20 +666,14 @@ static int em28xx_i2c_eeprom(struct em28xx *dev, unsigned bus,
 		goto error;
 	}
 
-	/* Display eeprom content */
-	for (i = 0; i < len; i++) {
-		if (0 == (i % 16)) {
-			if (dev->eeprom_addrwidth_16bit)
-				em28xx_info("i2c eeprom %04x:", i);
-			else
-				em28xx_info("i2c eeprom %02x:", i);
-		}
-		printk(" %02x", data[i]);
-		if (15 == (i % 16))
-			printk("\n");
+	if (i2c_debug) {
+		/* Display eeprom content */
+		print_hex_dump(KERN_INFO, "eeprom ", DUMP_PREFIX_OFFSET,
+			       16, 1, data, len, true);
+
+		if (dev->eeprom_addrwidth_16bit)
+			em28xx_info("eeprom %06x: ... (skipped)\n", 256);
 	}
-	if (dev->eeprom_addrwidth_16bit)
-		em28xx_info("i2c eeprom %04x: ... (skipped)\n", i);
 
 	if (dev->eeprom_addrwidth_16bit &&
 	    data[0] == 0x26 && data[3] == 0x00) {
@@ -736,10 +765,16 @@ static int em28xx_i2c_eeprom(struct em28xx *dev, unsigned bus,
 		em28xx_info("\tAC97 audio (5 sample rates)\n");
 		break;
 	case 2:
-		em28xx_info("\tI2S audio, sample rate=32k\n");
+		if (dev->chip_id < CHIP_ID_EM2860)
+			em28xx_info("\tI2S audio, sample rate=32k\n");
+		else
+			em28xx_info("\tI2S audio, 3 sample rates\n");
 		break;
 	case 3:
-		em28xx_info("\tI2S audio, 3 sample rates\n");
+		if (dev->chip_id < CHIP_ID_EM2860)
+			em28xx_info("\tI2S audio, 3 sample rates\n");
+		else
+			em28xx_info("\tI2S audio, 5 sample rates\n");
 		break;
 	}
 
diff --git a/drivers/media/usb/em28xx/em28xx-input.c b/drivers/media/usb/em28xx/em28xx-input.c
index ea181e4b68c5..18f65d89d4bc 100644
--- a/drivers/media/usb/em28xx/em28xx-input.c
+++ b/drivers/media/usb/em28xx/em28xx-input.c
@@ -30,8 +30,9 @@
 
 #include "em28xx.h"
 
-#define EM28XX_SNAPSHOT_KEY KEY_CAMERA
-#define EM28XX_SBUTTON_QUERY_INTERVAL 500
+#define EM28XX_SNAPSHOT_KEY				KEY_CAMERA
+#define EM28XX_BUTTONS_DEBOUNCED_QUERY_INTERVAL		500 /* [ms] */
+#define EM28XX_BUTTONS_VOLATILE_QUERY_INTERVAL		100 /* [ms] */
 
 static unsigned int ir_debug;
 module_param(ir_debug, int, 0644);
@@ -442,6 +443,7 @@ static int em28xx_ir_change_protocol(struct rc_dev *rc_dev, u64 *rc_type)
 	case CHIP_ID_EM2884:
 	case CHIP_ID_EM2874:
 	case CHIP_ID_EM28174:
+	case CHIP_ID_EM28178:
 		return em2874_ir_change_protocol(rc_dev, rc_type);
 	default:
 		printk("Unrecognized em28xx chip id 0x%02x: IR not supported\n",
@@ -470,54 +472,98 @@ static int em28xx_probe_i2c_ir(struct em28xx *dev)
 }
 
 /**********************************************************
- Handle Webcam snapshot button
+ Handle buttons
  **********************************************************/
 
-static void em28xx_query_sbutton(struct work_struct *work)
+static void em28xx_query_buttons(struct work_struct *work)
 {
-	/* Poll the register and see if the button is depressed */
 	struct em28xx *dev =
-		container_of(work, struct em28xx, sbutton_query_work.work);
-	int ret;
-
-	ret = em28xx_read_reg(dev, EM28XX_R0C_USBSUSP);
-
-	if (ret & EM28XX_R0C_USBSUSP_SNAPSHOT) {
-		u8 cleared;
-		/* Button is depressed, clear the register */
-		cleared = ((u8) ret) & ~EM28XX_R0C_USBSUSP_SNAPSHOT;
-		em28xx_write_regs(dev, EM28XX_R0C_USBSUSP, &cleared, 1);
-
-		/* Not emulate the keypress */
-		input_report_key(dev->sbutton_input_dev, EM28XX_SNAPSHOT_KEY,
-				 1);
-		/* Now unpress the key */
-		input_report_key(dev->sbutton_input_dev, EM28XX_SNAPSHOT_KEY,
-				 0);
+		container_of(work, struct em28xx, buttons_query_work.work);
+	u8 i, j;
+	int regval;
+	bool is_pressed, was_pressed;
+	const struct em28xx_led *led;
+
+	/* Poll and evaluate all addresses */
+	for (i = 0; i < dev->num_button_polling_addresses; i++) {
+		/* Read value from register */
+		regval = em28xx_read_reg(dev, dev->button_polling_addresses[i]);
+		if (regval < 0)
+			continue;
+		/* Check states of the buttons and act */
+		j = 0;
+		while (dev->board.buttons[j].role >= 0 &&
+			 dev->board.buttons[j].role < EM28XX_NUM_BUTTON_ROLES) {
+			struct em28xx_button *button = &dev->board.buttons[j];
+			/* Check if button uses the current address */
+			if (button->reg_r != dev->button_polling_addresses[i]) {
+				j++;
+				continue;
+			}
+			/* Determine if button is and was pressed last time */
+			is_pressed = regval & button->mask;
+			was_pressed = dev->button_polling_last_values[i]
+				       & button->mask;
+			if (button->inverted) {
+				is_pressed = !is_pressed;
+				was_pressed = !was_pressed;
+			}
+			/* Clear button state (if needed) */
+			if (is_pressed && button->reg_clearing)
+				em28xx_write_reg(dev, button->reg_clearing,
+						 (~regval & button->mask)
+						    | (regval & ~button->mask));
+			/* Handle button state */
+			if (!is_pressed || was_pressed) {
+				j++;
+				continue;
+			}
+			switch (button->role) {
+			case EM28XX_BUTTON_SNAPSHOT:
+				/* Emulate the keypress */
+				input_report_key(dev->sbutton_input_dev,
+						 EM28XX_SNAPSHOT_KEY, 1);
+				/* Unpress the key */
+				input_report_key(dev->sbutton_input_dev,
+						 EM28XX_SNAPSHOT_KEY, 0);
+				break;
+			case EM28XX_BUTTON_ILLUMINATION:
+				led = em28xx_find_led(dev,
+						      EM28XX_LED_ILLUMINATION);
+				/* Switch illumination LED on/off */
+				if (led)
+					em28xx_toggle_reg_bits(dev,
+							       led->gpio_reg,
+							       led->gpio_mask);
+				break;
+			default:
+				WARN_ONCE(1, "BUG: unhandled button role.");
+			}
+			/* Next button */
+			j++;
+		}
+		/* Save current value for comparison during the next polling */
+		dev->button_polling_last_values[i] = regval;
 	}
-
 	/* Schedule next poll */
-	schedule_delayed_work(&dev->sbutton_query_work,
-			      msecs_to_jiffies(EM28XX_SBUTTON_QUERY_INTERVAL));
+	schedule_delayed_work(&dev->buttons_query_work,
+			      msecs_to_jiffies(dev->button_polling_interval));
 }
 
-static void em28xx_register_snapshot_button(struct em28xx *dev)
+static int em28xx_register_snapshot_button(struct em28xx *dev)
 {
 	struct input_dev *input_dev;
 	int err;
 
 	em28xx_info("Registering snapshot button...\n");
 	input_dev = input_allocate_device();
-	if (!input_dev) {
-		em28xx_errdev("input_allocate_device failed\n");
-		return;
-	}
+	if (!input_dev)
+		return -ENOMEM;
 
 	usb_make_path(dev->udev, dev->snapshot_button_path,
 		      sizeof(dev->snapshot_button_path));
 	strlcat(dev->snapshot_button_path, "/sbutton",
 		sizeof(dev->snapshot_button_path));
-	INIT_DELAYED_WORK(&dev->sbutton_query_work, em28xx_query_sbutton);
 
 	input_dev->name = "em28xx snapshot button";
 	input_dev->phys = dev->snapshot_button_path;
@@ -535,25 +581,86 @@ static void em28xx_register_snapshot_button(struct em28xx *dev)
 	if (err) {
 		em28xx_errdev("input_register_device failed\n");
 		input_free_device(input_dev);
-		return;
+		return err;
 	}
 
 	dev->sbutton_input_dev = input_dev;
-	schedule_delayed_work(&dev->sbutton_query_work,
-			      msecs_to_jiffies(EM28XX_SBUTTON_QUERY_INTERVAL));
-	return;
+	return 0;
+}
 
+static void em28xx_init_buttons(struct em28xx *dev)
+{
+	u8  i = 0, j = 0;
+	bool addr_new = 0;
+
+	dev->button_polling_interval = EM28XX_BUTTONS_DEBOUNCED_QUERY_INTERVAL;
+	while (dev->board.buttons[i].role >= 0 &&
+			 dev->board.buttons[i].role < EM28XX_NUM_BUTTON_ROLES) {
+		struct em28xx_button *button = &dev->board.buttons[i];
+		/* Check if polling address is already on the list */
+		addr_new = 1;
+		for (j = 0; j < dev->num_button_polling_addresses; j++) {
+			if (button->reg_r == dev->button_polling_addresses[j]) {
+				addr_new = 0;
+				break;
+			}
+		}
+		/* Check if max. number of polling addresses is exceeded */
+		if (addr_new && dev->num_button_polling_addresses
+					   >= EM28XX_NUM_BUTTON_ADDRESSES_MAX) {
+			WARN_ONCE(1, "BUG: maximum number of button polling addresses exceeded.");
+			goto next_button;
+		}
+		/* Button role specific checks and actions */
+		if (button->role == EM28XX_BUTTON_SNAPSHOT) {
+			/* Register input device */
+			if (em28xx_register_snapshot_button(dev) < 0)
+				goto next_button;
+		} else if (button->role == EM28XX_BUTTON_ILLUMINATION) {
+			/* Check sanity */
+			if (!em28xx_find_led(dev, EM28XX_LED_ILLUMINATION)) {
+				em28xx_errdev("BUG: illumination button defined, but no illumination LED.\n");
+				goto next_button;
+			}
+		}
+		/* Add read address to list of polling addresses */
+		if (addr_new) {
+			unsigned int index = dev->num_button_polling_addresses;
+			dev->button_polling_addresses[index] = button->reg_r;
+			dev->num_button_polling_addresses++;
+		}
+		/* Reduce polling interval if necessary */
+		if (!button->reg_clearing)
+			dev->button_polling_interval =
+					 EM28XX_BUTTONS_VOLATILE_QUERY_INTERVAL;
+next_button:
+		/* Next button */
+		i++;
+	}
+
+	/* Start polling */
+	if (dev->num_button_polling_addresses) {
+		memset(dev->button_polling_last_values, 0,
+					       EM28XX_NUM_BUTTON_ADDRESSES_MAX);
+		INIT_DELAYED_WORK(&dev->buttons_query_work,
+							  em28xx_query_buttons);
+		schedule_delayed_work(&dev->buttons_query_work,
+			       msecs_to_jiffies(dev->button_polling_interval));
+	}
 }
 
-static void em28xx_deregister_snapshot_button(struct em28xx *dev)
+static void em28xx_shutdown_buttons(struct em28xx *dev)
 {
+	/* Cancel polling */
+	cancel_delayed_work_sync(&dev->buttons_query_work);
+	/* Clear polling addresses list */
+	dev->num_button_polling_addresses = 0;
+	/* Deregister input devices */
 	if (dev->sbutton_input_dev != NULL) {
 		em28xx_info("Deregistering snapshot button\n");
-		cancel_delayed_work_sync(&dev->sbutton_query_work);
 		input_unregister_device(dev->sbutton_input_dev);
 		dev->sbutton_input_dev = NULL;
 	}
-	return;
 }
 
 static int em28xx_ir_init(struct em28xx *dev)
@@ -564,8 +671,13 @@ static int em28xx_ir_init(struct em28xx *dev)
 	u64 rc_type;
 	u16 i2c_rc_dev_addr = 0;
 
-	if (dev->board.has_snapshot_button)
-		em28xx_register_snapshot_button(dev);
+	if (dev->is_audio_only) {
+		/* Shouldn't initialize IR for this interface */
+		return 0;
+	}
+
+	if (dev->board.buttons)
+		em28xx_init_buttons(dev);
 
 	if (dev->board.has_ir_i2c) {
 		i2c_rc_dev_addr = em28xx_probe_i2c_ir(dev);
@@ -583,6 +695,8 @@ static int em28xx_ir_init(struct em28xx *dev)
 		return 0;
 	}
 
+	em28xx_info("Registering input extension\n");
+
 	ir = kzalloc(sizeof(*ir), GFP_KERNEL);
 	rc = rc_allocate_device();
 	if (!ir || !rc)
@@ -633,6 +747,7 @@ static int em28xx_ir_init(struct em28xx *dev)
 		case CHIP_ID_EM2884:
 		case CHIP_ID_EM2874:
 		case CHIP_ID_EM28174:
+		case CHIP_ID_EM28178:
 			ir->get_key = em2874_polling_getkey;
 			rc->allowed_protos = RC_BIT_RC5 | RC_BIT_NEC |
 					     RC_BIT_RC6_0;
@@ -675,6 +790,8 @@ static int em28xx_ir_init(struct em28xx *dev)
 	if (err)
 		goto error;
 
+	em28xx_info("Input extension successfully initalized\n");
+
 	return 0;
 
 error:
@@ -688,7 +805,14 @@ static int em28xx_ir_fini(struct em28xx *dev)
 {
 	struct em28xx_IR *ir = dev->ir;
 
-	em28xx_deregister_snapshot_button(dev);
+	if (dev->is_audio_only) {
+		/* Shouldn't initialize IR for this interface */
+		return 0;
+	}
+
+	em28xx_info("Closing input extension");
+
+	em28xx_shutdown_buttons(dev);
 
 	/* skip detach on non attached boards */
 	if (!ir)
@@ -722,7 +846,8 @@ static void __exit em28xx_rc_unregister(void)
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@redhat.com>");
-MODULE_DESCRIPTION("Em28xx Input driver");
+MODULE_DESCRIPTION(DRIVER_DESC " - input interface");
+MODULE_VERSION(EM28XX_VERSION);
 
 module_init(em28xx_rc_register);
 module_exit(em28xx_rc_unregister);
diff --git a/drivers/media/usb/em28xx/em28xx-reg.h b/drivers/media/usb/em28xx/em28xx-reg.h
index 0e0477847965..311fb349dafa 100644
--- a/drivers/media/usb/em28xx/em28xx-reg.h
+++ b/drivers/media/usb/em28xx/em28xx-reg.h
@@ -25,10 +25,12 @@
 #define EM28XX_R00_CHIPCFG	0x00
 
 /* em28xx Chip Configuration 0x00 */
-#define EM28XX_CHIPCFG_VENDOR_AUDIO		0x80
-#define EM28XX_CHIPCFG_I2S_VOLUME_CAPABLE	0x40
-#define EM28XX_CHIPCFG_I2S_5_SAMPRATES		0x30
-#define EM28XX_CHIPCFG_I2S_3_SAMPRATES		0x20
+#define EM2860_CHIPCFG_VENDOR_AUDIO		0x80
+#define EM2860_CHIPCFG_I2S_VOLUME_CAPABLE	0x40
+#define EM2820_CHIPCFG_I2S_3_SAMPRATES		0x30
+#define EM2860_CHIPCFG_I2S_5_SAMPRATES		0x30
+#define EM2820_CHIPCFG_I2S_1_SAMPRATE		0x20
+#define EM2860_CHIPCFG_I2S_3_SAMPRATES		0x20
 #define EM28XX_CHIPCFG_AC97			0x10
 #define EM28XX_CHIPCFG_AUDIOMASK		0x30
 
@@ -245,6 +247,7 @@ enum em28xx_chip_id {
 	CHIP_ID_EM2874 = 65,
 	CHIP_ID_EM2884 = 68,
 	CHIP_ID_EM28174 = 113,
+	CHIP_ID_EM28178 = 114,
 };
 
 /*
diff --git a/drivers/media/usb/em28xx/em28xx-v4l.h b/drivers/media/usb/em28xx/em28xx-v4l.h
new file mode 100644
index 000000000000..bce438691e0e
--- /dev/null
+++ b/drivers/media/usb/em28xx/em28xx-v4l.h
@@ -0,0 +1,20 @@
+/*
+   em28xx-video.c - driver for Empia EM2800/EM2820/2840 USB
+		    video capture devices
+
+   Copyright (C) 2013-2014 Mauro Carvalho Chehab <m.chehab@samsung.com>
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation 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.
+ */
+
+
+int em28xx_start_analog_streaming(struct vb2_queue *vq, unsigned int count);
+int 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 39f39c527c13..db3d655600df 100644
--- a/drivers/media/usb/em28xx/em28xx-vbi.c
+++ b/drivers/media/usb/em28xx/em28xx-vbi.c
@@ -27,6 +27,7 @@
 #include <linux/init.h>
 
 #include "em28xx.h"
+#include "em28xx-v4l.h"
 
 static unsigned int vbibufs = 5;
 module_param(vbibufs, int, 0644);
diff --git a/drivers/media/usb/em28xx/em28xx-video.c b/drivers/media/usb/em28xx/em28xx-video.c
index dd19c9ff76e0..c3c928937dcd 100644
--- a/drivers/media/usb/em28xx/em28xx-video.c
+++ b/drivers/media/usb/em28xx/em28xx-video.c
@@ -38,9 +38,11 @@
 #include <linux/slab.h>
 
 #include "em28xx.h"
+#include "em28xx-v4l.h"
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-event.h>
+#include <media/v4l2-clk.h>
 #include <media/msp3400.h>
 #include <media/tuner.h>
 
@@ -49,19 +51,23 @@
 		      "Mauro Carvalho Chehab <mchehab@infradead.org>, " \
 		      "Sascha Sommer <saschasommer@freenet.de>"
 
-#define DRIVER_DESC         "Empia em28xx based USB video device driver"
+static unsigned int isoc_debug;
+module_param(isoc_debug, int, 0644);
+MODULE_PARM_DESC(isoc_debug, "enable debug messages [isoc transfers]");
+
+static unsigned int disable_vbi;
+module_param(disable_vbi, int, 0644);
+MODULE_PARM_DESC(disable_vbi, "disable vbi support");
 
-#define EM28XX_VERSION "0.2.0"
+static int alt;
+module_param(alt, int, 0644);
+MODULE_PARM_DESC(alt, "alternate setting to use for video endpoint");
 
 #define em28xx_videodbg(fmt, arg...) do {\
 	if (video_debug) \
 		printk(KERN_INFO "%s %s :"fmt, \
 			 dev->name, __func__ , ##arg); } while (0)
 
-static unsigned int isoc_debug;
-module_param(isoc_debug, int, 0644);
-MODULE_PARM_DESC(isoc_debug, "enable debug messages [isoc transfers]");
-
 #define em28xx_isocdbg(fmt, arg...) \
 do {\
 	if (isoc_debug) { \
@@ -71,7 +77,7 @@ do {\
   } while (0)
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
-MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_DESCRIPTION(DRIVER_DESC " - v4l2 interface");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(EM28XX_VERSION);
 
@@ -135,6 +141,257 @@ static struct em28xx_fmt format[] = {
 	},
 };
 
+static int em28xx_vbi_supported(struct em28xx *dev)
+{
+	/* Modprobe option to manually disable */
+	if (disable_vbi == 1)
+		return 0;
+
+	if (dev->board.is_webcam)
+		return 0;
+
+	/* FIXME: check subdevices for VBI support */
+
+	if (dev->chip_id == CHIP_ID_EM2860 ||
+	    dev->chip_id == CHIP_ID_EM2883)
+		return 1;
+
+	/* Version of em28xx that does not support VBI */
+	return 0;
+}
+
+/*
+ * em28xx_wake_i2c()
+ * configure i2c attached devices
+ */
+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,
+			INPUT(dev->ctl_input)->vmux, 0, 0);
+	v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_stream, 0);
+}
+
+static int em28xx_colorlevels_set_default(struct em28xx *dev)
+{
+	em28xx_write_reg(dev, EM28XX_R20_YGAIN, CONTRAST_DEFAULT);
+	em28xx_write_reg(dev, EM28XX_R21_YOFFSET, BRIGHTNESS_DEFAULT);
+	em28xx_write_reg(dev, EM28XX_R22_UVGAIN, SATURATION_DEFAULT);
+	em28xx_write_reg(dev, EM28XX_R23_UOFFSET, BLUE_BALANCE_DEFAULT);
+	em28xx_write_reg(dev, EM28XX_R24_VOFFSET, RED_BALANCE_DEFAULT);
+	em28xx_write_reg(dev, EM28XX_R25_SHARPNESS, SHARPNESS_DEFAULT);
+
+	em28xx_write_reg(dev, EM28XX_R14_GAMMA, 0x20);
+	em28xx_write_reg(dev, EM28XX_R15_RGAIN, 0x20);
+	em28xx_write_reg(dev, EM28XX_R16_GGAIN, 0x20);
+	em28xx_write_reg(dev, EM28XX_R17_BGAIN, 0x20);
+	em28xx_write_reg(dev, EM28XX_R18_ROFFSET, 0x00);
+	em28xx_write_reg(dev, EM28XX_R19_GOFFSET, 0x00);
+	return em28xx_write_reg(dev, EM28XX_R1A_BOFFSET, 0x00);
+}
+
+static int em28xx_set_outfmt(struct em28xx *dev)
+{
+	int ret;
+	u8 fmt, vinctrl;
+
+	fmt = dev->format->reg;
+	if (!dev->is_em25xx)
+		fmt |= 0x20;
+	/*
+	 * NOTE: it's not clear if this is really needed !
+	 * The datasheets say bit 5 is a reserved bit and devices seem to work
+	 * fine without it. But the Windows driver sets it for em2710/50+em28xx
+	 * devices and we've always been setting it, too.
+	 *
+	 * em2765 (em25xx, em276x/7x/8x) devices do NOT work with this bit set,
+	 * it's likely used for an additional (compressed ?) format there.
+	 */
+	ret = em28xx_write_reg(dev, EM28XX_R27_OUTFMT, fmt);
+	if (ret < 0)
+		return ret;
+
+	ret = em28xx_write_reg(dev, EM28XX_R10_VINMODE, dev->vinmode);
+	if (ret < 0)
+		return ret;
+
+	vinctrl = dev->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) {
+			/* NTSC */
+			em28xx_write_reg(dev, EM28XX_R35_VBI_START_V, 0x09);
+		} else if (dev->norm & V4L2_STD_625_50) {
+			/* PAL */
+			em28xx_write_reg(dev, EM28XX_R35_VBI_START_V, 0x07);
+		}
+	}
+
+	return em28xx_write_reg(dev, EM28XX_R11_VINCTRL, vinctrl);
+}
+
+static int em28xx_accumulator_set(struct em28xx *dev, u8 xmin, u8 xmax,
+				  u8 ymin, u8 ymax)
+{
+	em28xx_videodbg("em28xx Scale: (%d,%d)-(%d,%d)\n",
+			xmin, ymin, xmax, ymax);
+
+	em28xx_write_regs(dev, EM28XX_R28_XMIN, &xmin, 1);
+	em28xx_write_regs(dev, EM28XX_R29_XMAX, &xmax, 1);
+	em28xx_write_regs(dev, EM28XX_R2A_YMIN, &ymin, 1);
+	return em28xx_write_regs(dev, EM28XX_R2B_YMAX, &ymax, 1);
+}
+
+static void em28xx_capture_area_set(struct em28xx *dev, u8 hstart, u8 vstart,
+				   u16 width, u16 height)
+{
+	u8 cwidth = width >> 2;
+	u8 cheight = height >> 2;
+	u8 overflow = (height >> 9 & 0x02) | (width >> 10 & 0x01);
+	/* NOTE: size limit: 2047x1023 = 2MPix */
+
+	em28xx_videodbg("capture area set to (%d,%d): %dx%d\n",
+		       hstart, vstart,
+		       ((overflow & 2) << 9 | cwidth << 2),
+		       ((overflow & 1) << 10 | cheight << 2));
+
+	em28xx_write_regs(dev, EM28XX_R1C_HSTART, &hstart, 1);
+	em28xx_write_regs(dev, EM28XX_R1D_VSTART, &vstart, 1);
+	em28xx_write_regs(dev, EM28XX_R1E_CWIDTH, &cwidth, 1);
+	em28xx_write_regs(dev, EM28XX_R1F_CHEIGHT, &cheight, 1);
+	em28xx_write_regs(dev, EM28XX_R1B_OFLOW, &overflow, 1);
+
+	/* FIXME: function/meaning of these registers ? */
+	/* FIXME: align width+height to multiples of 4 ?! */
+	if (dev->is_em25xx) {
+		em28xx_write_reg(dev, 0x34, width >> 4);
+		em28xx_write_reg(dev, 0x35, height >> 4);
+	}
+}
+
+static int em28xx_scaler_set(struct em28xx *dev, u16 h, u16 v)
+{
+	u8 mode;
+	/* the em2800 scaler only supports scaling down to 50% */
+
+	if (dev->board.is_em2800) {
+		mode = (v ? 0x20 : 0x00) | (h ? 0x10 : 0x00);
+	} else {
+		u8 buf[2];
+
+		buf[0] = h;
+		buf[1] = h >> 8;
+		em28xx_write_regs(dev, EM28XX_R30_HSCALELOW, (char *)buf, 2);
+
+		buf[0] = v;
+		buf[1] = v >> 8;
+		em28xx_write_regs(dev, EM28XX_R32_VSCALELOW, (char *)buf, 2);
+		/* it seems that both H and V scalers must be active
+		   to work correctly */
+		mode = (h || v) ? 0x30 : 0x00;
+	}
+	return em28xx_write_reg_bits(dev, EM28XX_R26_COMPR, mode, 0x30);
+}
+
+/* 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);
+
+	/* Properly setup VBI */
+	dev->vbi_width = 720;
+	if (dev->norm & V4L2_STD_525_60)
+		dev->vbi_height = 12;
+	else
+		dev->vbi_height = 18;
+
+	em28xx_set_outfmt(dev);
+
+	em28xx_accumulator_set(dev, 1, (width - 4) >> 2, 1, (height - 4) >> 2);
+
+	/* If we don't set the start position to 2 in VBI mode, we end up
+	   with line 20/21 being YUYV encoded instead of being in 8-bit
+	   greyscale.  The core of the issue is that line 21 (and line 23 for
+	   PAL WSS) are inside of active video region, and as a result they
+	   get the pixelformatting associated with that area.  So by cropping
+	   it out, we end up with the same format as the rest of the VBI
+	   region */
+	if (em28xx_vbi_supported(dev) == 1)
+		em28xx_capture_area_set(dev, 0, 2, width, height);
+	else
+		em28xx_capture_area_set(dev, 0, 0, width, height);
+
+	return em28xx_scaler_set(dev, dev->hscale, dev->vscale);
+}
+
+/* Set USB alternate setting for analog video */
+static int em28xx_set_alternate(struct em28xx *dev)
+{
+	int errCode;
+	int i;
+	unsigned int min_pkt_size = dev->width * 2 + 4;
+
+	/* NOTE: for isoc transfers, only alt settings > 0 are allowed
+		 bulk transfers seem to work only with alt=0 ! */
+	dev->alt = 0;
+	if ((alt > 0) && (alt < dev->num_alt)) {
+		em28xx_videodbg("alternate forced to %d\n", dev->alt);
+		dev->alt = alt;
+		goto set_alt;
+	}
+	if (dev->analog_xfer_bulk)
+		goto set_alt;
+
+	/* When image size is bigger than a certain value,
+	   the frame size should be increased, otherwise, only
+	   green screen will be received.
+	 */
+	if (dev->width * 2 * dev->height > 720 * 240 * 2)
+		min_pkt_size *= 2;
+
+	for (i = 0; i < dev->num_alt; i++) {
+		/* stop when the selected alt setting offers enough bandwidth */
+		if (dev->alt_max_pkt_size_isoc[i] >= min_pkt_size) {
+			dev->alt = i;
+			break;
+		/* otherwise make sure that we end up with the maximum bandwidth
+		   because the min_pkt_size equation might be wrong...
+		*/
+		} else if (dev->alt_max_pkt_size_isoc[i] >
+			   dev->alt_max_pkt_size_isoc[dev->alt])
+			dev->alt = i;
+	}
+
+set_alt:
+	/* NOTE: for bulk transfers, we need to call usb_set_interface()
+	 * even if the previous settings were the same. Otherwise streaming
+	 * fails with all urbs having status = -EOVERFLOW ! */
+	if (dev->analog_xfer_bulk) {
+		dev->max_pkt_size = 512; /* USB 2.0 spec */
+		dev->packet_multiplier = EM28XX_BULK_PACKET_MULTIPLIER;
+	} else { /* isoc */
+		em28xx_videodbg("minimum isoc packet size: %u (alt=%d)\n",
+			       min_pkt_size, dev->alt);
+		dev->max_pkt_size =
+				  dev->alt_max_pkt_size_isoc[dev->alt];
+		dev->packet_multiplier = EM28XX_NUM_ISOC_PACKETS;
+	}
+	em28xx_videodbg("setting alternate %d with wMaxPacketSize=%u\n",
+		       dev->alt, dev->max_pkt_size);
+	errCode = usb_set_interface(dev->udev, dev->ifnum, dev->alt);
+	if (errCode < 0) {
+		em28xx_errdev("cannot change alternate number to %d (error=%i)\n",
+			      dev->alt, errCode);
+		return errCode;
+	}
+	return 0;
+}
+
 /* ------------------------------------------------------------------
 	DMA and thread functions
    ------------------------------------------------------------------*/
@@ -763,7 +1020,7 @@ static struct vb2_ops em28xx_video_qops = {
 	.wait_finish    = vb2_ops_wait_finish,
 };
 
-int em28xx_vb2_setup(struct em28xx *dev)
+static int em28xx_vb2_setup(struct em28xx *dev)
 {
 	int rc;
 	struct vb2_queue *q;
@@ -831,7 +1088,7 @@ static void video_mux(struct em28xx *dev, int index)
 	em28xx_audio_analog_set(dev);
 }
 
-void em28xx_ctrl_notify(struct v4l2_ctrl *ctrl, void *priv)
+static void em28xx_ctrl_notify(struct v4l2_ctrl *ctrl, void *priv)
 {
 	struct em28xx *dev = priv;
 
@@ -890,7 +1147,7 @@ static int em28xx_s_ctrl(struct v4l2_ctrl *ctrl)
 	return (ret < 0) ? ret : 0;
 }
 
-const struct v4l2_ctrl_ops em28xx_ctrl_ops = {
+static const struct v4l2_ctrl_ops em28xx_ctrl_ops = {
 	.s_ctrl = em28xx_s_ctrl,
 };
 
@@ -1368,7 +1625,7 @@ static int vidioc_g_register(struct file *file, void *priv,
 		reg->val = ret;
 	} else {
 		__le16 val = 0;
-		ret = em28xx_read_reg_req_len(dev, USB_REQ_GET_STATUS,
+		ret = dev->em28xx_read_reg_req_len(dev, USB_REQ_GET_STATUS,
 						   reg->reg, (char *)&val, 2);
 		if (ret < 0)
 			return ret;
@@ -1570,6 +1827,10 @@ static int em28xx_v4l2_open(struct file *filp)
 	case VFL_TYPE_VBI:
 		fh_type = V4L2_BUF_TYPE_VBI_CAPTURE;
 		break;
+	case VFL_TYPE_RADIO:
+		break;
+	default:
+		return -EINVAL;
 	}
 
 	em28xx_videodbg("open dev=%s type=%s users=%d\n",
@@ -1590,15 +1851,17 @@ static int em28xx_v4l2_open(struct file *filp)
 	fh->type = fh_type;
 	filp->private_data = fh;
 
-	if (fh->type == V4L2_BUF_TYPE_VIDEO_CAPTURE && dev->users == 0) {
+	if (dev->users == 0) {
 		em28xx_set_mode(dev, EM28XX_ANALOG_MODE);
-		em28xx_resolution_set(dev);
 
-		/* Needed, since GPIO might have disabled power of
-		   some i2c device
+		if (vdev->vfl_type != VFL_TYPE_RADIO)
+			em28xx_resolution_set(dev);
+
+		/*
+		 * Needed, since GPIO might have disabled power
+		 * of some i2c devices
 		 */
 		em28xx_wake_i2c(dev);
-
 	}
 
 	if (vdev->vfl_type == VFL_TYPE_RADIO) {
@@ -1615,40 +1878,59 @@ static int em28xx_v4l2_open(struct file *filp)
 }
 
 /*
- * em28xx_realease_resources()
+ * em28xx_v4l2_fini()
  * unregisters the v4l2,i2c and usb devices
  * called when the device gets disconected or at module unload
 */
-void em28xx_release_analog_resources(struct em28xx *dev)
+static int em28xx_v4l2_fini(struct em28xx *dev)
 {
+	if (dev->is_audio_only) {
+		/* Shouldn't initialize IR for this interface */
+		return 0;
+	}
+
+	if (!dev->has_video) {
+		/* This device does not support the v4l2 extension */
+		return 0;
+	}
 
-	/*FIXME: I2C IR should be disconnected */
+	em28xx_info("Closing video extension");
+
+	mutex_lock(&dev->lock);
+
+	v4l2_device_disconnect(&dev->v4l2_dev);
+
+	em28xx_uninit_usb_xfer(dev, EM28XX_ANALOG_MODE);
 
 	if (dev->radio_dev) {
-		if (video_is_registered(dev->radio_dev))
-			video_unregister_device(dev->radio_dev);
-		else
-			video_device_release(dev->radio_dev);
-		dev->radio_dev = NULL;
+		em28xx_info("V4L2 device %s deregistered\n",
+			    video_device_node_name(dev->radio_dev));
+		video_unregister_device(dev->radio_dev);
 	}
 	if (dev->vbi_dev) {
 		em28xx_info("V4L2 device %s deregistered\n",
 			    video_device_node_name(dev->vbi_dev));
-		if (video_is_registered(dev->vbi_dev))
-			video_unregister_device(dev->vbi_dev);
-		else
-			video_device_release(dev->vbi_dev);
-		dev->vbi_dev = NULL;
+		video_unregister_device(dev->vbi_dev);
 	}
 	if (dev->vdev) {
 		em28xx_info("V4L2 device %s deregistered\n",
 			    video_device_node_name(dev->vdev));
-		if (video_is_registered(dev->vdev))
-			video_unregister_device(dev->vdev);
-		else
-			video_device_release(dev->vdev);
-		dev->vdev = NULL;
+		video_unregister_device(dev->vdev);
 	}
+
+	if (dev->clk) {
+		v4l2_clk_unregister_fixed(dev->clk);
+		dev->clk = NULL;
+	}
+
+	v4l2_ctrl_handler_free(&dev->ctrl_handler);
+	v4l2_device_unregister(&dev->v4l2_dev);
+
+	if (dev->users)
+		em28xx_warn("Device is open ! Memory deallocation is deferred on last close.\n");
+	mutex_unlock(&dev->lock);
+
+	return 0;
 }
 
 /*
@@ -1668,14 +1950,10 @@ static int em28xx_v4l2_close(struct file *filp)
 	mutex_lock(&dev->lock);
 
 	if (dev->users == 1) {
-		/* the device is already disconnect,
-		   free the remaining resources */
+		/* free the remaining resources if device is disconnected */
 		if (dev->disconnected) {
-			em28xx_release_resources(dev);
 			kfree(dev->alt_max_pkt_size_isoc);
-			mutex_unlock(&dev->lock);
-			kfree(dev);
-			return 0;
+			goto exit;
 		}
 
 		/* Save some power by putting tuner to sleep */
@@ -1694,11 +1972,29 @@ static int em28xx_v4l2_close(struct file *filp)
 		}
 	}
 
+exit:
 	dev->users--;
 	mutex_unlock(&dev->lock);
 	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,
@@ -1753,11 +2049,10 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 };
 
 static const struct video_device em28xx_video_template = {
-	.fops                       = &em28xx_v4l_fops,
-	.release                    = video_device_release_empty,
-	.ioctl_ops 		    = &video_ioctl_ops,
-
-	.tvnorms                    = V4L2_STD_ALL,
+	.fops		= &em28xx_v4l_fops,
+	.ioctl_ops	= &video_ioctl_ops,
+	.release	= em28xx_videodevice_release,
+	.tvnorms	= V4L2_STD_ALL,
 };
 
 static const struct v4l2_file_operations radio_fops = {
@@ -1783,14 +2078,30 @@ static const struct v4l2_ioctl_ops radio_ioctl_ops = {
 };
 
 static struct video_device em28xx_radio_template = {
-	.name                 = "em28xx-radio",
-	.fops                 = &radio_fops,
-	.ioctl_ops 	      = &radio_ioctl_ops,
+	.fops		= &radio_fops,
+	.ioctl_ops	= &radio_ioctl_ops,
+	.release	= em28xx_videodevice_release,
 };
 
-/******************************** usb interface ******************************/
+/* I2C possible address to saa7115, tvp5150, msp3400, tvaudio */
+static unsigned short saa711x_addrs[] = {
+	0x4a >> 1, 0x48 >> 1,   /* SAA7111, SAA7111A and SAA7113 */
+	0x42 >> 1, 0x40 >> 1,   /* SAA7114, SAA7115 and SAA7118 */
+	I2C_CLIENT_END };
 
+static unsigned short tvp5150_addrs[] = {
+	0xb8 >> 1,
+	0xba >> 1,
+	I2C_CLIENT_END
+};
 
+static unsigned short msp3400_addrs[] = {
+	0x80 >> 1,
+	0x88 >> 1,
+	I2C_CLIENT_END
+};
+
+/******************************** usb interface ******************************/
 
 static struct video_device *em28xx_vdev_init(struct em28xx *dev,
 					const struct video_device *template,
@@ -1817,14 +2128,198 @@ static struct video_device *em28xx_vdev_init(struct em28xx *dev,
 	return vfd;
 }
 
-int em28xx_register_analog_devices(struct em28xx *dev)
+static void em28xx_tuner_setup(struct em28xx *dev)
+{
+	struct tuner_setup           tun_setup;
+	struct v4l2_frequency        f;
+
+	if (dev->tuner_type == TUNER_ABSENT)
+		return;
+
+	memset(&tun_setup, 0, sizeof(tun_setup));
+
+	tun_setup.mode_mask = T_ANALOG_TV | T_RADIO;
+	tun_setup.tuner_callback = em28xx_tuner_callback;
+
+	if (dev->board.radio.type) {
+		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);
+	}
+
+	if ((dev->tuner_type != TUNER_ABSENT) && (dev->tuner_type)) {
+		tun_setup.type   = dev->tuner_type;
+		tun_setup.addr   = dev->tuner_addr;
+
+		v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_type_addr, &tun_setup);
+	}
+
+	if (dev->tda9887_conf) {
+		struct v4l2_priv_tun_config tda9887_cfg;
+
+		tda9887_cfg.tuner = TUNER_TDA9887;
+		tda9887_cfg.priv = &dev->tda9887_conf;
+
+		v4l2_device_call_all(&dev->v4l2_dev, 0, tuner, s_config, &tda9887_cfg);
+	}
+
+	if (dev->tuner_type == TUNER_XC2028) {
+		struct v4l2_priv_tun_config  xc2028_cfg;
+		struct xc2028_ctrl           ctl;
+
+		memset(&xc2028_cfg, 0, sizeof(xc2028_cfg));
+		memset(&ctl, 0, sizeof(ctl));
+
+		em28xx_setup_xc3028(dev, &ctl);
+
+		xc2028_cfg.tuner = TUNER_XC2028;
+		xc2028_cfg.priv  = &ctl;
+
+		v4l2_device_call_all(&dev->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);
+}
+
+static int em28xx_v4l2_init(struct em28xx *dev)
 {
 	u8 val;
 	int ret;
 	unsigned int maxw;
+	struct v4l2_ctrl_handler *hdl = &dev->ctrl_handler;
+
+	if (dev->is_audio_only) {
+		/* Shouldn't initialize IR for this interface */
+		return 0;
+	}
+
+	if (!dev->has_video) {
+		/* This device does not support the v4l2 extension */
+		return 0;
+	}
+
+	em28xx_info("Registering V4L2 extension\n");
+
+	mutex_lock(&dev->lock);
+
+	ret = v4l2_device_register(&dev->udev->dev, &dev->v4l2_dev);
+	if (ret < 0) {
+		em28xx_errdev("Call to v4l2_device_register() failed!\n");
+		goto err;
+	}
+
+	v4l2_ctrl_handler_init(hdl, 8);
+	dev->v4l2_dev.ctrl_handler = hdl;
+
+	/*
+	 * Default format, used for tvp5150 or saa711x output formats
+	 */
+	dev->vinmode = 0x10;
+	dev->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);
+
+	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);
+
+	if (dev->board.decoder == EM28XX_TVP5150)
+		v4l2_i2c_new_subdev(&dev->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);
+
+	/* Initialize tuner and camera */
+
+	if (dev->board.tuner_type != TUNER_ABSENT) {
+		int has_demod = (dev->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);
+
+		if (has_demod)
+			v4l2_i2c_new_subdev(&dev->v4l2_dev,
+				&dev->i2c_adap[dev->def_i2c_bus], "tuner",
+				0, v4l2_i2c_tuner_addrs(ADDRS_DEMOD));
+		if (dev->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,
+				&dev->i2c_adap[dev->def_i2c_bus], "tuner",
+				0, v4l2_i2c_tuner_addrs(type));
+
+			if (sd)
+				dev->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);
+		}
+	}
+
+	em28xx_tuner_setup(dev);
+	em28xx_init_camera(dev);
+
+	/* Configure audio */
+	ret = em28xx_audio_setup(dev);
+	if (ret < 0) {
+		em28xx_errdev("%s: Error while setting audio - error [%d]!\n",
+			__func__, ret);
+		goto unregister_dev;
+	}
+	if (dev->audio_mode.ac97 != EM28XX_NO_AC97) {
+		v4l2_ctrl_new_std(hdl, &em28xx_ctrl_ops,
+			V4L2_CID_AUDIO_MUTE, 0, 1, 1, 1);
+		v4l2_ctrl_new_std(hdl, &em28xx_ctrl_ops,
+			V4L2_CID_AUDIO_VOLUME, 0, 0x1f, 1, 0x1f);
+	} else {
+		/* install the em28xx notify callback */
+		v4l2_ctrl_notify(v4l2_ctrl_find(hdl, V4L2_CID_AUDIO_MUTE),
+				em28xx_ctrl_notify, dev);
+		v4l2_ctrl_notify(v4l2_ctrl_find(hdl, V4L2_CID_AUDIO_VOLUME),
+				em28xx_ctrl_notify, dev);
+	}
+
+	/* wake i2c devices */
+	em28xx_wake_i2c(dev);
+
+	/* init video dma queues */
+	INIT_LIST_HEAD(&dev->vidq.active);
+	INIT_LIST_HEAD(&dev->vbiq.active);
 
-	printk(KERN_INFO "%s: v4l2 driver version %s\n",
-		dev->name, EM28XX_VERSION);
+	if (dev->board.has_msp34xx) {
+		/* Send a reset to other chips via gpio */
+		ret = em28xx_write_reg(dev, EM2820_R08_GPIO_CTRL, 0xf7);
+		if (ret < 0) {
+			em28xx_errdev("%s: em28xx_write_reg - msp34xx(1) failed! error [%d]\n",
+				      __func__, ret);
+			goto unregister_dev;
+		}
+		msleep(3);
+
+		ret = em28xx_write_reg(dev, EM2820_R08_GPIO_CTRL, 0xff);
+		if (ret < 0) {
+			em28xx_errdev("%s: em28xx_write_reg - msp34xx(2) failed! error [%d]\n",
+				      __func__, ret);
+			goto unregister_dev;
+		}
+		msleep(3);
+	}
 
 	/* set default norm */
 	dev->norm = V4L2_STD_PAL;
@@ -1888,14 +2383,16 @@ int em28xx_register_analog_devices(struct em28xx *dev)
 	/* Reset image controls */
 	em28xx_colorlevels_set_default(dev);
 	v4l2_ctrl_handler_setup(&dev->ctrl_handler);
-	if (dev->ctrl_handler.error)
-		return dev->ctrl_handler.error;
+	ret = dev->ctrl_handler.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) {
 		em28xx_errdev("cannot allocate video_device.\n");
-		return -ENODEV;
+		ret = -ENODEV;
+		goto unregister_dev;
 	}
 	dev->vdev->queue = &dev->vb_vidq;
 	dev->vdev->queue->lock = &dev->vb_queue_lock;
@@ -1925,7 +2422,7 @@ int em28xx_register_analog_devices(struct em28xx *dev)
 	if (ret) {
 		em28xx_errdev("unable to register video device (error=%i).\n",
 			      ret);
-		return ret;
+		goto unregister_dev;
 	}
 
 	/* Allocate and fill vbi video_device struct */
@@ -1954,7 +2451,7 @@ int em28xx_register_analog_devices(struct em28xx *dev)
 					    vbi_nr[dev->devno]);
 		if (ret < 0) {
 			em28xx_errdev("unable to register vbi device\n");
-			return ret;
+			goto unregister_dev;
 		}
 	}
 
@@ -1963,13 +2460,14 @@ int em28xx_register_analog_devices(struct em28xx *dev)
 						  "radio");
 		if (!dev->radio_dev) {
 			em28xx_errdev("cannot allocate video_device.\n");
-			return -ENODEV;
+			ret = -ENODEV;
+			goto unregister_dev;
 		}
 		ret = video_register_device(dev->radio_dev, VFL_TYPE_RADIO,
 					    radio_nr[dev->devno]);
 		if (ret < 0) {
 			em28xx_errdev("can't register radio device\n");
-			return ret;
+			goto unregister_dev;
 		}
 		em28xx_info("Registered radio device as %s\n",
 			    video_device_node_name(dev->radio_dev));
@@ -1982,5 +2480,41 @@ int em28xx_register_analog_devices(struct em28xx *dev)
 		em28xx_info("V4L2 VBI device registered as %s\n",
 			    video_device_node_name(dev->vbi_dev));
 
+	/* Save some power by putting tuner to sleep */
+	v4l2_device_call_all(&dev->v4l2_dev, 0, core, s_power, 0);
+
+	/* initialize videobuf2 stuff */
+	em28xx_vb2_setup(dev);
+
+	em28xx_info("V4L2 extension successfully initialized\n");
+
+	mutex_unlock(&dev->lock);
 	return 0;
+
+unregister_dev:
+	v4l2_ctrl_handler_free(&dev->ctrl_handler);
+	v4l2_device_unregister(&dev->v4l2_dev);
+err:
+	mutex_unlock(&dev->lock);
+	return ret;
+}
+
+static struct em28xx_ops v4l2_ops = {
+	.id   = EM28XX_V4L2,
+	.name = "Em28xx v4l2 Extension",
+	.init = em28xx_v4l2_init,
+	.fini = em28xx_v4l2_fini,
+};
+
+static int __init em28xx_video_register(void)
+{
+	return em28xx_register_extension(&v4l2_ops);
+}
+
+static void __exit em28xx_video_unregister(void)
+{
+	em28xx_unregister_extension(&v4l2_ops);
 }
+
+module_init(em28xx_video_register);
+module_exit(em28xx_video_unregister);
diff --git a/drivers/media/usb/em28xx/em28xx.h b/drivers/media/usb/em28xx/em28xx.h
index f8726ad5d0a8..32d8a4bb7961 100644
--- a/drivers/media/usb/em28xx/em28xx.h
+++ b/drivers/media/usb/em28xx/em28xx.h
@@ -26,6 +26,9 @@
 #ifndef _EM28XX_H
 #define _EM28XX_H
 
+#define EM28XX_VERSION "0.2.1"
+#define DRIVER_DESC    "Empia em28xx device driver"
+
 #include <linux/workqueue.h>
 #include <linux/i2c.h>
 #include <linux/mutex.h>
@@ -132,6 +135,8 @@
 #define EM2884_BOARD_C3TECH_DIGITAL_DUO		  88
 #define EM2874_BOARD_DELOCK_61959		  89
 #define EM2874_BOARD_KWORLD_UB435Q_V2		  90
+#define EM2765_BOARD_SPEEDLINK_VAD_LAPLACE	  91
+#define EM28178_BOARD_PCTV_461E                   92
 
 /* Limits minimum and default number of buffers */
 #define EM28XX_MIN_BUF 4
@@ -178,8 +183,27 @@
 
 #define EM28XX_INTERLACED_DEFAULT 1
 
-/* time in msecs to wait for i2c writes to finish */
-#define EM2800_I2C_XFER_TIMEOUT		20
+/*
+ * Time in msecs to wait for i2c xfers to finish.
+ * 35ms is the maximum time a SMBUS device could wait when
+ * clock stretching is used. As the transfer itself will take
+ * some time to happen, set it to 35 ms.
+ *
+ * Ok, I2C doesn't specify any limit. So, eventually, we may need
+ * to increase this timeout.
+ *
+ * FIXME: this assumes that an I2C message is not longer than 1ms.
+ * This is actually dependent on the I2C bus speed, although most
+ * devices use a 100kHz clock. So, this assumtion is true most of
+ * the time.
+ */
+#define EM28XX_I2C_XFER_TIMEOUT		36
+
+/* time in msecs to wait for AC97 xfers to finish */
+#define EM28XX_AC97_XFER_TIMEOUT	100
+
+/* max. number of button state polling addresses */
+#define EM28XX_NUM_BUTTON_ADDRESSES_MAX		5
 
 enum em28xx_mode {
 	EM28XX_SUSPEND,
@@ -287,8 +311,7 @@ struct em28xx_audio_mode {
 
 	unsigned int has_audio:1;
 
-	unsigned int i2s_3rates:1;
-	unsigned int i2s_5rates:1;
+	u8 i2s_samplerates;
 };
 
 /* em28xx has two audio inputs: tuner and line in.
@@ -374,6 +397,33 @@ enum em28xx_adecoder {
 	EM28XX_TVAUDIO,
 };
 
+enum em28xx_led_role {
+	EM28XX_LED_ANALOG_CAPTURING = 0,
+	EM28XX_LED_ILLUMINATION,
+	EM28XX_NUM_LED_ROLES, /* must be the last */
+};
+
+struct em28xx_led {
+	enum em28xx_led_role role;
+	u8 gpio_reg;
+	u8 gpio_mask;
+	bool inverted;
+};
+
+enum em28xx_button_role {
+	EM28XX_BUTTON_SNAPSHOT = 0,
+	EM28XX_BUTTON_ILLUMINATION,
+	EM28XX_NUM_BUTTON_ROLES, /* must be the last */
+};
+
+struct em28xx_button {
+	enum em28xx_button_role role;
+	u8 reg_r;
+	u8 reg_clearing;
+	u8 mask;
+	bool inverted;
+};
+
 struct em28xx_board {
 	char *name;
 	int vchannels;
@@ -395,7 +445,6 @@ struct em28xx_board {
 	unsigned int mts_firmware:1;
 	unsigned int max_range_640_480:1;
 	unsigned int has_dvb:1;
-	unsigned int has_snapshot_button:1;
 	unsigned int is_webcam:1;
 	unsigned int valid:1;
 	unsigned int has_ir_i2c:1;
@@ -410,6 +459,12 @@ struct em28xx_board {
 	struct em28xx_input       input[MAX_EM28XX_INPUT];
 	struct em28xx_input	  radio;
 	char			  *ir_codes;
+
+	/* LEDs that need to be controlled explicitly */
+	struct em28xx_led	  *leds;
+
+	/* Buttons */
+	struct em28xx_button	  *buttons;
 };
 
 struct em28xx_eeprom {
@@ -426,15 +481,13 @@ struct em28xx_eeprom {
 	u8 string_idx_table;
 };
 
-#define EM28XX_AUDIO_BUFS 5
-#define EM28XX_NUM_AUDIO_PACKETS 64
-#define EM28XX_AUDIO_MAX_PACKET_SIZE 196 /* static value */
 #define EM28XX_CAPTURE_STREAM_EN 1
 
 /* em28xx extensions */
 #define EM28XX_AUDIO   0x10
 #define EM28XX_DVB     0x20
 #define EM28XX_RC      0x30
+#define EM28XX_V4L2    0x40
 
 /* em28xx resource types (used for res_get/res_lock etc */
 #define EM28XX_RESOURCE_VIDEO 0x01
@@ -442,8 +495,9 @@ struct em28xx_eeprom {
 
 struct em28xx_audio {
 	char name[50];
-	char *transfer_buffer[EM28XX_AUDIO_BUFS];
-	struct urb *urb[EM28XX_AUDIO_BUFS];
+	unsigned num_urb;
+	char **transfer_buffer;
+	struct urb **urb;
 	struct usb_device *udev;
 	unsigned int capture_transfer_done;
 	struct snd_pcm_substream   *capture_pcm_substream;
@@ -451,6 +505,8 @@ struct em28xx_audio {
 	unsigned int hwptr_done_capture;
 	struct snd_card            *sndcard;
 
+	size_t period;
+
 	int users;
 	spinlock_t slock;
 };
@@ -485,11 +541,13 @@ struct em28xx {
 	int model;		/* index in the device_data struct */
 	int devno;		/* marks the number of this device */
 	enum em28xx_chip_id chip_id;
-	unsigned int is_em25xx:1;	/* em25xx/em276x/7x/8x family bridge */
 
+	unsigned int is_em25xx:1;	/* em25xx/em276x/7x/8x family bridge */
 	unsigned char disconnected:1;	/* device has been diconnected */
-
-	int audio_ifnum;
+	unsigned int has_video:1;
+	unsigned int has_audio_class:1;
+	unsigned int has_alsa_audio:1;
+	unsigned int is_audio_only:1;
 
 	struct v4l2_device v4l2_dev;
 	struct v4l2_ctrl_handler ctrl_handler;
@@ -507,10 +565,6 @@ struct em28xx {
 	/* Vinmode/Vinctl used at the driver */
 	int vinmode, vinctl;
 
-	unsigned int has_audio_class:1;
-	unsigned int has_alsa_audio:1;
-	unsigned int is_audio_only:1;
-
 	/* 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 */
@@ -608,6 +662,7 @@ struct em28xx {
 
 	/* usb transfer */
 	struct usb_device *udev;	/* the usb device */
+	u8 ifnum;		/* number of the assigned usb interface */
 	u8 analog_ep_isoc;	/* address of isoc endpoint for analog */
 	u8 analog_ep_bulk;	/* address of bulk endpoint for analog */
 	u8 dvb_ep_isoc;		/* address of isoc endpoint for DVB */
@@ -639,10 +694,15 @@ struct em28xx {
 
 	enum em28xx_mode mode;
 
-	/* Snapshot button */
+	/* Button state polling */
+	struct delayed_work buttons_query_work;
+	u8 button_polling_addresses[EM28XX_NUM_BUTTON_ADDRESSES_MAX];
+	u8 button_polling_last_values[EM28XX_NUM_BUTTON_ADDRESSES_MAX];
+	u8 num_button_polling_addresses;
+	u16 button_polling_interval; /* [ms] */
+	/* Snapshot button input device */
 	char snapshot_button_path[30];	/* path of the input dev */
 	struct input_dev *sbutton_input_dev;
-	struct delayed_work sbutton_query_work;
 
 	struct em28xx_dvb *dvb;
 };
@@ -672,6 +732,7 @@ int em28xx_write_regs(struct em28xx *dev, u16 reg, char *buf, int len);
 int em28xx_write_reg(struct em28xx *dev, u16 reg, u8 val);
 int em28xx_write_reg_bits(struct em28xx *dev, u16 reg, u8 val,
 				 u8 bitmask);
+int em28xx_toggle_reg_bits(struct em28xx *dev, u16 reg, u8 bitmask);
 
 int em28xx_read_ac97(struct em28xx *dev, u8 reg);
 int em28xx_write_ac97(struct em28xx *dev, u8 reg, u16 val);
@@ -679,12 +740,9 @@ int em28xx_write_ac97(struct em28xx *dev, u8 reg, u16 val);
 int em28xx_audio_analog_set(struct em28xx *dev);
 int em28xx_audio_setup(struct em28xx *dev);
 
-int em28xx_colorlevels_set_default(struct em28xx *dev);
+const struct em28xx_led *em28xx_find_led(struct em28xx *dev,
+					 enum em28xx_led_role role);
 int em28xx_capture_start(struct em28xx *dev, int start);
-int em28xx_vbi_supported(struct em28xx *dev);
-int em28xx_set_outfmt(struct em28xx *dev);
-int em28xx_resolution_set(struct em28xx *dev);
-int em28xx_set_alternate(struct em28xx *dev);
 int em28xx_alloc_urbs(struct em28xx *dev, enum em28xx_mode mode, int xfer_bulk,
 		      int num_bufs, int max_pkt_size, int packet_multiplier);
 int em28xx_init_usb_xfer(struct em28xx *dev, enum em28xx_mode mode,
@@ -696,30 +754,18 @@ void em28xx_uninit_usb_xfer(struct em28xx *dev, enum em28xx_mode mode);
 void em28xx_stop_urbs(struct em28xx *dev);
 int em28xx_set_mode(struct em28xx *dev, enum em28xx_mode set_mode);
 int em28xx_gpio_set(struct em28xx *dev, struct em28xx_reg_seq *gpio);
-void em28xx_wake_i2c(struct em28xx *dev);
 int em28xx_register_extension(struct em28xx_ops *dev);
 void em28xx_unregister_extension(struct em28xx_ops *dev);
 void em28xx_init_extension(struct em28xx *dev);
 void em28xx_close_extension(struct em28xx *dev);
 
-/* Provided by em28xx-video.c */
-int em28xx_vb2_setup(struct em28xx *dev);
-int em28xx_register_analog_devices(struct em28xx *dev);
-void em28xx_release_analog_resources(struct em28xx *dev);
-void em28xx_ctrl_notify(struct v4l2_ctrl *ctrl, void *priv);
-int em28xx_start_analog_streaming(struct vb2_queue *vq, unsigned int count);
-int em28xx_stop_vbi_streaming(struct vb2_queue *vq);
-extern const struct v4l2_ctrl_ops em28xx_ctrl_ops;
-
 /* Provided by em28xx-cards.c */
 extern struct em28xx_board em28xx_boards[];
 extern struct usb_device_id em28xx_id_table[];
 int em28xx_tuner_callback(void *ptr, int component, int command, int arg);
+void em28xx_setup_xc3028(struct em28xx *dev, struct xc2028_ctrl *ctl);
 void em28xx_release_resources(struct em28xx *dev);
 
-/* Provided by em28xx-vbi.c */
-extern struct vb2_ops em28xx_vbi_qops;
-
 /* Provided by em28xx-camera.c */
 int em28xx_detect_sensor(struct em28xx *dev);
 int em28xx_init_camera(struct em28xx *dev);
diff --git a/drivers/media/usb/hdpvr/hdpvr-core.c b/drivers/media/usb/hdpvr/hdpvr-core.c
index 2f0c89cbac76..c5638964c3f2 100644
--- a/drivers/media/usb/hdpvr/hdpvr-core.c
+++ b/drivers/media/usb/hdpvr/hdpvr-core.c
@@ -198,7 +198,6 @@ static int device_authorization(struct hdpvr_device *dev)
 	hex_dump_to_buffer(response, 8, 16, 1, print_buf, 5*buf_size+1, 0);
 	v4l2_dbg(MSG_INFO, hdpvr_debug, &dev->v4l2_dev, " response: %s\n",
 		 print_buf);
-	kfree(print_buf);
 #endif
 
 	msleep(100);
@@ -214,6 +213,9 @@ static int device_authorization(struct hdpvr_device *dev)
 	retval = ret != 8;
 unlock:
 	mutex_unlock(&dev->usbc_mutex);
+#ifdef HDPVR_DEBUG
+	kfree(print_buf);
+#endif
 	return retval;
 }
 
diff --git a/drivers/media/usb/pwc/pwc-if.c b/drivers/media/usb/pwc/pwc-if.c
index 78c9bc8e7f56..abf365ab025d 100644
--- a/drivers/media/usb/pwc/pwc-if.c
+++ b/drivers/media/usb/pwc/pwc-if.c
@@ -1078,7 +1078,6 @@ static int usb_pwc_probe(struct usb_interface *intf, const struct usb_device_id
 	/* register webcam snapshot button input device */
 	pdev->button_dev = input_allocate_device();
 	if (!pdev->button_dev) {
-		PWC_ERROR("Err, insufficient memory for webcam snapshot button device.");
 		rc = -ENOMEM;
 		goto err_video_unreg;
 	}
diff --git a/drivers/media/v4l2-core/Kconfig b/drivers/media/v4l2-core/Kconfig
index 8c05565a240e..2189bfb2e828 100644
--- a/drivers/media/v4l2-core/Kconfig
+++ b/drivers/media/v4l2-core/Kconfig
@@ -83,14 +83,3 @@ config VIDEOBUF2_DMA_SG
 	#depends on HAS_DMA
 	select VIDEOBUF2_CORE
 	select VIDEOBUF2_MEMOPS
-
-config VIDEO_V4L2_INT_DEVICE
-	tristate "V4L2 int device (DEPRECATED)"
-	depends on VIDEO_V4L2
-	---help---
-	  An early framework for a hardware-independent interface for
-	  image sensors and bridges etc. Currently used by omap24xxcam and
-	  tcm825x drivers that should be converted to V4L2 subdev.
-
-	  Do not use for new developments.
-
diff --git a/drivers/media/v4l2-core/Makefile b/drivers/media/v4l2-core/Makefile
index 1a85eee581f8..c6ae7bad951e 100644
--- a/drivers/media/v4l2-core/Makefile
+++ b/drivers/media/v4l2-core/Makefile
@@ -15,7 +15,6 @@ ifeq ($(CONFIG_OF),y)
 endif
 
 obj-$(CONFIG_VIDEO_V4L2) += videodev.o
-obj-$(CONFIG_VIDEO_V4L2_INT_DEVICE) += v4l2-int-device.o
 obj-$(CONFIG_VIDEO_V4L2) += v4l2-common.o
 obj-$(CONFIG_VIDEO_V4L2) += v4l2-dv-timings.o
 
diff --git a/drivers/media/v4l2-core/v4l2-ctrls.c b/drivers/media/v4l2-core/v4l2-ctrls.c
index fb46790d0eca..6ff002bd5909 100644
--- a/drivers/media/v4l2-core/v4l2-ctrls.c
+++ b/drivers/media/v4l2-core/v4l2-ctrls.c
@@ -745,6 +745,11 @@ const char *v4l2_ctrl_get_name(u32 id)
 	case V4L2_CID_MPEG_VIDEO_VPX_FILTER_SHARPNESS:		return "VPX Deblocking Effect Control";
 	case V4L2_CID_MPEG_VIDEO_VPX_GOLDEN_FRAME_REF_PERIOD:	return "VPX Golden Frame Refresh Period";
 	case V4L2_CID_MPEG_VIDEO_VPX_GOLDEN_FRAME_SEL:		return "VPX Golden Frame Indicator";
+	case V4L2_CID_MPEG_VIDEO_VPX_MIN_QP:			return "VPX Minimum QP Value";
+	case V4L2_CID_MPEG_VIDEO_VPX_MAX_QP:			return "VPX Maximum QP Value";
+	case V4L2_CID_MPEG_VIDEO_VPX_I_FRAME_QP:		return "VPX I-Frame QP Value";
+	case V4L2_CID_MPEG_VIDEO_VPX_P_FRAME_QP:		return "VPX P-Frame QP Value";
+	case V4L2_CID_MPEG_VIDEO_VPX_PROFILE:			return "VPX Profile";
 
 	/* CAMERA controls */
 	/* Keep the order of the 'case's the same as in videodev2.h! */
diff --git a/drivers/media/v4l2-core/v4l2-dev.c b/drivers/media/v4l2-core/v4l2-dev.c
index b5aaaac427ad..0a30dbf3d05c 100644
--- a/drivers/media/v4l2-core/v4l2-dev.c
+++ b/drivers/media/v4l2-core/v4l2-dev.c
@@ -872,8 +872,8 @@ int __video_register_device(struct video_device *vdev, int type, int nr,
 
 	/* Should not happen since we thought this minor was free */
 	WARN_ON(video_device[vdev->minor] != NULL);
-	video_device[vdev->minor] = vdev;
 	vdev->index = get_index(vdev);
+	video_device[vdev->minor] = vdev;
 	mutex_unlock(&videodev_lock);
 
 	if (vdev->ioctl_ops)
diff --git a/drivers/media/v4l2-core/v4l2-dv-timings.c b/drivers/media/v4l2-core/v4l2-dv-timings.c
index ee52b9f4a944..f7902fe8a526 100644
--- a/drivers/media/v4l2-core/v4l2-dv-timings.c
+++ b/drivers/media/v4l2-core/v4l2-dv-timings.c
@@ -515,6 +515,7 @@ bool v4l2_detect_gtf(unsigned frame_height,
 		aspect.denominator = 9;
 	}
 	image_width = ((image_height * aspect.numerator) / aspect.denominator);
+	image_width = (image_width + GTF_CELL_GRAN/2) & ~(GTF_CELL_GRAN - 1);
 
 	/* Horizontal */
 	if (default_gtf)
diff --git a/drivers/media/v4l2-core/v4l2-ioctl.c b/drivers/media/v4l2-core/v4l2-ioctl.c
index 68e6b5e912ff..707aef705a47 100644
--- a/drivers/media/v4l2-core/v4l2-ioctl.c
+++ b/drivers/media/v4l2-core/v4l2-ioctl.c
@@ -28,6 +28,9 @@
 #include <media/v4l2-device.h>
 #include <media/videobuf2-core.h>
 
+#define CREATE_TRACE_POINTS
+#include <trace/events/v4l2.h>
+
 /* Zero out the end of the struct pointed to by p.  Everything after, but
  * not including, the specified field is cleared. */
 #define CLEAR_AFTER_FIELD(p, field) \
@@ -2338,6 +2341,12 @@ video_usercopy(struct file *file, unsigned int cmd, unsigned long arg,
 	err = func(file, cmd, parg);
 	if (err == -ENOIOCTLCMD)
 		err = -ENOTTY;
+	if (err == 0) {
+		if (cmd == VIDIOC_DQBUF)
+			trace_v4l2_dqbuf(video_devdata(file)->minor, parg);
+		else if (cmd == VIDIOC_QBUF)
+			trace_v4l2_qbuf(video_devdata(file)->minor, parg);
+	}
 
 	if (has_array_args) {
 		*kernel_ptr = user_ptr;
diff --git a/drivers/media/v4l2-core/v4l2-mem2mem.c b/drivers/media/v4l2-core/v4l2-mem2mem.c
index 73035ee0f4de..178ce96556c6 100644
--- a/drivers/media/v4l2-core/v4l2-mem2mem.c
+++ b/drivers/media/v4l2-core/v4l2-mem2mem.c
@@ -558,6 +558,8 @@ unsigned int v4l2_m2m_poll(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 
 	if (m2m_ctx->m2m_dev->m2m_ops->unlock)
 		m2m_ctx->m2m_dev->m2m_ops->unlock(m2m_ctx->priv);
+	else if (m2m_ctx->q_lock)
+		mutex_unlock(m2m_ctx->q_lock);
 
 	if (list_empty(&src_q->done_list))
 		poll_wait(file, &src_q->done_wq, wait);
@@ -566,6 +568,8 @@ unsigned int v4l2_m2m_poll(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 
 	if (m2m_ctx->m2m_dev->m2m_ops->lock)
 		m2m_ctx->m2m_dev->m2m_ops->lock(m2m_ctx->priv);
+	else if (m2m_ctx->q_lock)
+		mutex_lock(m2m_ctx->q_lock);
 
 	spin_lock_irqsave(&src_q->done_lock, flags);
 	if (!list_empty(&src_q->done_list))
@@ -693,6 +697,13 @@ struct v4l2_m2m_ctx *v4l2_m2m_ctx_init(struct v4l2_m2m_dev *m2m_dev,
 
 	if (ret)
 		goto err;
+	/*
+	 * If both queues use same mutex assign it as the common buffer
+	 * queues lock to the m2m context. This lock is used in the
+	 * v4l2_m2m_ioctl_* helpers.
+	 */
+	if (out_q_ctx->q.lock == cap_q_ctx->q.lock)
+		m2m_ctx->q_lock = out_q_ctx->q.lock;
 
 	return m2m_ctx;
 err:
@@ -740,3 +751,118 @@ void v4l2_m2m_buf_queue(struct v4l2_m2m_ctx *m2m_ctx, struct vb2_buffer *vb)
 }
 EXPORT_SYMBOL_GPL(v4l2_m2m_buf_queue);
 
+/* Videobuf2 ioctl helpers */
+
+int v4l2_m2m_ioctl_reqbufs(struct file *file, void *priv,
+				struct v4l2_requestbuffers *rb)
+{
+	struct v4l2_fh *fh = file->private_data;
+
+	return v4l2_m2m_reqbufs(file, fh->m2m_ctx, rb);
+}
+EXPORT_SYMBOL_GPL(v4l2_m2m_ioctl_reqbufs);
+
+int v4l2_m2m_ioctl_create_bufs(struct file *file, void *priv,
+				struct v4l2_create_buffers *create)
+{
+	struct v4l2_fh *fh = file->private_data;
+
+	return v4l2_m2m_create_bufs(file, fh->m2m_ctx, create);
+}
+EXPORT_SYMBOL_GPL(v4l2_m2m_ioctl_create_bufs);
+
+int v4l2_m2m_ioctl_querybuf(struct file *file, void *priv,
+				struct v4l2_buffer *buf)
+{
+	struct v4l2_fh *fh = file->private_data;
+
+	return v4l2_m2m_querybuf(file, fh->m2m_ctx, buf);
+}
+EXPORT_SYMBOL_GPL(v4l2_m2m_ioctl_querybuf);
+
+int v4l2_m2m_ioctl_qbuf(struct file *file, void *priv,
+				struct v4l2_buffer *buf)
+{
+	struct v4l2_fh *fh = file->private_data;
+
+	return v4l2_m2m_qbuf(file, fh->m2m_ctx, buf);
+}
+EXPORT_SYMBOL_GPL(v4l2_m2m_ioctl_qbuf);
+
+int v4l2_m2m_ioctl_dqbuf(struct file *file, void *priv,
+				struct v4l2_buffer *buf)
+{
+	struct v4l2_fh *fh = file->private_data;
+
+	return v4l2_m2m_dqbuf(file, fh->m2m_ctx, buf);
+}
+EXPORT_SYMBOL_GPL(v4l2_m2m_ioctl_dqbuf);
+
+int v4l2_m2m_ioctl_expbuf(struct file *file, void *priv,
+				struct v4l2_exportbuffer *eb)
+{
+	struct v4l2_fh *fh = file->private_data;
+
+	return v4l2_m2m_expbuf(file, fh->m2m_ctx, eb);
+}
+EXPORT_SYMBOL_GPL(v4l2_m2m_ioctl_expbuf);
+
+int v4l2_m2m_ioctl_streamon(struct file *file, void *priv,
+				enum v4l2_buf_type type)
+{
+	struct v4l2_fh *fh = file->private_data;
+
+	return v4l2_m2m_streamon(file, fh->m2m_ctx, type);
+}
+EXPORT_SYMBOL_GPL(v4l2_m2m_ioctl_streamon);
+
+int v4l2_m2m_ioctl_streamoff(struct file *file, void *priv,
+				enum v4l2_buf_type type)
+{
+	struct v4l2_fh *fh = file->private_data;
+
+	return v4l2_m2m_streamoff(file, fh->m2m_ctx, type);
+}
+EXPORT_SYMBOL_GPL(v4l2_m2m_ioctl_streamoff);
+
+/*
+ * v4l2_file_operations helpers. It is assumed here same lock is used
+ * for the output and the capture buffer queue.
+ */
+
+int v4l2_m2m_fop_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct v4l2_fh *fh = file->private_data;
+	struct v4l2_m2m_ctx *m2m_ctx = fh->m2m_ctx;
+	int ret;
+
+	if (m2m_ctx->q_lock && mutex_lock_interruptible(m2m_ctx->q_lock))
+		return -ERESTARTSYS;
+
+	ret = v4l2_m2m_mmap(file, m2m_ctx, vma);
+
+	if (m2m_ctx->q_lock)
+		mutex_unlock(m2m_ctx->q_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(v4l2_m2m_fop_mmap);
+
+unsigned int v4l2_m2m_fop_poll(struct file *file, poll_table *wait)
+{
+	struct v4l2_fh *fh = file->private_data;
+	struct v4l2_m2m_ctx *m2m_ctx = fh->m2m_ctx;
+	unsigned int ret;
+
+	if (m2m_ctx->q_lock)
+		mutex_lock(m2m_ctx->q_lock);
+
+	ret = v4l2_m2m_poll(file, m2m_ctx, wait);
+
+	if (m2m_ctx->q_lock)
+		mutex_unlock(m2m_ctx->q_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(v4l2_m2m_fop_poll);
+
diff --git a/drivers/media/v4l2-core/v4l2-of.c b/drivers/media/v4l2-core/v4l2-of.c
index a6478dca0cde..42e3e8a5e361 100644
--- a/drivers/media/v4l2-core/v4l2-of.c
+++ b/drivers/media/v4l2-core/v4l2-of.c
@@ -121,9 +121,11 @@ static void v4l2_of_parse_parallel_bus(const struct device_node *node,
  * the bus as serial CSI-2 and clock-noncontinuous isn't set, we set the
  * V4L2_MBUS_CSI2_CONTINUOUS_CLOCK flag.
  * The caller should hold a reference to @node.
+ *
+ * Return: 0.
  */
-void v4l2_of_parse_endpoint(const struct device_node *node,
-			    struct v4l2_of_endpoint *endpoint)
+int v4l2_of_parse_endpoint(const struct device_node *node,
+			   struct v4l2_of_endpoint *endpoint)
 {
 	struct device_node *port_node = of_get_parent(node);
 
@@ -146,6 +148,8 @@ void v4l2_of_parse_endpoint(const struct device_node *node,
 		v4l2_of_parse_parallel_bus(node, endpoint);
 
 	of_node_put(port_node);
+
+	return 0;
 }
 EXPORT_SYMBOL(v4l2_of_parse_endpoint);
 
@@ -262,6 +266,6 @@ struct device_node *v4l2_of_get_remote_port(const struct device_node *node)
 	np = of_parse_phandle(node, "remote-endpoint", 0);
 	if (!np)
 		return NULL;
-	return of_get_parent(np);
+	return of_get_next_parent(np);
 }
 EXPORT_SYMBOL(v4l2_of_get_remote_port);
diff --git a/drivers/media/v4l2-core/videobuf-dma-contig.c b/drivers/media/v4l2-core/videobuf-dma-contig.c
index 65411adcd0ea..7e6b209b7002 100644
--- a/drivers/media/v4l2-core/videobuf-dma-contig.c
+++ b/drivers/media/v4l2-core/videobuf-dma-contig.c
@@ -66,14 +66,11 @@ static void __videobuf_dc_free(struct device *dev,
 static void videobuf_vm_open(struct vm_area_struct *vma)
 {
 	struct videobuf_mapping *map = vma->vm_private_data;
-	struct videobuf_queue *q = map->q;
 
-	dev_dbg(q->dev, "vm_open %p [count=%u,vma=%08lx-%08lx]\n",
+	dev_dbg(map->q->dev, "vm_open %p [count=%u,vma=%08lx-%08lx]\n",
 		map, map->count, vma->vm_start, vma->vm_end);
 
-	videobuf_queue_lock(q);
 	map->count++;
-	videobuf_queue_unlock(q);
 }
 
 static void videobuf_vm_close(struct vm_area_struct *vma)
@@ -85,11 +82,12 @@ static void videobuf_vm_close(struct vm_area_struct *vma)
 	dev_dbg(q->dev, "vm_close %p [count=%u,vma=%08lx-%08lx]\n",
 		map, map->count, vma->vm_start, vma->vm_end);
 
-	videobuf_queue_lock(q);
-	if (!--map->count) {
+	map->count--;
+	if (0 == map->count) {
 		struct videobuf_dma_contig_memory *mem;
 
 		dev_dbg(q->dev, "munmap %p q=%p\n", map, q);
+		videobuf_queue_lock(q);
 
 		/* We need first to cancel streams, before unmapping */
 		if (q->streaming)
@@ -128,8 +126,8 @@ static void videobuf_vm_close(struct vm_area_struct *vma)
 
 		kfree(map);
 
+		videobuf_queue_unlock(q);
 	}
-	videobuf_queue_unlock(q);
 }
 
 static const struct vm_operations_struct videobuf_vm_ops = {
diff --git a/drivers/media/v4l2-core/videobuf-dma-sg.c b/drivers/media/v4l2-core/videobuf-dma-sg.c
index 9db674ccdc68..828e7c10bd70 100644
--- a/drivers/media/v4l2-core/videobuf-dma-sg.c
+++ b/drivers/media/v4l2-core/videobuf-dma-sg.c
@@ -338,14 +338,11 @@ EXPORT_SYMBOL_GPL(videobuf_dma_free);
 static void videobuf_vm_open(struct vm_area_struct *vma)
 {
 	struct videobuf_mapping *map = vma->vm_private_data;
-	struct videobuf_queue *q = map->q;
 
 	dprintk(2, "vm_open %p [count=%d,vma=%08lx-%08lx]\n", map,
 		map->count, vma->vm_start, vma->vm_end);
 
-	videobuf_queue_lock(q);
 	map->count++;
-	videobuf_queue_unlock(q);
 }
 
 static void videobuf_vm_close(struct vm_area_struct *vma)
@@ -358,9 +355,10 @@ static void videobuf_vm_close(struct vm_area_struct *vma)
 	dprintk(2, "vm_close %p [count=%d,vma=%08lx-%08lx]\n", map,
 		map->count, vma->vm_start, vma->vm_end);
 
-	videobuf_queue_lock(q);
-	if (!--map->count) {
+	map->count--;
+	if (0 == map->count) {
 		dprintk(1, "munmap %p q=%p\n", map, q);
+		videobuf_queue_lock(q);
 		for (i = 0; i < VIDEO_MAX_FRAME; i++) {
 			if (NULL == q->bufs[i])
 				continue;
@@ -376,9 +374,9 @@ static void videobuf_vm_close(struct vm_area_struct *vma)
 			q->bufs[i]->baddr = 0;
 			q->ops->buf_release(q, q->bufs[i]);
 		}
+		videobuf_queue_unlock(q);
 		kfree(map);
 	}
-	videobuf_queue_unlock(q);
 	return;
 }
 
diff --git a/drivers/media/v4l2-core/videobuf-vmalloc.c b/drivers/media/v4l2-core/videobuf-vmalloc.c
index 1365c651c177..2ff7fcc77b11 100644
--- a/drivers/media/v4l2-core/videobuf-vmalloc.c
+++ b/drivers/media/v4l2-core/videobuf-vmalloc.c
@@ -54,14 +54,11 @@ MODULE_LICENSE("GPL");
 static void videobuf_vm_open(struct vm_area_struct *vma)
 {
 	struct videobuf_mapping *map = vma->vm_private_data;
-	struct videobuf_queue *q = map->q;
 
 	dprintk(2, "vm_open %p [count=%u,vma=%08lx-%08lx]\n", map,
 		map->count, vma->vm_start, vma->vm_end);
 
-	videobuf_queue_lock(q);
 	map->count++;
-	videobuf_queue_unlock(q);
 }
 
 static void videobuf_vm_close(struct vm_area_struct *vma)
@@ -73,11 +70,12 @@ static void videobuf_vm_close(struct vm_area_struct *vma)
 	dprintk(2, "vm_close %p [count=%u,vma=%08lx-%08lx]\n", map,
 		map->count, vma->vm_start, vma->vm_end);
 
-	videobuf_queue_lock(q);
-	if (!--map->count) {
+	map->count--;
+	if (0 == map->count) {
 		struct videobuf_vmalloc_memory *mem;
 
 		dprintk(1, "munmap %p q=%p\n", map, q);
+		videobuf_queue_lock(q);
 
 		/* We need first to cancel streams, before unmapping */
 		if (q->streaming)
@@ -116,8 +114,8 @@ static void videobuf_vm_close(struct vm_area_struct *vma)
 
 		kfree(map);
 
+		videobuf_queue_unlock(q);
 	}
-	videobuf_queue_unlock(q);
 
 	return;
 }
diff --git a/drivers/media/v4l2-core/videobuf2-core.c b/drivers/media/v4l2-core/videobuf2-core.c
index 0edc165f418d..a127925c9d61 100644
--- a/drivers/media/v4l2-core/videobuf2-core.c
+++ b/drivers/media/v4l2-core/videobuf2-core.c
@@ -298,10 +298,28 @@ static void __vb2_free_mem(struct vb2_queue *q, unsigned int buffers)
  * related information, if no buffers are left return the queue to an
  * uninitialized state. Might be called even if the queue has already been freed.
  */
-static void __vb2_queue_free(struct vb2_queue *q, unsigned int buffers)
+static int __vb2_queue_free(struct vb2_queue *q, unsigned int buffers)
 {
 	unsigned int buffer;
 
+	/*
+	 * Sanity check: when preparing a buffer the queue lock is released for
+	 * a short while (see __buf_prepare for the details), which would allow
+	 * a race with a reqbufs which can call this function. Removing the
+	 * buffers from underneath __buf_prepare is obviously a bad idea, so we
+	 * check if any of the buffers is in the state PREPARING, and if so we
+	 * just return -EAGAIN.
+	 */
+	for (buffer = q->num_buffers - buffers; buffer < q->num_buffers;
+	     ++buffer) {
+		if (q->bufs[buffer] == NULL)
+			continue;
+		if (q->bufs[buffer]->state == VB2_BUF_STATE_PREPARING) {
+			dprintk(1, "reqbufs: preparing buffers, cannot free\n");
+			return -EAGAIN;
+		}
+	}
+
 	/* Call driver-provided cleanup function for each buffer, if provided */
 	if (q->ops->buf_cleanup) {
 		for (buffer = q->num_buffers - buffers; buffer < q->num_buffers;
@@ -326,6 +344,7 @@ static void __vb2_queue_free(struct vb2_queue *q, unsigned int buffers)
 	if (!q->num_buffers)
 		q->memory = 0;
 	INIT_LIST_HEAD(&q->queued_list);
+	return 0;
 }
 
 /**
@@ -481,6 +500,7 @@ static void __fill_v4l2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b)
 	case VB2_BUF_STATE_PREPARED:
 		b->flags |= V4L2_BUF_FLAG_PREPARED;
 		break;
+	case VB2_BUF_STATE_PREPARING:
 	case VB2_BUF_STATE_DEQUEUED:
 		/* nothing */
 		break;
@@ -657,7 +677,9 @@ static int __reqbufs(struct vb2_queue *q, struct v4l2_requestbuffers *req)
 			return -EBUSY;
 		}
 
-		__vb2_queue_free(q, q->num_buffers);
+		ret = __vb2_queue_free(q, q->num_buffers);
+		if (ret)
+			return ret;
 
 		/*
 		 * In case of REQBUFS(0) return immediately without calling
@@ -1116,7 +1138,7 @@ static int __qbuf_dmabuf(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 	int ret;
 	int write = !V4L2_TYPE_IS_OUTPUT(q->type);
 
-	/* Verify and copy relevant information provided by the userspace */
+	/* Copy relevant information provided by the userspace */
 	__fill_vb2_buffer(vb, b, planes);
 
 	for (plane = 0; plane < vb->num_planes; ++plane) {
@@ -1135,6 +1157,8 @@ static int __qbuf_dmabuf(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 
 		if (planes[plane].length < planes[plane].data_offset +
 		    q->plane_sizes[plane]) {
+			dprintk(1, "qbuf: invalid dmabuf length for plane %d\n",
+				plane);
 			ret = -EINVAL;
 			goto err;
 		}
@@ -1226,6 +1250,7 @@ static void __enqueue_in_driver(struct vb2_buffer *vb)
 static int __buf_prepare(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 {
 	struct vb2_queue *q = vb->vb2_queue;
+	struct rw_semaphore *mmap_sem;
 	int ret;
 
 	ret = __verify_length(vb, b);
@@ -1235,12 +1260,32 @@ static int __buf_prepare(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 		return ret;
 	}
 
+	vb->state = VB2_BUF_STATE_PREPARING;
 	switch (q->memory) {
 	case V4L2_MEMORY_MMAP:
 		ret = __qbuf_mmap(vb, b);
 		break;
 	case V4L2_MEMORY_USERPTR:
+		/*
+		 * In case of user pointer buffers vb2 allocators need to get
+		 * direct access to userspace pages. This requires getting
+		 * the mmap semaphore for read access in the current process
+		 * structure. The same semaphore is taken before calling mmap
+		 * operation, while both qbuf/prepare_buf and mmap are called
+		 * by the driver or v4l2 core with the driver's lock held.
+		 * To avoid an AB-BA deadlock (mmap_sem then driver's lock in
+		 * mmap and driver's lock then mmap_sem in qbuf/prepare_buf),
+		 * the videobuf2 core releases the driver's lock, takes
+		 * mmap_sem and then takes the driver's lock again.
+		 */
+		mmap_sem = &current->mm->mmap_sem;
+		call_qop(q, wait_prepare, q);
+		down_read(mmap_sem);
+		call_qop(q, wait_finish, q);
+
 		ret = __qbuf_userptr(vb, b);
+
+		up_read(mmap_sem);
 		break;
 	case V4L2_MEMORY_DMABUF:
 		ret = __qbuf_dmabuf(vb, b);
@@ -1254,105 +1299,36 @@ static int __buf_prepare(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 		ret = call_qop(q, buf_prepare, vb);
 	if (ret)
 		dprintk(1, "qbuf: buffer preparation failed: %d\n", ret);
-	else
-		vb->state = VB2_BUF_STATE_PREPARED;
+	vb->state = ret ? VB2_BUF_STATE_DEQUEUED : VB2_BUF_STATE_PREPARED;
 
 	return ret;
 }
 
 static int vb2_queue_or_prepare_buf(struct vb2_queue *q, struct v4l2_buffer *b,
-				    const char *opname,
-				    int (*handler)(struct vb2_queue *,
-						   struct v4l2_buffer *,
-						   struct vb2_buffer *))
+				    const char *opname)
 {
-	struct rw_semaphore *mmap_sem = NULL;
-	struct vb2_buffer *vb;
-	int ret;
-
-	/*
-	 * In case of user pointer buffers vb2 allocators need to get direct
-	 * access to userspace pages. This requires getting the mmap semaphore
-	 * for read access in the current process structure. The same semaphore
-	 * is taken before calling mmap operation, while both qbuf/prepare_buf
-	 * and mmap are called by the driver or v4l2 core with the driver's lock
-	 * held. To avoid an AB-BA deadlock (mmap_sem then driver's lock in mmap
-	 * and driver's lock then mmap_sem in qbuf/prepare_buf) the videobuf2
-	 * core releases the driver's lock, takes mmap_sem and then takes the
-	 * driver's lock again.
-	 *
-	 * To avoid racing with other vb2 calls, which might be called after
-	 * releasing the driver's lock, this operation is performed at the
-	 * beginning of qbuf/prepare_buf processing. This way the queue status
-	 * is consistent after getting the driver's lock back.
-	 */
-	if (q->memory == V4L2_MEMORY_USERPTR) {
-		mmap_sem = &current->mm->mmap_sem;
-		call_qop(q, wait_prepare, q);
-		down_read(mmap_sem);
-		call_qop(q, wait_finish, q);
-	}
-
-	if (q->fileio) {
-		dprintk(1, "%s(): file io in progress\n", opname);
-		ret = -EBUSY;
-		goto unlock;
-	}
-
 	if (b->type != q->type) {
 		dprintk(1, "%s(): invalid buffer type\n", opname);
-		ret = -EINVAL;
-		goto unlock;
+		return -EINVAL;
 	}
 
 	if (b->index >= q->num_buffers) {
 		dprintk(1, "%s(): buffer index out of range\n", opname);
-		ret = -EINVAL;
-		goto unlock;
+		return -EINVAL;
 	}
 
-	vb = q->bufs[b->index];
-	if (NULL == vb) {
+	if (q->bufs[b->index] == NULL) {
 		/* Should never happen */
 		dprintk(1, "%s(): buffer is NULL\n", opname);
-		ret = -EINVAL;
-		goto unlock;
+		return -EINVAL;
 	}
 
 	if (b->memory != q->memory) {
 		dprintk(1, "%s(): invalid memory type\n", opname);
-		ret = -EINVAL;
-		goto unlock;
-	}
-
-	ret = __verify_planes_array(vb, b);
-	if (ret)
-		goto unlock;
-
-	ret = handler(q, b, vb);
-	if (ret)
-		goto unlock;
-
-	/* Fill buffer information for the userspace */
-	__fill_v4l2_buffer(vb, b);
-
-	dprintk(1, "%s() of buffer %d succeeded\n", opname, vb->v4l2_buf.index);
-unlock:
-	if (mmap_sem)
-		up_read(mmap_sem);
-	return ret;
-}
-
-static int __vb2_prepare_buf(struct vb2_queue *q, struct v4l2_buffer *b,
-			     struct vb2_buffer *vb)
-{
-	if (vb->state != VB2_BUF_STATE_DEQUEUED) {
-		dprintk(1, "%s(): invalid buffer state %d\n", __func__,
-			vb->state);
 		return -EINVAL;
 	}
 
-	return __buf_prepare(vb, b);
+	return __verify_planes_array(q->bufs[b->index], b);
 }
 
 /**
@@ -1372,22 +1348,95 @@ static int __vb2_prepare_buf(struct vb2_queue *q, struct v4l2_buffer *b,
  */
 int vb2_prepare_buf(struct vb2_queue *q, struct v4l2_buffer *b)
 {
-	return vb2_queue_or_prepare_buf(q, b, "prepare_buf", __vb2_prepare_buf);
+	struct vb2_buffer *vb;
+	int ret;
+
+	if (q->fileio) {
+		dprintk(1, "%s(): file io in progress\n", __func__);
+		return -EBUSY;
+	}
+
+	ret = vb2_queue_or_prepare_buf(q, b, "prepare_buf");
+	if (ret)
+		return ret;
+
+	vb = q->bufs[b->index];
+	if (vb->state != VB2_BUF_STATE_DEQUEUED) {
+		dprintk(1, "%s(): invalid buffer state %d\n", __func__,
+			vb->state);
+		return -EINVAL;
+	}
+
+	ret = __buf_prepare(vb, b);
+	if (!ret) {
+		/* Fill buffer information for the userspace */
+		__fill_v4l2_buffer(vb, b);
+
+		dprintk(1, "%s() of buffer %d succeeded\n", __func__, vb->v4l2_buf.index);
+	}
+	return ret;
 }
 EXPORT_SYMBOL_GPL(vb2_prepare_buf);
 
-static int __vb2_qbuf(struct vb2_queue *q, struct v4l2_buffer *b,
-		      struct vb2_buffer *vb)
+/**
+ * vb2_start_streaming() - Attempt to start streaming.
+ * @q:		videobuf2 queue
+ *
+ * If there are not enough buffers, then retry_start_streaming is set to
+ * 1 and 0 is returned. The next time a buffer is queued and
+ * retry_start_streaming is 1, this function will be called again to
+ * retry starting the DMA engine.
+ */
+static int vb2_start_streaming(struct vb2_queue *q)
 {
 	int ret;
 
+	/* Tell the driver to start streaming */
+	ret = call_qop(q, start_streaming, q, atomic_read(&q->queued_count));
+
+	/*
+	 * If there are not enough buffers queued to start streaming, then
+	 * the start_streaming operation will return -ENOBUFS and you have to
+	 * retry when the next buffer is queued.
+	 */
+	if (ret == -ENOBUFS) {
+		dprintk(1, "qbuf: not enough buffers, retry when more buffers are queued.\n");
+		q->retry_start_streaming = 1;
+		return 0;
+	}
+	if (ret)
+		dprintk(1, "qbuf: driver refused to start streaming\n");
+	else
+		q->retry_start_streaming = 0;
+	return ret;
+}
+
+static int vb2_internal_qbuf(struct vb2_queue *q, struct v4l2_buffer *b)
+{
+	int ret = vb2_queue_or_prepare_buf(q, b, "qbuf");
+	struct vb2_buffer *vb;
+
+	if (ret)
+		return ret;
+
+	vb = q->bufs[b->index];
+	if (vb->state != VB2_BUF_STATE_DEQUEUED) {
+		dprintk(1, "%s(): invalid buffer state %d\n", __func__,
+			vb->state);
+		return -EINVAL;
+	}
+
 	switch (vb->state) {
 	case VB2_BUF_STATE_DEQUEUED:
 		ret = __buf_prepare(vb, b);
 		if (ret)
 			return ret;
+		break;
 	case VB2_BUF_STATE_PREPARED:
 		break;
+	case VB2_BUF_STATE_PREPARING:
+		dprintk(1, "qbuf: buffer still being prepared\n");
+		return -EINVAL;
 	default:
 		dprintk(1, "qbuf: buffer already in use\n");
 		return -EINVAL;
@@ -1407,6 +1456,16 @@ static int __vb2_qbuf(struct vb2_queue *q, struct v4l2_buffer *b,
 	if (q->streaming)
 		__enqueue_in_driver(vb);
 
+	/* Fill buffer information for the userspace */
+	__fill_v4l2_buffer(vb, b);
+
+	if (q->retry_start_streaming) {
+		ret = vb2_start_streaming(q);
+		if (ret)
+			return ret;
+	}
+
+	dprintk(1, "%s() of buffer %d succeeded\n", __func__, vb->v4l2_buf.index);
 	return 0;
 }
 
@@ -1429,7 +1488,12 @@ static int __vb2_qbuf(struct vb2_queue *q, struct v4l2_buffer *b,
  */
 int vb2_qbuf(struct vb2_queue *q, struct v4l2_buffer *b)
 {
-	return vb2_queue_or_prepare_buf(q, b, "qbuf", __vb2_qbuf);
+	if (q->fileio) {
+		dprintk(1, "%s(): file io in progress\n", __func__);
+		return -EBUSY;
+	}
+
+	return vb2_internal_qbuf(q, b);
 }
 EXPORT_SYMBOL_GPL(vb2_qbuf);
 
@@ -1550,7 +1614,8 @@ int vb2_wait_for_all_buffers(struct vb2_queue *q)
 		return -EINVAL;
 	}
 
-	wait_event(q->done_wq, !atomic_read(&q->queued_count));
+	if (!q->retry_start_streaming)
+		wait_event(q->done_wq, !atomic_read(&q->queued_count));
 	return 0;
 }
 EXPORT_SYMBOL_GPL(vb2_wait_for_all_buffers);
@@ -1579,37 +1644,11 @@ static void __vb2_dqbuf(struct vb2_buffer *vb)
 		}
 }
 
-/**
- * vb2_dqbuf() - Dequeue a buffer to the userspace
- * @q:		videobuf2 queue
- * @b:		buffer structure passed from userspace to vidioc_dqbuf handler
- *		in driver
- * @nonblocking: if true, this call will not sleep waiting for a buffer if no
- *		 buffers ready for dequeuing are present. Normally the driver
- *		 would be passing (file->f_flags & O_NONBLOCK) here
- *
- * Should be called from vidioc_dqbuf ioctl handler of a driver.
- * This function:
- * 1) verifies the passed buffer,
- * 2) calls buf_finish callback in the driver (if provided), in which
- *    driver can perform any additional operations that may be required before
- *    returning the buffer to userspace, such as cache sync,
- * 3) the buffer struct members are filled with relevant information for
- *    the userspace.
- *
- * The return values from this function are intended to be directly returned
- * from vidioc_dqbuf handler in driver.
- */
-int vb2_dqbuf(struct vb2_queue *q, struct v4l2_buffer *b, bool nonblocking)
+static int vb2_internal_dqbuf(struct vb2_queue *q, struct v4l2_buffer *b, bool nonblocking)
 {
 	struct vb2_buffer *vb = NULL;
 	int ret;
 
-	if (q->fileio) {
-		dprintk(1, "dqbuf: file io in progress\n");
-		return -EBUSY;
-	}
-
 	if (b->type != q->type) {
 		dprintk(1, "dqbuf: invalid buffer type\n");
 		return -EINVAL;
@@ -1648,6 +1687,36 @@ int vb2_dqbuf(struct vb2_queue *q, struct v4l2_buffer *b, bool nonblocking)
 
 	return 0;
 }
+
+/**
+ * vb2_dqbuf() - Dequeue a buffer to the userspace
+ * @q:		videobuf2 queue
+ * @b:		buffer structure passed from userspace to vidioc_dqbuf handler
+ *		in driver
+ * @nonblocking: if true, this call will not sleep waiting for a buffer if no
+ *		 buffers ready for dequeuing are present. Normally the driver
+ *		 would be passing (file->f_flags & O_NONBLOCK) here
+ *
+ * Should be called from vidioc_dqbuf ioctl handler of a driver.
+ * This function:
+ * 1) verifies the passed buffer,
+ * 2) calls buf_finish callback in the driver (if provided), in which
+ *    driver can perform any additional operations that may be required before
+ *    returning the buffer to userspace, such as cache sync,
+ * 3) the buffer struct members are filled with relevant information for
+ *    the userspace.
+ *
+ * The return values from this function are intended to be directly returned
+ * from vidioc_dqbuf handler in driver.
+ */
+int vb2_dqbuf(struct vb2_queue *q, struct v4l2_buffer *b, bool nonblocking)
+{
+	if (q->fileio) {
+		dprintk(1, "dqbuf: file io in progress\n");
+		return -EBUSY;
+	}
+	return vb2_internal_dqbuf(q, b, nonblocking);
+}
 EXPORT_SYMBOL_GPL(vb2_dqbuf);
 
 /**
@@ -1660,6 +1729,11 @@ static void __vb2_queue_cancel(struct vb2_queue *q)
 {
 	unsigned int i;
 
+	if (q->retry_start_streaming) {
+		q->retry_start_streaming = 0;
+		q->streaming = 0;
+	}
+
 	/*
 	 * Tell driver to stop all transactions and release all queued
 	 * buffers.
@@ -1687,37 +1761,24 @@ static void __vb2_queue_cancel(struct vb2_queue *q)
 		__vb2_dqbuf(q->bufs[i]);
 }
 
-/**
- * vb2_streamon - start streaming
- * @q:		videobuf2 queue
- * @type:	type argument passed from userspace to vidioc_streamon handler
- *
- * Should be called from vidioc_streamon handler of a driver.
- * This function:
- * 1) verifies current state
- * 2) passes any previously queued buffers to the driver and starts streaming
- *
- * The return values from this function are intended to be directly returned
- * from vidioc_streamon handler in the driver.
- */
-int vb2_streamon(struct vb2_queue *q, enum v4l2_buf_type type)
+static int vb2_internal_streamon(struct vb2_queue *q, enum v4l2_buf_type type)
 {
 	struct vb2_buffer *vb;
 	int ret;
 
-	if (q->fileio) {
-		dprintk(1, "streamon: file io in progress\n");
-		return -EBUSY;
-	}
-
 	if (type != q->type) {
 		dprintk(1, "streamon: invalid stream type\n");
 		return -EINVAL;
 	}
 
 	if (q->streaming) {
-		dprintk(1, "streamon: already streaming\n");
-		return -EBUSY;
+		dprintk(3, "streamon successful: already streaming\n");
+		return 0;
+	}
+
+	if (!q->num_buffers) {
+		dprintk(1, "streamon: no buffers have been allocated\n");
+		return -EINVAL;
 	}
 
 	/*
@@ -1727,12 +1788,9 @@ int vb2_streamon(struct vb2_queue *q, enum v4l2_buf_type type)
 	list_for_each_entry(vb, &q->queued_list, queued_entry)
 		__enqueue_in_driver(vb);
 
-	/*
-	 * Let driver notice that streaming state has been enabled.
-	 */
-	ret = call_qop(q, start_streaming, q, atomic_read(&q->queued_count));
+	/* Tell driver to start streaming. */
+	ret = vb2_start_streaming(q);
 	if (ret) {
-		dprintk(1, "streamon: driver refused to start streaming\n");
 		__vb2_queue_cancel(q);
 		return ret;
 	}
@@ -1742,39 +1800,40 @@ int vb2_streamon(struct vb2_queue *q, enum v4l2_buf_type type)
 	dprintk(3, "Streamon successful\n");
 	return 0;
 }
-EXPORT_SYMBOL_GPL(vb2_streamon);
-
 
 /**
- * vb2_streamoff - stop streaming
+ * vb2_streamon - start streaming
  * @q:		videobuf2 queue
- * @type:	type argument passed from userspace to vidioc_streamoff handler
+ * @type:	type argument passed from userspace to vidioc_streamon handler
  *
- * Should be called from vidioc_streamoff handler of a driver.
+ * Should be called from vidioc_streamon handler of a driver.
  * This function:
- * 1) verifies current state,
- * 2) stop streaming and dequeues any queued buffers, including those previously
- *    passed to the driver (after waiting for the driver to finish).
+ * 1) verifies current state
+ * 2) passes any previously queued buffers to the driver and starts streaming
  *
- * This call can be used for pausing playback.
  * The return values from this function are intended to be directly returned
- * from vidioc_streamoff handler in the driver
+ * from vidioc_streamon handler in the driver.
  */
-int vb2_streamoff(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, "streamoff: file io in progress\n");
+		dprintk(1, "streamon: file io in progress\n");
 		return -EBUSY;
 	}
+	return vb2_internal_streamon(q, type);
+}
+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");
 		return -EINVAL;
 	}
 
 	if (!q->streaming) {
-		dprintk(1, "streamoff: not streaming\n");
-		return -EINVAL;
+		dprintk(3, "streamoff successful: not streaming\n");
+		return 0;
 	}
 
 	/*
@@ -1786,6 +1845,30 @@ int vb2_streamoff(struct vb2_queue *q, enum v4l2_buf_type type)
 	dprintk(3, "Streamoff successful\n");
 	return 0;
 }
+
+/**
+ * vb2_streamoff - stop streaming
+ * @q:		videobuf2 queue
+ * @type:	type argument passed from userspace to vidioc_streamoff handler
+ *
+ * Should be called from vidioc_streamoff handler of a driver.
+ * This function:
+ * 1) verifies current state,
+ * 2) stop streaming and dequeues any queued buffers, including those previously
+ *    passed to the driver (after waiting for the driver to finish).
+ *
+ * This call can be used for pausing playback.
+ * The return values from this function are intended to be directly returned
+ * from vidioc_streamoff handler in the driver
+ */
+int vb2_streamoff(struct vb2_queue *q, enum v4l2_buf_type type)
+{
+	if (q->fileio) {
+		dprintk(1, "streamoff: file io in progress\n");
+		return -EBUSY;
+	}
+	return vb2_internal_streamoff(q, type);
+}
 EXPORT_SYMBOL_GPL(vb2_streamoff);
 
 /**
@@ -2277,15 +2360,16 @@ static int __vb2_init_fileio(struct vb2_queue *q, int read)
 				goto err_reqbufs;
 			fileio->bufs[i].queued = 1;
 		}
-
-		/*
-		 * Start streaming.
-		 */
-		ret = vb2_streamon(q, q->type);
-		if (ret)
-			goto err_reqbufs;
+		fileio->index = q->num_buffers;
 	}
 
+	/*
+	 * Start streaming.
+	 */
+	ret = vb2_streamon(q, q->type);
+	if (ret)
+		goto err_reqbufs;
+
 	q->fileio = fileio;
 
 	return ret;
@@ -2308,13 +2392,8 @@ static int __vb2_cleanup_fileio(struct vb2_queue *q)
 	struct vb2_fileio_data *fileio = q->fileio;
 
 	if (fileio) {
-		/*
-		 * Hack fileio context to enable direct calls to vb2 ioctl
-		 * interface.
-		 */
+		vb2_internal_streamoff(q, q->type);
 		q->fileio = NULL;
-
-		vb2_streamoff(q, q->type);
 		fileio->req.count = 0;
 		vb2_reqbufs(q, &fileio->req);
 		kfree(fileio);
@@ -2358,39 +2437,34 @@ static size_t __vb2_perform_fileio(struct vb2_queue *q, char __user *data, size_
 	fileio = q->fileio;
 
 	/*
-	 * Hack fileio context to enable direct calls to vb2 ioctl interface.
-	 * The pointer will be restored before returning from this function.
-	 */
-	q->fileio = NULL;
-
-	index = fileio->index;
-	buf = &fileio->bufs[index];
-
-	/*
 	 * Check if we need to dequeue the buffer.
 	 */
-	if (buf->queued) {
-		struct vb2_buffer *vb;
-
+	index = fileio->index;
+	if (index >= q->num_buffers) {
 		/*
 		 * Call vb2_dqbuf to get buffer back.
 		 */
 		memset(&fileio->b, 0, sizeof(fileio->b));
 		fileio->b.type = q->type;
 		fileio->b.memory = q->memory;
-		fileio->b.index = index;
-		ret = vb2_dqbuf(q, &fileio->b, nonblock);
+		ret = vb2_internal_dqbuf(q, &fileio->b, nonblock);
 		dprintk(5, "file io: vb2_dqbuf result: %d\n", ret);
 		if (ret)
-			goto end;
+			return ret;
 		fileio->dq_count += 1;
 
+		index = fileio->b.index;
+		buf = &fileio->bufs[index];
+
 		/*
 		 * Get number of bytes filled by the driver
 		 */
-		vb = q->bufs[index];
-		buf->size = vb2_get_plane_payload(vb, 0);
+		buf->pos = 0;
 		buf->queued = 0;
+		buf->size = read ? vb2_get_plane_payload(q->bufs[index], 0)
+				 : vb2_plane_size(q->bufs[index], 0);
+	} else {
+		buf = &fileio->bufs[index];
 	}
 
 	/*
@@ -2412,8 +2486,7 @@ static size_t __vb2_perform_fileio(struct vb2_queue *q, char __user *data, size_
 		ret = copy_from_user(buf->vaddr + buf->pos, data, count);
 	if (ret) {
 		dprintk(3, "file io: error copying data\n");
-		ret = -EFAULT;
-		goto end;
+		return -EFAULT;
 	}
 
 	/*
@@ -2433,10 +2506,6 @@ 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");
-			/*
-			 * Restore fileio pointer and release the context.
-			 */
-			q->fileio = fileio;
 			return __vb2_cleanup_fileio(q);
 		}
 
@@ -2448,32 +2517,20 @@ 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;
-		ret = vb2_qbuf(q, &fileio->b);
+		ret = vb2_internal_qbuf(q, &fileio->b);
 		dprintk(5, "file io: vb2_dbuf result: %d\n", ret);
 		if (ret)
-			goto end;
+			return ret;
 
 		/*
 		 * Buffer has been queued, update the status
 		 */
 		buf->pos = 0;
 		buf->queued = 1;
-		buf->size = q->bufs[0]->v4l2_planes[0].length;
+		buf->size = vb2_plane_size(q->bufs[index], 0);
 		fileio->q_count += 1;
-
-		/*
-		 * Switch to the next buffer
-		 */
-		fileio->index = (index + 1) % q->num_buffers;
-
-		/*
-		 * Start streaming if required.
-		 */
-		if (!read && !q->streaming) {
-			ret = vb2_streamon(q, q->type);
-			if (ret)
-				goto end;
-		}
+		if (fileio->index < q->num_buffers)
+			fileio->index++;
 	}
 
 	/*
@@ -2481,11 +2538,6 @@ static size_t __vb2_perform_fileio(struct vb2_queue *q, char __user *data, size_
 	 */
 	if (ret == 0)
 		ret = count;
-end:
-	/*
-	 * Restore the fileio context and block vb2 ioctl interface.
-	 */
-	q->fileio = fileio;
 	return ret;
 }
 
@@ -2649,16 +2701,29 @@ int vb2_fop_mmap(struct file *file, struct vm_area_struct *vma)
 }
 EXPORT_SYMBOL_GPL(vb2_fop_mmap);
 
-int vb2_fop_release(struct file *file)
+int _vb2_fop_release(struct file *file, struct mutex *lock)
 {
 	struct video_device *vdev = video_devdata(file);
 
 	if (file->private_data == vdev->queue->owner) {
+		if (lock)
+			mutex_lock(lock);
 		vb2_queue_release(vdev->queue);
 		vdev->queue->owner = NULL;
+		if (lock)
+			mutex_unlock(lock);
 	}
 	return v4l2_fh_release(file);
 }
+EXPORT_SYMBOL_GPL(_vb2_fop_release);
+
+int vb2_fop_release(struct file *file)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct mutex *lock = vdev->queue->lock ? vdev->queue->lock : vdev->lock;
+
+	return _vb2_fop_release(file, lock);
+}
 EXPORT_SYMBOL_GPL(vb2_fop_release);
 
 ssize_t vb2_fop_write(struct file *file, const char __user *buf,
diff --git a/drivers/media/v4l2-core/videobuf2-dma-sg.c b/drivers/media/v4l2-core/videobuf2-dma-sg.c
index 0d3a8ffe47a3..c779f210d2c6 100644
--- a/drivers/media/v4l2-core/videobuf2-dma-sg.c
+++ b/drivers/media/v4l2-core/videobuf2-dma-sg.c
@@ -40,6 +40,7 @@ struct vb2_dma_sg_buf {
 	unsigned int			num_pages;
 	atomic_t			refcount;
 	struct vb2_vmarea_handler	handler;
+	struct vm_area_struct		*vma;
 };
 
 static void vb2_dma_sg_put(void *buf_priv);
@@ -155,12 +156,18 @@ static void vb2_dma_sg_put(void *buf_priv)
 	}
 }
 
+static inline int vma_is_io(struct vm_area_struct *vma)
+{
+	return !!(vma->vm_flags & (VM_IO | VM_PFNMAP));
+}
+
 static void *vb2_dma_sg_get_userptr(void *alloc_ctx, unsigned long vaddr,
 				    unsigned long size, int write)
 {
 	struct vb2_dma_sg_buf *buf;
 	unsigned long first, last;
 	int num_pages_from_user;
+	struct vm_area_struct *vma;
 
 	buf = kzalloc(sizeof *buf, GFP_KERNEL);
 	if (!buf)
@@ -180,7 +187,38 @@ static void *vb2_dma_sg_get_userptr(void *alloc_ctx, unsigned long vaddr,
 	if (!buf->pages)
 		goto userptr_fail_alloc_pages;
 
-	num_pages_from_user = get_user_pages(current, current->mm,
+	vma = find_vma(current->mm, vaddr);
+	if (!vma) {
+		dprintk(1, "no vma for address %lu\n", vaddr);
+		goto userptr_fail_find_vma;
+	}
+
+	if (vma->vm_end < vaddr + size) {
+		dprintk(1, "vma at %lu is too small for %lu bytes\n",
+			vaddr, size);
+		goto userptr_fail_find_vma;
+	}
+
+	buf->vma = vb2_get_vma(vma);
+	if (!buf->vma) {
+		dprintk(1, "failed to copy vma\n");
+		goto userptr_fail_find_vma;
+	}
+
+	if (vma_is_io(buf->vma)) {
+		for (num_pages_from_user = 0;
+		     num_pages_from_user < buf->num_pages;
+		     ++num_pages_from_user, vaddr += PAGE_SIZE) {
+			unsigned long pfn;
+
+			if (follow_pfn(buf->vma, vaddr, &pfn)) {
+				dprintk(1, "no page for address %lu\n", vaddr);
+				break;
+			}
+			buf->pages[num_pages_from_user] = pfn_to_page(pfn);
+		}
+	} else
+		num_pages_from_user = get_user_pages(current, current->mm,
 					     vaddr & PAGE_MASK,
 					     buf->num_pages,
 					     write,
@@ -200,9 +238,12 @@ static void *vb2_dma_sg_get_userptr(void *alloc_ctx, unsigned long vaddr,
 userptr_fail_alloc_table_from_pages:
 userptr_fail_get_user_pages:
 	dprintk(1, "get_user_pages requested/got: %d/%d]\n",
-	       num_pages_from_user, buf->num_pages);
-	while (--num_pages_from_user >= 0)
-		put_page(buf->pages[num_pages_from_user]);
+		buf->num_pages, num_pages_from_user);
+	if (!vma_is_io(buf->vma))
+		while (--num_pages_from_user >= 0)
+			put_page(buf->pages[num_pages_from_user]);
+	vb2_put_vma(buf->vma);
+userptr_fail_find_vma:
 	kfree(buf->pages);
 userptr_fail_alloc_pages:
 	kfree(buf);
@@ -226,9 +267,11 @@ static void vb2_dma_sg_put_userptr(void *buf_priv)
 	while (--i >= 0) {
 		if (buf->write)
 			set_page_dirty_lock(buf->pages[i]);
-		put_page(buf->pages[i]);
+		if (!vma_is_io(buf->vma))
+			put_page(buf->pages[i]);
 	}
 	kfree(buf->pages);
+	vb2_put_vma(buf->vma);
 	kfree(buf);
 }
 
diff --git a/drivers/message/fusion/mptsas.c b/drivers/message/fusion/mptsas.c
index dd239bdbfcb4..00d339c361fc 100644
--- a/drivers/message/fusion/mptsas.c
+++ b/drivers/message/fusion/mptsas.c
@@ -2235,10 +2235,10 @@ static int mptsas_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 	}
 
 	/* do we need to support multiple segments? */
-	if (bio_segments(req->bio) > 1 || bio_segments(rsp->bio) > 1) {
-		printk(MYIOC_s_ERR_FMT "%s: multiple segments req %u %u, rsp %u %u\n",
-		    ioc->name, __func__, bio_segments(req->bio), blk_rq_bytes(req),
-		    bio_segments(rsp->bio), blk_rq_bytes(rsp));
+	if (bio_multiple_segments(req->bio) ||
+	    bio_multiple_segments(rsp->bio)) {
+		printk(MYIOC_s_ERR_FMT "%s: multiple segments req %u, rsp %u\n",
+		    ioc->name, __func__, blk_rq_bytes(req), blk_rq_bytes(rsp));
 		return -EINVAL;
 	}
 
diff --git a/drivers/mtd/nand/mpc5121_nfc.c b/drivers/mtd/nand/mpc5121_nfc.c
index 61e2abc216ad..31ee7cfbc12b 100644
--- a/drivers/mtd/nand/mpc5121_nfc.c
+++ b/drivers/mtd/nand/mpc5121_nfc.c
@@ -729,7 +729,7 @@ static int mpc5121_nfc_probe(struct platform_device *op)
 	of_node_put(rootnode);
 
 	/* Enable NFC clock */
-	clk = devm_clk_get(dev, "nfc_clk");
+	clk = devm_clk_get(dev, "ipg");
 	if (IS_ERR(clk)) {
 		dev_err(dev, "Unable to acquire NFC clock!\n");
 		retval = PTR_ERR(clk);
diff --git a/drivers/mtd/ubi/attach.c b/drivers/mtd/ubi/attach.c
index 33bb1f2b63e4..6f27d9a1be3b 100644
--- a/drivers/mtd/ubi/attach.c
+++ b/drivers/mtd/ubi/attach.c
@@ -1453,8 +1453,10 @@ int ubi_attach(struct ubi_device *ubi, int force_scan)
 		struct ubi_attach_info *scan_ai;
 
 		scan_ai = alloc_ai("ubi_ckh_aeb_slab_cache");
-		if (!scan_ai)
+		if (!scan_ai) {
+			err = -ENOMEM;
 			goto out_wl;
+		}
 
 		err = scan_all(ubi, scan_ai, 0);
 		if (err) {
diff --git a/drivers/mtd/ubi/build.c b/drivers/mtd/ubi/build.c
index e05dc6298c1d..57deae961429 100644
--- a/drivers/mtd/ubi/build.c
+++ b/drivers/mtd/ubi/build.c
@@ -1245,8 +1245,10 @@ static int __init ubi_init(void)
 	ubi_wl_entry_slab = kmem_cache_create("ubi_wl_entry_slab",
 					      sizeof(struct ubi_wl_entry),
 					      0, 0, NULL);
-	if (!ubi_wl_entry_slab)
+	if (!ubi_wl_entry_slab) {
+		err = -ENOMEM;
 		goto out_dev_unreg;
+	}
 
 	err = ubi_debugfs_init();
 	if (err)
diff --git a/drivers/mtd/ubi/io.c b/drivers/mtd/ubi/io.c
index bf79def40126..d36134925d31 100644
--- a/drivers/mtd/ubi/io.c
+++ b/drivers/mtd/ubi/io.c
@@ -495,10 +495,12 @@ out:
  */
 static int nor_erase_prepare(struct ubi_device *ubi, int pnum)
 {
-	int err, err1;
+	int err;
 	size_t written;
 	loff_t addr;
 	uint32_t data = 0;
+	struct ubi_ec_hdr ec_hdr;
+
 	/*
 	 * Note, we cannot generally define VID header buffers on stack,
 	 * because of the way we deal with these buffers (see the header
@@ -509,50 +511,38 @@ static int nor_erase_prepare(struct ubi_device *ubi, int pnum)
 	struct ubi_vid_hdr vid_hdr;
 
 	/*
+	 * If VID or EC is valid, we have to corrupt them before erasing.
 	 * It is important to first invalidate the EC header, and then the VID
 	 * header. Otherwise a power cut may lead to valid EC header and
 	 * invalid VID header, in which case UBI will treat this PEB as
 	 * corrupted and will try to preserve it, and print scary warnings.
 	 */
 	addr = (loff_t)pnum * ubi->peb_size;
-	err = mtd_write(ubi->mtd, addr, 4, &written, (void *)&data);
-	if (!err) {
-		addr += ubi->vid_hdr_aloffset;
+	err = ubi_io_read_ec_hdr(ubi, pnum, &ec_hdr, 0);
+	if (err != UBI_IO_BAD_HDR_EBADMSG && err != UBI_IO_BAD_HDR &&
+	    err != UBI_IO_FF){
 		err = mtd_write(ubi->mtd, addr, 4, &written, (void *)&data);
-		if (!err)
-			return 0;
+		if(err)
+			goto error;
 	}
 
-	/*
-	 * We failed to write to the media. This was observed with Spansion
-	 * S29GL512N NOR flash. Most probably the previously eraseblock erasure
-	 * was interrupted at a very inappropriate moment, so it became
-	 * unwritable. In this case we probably anyway have garbage in this
-	 * PEB.
-	 */
-	err1 = ubi_io_read_vid_hdr(ubi, pnum, &vid_hdr, 0);
-	if (err1 == UBI_IO_BAD_HDR_EBADMSG || err1 == UBI_IO_BAD_HDR ||
-	    err1 == UBI_IO_FF) {
-		struct ubi_ec_hdr ec_hdr;
-
-		err1 = ubi_io_read_ec_hdr(ubi, pnum, &ec_hdr, 0);
-		if (err1 == UBI_IO_BAD_HDR_EBADMSG || err1 == UBI_IO_BAD_HDR ||
-		    err1 == UBI_IO_FF)
-			/*
-			 * Both VID and EC headers are corrupted, so we can
-			 * safely erase this PEB and not afraid that it will be
-			 * treated as a valid PEB in case of an unclean reboot.
-			 */
-			return 0;
+	err = ubi_io_read_vid_hdr(ubi, pnum, &vid_hdr, 0);
+	if (err != UBI_IO_BAD_HDR_EBADMSG && err != UBI_IO_BAD_HDR &&
+	    err != UBI_IO_FF){
+		addr += ubi->vid_hdr_aloffset;
+		err = mtd_write(ubi->mtd, addr, 4, &written, (void *)&data);
+		if (err)
+			goto error;
 	}
+	return 0;
 
+error:
 	/*
-	 * The PEB contains a valid VID header, but we cannot invalidate it.
-	 * Supposedly the flash media or the driver is screwed up, so return an
-	 * error.
+	 * The PEB contains a valid VID or EC header, but we cannot invalidate
+	 * it. Supposedly the flash media or the driver is screwed up, so
+	 * return an error.
 	 */
-	ubi_err("cannot invalidate PEB %d, write returned %d read returned %d",
-		pnum, err, err1);
+	ubi_err("cannot invalidate PEB %d, write returned %d", pnum, err);
 	ubi_dump_flash(ubi, pnum, 0, ubi->peb_size);
 	return -EIO;
 }
diff --git a/drivers/net/can/mscan/mpc5xxx_can.c b/drivers/net/can/mscan/mpc5xxx_can.c
index 035e235e3118..44725296f72a 100644
--- a/drivers/net/can/mscan/mpc5xxx_can.c
+++ b/drivers/net/can/mscan/mpc5xxx_can.c
@@ -108,135 +108,170 @@ static u32 mpc52xx_can_get_clock(struct platform_device *ofdev,
 #endif /* CONFIG_PPC_MPC52xx */
 
 #ifdef CONFIG_PPC_MPC512x
-struct mpc512x_clockctl {
-	u32 spmr;		/* System PLL Mode Reg */
-	u32 sccr[2];		/* System Clk Ctrl Reg 1 & 2 */
-	u32 scfr1;		/* System Clk Freq Reg 1 */
-	u32 scfr2;		/* System Clk Freq Reg 2 */
-	u32 reserved;
-	u32 bcr;		/* Bread Crumb Reg */
-	u32 pccr[12];		/* PSC Clk Ctrl Reg 0-11 */
-	u32 spccr;		/* SPDIF Clk Ctrl Reg */
-	u32 cccr;		/* CFM Clk Ctrl Reg */
-	u32 dccr;		/* DIU Clk Cnfg Reg */
-	u32 mccr[4];		/* MSCAN Clk Ctrl Reg 1-3 */
-};
-
-static struct of_device_id mpc512x_clock_ids[] = {
-	{ .compatible = "fsl,mpc5121-clock", },
-	{}
-};
-
 static u32 mpc512x_can_get_clock(struct platform_device *ofdev,
-				 const char *clock_name, int *mscan_clksrc)
+				 const char *clock_source, int *mscan_clksrc)
 {
-	struct mpc512x_clockctl __iomem *clockctl;
-	struct device_node *np_clock;
-	struct clk *sys_clk, *ref_clk;
-	int plen, clockidx, clocksrc = -1;
-	u32 sys_freq, val, clockdiv = 1, freq = 0;
-	const u32 *pval;
-
-	np_clock = of_find_matching_node(NULL, mpc512x_clock_ids);
-	if (!np_clock) {
-		dev_err(&ofdev->dev, "couldn't find clock node\n");
-		return 0;
-	}
-	clockctl = of_iomap(np_clock, 0);
-	if (!clockctl) {
-		dev_err(&ofdev->dev, "couldn't map clock registers\n");
-		goto exit_put;
-	}
+	struct device_node *np;
+	u32 clockdiv;
+	enum {
+		CLK_FROM_AUTO,
+		CLK_FROM_IPS,
+		CLK_FROM_SYS,
+		CLK_FROM_REF,
+	} clk_from;
+	struct clk *clk_in, *clk_can;
+	unsigned long freq_calc;
+	struct mscan_priv *priv;
+	struct clk *clk_ipg;
 
-	/* Determine the MSCAN device index from the peripheral's
-	 * physical address. Register address offsets against the
-	 * IMMR base are:  0x1300, 0x1380, 0x2300, 0x2380
+	/* the caller passed in the clock source spec that was read from
+	 * the device tree, get the optional clock divider as well
 	 */
-	pval = of_get_property(ofdev->dev.of_node, "reg", &plen);
-	BUG_ON(!pval || plen < sizeof(*pval));
-	clockidx = (*pval & 0x80) ? 1 : 0;
-	if (*pval & 0x2000)
-		clockidx += 2;
+	np = ofdev->dev.of_node;
+	clockdiv = 1;
+	of_property_read_u32(np, "fsl,mscan-clock-divider", &clockdiv);
+	dev_dbg(&ofdev->dev, "device tree specs: clk src[%s] div[%d]\n",
+		clock_source ? clock_source : "<NULL>", clockdiv);
+
+	/* when clock-source is 'ip', the CANCTL1[CLKSRC] bit needs to
+	 * get set, and the 'ips' clock is the input to the MSCAN
+	 * component
+	 *
+	 * for clock-source values of 'ref' or 'sys' the CANCTL1[CLKSRC]
+	 * bit needs to get cleared, an optional clock-divider may have
+	 * been specified (the default value is 1), the appropriate
+	 * MSCAN related MCLK is the input to the MSCAN component
+	 *
+	 * in the absence of a clock-source spec, first an optimal clock
+	 * gets determined based on the 'sys' clock, if that fails the
+	 * 'ref' clock is used
+	 */
+	clk_from = CLK_FROM_AUTO;
+	if (clock_source) {
+		/* interpret the device tree's spec for the clock source */
+		if (!strcmp(clock_source, "ip"))
+			clk_from = CLK_FROM_IPS;
+		else if (!strcmp(clock_source, "sys"))
+			clk_from = CLK_FROM_SYS;
+		else if (!strcmp(clock_source, "ref"))
+			clk_from = CLK_FROM_REF;
+		else
+			goto err_invalid;
+		dev_dbg(&ofdev->dev, "got a clk source spec[%d]\n", clk_from);
+	}
+	if (clk_from == CLK_FROM_AUTO) {
+		/* no spec so far, try the 'sys' clock; round to the
+		 * next MHz and see if we can get a multiple of 16MHz
+		 */
+		dev_dbg(&ofdev->dev, "no clk source spec, trying SYS\n");
+		clk_in = devm_clk_get(&ofdev->dev, "sys");
+		if (IS_ERR(clk_in))
+			goto err_notavail;
+		freq_calc = clk_get_rate(clk_in);
+		freq_calc +=  499999;
+		freq_calc /= 1000000;
+		freq_calc *= 1000000;
+		if ((freq_calc % 16000000) == 0) {
+			clk_from = CLK_FROM_SYS;
+			clockdiv = freq_calc / 16000000;
+			dev_dbg(&ofdev->dev,
+				"clk fit, sys[%lu] div[%d] freq[%lu]\n",
+				freq_calc, clockdiv, freq_calc / clockdiv);
+		}
+	}
+	if (clk_from == CLK_FROM_AUTO) {
+		/* no spec so far, use the 'ref' clock */
+		dev_dbg(&ofdev->dev, "no clk source spec, trying REF\n");
+		clk_in = devm_clk_get(&ofdev->dev, "ref");
+		if (IS_ERR(clk_in))
+			goto err_notavail;
+		clk_from = CLK_FROM_REF;
+		freq_calc = clk_get_rate(clk_in);
+		dev_dbg(&ofdev->dev,
+			"clk fit, ref[%lu] (no div) freq[%lu]\n",
+			freq_calc, freq_calc);
+	}
 
-	/*
-	 * Clock source and divider selection: 3 different clock sources
-	 * can be selected: "ip", "ref" or "sys". For the latter two, a
-	 * clock divider can be defined as well. If the clock source is
-	 * not specified by the device tree, we first try to find an
-	 * optimal CAN source clock based on the system clock. If that
-	 * is not posslible, the reference clock will be used.
+	/* select IPS or MCLK as the MSCAN input (returned to the caller),
+	 * setup the MCLK mux source and rate if applicable, apply the
+	 * optionally specified or derived above divider, and determine
+	 * the actual resulting clock rate to return to the caller
 	 */
-	if (clock_name && !strcmp(clock_name, "ip")) {
+	switch (clk_from) {
+	case CLK_FROM_IPS:
+		clk_can = devm_clk_get(&ofdev->dev, "ips");
+		if (IS_ERR(clk_can))
+			goto err_notavail;
+		priv = netdev_priv(dev_get_drvdata(&ofdev->dev));
+		priv->clk_can = clk_can;
+		freq_calc = clk_get_rate(clk_can);
 		*mscan_clksrc = MSCAN_CLKSRC_IPS;
-		freq = mpc5xxx_get_bus_frequency(ofdev->dev.of_node);
-	} else {
+		dev_dbg(&ofdev->dev, "clk from IPS, clksrc[%d] freq[%lu]\n",
+			*mscan_clksrc, freq_calc);
+		break;
+	case CLK_FROM_SYS:
+	case CLK_FROM_REF:
+		clk_can = devm_clk_get(&ofdev->dev, "mclk");
+		if (IS_ERR(clk_can))
+			goto err_notavail;
+		priv = netdev_priv(dev_get_drvdata(&ofdev->dev));
+		priv->clk_can = clk_can;
+		if (clk_from == CLK_FROM_SYS)
+			clk_in = devm_clk_get(&ofdev->dev, "sys");
+		if (clk_from == CLK_FROM_REF)
+			clk_in = devm_clk_get(&ofdev->dev, "ref");
+		if (IS_ERR(clk_in))
+			goto err_notavail;
+		clk_set_parent(clk_can, clk_in);
+		freq_calc = clk_get_rate(clk_in);
+		freq_calc /= clockdiv;
+		clk_set_rate(clk_can, freq_calc);
+		freq_calc = clk_get_rate(clk_can);
 		*mscan_clksrc = MSCAN_CLKSRC_BUS;
-
-		pval = of_get_property(ofdev->dev.of_node,
-				       "fsl,mscan-clock-divider", &plen);
-		if (pval && plen == sizeof(*pval))
-			clockdiv = *pval;
-		if (!clockdiv)
-			clockdiv = 1;
-
-		if (!clock_name || !strcmp(clock_name, "sys")) {
-			sys_clk = devm_clk_get(&ofdev->dev, "sys_clk");
-			if (IS_ERR(sys_clk)) {
-				dev_err(&ofdev->dev, "couldn't get sys_clk\n");
-				goto exit_unmap;
-			}
-			/* Get and round up/down sys clock rate */
-			sys_freq = 1000000 *
-				((clk_get_rate(sys_clk) + 499999) / 1000000);
-
-			if (!clock_name) {
-				/* A multiple of 16 MHz would be optimal */
-				if ((sys_freq % 16000000) == 0) {
-					clocksrc = 0;
-					clockdiv = sys_freq / 16000000;
-					freq = sys_freq / clockdiv;
-				}
-			} else {
-				clocksrc = 0;
-				freq = sys_freq / clockdiv;
-			}
-		}
-
-		if (clocksrc < 0) {
-			ref_clk = devm_clk_get(&ofdev->dev, "ref_clk");
-			if (IS_ERR(ref_clk)) {
-				dev_err(&ofdev->dev, "couldn't get ref_clk\n");
-				goto exit_unmap;
-			}
-			clocksrc = 1;
-			freq = clk_get_rate(ref_clk) / clockdiv;
-		}
+		dev_dbg(&ofdev->dev, "clk from MCLK, clksrc[%d] freq[%lu]\n",
+			*mscan_clksrc, freq_calc);
+		break;
+	default:
+		goto err_invalid;
 	}
 
-	/* Disable clock */
-	out_be32(&clockctl->mccr[clockidx], 0x0);
-	if (clocksrc >= 0) {
-		/* Set source and divider */
-		val = (clocksrc << 14) | ((clockdiv - 1) << 17);
-		out_be32(&clockctl->mccr[clockidx], val);
-		/* Enable clock */
-		out_be32(&clockctl->mccr[clockidx], val | 0x10000);
-	}
+	/* the above clk_can item is used for the bitrate, access to
+	 * the peripheral's register set needs the clk_ipg item
+	 */
+	clk_ipg = devm_clk_get(&ofdev->dev, "ipg");
+	if (IS_ERR(clk_ipg))
+		goto err_notavail_ipg;
+	if (clk_prepare_enable(clk_ipg))
+		goto err_notavail_ipg;
+	priv = netdev_priv(dev_get_drvdata(&ofdev->dev));
+	priv->clk_ipg = clk_ipg;
+
+	/* return the determined clock source rate */
+	return freq_calc;
+
+err_invalid:
+	dev_err(&ofdev->dev, "invalid clock source specification\n");
+	/* clock source rate could not get determined */
+	return 0;
 
-	/* Enable MSCAN clock domain */
-	val = in_be32(&clockctl->sccr[1]);
-	if (!(val & (1 << 25)))
-		out_be32(&clockctl->sccr[1], val | (1 << 25));
+err_notavail:
+	dev_err(&ofdev->dev, "cannot acquire or setup bitrate clock source\n");
+	/* clock source rate could not get determined */
+	return 0;
 
-	dev_dbg(&ofdev->dev, "using '%s' with frequency divider %d\n",
-		*mscan_clksrc == MSCAN_CLKSRC_IPS ? "ips_clk" :
-		clocksrc == 1 ? "ref_clk" : "sys_clk", clockdiv);
+err_notavail_ipg:
+	dev_err(&ofdev->dev, "cannot acquire or setup register clock\n");
+	/* clock source rate could not get determined */
+	return 0;
+}
 
-exit_unmap:
-	iounmap(clockctl);
-exit_put:
-	of_node_put(np_clock);
-	return freq;
+static void mpc512x_can_put_clock(struct platform_device *ofdev)
+{
+	struct mscan_priv *priv;
+
+	priv = netdev_priv(dev_get_drvdata(&ofdev->dev));
+	if (priv->clk_ipg)
+		clk_disable_unprepare(priv->clk_ipg);
 }
 #else /* !CONFIG_PPC_MPC512x */
 static u32 mpc512x_can_get_clock(struct platform_device *ofdev,
@@ -244,6 +279,7 @@ static u32 mpc512x_can_get_clock(struct platform_device *ofdev,
 {
 	return 0;
 }
+#define mpc512x_can_put_clock NULL
 #endif /* CONFIG_PPC_MPC512x */
 
 static const struct of_device_id mpc5xxx_can_table[];
@@ -385,11 +421,13 @@ static int mpc5xxx_can_resume(struct platform_device *ofdev)
 static const struct mpc5xxx_can_data mpc5200_can_data = {
 	.type = MSCAN_TYPE_MPC5200,
 	.get_clock = mpc52xx_can_get_clock,
+	/* .put_clock not applicable */
 };
 
 static const struct mpc5xxx_can_data mpc5121_can_data = {
 	.type = MSCAN_TYPE_MPC5121,
 	.get_clock = mpc512x_can_get_clock,
+	.put_clock = mpc512x_can_put_clock,
 };
 
 static const struct of_device_id mpc5xxx_can_table[] = {
diff --git a/drivers/net/phy/mdio_bus.c b/drivers/net/phy/mdio_bus.c
index 930694d3a13f..71e49000fbf3 100644
--- a/drivers/net/phy/mdio_bus.c
+++ b/drivers/net/phy/mdio_bus.c
@@ -150,6 +150,7 @@ int mdiobus_register(struct mii_bus *bus)
 	err = device_register(&bus->dev);
 	if (err) {
 		pr_err("mii_bus %s failed to register\n", bus->id);
+		put_device(&bus->dev);
 		return -EINVAL;
 	}
 
diff --git a/drivers/net/wireless/Kconfig b/drivers/net/wireless/Kconfig
index 200020eb3005..d1fab435f5a3 100644
--- a/drivers/net/wireless/Kconfig
+++ b/drivers/net/wireless/Kconfig
@@ -53,7 +53,7 @@ config LIBERTAS_THINFIRM_USB
 
 config AIRO
 	tristate "Cisco/Aironet 34X/35X/4500/4800 ISA and PCI cards"
-	depends on ISA_DMA_API && (PCI || BROKEN)
+	depends on CFG80211 && ISA_DMA_API && (PCI || BROKEN)
 	select WIRELESS_EXT
 	select CRYPTO
 	select WEXT_SPY
@@ -73,7 +73,7 @@ config AIRO
 
 config ATMEL
       tristate "Atmel at76c50x chipset  802.11b support"
-      depends on (PCI || PCMCIA)
+      depends on CFG80211 && (PCI || PCMCIA)
       select WIRELESS_EXT
       select WEXT_PRIV
       select FW_LOADER
@@ -138,7 +138,7 @@ config AIRO_CS
 
 config PCMCIA_WL3501
 	tristate "Planet WL3501 PCMCIA cards"
-	depends on PCMCIA
+	depends on CFG80211 && PCMCIA
 	select WIRELESS_EXT
 	select WEXT_SPY
 	help
@@ -168,7 +168,7 @@ config PRISM54
 
 config USB_ZD1201
 	tristate "USB ZD1201 based Wireless device support"
-	depends on USB
+	depends on CFG80211 && USB
 	select WIRELESS_EXT
 	select WEXT_PRIV
 	select FW_LOADER
diff --git a/drivers/net/wireless/airo.c b/drivers/net/wireless/airo.c
index edf4b57c4aaa..64747d457bb3 100644
--- a/drivers/net/wireless/airo.c
+++ b/drivers/net/wireless/airo.c
@@ -36,7 +36,7 @@
 #include <linux/bitops.h>
 #include <linux/scatterlist.h>
 #include <linux/crypto.h>
-#include <asm/io.h>
+#include <linux/io.h>
 #include <asm/unaligned.h>
 
 #include <linux/netdevice.h>
@@ -45,11 +45,11 @@
 #include <linux/if_arp.h>
 #include <linux/ioport.h>
 #include <linux/pci.h>
-#include <asm/uaccess.h>
+#include <linux/uaccess.h>
 #include <linux/kthread.h>
 #include <linux/freezer.h>
 
-#include <linux/ieee80211.h>
+#include <net/cfg80211.h>
 #include <net/iw_handler.h>
 
 #include "airo.h"
@@ -5797,7 +5797,7 @@ static int airo_set_freq(struct net_device *dev,
 
 		/* Hack to fall through... */
 		fwrq->e = 0;
-		fwrq->m = ieee80211_freq_to_dsss_chan(f);
+		fwrq->m = ieee80211_frequency_to_channel(f);
 	}
 	/* Setting by channel number */
 	if((fwrq->m > 1000) || (fwrq->e > 0))
@@ -5841,7 +5841,8 @@ static int airo_get_freq(struct net_device *dev,
 
 	ch = le16_to_cpu(status_rid.channel);
 	if((ch > 0) && (ch < 15)) {
-		fwrq->m = ieee80211_dsss_chan_to_freq(ch) * 100000;
+		fwrq->m = 100000 *
+			ieee80211_channel_to_frequency(ch, IEEE80211_BAND_2GHZ);
 		fwrq->e = 1;
 	} else {
 		fwrq->m = ch;
@@ -6898,7 +6899,8 @@ static int airo_get_range(struct net_device *dev,
 	k = 0;
 	for(i = 0; i < 14; i++) {
 		range->freq[k].i = i + 1; /* List index */
-		range->freq[k].m = ieee80211_dsss_chan_to_freq(i + 1) * 100000;
+		range->freq[k].m = 100000 *
+		     ieee80211_channel_to_frequency(i + 1, IEEE80211_BAND_2GHZ);
 		range->freq[k++].e = 1;	/* Values in MHz -> * 10^5 * 10 */
 	}
 	range->num_frequency = k;
@@ -7297,7 +7299,8 @@ static inline char *airo_translate_scan(struct net_device *dev,
 	/* Add frequency */
 	iwe.cmd = SIOCGIWFREQ;
 	iwe.u.freq.m = le16_to_cpu(bss->dsChannel);
-	iwe.u.freq.m = ieee80211_dsss_chan_to_freq(iwe.u.freq.m) * 100000;
+	iwe.u.freq.m = 100000 *
+	      ieee80211_channel_to_frequency(iwe.u.freq.m, IEEE80211_BAND_2GHZ);
 	iwe.u.freq.e = 1;
 	current_ev = iwe_stream_add_event(info, current_ev, end_buf,
 					  &iwe, IW_EV_FREQ_LEN);
diff --git a/drivers/net/wireless/ath/ar5523/ar5523.c b/drivers/net/wireless/ath/ar5523/ar5523.c
index 8aa20df55e50..507d9a9ee69a 100644
--- a/drivers/net/wireless/ath/ar5523/ar5523.c
+++ b/drivers/net/wireless/ath/ar5523/ar5523.c
@@ -1764,7 +1764,7 @@ static struct usb_device_id ar5523_id_table[] = {
 	AR5523_DEVICE_UG(0x07d1, 0x3a07),	/* D-Link / WUA-2340 rev A1 */
 	AR5523_DEVICE_UG(0x1690, 0x0712),	/* Gigaset / AR5523 */
 	AR5523_DEVICE_UG(0x1690, 0x0710),	/* Gigaset / SMCWUSBTG */
-	AR5523_DEVICE_UG(0x129b, 0x160c),	/* Gigaset / USB stick 108
+	AR5523_DEVICE_UG(0x129b, 0x160b),	/* Gigaset / USB stick 108
 						   (CyberTAN Technology) */
 	AR5523_DEVICE_UG(0x16ab, 0x7801),	/* Globalsun / AR5523_1 */
 	AR5523_DEVICE_UX(0x16ab, 0x7811),	/* Globalsun / AR5523_2 */
diff --git a/drivers/net/wireless/ath/ath.h b/drivers/net/wireless/ath/ath.h
index b59cfbe0276b..d239acc26125 100644
--- a/drivers/net/wireless/ath/ath.h
+++ b/drivers/net/wireless/ath/ath.h
@@ -63,7 +63,7 @@ enum ath_bus_type {
 };
 
 struct reg_dmn_pair_mapping {
-	u16 regDmnEnum;
+	u16 reg_domain;
 	u16 reg_5ghz_ctl;
 	u16 reg_2ghz_ctl;
 };
@@ -161,6 +161,9 @@ struct ath_common {
 	bool btcoex_enabled;
 	bool disable_ani;
 	bool bt_ant_diversity;
+
+	int last_rssi;
+	struct ieee80211_supported_band sbands[IEEE80211_NUM_BANDS];
 };
 
 struct sk_buff *ath_rxbuf_alloc(struct ath_common *common,
diff --git a/drivers/net/wireless/ath/ath10k/ce.c b/drivers/net/wireless/ath/ath10k/ce.c
index d44d618b05f9..a79499c82350 100644
--- a/drivers/net/wireless/ath/ath10k/ce.c
+++ b/drivers/net/wireless/ath/ath10k/ce.c
@@ -266,12 +266,12 @@ static inline void ath10k_ce_engine_int_status_clear(struct ath10k *ar,
  * ath10k_ce_sendlist_send.
  * The caller takes responsibility for any needed locking.
  */
-static int ath10k_ce_send_nolock(struct ath10k_ce_pipe *ce_state,
-				 void *per_transfer_context,
-				 u32 buffer,
-				 unsigned int nbytes,
-				 unsigned int transfer_id,
-				 unsigned int flags)
+int ath10k_ce_send_nolock(struct ath10k_ce_pipe *ce_state,
+			  void *per_transfer_context,
+			  u32 buffer,
+			  unsigned int nbytes,
+			  unsigned int transfer_id,
+			  unsigned int flags)
 {
 	struct ath10k *ar = ce_state->ar;
 	struct ath10k_ce_ring *src_ring = ce_state->src_ring;
@@ -1067,9 +1067,9 @@ struct ath10k_ce_pipe *ath10k_ce_init(struct ath10k *ar,
 	 *
 	 * For the lack of a better place do the check here.
 	 */
-	BUILD_BUG_ON(TARGET_NUM_MSDU_DESC >
+	BUILD_BUG_ON(2*TARGET_NUM_MSDU_DESC >
 		     (CE_HTT_H2T_MSG_SRC_NENTRIES - 1));
-	BUILD_BUG_ON(TARGET_10X_NUM_MSDU_DESC >
+	BUILD_BUG_ON(2*TARGET_10X_NUM_MSDU_DESC >
 		     (CE_HTT_H2T_MSG_SRC_NENTRIES - 1));
 
 	ret = ath10k_pci_wake(ar);
diff --git a/drivers/net/wireless/ath/ath10k/ce.h b/drivers/net/wireless/ath/ath10k/ce.h
index 67dbde6a5c74..8eb7f99ed992 100644
--- a/drivers/net/wireless/ath/ath10k/ce.h
+++ b/drivers/net/wireless/ath/ath10k/ce.h
@@ -23,7 +23,7 @@
 
 /* Maximum number of Copy Engine's supported */
 #define CE_COUNT_MAX 8
-#define CE_HTT_H2T_MSG_SRC_NENTRIES 2048
+#define CE_HTT_H2T_MSG_SRC_NENTRIES 4096
 
 /* Descriptor rings must be aligned to this boundary */
 #define CE_DESC_RING_ALIGN	8
@@ -152,6 +152,13 @@ int ath10k_ce_send(struct ath10k_ce_pipe *ce_state,
 		   unsigned int transfer_id,
 		   unsigned int flags);
 
+int ath10k_ce_send_nolock(struct ath10k_ce_pipe *ce_state,
+			  void *per_transfer_context,
+			  u32 buffer,
+			  unsigned int nbytes,
+			  unsigned int transfer_id,
+			  unsigned int flags);
+
 void ath10k_ce_send_cb_register(struct ath10k_ce_pipe *ce_state,
 				void (*send_cb)(struct ath10k_ce_pipe *),
 				int disable_interrupts);
diff --git a/drivers/net/wireless/ath/ath10k/core.c b/drivers/net/wireless/ath/ath10k/core.c
index 3b59af3bddf4..ebc5fc2ede75 100644
--- a/drivers/net/wireless/ath/ath10k/core.c
+++ b/drivers/net/wireless/ath/ath10k/core.c
@@ -55,8 +55,7 @@ static void ath10k_send_suspend_complete(struct ath10k *ar)
 {
 	ath10k_dbg(ATH10K_DBG_BOOT, "boot suspend complete\n");
 
-	ar->is_target_paused = true;
-	wake_up(&ar->event_queue);
+	complete(&ar->target_suspend);
 }
 
 static int ath10k_init_connect_htc(struct ath10k *ar)
@@ -470,8 +469,12 @@ static int ath10k_core_fetch_firmware_api_n(struct ath10k *ar, const char *name)
 				if (index == ie_len)
 					break;
 
-				if (data[index] & (1 << bit))
+				if (data[index] & (1 << bit)) {
+					ath10k_dbg(ATH10K_DBG_BOOT,
+						   "Enabling feature bit: %i\n",
+						   i);
 					__set_bit(i, ar->fw_features);
+				}
 			}
 
 			ath10k_dbg_dump(ATH10K_DBG_BOOT, "features", "",
@@ -699,6 +702,7 @@ struct ath10k *ath10k_core_create(void *hif_priv, struct device *dev,
 	init_completion(&ar->scan.started);
 	init_completion(&ar->scan.completed);
 	init_completion(&ar->scan.on_channel);
+	init_completion(&ar->target_suspend);
 
 	init_completion(&ar->install_key_done);
 	init_completion(&ar->vdev_setup_done);
@@ -722,8 +726,6 @@ struct ath10k *ath10k_core_create(void *hif_priv, struct device *dev,
 	INIT_WORK(&ar->wmi_mgmt_tx_work, ath10k_mgmt_over_wmi_tx_work);
 	skb_queue_head_init(&ar->wmi_mgmt_tx_queue);
 
-	init_waitqueue_head(&ar->event_queue);
-
 	INIT_WORK(&ar->restart_work, ath10k_core_restart);
 
 	return ar;
@@ -856,10 +858,34 @@ err:
 }
 EXPORT_SYMBOL(ath10k_core_start);
 
+int ath10k_wait_for_suspend(struct ath10k *ar, u32 suspend_opt)
+{
+	int ret;
+
+	reinit_completion(&ar->target_suspend);
+
+	ret = ath10k_wmi_pdev_suspend_target(ar, suspend_opt);
+	if (ret) {
+		ath10k_warn("could not suspend target (%d)\n", ret);
+		return ret;
+	}
+
+	ret = wait_for_completion_timeout(&ar->target_suspend, 1 * HZ);
+
+	if (ret == 0) {
+		ath10k_warn("suspend timed out - target pause event never came\n");
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
 void ath10k_core_stop(struct ath10k *ar)
 {
 	lockdep_assert_held(&ar->conf_mutex);
 
+	/* try to suspend target */
+	ath10k_wait_for_suspend(ar, WMI_PDEV_SUSPEND_AND_DISABLE_INTR);
 	ath10k_debug_stop(ar);
 	ath10k_htc_stop(&ar->htc);
 	ath10k_htt_detach(&ar->htt);
diff --git a/drivers/net/wireless/ath/ath10k/core.h b/drivers/net/wireless/ath/ath10k/core.h
index ade1781c7186..0e71979d837c 100644
--- a/drivers/net/wireless/ath/ath10k/core.h
+++ b/drivers/net/wireless/ath/ath10k/core.h
@@ -46,21 +46,35 @@
 
 #define ATH10K_MAX_NUM_MGMT_PENDING 128
 
+/* number of failed packets */
+#define ATH10K_KICKOUT_THRESHOLD 50
+
+/*
+ * Use insanely high numbers to make sure that the firmware implementation
+ * won't start, we have the same functionality already in hostapd. Unit
+ * is seconds.
+ */
+#define ATH10K_KEEPALIVE_MIN_IDLE 3747
+#define ATH10K_KEEPALIVE_MAX_IDLE 3895
+#define ATH10K_KEEPALIVE_MAX_UNRESPONSIVE 3900
+
 struct ath10k;
 
 struct ath10k_skb_cb {
 	dma_addr_t paddr;
-	bool is_mapped;
-	bool is_aborted;
 	u8 vdev_id;
 
 	struct {
 		u8 tid;
 		bool is_offchan;
-
-		u8 frag_len;
-		u8 pad_len;
+		struct ath10k_htt_txbuf *txbuf;
+		u32 txbuf_paddr;
 	} __packed htt;
+
+	struct {
+		bool dtim_zero;
+		bool deliver_cab;
+	} bcn;
 } __packed;
 
 static inline struct ath10k_skb_cb *ATH10K_SKB_CB(struct sk_buff *skb)
@@ -70,32 +84,6 @@ static inline struct ath10k_skb_cb *ATH10K_SKB_CB(struct sk_buff *skb)
 	return (struct ath10k_skb_cb *)&IEEE80211_SKB_CB(skb)->driver_data;
 }
 
-static inline int ath10k_skb_map(struct device *dev, struct sk_buff *skb)
-{
-	if (ATH10K_SKB_CB(skb)->is_mapped)
-		return -EINVAL;
-
-	ATH10K_SKB_CB(skb)->paddr = dma_map_single(dev, skb->data, skb->len,
-						   DMA_TO_DEVICE);
-
-	if (unlikely(dma_mapping_error(dev, ATH10K_SKB_CB(skb)->paddr)))
-		return -EIO;
-
-	ATH10K_SKB_CB(skb)->is_mapped = true;
-	return 0;
-}
-
-static inline int ath10k_skb_unmap(struct device *dev, struct sk_buff *skb)
-{
-	if (!ATH10K_SKB_CB(skb)->is_mapped)
-		return -EINVAL;
-
-	dma_unmap_single(dev, ATH10K_SKB_CB(skb)->paddr, skb->len,
-			 DMA_TO_DEVICE);
-	ATH10K_SKB_CB(skb)->is_mapped = false;
-	return 0;
-}
-
 static inline u32 host_interest_item_address(u32 item_offset)
 {
 	return QCA988X_HOST_INTEREST_ADDRESS + item_offset;
@@ -211,6 +199,18 @@ struct ath10k_peer {
 	struct ieee80211_key_conf *keys[WMI_MAX_KEY_INDEX + 1];
 };
 
+struct ath10k_sta {
+	struct ath10k_vif *arvif;
+
+	/* the following are protected by ar->data_lock */
+	u32 changed; /* IEEE80211_RC_* */
+	u32 bw;
+	u32 nss;
+	u32 smps;
+
+	struct work_struct update_wk;
+};
+
 #define ATH10K_VDEV_SETUP_TIMEOUT_HZ (5*HZ)
 
 struct ath10k_vif {
@@ -222,10 +222,17 @@ struct ath10k_vif {
 	u32 beacon_interval;
 	u32 dtim_period;
 	struct sk_buff *beacon;
+	/* protected by data_lock */
+	bool beacon_sent;
 
 	struct ath10k *ar;
 	struct ieee80211_vif *vif;
 
+	bool is_started;
+	bool is_up;
+	u32 aid;
+	u8 bssid[ETH_ALEN];
+
 	struct work_struct wep_key_work;
 	struct ieee80211_key_conf *wep_keys[WMI_MAX_KEY_INDEX + 1];
 	u8 def_wep_key_idx;
@@ -235,7 +242,6 @@ struct ath10k_vif {
 
 	union {
 		struct {
-			u8 bssid[ETH_ALEN];
 			u32 uapsd;
 		} sta;
 		struct {
@@ -249,13 +255,11 @@ struct ath10k_vif {
 			u32 noa_len;
 			u8 *noa_data;
 		} ap;
-		struct {
-			u8 bssid[ETH_ALEN];
-		} ibss;
 	} u;
 
 	u8 fixed_rate;
 	u8 fixed_nss;
+	u8 force_sgi;
 };
 
 struct ath10k_vif_iter {
@@ -355,8 +359,7 @@ struct ath10k {
 		const struct ath10k_hif_ops *ops;
 	} hif;
 
-	wait_queue_head_t event_queue;
-	bool is_target_paused;
+	struct completion target_suspend;
 
 	struct ath10k_bmi bmi;
 	struct ath10k_wmi wmi;
@@ -412,6 +415,9 @@ struct ath10k {
 	/* valid during scan; needed for mgmt rx during scan */
 	struct ieee80211_channel *scan_channel;
 
+	/* current operating channel definition */
+	struct cfg80211_chan_def chandef;
+
 	int free_vdev_map;
 	int monitor_vdev_id;
 	bool monitor_enabled;
@@ -470,6 +476,7 @@ struct ath10k *ath10k_core_create(void *hif_priv, struct device *dev,
 void ath10k_core_destroy(struct ath10k *ar);
 
 int ath10k_core_start(struct ath10k *ar);
+int ath10k_wait_for_suspend(struct ath10k *ar, u32 suspend_opt);
 void ath10k_core_stop(struct ath10k *ar);
 int ath10k_core_register(struct ath10k *ar, u32 chip_id);
 void ath10k_core_unregister(struct ath10k *ar);
diff --git a/drivers/net/wireless/ath/ath10k/debug.h b/drivers/net/wireless/ath/ath10k/debug.h
index 1773c36c71a0..a5824990bd2a 100644
--- a/drivers/net/wireless/ath/ath10k/debug.h
+++ b/drivers/net/wireless/ath/ath10k/debug.h
@@ -92,7 +92,7 @@ static inline void ath10k_debug_read_target_stats(struct ath10k *ar,
 
 #ifdef CONFIG_ATH10K_DEBUG
 __printf(2, 3) void ath10k_dbg(enum ath10k_debug_mask mask,
-				      const char *fmt, ...);
+			       const char *fmt, ...);
 void ath10k_dbg_dump(enum ath10k_debug_mask mask,
 		     const char *msg, const char *prefix,
 		     const void *buf, size_t len);
diff --git a/drivers/net/wireless/ath/ath10k/hif.h b/drivers/net/wireless/ath/ath10k/hif.h
index dcdea68bcc0a..2ac7beacddca 100644
--- a/drivers/net/wireless/ath/ath10k/hif.h
+++ b/drivers/net/wireless/ath/ath10k/hif.h
@@ -21,6 +21,14 @@
 #include <linux/kernel.h>
 #include "core.h"
 
+struct ath10k_hif_sg_item {
+	u16 transfer_id;
+	void *transfer_context; /* NULL = tx completion callback not called */
+	void *vaddr; /* for debugging mostly */
+	u32 paddr;
+	u16 len;
+};
+
 struct ath10k_hif_cb {
 	int (*tx_completion)(struct ath10k *ar,
 			     struct sk_buff *wbuf,
@@ -31,11 +39,9 @@ struct ath10k_hif_cb {
 };
 
 struct ath10k_hif_ops {
-	/* Send the head of a buffer to HIF for transmission to the target. */
-	int (*send_head)(struct ath10k *ar, u8 pipe_id,
-			 unsigned int transfer_id,
-			 unsigned int nbytes,
-			 struct sk_buff *buf);
+	/* send a scatter-gather list to the target */
+	int (*tx_sg)(struct ath10k *ar, u8 pipe_id,
+		     struct ath10k_hif_sg_item *items, int n_items);
 
 	/*
 	 * API to handle HIF-specific BMI message exchanges, this API is
@@ -86,12 +92,11 @@ struct ath10k_hif_ops {
 };
 
 
-static inline int ath10k_hif_send_head(struct ath10k *ar, u8 pipe_id,
-				       unsigned int transfer_id,
-				       unsigned int nbytes,
-				       struct sk_buff *buf)
+static inline int ath10k_hif_tx_sg(struct ath10k *ar, u8 pipe_id,
+				   struct ath10k_hif_sg_item *items,
+				   int n_items)
 {
-	return ar->hif.ops->send_head(ar, pipe_id, transfer_id, nbytes, buf);
+	return ar->hif.ops->tx_sg(ar, pipe_id, items, n_items);
 }
 
 static inline int ath10k_hif_exchange_bmi_msg(struct ath10k *ar,
diff --git a/drivers/net/wireless/ath/ath10k/htc.c b/drivers/net/wireless/ath/ath10k/htc.c
index edc57ab505c8..7f1bccd3597f 100644
--- a/drivers/net/wireless/ath/ath10k/htc.c
+++ b/drivers/net/wireless/ath/ath10k/htc.c
@@ -63,7 +63,9 @@ static struct sk_buff *ath10k_htc_build_tx_ctrl_skb(void *ar)
 static inline void ath10k_htc_restore_tx_skb(struct ath10k_htc *htc,
 					     struct sk_buff *skb)
 {
-	ath10k_skb_unmap(htc->ar->dev, skb);
+	struct ath10k_skb_cb *skb_cb = ATH10K_SKB_CB(skb);
+
+	dma_unmap_single(htc->ar->dev, skb_cb->paddr, skb->len, DMA_TO_DEVICE);
 	skb_pull(skb, sizeof(struct ath10k_htc_hdr));
 }
 
@@ -122,6 +124,9 @@ int ath10k_htc_send(struct ath10k_htc *htc,
 		    struct sk_buff *skb)
 {
 	struct ath10k_htc_ep *ep = &htc->endpoint[eid];
+	struct ath10k_skb_cb *skb_cb = ATH10K_SKB_CB(skb);
+	struct ath10k_hif_sg_item sg_item;
+	struct device *dev = htc->ar->dev;
 	int credits = 0;
 	int ret;
 
@@ -157,19 +162,25 @@ int ath10k_htc_send(struct ath10k_htc *htc,
 
 	ath10k_htc_prepare_tx_skb(ep, skb);
 
-	ret = ath10k_skb_map(htc->ar->dev, skb);
+	skb_cb->paddr = dma_map_single(dev, skb->data, skb->len, DMA_TO_DEVICE);
+	ret = dma_mapping_error(dev, skb_cb->paddr);
 	if (ret)
 		goto err_credits;
 
-	ret = ath10k_hif_send_head(htc->ar, ep->ul_pipe_id, ep->eid,
-				   skb->len, skb);
+	sg_item.transfer_id = ep->eid;
+	sg_item.transfer_context = skb;
+	sg_item.vaddr = skb->data;
+	sg_item.paddr = skb_cb->paddr;
+	sg_item.len = skb->len;
+
+	ret = ath10k_hif_tx_sg(htc->ar, ep->ul_pipe_id, &sg_item, 1);
 	if (ret)
 		goto err_unmap;
 
 	return 0;
 
 err_unmap:
-	ath10k_skb_unmap(htc->ar->dev, skb);
+	dma_unmap_single(dev, skb_cb->paddr, skb->len, DMA_TO_DEVICE);
 err_credits:
 	if (ep->tx_credit_flow_enabled) {
 		spin_lock_bh(&htc->tx_lock);
@@ -191,10 +202,8 @@ static int ath10k_htc_tx_completion_handler(struct ath10k *ar,
 	struct ath10k_htc *htc = &ar->htc;
 	struct ath10k_htc_ep *ep = &htc->endpoint[eid];
 
-	if (!skb) {
-		ath10k_warn("invalid sk_buff completion - NULL pointer. firmware crashed?\n");
+	if (WARN_ON_ONCE(!skb))
 		return 0;
-	}
 
 	ath10k_htc_notify_tx_completion(ep, skb);
 	/* the skb now belongs to the completion handler */
diff --git a/drivers/net/wireless/ath/ath10k/htt.h b/drivers/net/wireless/ath/ath10k/htt.h
index b93ae355bc08..654867fc1ae7 100644
--- a/drivers/net/wireless/ath/ath10k/htt.h
+++ b/drivers/net/wireless/ath/ath10k/htt.h
@@ -20,6 +20,7 @@
 
 #include <linux/bug.h>
 #include <linux/interrupt.h>
+#include <linux/dmapool.h>
 
 #include "htc.h"
 #include "rx_desc.h"
@@ -1181,11 +1182,20 @@ struct htt_rx_info {
 		u32 info1;
 		u32 info2;
 	} rate;
+
+	u32 tsf;
 	bool fcs_err;
 	bool amsdu_more;
 	bool mic_err;
 };
 
+struct ath10k_htt_txbuf {
+	struct htt_data_tx_desc_frag frags[2];
+	struct ath10k_htc_hdr htc_hdr;
+	struct htt_cmd_hdr cmd_hdr;
+	struct htt_data_tx_desc cmd_tx;
+} __packed;
+
 struct ath10k_htt {
 	struct ath10k *ar;
 	enum ath10k_htc_ep_id eid;
@@ -1267,11 +1277,18 @@ struct ath10k_htt {
 	struct sk_buff **pending_tx;
 	unsigned long *used_msdu_ids; /* bitmap */
 	wait_queue_head_t empty_tx_wq;
+	struct dma_pool *tx_pool;
 
 	/* set if host-fw communication goes haywire
 	 * used to avoid further failures */
 	bool rx_confused;
 	struct tasklet_struct rx_replenish_task;
+
+	/* This is used to group tx/rx completions separately and process them
+	 * in batches to reduce cache stalls */
+	struct tasklet_struct txrx_compl_task;
+	struct sk_buff_head tx_compl_q;
+	struct sk_buff_head rx_compl_q;
 };
 
 #define RX_HTT_HDR_STATUS_LEN 64
@@ -1343,4 +1360,5 @@ int ath10k_htt_tx_alloc_msdu_id(struct ath10k_htt *htt);
 void ath10k_htt_tx_free_msdu_id(struct ath10k_htt *htt, u16 msdu_id);
 int ath10k_htt_mgmt_tx(struct ath10k_htt *htt, struct sk_buff *);
 int ath10k_htt_tx(struct ath10k_htt *htt, struct sk_buff *);
+
 #endif
diff --git a/drivers/net/wireless/ath/ath10k/htt_rx.c b/drivers/net/wireless/ath/ath10k/htt_rx.c
index fe8bd1b59f0e..cdcbe2de95f9 100644
--- a/drivers/net/wireless/ath/ath10k/htt_rx.c
+++ b/drivers/net/wireless/ath/ath10k/htt_rx.c
@@ -43,7 +43,7 @@
 
 
 static int ath10k_htt_rx_get_csum_state(struct sk_buff *skb);
-
+static void ath10k_htt_txrx_compl_task(unsigned long ptr);
 
 static int ath10k_htt_rx_ring_size(struct ath10k_htt *htt)
 {
@@ -225,18 +225,16 @@ static void ath10k_htt_rx_ring_refill_retry(unsigned long arg)
 	ath10k_htt_rx_msdu_buff_replenish(htt);
 }
 
-static unsigned ath10k_htt_rx_ring_elems(struct ath10k_htt *htt)
-{
-	return (__le32_to_cpu(*htt->rx_ring.alloc_idx.vaddr) -
-		htt->rx_ring.sw_rd_idx.msdu_payld) & htt->rx_ring.size_mask;
-}
-
 void ath10k_htt_rx_detach(struct ath10k_htt *htt)
 {
 	int sw_rd_idx = htt->rx_ring.sw_rd_idx.msdu_payld;
 
 	del_timer_sync(&htt->rx_ring.refill_retry_timer);
 	tasklet_kill(&htt->rx_replenish_task);
+	tasklet_kill(&htt->txrx_compl_task);
+
+	skb_queue_purge(&htt->tx_compl_q);
+	skb_queue_purge(&htt->rx_compl_q);
 
 	while (sw_rd_idx != __le32_to_cpu(*(htt->rx_ring.alloc_idx.vaddr))) {
 		struct sk_buff *skb =
@@ -270,10 +268,12 @@ static inline struct sk_buff *ath10k_htt_rx_netbuf_pop(struct ath10k_htt *htt)
 	int idx;
 	struct sk_buff *msdu;
 
-	spin_lock_bh(&htt->rx_ring.lock);
+	lockdep_assert_held(&htt->rx_ring.lock);
 
-	if (ath10k_htt_rx_ring_elems(htt) == 0)
-		ath10k_warn("htt rx ring is empty!\n");
+	if (htt->rx_ring.fill_cnt == 0) {
+		ath10k_warn("tried to pop sk_buff from an empty rx ring\n");
+		return NULL;
+	}
 
 	idx = htt->rx_ring.sw_rd_idx.msdu_payld;
 	msdu = htt->rx_ring.netbufs_ring[idx];
@@ -283,7 +283,6 @@ static inline struct sk_buff *ath10k_htt_rx_netbuf_pop(struct ath10k_htt *htt)
 	htt->rx_ring.sw_rd_idx.msdu_payld = idx;
 	htt->rx_ring.fill_cnt--;
 
-	spin_unlock_bh(&htt->rx_ring.lock);
 	return msdu;
 }
 
@@ -307,8 +306,7 @@ static int ath10k_htt_rx_amsdu_pop(struct ath10k_htt *htt,
 	struct sk_buff *msdu;
 	struct htt_rx_desc *rx_desc;
 
-	if (ath10k_htt_rx_ring_elems(htt) == 0)
-		ath10k_warn("htt rx ring is empty!\n");
+	lockdep_assert_held(&htt->rx_ring.lock);
 
 	if (htt->rx_confused) {
 		ath10k_warn("htt is confused. refusing rx\n");
@@ -324,7 +322,7 @@ static int ath10k_htt_rx_amsdu_pop(struct ath10k_htt *htt,
 				 msdu->len + skb_tailroom(msdu),
 				 DMA_FROM_DEVICE);
 
-		ath10k_dbg_dump(ATH10K_DBG_HTT_DUMP, NULL, "htt rx: ",
+		ath10k_dbg_dump(ATH10K_DBG_HTT_DUMP, NULL, "htt rx pop: ",
 				msdu->data, msdu->len + skb_tailroom(msdu));
 
 		rx_desc = (struct htt_rx_desc *)msdu->data;
@@ -400,6 +398,7 @@ static int ath10k_htt_rx_amsdu_pop(struct ath10k_htt *htt,
 		msdu_len = MS(__le32_to_cpu(rx_desc->msdu_start.info0),
 			      RX_MSDU_START_INFO0_MSDU_LENGTH);
 		msdu_chained = rx_desc->frag_info.ring2_more_count;
+		msdu_chaining = msdu_chained;
 
 		if (msdu_len_invalid)
 			msdu_len = 0;
@@ -417,8 +416,8 @@ static int ath10k_htt_rx_amsdu_pop(struct ath10k_htt *htt,
 					 next->len + skb_tailroom(next),
 					 DMA_FROM_DEVICE);
 
-			ath10k_dbg_dump(ATH10K_DBG_HTT_DUMP, NULL, "htt rx: ",
-					next->data,
+			ath10k_dbg_dump(ATH10K_DBG_HTT_DUMP, NULL,
+					"htt rx chained: ", next->data,
 					next->len + skb_tailroom(next));
 
 			skb_trim(next, 0);
@@ -427,13 +426,6 @@ static int ath10k_htt_rx_amsdu_pop(struct ath10k_htt *htt,
 
 			msdu->next = next;
 			msdu = next;
-			msdu_chaining = 1;
-		}
-
-		if (msdu_len > 0) {
-			/* This may suggest FW bug? */
-			ath10k_warn("htt rx msdu len not consumed (%d)\n",
-				    msdu_len);
 		}
 
 		last_msdu = __le32_to_cpu(rx_desc->msdu_end.info0) &
@@ -535,6 +527,12 @@ int ath10k_htt_rx_attach(struct ath10k_htt *htt)
 	tasklet_init(&htt->rx_replenish_task, ath10k_htt_rx_replenish_task,
 		     (unsigned long)htt);
 
+	skb_queue_head_init(&htt->tx_compl_q);
+	skb_queue_head_init(&htt->rx_compl_q);
+
+	tasklet_init(&htt->txrx_compl_task, ath10k_htt_txrx_compl_task,
+		     (unsigned long)htt);
+
 	ath10k_dbg(ATH10K_DBG_BOOT, "htt rx ring size %d fill_level %d\n",
 		   htt->rx_ring.size, htt->rx_ring.fill_level);
 	return 0;
@@ -638,6 +636,12 @@ struct amsdu_subframe_hdr {
 	__be16 len;
 } __packed;
 
+static int ath10k_htt_rx_nwifi_hdrlen(struct ieee80211_hdr *hdr)
+{
+	/* nwifi header is padded to 4 bytes. this fixes 4addr rx */
+	return round_up(ieee80211_hdrlen(hdr->frame_control), 4);
+}
+
 static void ath10k_htt_rx_amsdu(struct ath10k_htt *htt,
 				struct htt_rx_info *info)
 {
@@ -687,7 +691,7 @@ static void ath10k_htt_rx_amsdu(struct ath10k_htt *htt,
 		case RX_MSDU_DECAP_NATIVE_WIFI:
 			/* pull decapped header and copy DA */
 			hdr = (struct ieee80211_hdr *)skb->data;
-			hdr_len = ieee80211_hdrlen(hdr->frame_control);
+			hdr_len = ath10k_htt_rx_nwifi_hdrlen(hdr);
 			memcpy(addr, ieee80211_get_DA(hdr), ETH_ALEN);
 			skb_pull(skb, hdr_len);
 
@@ -751,7 +755,7 @@ static void ath10k_htt_rx_msdu(struct ath10k_htt *htt, struct htt_rx_info *info)
 
 	/* This shouldn't happen. If it does than it may be a FW bug. */
 	if (skb->next) {
-		ath10k_warn("received chained non A-MSDU frame\n");
+		ath10k_warn("htt rx received chained non A-MSDU frame\n");
 		ath10k_htt_rx_free_msdu_chain(skb->next);
 		skb->next = NULL;
 	}
@@ -774,7 +778,7 @@ static void ath10k_htt_rx_msdu(struct ath10k_htt *htt, struct htt_rx_info *info)
 	case RX_MSDU_DECAP_NATIVE_WIFI:
 		/* Pull decapped header */
 		hdr = (struct ieee80211_hdr *)skb->data;
-		hdr_len = ieee80211_hdrlen(hdr->frame_control);
+		hdr_len = ath10k_htt_rx_nwifi_hdrlen(hdr);
 		skb_pull(skb, hdr_len);
 
 		/* Push original header */
@@ -852,6 +856,20 @@ static bool ath10k_htt_rx_has_mic_err(struct sk_buff *skb)
 	return false;
 }
 
+static bool ath10k_htt_rx_is_mgmt(struct sk_buff *skb)
+{
+	struct htt_rx_desc *rxd;
+	u32 flags;
+
+	rxd = (void *)skb->data - sizeof(*rxd);
+	flags = __le32_to_cpu(rxd->attention.flags);
+
+	if (flags & RX_ATTENTION_FLAGS_MGMT_TYPE)
+		return true;
+
+	return false;
+}
+
 static int ath10k_htt_rx_get_csum_state(struct sk_buff *skb)
 {
 	struct htt_rx_desc *rxd;
@@ -883,6 +901,57 @@ static int ath10k_htt_rx_get_csum_state(struct sk_buff *skb)
 	return CHECKSUM_UNNECESSARY;
 }
 
+static int ath10k_unchain_msdu(struct sk_buff *msdu_head)
+{
+	struct sk_buff *next = msdu_head->next;
+	struct sk_buff *to_free = next;
+	int space;
+	int total_len = 0;
+
+	/* TODO:  Might could optimize this by using
+	 * skb_try_coalesce or similar method to
+	 * decrease copying, or maybe get mac80211 to
+	 * provide a way to just receive a list of
+	 * skb?
+	 */
+
+	msdu_head->next = NULL;
+
+	/* Allocate total length all at once. */
+	while (next) {
+		total_len += next->len;
+		next = next->next;
+	}
+
+	space = total_len - skb_tailroom(msdu_head);
+	if ((space > 0) &&
+	    (pskb_expand_head(msdu_head, 0, space, GFP_ATOMIC) < 0)) {
+		/* TODO:  bump some rx-oom error stat */
+		/* put it back together so we can free the
+		 * whole list at once.
+		 */
+		msdu_head->next = to_free;
+		return -1;
+	}
+
+	/* Walk list again, copying contents into
+	 * msdu_head
+	 */
+	next = to_free;
+	while (next) {
+		skb_copy_from_linear_data(next, skb_put(msdu_head, next->len),
+					  next->len);
+		next = next->next;
+	}
+
+	/* If here, we have consolidated skb.  Free the
+	 * fragments and pass the main skb on up the
+	 * stack.
+	 */
+	ath10k_htt_rx_free_msdu_chain(to_free);
+	return 0;
+}
+
 static void ath10k_htt_rx_handler(struct ath10k_htt *htt,
 				  struct htt_rx_indication *rx)
 {
@@ -894,6 +963,8 @@ static void ath10k_htt_rx_handler(struct ath10k_htt *htt,
 	u8 *fw_desc;
 	int i, j;
 
+	lockdep_assert_held(&htt->rx_ring.lock);
+
 	memset(&info, 0, sizeof(info));
 
 	fw_desc_len = __le16_to_cpu(rx->prefix.fw_rx_desc_bytes);
@@ -937,6 +1008,8 @@ static void ath10k_htt_rx_handler(struct ath10k_htt *htt,
 			}
 
 			if (ath10k_htt_rx_has_decrypt_err(msdu_head)) {
+				ath10k_dbg(ATH10K_DBG_HTT,
+					   "htt rx dropping due to decrypt-err\n");
 				ath10k_htt_rx_free_msdu_chain(msdu_head);
 				continue;
 			}
@@ -944,13 +1017,16 @@ static void ath10k_htt_rx_handler(struct ath10k_htt *htt,
 			status = info.status;
 
 			/* Skip mgmt frames while we handle this in WMI */
-			if (status == HTT_RX_IND_MPDU_STATUS_MGMT_CTRL) {
+			if (status == HTT_RX_IND_MPDU_STATUS_MGMT_CTRL ||
+			    ath10k_htt_rx_is_mgmt(msdu_head)) {
+				ath10k_dbg(ATH10K_DBG_HTT, "htt rx mgmt ctrl\n");
 				ath10k_htt_rx_free_msdu_chain(msdu_head);
 				continue;
 			}
 
 			if (status != HTT_RX_IND_MPDU_STATUS_OK &&
 			    status != HTT_RX_IND_MPDU_STATUS_TKIP_MIC_ERR &&
+			    status != HTT_RX_IND_MPDU_STATUS_ERR_INV_PEER &&
 			    !htt->ar->monitor_enabled) {
 				ath10k_dbg(ATH10K_DBG_HTT,
 					   "htt rx ignoring frame w/ status %d\n",
@@ -960,14 +1036,14 @@ static void ath10k_htt_rx_handler(struct ath10k_htt *htt,
 			}
 
 			if (test_bit(ATH10K_CAC_RUNNING, &htt->ar->dev_flags)) {
+				ath10k_dbg(ATH10K_DBG_HTT,
+					   "htt rx CAC running\n");
 				ath10k_htt_rx_free_msdu_chain(msdu_head);
 				continue;
 			}
 
-			/* FIXME: we do not support chaining yet.
-			 * this needs investigation */
-			if (msdu_chaining) {
-				ath10k_warn("msdu_chaining is true\n");
+			if (msdu_chaining &&
+			    (ath10k_unchain_msdu(msdu_head) < 0)) {
 				ath10k_htt_rx_free_msdu_chain(msdu_head);
 				continue;
 			}
@@ -975,12 +1051,22 @@ static void ath10k_htt_rx_handler(struct ath10k_htt *htt,
 			info.skb     = msdu_head;
 			info.fcs_err = ath10k_htt_rx_has_fcs_err(msdu_head);
 			info.mic_err = ath10k_htt_rx_has_mic_err(msdu_head);
+
+			if (info.fcs_err)
+				ath10k_dbg(ATH10K_DBG_HTT,
+					   "htt rx has FCS err\n");
+
+			if (info.mic_err)
+				ath10k_dbg(ATH10K_DBG_HTT,
+					   "htt rx has MIC err\n");
+
 			info.signal  = ATH10K_DEFAULT_NOISE_FLOOR;
 			info.signal += rx->ppdu.combined_rssi;
 
 			info.rate.info0 = rx->ppdu.info0;
 			info.rate.info1 = __le32_to_cpu(rx->ppdu.info1);
 			info.rate.info2 = __le32_to_cpu(rx->ppdu.info2);
+			info.tsf = __le32_to_cpu(rx->ppdu.tsf);
 
 			hdr = ath10k_htt_rx_skb_get_hdr(msdu_head);
 
@@ -1014,8 +1100,11 @@ static void ath10k_htt_rx_frag_handler(struct ath10k_htt *htt,
 
 	msdu_head = NULL;
 	msdu_tail = NULL;
+
+	spin_lock_bh(&htt->rx_ring.lock);
 	msdu_chaining = ath10k_htt_rx_amsdu_pop(htt, &fw_desc, &fw_desc_len,
 						&msdu_head, &msdu_tail);
+	spin_unlock_bh(&htt->rx_ring.lock);
 
 	ath10k_dbg(ATH10K_DBG_HTT_DUMP, "htt rx frag ahead\n");
 
@@ -1095,7 +1184,7 @@ static void ath10k_htt_rx_frag_handler(struct ath10k_htt *htt,
 
 	skb_trim(info.skb, info.skb->len - trim);
 
-	ath10k_dbg_dump(ATH10K_DBG_HTT_DUMP, NULL, "htt frag mpdu: ",
+	ath10k_dbg_dump(ATH10K_DBG_HTT_DUMP, NULL, "htt rx frag mpdu: ",
 			info.skb->data, info.skb->len);
 	ath10k_process_rx(htt->ar, &info);
 
@@ -1107,6 +1196,45 @@ end:
 	}
 }
 
+static void ath10k_htt_rx_frm_tx_compl(struct ath10k *ar,
+				       struct sk_buff *skb)
+{
+	struct ath10k_htt *htt = &ar->htt;
+	struct htt_resp *resp = (struct htt_resp *)skb->data;
+	struct htt_tx_done tx_done = {};
+	int status = MS(resp->data_tx_completion.flags, HTT_DATA_TX_STATUS);
+	__le16 msdu_id;
+	int i;
+
+	lockdep_assert_held(&htt->tx_lock);
+
+	switch (status) {
+	case HTT_DATA_TX_STATUS_NO_ACK:
+		tx_done.no_ack = true;
+		break;
+	case HTT_DATA_TX_STATUS_OK:
+		break;
+	case HTT_DATA_TX_STATUS_DISCARD:
+	case HTT_DATA_TX_STATUS_POSTPONE:
+	case HTT_DATA_TX_STATUS_DOWNLOAD_FAIL:
+		tx_done.discard = true;
+		break;
+	default:
+		ath10k_warn("unhandled tx completion status %d\n", status);
+		tx_done.discard = true;
+		break;
+	}
+
+	ath10k_dbg(ATH10K_DBG_HTT, "htt tx completion num_msdus %d\n",
+		   resp->data_tx_completion.num_msdus);
+
+	for (i = 0; i < resp->data_tx_completion.num_msdus; i++) {
+		msdu_id = resp->data_tx_completion.msdus[i];
+		tx_done.msdu_id = __le16_to_cpu(msdu_id);
+		ath10k_txrx_tx_unref(htt, &tx_done);
+	}
+}
+
 void ath10k_htt_t2h_msg_handler(struct ath10k *ar, struct sk_buff *skb)
 {
 	struct ath10k_htt *htt = &ar->htt;
@@ -1116,7 +1244,7 @@ void ath10k_htt_t2h_msg_handler(struct ath10k *ar, struct sk_buff *skb)
 	if (!IS_ALIGNED((unsigned long)skb->data, 4))
 		ath10k_warn("unaligned htt message, expect trouble\n");
 
-	ath10k_dbg(ATH10K_DBG_HTT, "HTT RX, msg_type: 0x%0X\n",
+	ath10k_dbg(ATH10K_DBG_HTT, "htt rx, msg_type: 0x%0X\n",
 		   resp->hdr.msg_type);
 	switch (resp->hdr.msg_type) {
 	case HTT_T2H_MSG_TYPE_VERSION_CONF: {
@@ -1125,10 +1253,12 @@ void ath10k_htt_t2h_msg_handler(struct ath10k *ar, struct sk_buff *skb)
 		complete(&htt->target_version_received);
 		break;
 	}
-	case HTT_T2H_MSG_TYPE_RX_IND: {
-		ath10k_htt_rx_handler(htt, &resp->rx_ind);
-		break;
-	}
+	case HTT_T2H_MSG_TYPE_RX_IND:
+		spin_lock_bh(&htt->rx_ring.lock);
+		__skb_queue_tail(&htt->rx_compl_q, skb);
+		spin_unlock_bh(&htt->rx_ring.lock);
+		tasklet_schedule(&htt->txrx_compl_task);
+		return;
 	case HTT_T2H_MSG_TYPE_PEER_MAP: {
 		struct htt_peer_map_event ev = {
 			.vdev_id = resp->peer_map.vdev_id,
@@ -1163,44 +1293,17 @@ void ath10k_htt_t2h_msg_handler(struct ath10k *ar, struct sk_buff *skb)
 			break;
 		}
 
+		spin_lock_bh(&htt->tx_lock);
 		ath10k_txrx_tx_unref(htt, &tx_done);
+		spin_unlock_bh(&htt->tx_lock);
 		break;
 	}
-	case HTT_T2H_MSG_TYPE_TX_COMPL_IND: {
-		struct htt_tx_done tx_done = {};
-		int status = MS(resp->data_tx_completion.flags,
-				HTT_DATA_TX_STATUS);
-		__le16 msdu_id;
-		int i;
-
-		switch (status) {
-		case HTT_DATA_TX_STATUS_NO_ACK:
-			tx_done.no_ack = true;
-			break;
-		case HTT_DATA_TX_STATUS_OK:
-			break;
-		case HTT_DATA_TX_STATUS_DISCARD:
-		case HTT_DATA_TX_STATUS_POSTPONE:
-		case HTT_DATA_TX_STATUS_DOWNLOAD_FAIL:
-			tx_done.discard = true;
-			break;
-		default:
-			ath10k_warn("unhandled tx completion status %d\n",
-				    status);
-			tx_done.discard = true;
-			break;
-		}
-
-		ath10k_dbg(ATH10K_DBG_HTT, "htt tx completion num_msdus %d\n",
-			   resp->data_tx_completion.num_msdus);
-
-		for (i = 0; i < resp->data_tx_completion.num_msdus; i++) {
-			msdu_id = resp->data_tx_completion.msdus[i];
-			tx_done.msdu_id = __le16_to_cpu(msdu_id);
-			ath10k_txrx_tx_unref(htt, &tx_done);
-		}
-		break;
-	}
+	case HTT_T2H_MSG_TYPE_TX_COMPL_IND:
+		spin_lock_bh(&htt->tx_lock);
+		__skb_queue_tail(&htt->tx_compl_q, skb);
+		spin_unlock_bh(&htt->tx_lock);
+		tasklet_schedule(&htt->txrx_compl_task);
+		return;
 	case HTT_T2H_MSG_TYPE_SEC_IND: {
 		struct ath10k *ar = htt->ar;
 		struct htt_security_indication *ev = &resp->security_indication;
@@ -1240,3 +1343,25 @@ void ath10k_htt_t2h_msg_handler(struct ath10k *ar, struct sk_buff *skb)
 	/* Free the indication buffer */
 	dev_kfree_skb_any(skb);
 }
+
+static void ath10k_htt_txrx_compl_task(unsigned long ptr)
+{
+	struct ath10k_htt *htt = (struct ath10k_htt *)ptr;
+	struct htt_resp *resp;
+	struct sk_buff *skb;
+
+	spin_lock_bh(&htt->tx_lock);
+	while ((skb = __skb_dequeue(&htt->tx_compl_q))) {
+		ath10k_htt_rx_frm_tx_compl(htt->ar, skb);
+		dev_kfree_skb_any(skb);
+	}
+	spin_unlock_bh(&htt->tx_lock);
+
+	spin_lock_bh(&htt->rx_ring.lock);
+	while ((skb = __skb_dequeue(&htt->rx_compl_q))) {
+		resp = (struct htt_resp *)skb->data;
+		ath10k_htt_rx_handler(htt, &resp->rx_ind);
+		dev_kfree_skb_any(skb);
+	}
+	spin_unlock_bh(&htt->rx_ring.lock);
+}
diff --git a/drivers/net/wireless/ath/ath10k/htt_tx.c b/drivers/net/wireless/ath/ath10k/htt_tx.c
index f1d36d2d2723..7a3e2e40dd5c 100644
--- a/drivers/net/wireless/ath/ath10k/htt_tx.c
+++ b/drivers/net/wireless/ath/ath10k/htt_tx.c
@@ -109,6 +109,14 @@ int ath10k_htt_tx_attach(struct ath10k_htt *htt)
 		return -ENOMEM;
 	}
 
+	htt->tx_pool = dma_pool_create("ath10k htt tx pool", htt->ar->dev,
+				       sizeof(struct ath10k_htt_txbuf), 4, 0);
+	if (!htt->tx_pool) {
+		kfree(htt->used_msdu_ids);
+		kfree(htt->pending_tx);
+		return -ENOMEM;
+	}
+
 	return 0;
 }
 
@@ -117,9 +125,7 @@ static void ath10k_htt_tx_cleanup_pending(struct ath10k_htt *htt)
 	struct htt_tx_done tx_done = {0};
 	int msdu_id;
 
-	/* No locks needed. Called after communication with the device has
-	 * been stopped. */
-
+	spin_lock_bh(&htt->tx_lock);
 	for (msdu_id = 0; msdu_id < htt->max_num_pending_tx; msdu_id++) {
 		if (!test_bit(msdu_id, htt->used_msdu_ids))
 			continue;
@@ -132,6 +138,7 @@ static void ath10k_htt_tx_cleanup_pending(struct ath10k_htt *htt)
 
 		ath10k_txrx_tx_unref(htt, &tx_done);
 	}
+	spin_unlock_bh(&htt->tx_lock);
 }
 
 void ath10k_htt_tx_detach(struct ath10k_htt *htt)
@@ -139,6 +146,7 @@ void ath10k_htt_tx_detach(struct ath10k_htt *htt)
 	ath10k_htt_tx_cleanup_pending(htt);
 	kfree(htt->pending_tx);
 	kfree(htt->used_msdu_ids);
+	dma_pool_destroy(htt->tx_pool);
 	return;
 }
 
@@ -334,7 +342,9 @@ int ath10k_htt_mgmt_tx(struct ath10k_htt *htt, struct sk_buff *msdu)
 		goto err_free_msdu_id;
 	}
 
-	res = ath10k_skb_map(dev, msdu);
+	skb_cb->paddr = dma_map_single(dev, msdu->data, msdu->len,
+				       DMA_TO_DEVICE);
+	res = dma_mapping_error(dev, skb_cb->paddr);
 	if (res)
 		goto err_free_txdesc;
 
@@ -348,8 +358,7 @@ int ath10k_htt_mgmt_tx(struct ath10k_htt *htt, struct sk_buff *msdu)
 	memcpy(cmd->mgmt_tx.hdr, msdu->data,
 	       min_t(int, msdu->len, HTT_MGMT_FRM_HDR_DOWNLOAD_LEN));
 
-	skb_cb->htt.frag_len = 0;
-	skb_cb->htt.pad_len = 0;
+	skb_cb->htt.txbuf = NULL;
 
 	res = ath10k_htc_send(&htt->ar->htc, htt->eid, txdesc);
 	if (res)
@@ -358,7 +367,7 @@ int ath10k_htt_mgmt_tx(struct ath10k_htt *htt, struct sk_buff *msdu)
 	return 0;
 
 err_unmap_msdu:
-	ath10k_skb_unmap(dev, msdu);
+	dma_unmap_single(dev, skb_cb->paddr, msdu->len, DMA_TO_DEVICE);
 err_free_txdesc:
 	dev_kfree_skb_any(txdesc);
 err_free_msdu_id:
@@ -375,19 +384,19 @@ err:
 int ath10k_htt_tx(struct ath10k_htt *htt, struct sk_buff *msdu)
 {
 	struct device *dev = htt->ar->dev;
-	struct htt_cmd *cmd;
-	struct htt_data_tx_desc_frag *tx_frags;
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)msdu->data;
 	struct ath10k_skb_cb *skb_cb = ATH10K_SKB_CB(msdu);
-	struct sk_buff *txdesc = NULL;
-	bool use_frags;
-	u8 vdev_id = ATH10K_SKB_CB(msdu)->vdev_id;
-	u8 tid;
-	int prefetch_len, desc_len;
-	int msdu_id = -1;
+	struct ath10k_hif_sg_item sg_items[2];
+	struct htt_data_tx_desc_frag *frags;
+	u8 vdev_id = skb_cb->vdev_id;
+	u8 tid = skb_cb->htt.tid;
+	int prefetch_len;
 	int res;
-	u8 flags0;
-	u16 flags1;
+	u8 flags0 = 0;
+	u16 msdu_id, flags1 = 0;
+	dma_addr_t paddr;
+	u32 frags_paddr;
+	bool use_frags;
 
 	res = ath10k_htt_tx_inc_pending(htt);
 	if (res)
@@ -406,114 +415,120 @@ int ath10k_htt_tx(struct ath10k_htt *htt, struct sk_buff *msdu)
 	prefetch_len = min(htt->prefetch_len, msdu->len);
 	prefetch_len = roundup(prefetch_len, 4);
 
-	desc_len = sizeof(cmd->hdr) + sizeof(cmd->data_tx) + prefetch_len;
-
-	txdesc = ath10k_htc_alloc_skb(desc_len);
-	if (!txdesc) {
-		res = -ENOMEM;
-		goto err_free_msdu_id;
-	}
-
 	/* Since HTT 3.0 there is no separate mgmt tx command. However in case
 	 * of mgmt tx using TX_FRM there is not tx fragment list. Instead of tx
 	 * fragment list host driver specifies directly frame pointer. */
 	use_frags = htt->target_version_major < 3 ||
 		    !ieee80211_is_mgmt(hdr->frame_control);
 
-	if (!IS_ALIGNED((unsigned long)txdesc->data, 4)) {
-		ath10k_warn("htt alignment check failed. dropping packet.\n");
-		res = -EIO;
-		goto err_free_txdesc;
-	}
+	skb_cb->htt.txbuf = dma_pool_alloc(htt->tx_pool, GFP_ATOMIC,
+					   &paddr);
+	if (!skb_cb->htt.txbuf)
+		goto err_free_msdu_id;
+	skb_cb->htt.txbuf_paddr = paddr;
 
-	if (use_frags) {
-		skb_cb->htt.frag_len = sizeof(*tx_frags) * 2;
-		skb_cb->htt.pad_len = (unsigned long)msdu->data -
-				      round_down((unsigned long)msdu->data, 4);
+	skb_cb->paddr = dma_map_single(dev, msdu->data, msdu->len,
+				       DMA_TO_DEVICE);
+	res = dma_mapping_error(dev, skb_cb->paddr);
+	if (res)
+		goto err_free_txbuf;
 
-		skb_push(msdu, skb_cb->htt.frag_len + skb_cb->htt.pad_len);
-	} else {
-		skb_cb->htt.frag_len = 0;
-		skb_cb->htt.pad_len = 0;
-	}
+	if (likely(use_frags)) {
+		frags = skb_cb->htt.txbuf->frags;
 
-	res = ath10k_skb_map(dev, msdu);
-	if (res)
-		goto err_pull_txfrag;
-
-	if (use_frags) {
-		dma_sync_single_for_cpu(dev, skb_cb->paddr, msdu->len,
-					DMA_TO_DEVICE);
-
-		/* tx fragment list must be terminated with zero-entry */
-		tx_frags = (struct htt_data_tx_desc_frag *)msdu->data;
-		tx_frags[0].paddr = __cpu_to_le32(skb_cb->paddr +
-						  skb_cb->htt.frag_len +
-						  skb_cb->htt.pad_len);
-		tx_frags[0].len   = __cpu_to_le32(msdu->len -
-						  skb_cb->htt.frag_len -
-						  skb_cb->htt.pad_len);
-		tx_frags[1].paddr = __cpu_to_le32(0);
-		tx_frags[1].len   = __cpu_to_le32(0);
-
-		dma_sync_single_for_device(dev, skb_cb->paddr, msdu->len,
-					   DMA_TO_DEVICE);
-	}
+		frags[0].paddr = __cpu_to_le32(skb_cb->paddr);
+		frags[0].len = __cpu_to_le32(msdu->len);
+		frags[1].paddr = 0;
+		frags[1].len = 0;
 
-	ath10k_dbg(ATH10K_DBG_HTT, "msdu 0x%llx\n",
-		   (unsigned long long) ATH10K_SKB_CB(msdu)->paddr);
-	ath10k_dbg_dump(ATH10K_DBG_HTT_DUMP, NULL, "msdu: ",
-			msdu->data, msdu->len);
+		flags0 |= SM(ATH10K_HW_TXRX_NATIVE_WIFI,
+			     HTT_DATA_TX_DESC_FLAGS0_PKT_TYPE);
 
-	skb_put(txdesc, desc_len);
-	cmd = (struct htt_cmd *)txdesc->data;
+		frags_paddr = skb_cb->htt.txbuf_paddr;
+	} else {
+		flags0 |= SM(ATH10K_HW_TXRX_MGMT,
+			     HTT_DATA_TX_DESC_FLAGS0_PKT_TYPE);
 
-	tid = ATH10K_SKB_CB(msdu)->htt.tid;
+		frags_paddr = skb_cb->paddr;
+	}
 
-	ath10k_dbg(ATH10K_DBG_HTT, "htt data tx using tid %hhu\n", tid);
+	/* Normally all commands go through HTC which manages tx credits for
+	 * each endpoint and notifies when tx is completed.
+	 *
+	 * HTT endpoint is creditless so there's no need to care about HTC
+	 * flags. In that case it is trivial to fill the HTC header here.
+	 *
+	 * MSDU transmission is considered completed upon HTT event. This
+	 * implies no relevant resources can be freed until after the event is
+	 * received. That's why HTC tx completion handler itself is ignored by
+	 * setting NULL to transfer_context for all sg items.
+	 *
+	 * There is simply no point in pushing HTT TX_FRM through HTC tx path
+	 * as it's a waste of resources. By bypassing HTC it is possible to
+	 * avoid extra memory allocations, compress data structures and thus
+	 * improve performance. */
+
+	skb_cb->htt.txbuf->htc_hdr.eid = htt->eid;
+	skb_cb->htt.txbuf->htc_hdr.len = __cpu_to_le16(
+			sizeof(skb_cb->htt.txbuf->cmd_hdr) +
+			sizeof(skb_cb->htt.txbuf->cmd_tx) +
+			prefetch_len);
+	skb_cb->htt.txbuf->htc_hdr.flags = 0;
 
-	flags0  = 0;
 	if (!ieee80211_has_protected(hdr->frame_control))
 		flags0 |= HTT_DATA_TX_DESC_FLAGS0_NO_ENCRYPT;
-	flags0 |= HTT_DATA_TX_DESC_FLAGS0_MAC_HDR_PRESENT;
 
-	if (use_frags)
-		flags0 |= SM(ATH10K_HW_TXRX_NATIVE_WIFI,
-			     HTT_DATA_TX_DESC_FLAGS0_PKT_TYPE);
-	else
-		flags0 |= SM(ATH10K_HW_TXRX_MGMT,
-			     HTT_DATA_TX_DESC_FLAGS0_PKT_TYPE);
+	flags0 |= HTT_DATA_TX_DESC_FLAGS0_MAC_HDR_PRESENT;
 
-	flags1  = 0;
 	flags1 |= SM((u16)vdev_id, HTT_DATA_TX_DESC_FLAGS1_VDEV_ID);
 	flags1 |= SM((u16)tid, HTT_DATA_TX_DESC_FLAGS1_EXT_TID);
 	flags1 |= HTT_DATA_TX_DESC_FLAGS1_CKSUM_L3_OFFLOAD;
 	flags1 |= HTT_DATA_TX_DESC_FLAGS1_CKSUM_L4_OFFLOAD;
 
-	cmd->hdr.msg_type        = HTT_H2T_MSG_TYPE_TX_FRM;
-	cmd->data_tx.flags0      = flags0;
-	cmd->data_tx.flags1      = __cpu_to_le16(flags1);
-	cmd->data_tx.len         = __cpu_to_le16(msdu->len -
-						 skb_cb->htt.frag_len -
-						 skb_cb->htt.pad_len);
-	cmd->data_tx.id          = __cpu_to_le16(msdu_id);
-	cmd->data_tx.frags_paddr = __cpu_to_le32(skb_cb->paddr);
-	cmd->data_tx.peerid      = __cpu_to_le32(HTT_INVALID_PEERID);
-
-	memcpy(cmd->data_tx.prefetch, hdr, prefetch_len);
+	skb_cb->htt.txbuf->cmd_hdr.msg_type = HTT_H2T_MSG_TYPE_TX_FRM;
+	skb_cb->htt.txbuf->cmd_tx.flags0 = flags0;
+	skb_cb->htt.txbuf->cmd_tx.flags1 = __cpu_to_le16(flags1);
+	skb_cb->htt.txbuf->cmd_tx.len = __cpu_to_le16(msdu->len);
+	skb_cb->htt.txbuf->cmd_tx.id = __cpu_to_le16(msdu_id);
+	skb_cb->htt.txbuf->cmd_tx.frags_paddr = __cpu_to_le32(frags_paddr);
+	skb_cb->htt.txbuf->cmd_tx.peerid = __cpu_to_le32(HTT_INVALID_PEERID);
+
+	ath10k_dbg(ATH10K_DBG_HTT,
+		   "htt tx flags0 %hhu flags1 %hu len %d id %hu frags_paddr %08x, msdu_paddr %08x vdev %hhu tid %hhu\n",
+		   flags0, flags1, msdu->len, msdu_id, frags_paddr,
+		   (u32)skb_cb->paddr, vdev_id, tid);
+	ath10k_dbg_dump(ATH10K_DBG_HTT_DUMP, NULL, "htt tx msdu: ",
+			msdu->data, msdu->len);
 
-	res = ath10k_htc_send(&htt->ar->htc, htt->eid, txdesc);
+	sg_items[0].transfer_id = 0;
+	sg_items[0].transfer_context = NULL;
+	sg_items[0].vaddr = &skb_cb->htt.txbuf->htc_hdr;
+	sg_items[0].paddr = skb_cb->htt.txbuf_paddr +
+			    sizeof(skb_cb->htt.txbuf->frags);
+	sg_items[0].len = sizeof(skb_cb->htt.txbuf->htc_hdr) +
+			  sizeof(skb_cb->htt.txbuf->cmd_hdr) +
+			  sizeof(skb_cb->htt.txbuf->cmd_tx);
+
+	sg_items[1].transfer_id = 0;
+	sg_items[1].transfer_context = NULL;
+	sg_items[1].vaddr = msdu->data;
+	sg_items[1].paddr = skb_cb->paddr;
+	sg_items[1].len = prefetch_len;
+
+	res = ath10k_hif_tx_sg(htt->ar,
+			       htt->ar->htc.endpoint[htt->eid].ul_pipe_id,
+			       sg_items, ARRAY_SIZE(sg_items));
 	if (res)
 		goto err_unmap_msdu;
 
 	return 0;
 
 err_unmap_msdu:
-	ath10k_skb_unmap(dev, msdu);
-err_pull_txfrag:
-	skb_pull(msdu, skb_cb->htt.frag_len + skb_cb->htt.pad_len);
-err_free_txdesc:
-	dev_kfree_skb_any(txdesc);
+	dma_unmap_single(dev, skb_cb->paddr, msdu->len, DMA_TO_DEVICE);
+err_free_txbuf:
+	dma_pool_free(htt->tx_pool,
+		      skb_cb->htt.txbuf,
+		      skb_cb->htt.txbuf_paddr);
 err_free_msdu_id:
 	spin_lock_bh(&htt->tx_lock);
 	htt->pending_tx[msdu_id] = NULL;
diff --git a/drivers/net/wireless/ath/ath10k/hw.h b/drivers/net/wireless/ath/ath10k/hw.h
index f1505a25d810..35fc44e281f5 100644
--- a/drivers/net/wireless/ath/ath10k/hw.h
+++ b/drivers/net/wireless/ath/ath10k/hw.h
@@ -205,8 +205,11 @@ enum ath10k_mcast2ucast_mode {
 #define WLAN_ANALOG_INTF_PCIE_BASE_ADDRESS	0x0006c000
 #define PCIE_LOCAL_BASE_ADDRESS			0x00080000
 
+#define SOC_RESET_CONTROL_ADDRESS		0x00000000
 #define SOC_RESET_CONTROL_OFFSET		0x00000000
 #define SOC_RESET_CONTROL_SI0_RST_MASK		0x00000001
+#define SOC_RESET_CONTROL_CE_RST_MASK		0x00040000
+#define SOC_RESET_CONTROL_CPU_WARM_RST_MASK	0x00000040
 #define SOC_CPU_CLOCK_OFFSET			0x00000020
 #define SOC_CPU_CLOCK_STANDARD_LSB		0
 #define SOC_CPU_CLOCK_STANDARD_MASK		0x00000003
@@ -216,6 +219,8 @@ enum ath10k_mcast2ucast_mode {
 #define SOC_LPO_CAL_OFFSET			0x000000e0
 #define SOC_LPO_CAL_ENABLE_LSB			20
 #define SOC_LPO_CAL_ENABLE_MASK			0x00100000
+#define SOC_LF_TIMER_CONTROL0_ADDRESS		0x00000050
+#define SOC_LF_TIMER_CONTROL0_ENABLE_MASK	0x00000004
 
 #define SOC_CHIP_ID_ADDRESS			0x000000ec
 #define SOC_CHIP_ID_REV_LSB			8
@@ -273,6 +278,7 @@ enum ath10k_mcast2ucast_mode {
 #define PCIE_INTR_CAUSE_ADDRESS			0x000c
 #define PCIE_INTR_CLR_ADDRESS			0x0014
 #define SCRATCH_3_ADDRESS			0x0030
+#define CPU_INTR_ADDRESS			0x0010
 
 /* Firmware indications to the Host via SCRATCH_3 register. */
 #define FW_INDICATOR_ADDRESS	(SOC_CORE_BASE_ADDRESS + SCRATCH_3_ADDRESS)
diff --git a/drivers/net/wireless/ath/ath10k/mac.c b/drivers/net/wireless/ath/ath10k/mac.c
index 776e364eadcd..511a2f81e7af 100644
--- a/drivers/net/wireless/ath/ath10k/mac.c
+++ b/drivers/net/wireless/ath/ath10k/mac.c
@@ -323,13 +323,15 @@ static int ath10k_peer_create(struct ath10k *ar, u32 vdev_id, const u8 *addr)
 
 	ret = ath10k_wmi_peer_create(ar, vdev_id, addr);
 	if (ret) {
-		ath10k_warn("Failed to create wmi peer: %i\n", ret);
+		ath10k_warn("Failed to create wmi peer %pM on vdev %i: %i\n",
+			    addr, vdev_id, ret);
 		return ret;
 	}
 
 	ret = ath10k_wait_for_peer_created(ar, vdev_id, addr);
 	if (ret) {
-		ath10k_warn("Failed to wait for created wmi peer: %i\n", ret);
+		ath10k_warn("Failed to wait for created wmi peer %pM on vdev %i: %i\n",
+			    addr, vdev_id, ret);
 		return ret;
 	}
 	spin_lock_bh(&ar->data_lock);
@@ -339,6 +341,51 @@ static int ath10k_peer_create(struct ath10k *ar, u32 vdev_id, const u8 *addr)
 	return 0;
 }
 
+static int ath10k_mac_set_kickout(struct ath10k_vif *arvif)
+{
+	struct ath10k *ar = arvif->ar;
+	u32 param;
+	int ret;
+
+	param = ar->wmi.pdev_param->sta_kickout_th;
+	ret = ath10k_wmi_pdev_set_param(ar, param,
+					ATH10K_KICKOUT_THRESHOLD);
+	if (ret) {
+		ath10k_warn("Failed to set kickout threshold on vdev %i: %d\n",
+			    arvif->vdev_id, ret);
+		return ret;
+	}
+
+	param = ar->wmi.vdev_param->ap_keepalive_min_idle_inactive_time_secs;
+	ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, param,
+					ATH10K_KEEPALIVE_MIN_IDLE);
+	if (ret) {
+		ath10k_warn("Failed to set keepalive minimum idle time on vdev %i : %d\n",
+			    arvif->vdev_id, ret);
+		return ret;
+	}
+
+	param = ar->wmi.vdev_param->ap_keepalive_max_idle_inactive_time_secs;
+	ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, param,
+					ATH10K_KEEPALIVE_MAX_IDLE);
+	if (ret) {
+		ath10k_warn("Failed to set keepalive maximum idle time on vdev %i: %d\n",
+			    arvif->vdev_id, ret);
+		return ret;
+	}
+
+	param = ar->wmi.vdev_param->ap_keepalive_max_unresponsive_time_secs;
+	ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, param,
+					ATH10K_KEEPALIVE_MAX_UNRESPONSIVE);
+	if (ret) {
+		ath10k_warn("Failed to set keepalive maximum unresponsive time on vdev %i: %d\n",
+			    arvif->vdev_id, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
 static int  ath10k_mac_set_rts(struct ath10k_vif *arvif, u32 value)
 {
 	struct ath10k *ar = arvif->ar;
@@ -444,8 +491,7 @@ static inline int ath10k_vdev_setup_sync(struct ath10k *ar)
 static int ath10k_vdev_start(struct ath10k_vif *arvif)
 {
 	struct ath10k *ar = arvif->ar;
-	struct ieee80211_conf *conf = &ar->hw->conf;
-	struct ieee80211_channel *channel = conf->chandef.chan;
+	struct cfg80211_chan_def *chandef = &ar->chandef;
 	struct wmi_vdev_start_request_arg arg = {};
 	int ret = 0;
 
@@ -457,16 +503,14 @@ static int ath10k_vdev_start(struct ath10k_vif *arvif)
 	arg.dtim_period = arvif->dtim_period;
 	arg.bcn_intval = arvif->beacon_interval;
 
-	arg.channel.freq = channel->center_freq;
-
-	arg.channel.band_center_freq1 = conf->chandef.center_freq1;
-
-	arg.channel.mode = chan_to_phymode(&conf->chandef);
+	arg.channel.freq = chandef->chan->center_freq;
+	arg.channel.band_center_freq1 = chandef->center_freq1;
+	arg.channel.mode = chan_to_phymode(chandef);
 
 	arg.channel.min_power = 0;
-	arg.channel.max_power = channel->max_power * 2;
-	arg.channel.max_reg_power = channel->max_reg_power * 2;
-	arg.channel.max_antenna_gain = channel->max_antenna_gain * 2;
+	arg.channel.max_power = chandef->chan->max_power * 2;
+	arg.channel.max_reg_power = chandef->chan->max_reg_power * 2;
+	arg.channel.max_antenna_gain = chandef->chan->max_antenna_gain * 2;
 
 	if (arvif->vdev_type == WMI_VDEV_TYPE_AP) {
 		arg.ssid = arvif->u.ap.ssid;
@@ -475,7 +519,7 @@ static int ath10k_vdev_start(struct ath10k_vif *arvif)
 
 		/* For now allow DFS for AP mode */
 		arg.channel.chan_radar =
-			!!(channel->flags & IEEE80211_CHAN_RADAR);
+			!!(chandef->chan->flags & IEEE80211_CHAN_RADAR);
 	} else if (arvif->vdev_type == WMI_VDEV_TYPE_IBSS) {
 		arg.ssid = arvif->vif->bss_conf.ssid;
 		arg.ssid_len = arvif->vif->bss_conf.ssid_len;
@@ -488,13 +532,15 @@ static int ath10k_vdev_start(struct ath10k_vif *arvif)
 
 	ret = ath10k_wmi_vdev_start(ar, &arg);
 	if (ret) {
-		ath10k_warn("WMI vdev start failed: ret %d\n", ret);
+		ath10k_warn("WMI vdev %i start failed: ret %d\n",
+			    arg.vdev_id, ret);
 		return ret;
 	}
 
 	ret = ath10k_vdev_setup_sync(ar);
 	if (ret) {
-		ath10k_warn("vdev setup failed %d\n", ret);
+		ath10k_warn("vdev %i setup failed %d\n",
+			    arg.vdev_id, ret);
 		return ret;
 	}
 
@@ -512,13 +558,15 @@ static int ath10k_vdev_stop(struct ath10k_vif *arvif)
 
 	ret = ath10k_wmi_vdev_stop(ar, arvif->vdev_id);
 	if (ret) {
-		ath10k_warn("WMI vdev stop failed: ret %d\n", ret);
+		ath10k_warn("WMI vdev %i stop failed: ret %d\n",
+			    arvif->vdev_id, ret);
 		return ret;
 	}
 
 	ret = ath10k_vdev_setup_sync(ar);
 	if (ret) {
-		ath10k_warn("vdev setup failed %d\n", ret);
+		ath10k_warn("vdev %i setup sync failed %d\n",
+			    arvif->vdev_id, ret);
 		return ret;
 	}
 
@@ -527,7 +575,8 @@ static int ath10k_vdev_stop(struct ath10k_vif *arvif)
 
 static int ath10k_monitor_start(struct ath10k *ar, int vdev_id)
 {
-	struct ieee80211_channel *channel = ar->hw->conf.chandef.chan;
+	struct cfg80211_chan_def *chandef = &ar->chandef;
+	struct ieee80211_channel *channel = chandef->chan;
 	struct wmi_vdev_start_request_arg arg = {};
 	int ret = 0;
 
@@ -540,11 +589,11 @@ static int ath10k_monitor_start(struct ath10k *ar, int vdev_id)
 
 	arg.vdev_id = vdev_id;
 	arg.channel.freq = channel->center_freq;
-	arg.channel.band_center_freq1 = ar->hw->conf.chandef.center_freq1;
+	arg.channel.band_center_freq1 = chandef->center_freq1;
 
 	/* TODO setup this dynamically, what in case we
 	   don't have any vifs? */
-	arg.channel.mode = chan_to_phymode(&ar->hw->conf.chandef);
+	arg.channel.mode = chan_to_phymode(chandef);
 	arg.channel.chan_radar =
 			!!(channel->flags & IEEE80211_CHAN_RADAR);
 
@@ -555,19 +604,22 @@ static int ath10k_monitor_start(struct ath10k *ar, int vdev_id)
 
 	ret = ath10k_wmi_vdev_start(ar, &arg);
 	if (ret) {
-		ath10k_warn("Monitor vdev start failed: ret %d\n", ret);
+		ath10k_warn("Monitor vdev %i start failed: ret %d\n",
+			    vdev_id, ret);
 		return ret;
 	}
 
 	ret = ath10k_vdev_setup_sync(ar);
 	if (ret) {
-		ath10k_warn("Monitor vdev setup failed %d\n", ret);
+		ath10k_warn("Monitor vdev %i setup failed %d\n",
+			    vdev_id, ret);
 		return ret;
 	}
 
 	ret = ath10k_wmi_vdev_up(ar, vdev_id, 0, ar->mac_addr);
 	if (ret) {
-		ath10k_warn("Monitor vdev up failed: %d\n", ret);
+		ath10k_warn("Monitor vdev %i up failed: %d\n",
+			    vdev_id, ret);
 		goto vdev_stop;
 	}
 
@@ -579,7 +631,8 @@ static int ath10k_monitor_start(struct ath10k *ar, int vdev_id)
 vdev_stop:
 	ret = ath10k_wmi_vdev_stop(ar, ar->monitor_vdev_id);
 	if (ret)
-		ath10k_warn("Monitor vdev stop failed: %d\n", ret);
+		ath10k_warn("Monitor vdev %i stop failed: %d\n",
+			    ar->monitor_vdev_id, ret);
 
 	return ret;
 }
@@ -602,15 +655,18 @@ static int ath10k_monitor_stop(struct ath10k *ar)
 
 	ret = ath10k_wmi_vdev_down(ar, ar->monitor_vdev_id);
 	if (ret)
-		ath10k_warn("Monitor vdev down failed: %d\n", ret);
+		ath10k_warn("Monitor vdev %i down failed: %d\n",
+			    ar->monitor_vdev_id, ret);
 
 	ret = ath10k_wmi_vdev_stop(ar, ar->monitor_vdev_id);
 	if (ret)
-		ath10k_warn("Monitor vdev stop failed: %d\n", ret);
+		ath10k_warn("Monitor vdev %i stop failed: %d\n",
+			    ar->monitor_vdev_id, ret);
 
 	ret = ath10k_vdev_setup_sync(ar);
 	if (ret)
-		ath10k_warn("Monitor_down sync failed: %d\n", ret);
+		ath10k_warn("Monitor_down sync failed, vdev %i: %d\n",
+			    ar->monitor_vdev_id, ret);
 
 	ar->monitor_enabled = false;
 	return ret;
@@ -640,7 +696,8 @@ static int ath10k_monitor_create(struct ath10k *ar)
 				     WMI_VDEV_TYPE_MONITOR,
 				     0, ar->mac_addr);
 	if (ret) {
-		ath10k_warn("WMI vdev monitor create failed: ret %d\n", ret);
+		ath10k_warn("WMI vdev %i monitor create failed: ret %d\n",
+			    ar->monitor_vdev_id, ret);
 		goto vdev_fail;
 	}
 
@@ -669,7 +726,8 @@ static int ath10k_monitor_destroy(struct ath10k *ar)
 
 	ret = ath10k_wmi_vdev_delete(ar, ar->monitor_vdev_id);
 	if (ret) {
-		ath10k_warn("WMI vdev monitor delete failed: %d\n", ret);
+		ath10k_warn("WMI vdev %i monitor delete failed: %d\n",
+			    ar->monitor_vdev_id, ret);
 		return ret;
 	}
 
@@ -791,6 +849,22 @@ static void ath10k_control_beaconing(struct ath10k_vif *arvif,
 
 	if (!info->enable_beacon) {
 		ath10k_vdev_stop(arvif);
+
+		arvif->is_started = false;
+		arvif->is_up = false;
+
+		spin_lock_bh(&arvif->ar->data_lock);
+		if (arvif->beacon) {
+			dma_unmap_single(arvif->ar->dev,
+					 ATH10K_SKB_CB(arvif->beacon)->paddr,
+					 arvif->beacon->len, DMA_TO_DEVICE);
+			dev_kfree_skb_any(arvif->beacon);
+
+			arvif->beacon = NULL;
+			arvif->beacon_sent = false;
+		}
+		spin_unlock_bh(&arvif->ar->data_lock);
+
 		return;
 	}
 
@@ -800,12 +874,21 @@ static void ath10k_control_beaconing(struct ath10k_vif *arvif,
 	if (ret)
 		return;
 
-	ret = ath10k_wmi_vdev_up(arvif->ar, arvif->vdev_id, 0, info->bssid);
+	arvif->aid = 0;
+	memcpy(arvif->bssid, info->bssid, ETH_ALEN);
+
+	ret = ath10k_wmi_vdev_up(arvif->ar, arvif->vdev_id, arvif->aid,
+				 arvif->bssid);
 	if (ret) {
-		ath10k_warn("Failed to bring up VDEV: %d\n",
-			    arvif->vdev_id);
+		ath10k_warn("Failed to bring up vdev %d: %i\n",
+			    arvif->vdev_id, ret);
+		ath10k_vdev_stop(arvif);
 		return;
 	}
+
+	arvif->is_started = true;
+	arvif->is_up = true;
+
 	ath10k_dbg(ATH10K_DBG_MAC, "mac vdev %d up\n", arvif->vdev_id);
 }
 
@@ -824,18 +907,18 @@ static void ath10k_control_ibss(struct ath10k_vif *arvif,
 			ath10k_warn("Failed to delete IBSS self peer:%pM for VDEV:%d ret:%d\n",
 				    self_peer, arvif->vdev_id, ret);
 
-		if (is_zero_ether_addr(arvif->u.ibss.bssid))
+		if (is_zero_ether_addr(arvif->bssid))
 			return;
 
 		ret = ath10k_peer_delete(arvif->ar, arvif->vdev_id,
-					 arvif->u.ibss.bssid);
+					 arvif->bssid);
 		if (ret) {
 			ath10k_warn("Failed to delete IBSS BSSID peer:%pM for VDEV:%d ret:%d\n",
-				    arvif->u.ibss.bssid, arvif->vdev_id, ret);
+				    arvif->bssid, arvif->vdev_id, ret);
 			return;
 		}
 
-		memset(arvif->u.ibss.bssid, 0, ETH_ALEN);
+		memset(arvif->bssid, 0, ETH_ALEN);
 
 		return;
 	}
@@ -878,8 +961,8 @@ static int ath10k_mac_vif_setup_ps(struct ath10k_vif *arvif)
 		ret = ath10k_wmi_set_sta_ps_param(ar, arvif->vdev_id, param,
 						  conf->dynamic_ps_timeout);
 		if (ret) {
-			ath10k_warn("Failed to set inactivity time for VDEV: %d\n",
-				    arvif->vdev_id);
+			ath10k_warn("Failed to set inactivity time for vdev %d: %i\n",
+				    arvif->vdev_id, ret);
 			return ret;
 		}
 	} else {
@@ -1017,7 +1100,6 @@ static void ath10k_peer_assoc_h_ht(struct ath10k *ar,
 				   struct wmi_peer_assoc_complete_arg *arg)
 {
 	const struct ieee80211_sta_ht_cap *ht_cap = &sta->ht_cap;
-	int smps;
 	int i, n;
 
 	lockdep_assert_held(&ar->conf_mutex);
@@ -1063,17 +1145,6 @@ static void ath10k_peer_assoc_h_ht(struct ath10k *ar,
 		arg->peer_flags |= WMI_PEER_STBC;
 	}
 
-	smps = ht_cap->cap & IEEE80211_HT_CAP_SM_PS;
-	smps >>= IEEE80211_HT_CAP_SM_PS_SHIFT;
-
-	if (smps == WLAN_HT_CAP_SM_PS_STATIC) {
-		arg->peer_flags |= WMI_PEER_SPATIAL_MUX;
-		arg->peer_flags |= WMI_PEER_STATIC_MIMOPS;
-	} else if (smps == WLAN_HT_CAP_SM_PS_DYNAMIC) {
-		arg->peer_flags |= WMI_PEER_SPATIAL_MUX;
-		arg->peer_flags |= WMI_PEER_DYN_MIMOPS;
-	}
-
 	if (ht_cap->mcs.rx_mask[1] && ht_cap->mcs.rx_mask[2])
 		arg->peer_rate_caps |= WMI_RC_TS_FLAG;
 	else if (ht_cap->mcs.rx_mask[1])
@@ -1083,8 +1154,23 @@ static void ath10k_peer_assoc_h_ht(struct ath10k *ar,
 		if (ht_cap->mcs.rx_mask[i/8] & (1 << i%8))
 			arg->peer_ht_rates.rates[n++] = i;
 
-	arg->peer_ht_rates.num_rates = n;
-	arg->peer_num_spatial_streams = max((n+7) / 8, 1);
+	/*
+	 * This is a workaround for HT-enabled STAs which break the spec
+	 * and have no HT capabilities RX mask (no HT RX MCS map).
+	 *
+	 * As per spec, in section 20.3.5 Modulation and coding scheme (MCS),
+	 * MCS 0 through 7 are mandatory in 20MHz with 800 ns GI at all STAs.
+	 *
+	 * Firmware asserts if such situation occurs.
+	 */
+	if (n == 0) {
+		arg->peer_ht_rates.num_rates = 8;
+		for (i = 0; i < arg->peer_ht_rates.num_rates; i++)
+			arg->peer_ht_rates.rates[i] = i;
+	} else {
+		arg->peer_ht_rates.num_rates = n;
+		arg->peer_num_spatial_streams = sta->rx_nss;
+	}
 
 	ath10k_dbg(ATH10K_DBG_MAC, "mac ht peer %pM mcs cnt %d nss %d\n",
 		   arg->addr,
@@ -1092,27 +1178,20 @@ static void ath10k_peer_assoc_h_ht(struct ath10k *ar,
 		   arg->peer_num_spatial_streams);
 }
 
-static void ath10k_peer_assoc_h_qos_ap(struct ath10k *ar,
-				       struct ath10k_vif *arvif,
-				       struct ieee80211_sta *sta,
-				       struct ieee80211_bss_conf *bss_conf,
-				       struct wmi_peer_assoc_complete_arg *arg)
+static int ath10k_peer_assoc_qos_ap(struct ath10k *ar,
+				    struct ath10k_vif *arvif,
+				    struct ieee80211_sta *sta)
 {
 	u32 uapsd = 0;
 	u32 max_sp = 0;
+	int ret = 0;
 
 	lockdep_assert_held(&ar->conf_mutex);
 
-	if (sta->wme)
-		arg->peer_flags |= WMI_PEER_QOS;
-
 	if (sta->wme && sta->uapsd_queues) {
 		ath10k_dbg(ATH10K_DBG_MAC, "mac uapsd_queues 0x%x max_sp %d\n",
 			   sta->uapsd_queues, sta->max_sp);
 
-		arg->peer_flags |= WMI_PEER_APSD;
-		arg->peer_rate_caps |= WMI_RC_UAPSD_FLAG;
-
 		if (sta->uapsd_queues & IEEE80211_WMM_IE_STA_QOSINFO_AC_VO)
 			uapsd |= WMI_AP_PS_UAPSD_AC3_DELIVERY_EN |
 				 WMI_AP_PS_UAPSD_AC3_TRIGGER_EN;
@@ -1130,35 +1209,40 @@ static void ath10k_peer_assoc_h_qos_ap(struct ath10k *ar,
 		if (sta->max_sp < MAX_WMI_AP_PS_PEER_PARAM_MAX_SP)
 			max_sp = sta->max_sp;
 
-		ath10k_wmi_set_ap_ps_param(ar, arvif->vdev_id,
-					   sta->addr,
-					   WMI_AP_PS_PEER_PARAM_UAPSD,
-					   uapsd);
+		ret = ath10k_wmi_set_ap_ps_param(ar, arvif->vdev_id,
+						 sta->addr,
+						 WMI_AP_PS_PEER_PARAM_UAPSD,
+						 uapsd);
+		if (ret) {
+			ath10k_warn("failed to set ap ps peer param uapsd for vdev %i: %d\n",
+				    arvif->vdev_id, ret);
+			return ret;
+		}
 
-		ath10k_wmi_set_ap_ps_param(ar, arvif->vdev_id,
-					   sta->addr,
-					   WMI_AP_PS_PEER_PARAM_MAX_SP,
-					   max_sp);
+		ret = ath10k_wmi_set_ap_ps_param(ar, arvif->vdev_id,
+						 sta->addr,
+						 WMI_AP_PS_PEER_PARAM_MAX_SP,
+						 max_sp);
+		if (ret) {
+			ath10k_warn("failed to set ap ps peer param max sp for vdev %i: %d\n",
+				    arvif->vdev_id, ret);
+			return ret;
+		}
 
 		/* TODO setup this based on STA listen interval and
 		   beacon interval. Currently we don't know
 		   sta->listen_interval - mac80211 patch required.
 		   Currently use 10 seconds */
-		ath10k_wmi_set_ap_ps_param(ar, arvif->vdev_id,
-					   sta->addr,
-					   WMI_AP_PS_PEER_PARAM_AGEOUT_TIME,
-					   10);
+		ret = ath10k_wmi_set_ap_ps_param(ar, arvif->vdev_id, sta->addr,
+					WMI_AP_PS_PEER_PARAM_AGEOUT_TIME, 10);
+		if (ret) {
+			ath10k_warn("failed to set ap ps peer param ageout time for vdev %i: %d\n",
+				    arvif->vdev_id, ret);
+			return ret;
+		}
 	}
-}
 
-static void ath10k_peer_assoc_h_qos_sta(struct ath10k *ar,
-					struct ath10k_vif *arvif,
-					struct ieee80211_sta *sta,
-					struct ieee80211_bss_conf *bss_conf,
-					struct wmi_peer_assoc_complete_arg *arg)
-{
-	if (bss_conf->qos)
-		arg->peer_flags |= WMI_PEER_QOS;
+	return 0;
 }
 
 static void ath10k_peer_assoc_h_vht(struct ath10k *ar,
@@ -1211,10 +1295,17 @@ static void ath10k_peer_assoc_h_qos(struct ath10k *ar,
 {
 	switch (arvif->vdev_type) {
 	case WMI_VDEV_TYPE_AP:
-		ath10k_peer_assoc_h_qos_ap(ar, arvif, sta, bss_conf, arg);
+		if (sta->wme)
+			arg->peer_flags |= WMI_PEER_QOS;
+
+		if (sta->wme && sta->uapsd_queues) {
+			arg->peer_flags |= WMI_PEER_APSD;
+			arg->peer_rate_caps |= WMI_RC_UAPSD_FLAG;
+		}
 		break;
 	case WMI_VDEV_TYPE_STA:
-		ath10k_peer_assoc_h_qos_sta(ar, arvif, sta, bss_conf, arg);
+		if (bss_conf->qos)
+			arg->peer_flags |= WMI_PEER_QOS;
 		break;
 	default:
 		break;
@@ -1293,6 +1384,33 @@ static int ath10k_peer_assoc_prepare(struct ath10k *ar,
 	return 0;
 }
 
+static const u32 ath10k_smps_map[] = {
+	[WLAN_HT_CAP_SM_PS_STATIC] = WMI_PEER_SMPS_STATIC,
+	[WLAN_HT_CAP_SM_PS_DYNAMIC] = WMI_PEER_SMPS_DYNAMIC,
+	[WLAN_HT_CAP_SM_PS_INVALID] = WMI_PEER_SMPS_PS_NONE,
+	[WLAN_HT_CAP_SM_PS_DISABLED] = WMI_PEER_SMPS_PS_NONE,
+};
+
+static int ath10k_setup_peer_smps(struct ath10k *ar, struct ath10k_vif *arvif,
+				  const u8 *addr,
+				  const struct ieee80211_sta_ht_cap *ht_cap)
+{
+	int smps;
+
+	if (!ht_cap->ht_supported)
+		return 0;
+
+	smps = ht_cap->cap & IEEE80211_HT_CAP_SM_PS;
+	smps >>= IEEE80211_HT_CAP_SM_PS_SHIFT;
+
+	if (smps >= ARRAY_SIZE(ath10k_smps_map))
+		return -EINVAL;
+
+	return ath10k_wmi_peer_set_param(ar, arvif->vdev_id, addr,
+					 WMI_PEER_SMPS_STATE,
+					 ath10k_smps_map[smps]);
+}
+
 /* can be called only in mac80211 callbacks due to `key_count` usage */
 static void ath10k_bss_assoc(struct ieee80211_hw *hw,
 			     struct ieee80211_vif *vif,
@@ -1300,6 +1418,7 @@ static void ath10k_bss_assoc(struct ieee80211_hw *hw,
 {
 	struct ath10k *ar = hw->priv;
 	struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
+	struct ieee80211_sta_ht_cap ht_cap;
 	struct wmi_peer_assoc_complete_arg peer_arg;
 	struct ieee80211_sta *ap_sta;
 	int ret;
@@ -1310,17 +1429,21 @@ static void ath10k_bss_assoc(struct ieee80211_hw *hw,
 
 	ap_sta = ieee80211_find_sta(vif, bss_conf->bssid);
 	if (!ap_sta) {
-		ath10k_warn("Failed to find station entry for %pM\n",
-			    bss_conf->bssid);
+		ath10k_warn("Failed to find station entry for %pM, vdev %i\n",
+			    bss_conf->bssid, arvif->vdev_id);
 		rcu_read_unlock();
 		return;
 	}
 
+	/* ap_sta must be accessed only within rcu section which must be left
+	 * before calling ath10k_setup_peer_smps() which might sleep. */
+	ht_cap = ap_sta->ht_cap;
+
 	ret = ath10k_peer_assoc_prepare(ar, arvif, ap_sta,
 					bss_conf, &peer_arg);
 	if (ret) {
-		ath10k_warn("Peer assoc prepare failed for %pM\n: %d",
-			    bss_conf->bssid, ret);
+		ath10k_warn("Peer assoc prepare failed for %pM vdev %i\n: %d",
+			    bss_conf->bssid, arvif->vdev_id, ret);
 		rcu_read_unlock();
 		return;
 	}
@@ -1329,8 +1452,15 @@ static void ath10k_bss_assoc(struct ieee80211_hw *hw,
 
 	ret = ath10k_wmi_peer_assoc(ar, &peer_arg);
 	if (ret) {
-		ath10k_warn("Peer assoc failed for %pM\n: %d",
-			    bss_conf->bssid, ret);
+		ath10k_warn("Peer assoc failed for %pM vdev %i\n: %d",
+			    bss_conf->bssid, arvif->vdev_id, ret);
+		return;
+	}
+
+	ret = ath10k_setup_peer_smps(ar, arvif, bss_conf->bssid, &ht_cap);
+	if (ret) {
+		ath10k_warn("failed to setup peer SMPS for vdev %i: %d\n",
+			    arvif->vdev_id, ret);
 		return;
 	}
 
@@ -1338,11 +1468,17 @@ static void ath10k_bss_assoc(struct ieee80211_hw *hw,
 		   "mac vdev %d up (associated) bssid %pM aid %d\n",
 		   arvif->vdev_id, bss_conf->bssid, bss_conf->aid);
 
-	ret = ath10k_wmi_vdev_up(ar, arvif->vdev_id, bss_conf->aid,
-				 bss_conf->bssid);
-	if (ret)
+	arvif->aid = bss_conf->aid;
+	memcpy(arvif->bssid, bss_conf->bssid, ETH_ALEN);
+
+	ret = ath10k_wmi_vdev_up(ar, arvif->vdev_id, arvif->aid, arvif->bssid);
+	if (ret) {
 		ath10k_warn("VDEV: %d up failed: ret %d\n",
 			    arvif->vdev_id, ret);
+		return;
+	}
+
+	arvif->is_up = true;
 }
 
 /*
@@ -1382,6 +1518,9 @@ static void ath10k_bss_disassoc(struct ieee80211_hw *hw,
 	ret = ath10k_wmi_vdev_down(ar, arvif->vdev_id);
 
 	arvif->def_wep_key_idx = 0;
+
+	arvif->is_started = false;
+	arvif->is_up = false;
 }
 
 static int ath10k_station_assoc(struct ath10k *ar, struct ath10k_vif *arvif,
@@ -1394,21 +1533,35 @@ static int ath10k_station_assoc(struct ath10k *ar, struct ath10k_vif *arvif,
 
 	ret = ath10k_peer_assoc_prepare(ar, arvif, sta, NULL, &peer_arg);
 	if (ret) {
-		ath10k_warn("WMI peer assoc prepare failed for %pM\n",
-			    sta->addr);
+		ath10k_warn("WMI peer assoc prepare failed for %pM vdev %i: %i\n",
+			    sta->addr, arvif->vdev_id, ret);
 		return ret;
 	}
 
 	ret = ath10k_wmi_peer_assoc(ar, &peer_arg);
 	if (ret) {
-		ath10k_warn("Peer assoc failed for STA %pM\n: %d",
-			    sta->addr, ret);
+		ath10k_warn("Peer assoc failed for STA %pM vdev %i: %d\n",
+			    sta->addr, arvif->vdev_id, ret);
+		return ret;
+	}
+
+	ret = ath10k_setup_peer_smps(ar, arvif, sta->addr, &sta->ht_cap);
+	if (ret) {
+		ath10k_warn("failed to setup peer SMPS for vdev: %d\n", ret);
 		return ret;
 	}
 
 	ret = ath10k_install_peer_wep_keys(arvif, sta->addr);
 	if (ret) {
-		ath10k_warn("could not install peer wep keys (%d)\n", ret);
+		ath10k_warn("could not install peer wep keys for vdev %i: %d\n",
+			    arvif->vdev_id, ret);
+		return ret;
+	}
+
+	ret = ath10k_peer_assoc_qos_ap(ar, arvif, sta);
+	if (ret) {
+		ath10k_warn("could not set qos params for STA %pM for vdev %i: %d\n",
+			    sta->addr, arvif->vdev_id, ret);
 		return ret;
 	}
 
@@ -1424,7 +1577,8 @@ static int ath10k_station_disassoc(struct ath10k *ar, struct ath10k_vif *arvif,
 
 	ret = ath10k_clear_peer_keys(arvif, sta->addr);
 	if (ret) {
-		ath10k_warn("could not clear all peer wep keys (%d)\n", ret);
+		ath10k_warn("could not clear all peer wep keys for vdev %i: %d\n",
+			    arvif->vdev_id, ret);
 		return ret;
 	}
 
@@ -1547,9 +1701,9 @@ static void ath10k_regd_update(struct ath10k *ar)
 	/* Target allows setting up per-band regdomain but ath_common provides
 	 * a combined one only */
 	ret = ath10k_wmi_pdev_set_regdomain(ar,
-					    regpair->regDmnEnum,
-					    regpair->regDmnEnum, /* 2ghz */
-					    regpair->regDmnEnum, /* 5ghz */
+					    regpair->reg_domain,
+					    regpair->reg_domain, /* 2ghz */
+					    regpair->reg_domain, /* 5ghz */
 					    regpair->reg_2ghz_ctl,
 					    regpair->reg_5ghz_ctl);
 	if (ret)
@@ -2100,11 +2254,29 @@ static int ath10k_start(struct ieee80211_hw *hw)
 		ath10k_warn("could not init WMI_PDEV_PARAM_DYNAMIC_BW (%d)\n",
 			    ret);
 
+	/*
+	 * By default FW set ARP frames ac to voice (6). In that case ARP
+	 * exchange is not working properly for UAPSD enabled AP. ARP requests
+	 * which arrives with access category 0 are processed by network stack
+	 * and send back with access category 0, but FW changes access category
+	 * to 6. Set ARP frames access category to best effort (0) solves
+	 * this problem.
+	 */
+
+	ret = ath10k_wmi_pdev_set_param(ar,
+					ar->wmi.pdev_param->arp_ac_override, 0);
+	if (ret) {
+		ath10k_warn("could not set arp ac override parameter: %d\n",
+			    ret);
+		goto exit;
+	}
+
 	ath10k_regd_update(ar);
+	ret = 0;
 
 exit:
 	mutex_unlock(&ar->conf_mutex);
-	return 0;
+	return ret;
 }
 
 static void ath10k_stop(struct ieee80211_hw *hw)
@@ -2145,6 +2317,98 @@ static int ath10k_config_ps(struct ath10k *ar)
 	return ret;
 }
 
+static const char *chandef_get_width(enum nl80211_chan_width width)
+{
+	switch (width) {
+	case NL80211_CHAN_WIDTH_20_NOHT:
+		return "20 (noht)";
+	case NL80211_CHAN_WIDTH_20:
+		return "20";
+	case NL80211_CHAN_WIDTH_40:
+		return "40";
+	case NL80211_CHAN_WIDTH_80:
+		return "80";
+	case NL80211_CHAN_WIDTH_80P80:
+		return "80+80";
+	case NL80211_CHAN_WIDTH_160:
+		return "160";
+	case NL80211_CHAN_WIDTH_5:
+		return "5";
+	case NL80211_CHAN_WIDTH_10:
+		return "10";
+	}
+	return "?";
+}
+
+static void ath10k_config_chan(struct ath10k *ar)
+{
+	struct ath10k_vif *arvif;
+	bool monitor_was_enabled;
+	int ret;
+
+	lockdep_assert_held(&ar->conf_mutex);
+
+	ath10k_dbg(ATH10K_DBG_MAC,
+		   "mac config channel to %dMHz (cf1 %dMHz cf2 %dMHz width %s)\n",
+		   ar->chandef.chan->center_freq,
+		   ar->chandef.center_freq1,
+		   ar->chandef.center_freq2,
+		   chandef_get_width(ar->chandef.width));
+
+	/* First stop monitor interface. Some FW versions crash if there's a
+	 * lone monitor interface. */
+	monitor_was_enabled = ar->monitor_enabled;
+
+	if (ar->monitor_enabled)
+		ath10k_monitor_stop(ar);
+
+	list_for_each_entry(arvif, &ar->arvifs, list) {
+		if (!arvif->is_started)
+			continue;
+
+		if (arvif->vdev_type == WMI_VDEV_TYPE_MONITOR)
+			continue;
+
+		ret = ath10k_vdev_stop(arvif);
+		if (ret) {
+			ath10k_warn("could not stop vdev %d (%d)\n",
+				    arvif->vdev_id, ret);
+			continue;
+		}
+	}
+
+	/* all vdevs are now stopped - now attempt to restart them */
+
+	list_for_each_entry(arvif, &ar->arvifs, list) {
+		if (!arvif->is_started)
+			continue;
+
+		if (arvif->vdev_type == WMI_VDEV_TYPE_MONITOR)
+			continue;
+
+		ret = ath10k_vdev_start(arvif);
+		if (ret) {
+			ath10k_warn("could not start vdev %d (%d)\n",
+				    arvif->vdev_id, ret);
+			continue;
+		}
+
+		if (!arvif->is_up)
+			continue;
+
+		ret = ath10k_wmi_vdev_up(arvif->ar, arvif->vdev_id, arvif->aid,
+					 arvif->bssid);
+		if (ret) {
+			ath10k_warn("could not bring vdev up %d (%d)\n",
+				    arvif->vdev_id, ret);
+			continue;
+		}
+	}
+
+	if (monitor_was_enabled)
+		ath10k_monitor_start(ar, ar->monitor_vdev_id);
+}
+
 static int ath10k_config(struct ieee80211_hw *hw, u32 changed)
 {
 	struct ath10k *ar = hw->priv;
@@ -2165,6 +2429,11 @@ static int ath10k_config(struct ieee80211_hw *hw, u32 changed)
 		spin_unlock_bh(&ar->data_lock);
 
 		ath10k_config_radar_detection(ar);
+
+		if (!cfg80211_chandef_identical(&ar->chandef, &conf->chandef)) {
+			ar->chandef = conf->chandef;
+			ath10k_config_chan(ar);
+		}
 	}
 
 	if (changed & IEEE80211_CONF_CHANGE_POWER) {
@@ -2214,7 +2483,7 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
 	struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
 	enum wmi_sta_powersave_param param;
 	int ret = 0;
-	u32 value, param_id;
+	u32 value;
 	int bit;
 	u32 vdev_param;
 
@@ -2276,7 +2545,8 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
 	ret = ath10k_wmi_vdev_create(ar, arvif->vdev_id, arvif->vdev_type,
 				     arvif->vdev_subtype, vif->addr);
 	if (ret) {
-		ath10k_warn("WMI vdev create failed: ret %d\n", ret);
+		ath10k_warn("WMI vdev %i create failed: ret %d\n",
+			    arvif->vdev_id, ret);
 		goto err;
 	}
 
@@ -2287,7 +2557,8 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
 	ret = ath10k_wmi_vdev_set_param(ar, 0, vdev_param,
 					arvif->def_wep_key_idx);
 	if (ret) {
-		ath10k_warn("Failed to set default keyid: %d\n", ret);
+		ath10k_warn("Failed to set vdev %i default keyid: %d\n",
+			    arvif->vdev_id, ret);
 		goto err_vdev_delete;
 	}
 
@@ -2296,23 +2567,25 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
 					ATH10K_HW_TXRX_NATIVE_WIFI);
 	/* 10.X firmware does not support this VDEV parameter. Do not warn */
 	if (ret && ret != -EOPNOTSUPP) {
-		ath10k_warn("Failed to set TX encap: %d\n", ret);
+		ath10k_warn("Failed to set vdev %i TX encap: %d\n",
+			    arvif->vdev_id, ret);
 		goto err_vdev_delete;
 	}
 
 	if (arvif->vdev_type == WMI_VDEV_TYPE_AP) {
 		ret = ath10k_peer_create(ar, arvif->vdev_id, vif->addr);
 		if (ret) {
-			ath10k_warn("Failed to create peer for AP: %d\n", ret);
+			ath10k_warn("Failed to create vdev %i peer for AP: %d\n",
+				    arvif->vdev_id, ret);
 			goto err_vdev_delete;
 		}
 
-		param_id = ar->wmi.pdev_param->sta_kickout_th;
-
-		/* Disable STA KICKOUT functionality in FW */
-		ret = ath10k_wmi_pdev_set_param(ar, param_id, 0);
-		if (ret)
-			ath10k_warn("Failed to disable STA KICKOUT\n");
+		ret = ath10k_mac_set_kickout(arvif);
+		if (ret) {
+			ath10k_warn("Failed to set vdev %i kickout parameters: %d\n",
+				    arvif->vdev_id, ret);
+			goto err_peer_delete;
+		}
 	}
 
 	if (arvif->vdev_type == WMI_VDEV_TYPE_STA) {
@@ -2321,7 +2594,8 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
 		ret = ath10k_wmi_set_sta_ps_param(ar, arvif->vdev_id,
 						  param, value);
 		if (ret) {
-			ath10k_warn("Failed to set RX wake policy: %d\n", ret);
+			ath10k_warn("Failed to set vdev %i RX wake policy: %d\n",
+				    arvif->vdev_id, ret);
 			goto err_peer_delete;
 		}
 
@@ -2330,7 +2604,8 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
 		ret = ath10k_wmi_set_sta_ps_param(ar, arvif->vdev_id,
 						  param, value);
 		if (ret) {
-			ath10k_warn("Failed to set TX wake thresh: %d\n", ret);
+			ath10k_warn("Failed to set vdev %i TX wake thresh: %d\n",
+				    arvif->vdev_id, ret);
 			goto err_peer_delete;
 		}
 
@@ -2339,7 +2614,8 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
 		ret = ath10k_wmi_set_sta_ps_param(ar, arvif->vdev_id,
 						  param, value);
 		if (ret) {
-			ath10k_warn("Failed to set PSPOLL count: %d\n", ret);
+			ath10k_warn("Failed to set vdev %i PSPOLL count: %d\n",
+				    arvif->vdev_id, ret);
 			goto err_peer_delete;
 		}
 	}
@@ -2403,17 +2679,19 @@ static void ath10k_remove_interface(struct ieee80211_hw *hw,
 	if (arvif->vdev_type == WMI_VDEV_TYPE_AP) {
 		ret = ath10k_peer_delete(arvif->ar, arvif->vdev_id, vif->addr);
 		if (ret)
-			ath10k_warn("Failed to remove peer for AP: %d\n", ret);
+			ath10k_warn("Failed to remove peer for AP vdev %i: %d\n",
+				    arvif->vdev_id, ret);
 
 		kfree(arvif->u.ap.noa_data);
 	}
 
-	ath10k_dbg(ATH10K_DBG_MAC, "mac vdev delete %d (remove interface)\n",
+	ath10k_dbg(ATH10K_DBG_MAC, "mac vdev %i delete (remove interface)\n",
 		   arvif->vdev_id);
 
 	ret = ath10k_wmi_vdev_delete(ar, arvif->vdev_id);
 	if (ret)
-		ath10k_warn("WMI vdev delete failed: %d\n", ret);
+		ath10k_warn("WMI vdev %i delete failed: %d\n",
+			    arvif->vdev_id, ret);
 
 	if (arvif->vdev_type == WMI_VDEV_TYPE_MONITOR)
 		ar->monitor_present = false;
@@ -2502,8 +2780,8 @@ static void ath10k_bss_info_changed(struct ieee80211_hw *hw,
 			   arvif->vdev_id, arvif->beacon_interval);
 
 		if (ret)
-			ath10k_warn("Failed to set beacon interval for VDEV: %d\n",
-				    arvif->vdev_id);
+			ath10k_warn("Failed to set beacon interval for vdev %d: %i\n",
+				    arvif->vdev_id, ret);
 	}
 
 	if (changed & BSS_CHANGED_BEACON) {
@@ -2515,8 +2793,8 @@ static void ath10k_bss_info_changed(struct ieee80211_hw *hw,
 		ret = ath10k_wmi_pdev_set_param(ar, pdev_param,
 						WMI_BEACON_STAGGERED_MODE);
 		if (ret)
-			ath10k_warn("Failed to set beacon mode for VDEV: %d\n",
-				    arvif->vdev_id);
+			ath10k_warn("Failed to set beacon mode for vdev %d: %i\n",
+				    arvif->vdev_id, ret);
 	}
 
 	if (changed & BSS_CHANGED_BEACON_INFO) {
@@ -2530,8 +2808,8 @@ static void ath10k_bss_info_changed(struct ieee80211_hw *hw,
 		ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, vdev_param,
 						arvif->dtim_period);
 		if (ret)
-			ath10k_warn("Failed to set dtim period for VDEV: %d\n",
-				    arvif->vdev_id);
+			ath10k_warn("Failed to set dtim period for vdev %d: %i\n",
+				    arvif->vdev_id, ret);
 	}
 
 	if (changed & BSS_CHANGED_SSID &&
@@ -2551,7 +2829,7 @@ static void ath10k_bss_info_changed(struct ieee80211_hw *hw,
 			ret = ath10k_peer_create(ar, arvif->vdev_id,
 						 info->bssid);
 			if (ret)
-				ath10k_warn("Failed to add peer %pM for vdev %d when changin bssid: %i\n",
+				ath10k_warn("Failed to add peer %pM for vdev %d when changing bssid: %i\n",
 					    info->bssid, arvif->vdev_id, ret);
 
 			if (vif->type == NL80211_IFTYPE_STATION) {
@@ -2559,15 +2837,20 @@ static void ath10k_bss_info_changed(struct ieee80211_hw *hw,
 				 * this is never erased as we it for crypto key
 				 * clearing; this is FW requirement
 				 */
-				memcpy(arvif->u.sta.bssid, info->bssid,
-				       ETH_ALEN);
+				memcpy(arvif->bssid, info->bssid, ETH_ALEN);
 
 				ath10k_dbg(ATH10K_DBG_MAC,
 					   "mac vdev %d start %pM\n",
 					   arvif->vdev_id, info->bssid);
 
-				/* FIXME: check return value */
 				ret = ath10k_vdev_start(arvif);
+				if (ret) {
+					ath10k_warn("failed to start vdev %i: %d\n",
+						    arvif->vdev_id, ret);
+					goto exit;
+				}
+
+				arvif->is_started = true;
 			}
 
 			/*
@@ -2576,7 +2859,7 @@ static void ath10k_bss_info_changed(struct ieee80211_hw *hw,
 			 * IBSS in order to remove BSSID peer.
 			 */
 			if (vif->type == NL80211_IFTYPE_ADHOC)
-				memcpy(arvif->u.ibss.bssid, info->bssid,
+				memcpy(arvif->bssid, info->bssid,
 				       ETH_ALEN);
 		}
 	}
@@ -2598,8 +2881,8 @@ static void ath10k_bss_info_changed(struct ieee80211_hw *hw,
 		ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, vdev_param,
 						cts_prot);
 		if (ret)
-			ath10k_warn("Failed to set CTS prot for VDEV: %d\n",
-				    arvif->vdev_id);
+			ath10k_warn("Failed to set CTS prot for vdev %d: %d\n",
+				    arvif->vdev_id, ret);
 	}
 
 	if (changed & BSS_CHANGED_ERP_SLOT) {
@@ -2617,8 +2900,8 @@ static void ath10k_bss_info_changed(struct ieee80211_hw *hw,
 		ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, vdev_param,
 						slottime);
 		if (ret)
-			ath10k_warn("Failed to set erp slot for VDEV: %d\n",
-				    arvif->vdev_id);
+			ath10k_warn("Failed to set erp slot for vdev %d: %i\n",
+				    arvif->vdev_id, ret);
 	}
 
 	if (changed & BSS_CHANGED_ERP_PREAMBLE) {
@@ -2636,8 +2919,8 @@ static void ath10k_bss_info_changed(struct ieee80211_hw *hw,
 		ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, vdev_param,
 						preamble);
 		if (ret)
-			ath10k_warn("Failed to set preamble for VDEV: %d\n",
-				    arvif->vdev_id);
+			ath10k_warn("Failed to set preamble for vdev %d: %i\n",
+				    arvif->vdev_id, ret);
 	}
 
 	if (changed & BSS_CHANGED_ASSOC) {
@@ -2645,6 +2928,7 @@ static void ath10k_bss_info_changed(struct ieee80211_hw *hw,
 			ath10k_bss_assoc(hw, vif, info);
 	}
 
+exit:
 	mutex_unlock(&ar->conf_mutex);
 }
 
@@ -2767,8 +3051,8 @@ static void ath10k_set_key_h_def_keyidx(struct ath10k *ar,
 	ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, vdev_param,
 					key->keyidx);
 	if (ret)
-		ath10k_warn("failed to set group key as default key: %d\n",
-			    ret);
+		ath10k_warn("failed to set vdev %i group key as default key: %d\n",
+			    arvif->vdev_id, ret);
 }
 
 static int ath10k_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
@@ -2828,7 +3112,8 @@ static int ath10k_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 
 	ret = ath10k_install_key(arvif, key, cmd, peer_addr);
 	if (ret) {
-		ath10k_warn("ath10k_install_key failed (%d)\n", ret);
+		ath10k_warn("key installation failed for vdev %i peer %pM: %d\n",
+			    arvif->vdev_id, peer_addr, ret);
 		goto exit;
 	}
 
@@ -2850,6 +3135,69 @@ exit:
 	return ret;
 }
 
+static void ath10k_sta_rc_update_wk(struct work_struct *wk)
+{
+	struct ath10k *ar;
+	struct ath10k_vif *arvif;
+	struct ath10k_sta *arsta;
+	struct ieee80211_sta *sta;
+	u32 changed, bw, nss, smps;
+	int err;
+
+	arsta = container_of(wk, struct ath10k_sta, update_wk);
+	sta = container_of((void *)arsta, struct ieee80211_sta, drv_priv);
+	arvif = arsta->arvif;
+	ar = arvif->ar;
+
+	spin_lock_bh(&ar->data_lock);
+
+	changed = arsta->changed;
+	arsta->changed = 0;
+
+	bw = arsta->bw;
+	nss = arsta->nss;
+	smps = arsta->smps;
+
+	spin_unlock_bh(&ar->data_lock);
+
+	mutex_lock(&ar->conf_mutex);
+
+	if (changed & IEEE80211_RC_BW_CHANGED) {
+		ath10k_dbg(ATH10K_DBG_MAC, "mac update sta %pM peer bw %d\n",
+			   sta->addr, bw);
+
+		err = ath10k_wmi_peer_set_param(ar, arvif->vdev_id, sta->addr,
+						WMI_PEER_CHAN_WIDTH, bw);
+		if (err)
+			ath10k_warn("failed to update STA %pM peer bw %d: %d\n",
+				    sta->addr, bw, err);
+	}
+
+	if (changed & IEEE80211_RC_NSS_CHANGED) {
+		ath10k_dbg(ATH10K_DBG_MAC, "mac update sta %pM nss %d\n",
+			   sta->addr, nss);
+
+		err = ath10k_wmi_peer_set_param(ar, arvif->vdev_id, sta->addr,
+						WMI_PEER_NSS, nss);
+		if (err)
+			ath10k_warn("failed to update STA %pM nss %d: %d\n",
+				    sta->addr, nss, err);
+	}
+
+	if (changed & IEEE80211_RC_SMPS_CHANGED) {
+		ath10k_dbg(ATH10K_DBG_MAC, "mac update sta %pM smps %d\n",
+			   sta->addr, smps);
+
+		err = ath10k_wmi_peer_set_param(ar, arvif->vdev_id, sta->addr,
+						WMI_PEER_SMPS_STATE, smps);
+		if (err)
+			ath10k_warn("failed to update STA %pM smps %d: %d\n",
+				    sta->addr, smps, err);
+	}
+
+	mutex_unlock(&ar->conf_mutex);
+}
+
 static int ath10k_sta_state(struct ieee80211_hw *hw,
 			    struct ieee80211_vif *vif,
 			    struct ieee80211_sta *sta,
@@ -2858,9 +3206,22 @@ static int ath10k_sta_state(struct ieee80211_hw *hw,
 {
 	struct ath10k *ar = hw->priv;
 	struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
+	struct ath10k_sta *arsta = (struct ath10k_sta *)sta->drv_priv;
 	int max_num_peers;
 	int ret = 0;
 
+	if (old_state == IEEE80211_STA_NOTEXIST &&
+	    new_state == IEEE80211_STA_NONE) {
+		memset(arsta, 0, sizeof(*arsta));
+		arsta->arvif = arvif;
+		INIT_WORK(&arsta->update_wk, ath10k_sta_rc_update_wk);
+	}
+
+	/* cancel must be done outside the mutex to avoid deadlock */
+	if ((old_state == IEEE80211_STA_NONE &&
+	     new_state == IEEE80211_STA_NOTEXIST))
+		cancel_work_sync(&arsta->update_wk);
+
 	mutex_lock(&ar->conf_mutex);
 
 	if (old_state == IEEE80211_STA_NOTEXIST &&
@@ -2899,8 +3260,8 @@ static int ath10k_sta_state(struct ieee80211_hw *hw,
 			   arvif->vdev_id, sta->addr);
 		ret = ath10k_peer_delete(ar, arvif->vdev_id, sta->addr);
 		if (ret)
-			ath10k_warn("Failed to delete peer: %pM for VDEV: %d\n",
-				    sta->addr, arvif->vdev_id);
+			ath10k_warn("Failed to delete peer %pM for vdev %d: %i\n",
+				    sta->addr, arvif->vdev_id, ret);
 
 		if (vif->type == NL80211_IFTYPE_STATION)
 			ath10k_bss_disassoc(hw, vif);
@@ -2916,8 +3277,8 @@ static int ath10k_sta_state(struct ieee80211_hw *hw,
 
 		ret = ath10k_station_assoc(ar, arvif, sta);
 		if (ret)
-			ath10k_warn("Failed to associate station: %pM\n",
-				    sta->addr);
+			ath10k_warn("Failed to associate station %pM for vdev %i: %i\n",
+				    sta->addr, arvif->vdev_id, ret);
 	} else if (old_state == IEEE80211_STA_ASSOC &&
 		   new_state == IEEE80211_STA_AUTH &&
 		   (vif->type == NL80211_IFTYPE_AP ||
@@ -2930,8 +3291,8 @@ static int ath10k_sta_state(struct ieee80211_hw *hw,
 
 		ret = ath10k_station_disassoc(ar, arvif, sta);
 		if (ret)
-			ath10k_warn("Failed to disassociate station: %pM\n",
-				    sta->addr);
+			ath10k_warn("Failed to disassociate station: %pM vdev %i ret %i\n",
+				    sta->addr, arvif->vdev_id, ret);
 	}
 exit:
 	mutex_unlock(&ar->conf_mutex);
@@ -3212,7 +3573,8 @@ static void ath10k_flush(struct ieee80211_hw *hw, u32 queues, bool drop)
 		}), ATH10K_FLUSH_TIMEOUT_HZ);
 
 	if (ret <= 0 || skip)
-		ath10k_warn("tx not flushed\n");
+		ath10k_warn("tx not flushed (skip %i ar-state %i): %i\n",
+			    skip, ar->state, ret);
 
 skip:
 	mutex_unlock(&ar->conf_mutex);
@@ -3234,23 +3596,14 @@ static int ath10k_suspend(struct ieee80211_hw *hw,
 	struct ath10k *ar = hw->priv;
 	int ret;
 
-	ar->is_target_paused = false;
+	mutex_lock(&ar->conf_mutex);
 
-	ret = ath10k_wmi_pdev_suspend_target(ar);
+	ret = ath10k_wait_for_suspend(ar, WMI_PDEV_SUSPEND);
 	if (ret) {
-		ath10k_warn("could not suspend target (%d)\n", ret);
-		return 1;
-	}
-
-	ret = wait_event_interruptible_timeout(ar->event_queue,
-					       ar->is_target_paused == true,
-					       1 * HZ);
-	if (ret < 0) {
-		ath10k_warn("suspend interrupted (%d)\n", ret);
-		goto resume;
-	} else if (ret == 0) {
-		ath10k_warn("suspend timed out - target pause event never came\n");
-		goto resume;
+		if (ret == -ETIMEDOUT)
+			goto resume;
+		ret = 1;
+		goto exit;
 	}
 
 	ret = ath10k_hif_suspend(ar);
@@ -3259,12 +3612,17 @@ static int ath10k_suspend(struct ieee80211_hw *hw,
 		goto resume;
 	}
 
-	return 0;
+	ret = 0;
+	goto exit;
 resume:
 	ret = ath10k_wmi_pdev_resume_target(ar);
 	if (ret)
 		ath10k_warn("could not resume target (%d)\n", ret);
-	return 1;
+
+	ret = 1;
+exit:
+	mutex_unlock(&ar->conf_mutex);
+	return ret;
 }
 
 static int ath10k_resume(struct ieee80211_hw *hw)
@@ -3272,19 +3630,26 @@ static int ath10k_resume(struct ieee80211_hw *hw)
 	struct ath10k *ar = hw->priv;
 	int ret;
 
+	mutex_lock(&ar->conf_mutex);
+
 	ret = ath10k_hif_resume(ar);
 	if (ret) {
 		ath10k_warn("could not resume hif (%d)\n", ret);
-		return 1;
+		ret = 1;
+		goto exit;
 	}
 
 	ret = ath10k_wmi_pdev_resume_target(ar);
 	if (ret) {
 		ath10k_warn("could not resume target (%d)\n", ret);
-		return 1;
+		ret = 1;
+		goto exit;
 	}
 
-	return 0;
+	ret = 0;
+exit:
+	mutex_unlock(&ar->conf_mutex);
+	return ret;
 }
 #endif
 
@@ -3575,7 +3940,8 @@ static bool ath10k_get_fixed_rate_nss(const struct cfg80211_bitrate_mask *mask,
 
 static int ath10k_set_fixed_rate_param(struct ath10k_vif *arvif,
 				       u8 fixed_rate,
-				       u8 fixed_nss)
+				       u8 fixed_nss,
+				       u8 force_sgi)
 {
 	struct ath10k *ar = arvif->ar;
 	u32 vdev_param;
@@ -3584,12 +3950,16 @@ static int ath10k_set_fixed_rate_param(struct ath10k_vif *arvif,
 	mutex_lock(&ar->conf_mutex);
 
 	if (arvif->fixed_rate == fixed_rate &&
-	    arvif->fixed_nss == fixed_nss)
+	    arvif->fixed_nss == fixed_nss &&
+	    arvif->force_sgi == force_sgi)
 		goto exit;
 
 	if (fixed_rate == WMI_FIXED_RATE_NONE)
 		ath10k_dbg(ATH10K_DBG_MAC, "mac disable fixed bitrate mask\n");
 
+	if (force_sgi)
+		ath10k_dbg(ATH10K_DBG_MAC, "mac force sgi\n");
+
 	vdev_param = ar->wmi.vdev_param->fixed_rate;
 	ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id,
 					vdev_param, fixed_rate);
@@ -3615,6 +3985,19 @@ static int ath10k_set_fixed_rate_param(struct ath10k_vif *arvif,
 
 	arvif->fixed_nss = fixed_nss;
 
+	vdev_param = ar->wmi.vdev_param->sgi;
+	ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, vdev_param,
+					force_sgi);
+
+	if (ret) {
+		ath10k_warn("Could not set sgi param %d: %d\n",
+			    force_sgi, ret);
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	arvif->force_sgi = force_sgi;
+
 exit:
 	mutex_unlock(&ar->conf_mutex);
 	return ret;
@@ -3629,6 +4012,11 @@ static int ath10k_set_bitrate_mask(struct ieee80211_hw *hw,
 	enum ieee80211_band band = ar->hw->conf.chandef.chan->band;
 	u8 fixed_rate = WMI_FIXED_RATE_NONE;
 	u8 fixed_nss = ar->num_rf_chains;
+	u8 force_sgi;
+
+	force_sgi = mask->control[band].gi;
+	if (force_sgi == NL80211_TXRATE_FORCE_LGI)
+		return -EINVAL;
 
 	if (!ath10k_default_bitrate_mask(ar, band, mask)) {
 		if (!ath10k_get_fixed_rate_nss(mask, band,
@@ -3637,7 +4025,113 @@ static int ath10k_set_bitrate_mask(struct ieee80211_hw *hw,
 			return -EINVAL;
 	}
 
-	return ath10k_set_fixed_rate_param(arvif, fixed_rate, fixed_nss);
+	if (fixed_rate == WMI_FIXED_RATE_NONE && force_sgi) {
+		ath10k_warn("Could not force SGI usage for default rate settings\n");
+		return -EINVAL;
+	}
+
+	return ath10k_set_fixed_rate_param(arvif, fixed_rate,
+					   fixed_nss, force_sgi);
+}
+
+static void ath10k_channel_switch_beacon(struct ieee80211_hw *hw,
+					 struct ieee80211_vif *vif,
+					 struct cfg80211_chan_def *chandef)
+{
+	/* there's no need to do anything here. vif->csa_active is enough */
+	return;
+}
+
+static void ath10k_sta_rc_update(struct ieee80211_hw *hw,
+				 struct ieee80211_vif *vif,
+				 struct ieee80211_sta *sta,
+				 u32 changed)
+{
+	struct ath10k *ar = hw->priv;
+	struct ath10k_sta *arsta = (struct ath10k_sta *)sta->drv_priv;
+	u32 bw, smps;
+
+	spin_lock_bh(&ar->data_lock);
+
+	ath10k_dbg(ATH10K_DBG_MAC,
+		   "mac sta rc update for %pM changed %08x bw %d nss %d smps %d\n",
+		   sta->addr, changed, sta->bandwidth, sta->rx_nss,
+		   sta->smps_mode);
+
+	if (changed & IEEE80211_RC_BW_CHANGED) {
+		bw = WMI_PEER_CHWIDTH_20MHZ;
+
+		switch (sta->bandwidth) {
+		case IEEE80211_STA_RX_BW_20:
+			bw = WMI_PEER_CHWIDTH_20MHZ;
+			break;
+		case IEEE80211_STA_RX_BW_40:
+			bw = WMI_PEER_CHWIDTH_40MHZ;
+			break;
+		case IEEE80211_STA_RX_BW_80:
+			bw = WMI_PEER_CHWIDTH_80MHZ;
+			break;
+		case IEEE80211_STA_RX_BW_160:
+			ath10k_warn("mac sta rc update for %pM: invalid bw %d\n",
+				    sta->addr, sta->bandwidth);
+			bw = WMI_PEER_CHWIDTH_20MHZ;
+			break;
+		}
+
+		arsta->bw = bw;
+	}
+
+	if (changed & IEEE80211_RC_NSS_CHANGED)
+		arsta->nss = sta->rx_nss;
+
+	if (changed & IEEE80211_RC_SMPS_CHANGED) {
+		smps = WMI_PEER_SMPS_PS_NONE;
+
+		switch (sta->smps_mode) {
+		case IEEE80211_SMPS_AUTOMATIC:
+		case IEEE80211_SMPS_OFF:
+			smps = WMI_PEER_SMPS_PS_NONE;
+			break;
+		case IEEE80211_SMPS_STATIC:
+			smps = WMI_PEER_SMPS_STATIC;
+			break;
+		case IEEE80211_SMPS_DYNAMIC:
+			smps = WMI_PEER_SMPS_DYNAMIC;
+			break;
+		case IEEE80211_SMPS_NUM_MODES:
+			ath10k_warn("mac sta rc update for %pM: invalid smps: %d\n",
+				    sta->addr, sta->smps_mode);
+			smps = WMI_PEER_SMPS_PS_NONE;
+			break;
+		}
+
+		arsta->smps = smps;
+	}
+
+	if (changed & IEEE80211_RC_SUPP_RATES_CHANGED) {
+		/* FIXME: Not implemented. Probably the only way to do it would
+		 * be to re-assoc the peer. */
+		changed &= ~IEEE80211_RC_SUPP_RATES_CHANGED;
+		ath10k_dbg(ATH10K_DBG_MAC,
+			   "mac sta rc update for %pM: changing supported rates not implemented\n",
+			   sta->addr);
+	}
+
+	arsta->changed |= changed;
+
+	spin_unlock_bh(&ar->data_lock);
+
+	ieee80211_queue_work(hw, &arsta->update_wk);
+}
+
+static u64 ath10k_get_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
+{
+	/*
+	 * FIXME: Return 0 for time being. Need to figure out whether FW
+	 * has the API to fetch 64-bit local TSF
+	 */
+
+	return 0;
 }
 
 static const struct ieee80211_ops ath10k_ops = {
@@ -3663,6 +4157,9 @@ static const struct ieee80211_ops ath10k_ops = {
 	.restart_complete		= ath10k_restart_complete,
 	.get_survey			= ath10k_get_survey,
 	.set_bitrate_mask		= ath10k_set_bitrate_mask,
+	.channel_switch_beacon		= ath10k_channel_switch_beacon,
+	.sta_rc_update			= ath10k_sta_rc_update,
+	.get_tsf			= ath10k_get_tsf,
 #ifdef CONFIG_PM
 	.suspend			= ath10k_suspend,
 	.resume				= ath10k_resume,
@@ -3939,7 +4436,7 @@ struct ath10k_vif *ath10k_get_arvif(struct ath10k *ar, u32 vdev_id)
 						   ath10k_get_arvif_iter,
 						   &arvif_iter);
 	if (!arvif_iter.arvif) {
-		ath10k_warn("No VIF found for VDEV: %d\n", vdev_id);
+		ath10k_warn("No VIF found for vdev %d\n", vdev_id);
 		return NULL;
 	}
 
@@ -4020,7 +4517,8 @@ int ath10k_mac_register(struct ath10k *ar)
 			IEEE80211_HW_HAS_RATE_CONTROL |
 			IEEE80211_HW_SUPPORTS_STATIC_SMPS |
 			IEEE80211_HW_WANT_MONITOR_VIF |
-			IEEE80211_HW_AP_LINK_PS;
+			IEEE80211_HW_AP_LINK_PS |
+			IEEE80211_HW_SPECTRUM_MGMT;
 
 	/* MSDU can have HTT TX fragment pushed in front. The additional 4
 	 * bytes is used for padding/alignment if necessary. */
@@ -4038,10 +4536,12 @@ int ath10k_mac_register(struct ath10k *ar)
 	ar->hw->wiphy->max_scan_ie_len = WLAN_SCAN_PARAMS_MAX_IE_LEN;
 
 	ar->hw->vif_data_size = sizeof(struct ath10k_vif);
+	ar->hw->sta_data_size = sizeof(struct ath10k_sta);
 
 	ar->hw->max_listen_interval = ATH10K_MAX_HW_LISTEN_INTERVAL;
 
 	ar->hw->wiphy->flags |= WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL;
+	ar->hw->wiphy->flags |= WIPHY_FLAG_HAS_CHANNEL_SWITCH;
 	ar->hw->wiphy->max_remain_on_channel_duration = 5000;
 
 	ar->hw->wiphy->flags |= WIPHY_FLAG_AP_UAPSD;
@@ -4076,7 +4576,7 @@ int ath10k_mac_register(struct ath10k *ar)
 	ret = ath_regd_init(&ar->ath_common.regulatory, ar->hw->wiphy,
 			    ath10k_reg_notifier);
 	if (ret) {
-		ath10k_err("Regulatory initialization failed\n");
+		ath10k_err("Regulatory initialization failed: %i\n", ret);
 		goto err_free;
 	}
 
diff --git a/drivers/net/wireless/ath/ath10k/pci.c b/drivers/net/wireless/ath/ath10k/pci.c
index 29fd197d1fd8..9d242d801d9d 100644
--- a/drivers/net/wireless/ath/ath10k/pci.c
+++ b/drivers/net/wireless/ath/ath10k/pci.c
@@ -58,13 +58,12 @@ static DEFINE_PCI_DEVICE_TABLE(ath10k_pci_id_table) = {
 static int ath10k_pci_diag_read_access(struct ath10k *ar, u32 address,
 				       u32 *data);
 
-static void ath10k_pci_process_ce(struct ath10k *ar);
 static int ath10k_pci_post_rx(struct ath10k *ar);
 static int ath10k_pci_post_rx_pipe(struct ath10k_pci_pipe *pipe_info,
 					     int num);
 static void ath10k_pci_rx_pipe_cleanup(struct ath10k_pci_pipe *pipe_info);
-static void ath10k_pci_stop_ce(struct ath10k *ar);
-static int ath10k_pci_device_reset(struct ath10k *ar);
+static int ath10k_pci_cold_reset(struct ath10k *ar);
+static int ath10k_pci_warm_reset(struct ath10k *ar);
 static int ath10k_pci_wait_for_target_init(struct ath10k *ar);
 static int ath10k_pci_init_irq(struct ath10k *ar);
 static int ath10k_pci_deinit_irq(struct ath10k *ar);
@@ -73,7 +72,6 @@ static void ath10k_pci_free_irq(struct ath10k *ar);
 static int ath10k_pci_bmi_wait(struct ath10k_ce_pipe *tx_pipe,
 			       struct ath10k_ce_pipe *rx_pipe,
 			       struct bmi_xfer *xfer);
-static void ath10k_pci_cleanup_ce(struct ath10k *ar);
 
 static const struct ce_attr host_ce_config_wlan[] = {
 	/* CE0: host->target HTC control and raw streams */
@@ -678,34 +676,12 @@ void ath10k_do_pci_sleep(struct ath10k *ar)
 	}
 }
 
-/*
- * FIXME: Handle OOM properly.
- */
-static inline
-struct ath10k_pci_compl *get_free_compl(struct ath10k_pci_pipe *pipe_info)
-{
-	struct ath10k_pci_compl *compl = NULL;
-
-	spin_lock_bh(&pipe_info->pipe_lock);
-	if (list_empty(&pipe_info->compl_free)) {
-		ath10k_warn("Completion buffers are full\n");
-		goto exit;
-	}
-	compl = list_first_entry(&pipe_info->compl_free,
-				 struct ath10k_pci_compl, list);
-	list_del(&compl->list);
-exit:
-	spin_unlock_bh(&pipe_info->pipe_lock);
-	return compl;
-}
-
 /* Called by lower (CE) layer when a send to Target completes. */
 static void ath10k_pci_ce_send_done(struct ath10k_ce_pipe *ce_state)
 {
 	struct ath10k *ar = ce_state->ar;
 	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
-	struct ath10k_pci_pipe *pipe_info =  &ar_pci->pipe_info[ce_state->id];
-	struct ath10k_pci_compl *compl;
+	struct ath10k_hif_cb *cb = &ar_pci->msg_callbacks_current;
 	void *transfer_context;
 	u32 ce_data;
 	unsigned int nbytes;
@@ -714,27 +690,12 @@ static void ath10k_pci_ce_send_done(struct ath10k_ce_pipe *ce_state)
 	while (ath10k_ce_completed_send_next(ce_state, &transfer_context,
 					     &ce_data, &nbytes,
 					     &transfer_id) == 0) {
-		compl = get_free_compl(pipe_info);
-		if (!compl)
-			break;
-
-		compl->state = ATH10K_PCI_COMPL_SEND;
-		compl->ce_state = ce_state;
-		compl->pipe_info = pipe_info;
-		compl->skb = transfer_context;
-		compl->nbytes = nbytes;
-		compl->transfer_id = transfer_id;
-		compl->flags = 0;
+		/* no need to call tx completion for NULL pointers */
+		if (transfer_context == NULL)
+			continue;
 
-		/*
-		 * Add the completion to the processing queue.
-		 */
-		spin_lock_bh(&ar_pci->compl_lock);
-		list_add_tail(&compl->list, &ar_pci->compl_process);
-		spin_unlock_bh(&ar_pci->compl_lock);
+		cb->tx_completion(ar, transfer_context, transfer_id);
 	}
-
-	ath10k_pci_process_ce(ar);
 }
 
 /* Called by lower (CE) layer when data is received from the Target. */
@@ -743,77 +704,100 @@ static void ath10k_pci_ce_recv_data(struct ath10k_ce_pipe *ce_state)
 	struct ath10k *ar = ce_state->ar;
 	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
 	struct ath10k_pci_pipe *pipe_info =  &ar_pci->pipe_info[ce_state->id];
-	struct ath10k_pci_compl *compl;
+	struct ath10k_hif_cb *cb = &ar_pci->msg_callbacks_current;
 	struct sk_buff *skb;
 	void *transfer_context;
 	u32 ce_data;
-	unsigned int nbytes;
+	unsigned int nbytes, max_nbytes;
 	unsigned int transfer_id;
 	unsigned int flags;
+	int err;
 
 	while (ath10k_ce_completed_recv_next(ce_state, &transfer_context,
 					     &ce_data, &nbytes, &transfer_id,
 					     &flags) == 0) {
-		compl = get_free_compl(pipe_info);
-		if (!compl)
-			break;
-
-		compl->state = ATH10K_PCI_COMPL_RECV;
-		compl->ce_state = ce_state;
-		compl->pipe_info = pipe_info;
-		compl->skb = transfer_context;
-		compl->nbytes = nbytes;
-		compl->transfer_id = transfer_id;
-		compl->flags = flags;
+		err = ath10k_pci_post_rx_pipe(pipe_info, 1);
+		if (unlikely(err)) {
+			/* FIXME: retry */
+			ath10k_warn("failed to replenish CE rx ring %d: %d\n",
+				    pipe_info->pipe_num, err);
+		}
 
 		skb = transfer_context;
+		max_nbytes = skb->len + skb_tailroom(skb);
 		dma_unmap_single(ar->dev, ATH10K_SKB_CB(skb)->paddr,
-				 skb->len + skb_tailroom(skb),
-				 DMA_FROM_DEVICE);
-		/*
-		 * Add the completion to the processing queue.
-		 */
-		spin_lock_bh(&ar_pci->compl_lock);
-		list_add_tail(&compl->list, &ar_pci->compl_process);
-		spin_unlock_bh(&ar_pci->compl_lock);
-	}
+				 max_nbytes, DMA_FROM_DEVICE);
+
+		if (unlikely(max_nbytes < nbytes)) {
+			ath10k_warn("rxed more than expected (nbytes %d, max %d)",
+				    nbytes, max_nbytes);
+			dev_kfree_skb_any(skb);
+			continue;
+		}
 
-	ath10k_pci_process_ce(ar);
+		skb_put(skb, nbytes);
+		cb->rx_completion(ar, skb, pipe_info->pipe_num);
+	}
 }
 
-/* Send the first nbytes bytes of the buffer */
-static int ath10k_pci_hif_send_head(struct ath10k *ar, u8 pipe_id,
-				    unsigned int transfer_id,
-				    unsigned int bytes, struct sk_buff *nbuf)
+static int ath10k_pci_hif_tx_sg(struct ath10k *ar, u8 pipe_id,
+				struct ath10k_hif_sg_item *items, int n_items)
 {
-	struct ath10k_skb_cb *skb_cb = ATH10K_SKB_CB(nbuf);
 	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
-	struct ath10k_pci_pipe *pipe_info = &(ar_pci->pipe_info[pipe_id]);
-	struct ath10k_ce_pipe *ce_hdl = pipe_info->ce_hdl;
-	unsigned int len;
-	u32 flags = 0;
-	int ret;
+	struct ath10k_pci_pipe *pci_pipe = &ar_pci->pipe_info[pipe_id];
+	struct ath10k_ce_pipe *ce_pipe = pci_pipe->ce_hdl;
+	struct ath10k_ce_ring *src_ring = ce_pipe->src_ring;
+	unsigned int nentries_mask = src_ring->nentries_mask;
+	unsigned int sw_index = src_ring->sw_index;
+	unsigned int write_index = src_ring->write_index;
+	int err, i;
 
-	len = min(bytes, nbuf->len);
-	bytes -= len;
+	spin_lock_bh(&ar_pci->ce_lock);
 
-	if (len & 3)
-		ath10k_warn("skb not aligned to 4-byte boundary (%d)\n", len);
+	if (unlikely(CE_RING_DELTA(nentries_mask,
+				   write_index, sw_index - 1) < n_items)) {
+		err = -ENOBUFS;
+		goto unlock;
+	}
 
-	ath10k_dbg(ATH10K_DBG_PCI,
-		   "pci send data vaddr %p paddr 0x%llx len %d as %d bytes\n",
-		   nbuf->data, (unsigned long long) skb_cb->paddr,
-		   nbuf->len, len);
-	ath10k_dbg_dump(ATH10K_DBG_PCI_DUMP, NULL,
-			"ath10k tx: data: ",
-			nbuf->data, nbuf->len);
-
-	ret = ath10k_ce_send(ce_hdl, nbuf, skb_cb->paddr, len, transfer_id,
-			     flags);
-	if (ret)
-		ath10k_warn("failed to send sk_buff to CE: %p\n", nbuf);
+	for (i = 0; i < n_items - 1; i++) {
+		ath10k_dbg(ATH10K_DBG_PCI,
+			   "pci tx item %d paddr 0x%08x len %d n_items %d\n",
+			   i, items[i].paddr, items[i].len, n_items);
+		ath10k_dbg_dump(ATH10K_DBG_PCI_DUMP, NULL, "item data: ",
+				items[i].vaddr, items[i].len);
 
-	return ret;
+		err = ath10k_ce_send_nolock(ce_pipe,
+					    items[i].transfer_context,
+					    items[i].paddr,
+					    items[i].len,
+					    items[i].transfer_id,
+					    CE_SEND_FLAG_GATHER);
+		if (err)
+			goto unlock;
+	}
+
+	/* `i` is equal to `n_items -1` after for() */
+
+	ath10k_dbg(ATH10K_DBG_PCI,
+		   "pci tx item %d paddr 0x%08x len %d n_items %d\n",
+		   i, items[i].paddr, items[i].len, n_items);
+	ath10k_dbg_dump(ATH10K_DBG_PCI_DUMP, NULL, "item data: ",
+			items[i].vaddr, items[i].len);
+
+	err = ath10k_ce_send_nolock(ce_pipe,
+				    items[i].transfer_context,
+				    items[i].paddr,
+				    items[i].len,
+				    items[i].transfer_id,
+				    0);
+	if (err)
+		goto unlock;
+
+	err = 0;
+unlock:
+	spin_unlock_bh(&ar_pci->ce_lock);
+	return err;
 }
 
 static u16 ath10k_pci_hif_get_free_queue_number(struct ath10k *ar, u8 pipe)
@@ -833,9 +817,7 @@ static void ath10k_pci_hif_dump_area(struct ath10k *ar)
 	ath10k_err("firmware crashed!\n");
 	ath10k_err("hardware name %s version 0x%x\n",
 		   ar->hw_params.name, ar->target_version);
-	ath10k_err("firmware version: %u.%u.%u.%u\n", ar->fw_version_major,
-		   ar->fw_version_minor, ar->fw_version_release,
-		   ar->fw_version_build);
+	ath10k_err("firmware version: %s\n", ar->hw->wiphy->fw_version);
 
 	host_addr = host_interest_item_address(HI_ITEM(hi_failure_state));
 	ret = ath10k_pci_diag_read_mem(ar, host_addr,
@@ -904,52 +886,6 @@ static void ath10k_pci_hif_set_callbacks(struct ath10k *ar,
 	       sizeof(ar_pci->msg_callbacks_current));
 }
 
-static int ath10k_pci_alloc_compl(struct ath10k *ar)
-{
-	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
-	const struct ce_attr *attr;
-	struct ath10k_pci_pipe *pipe_info;
-	struct ath10k_pci_compl *compl;
-	int i, pipe_num, completions;
-
-	spin_lock_init(&ar_pci->compl_lock);
-	INIT_LIST_HEAD(&ar_pci->compl_process);
-
-	for (pipe_num = 0; pipe_num < CE_COUNT; pipe_num++) {
-		pipe_info = &ar_pci->pipe_info[pipe_num];
-
-		spin_lock_init(&pipe_info->pipe_lock);
-		INIT_LIST_HEAD(&pipe_info->compl_free);
-
-		/* Handle Diagnostic CE specially */
-		if (pipe_info->ce_hdl == ar_pci->ce_diag)
-			continue;
-
-		attr = &host_ce_config_wlan[pipe_num];
-		completions = 0;
-
-		if (attr->src_nentries)
-			completions += attr->src_nentries;
-
-		if (attr->dest_nentries)
-			completions += attr->dest_nentries;
-
-		for (i = 0; i < completions; i++) {
-			compl = kmalloc(sizeof(*compl), GFP_KERNEL);
-			if (!compl) {
-				ath10k_warn("No memory for completion state\n");
-				ath10k_pci_cleanup_ce(ar);
-				return -ENOMEM;
-			}
-
-			compl->state = ATH10K_PCI_COMPL_FREE;
-			list_add_tail(&compl->list, &pipe_info->compl_free);
-		}
-	}
-
-	return 0;
-}
-
 static int ath10k_pci_setup_ce_irq(struct ath10k *ar)
 {
 	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
@@ -994,147 +930,6 @@ static void ath10k_pci_kill_tasklet(struct ath10k *ar)
 		tasklet_kill(&ar_pci->pipe_info[i].intr);
 }
 
-static void ath10k_pci_stop_ce(struct ath10k *ar)
-{
-	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
-	struct ath10k_pci_compl *compl;
-	struct sk_buff *skb;
-
-	/* Mark pending completions as aborted, so that upper layers free up
-	 * their associated resources */
-	spin_lock_bh(&ar_pci->compl_lock);
-	list_for_each_entry(compl, &ar_pci->compl_process, list) {
-		skb = compl->skb;
-		ATH10K_SKB_CB(skb)->is_aborted = true;
-	}
-	spin_unlock_bh(&ar_pci->compl_lock);
-}
-
-static void ath10k_pci_cleanup_ce(struct ath10k *ar)
-{
-	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
-	struct ath10k_pci_compl *compl, *tmp;
-	struct ath10k_pci_pipe *pipe_info;
-	struct sk_buff *netbuf;
-	int pipe_num;
-
-	/* Free pending completions. */
-	spin_lock_bh(&ar_pci->compl_lock);
-	if (!list_empty(&ar_pci->compl_process))
-		ath10k_warn("pending completions still present! possible memory leaks.\n");
-
-	list_for_each_entry_safe(compl, tmp, &ar_pci->compl_process, list) {
-		list_del(&compl->list);
-		netbuf = compl->skb;
-		dev_kfree_skb_any(netbuf);
-		kfree(compl);
-	}
-	spin_unlock_bh(&ar_pci->compl_lock);
-
-	/* Free unused completions for each pipe. */
-	for (pipe_num = 0; pipe_num < CE_COUNT; pipe_num++) {
-		pipe_info = &ar_pci->pipe_info[pipe_num];
-
-		spin_lock_bh(&pipe_info->pipe_lock);
-		list_for_each_entry_safe(compl, tmp,
-					 &pipe_info->compl_free, list) {
-			list_del(&compl->list);
-			kfree(compl);
-		}
-		spin_unlock_bh(&pipe_info->pipe_lock);
-	}
-}
-
-static void ath10k_pci_process_ce(struct ath10k *ar)
-{
-	struct ath10k_pci *ar_pci = ar->hif.priv;
-	struct ath10k_hif_cb *cb = &ar_pci->msg_callbacks_current;
-	struct ath10k_pci_compl *compl;
-	struct sk_buff *skb;
-	unsigned int nbytes;
-	int ret, send_done = 0;
-
-	/* Upper layers aren't ready to handle tx/rx completions in parallel so
-	 * we must serialize all completion processing. */
-
-	spin_lock_bh(&ar_pci->compl_lock);
-	if (ar_pci->compl_processing) {
-		spin_unlock_bh(&ar_pci->compl_lock);
-		return;
-	}
-	ar_pci->compl_processing = true;
-	spin_unlock_bh(&ar_pci->compl_lock);
-
-	for (;;) {
-		spin_lock_bh(&ar_pci->compl_lock);
-		if (list_empty(&ar_pci->compl_process)) {
-			spin_unlock_bh(&ar_pci->compl_lock);
-			break;
-		}
-		compl = list_first_entry(&ar_pci->compl_process,
-					 struct ath10k_pci_compl, list);
-		list_del(&compl->list);
-		spin_unlock_bh(&ar_pci->compl_lock);
-
-		switch (compl->state) {
-		case ATH10K_PCI_COMPL_SEND:
-			cb->tx_completion(ar,
-					  compl->skb,
-					  compl->transfer_id);
-			send_done = 1;
-			break;
-		case ATH10K_PCI_COMPL_RECV:
-			ret = ath10k_pci_post_rx_pipe(compl->pipe_info, 1);
-			if (ret) {
-				ath10k_warn("failed to post RX buffer for pipe %d: %d\n",
-					    compl->pipe_info->pipe_num, ret);
-				break;
-			}
-
-			skb = compl->skb;
-			nbytes = compl->nbytes;
-
-			ath10k_dbg(ATH10K_DBG_PCI,
-				   "ath10k_pci_ce_recv_data netbuf=%p  nbytes=%d\n",
-				   skb, nbytes);
-			ath10k_dbg_dump(ATH10K_DBG_PCI_DUMP, NULL,
-					"ath10k rx: ", skb->data, nbytes);
-
-			if (skb->len + skb_tailroom(skb) >= nbytes) {
-				skb_trim(skb, 0);
-				skb_put(skb, nbytes);
-				cb->rx_completion(ar, skb,
-						  compl->pipe_info->pipe_num);
-			} else {
-				ath10k_warn("rxed more than expected (nbytes %d, max %d)",
-					    nbytes,
-					    skb->len + skb_tailroom(skb));
-			}
-			break;
-		case ATH10K_PCI_COMPL_FREE:
-			ath10k_warn("free completion cannot be processed\n");
-			break;
-		default:
-			ath10k_warn("invalid completion state (%d)\n",
-				    compl->state);
-			break;
-		}
-
-		compl->state = ATH10K_PCI_COMPL_FREE;
-
-		/*
-		 * Add completion back to the pipe's free list.
-		 */
-		spin_lock_bh(&compl->pipe_info->pipe_lock);
-		list_add_tail(&compl->list, &compl->pipe_info->compl_free);
-		spin_unlock_bh(&compl->pipe_info->pipe_lock);
-	}
-
-	spin_lock_bh(&ar_pci->compl_lock);
-	ar_pci->compl_processing = false;
-	spin_unlock_bh(&ar_pci->compl_lock);
-}
-
 /* TODO - temporary mapping while we have too few CE's */
 static int ath10k_pci_hif_map_service_to_pipe(struct ath10k *ar,
 					      u16 service_id, u8 *ul_pipe,
@@ -1306,17 +1101,11 @@ static int ath10k_pci_hif_start(struct ath10k *ar)
 	ath10k_pci_free_early_irq(ar);
 	ath10k_pci_kill_tasklet(ar);
 
-	ret = ath10k_pci_alloc_compl(ar);
-	if (ret) {
-		ath10k_warn("failed to allocate CE completions: %d\n", ret);
-		goto err_early_irq;
-	}
-
 	ret = ath10k_pci_request_irq(ar);
 	if (ret) {
 		ath10k_warn("failed to post RX buffers for all pipes: %d\n",
 			    ret);
-		goto err_free_compl;
+		goto err_early_irq;
 	}
 
 	ret = ath10k_pci_setup_ce_irq(ar);
@@ -1340,10 +1129,6 @@ err_stop:
 	ath10k_ce_disable_interrupts(ar);
 	ath10k_pci_free_irq(ar);
 	ath10k_pci_kill_tasklet(ar);
-	ath10k_pci_stop_ce(ar);
-	ath10k_pci_process_ce(ar);
-err_free_compl:
-	ath10k_pci_cleanup_ce(ar);
 err_early_irq:
 	/* Though there should be no interrupts (device was reset)
 	 * power_down() expects the early IRQ to be installed as per the
@@ -1414,18 +1199,10 @@ static void ath10k_pci_tx_pipe_cleanup(struct ath10k_pci_pipe *pipe_info)
 
 	while (ath10k_ce_cancel_send_next(ce_hdl, (void **)&netbuf,
 					  &ce_data, &nbytes, &id) == 0) {
-		/*
-		 * Indicate the completion to higer layer to free
-		 * the buffer
-		 */
-
-		if (!netbuf) {
-			ath10k_warn("invalid sk_buff on CE %d - NULL pointer. firmware crashed?\n",
-				    ce_hdl->id);
+		/* no need to call tx completion for NULL pointers */
+		if (!netbuf)
 			continue;
-		}
 
-		ATH10K_SKB_CB(netbuf)->is_aborted = true;
 		ar_pci->msg_callbacks_current.tx_completion(ar,
 							    netbuf,
 							    id);
@@ -1483,7 +1260,6 @@ static void ath10k_pci_hif_stop(struct ath10k *ar)
 
 	ath10k_pci_free_irq(ar);
 	ath10k_pci_kill_tasklet(ar);
-	ath10k_pci_stop_ce(ar);
 
 	ret = ath10k_pci_request_early_irq(ar);
 	if (ret)
@@ -1493,8 +1269,6 @@ static void ath10k_pci_hif_stop(struct ath10k *ar)
 	 * not DMA nor interrupt. We process the leftovers and then free
 	 * everything else up. */
 
-	ath10k_pci_process_ce(ar);
-	ath10k_pci_cleanup_ce(ar);
 	ath10k_pci_buffer_cleanup(ar);
 
 	/* Make the sure the device won't access any structures on the host by
@@ -1502,7 +1276,7 @@ static void ath10k_pci_hif_stop(struct ath10k *ar)
 	 * configuration during init. If ringbuffers are freed and the device
 	 * were to access them this could lead to memory corruption on the
 	 * host. */
-	ath10k_pci_device_reset(ar);
+	ath10k_pci_warm_reset(ar);
 
 	ar_pci->started = 0;
 }
@@ -1993,7 +1767,94 @@ static void ath10k_pci_fw_interrupt_handler(struct ath10k *ar)
 	ath10k_pci_sleep(ar);
 }
 
-static int ath10k_pci_hif_power_up(struct ath10k *ar)
+static int ath10k_pci_warm_reset(struct ath10k *ar)
+{
+	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
+	int ret = 0;
+	u32 val;
+
+	ath10k_dbg(ATH10K_DBG_BOOT, "boot performing warm chip reset\n");
+
+	ret = ath10k_do_pci_wake(ar);
+	if (ret) {
+		ath10k_err("failed to wake up target: %d\n", ret);
+		return ret;
+	}
+
+	/* debug */
+	val = ath10k_pci_read32(ar, SOC_CORE_BASE_ADDRESS +
+				PCIE_INTR_CAUSE_ADDRESS);
+	ath10k_dbg(ATH10K_DBG_BOOT, "boot host cpu intr cause: 0x%08x\n", val);
+
+	val = ath10k_pci_read32(ar, SOC_CORE_BASE_ADDRESS +
+				CPU_INTR_ADDRESS);
+	ath10k_dbg(ATH10K_DBG_BOOT, "boot target cpu intr cause: 0x%08x\n",
+		   val);
+
+	/* disable pending irqs */
+	ath10k_pci_write32(ar, SOC_CORE_BASE_ADDRESS +
+			   PCIE_INTR_ENABLE_ADDRESS, 0);
+
+	ath10k_pci_write32(ar, SOC_CORE_BASE_ADDRESS +
+			   PCIE_INTR_CLR_ADDRESS, ~0);
+
+	msleep(100);
+
+	/* clear fw indicator */
+	ath10k_pci_write32(ar, ar_pci->fw_indicator_address, 0);
+
+	/* clear target LF timer interrupts */
+	val = ath10k_pci_read32(ar, RTC_SOC_BASE_ADDRESS +
+				SOC_LF_TIMER_CONTROL0_ADDRESS);
+	ath10k_pci_write32(ar, RTC_SOC_BASE_ADDRESS +
+			   SOC_LF_TIMER_CONTROL0_ADDRESS,
+			   val & ~SOC_LF_TIMER_CONTROL0_ENABLE_MASK);
+
+	/* reset CE */
+	val = ath10k_pci_read32(ar, RTC_SOC_BASE_ADDRESS +
+				SOC_RESET_CONTROL_ADDRESS);
+	ath10k_pci_write32(ar, RTC_SOC_BASE_ADDRESS + SOC_RESET_CONTROL_ADDRESS,
+			   val | SOC_RESET_CONTROL_CE_RST_MASK);
+	val = ath10k_pci_read32(ar, RTC_SOC_BASE_ADDRESS +
+				SOC_RESET_CONTROL_ADDRESS);
+	msleep(10);
+
+	/* unreset CE */
+	ath10k_pci_write32(ar, RTC_SOC_BASE_ADDRESS + SOC_RESET_CONTROL_ADDRESS,
+			   val & ~SOC_RESET_CONTROL_CE_RST_MASK);
+	val = ath10k_pci_read32(ar, RTC_SOC_BASE_ADDRESS +
+				SOC_RESET_CONTROL_ADDRESS);
+	msleep(10);
+
+	/* debug */
+	val = ath10k_pci_read32(ar, SOC_CORE_BASE_ADDRESS +
+				PCIE_INTR_CAUSE_ADDRESS);
+	ath10k_dbg(ATH10K_DBG_BOOT, "boot host cpu intr cause: 0x%08x\n", val);
+
+	val = ath10k_pci_read32(ar, SOC_CORE_BASE_ADDRESS +
+				CPU_INTR_ADDRESS);
+	ath10k_dbg(ATH10K_DBG_BOOT, "boot target cpu intr cause: 0x%08x\n",
+		   val);
+
+	/* CPU warm reset */
+	val = ath10k_pci_read32(ar, RTC_SOC_BASE_ADDRESS +
+				SOC_RESET_CONTROL_ADDRESS);
+	ath10k_pci_write32(ar, RTC_SOC_BASE_ADDRESS + SOC_RESET_CONTROL_ADDRESS,
+			   val | SOC_RESET_CONTROL_CPU_WARM_RST_MASK);
+
+	val = ath10k_pci_read32(ar, RTC_SOC_BASE_ADDRESS +
+				SOC_RESET_CONTROL_ADDRESS);
+	ath10k_dbg(ATH10K_DBG_BOOT, "boot target reset state: 0x%08x\n", val);
+
+	msleep(100);
+
+	ath10k_dbg(ATH10K_DBG_BOOT, "boot warm reset complete\n");
+
+	ath10k_do_pci_sleep(ar);
+	return ret;
+}
+
+static int __ath10k_pci_hif_power_up(struct ath10k *ar, bool cold_reset)
 {
 	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
 	const char *irq_mode;
@@ -2009,7 +1870,11 @@ static int ath10k_pci_hif_power_up(struct ath10k *ar)
 	 * is in an unexpected state. We try to catch that here in order to
 	 * reset the Target and retry the probe.
 	 */
-	ret = ath10k_pci_device_reset(ar);
+	if (cold_reset)
+		ret = ath10k_pci_cold_reset(ar);
+	else
+		ret = ath10k_pci_warm_reset(ar);
+
 	if (ret) {
 		ath10k_err("failed to reset target: %d\n", ret);
 		goto err;
@@ -2079,7 +1944,7 @@ err_deinit_irq:
 	ath10k_pci_deinit_irq(ar);
 err_ce:
 	ath10k_pci_ce_deinit(ar);
-	ath10k_pci_device_reset(ar);
+	ath10k_pci_warm_reset(ar);
 err_ps:
 	if (!test_bit(ATH10K_PCI_FEATURE_SOC_POWER_SAVE, ar_pci->features))
 		ath10k_do_pci_sleep(ar);
@@ -2087,6 +1952,34 @@ err:
 	return ret;
 }
 
+static int ath10k_pci_hif_power_up(struct ath10k *ar)
+{
+	int ret;
+
+	/*
+	 * Hardware CUS232 version 2 has some issues with cold reset and the
+	 * preferred (and safer) way to perform a device reset is through a
+	 * warm reset.
+	 *
+	 * Warm reset doesn't always work though (notably after a firmware
+	 * crash) so fall back to cold reset if necessary.
+	 */
+	ret = __ath10k_pci_hif_power_up(ar, false);
+	if (ret) {
+		ath10k_warn("failed to power up target using warm reset (%d), trying cold reset\n",
+			    ret);
+
+		ret = __ath10k_pci_hif_power_up(ar, true);
+		if (ret) {
+			ath10k_err("failed to power up target using cold reset too (%d)\n",
+				   ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
 static void ath10k_pci_hif_power_down(struct ath10k *ar)
 {
 	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
@@ -2094,7 +1987,7 @@ static void ath10k_pci_hif_power_down(struct ath10k *ar)
 	ath10k_pci_free_early_irq(ar);
 	ath10k_pci_kill_tasklet(ar);
 	ath10k_pci_deinit_irq(ar);
-	ath10k_pci_device_reset(ar);
+	ath10k_pci_warm_reset(ar);
 
 	ath10k_pci_ce_deinit(ar);
 	if (!test_bit(ATH10K_PCI_FEATURE_SOC_POWER_SAVE, ar_pci->features))
@@ -2151,7 +2044,7 @@ static int ath10k_pci_hif_resume(struct ath10k *ar)
 #endif
 
 static const struct ath10k_hif_ops ath10k_pci_hif_ops = {
-	.send_head		= ath10k_pci_hif_send_head,
+	.tx_sg			= ath10k_pci_hif_tx_sg,
 	.exchange_bmi_msg	= ath10k_pci_hif_exchange_bmi_msg,
 	.start			= ath10k_pci_hif_start,
 	.stop			= ath10k_pci_hif_stop,
@@ -2411,11 +2304,10 @@ static int ath10k_pci_init_irq(struct ath10k *ar)
 	/* Try MSI-X */
 	if (ath10k_pci_irq_mode == ATH10K_PCI_IRQ_AUTO && msix_supported) {
 		ar_pci->num_msi_intrs = MSI_NUM_REQUEST;
-		ret = pci_enable_msi_block(ar_pci->pdev, ar_pci->num_msi_intrs);
-		if (ret == 0)
-			return 0;
+		ret = pci_enable_msi_range(ar_pci->pdev, ar_pci->num_msi_intrs,
+							 ar_pci->num_msi_intrs);
 		if (ret > 0)
-			pci_disable_msi(ar_pci->pdev);
+			return 0;
 
 		/* fall-through */
 	}
@@ -2482,6 +2374,8 @@ static int ath10k_pci_deinit_irq(struct ath10k *ar)
 	case MSI_NUM_REQUEST:
 		pci_disable_msi(ar_pci->pdev);
 		return 0;
+	default:
+		pci_disable_msi(ar_pci->pdev);
 	}
 
 	ath10k_warn("unknown irq configuration upon deinit\n");
@@ -2523,7 +2417,7 @@ out:
 	return ret;
 }
 
-static int ath10k_pci_device_reset(struct ath10k *ar)
+static int ath10k_pci_cold_reset(struct ath10k *ar)
 {
 	int i, ret;
 	u32 val;
diff --git a/drivers/net/wireless/ath/ath10k/pci.h b/drivers/net/wireless/ath/ath10k/pci.h
index a4f32038c440..b43fdb4f7319 100644
--- a/drivers/net/wireless/ath/ath10k/pci.h
+++ b/drivers/net/wireless/ath/ath10k/pci.h
@@ -43,23 +43,6 @@ struct bmi_xfer {
 	u32 resp_len;
 };
 
-enum ath10k_pci_compl_state {
-	ATH10K_PCI_COMPL_FREE = 0,
-	ATH10K_PCI_COMPL_SEND,
-	ATH10K_PCI_COMPL_RECV,
-};
-
-struct ath10k_pci_compl {
-	struct list_head list;
-	enum ath10k_pci_compl_state state;
-	struct ath10k_ce_pipe *ce_state;
-	struct ath10k_pci_pipe *pipe_info;
-	struct sk_buff *skb;
-	unsigned int nbytes;
-	unsigned int transfer_id;
-	unsigned int flags;
-};
-
 /*
  * PCI-specific Target state
  *
@@ -175,9 +158,6 @@ struct ath10k_pci_pipe {
 	/* protects compl_free and num_send_allowed */
 	spinlock_t pipe_lock;
 
-	/* List of free CE completion slots */
-	struct list_head compl_free;
-
 	struct ath10k_pci *ar_pci;
 	struct tasklet_struct intr;
 };
@@ -205,14 +185,6 @@ struct ath10k_pci {
 	atomic_t keep_awake_count;
 	bool verified_awake;
 
-	/* List of CE completions to be processed */
-	struct list_head compl_process;
-
-	/* protects compl_processing and compl_process */
-	spinlock_t compl_lock;
-
-	bool compl_processing;
-
 	struct ath10k_pci_pipe pipe_info[CE_COUNT_MAX];
 
 	struct ath10k_hif_cb msg_callbacks_current;
diff --git a/drivers/net/wireless/ath/ath10k/txrx.c b/drivers/net/wireless/ath/ath10k/txrx.c
index 74f45fa6f428..0541dd939ce9 100644
--- a/drivers/net/wireless/ath/ath10k/txrx.c
+++ b/drivers/net/wireless/ath/ath10k/txrx.c
@@ -51,7 +51,8 @@ void ath10k_txrx_tx_unref(struct ath10k_htt *htt,
 	struct ieee80211_tx_info *info;
 	struct ath10k_skb_cb *skb_cb;
 	struct sk_buff *msdu;
-	int ret;
+
+	lockdep_assert_held(&htt->tx_lock);
 
 	ath10k_dbg(ATH10K_DBG_HTT, "htt tx completion msdu_id %u discard %d no_ack %d\n",
 		   tx_done->msdu_id, !!tx_done->discard, !!tx_done->no_ack);
@@ -65,12 +66,12 @@ void ath10k_txrx_tx_unref(struct ath10k_htt *htt,
 	msdu = htt->pending_tx[tx_done->msdu_id];
 	skb_cb = ATH10K_SKB_CB(msdu);
 
-	ret = ath10k_skb_unmap(dev, msdu);
-	if (ret)
-		ath10k_warn("data skb unmap failed (%d)\n", ret);
+	dma_unmap_single(dev, skb_cb->paddr, msdu->len, DMA_TO_DEVICE);
 
-	if (skb_cb->htt.frag_len)
-		skb_pull(msdu, skb_cb->htt.frag_len + skb_cb->htt.pad_len);
+	if (skb_cb->htt.txbuf)
+		dma_pool_free(htt->tx_pool,
+			      skb_cb->htt.txbuf,
+			      skb_cb->htt.txbuf_paddr);
 
 	ath10k_report_offchan_tx(htt->ar, msdu);
 
@@ -92,13 +93,11 @@ void ath10k_txrx_tx_unref(struct ath10k_htt *htt,
 	/* we do not own the msdu anymore */
 
 exit:
-	spin_lock_bh(&htt->tx_lock);
 	htt->pending_tx[tx_done->msdu_id] = NULL;
 	ath10k_htt_tx_free_msdu_id(htt, tx_done->msdu_id);
 	__ath10k_htt_tx_dec_pending(htt);
 	if (htt->num_pending_tx == 0)
 		wake_up(&htt->empty_tx_wq);
-	spin_unlock_bh(&htt->tx_lock);
 }
 
 static const u8 rx_legacy_rate_idx[] = {
@@ -204,7 +203,7 @@ static void process_rx_rates(struct ath10k *ar, struct htt_rx_info *info,
 			break;
 		/* 80MHZ */
 		case 2:
-			status->flag |= RX_FLAG_80MHZ;
+			status->vht_flag |= RX_VHT_FLAG_80MHZ;
 		}
 
 		status->flag |= RX_FLAG_VHT;
@@ -258,20 +257,26 @@ void ath10k_process_rx(struct ath10k *ar, struct htt_rx_info *info)
 	status->band = ch->band;
 	status->freq = ch->center_freq;
 
+	if (info->rate.info0 & HTT_RX_INDICATION_INFO0_END_VALID) {
+		/* TSF available only in 32-bit */
+		status->mactime = info->tsf & 0xffffffff;
+		status->flag |= RX_FLAG_MACTIME_END;
+	}
+
 	ath10k_dbg(ATH10K_DBG_DATA,
-		   "rx skb %p len %u %s%s%s%s%s %srate_idx %u vht_nss %u freq %u band %u\n",
+		   "rx skb %p len %u %s%s%s%s%s %srate_idx %u vht_nss %u freq %u band %u flag 0x%x fcs-err %i\n",
 		   info->skb,
 		   info->skb->len,
 		   status->flag == 0 ? "legacy" : "",
 		   status->flag & RX_FLAG_HT ? "ht" : "",
 		   status->flag & RX_FLAG_VHT ? "vht" : "",
 		   status->flag & RX_FLAG_40MHZ ? "40" : "",
-		   status->flag & RX_FLAG_80MHZ ? "80" : "",
+		   status->vht_flag & RX_VHT_FLAG_80MHZ ? "80" : "",
 		   status->flag & RX_FLAG_SHORT_GI ? "sgi " : "",
 		   status->rate_idx,
 		   status->vht_nss,
 		   status->freq,
-		   status->band);
+		   status->band, status->flag, info->fcs_err);
 	ath10k_dbg_dump(ATH10K_DBG_HTT_DUMP, NULL, "rx skb: ",
 			info->skb->data, info->skb->len);
 
@@ -378,7 +383,8 @@ void ath10k_peer_unmap_event(struct ath10k_htt *htt,
 	spin_lock_bh(&ar->data_lock);
 	peer = ath10k_peer_find_by_id(ar, ev->peer_id);
 	if (!peer) {
-		ath10k_warn("unknown peer id %d\n", ev->peer_id);
+		ath10k_warn("peer-unmap-event: unknown peer id %d\n",
+			    ev->peer_id);
 		goto exit;
 	}
 
diff --git a/drivers/net/wireless/ath/ath10k/wmi.c b/drivers/net/wireless/ath/ath10k/wmi.c
index 712a606a080a..cb1f7b5bcf4c 100644
--- a/drivers/net/wireless/ath/ath10k/wmi.c
+++ b/drivers/net/wireless/ath/ath10k/wmi.c
@@ -213,7 +213,7 @@ static struct wmi_cmd_map wmi_10x_cmd_map = {
 	.p2p_go_set_beacon_ie = WMI_10X_P2P_GO_SET_BEACON_IE,
 	.p2p_go_set_probe_resp_ie = WMI_10X_P2P_GO_SET_PROBE_RESP_IE,
 	.p2p_set_vendor_ie_data_cmdid = WMI_CMD_UNSUPPORTED,
-	.ap_ps_peer_param_cmdid = WMI_CMD_UNSUPPORTED,
+	.ap_ps_peer_param_cmdid = WMI_10X_AP_PS_PEER_PARAM_CMDID,
 	.ap_ps_peer_uapsd_coex_cmdid = WMI_CMD_UNSUPPORTED,
 	.peer_rate_retry_sched_cmdid = WMI_10X_PEER_RATE_RETRY_SCHED_CMDID,
 	.wlan_profile_trigger_cmdid = WMI_10X_WLAN_PROFILE_TRIGGER_CMDID,
@@ -420,7 +420,6 @@ static struct wmi_pdev_param_map wmi_pdev_param_map = {
 	.bcnflt_stats_update_period = WMI_PDEV_PARAM_BCNFLT_STATS_UPDATE_PERIOD,
 	.pmf_qos = WMI_PDEV_PARAM_PMF_QOS,
 	.arp_ac_override = WMI_PDEV_PARAM_ARP_AC_OVERRIDE,
-	.arpdhcp_ac_override = WMI_PDEV_PARAM_UNSUPPORTED,
 	.dcs = WMI_PDEV_PARAM_DCS,
 	.ani_enable = WMI_PDEV_PARAM_ANI_ENABLE,
 	.ani_poll_period = WMI_PDEV_PARAM_ANI_POLL_PERIOD,
@@ -472,8 +471,7 @@ static struct wmi_pdev_param_map wmi_10x_pdev_param_map = {
 	.bcnflt_stats_update_period =
 				WMI_10X_PDEV_PARAM_BCNFLT_STATS_UPDATE_PERIOD,
 	.pmf_qos = WMI_10X_PDEV_PARAM_PMF_QOS,
-	.arp_ac_override = WMI_PDEV_PARAM_UNSUPPORTED,
-	.arpdhcp_ac_override = WMI_10X_PDEV_PARAM_ARPDHCP_AC_OVERRIDE,
+	.arp_ac_override = WMI_10X_PDEV_PARAM_ARPDHCP_AC_OVERRIDE,
 	.dcs = WMI_10X_PDEV_PARAM_DCS,
 	.ani_enable = WMI_10X_PDEV_PARAM_ANI_ENABLE,
 	.ani_poll_period = WMI_10X_PDEV_PARAM_ANI_POLL_PERIOD,
@@ -561,7 +559,6 @@ err_pull:
 
 static void ath10k_wmi_tx_beacon_nowait(struct ath10k_vif *arvif)
 {
-	struct wmi_bcn_tx_arg arg = {0};
 	int ret;
 
 	lockdep_assert_held(&arvif->ar->data_lock);
@@ -569,18 +566,16 @@ static void ath10k_wmi_tx_beacon_nowait(struct ath10k_vif *arvif)
 	if (arvif->beacon == NULL)
 		return;
 
-	arg.vdev_id = arvif->vdev_id;
-	arg.tx_rate = 0;
-	arg.tx_power = 0;
-	arg.bcn = arvif->beacon->data;
-	arg.bcn_len = arvif->beacon->len;
+	if (arvif->beacon_sent)
+		return;
 
-	ret = ath10k_wmi_beacon_send_nowait(arvif->ar, &arg);
+	ret = ath10k_wmi_beacon_send_ref_nowait(arvif);
 	if (ret)
 		return;
 
-	dev_kfree_skb_any(arvif->beacon);
-	arvif->beacon = NULL;
+	/* We need to retain the arvif->beacon reference for DMA unmapping and
+	 * freeing the skbuff later. */
+	arvif->beacon_sent = true;
 }
 
 static void ath10k_wmi_tx_beacons_iter(void *data, u8 *mac,
@@ -1116,7 +1111,27 @@ static void ath10k_wmi_event_vdev_stopped(struct ath10k *ar,
 static void ath10k_wmi_event_peer_sta_kickout(struct ath10k *ar,
 					      struct sk_buff *skb)
 {
-	ath10k_dbg(ATH10K_DBG_WMI, "WMI_PEER_STA_KICKOUT_EVENTID\n");
+	struct wmi_peer_sta_kickout_event *ev;
+	struct ieee80211_sta *sta;
+
+	ev = (struct wmi_peer_sta_kickout_event *)skb->data;
+
+	ath10k_dbg(ATH10K_DBG_WMI, "wmi event peer sta kickout %pM\n",
+		   ev->peer_macaddr.addr);
+
+	rcu_read_lock();
+
+	sta = ieee80211_find_sta_by_ifaddr(ar->hw, ev->peer_macaddr.addr, NULL);
+	if (!sta) {
+		ath10k_warn("Spurious quick kickout for STA %pM\n",
+			    ev->peer_macaddr.addr);
+		goto exit;
+	}
+
+	ieee80211_report_low_ack(sta, 10);
+
+exit:
+	rcu_read_unlock();
 }
 
 /*
@@ -1217,6 +1232,13 @@ static void ath10k_wmi_update_tim(struct ath10k *ar,
 	tim->bitmap_ctrl = !!__le32_to_cpu(bcn_info->tim_info.tim_mcast);
 	memcpy(tim->virtual_map, arvif->u.ap.tim_bitmap, pvm_len);
 
+	if (tim->dtim_count == 0) {
+		ATH10K_SKB_CB(bcn)->bcn.dtim_zero = true;
+
+		if (__le32_to_cpu(bcn_info->tim_info.tim_mcast) == 1)
+			ATH10K_SKB_CB(bcn)->bcn.deliver_cab = true;
+	}
+
 	ath10k_dbg(ATH10K_DBG_MGMT, "dtim %d/%d mcast %d pvmlen %d\n",
 		   tim->dtim_count, tim->dtim_period,
 		   tim->bitmap_ctrl, pvm_len);
@@ -1338,7 +1360,7 @@ static void ath10k_wmi_event_host_swba(struct ath10k *ar, struct sk_buff *skb)
 	struct wmi_bcn_info *bcn_info;
 	struct ath10k_vif *arvif;
 	struct sk_buff *bcn;
-	int vdev_id = 0;
+	int ret, vdev_id = 0;
 
 	ath10k_dbg(ATH10K_DBG_MGMT, "WMI_HOST_SWBA_EVENTID\n");
 
@@ -1385,6 +1407,17 @@ static void ath10k_wmi_event_host_swba(struct ath10k *ar, struct sk_buff *skb)
 			continue;
 		}
 
+		/* There are no completions for beacons so wait for next SWBA
+		 * before telling mac80211 to decrement CSA counter
+		 *
+		 * Once CSA counter is completed stop sending beacons until
+		 * actual channel switch is done */
+		if (arvif->vif->csa_active &&
+		    ieee80211_csa_is_complete(arvif->vif)) {
+			ieee80211_csa_finish(arvif->vif);
+			continue;
+		}
+
 		bcn = ieee80211_beacon_get(ar->hw, arvif->vif);
 		if (!bcn) {
 			ath10k_warn("could not get mac80211 beacon\n");
@@ -1396,15 +1429,33 @@ static void ath10k_wmi_event_host_swba(struct ath10k *ar, struct sk_buff *skb)
 		ath10k_wmi_update_noa(ar, arvif, bcn, bcn_info);
 
 		spin_lock_bh(&ar->data_lock);
+
 		if (arvif->beacon) {
-			ath10k_warn("SWBA overrun on vdev %d\n",
-				    arvif->vdev_id);
+			if (!arvif->beacon_sent)
+				ath10k_warn("SWBA overrun on vdev %d\n",
+					    arvif->vdev_id);
+
+			dma_unmap_single(arvif->ar->dev,
+					 ATH10K_SKB_CB(arvif->beacon)->paddr,
+					 arvif->beacon->len, DMA_TO_DEVICE);
 			dev_kfree_skb_any(arvif->beacon);
 		}
 
+		ATH10K_SKB_CB(bcn)->paddr = dma_map_single(arvif->ar->dev,
+							   bcn->data, bcn->len,
+							   DMA_TO_DEVICE);
+		ret = dma_mapping_error(arvif->ar->dev,
+					ATH10K_SKB_CB(bcn)->paddr);
+		if (ret) {
+			ath10k_warn("failed to map beacon: %d\n", ret);
+			goto skip;
+		}
+
 		arvif->beacon = bcn;
+		arvif->beacon_sent = false;
 
 		ath10k_wmi_tx_beacon_nowait(arvif);
+skip:
 		spin_unlock_bh(&ar->data_lock);
 	}
 }
@@ -2031,11 +2082,11 @@ static int ath10k_wmi_ready_event_rx(struct ath10k *ar, struct sk_buff *skb)
 	memcpy(ar->mac_addr, ev->mac_addr.addr, ETH_ALEN);
 
 	ath10k_dbg(ATH10K_DBG_WMI,
-		   "wmi event ready sw_version %u abi_version %u mac_addr %pM status %d\n",
+		   "wmi event ready sw_version %u abi_version %u mac_addr %pM status %d skb->len %i ev-sz %zu\n",
 		   __le32_to_cpu(ev->sw_version),
 		   __le32_to_cpu(ev->abi_version),
 		   ev->mac_addr.addr,
-		   __le32_to_cpu(ev->status));
+		   __le32_to_cpu(ev->status), skb->len, sizeof(*ev));
 
 	complete(&ar->wmi.unified_ready);
 	return 0;
@@ -2403,7 +2454,7 @@ int ath10k_wmi_pdev_set_channel(struct ath10k *ar,
 				   ar->wmi.cmd->pdev_set_channel_cmdid);
 }
 
-int ath10k_wmi_pdev_suspend_target(struct ath10k *ar)
+int ath10k_wmi_pdev_suspend_target(struct ath10k *ar, u32 suspend_opt)
 {
 	struct wmi_pdev_suspend_cmd *cmd;
 	struct sk_buff *skb;
@@ -2413,7 +2464,7 @@ int ath10k_wmi_pdev_suspend_target(struct ath10k *ar)
 		return -ENOMEM;
 
 	cmd = (struct wmi_pdev_suspend_cmd *)skb->data;
-	cmd->suspend_opt = WMI_PDEV_SUSPEND;
+	cmd->suspend_opt = __cpu_to_le32(suspend_opt);
 
 	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->pdev_suspend_cmdid);
 }
@@ -3342,7 +3393,6 @@ int ath10k_wmi_scan_chan_list(struct ath10k *ar,
 		ci->max_power         = ch->max_power;
 		ci->reg_power         = ch->max_reg_power;
 		ci->antenna_max       = ch->max_antenna_gain;
-		ci->antenna_max       = 0;
 
 		/* mode & flags share storage */
 		ci->mode              = ch->mode;
@@ -3411,25 +3461,41 @@ int ath10k_wmi_peer_assoc(struct ath10k *ar,
 	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->peer_assoc_cmdid);
 }
 
-int ath10k_wmi_beacon_send_nowait(struct ath10k *ar,
-				  const struct wmi_bcn_tx_arg *arg)
+/* This function assumes the beacon is already DMA mapped */
+int ath10k_wmi_beacon_send_ref_nowait(struct ath10k_vif *arvif)
 {
-	struct wmi_bcn_tx_cmd *cmd;
+	struct wmi_bcn_tx_ref_cmd *cmd;
 	struct sk_buff *skb;
+	struct sk_buff *beacon = arvif->beacon;
+	struct ath10k *ar = arvif->ar;
+	struct ieee80211_hdr *hdr;
 	int ret;
+	u16 fc;
 
-	skb = ath10k_wmi_alloc_skb(sizeof(*cmd) + arg->bcn_len);
+	skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
 	if (!skb)
 		return -ENOMEM;
 
-	cmd = (struct wmi_bcn_tx_cmd *)skb->data;
-	cmd->hdr.vdev_id  = __cpu_to_le32(arg->vdev_id);
-	cmd->hdr.tx_rate  = __cpu_to_le32(arg->tx_rate);
-	cmd->hdr.tx_power = __cpu_to_le32(arg->tx_power);
-	cmd->hdr.bcn_len  = __cpu_to_le32(arg->bcn_len);
-	memcpy(cmd->bcn, arg->bcn, arg->bcn_len);
+	hdr = (struct ieee80211_hdr *)beacon->data;
+	fc = le16_to_cpu(hdr->frame_control);
+
+	cmd = (struct wmi_bcn_tx_ref_cmd *)skb->data;
+	cmd->vdev_id = __cpu_to_le32(arvif->vdev_id);
+	cmd->data_len = __cpu_to_le32(beacon->len);
+	cmd->data_ptr = __cpu_to_le32(ATH10K_SKB_CB(beacon)->paddr);
+	cmd->msdu_id = 0;
+	cmd->frame_control = __cpu_to_le32(fc);
+	cmd->flags = 0;
+
+	if (ATH10K_SKB_CB(beacon)->bcn.dtim_zero)
+		cmd->flags |= __cpu_to_le32(WMI_BCN_TX_REF_FLAG_DTIM_ZERO);
+
+	if (ATH10K_SKB_CB(beacon)->bcn.deliver_cab)
+		cmd->flags |= __cpu_to_le32(WMI_BCN_TX_REF_FLAG_DELIVER_CAB);
+
+	ret = ath10k_wmi_cmd_send_nowait(ar, skb,
+					 ar->wmi.cmd->pdev_send_bcn_cmdid);
 
-	ret = ath10k_wmi_cmd_send_nowait(ar, skb, ar->wmi.cmd->bcn_tx_cmdid);
 	if (ret)
 		dev_kfree_skb(skb);
 
diff --git a/drivers/net/wireless/ath/ath10k/wmi.h b/drivers/net/wireless/ath/ath10k/wmi.h
index 4b5e7d3d32b6..4fcc96aa9513 100644
--- a/drivers/net/wireless/ath/ath10k/wmi.h
+++ b/drivers/net/wireless/ath/ath10k/wmi.h
@@ -2277,7 +2277,6 @@ struct wmi_pdev_param_map {
 	u32 bcnflt_stats_update_period;
 	u32 pmf_qos;
 	u32 arp_ac_override;
-	u32 arpdhcp_ac_override;
 	u32 dcs;
 	u32 ani_enable;
 	u32 ani_poll_period;
@@ -3403,6 +3402,24 @@ struct wmi_bcn_tx_arg {
 	const void *bcn;
 };
 
+enum wmi_bcn_tx_ref_flags {
+	WMI_BCN_TX_REF_FLAG_DTIM_ZERO = 0x1,
+	WMI_BCN_TX_REF_FLAG_DELIVER_CAB = 0x2,
+};
+
+struct wmi_bcn_tx_ref_cmd {
+	__le32 vdev_id;
+	__le32 data_len;
+	/* physical address of the frame - dma pointer */
+	__le32 data_ptr;
+	/* id for host to track */
+	__le32 msdu_id;
+	/* frame ctrl to setup PPDU desc */
+	__le32 frame_control;
+	/* to control CABQ traffic: WMI_BCN_TX_REF_FLAG_ */
+	__le32 flags;
+} __packed;
+
 /* Beacon filter */
 #define WMI_BCN_FILTER_ALL   0 /* Filter all beacons */
 #define WMI_BCN_FILTER_NONE  1 /* Pass all beacons */
@@ -3859,6 +3876,12 @@ enum wmi_peer_smps_state {
 	WMI_PEER_SMPS_DYNAMIC = 0x2
 };
 
+enum wmi_peer_chwidth {
+	WMI_PEER_CHWIDTH_20MHZ = 0,
+	WMI_PEER_CHWIDTH_40MHZ = 1,
+	WMI_PEER_CHWIDTH_80MHZ = 2,
+};
+
 enum wmi_peer_param {
 	WMI_PEER_SMPS_STATE = 0x1, /* see %wmi_peer_smps_state */
 	WMI_PEER_AMPDU      = 0x2,
@@ -4039,6 +4062,10 @@ struct wmi_chan_info_event {
 	__le32 cycle_count;
 } __packed;
 
+struct wmi_peer_sta_kickout_event {
+	struct wmi_mac_addr peer_macaddr;
+} __packed;
+
 #define WMI_CHAN_INFO_FLAG_COMPLETE BIT(0)
 
 /* FIXME: empirically extrapolated */
@@ -4172,7 +4199,7 @@ int ath10k_wmi_wait_for_unified_ready(struct ath10k *ar);
 int ath10k_wmi_connect_htc_service(struct ath10k *ar);
 int ath10k_wmi_pdev_set_channel(struct ath10k *ar,
 				const struct wmi_channel_arg *);
-int ath10k_wmi_pdev_suspend_target(struct ath10k *ar);
+int ath10k_wmi_pdev_suspend_target(struct ath10k *ar, u32 suspend_opt);
 int ath10k_wmi_pdev_resume_target(struct ath10k *ar);
 int ath10k_wmi_pdev_set_regdomain(struct ath10k *ar, u16 rd, u16 rd2g,
 				  u16 rd5g, u16 ctl2g, u16 ctl5g);
@@ -4219,8 +4246,7 @@ int ath10k_wmi_set_ap_ps_param(struct ath10k *ar, u32 vdev_id, const u8 *mac,
 			       enum wmi_ap_ps_peer_param param_id, u32 value);
 int ath10k_wmi_scan_chan_list(struct ath10k *ar,
 			      const struct wmi_scan_chan_list_arg *arg);
-int ath10k_wmi_beacon_send_nowait(struct ath10k *ar,
-				  const struct wmi_bcn_tx_arg *arg);
+int ath10k_wmi_beacon_send_ref_nowait(struct ath10k_vif *arvif);
 int ath10k_wmi_pdev_set_wmm_params(struct ath10k *ar,
 			const struct wmi_pdev_set_wmm_params_arg *arg);
 int ath10k_wmi_request_stats(struct ath10k *ar, enum wmi_stats_id stats_id);
diff --git a/drivers/net/wireless/ath/ath5k/mac80211-ops.c b/drivers/net/wireless/ath/ath5k/mac80211-ops.c
index 4ee01f654235..afb23b3cc7be 100644
--- a/drivers/net/wireless/ath/ath5k/mac80211-ops.c
+++ b/drivers/net/wireless/ath/ath5k/mac80211-ops.c
@@ -681,6 +681,7 @@ ath5k_get_survey(struct ieee80211_hw *hw, int idx, struct survey_info *survey)
 	survey->channel = conf->chandef.chan;
 	survey->noise = ah->ah_noise_floor;
 	survey->filled = SURVEY_INFO_NOISE_DBM |
+			SURVEY_INFO_IN_USE |
 			SURVEY_INFO_CHANNEL_TIME |
 			SURVEY_INFO_CHANNEL_TIME_BUSY |
 			SURVEY_INFO_CHANNEL_TIME_RX |
diff --git a/drivers/net/wireless/ath/ath5k/phy.c b/drivers/net/wireless/ath/ath5k/phy.c
index d6bc7cb61bfb..1a2973b7acf2 100644
--- a/drivers/net/wireless/ath/ath5k/phy.c
+++ b/drivers/net/wireless/ath/ath5k/phy.c
@@ -110,7 +110,7 @@ ath5k_hw_radio_revision(struct ath5k_hw *ah, enum ieee80211_band band)
 		ath5k_hw_reg_write(ah, 0x00010000, AR5K_PHY(0x20));
 
 	if (ah->ah_version == AR5K_AR5210) {
-		srev = ath5k_hw_reg_read(ah, AR5K_PHY(256) >> 28) & 0xf;
+		srev = (ath5k_hw_reg_read(ah, AR5K_PHY(256)) >> 28) & 0xf;
 		ret = (u16)ath5k_hw_bitswap(srev, 4) + 1;
 	} else {
 		srev = (ath5k_hw_reg_read(ah, AR5K_PHY(0x100)) >> 24) & 0xff;
diff --git a/drivers/net/wireless/ath/ath6kl/cfg80211.c b/drivers/net/wireless/ath/ath6kl/cfg80211.c
index fd4c89df67e1..c2c6f4604958 100644
--- a/drivers/net/wireless/ath/ath6kl/cfg80211.c
+++ b/drivers/net/wireless/ath/ath6kl/cfg80211.c
@@ -790,7 +790,7 @@ void ath6kl_cfg80211_connect_event(struct ath6kl_vif *vif, u16 channel,
 	if (nw_type & ADHOC_NETWORK) {
 		ath6kl_dbg(ATH6KL_DBG_WLAN_CFG, "ad-hoc %s selected\n",
 			   nw_type & ADHOC_CREATOR ? "creator" : "joiner");
-		cfg80211_ibss_joined(vif->ndev, bssid, GFP_KERNEL);
+		cfg80211_ibss_joined(vif->ndev, bssid, chan, GFP_KERNEL);
 		cfg80211_put_bss(ar->wiphy, bss);
 		return;
 	}
@@ -861,13 +861,9 @@ void ath6kl_cfg80211_disconnect_event(struct ath6kl_vif *vif, u8 reason,
 	}
 
 	if (vif->nw_type & ADHOC_NETWORK) {
-		if (vif->wdev.iftype != NL80211_IFTYPE_ADHOC) {
+		if (vif->wdev.iftype != NL80211_IFTYPE_ADHOC)
 			ath6kl_dbg(ATH6KL_DBG_WLAN_CFG,
 				   "%s: ath6k not in ibss mode\n", __func__);
-			return;
-		}
-		memset(bssid, 0, ETH_ALEN);
-		cfg80211_ibss_joined(vif->ndev, bssid, GFP_KERNEL);
 		return;
 	}
 
@@ -3256,6 +3252,15 @@ static int ath6kl_cfg80211_sscan_start(struct wiphy *wiphy,
 	struct ath6kl_vif *vif = netdev_priv(dev);
 	u16 interval;
 	int ret, rssi_thold;
+	int n_match_sets = request->n_match_sets;
+
+	/*
+	 * If there's a matchset w/o an SSID, then assume it's just for
+	 * the RSSI (nothing else is currently supported) and ignore it.
+	 * The device only supports a global RSSI filter that we set below.
+	 */
+	if (n_match_sets == 1 && !request->match_sets[0].ssid.ssid_len)
+		n_match_sets = 0;
 
 	if (ar->state != ATH6KL_STATE_ON)
 		return -EIO;
@@ -3268,11 +3273,11 @@ static int ath6kl_cfg80211_sscan_start(struct wiphy *wiphy,
 	ret = ath6kl_set_probed_ssids(ar, vif, request->ssids,
 				      request->n_ssids,
 				      request->match_sets,
-				      request->n_match_sets);
+				      n_match_sets);
 	if (ret < 0)
 		return ret;
 
-	if (!request->n_match_sets) {
+	if (!n_match_sets) {
 		ret = ath6kl_wmi_bssfilter_cmd(ar->wmi, vif->fw_vif_idx,
 					       ALL_BSS_FILTER, 0);
 		if (ret < 0)
@@ -3286,12 +3291,12 @@ static int ath6kl_cfg80211_sscan_start(struct wiphy *wiphy,
 
 	if (test_bit(ATH6KL_FW_CAPABILITY_RSSI_SCAN_THOLD,
 		     ar->fw_capabilities)) {
-		if (request->rssi_thold <= NL80211_SCAN_RSSI_THOLD_OFF)
+		if (request->min_rssi_thold <= NL80211_SCAN_RSSI_THOLD_OFF)
 			rssi_thold = 0;
-		else if (request->rssi_thold < -127)
+		else if (request->min_rssi_thold < -127)
 			rssi_thold = -127;
 		else
-			rssi_thold = request->rssi_thold;
+			rssi_thold = request->min_rssi_thold;
 
 		ret = ath6kl_wmi_set_rssi_filter_cmd(ar->wmi, vif->fw_vif_idx,
 						     rssi_thold);
diff --git a/drivers/net/wireless/ath/ath6kl/usb.c b/drivers/net/wireless/ath/ath6kl/usb.c
index f38ff6a6255e..56c3fd5cef65 100644
--- a/drivers/net/wireless/ath/ath6kl/usb.c
+++ b/drivers/net/wireless/ath/ath6kl/usb.c
@@ -24,7 +24,7 @@
 /* constants */
 #define TX_URB_COUNT            32
 #define RX_URB_COUNT            32
-#define ATH6KL_USB_RX_BUFFER_SIZE  1700
+#define ATH6KL_USB_RX_BUFFER_SIZE  4096
 
 /* tx/rx pipes for usb */
 enum ATH6KL_USB_PIPE_ID {
@@ -481,8 +481,8 @@ static void ath6kl_usb_start_recv_pipes(struct ath6kl_usb *ar_usb)
 	 *		ATH6KL_USB_RX_BUFFER_SIZE);
 	 */
 
-	ar_usb->pipes[ATH6KL_USB_PIPE_RX_DATA].urb_cnt_thresh =
-	    ar_usb->pipes[ATH6KL_USB_PIPE_RX_DATA].urb_alloc / 2;
+	ar_usb->pipes[ATH6KL_USB_PIPE_RX_DATA].urb_cnt_thresh = 1;
+
 	ath6kl_usb_post_recv_transfers(&ar_usb->pipes[ATH6KL_USB_PIPE_RX_DATA],
 				       ATH6KL_USB_RX_BUFFER_SIZE);
 }
diff --git a/drivers/net/wireless/ath/ath6kl/wmi.c b/drivers/net/wireless/ath/ath6kl/wmi.c
index 4f16d79c9eb1..8b4ce28e3ce8 100644
--- a/drivers/net/wireless/ath/ath6kl/wmi.c
+++ b/drivers/net/wireless/ath/ath6kl/wmi.c
@@ -914,7 +914,7 @@ ath6kl_get_regpair(u16 regdmn)
 		return NULL;
 
 	for (i = 0; i < ARRAY_SIZE(regDomainPairs); i++) {
-		if (regDomainPairs[i].regDmnEnum == regdmn)
+		if (regDomainPairs[i].reg_domain == regdmn)
 			return &regDomainPairs[i];
 	}
 
@@ -954,7 +954,7 @@ static void ath6kl_wmi_regdomain_event(struct wmi *wmi, u8 *datap, int len)
 		country = ath6kl_regd_find_country_by_rd((u16) reg_code);
 		if (regpair)
 			ath6kl_dbg(ATH6KL_DBG_WMI, "Regpair used: 0x%0x\n",
-				   regpair->regDmnEnum);
+				   regpair->reg_domain);
 		else
 			ath6kl_warn("Regpair not found reg_code 0x%0x\n",
 				    reg_code);
diff --git a/drivers/net/wireless/ath/ath9k/Kconfig b/drivers/net/wireless/ath/ath9k/Kconfig
index 7b96b3e5712d..8fcc029a76a6 100644
--- a/drivers/net/wireless/ath/ath9k/Kconfig
+++ b/drivers/net/wireless/ath/ath9k/Kconfig
@@ -120,18 +120,6 @@ config ATH9K_WOW
 	  This option enables Wake on Wireless LAN support for certain cards.
 	  Currently, AR9462 is supported.
 
-config ATH9K_LEGACY_RATE_CONTROL
-	bool "Atheros ath9k rate control"
-	depends on ATH9K
-	default n
-	---help---
-	  Say Y, if you want to use the ath9k specific rate control
-	  module instead of minstrel_ht. Be warned that there are various
-	  issues with the ath9k RC and minstrel is a more robust algorithm.
-	  Note that even if this option is selected, "ath9k_rate_control"
-	  has to be passed to mac80211 using the module parameter,
-	  ieee80211_default_rc_algo.
-
 config ATH9K_RFKILL
 	bool "Atheros ath9k rfkill support" if EXPERT
 	depends on ATH9K
diff --git a/drivers/net/wireless/ath/ath9k/Makefile b/drivers/net/wireless/ath/ath9k/Makefile
index a40e5c5d7418..b58fe99ef745 100644
--- a/drivers/net/wireless/ath/ath9k/Makefile
+++ b/drivers/net/wireless/ath/ath9k/Makefile
@@ -8,7 +8,6 @@ ath9k-y +=	beacon.o \
 		antenna.o
 
 ath9k-$(CONFIG_ATH9K_BTCOEX_SUPPORT) += mci.o
-ath9k-$(CONFIG_ATH9K_LEGACY_RATE_CONTROL) += rc.o
 ath9k-$(CONFIG_ATH9K_PCI) += pci.o
 ath9k-$(CONFIG_ATH9K_AHB) += ahb.o
 ath9k-$(CONFIG_ATH9K_DFS_DEBUGFS) += dfs_debug.o
@@ -52,7 +51,8 @@ ath9k_hw-$(CONFIG_ATH9K_BTCOEX_SUPPORT) += btcoex.o \
 obj-$(CONFIG_ATH9K_HW) += ath9k_hw.o
 
 obj-$(CONFIG_ATH9K_COMMON) += ath9k_common.o
-ath9k_common-y:=	common.o
+ath9k_common-y:=	common.o \
+			common-init.o
 
 ath9k_htc-y +=	htc_hst.o \
 		hif_usb.o \
diff --git a/drivers/net/wireless/ath/ath9k/ani.c b/drivers/net/wireless/ath/ath9k/ani.c
index d28923b7435b..2ce5079007b6 100644
--- a/drivers/net/wireless/ath/ath9k/ani.c
+++ b/drivers/net/wireless/ath/ath9k/ani.c
@@ -176,16 +176,26 @@ static void ath9k_hw_set_ofdm_nil(struct ath_hw *ah, u8 immunityLevel,
 	if (ah->opmode == NL80211_IFTYPE_STATION &&
 	    BEACON_RSSI(ah) <= ATH9K_ANI_RSSI_THR_HIGH)
 		weak_sig = true;
-
 	/*
-	 * OFDM Weak signal detection is always enabled for AP mode.
+	 * Newer chipsets are better at dealing with high PHY error counts -
+	 * keep weak signal detection enabled when no RSSI threshold is
+	 * available to determine if it is needed (mode != STA)
 	 */
-	if (ah->opmode != NL80211_IFTYPE_AP &&
-	    aniState->ofdmWeakSigDetect != weak_sig) {
-		ath9k_hw_ani_control(ah,
-				     ATH9K_ANI_OFDM_WEAK_SIGNAL_DETECTION,
-				     entry_ofdm->ofdm_weak_signal_on);
-	}
+	else if (AR_SREV_9300_20_OR_LATER(ah) &&
+		 ah->opmode != NL80211_IFTYPE_STATION)
+		weak_sig = true;
+
+	/* Older chipsets are more sensitive to high PHY error counts */
+	else if (!AR_SREV_9300_20_OR_LATER(ah) &&
+		 aniState->ofdmNoiseImmunityLevel >= 8)
+		weak_sig = false;
+
+	if (aniState->ofdmWeakSigDetect != weak_sig)
+		ath9k_hw_ani_control(ah, ATH9K_ANI_OFDM_WEAK_SIGNAL_DETECTION,
+				     weak_sig);
+
+	if (!AR_SREV_9300_20_OR_LATER(ah))
+		return;
 
 	if (aniState->ofdmNoiseImmunityLevel >= ATH9K_ANI_OFDM_DEF_LEVEL) {
 		ah->config.ofdm_trig_high = ATH9K_ANI_OFDM_TRIG_HIGH;
@@ -483,10 +493,17 @@ void ath9k_hw_ani_init(struct ath_hw *ah)
 
 	ath_dbg(common, ANI, "Initialize ANI\n");
 
-	ah->config.ofdm_trig_high = ATH9K_ANI_OFDM_TRIG_HIGH;
-	ah->config.ofdm_trig_low = ATH9K_ANI_OFDM_TRIG_LOW;
-	ah->config.cck_trig_high = ATH9K_ANI_CCK_TRIG_HIGH;
-	ah->config.cck_trig_low = ATH9K_ANI_CCK_TRIG_LOW;
+	if (AR_SREV_9300_20_OR_LATER(ah)) {
+		ah->config.ofdm_trig_high = ATH9K_ANI_OFDM_TRIG_HIGH;
+		ah->config.ofdm_trig_low = ATH9K_ANI_OFDM_TRIG_LOW;
+		ah->config.cck_trig_high = ATH9K_ANI_CCK_TRIG_HIGH;
+		ah->config.cck_trig_low = ATH9K_ANI_CCK_TRIG_LOW;
+	} else {
+		ah->config.ofdm_trig_high = ATH9K_ANI_OFDM_TRIG_HIGH_OLD;
+		ah->config.ofdm_trig_low = ATH9K_ANI_OFDM_TRIG_LOW_OLD;
+		ah->config.cck_trig_high = ATH9K_ANI_CCK_TRIG_HIGH_OLD;
+		ah->config.cck_trig_low = ATH9K_ANI_CCK_TRIG_LOW_OLD;
+	}
 
 	ani->spurImmunityLevel = ATH9K_ANI_SPUR_IMMUNE_LVL;
 	ani->firstepLevel = ATH9K_ANI_FIRSTEP_LVL;
diff --git a/drivers/net/wireless/ath/ath9k/ani.h b/drivers/net/wireless/ath/ath9k/ani.h
index 21e7b83c3f6a..c40965b4c1e2 100644
--- a/drivers/net/wireless/ath/ath9k/ani.h
+++ b/drivers/net/wireless/ath/ath9k/ani.h
@@ -22,12 +22,16 @@
 /* units are errors per second */
 #define ATH9K_ANI_OFDM_TRIG_HIGH           3500
 #define ATH9K_ANI_OFDM_TRIG_HIGH_BELOW_INI 1000
+#define ATH9K_ANI_OFDM_TRIG_HIGH_OLD       500
 
 #define ATH9K_ANI_OFDM_TRIG_LOW           400
 #define ATH9K_ANI_OFDM_TRIG_LOW_ABOVE_INI 900
+#define ATH9K_ANI_OFDM_TRIG_LOW_OLD       200
 
 #define ATH9K_ANI_CCK_TRIG_HIGH           600
+#define ATH9K_ANI_CCK_TRIG_HIGH_OLD       200
 #define ATH9K_ANI_CCK_TRIG_LOW            300
+#define ATH9K_ANI_CCK_TRIG_LOW_OLD        100
 
 #define ATH9K_ANI_SPUR_IMMUNE_LVL         3
 #define ATH9K_ANI_FIRSTEP_LVL             2
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_calib.c b/drivers/net/wireless/ath/ath9k/ar9003_calib.c
index a352128c40ad..ac8301ef5242 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_calib.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_calib.c
@@ -23,10 +23,11 @@
 #define MAX_MEASUREMENT	MAX_IQCAL_MEASUREMENT
 #define MAX_MAG_DELTA	11
 #define MAX_PHS_DELTA	10
+#define MAXIQCAL        3
 
 struct coeff {
-	int mag_coeff[AR9300_MAX_CHAINS][MAX_MEASUREMENT];
-	int phs_coeff[AR9300_MAX_CHAINS][MAX_MEASUREMENT];
+	int mag_coeff[AR9300_MAX_CHAINS][MAX_MEASUREMENT][MAXIQCAL];
+	int phs_coeff[AR9300_MAX_CHAINS][MAX_MEASUREMENT][MAXIQCAL];
 	int iqc_coeff[2];
 };
 
@@ -655,9 +656,6 @@ static bool ar9003_hw_calc_iq_corr(struct ath_hw *ah,
 	if (i2_m_q2_a0_d1 > 0x800)
 		i2_m_q2_a0_d1 = -((0xfff - i2_m_q2_a0_d1) + 1);
 
-	if (i2_p_q2_a0_d1 > 0x1000)
-		i2_p_q2_a0_d1 = -((0x1fff - i2_p_q2_a0_d1) + 1);
-
 	if (iq_corr_a0_d1 > 0x800)
 		iq_corr_a0_d1 = -((0xfff - iq_corr_a0_d1) + 1);
 
@@ -800,7 +798,7 @@ static bool ar9003_hw_calc_iq_corr(struct ath_hw *ah,
 	if (q_q_coff > 63)
 		q_q_coff = 63;
 
-	iqc_coeff[0] = (q_q_coff * 128) + q_i_coff;
+	iqc_coeff[0] = (q_q_coff * 128) + (0x7f & q_i_coff);
 
 	ath_dbg(common, CALIBRATE, "tx chain %d: iq corr coeff=%x\n",
 		chain_idx, iqc_coeff[0]);
@@ -831,7 +829,7 @@ static bool ar9003_hw_calc_iq_corr(struct ath_hw *ah,
 	if (q_q_coff > 63)
 		q_q_coff = 63;
 
-	iqc_coeff[1] = (q_q_coff * 128) + q_i_coff;
+	iqc_coeff[1] = (q_q_coff * 128) + (0x7f & q_i_coff);
 
 	ath_dbg(common, CALIBRATE, "rx chain %d: iq corr coeff=%x\n",
 		chain_idx, iqc_coeff[1]);
@@ -839,7 +837,8 @@ static bool ar9003_hw_calc_iq_corr(struct ath_hw *ah,
 	return true;
 }
 
-static void ar9003_hw_detect_outlier(int *mp_coeff, int nmeasurement,
+static void ar9003_hw_detect_outlier(int mp_coeff[][MAXIQCAL],
+				     int nmeasurement,
 				     int max_delta)
 {
 	int mp_max = -64, max_idx = 0;
@@ -848,20 +847,20 @@ static void ar9003_hw_detect_outlier(int *mp_coeff, int nmeasurement,
 
 	/* find min/max mismatch across all calibrated gains */
 	for (i = 0; i < nmeasurement; i++) {
-		if (mp_coeff[i] > mp_max) {
-			mp_max = mp_coeff[i];
+		if (mp_coeff[i][0] > mp_max) {
+			mp_max = mp_coeff[i][0];
 			max_idx = i;
-		} else if (mp_coeff[i] < mp_min) {
-			mp_min = mp_coeff[i];
+		} else if (mp_coeff[i][0] < mp_min) {
+			mp_min = mp_coeff[i][0];
 			min_idx = i;
 		}
 	}
 
 	/* find average (exclude max abs value) */
 	for (i = 0; i < nmeasurement; i++) {
-		if ((abs(mp_coeff[i]) < abs(mp_max)) ||
-		    (abs(mp_coeff[i]) < abs(mp_min))) {
-			mp_avg += mp_coeff[i];
+		if ((abs(mp_coeff[i][0]) < abs(mp_max)) ||
+		    (abs(mp_coeff[i][0]) < abs(mp_min))) {
+			mp_avg += mp_coeff[i][0];
 			mp_count++;
 		}
 	}
@@ -873,7 +872,7 @@ static void ar9003_hw_detect_outlier(int *mp_coeff, int nmeasurement,
 	if (mp_count)
 		mp_avg /= mp_count;
 	else
-		mp_avg = mp_coeff[nmeasurement - 1];
+		mp_avg = mp_coeff[nmeasurement - 1][0];
 
 	/* detect outlier */
 	if (abs(mp_max - mp_min) > max_delta) {
@@ -882,15 +881,16 @@ static void ar9003_hw_detect_outlier(int *mp_coeff, int nmeasurement,
 		else
 			outlier_idx = min_idx;
 
-		mp_coeff[outlier_idx] = mp_avg;
+		mp_coeff[outlier_idx][0] = mp_avg;
 	}
 }
 
-static void ar9003_hw_tx_iqcal_load_avg_2_passes(struct ath_hw *ah,
-						 struct coeff *coeff,
-						 bool is_reusable)
+static void ar9003_hw_tx_iq_cal_outlier_detection(struct ath_hw *ah,
+						  struct coeff *coeff,
+						  bool is_reusable)
 {
 	int i, im, nmeasurement;
+	int magnitude, phase;
 	u32 tx_corr_coeff[MAX_MEASUREMENT][AR9300_MAX_CHAINS];
 	struct ath9k_hw_cal_data *caldata = ah->caldata;
 
@@ -920,21 +920,30 @@ static void ar9003_hw_tx_iqcal_load_avg_2_passes(struct ath_hw *ah,
 		if (nmeasurement > MAX_MEASUREMENT)
 			nmeasurement = MAX_MEASUREMENT;
 
-		/* detect outlier only if nmeasurement > 1 */
-		if (nmeasurement > 1) {
-			/* Detect magnitude outlier */
-			ar9003_hw_detect_outlier(coeff->mag_coeff[i],
-					nmeasurement, MAX_MAG_DELTA);
-
-			/* Detect phase outlier */
-			ar9003_hw_detect_outlier(coeff->phs_coeff[i],
-					nmeasurement, MAX_PHS_DELTA);
+		/*
+		 * Skip normal outlier detection for AR9550.
+		 */
+		if (!AR_SREV_9550(ah)) {
+			/* detect outlier only if nmeasurement > 1 */
+			if (nmeasurement > 1) {
+				/* Detect magnitude outlier */
+				ar9003_hw_detect_outlier(coeff->mag_coeff[i],
+							 nmeasurement,
+							 MAX_MAG_DELTA);
+
+				/* Detect phase outlier */
+				ar9003_hw_detect_outlier(coeff->phs_coeff[i],
+							 nmeasurement,
+							 MAX_PHS_DELTA);
+			}
 		}
 
 		for (im = 0; im < nmeasurement; im++) {
+			magnitude = coeff->mag_coeff[i][im][0];
+			phase = coeff->phs_coeff[i][im][0];
 
-			coeff->iqc_coeff[0] = (coeff->mag_coeff[i][im] & 0x7f) |
-				((coeff->phs_coeff[i][im] & 0x7f) << 7);
+			coeff->iqc_coeff[0] =
+				(phase & 0x7f) | ((magnitude & 0x7f) << 7);
 
 			if ((im % 2) == 0)
 				REG_RMW_FIELD(ah, tx_corr_coeff[im][i],
@@ -991,7 +1000,63 @@ static bool ar9003_hw_tx_iq_cal_run(struct ath_hw *ah)
 	return true;
 }
 
-static void ar9003_hw_tx_iq_cal_post_proc(struct ath_hw *ah, bool is_reusable)
+static void __ar955x_tx_iq_cal_sort(struct ath_hw *ah,
+				    struct coeff *coeff,
+				    int i, int nmeasurement)
+{
+	struct ath_common *common = ath9k_hw_common(ah);
+	int im, ix, iy, temp;
+
+	for (im = 0; im < nmeasurement; im++) {
+		for (ix = 0; ix < MAXIQCAL - 1; ix++) {
+			for (iy = ix + 1; iy <= MAXIQCAL - 1; iy++) {
+				if (coeff->mag_coeff[i][im][iy] <
+				    coeff->mag_coeff[i][im][ix]) {
+					temp = coeff->mag_coeff[i][im][ix];
+					coeff->mag_coeff[i][im][ix] =
+						coeff->mag_coeff[i][im][iy];
+					coeff->mag_coeff[i][im][iy] = temp;
+				}
+				if (coeff->phs_coeff[i][im][iy] <
+				    coeff->phs_coeff[i][im][ix]) {
+					temp = coeff->phs_coeff[i][im][ix];
+					coeff->phs_coeff[i][im][ix] =
+						coeff->phs_coeff[i][im][iy];
+					coeff->phs_coeff[i][im][iy] = temp;
+				}
+			}
+		}
+		coeff->mag_coeff[i][im][0] = coeff->mag_coeff[i][im][MAXIQCAL / 2];
+		coeff->phs_coeff[i][im][0] = coeff->phs_coeff[i][im][MAXIQCAL / 2];
+
+		ath_dbg(common, CALIBRATE,
+			"IQCAL: Median [ch%d][gain%d]: mag = %d phase = %d\n",
+			i, im,
+			coeff->mag_coeff[i][im][0],
+			coeff->phs_coeff[i][im][0]);
+	}
+}
+
+static bool ar955x_tx_iq_cal_median(struct ath_hw *ah,
+				    struct coeff *coeff,
+				    int iqcal_idx,
+				    int nmeasurement)
+{
+	int i;
+
+	if ((iqcal_idx + 1) != MAXIQCAL)
+		return false;
+
+	for (i = 0; i < AR9300_MAX_CHAINS; i++) {
+		__ar955x_tx_iq_cal_sort(ah, coeff, i, nmeasurement);
+	}
+
+	return true;
+}
+
+static void ar9003_hw_tx_iq_cal_post_proc(struct ath_hw *ah,
+					  int iqcal_idx,
+					  bool is_reusable)
 {
 	struct ath_common *common = ath9k_hw_common(ah);
 	const u32 txiqcal_status[AR9300_MAX_CHAINS] = {
@@ -1004,10 +1069,11 @@ static void ar9003_hw_tx_iq_cal_post_proc(struct ath_hw *ah, bool is_reusable)
 		AR_PHY_CHAN_INFO_TAB_1,
 		AR_PHY_CHAN_INFO_TAB_2,
 	};
-	struct coeff coeff;
+	static struct coeff coeff;
 	s32 iq_res[6];
 	int i, im, j;
-	int nmeasurement;
+	int nmeasurement = 0;
+	bool outlier_detect = true;
 
 	for (i = 0; i < AR9300_MAX_CHAINS; i++) {
 		if (!(ah->txchainmask & (1 << i)))
@@ -1065,17 +1131,23 @@ static void ar9003_hw_tx_iq_cal_post_proc(struct ath_hw *ah, bool is_reusable)
 				goto tx_iqcal_fail;
 			}
 
-			coeff.mag_coeff[i][im] = coeff.iqc_coeff[0] & 0x7f;
-			coeff.phs_coeff[i][im] =
+			coeff.phs_coeff[i][im][iqcal_idx] =
+				coeff.iqc_coeff[0] & 0x7f;
+			coeff.mag_coeff[i][im][iqcal_idx] =
 				(coeff.iqc_coeff[0] >> 7) & 0x7f;
 
-			if (coeff.mag_coeff[i][im] > 63)
-				coeff.mag_coeff[i][im] -= 128;
-			if (coeff.phs_coeff[i][im] > 63)
-				coeff.phs_coeff[i][im] -= 128;
+			if (coeff.mag_coeff[i][im][iqcal_idx] > 63)
+				coeff.mag_coeff[i][im][iqcal_idx] -= 128;
+			if (coeff.phs_coeff[i][im][iqcal_idx] > 63)
+				coeff.phs_coeff[i][im][iqcal_idx] -= 128;
 		}
 	}
-	ar9003_hw_tx_iqcal_load_avg_2_passes(ah, &coeff, is_reusable);
+
+	if (AR_SREV_9550(ah))
+		outlier_detect = ar955x_tx_iq_cal_median(ah, &coeff,
+							 iqcal_idx, nmeasurement);
+	if (outlier_detect)
+		ar9003_hw_tx_iq_cal_outlier_detection(ah, &coeff, is_reusable);
 
 	return;
 
@@ -1409,7 +1481,7 @@ skip_tx_iqcal:
 	}
 
 	if (txiqcal_done)
-		ar9003_hw_tx_iq_cal_post_proc(ah, is_reusable);
+		ar9003_hw_tx_iq_cal_post_proc(ah, 0, is_reusable);
 	else if (caldata && test_bit(TXIQCAL_DONE, &caldata->cal_flags))
 		ar9003_hw_tx_iq_cal_reload(ah);
 
@@ -1455,14 +1527,38 @@ skip_tx_iqcal:
 	return true;
 }
 
+static bool do_ar9003_agc_cal(struct ath_hw *ah)
+{
+	struct ath_common *common = ath9k_hw_common(ah);
+	bool status;
+
+	REG_WRITE(ah, AR_PHY_AGC_CONTROL,
+		  REG_READ(ah, AR_PHY_AGC_CONTROL) |
+		  AR_PHY_AGC_CONTROL_CAL);
+
+	status = ath9k_hw_wait(ah, AR_PHY_AGC_CONTROL,
+			       AR_PHY_AGC_CONTROL_CAL,
+			       0, AH_WAIT_TIMEOUT);
+	if (!status) {
+		ath_dbg(common, CALIBRATE,
+			"offset calibration failed to complete in %d ms,"
+			"noisy environment?\n",
+			AH_WAIT_TIMEOUT / 1000);
+		return false;
+	}
+
+	return true;
+}
+
 static bool ar9003_hw_init_cal_soc(struct ath_hw *ah,
 				   struct ath9k_channel *chan)
 {
 	struct ath_common *common = ath9k_hw_common(ah);
 	struct ath9k_hw_cal_data *caldata = ah->caldata;
 	bool txiqcal_done = false;
-	bool is_reusable = true, status = true;
+	bool status = true;
 	bool run_agc_cal = false, sep_iq_cal = false;
+	int i = 0;
 
 	/* Use chip chainmask only for calibration */
 	ar9003_hw_set_chain_masks(ah, ah->caps.rx_chainmask, ah->caps.tx_chainmask);
@@ -1485,7 +1581,12 @@ static bool ar9003_hw_init_cal_soc(struct ath_hw *ah,
 	 * AGC calibration. Specifically, AR9550 in SoC chips.
 	 */
 	if (ah->enabled_cals & TX_IQ_ON_AGC_CAL) {
-		txiqcal_done = true;
+		if (REG_READ_FIELD(ah, AR_PHY_TX_IQCAL_CONTROL_0,
+				   AR_PHY_TX_IQCAL_CONTROL_0_ENABLE_TXIQ_CAL)) {
+				txiqcal_done = true;
+		} else {
+			txiqcal_done = false;
+		}
 		run_agc_cal = true;
 	} else {
 		sep_iq_cal = true;
@@ -1512,27 +1613,37 @@ skip_tx_iqcal:
 		if (AR_SREV_9330_11(ah))
 			ar9003_hw_manual_peak_cal(ah, 0, IS_CHAN_2GHZ(chan));
 
-		/* Calibrate the AGC */
-		REG_WRITE(ah, AR_PHY_AGC_CONTROL,
-			  REG_READ(ah, AR_PHY_AGC_CONTROL) |
-			  AR_PHY_AGC_CONTROL_CAL);
-
-		/* Poll for offset calibration complete */
-		status = ath9k_hw_wait(ah, AR_PHY_AGC_CONTROL,
-				       AR_PHY_AGC_CONTROL_CAL,
-				       0, AH_WAIT_TIMEOUT);
-	}
+		/*
+		 * For non-AR9550 chips, we just trigger AGC calibration
+		 * in the HW, poll for completion and then process
+		 * the results.
+		 *
+		 * For AR955x, we run it multiple times and use
+		 * median IQ correction.
+		 */
+		if (!AR_SREV_9550(ah)) {
+			status = do_ar9003_agc_cal(ah);
+			if (!status)
+				return false;
 
-	if (!status) {
-		ath_dbg(common, CALIBRATE,
-			"offset calibration failed to complete in %d ms; noisy environment?\n",
-			AH_WAIT_TIMEOUT / 1000);
-		return false;
+			if (txiqcal_done)
+				ar9003_hw_tx_iq_cal_post_proc(ah, 0, false);
+		} else {
+			if (!txiqcal_done) {
+				status = do_ar9003_agc_cal(ah);
+				if (!status)
+					return false;
+			} else {
+				for (i = 0; i < MAXIQCAL; i++) {
+					status = do_ar9003_agc_cal(ah);
+					if (!status)
+						return false;
+					ar9003_hw_tx_iq_cal_post_proc(ah, i, false);
+				}
+			}
+		}
 	}
 
-	if (txiqcal_done)
-		ar9003_hw_tx_iq_cal_post_proc(ah, is_reusable);
-
 	/* Revert chainmask to runtime parameters */
 	ar9003_hw_set_chain_masks(ah, ah->rxchainmask, ah->txchainmask);
 
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c b/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c
index 25243cbc07f0..b8daff78b9d1 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c
@@ -5065,6 +5065,10 @@ static u16 ar9003_hw_get_max_edge_power(struct ar9300_eeprom *eep,
 			break;
 		}
 	}
+
+	if (is2GHz && !twiceMaxEdgePower)
+		twiceMaxEdgePower = 60;
+
 	return twiceMaxEdgePower;
 }
 
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_phy.c b/drivers/net/wireless/ath/ath9k/ar9003_phy.c
index 09facba1dc6d..8927fc34d84c 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_phy.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_phy.c
@@ -868,10 +868,6 @@ static void ar9003_hw_set_rfmode(struct ath_hw *ah,
 
 	if (IS_CHAN_A_FAST_CLOCK(ah, chan))
 		rfMode |= (AR_PHY_MODE_DYNAMIC | AR_PHY_MODE_DYN_CCK_DISABLE);
-	if (IS_CHAN_QUARTER_RATE(chan))
-		rfMode |= AR_PHY_MODE_QUARTER;
-	if (IS_CHAN_HALF_RATE(chan))
-		rfMode |= AR_PHY_MODE_HALF;
 
 	if (rfMode & (AR_PHY_MODE_QUARTER | AR_PHY_MODE_HALF))
 		REG_RMW_FIELD(ah, AR_PHY_FRAME_CTL,
diff --git a/drivers/net/wireless/ath/ath9k/ar9462_2p0_initvals.h b/drivers/net/wireless/ath/ath9k/ar9462_2p0_initvals.h
index 1cc13569b17b..1b6b4d0cfa97 100644
--- a/drivers/net/wireless/ath/ath9k/ar9462_2p0_initvals.h
+++ b/drivers/net/wireless/ath/ath9k/ar9462_2p0_initvals.h
@@ -57,7 +57,7 @@ static const u32 ar9462_2p0_baseband_postamble[][5] = {
 	{0x00009e14, 0x37b95d5e, 0x37b9605e, 0x3236605e, 0x32365a5e},
 	{0x00009e18, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
 	{0x00009e1c, 0x0001cf9c, 0x0001cf9c, 0x00021f9c, 0x00021f9c},
-	{0x00009e20, 0x000003b5, 0x000003b5, 0x000003ce, 0x000003ce},
+	{0x00009e20, 0x000003a5, 0x000003a5, 0x000003a5, 0x000003a5},
 	{0x00009e2c, 0x0000001c, 0x0000001c, 0x00000021, 0x00000021},
 	{0x00009e3c, 0xcf946220, 0xcf946220, 0xcfd5c782, 0xcfd5c282},
 	{0x00009e44, 0x62321e27, 0x62321e27, 0xfe291e27, 0xfe291e27},
@@ -96,7 +96,7 @@ static const u32 ar9462_2p0_baseband_postamble[][5] = {
 	{0x0000ae04, 0x001c0000, 0x001c0000, 0x001c0000, 0x00100000},
 	{0x0000ae18, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
 	{0x0000ae1c, 0x0000019c, 0x0000019c, 0x0000019c, 0x0000019c},
-	{0x0000ae20, 0x000001b5, 0x000001b5, 0x000001ce, 0x000001ce},
+	{0x0000ae20, 0x000001a6, 0x000001a6, 0x000001aa, 0x000001aa},
 	{0x0000b284, 0x00000000, 0x00000000, 0x00000550, 0x00000550},
 };
 
diff --git a/drivers/net/wireless/ath/ath9k/ath9k.h b/drivers/net/wireless/ath/ath9k/ath9k.h
index b5ac32cfbeb8..f995c374a9b4 100644
--- a/drivers/net/wireless/ath/ath9k/ath9k.h
+++ b/drivers/net/wireless/ath/ath9k/ath9k.h
@@ -30,7 +30,6 @@
 #include "spectral.h"
 
 struct ath_node;
-struct ath_rate_table;
 
 extern struct ieee80211_ops ath9k_ops;
 extern int ath9k_modparam_nohwcrypt;
@@ -150,6 +149,11 @@ int ath_descdma_setup(struct ath_softc *sc, struct ath_descdma *dd,
 #define IS_CCK_RATE(rate)  ((rate >= 0x18) && (rate <= 0x1e))
 #define IS_OFDM_RATE(rate) ((rate >= 0x8) && (rate <= 0xf))
 
+enum {
+       WLAN_RC_PHY_OFDM,
+       WLAN_RC_PHY_CCK,
+};
+
 struct ath_txq {
 	int mac80211_qnum; /* mac80211 queue number, -1 means not mac80211 Q */
 	u32 axq_qnum; /* ath9k hardware queue number */
@@ -406,7 +410,6 @@ void ath9k_calculate_iter_data(struct ieee80211_hw *hw,
 
 struct ath_beacon_config {
 	int beacon_interval;
-	u16 listen_interval;
 	u16 dtim_period;
 	u16 bmiss_timeout;
 	u8 dtim_count;
@@ -442,7 +445,8 @@ void ath9k_beacon_config(struct ath_softc *sc, struct ieee80211_vif *vif,
 void ath9k_beacon_assign_slot(struct ath_softc *sc, struct ieee80211_vif *vif);
 void ath9k_beacon_remove_slot(struct ath_softc *sc, struct ieee80211_vif *vif);
 void ath9k_set_beacon(struct ath_softc *sc);
-bool ath9k_csa_is_finished(struct ath_softc *sc);
+bool ath9k_csa_is_finished(struct ath_softc *sc, struct ieee80211_vif *vif);
+void ath9k_csa_update(struct ath_softc *sc);
 
 /*******************/
 /* Link Monitoring */
@@ -748,7 +752,6 @@ struct ath_softc {
 	struct ath_rx rx;
 	struct ath_tx tx;
 	struct ath_beacon beacon;
-	struct ieee80211_supported_band sbands[IEEE80211_NUM_BANDS];
 
 #ifdef CONFIG_MAC80211_LEDS
 	bool led_registered;
@@ -757,7 +760,6 @@ struct ath_softc {
 #endif
 
 	struct ath9k_hw_cal_data caldata;
-	int last_rssi;
 
 #ifdef CONFIG_ATH9K_DEBUGFS
 	struct ath9k_debug debug;
@@ -774,7 +776,6 @@ struct ath_softc {
 #endif
 
 	struct ath_descdma txsdma;
-	struct ieee80211_vif *csa_vif;
 
 	struct ath_ant_comb ant_comb;
 	u8 ant_tx, ant_rx;
diff --git a/drivers/net/wireless/ath/ath9k/beacon.c b/drivers/net/wireless/ath/ath9k/beacon.c
index 2e8bba0eb361..02eb4f10332b 100644
--- a/drivers/net/wireless/ath/ath9k/beacon.c
+++ b/drivers/net/wireless/ath/ath9k/beacon.c
@@ -80,7 +80,7 @@ static void ath9k_beacon_setup(struct ath_softc *sc, struct ieee80211_vif *vif,
 	u8 chainmask = ah->txchainmask;
 	u8 rate = 0;
 
-	sband = &sc->sbands[common->hw->conf.chandef.chan->band];
+	sband = &common->sbands[common->hw->conf.chandef.chan->band];
 	rate = sband->bitrates[rateidx].hw_value;
 	if (vif->bss_conf.use_short_preamble)
 		rate |= sband->bitrates[rateidx].hw_value_short;
@@ -292,11 +292,8 @@ static void ath9k_set_tsfadjust(struct ath_softc *sc, struct ieee80211_vif *vif)
 		(unsigned long long)tsfadjust, avp->av_bslot);
 }
 
-bool ath9k_csa_is_finished(struct ath_softc *sc)
+bool ath9k_csa_is_finished(struct ath_softc *sc, struct ieee80211_vif *vif)
 {
-	struct ieee80211_vif *vif;
-
-	vif = sc->csa_vif;
 	if (!vif || !vif->csa_active)
 		return false;
 
@@ -304,11 +301,23 @@ bool ath9k_csa_is_finished(struct ath_softc *sc)
 		return false;
 
 	ieee80211_csa_finish(vif);
-
-	sc->csa_vif = NULL;
 	return true;
 }
 
+static void ath9k_csa_update_vif(void *data, u8 *mac, struct ieee80211_vif *vif)
+{
+	struct ath_softc *sc = data;
+	ath9k_csa_is_finished(sc, vif);
+}
+
+void ath9k_csa_update(struct ath_softc *sc)
+{
+	ieee80211_iterate_active_interfaces(sc->hw,
+					    IEEE80211_IFACE_ITER_NORMAL,
+					    ath9k_csa_update_vif,
+					    sc);
+}
+
 void ath9k_beacon_tasklet(unsigned long data)
 {
 	struct ath_softc *sc = (struct ath_softc *)data;
@@ -362,13 +371,13 @@ void ath9k_beacon_tasklet(unsigned long data)
 		return;
 	}
 
-	/* EDMA devices check that in the tx completion function. */
-	if (!edma && ath9k_csa_is_finished(sc))
-		return;
-
 	slot = ath9k_beacon_choose_slot(sc);
 	vif = sc->beacon.bslot[slot];
 
+	/* EDMA devices check that in the tx completion function. */
+	if (!edma && ath9k_csa_is_finished(sc, vif))
+		return;
+
 	if (!vif || !vif->bss_conf.enable_beacon)
 		return;
 
@@ -510,7 +519,7 @@ static void ath9k_beacon_config_sta(struct ath_softc *sc,
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_common *common = ath9k_hw_common(ah);
 	struct ath9k_beacon_state bs;
-	int dtim_intval, sleepduration;
+	int dtim_intval;
 	u32 nexttbtt = 0, intval;
 	u64 tsf;
 
@@ -529,7 +538,6 @@ static void ath9k_beacon_config_sta(struct ath_softc *sc,
 	 * last beacon we received (which may be none).
 	 */
 	dtim_intval = intval * conf->dtim_period;
-	sleepduration = conf->listen_interval * intval;
 
 	/*
 	 * Pull nexttbtt forward to reflect the current
@@ -551,16 +559,11 @@ static void ath9k_beacon_config_sta(struct ath_softc *sc,
 	 * need calculate based	on the beacon interval.  Note that we clamp the
 	 * result to at most 15 beacons.
 	 */
-	if (sleepduration > intval) {
-		bs.bs_bmissthreshold = conf->listen_interval *
-			ATH_DEFAULT_BMISS_LIMIT / 2;
-	} else {
-		bs.bs_bmissthreshold = DIV_ROUND_UP(conf->bmiss_timeout, intval);
-		if (bs.bs_bmissthreshold > 15)
-			bs.bs_bmissthreshold = 15;
-		else if (bs.bs_bmissthreshold <= 0)
-			bs.bs_bmissthreshold = 1;
-	}
+	bs.bs_bmissthreshold = DIV_ROUND_UP(conf->bmiss_timeout, intval);
+	if (bs.bs_bmissthreshold > 15)
+		bs.bs_bmissthreshold = 15;
+	else if (bs.bs_bmissthreshold <= 0)
+		bs.bs_bmissthreshold = 1;
 
 	/*
 	 * Calculate sleep duration. The configuration is given in ms.
@@ -572,7 +575,7 @@ static void ath9k_beacon_config_sta(struct ath_softc *sc,
 	 */
 
 	bs.bs_sleepduration = TU_TO_USEC(roundup(IEEE80211_MS_TO_TU(100),
-						 sleepduration));
+						 intval));
 	if (bs.bs_sleepduration > bs.bs_dtimperiod)
 		bs.bs_sleepduration = bs.bs_dtimperiod;
 
@@ -668,7 +671,6 @@ static void ath9k_cache_beacon_config(struct ath_softc *sc,
 
 	cur_conf->beacon_interval = bss_conf->beacon_int;
 	cur_conf->dtim_period = bss_conf->dtim_period;
-	cur_conf->listen_interval = 1;
 	cur_conf->dtim_count = 1;
 	cur_conf->ibss_creator = bss_conf->ibss_creator;
 	cur_conf->bmiss_timeout =
diff --git a/drivers/net/wireless/ath/ath9k/common-init.c b/drivers/net/wireless/ath/ath9k/common-init.c
new file mode 100644
index 000000000000..a006c1499728
--- /dev/null
+++ b/drivers/net/wireless/ath/ath9k/common-init.c
@@ -0,0 +1,244 @@
+/*
+ * Copyright (c) 2008-2011 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+/* We use the hw_value as an index into our private channel structure */
+
+#include "common.h"
+
+#define CHAN2G(_freq, _idx)  { \
+	.band = IEEE80211_BAND_2GHZ, \
+	.center_freq = (_freq), \
+	.hw_value = (_idx), \
+	.max_power = 20, \
+}
+
+#define CHAN5G(_freq, _idx) { \
+	.band = IEEE80211_BAND_5GHZ, \
+	.center_freq = (_freq), \
+	.hw_value = (_idx), \
+	.max_power = 20, \
+}
+
+/* Some 2 GHz radios are actually tunable on 2312-2732
+ * on 5 MHz steps, we support the channels which we know
+ * we have calibration data for all cards though to make
+ * this static */
+static const struct ieee80211_channel ath9k_2ghz_chantable[] = {
+	CHAN2G(2412, 0), /* Channel 1 */
+	CHAN2G(2417, 1), /* Channel 2 */
+	CHAN2G(2422, 2), /* Channel 3 */
+	CHAN2G(2427, 3), /* Channel 4 */
+	CHAN2G(2432, 4), /* Channel 5 */
+	CHAN2G(2437, 5), /* Channel 6 */
+	CHAN2G(2442, 6), /* Channel 7 */
+	CHAN2G(2447, 7), /* Channel 8 */
+	CHAN2G(2452, 8), /* Channel 9 */
+	CHAN2G(2457, 9), /* Channel 10 */
+	CHAN2G(2462, 10), /* Channel 11 */
+	CHAN2G(2467, 11), /* Channel 12 */
+	CHAN2G(2472, 12), /* Channel 13 */
+	CHAN2G(2484, 13), /* Channel 14 */
+};
+
+/* Some 5 GHz radios are actually tunable on XXXX-YYYY
+ * on 5 MHz steps, we support the channels which we know
+ * we have calibration data for all cards though to make
+ * this static */
+static const struct ieee80211_channel ath9k_5ghz_chantable[] = {
+	/* _We_ call this UNII 1 */
+	CHAN5G(5180, 14), /* Channel 36 */
+	CHAN5G(5200, 15), /* Channel 40 */
+	CHAN5G(5220, 16), /* Channel 44 */
+	CHAN5G(5240, 17), /* Channel 48 */
+	/* _We_ call this UNII 2 */
+	CHAN5G(5260, 18), /* Channel 52 */
+	CHAN5G(5280, 19), /* Channel 56 */
+	CHAN5G(5300, 20), /* Channel 60 */
+	CHAN5G(5320, 21), /* Channel 64 */
+	/* _We_ call this "Middle band" */
+	CHAN5G(5500, 22), /* Channel 100 */
+	CHAN5G(5520, 23), /* Channel 104 */
+	CHAN5G(5540, 24), /* Channel 108 */
+	CHAN5G(5560, 25), /* Channel 112 */
+	CHAN5G(5580, 26), /* Channel 116 */
+	CHAN5G(5600, 27), /* Channel 120 */
+	CHAN5G(5620, 28), /* Channel 124 */
+	CHAN5G(5640, 29), /* Channel 128 */
+	CHAN5G(5660, 30), /* Channel 132 */
+	CHAN5G(5680, 31), /* Channel 136 */
+	CHAN5G(5700, 32), /* Channel 140 */
+	/* _We_ call this UNII 3 */
+	CHAN5G(5745, 33), /* Channel 149 */
+	CHAN5G(5765, 34), /* Channel 153 */
+	CHAN5G(5785, 35), /* Channel 157 */
+	CHAN5G(5805, 36), /* Channel 161 */
+	CHAN5G(5825, 37), /* Channel 165 */
+};
+
+/* Atheros hardware rate code addition for short premble */
+#define SHPCHECK(__hw_rate, __flags) \
+	((__flags & IEEE80211_RATE_SHORT_PREAMBLE) ? (__hw_rate | 0x04 ) : 0)
+
+#define RATE(_bitrate, _hw_rate, _flags) {              \
+	.bitrate        = (_bitrate),                   \
+	.flags          = (_flags),                     \
+	.hw_value       = (_hw_rate),                   \
+	.hw_value_short = (SHPCHECK(_hw_rate, _flags))  \
+}
+
+static struct ieee80211_rate ath9k_legacy_rates[] = {
+	RATE(10, 0x1b, 0),
+	RATE(20, 0x1a, IEEE80211_RATE_SHORT_PREAMBLE),
+	RATE(55, 0x19, IEEE80211_RATE_SHORT_PREAMBLE),
+	RATE(110, 0x18, IEEE80211_RATE_SHORT_PREAMBLE),
+	RATE(60, 0x0b, (IEEE80211_RATE_SUPPORTS_5MHZ |
+			IEEE80211_RATE_SUPPORTS_10MHZ)),
+	RATE(90, 0x0f, (IEEE80211_RATE_SUPPORTS_5MHZ |
+			IEEE80211_RATE_SUPPORTS_10MHZ)),
+	RATE(120, 0x0a, (IEEE80211_RATE_SUPPORTS_5MHZ |
+			 IEEE80211_RATE_SUPPORTS_10MHZ)),
+	RATE(180, 0x0e, (IEEE80211_RATE_SUPPORTS_5MHZ |
+			 IEEE80211_RATE_SUPPORTS_10MHZ)),
+	RATE(240, 0x09, (IEEE80211_RATE_SUPPORTS_5MHZ |
+			 IEEE80211_RATE_SUPPORTS_10MHZ)),
+	RATE(360, 0x0d, (IEEE80211_RATE_SUPPORTS_5MHZ |
+			 IEEE80211_RATE_SUPPORTS_10MHZ)),
+	RATE(480, 0x08, (IEEE80211_RATE_SUPPORTS_5MHZ |
+			 IEEE80211_RATE_SUPPORTS_10MHZ)),
+	RATE(540, 0x0c, (IEEE80211_RATE_SUPPORTS_5MHZ |
+			 IEEE80211_RATE_SUPPORTS_10MHZ)),
+};
+
+int ath9k_cmn_init_channels_rates(struct ath_common *common)
+{
+	struct ath_hw *ah = (struct ath_hw *)common->ah;
+	void *channels;
+
+	BUILD_BUG_ON(ARRAY_SIZE(ath9k_2ghz_chantable) +
+		     ARRAY_SIZE(ath9k_5ghz_chantable) !=
+		     ATH9K_NUM_CHANNELS);
+
+	if (ah->caps.hw_caps & ATH9K_HW_CAP_2GHZ) {
+		channels = devm_kzalloc(ah->dev,
+			sizeof(ath9k_2ghz_chantable), GFP_KERNEL);
+		if (!channels)
+		    return -ENOMEM;
+
+		memcpy(channels, ath9k_2ghz_chantable,
+		       sizeof(ath9k_2ghz_chantable));
+		common->sbands[IEEE80211_BAND_2GHZ].channels = channels;
+		common->sbands[IEEE80211_BAND_2GHZ].band = IEEE80211_BAND_2GHZ;
+		common->sbands[IEEE80211_BAND_2GHZ].n_channels =
+			ARRAY_SIZE(ath9k_2ghz_chantable);
+		common->sbands[IEEE80211_BAND_2GHZ].bitrates = ath9k_legacy_rates;
+		common->sbands[IEEE80211_BAND_2GHZ].n_bitrates =
+			ARRAY_SIZE(ath9k_legacy_rates);
+	}
+
+	if (ah->caps.hw_caps & ATH9K_HW_CAP_5GHZ) {
+		channels = devm_kzalloc(ah->dev,
+			sizeof(ath9k_5ghz_chantable), GFP_KERNEL);
+		if (!channels)
+			return -ENOMEM;
+
+		memcpy(channels, ath9k_5ghz_chantable,
+		       sizeof(ath9k_5ghz_chantable));
+		common->sbands[IEEE80211_BAND_5GHZ].channels = channels;
+		common->sbands[IEEE80211_BAND_5GHZ].band = IEEE80211_BAND_5GHZ;
+		common->sbands[IEEE80211_BAND_5GHZ].n_channels =
+			ARRAY_SIZE(ath9k_5ghz_chantable);
+		common->sbands[IEEE80211_BAND_5GHZ].bitrates =
+			ath9k_legacy_rates + 4;
+		common->sbands[IEEE80211_BAND_5GHZ].n_bitrates =
+			ARRAY_SIZE(ath9k_legacy_rates) - 4;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(ath9k_cmn_init_channels_rates);
+
+void ath9k_cmn_setup_ht_cap(struct ath_hw *ah,
+			    struct ieee80211_sta_ht_cap *ht_info)
+{
+	struct ath_common *common = ath9k_hw_common(ah);
+	u8 tx_streams, rx_streams;
+	int i, max_streams;
+
+	ht_info->ht_supported = true;
+	ht_info->cap = IEEE80211_HT_CAP_SUP_WIDTH_20_40 |
+		       IEEE80211_HT_CAP_SM_PS |
+		       IEEE80211_HT_CAP_SGI_40 |
+		       IEEE80211_HT_CAP_DSSSCCK40;
+
+	if (ah->caps.hw_caps & ATH9K_HW_CAP_LDPC)
+		ht_info->cap |= IEEE80211_HT_CAP_LDPC_CODING;
+
+	if (ah->caps.hw_caps & ATH9K_HW_CAP_SGI_20)
+		ht_info->cap |= IEEE80211_HT_CAP_SGI_20;
+
+	ht_info->ampdu_factor = IEEE80211_HT_MAX_AMPDU_64K;
+	ht_info->ampdu_density = IEEE80211_HT_MPDU_DENSITY_8;
+
+	if (AR_SREV_9271(ah) || AR_SREV_9330(ah) || AR_SREV_9485(ah) || AR_SREV_9565(ah))
+		max_streams = 1;
+	else if (AR_SREV_9462(ah))
+		max_streams = 2;
+	else if (AR_SREV_9300_20_OR_LATER(ah))
+		max_streams = 3;
+	else
+		max_streams = 2;
+
+	if (AR_SREV_9280_20_OR_LATER(ah)) {
+		if (max_streams >= 2)
+			ht_info->cap |= IEEE80211_HT_CAP_TX_STBC;
+		ht_info->cap |= (1 << IEEE80211_HT_CAP_RX_STBC_SHIFT);
+	}
+
+	/* set up supported mcs set */
+	memset(&ht_info->mcs, 0, sizeof(ht_info->mcs));
+	tx_streams = ath9k_cmn_count_streams(ah->txchainmask, max_streams);
+	rx_streams = ath9k_cmn_count_streams(ah->rxchainmask, max_streams);
+
+	ath_dbg(common, CONFIG, "TX streams %d, RX streams: %d\n",
+		tx_streams, rx_streams);
+
+	if (tx_streams != rx_streams) {
+		ht_info->mcs.tx_params |= IEEE80211_HT_MCS_TX_RX_DIFF;
+		ht_info->mcs.tx_params |= ((tx_streams - 1) <<
+				IEEE80211_HT_MCS_TX_MAX_STREAMS_SHIFT);
+	}
+
+	for (i = 0; i < rx_streams; i++)
+		ht_info->mcs.rx_mask[i] = 0xff;
+
+	ht_info->mcs.tx_params |= IEEE80211_HT_MCS_TX_DEFINED;
+}
+EXPORT_SYMBOL(ath9k_cmn_setup_ht_cap);
+
+void ath9k_cmn_reload_chainmask(struct ath_hw *ah)
+{
+	struct ath_common *common = ath9k_hw_common(ah);
+
+	if (!(ah->caps.hw_caps & ATH9K_HW_CAP_HT))
+		return;
+
+	if (ah->caps.hw_caps & ATH9K_HW_CAP_2GHZ)
+		ath9k_cmn_setup_ht_cap(ah,
+			&common->sbands[IEEE80211_BAND_2GHZ].ht_cap);
+	if (ah->caps.hw_caps & ATH9K_HW_CAP_5GHZ)
+		ath9k_cmn_setup_ht_cap(ah,
+			&common->sbands[IEEE80211_BAND_5GHZ].ht_cap);
+}
+EXPORT_SYMBOL(ath9k_cmn_reload_chainmask);
diff --git a/drivers/net/wireless/ath/ath9k/common-init.h b/drivers/net/wireless/ath/ath9k/common-init.h
new file mode 100644
index 000000000000..ac03fca5ffdd
--- /dev/null
+++ b/drivers/net/wireless/ath/ath9k/common-init.h
@@ -0,0 +1,20 @@
+/*
+ * Copyright (c) 2009-2011 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+int ath9k_cmn_init_channels_rates(struct ath_common *common);
+void ath9k_cmn_setup_ht_cap(struct ath_hw *ah,
+			    struct ieee80211_sta_ht_cap *ht_info);
+void ath9k_cmn_reload_chainmask(struct ath_hw *ah);
diff --git a/drivers/net/wireless/ath/ath9k/common.c b/drivers/net/wireless/ath/ath9k/common.c
index 768c733cad31..c6dd7f1fed65 100644
--- a/drivers/net/wireless/ath/ath9k/common.c
+++ b/drivers/net/wireless/ath/ath9k/common.c
@@ -27,6 +27,250 @@ MODULE_AUTHOR("Atheros Communications");
 MODULE_DESCRIPTION("Shared library for Atheros wireless 802.11n LAN cards.");
 MODULE_LICENSE("Dual BSD/GPL");
 
+/* Assumes you've already done the endian to CPU conversion */
+bool ath9k_cmn_rx_accept(struct ath_common *common,
+			 struct ieee80211_hdr *hdr,
+			 struct ieee80211_rx_status *rxs,
+			 struct ath_rx_status *rx_stats,
+			 bool *decrypt_error,
+			 unsigned int rxfilter)
+{
+	struct ath_hw *ah = common->ah;
+	bool is_mc, is_valid_tkip, strip_mic, mic_error;
+	__le16 fc;
+
+	fc = hdr->frame_control;
+
+	is_mc = !!is_multicast_ether_addr(hdr->addr1);
+	is_valid_tkip = rx_stats->rs_keyix != ATH9K_RXKEYIX_INVALID &&
+		test_bit(rx_stats->rs_keyix, common->tkip_keymap);
+	strip_mic = is_valid_tkip && ieee80211_is_data(fc) &&
+		ieee80211_has_protected(fc) &&
+		!(rx_stats->rs_status &
+		(ATH9K_RXERR_DECRYPT | ATH9K_RXERR_CRC | ATH9K_RXERR_MIC |
+		 ATH9K_RXERR_KEYMISS));
+
+	/*
+	 * Key miss events are only relevant for pairwise keys where the
+	 * descriptor does contain a valid key index. This has been observed
+	 * mostly with CCMP encryption.
+	 */
+	if (rx_stats->rs_keyix == ATH9K_RXKEYIX_INVALID ||
+	    !test_bit(rx_stats->rs_keyix, common->ccmp_keymap))
+		rx_stats->rs_status &= ~ATH9K_RXERR_KEYMISS;
+
+	mic_error = is_valid_tkip && !ieee80211_is_ctl(fc) &&
+		!ieee80211_has_morefrags(fc) &&
+		!(le16_to_cpu(hdr->seq_ctrl) & IEEE80211_SCTL_FRAG) &&
+		(rx_stats->rs_status & ATH9K_RXERR_MIC);
+
+	/*
+	 * The rx_stats->rs_status will not be set until the end of the
+	 * chained descriptors so it can be ignored if rs_more is set. The
+	 * rs_more will be false at the last element of the chained
+	 * descriptors.
+	 */
+	if (rx_stats->rs_status != 0) {
+		u8 status_mask;
+
+		if (rx_stats->rs_status & ATH9K_RXERR_CRC) {
+			rxs->flag |= RX_FLAG_FAILED_FCS_CRC;
+			mic_error = false;
+		}
+
+		if ((rx_stats->rs_status & ATH9K_RXERR_DECRYPT) ||
+		    (!is_mc && (rx_stats->rs_status & ATH9K_RXERR_KEYMISS))) {
+			*decrypt_error = true;
+			mic_error = false;
+		}
+
+
+		/*
+		 * Reject error frames with the exception of
+		 * decryption and MIC failures. For monitor mode,
+		 * we also ignore the CRC error.
+		 */
+		status_mask = ATH9K_RXERR_DECRYPT | ATH9K_RXERR_MIC |
+			      ATH9K_RXERR_KEYMISS;
+
+		if (ah->is_monitoring && (rxfilter & FIF_FCSFAIL))
+			status_mask |= ATH9K_RXERR_CRC;
+
+		if (rx_stats->rs_status & ~status_mask)
+			return false;
+	}
+
+	/*
+	 * For unicast frames the MIC error bit can have false positives,
+	 * so all MIC error reports need to be validated in software.
+	 * False negatives are not common, so skip software verification
+	 * if the hardware considers the MIC valid.
+	 */
+	if (strip_mic)
+		rxs->flag |= RX_FLAG_MMIC_STRIPPED;
+	else if (is_mc && mic_error)
+		rxs->flag |= RX_FLAG_MMIC_ERROR;
+
+	return true;
+}
+EXPORT_SYMBOL(ath9k_cmn_rx_accept);
+
+void ath9k_cmn_rx_skb_postprocess(struct ath_common *common,
+				  struct sk_buff *skb,
+				  struct ath_rx_status *rx_stats,
+				  struct ieee80211_rx_status *rxs,
+				  bool decrypt_error)
+{
+	struct ath_hw *ah = common->ah;
+	struct ieee80211_hdr *hdr;
+	int hdrlen, padpos, padsize;
+	u8 keyix;
+	__le16 fc;
+
+	/* see if any padding is done by the hw and remove it */
+	hdr = (struct ieee80211_hdr *) skb->data;
+	hdrlen = ieee80211_get_hdrlen_from_skb(skb);
+	fc = hdr->frame_control;
+	padpos = ieee80211_hdrlen(fc);
+
+	/* The MAC header is padded to have 32-bit boundary if the
+	 * packet payload is non-zero. The general calculation for
+	 * padsize would take into account odd header lengths:
+	 * padsize = (4 - padpos % 4) % 4; However, since only
+	 * even-length headers are used, padding can only be 0 or 2
+	 * bytes and we can optimize this a bit. In addition, we must
+	 * not try to remove padding from short control frames that do
+	 * not have payload. */
+	padsize = padpos & 3;
+	if (padsize && skb->len>=padpos+padsize+FCS_LEN) {
+		memmove(skb->data + padsize, skb->data, padpos);
+		skb_pull(skb, padsize);
+	}
+
+	keyix = rx_stats->rs_keyix;
+
+	if (!(keyix == ATH9K_RXKEYIX_INVALID) && !decrypt_error &&
+	    ieee80211_has_protected(fc)) {
+		rxs->flag |= RX_FLAG_DECRYPTED;
+	} else if (ieee80211_has_protected(fc)
+		   && !decrypt_error && skb->len >= hdrlen + 4) {
+		keyix = skb->data[hdrlen + 3] >> 6;
+
+		if (test_bit(keyix, common->keymap))
+			rxs->flag |= RX_FLAG_DECRYPTED;
+	}
+	if (ah->sw_mgmt_crypto &&
+	    (rxs->flag & RX_FLAG_DECRYPTED) &&
+	    ieee80211_is_mgmt(fc))
+		/* Use software decrypt for management frames. */
+		rxs->flag &= ~RX_FLAG_DECRYPTED;
+}
+EXPORT_SYMBOL(ath9k_cmn_rx_skb_postprocess);
+
+int ath9k_cmn_process_rate(struct ath_common *common,
+			   struct ieee80211_hw *hw,
+			   struct ath_rx_status *rx_stats,
+			   struct ieee80211_rx_status *rxs)
+{
+	struct ieee80211_supported_band *sband;
+	enum ieee80211_band band;
+	unsigned int i = 0;
+	struct ath_hw *ah = common->ah;
+
+	band = ah->curchan->chan->band;
+	sband = hw->wiphy->bands[band];
+
+	if (IS_CHAN_QUARTER_RATE(ah->curchan))
+		rxs->flag |= RX_FLAG_5MHZ;
+	else if (IS_CHAN_HALF_RATE(ah->curchan))
+		rxs->flag |= RX_FLAG_10MHZ;
+
+	if (rx_stats->rs_rate & 0x80) {
+		/* HT rate */
+		rxs->flag |= RX_FLAG_HT;
+		rxs->flag |= rx_stats->flag;
+		rxs->rate_idx = rx_stats->rs_rate & 0x7f;
+		return 0;
+	}
+
+	for (i = 0; i < sband->n_bitrates; i++) {
+		if (sband->bitrates[i].hw_value == rx_stats->rs_rate) {
+			rxs->rate_idx = i;
+			return 0;
+		}
+		if (sband->bitrates[i].hw_value_short == rx_stats->rs_rate) {
+			rxs->flag |= RX_FLAG_SHORTPRE;
+			rxs->rate_idx = i;
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+EXPORT_SYMBOL(ath9k_cmn_process_rate);
+
+void ath9k_cmn_process_rssi(struct ath_common *common,
+			    struct ieee80211_hw *hw,
+			    struct ath_rx_status *rx_stats,
+			    struct ieee80211_rx_status *rxs)
+{
+	struct ath_hw *ah = common->ah;
+	int last_rssi;
+	int rssi = rx_stats->rs_rssi;
+	int i, j;
+
+	/*
+	 * RSSI is not available for subframes in an A-MPDU.
+	 */
+	if (rx_stats->rs_moreaggr) {
+		rxs->flag |= RX_FLAG_NO_SIGNAL_VAL;
+		return;
+	}
+
+	/*
+	 * Check if the RSSI for the last subframe in an A-MPDU
+	 * or an unaggregated frame is valid.
+	 */
+	if (rx_stats->rs_rssi == ATH9K_RSSI_BAD) {
+		rxs->flag |= RX_FLAG_NO_SIGNAL_VAL;
+		return;
+	}
+
+	for (i = 0, j = 0; i < ARRAY_SIZE(rx_stats->rs_rssi_ctl); i++) {
+		s8 rssi;
+
+		if (!(ah->rxchainmask & BIT(i)))
+			continue;
+
+		rssi = rx_stats->rs_rssi_ctl[i];
+		if (rssi != ATH9K_RSSI_BAD) {
+		    rxs->chains |= BIT(j);
+		    rxs->chain_signal[j] = ah->noise + rssi;
+		}
+		j++;
+	}
+
+	/*
+	 * Update Beacon RSSI, this is used by ANI.
+	 */
+	if (rx_stats->is_mybeacon &&
+	    ((ah->opmode == NL80211_IFTYPE_STATION) ||
+	     (ah->opmode == NL80211_IFTYPE_ADHOC))) {
+		ATH_RSSI_LPF(common->last_rssi, rx_stats->rs_rssi);
+		last_rssi = common->last_rssi;
+
+		if (likely(last_rssi != ATH_RSSI_DUMMY_MARKER))
+			rssi = ATH_EP_RND(last_rssi, ATH_RSSI_EP_MULTIPLIER);
+		if (rssi < 0)
+			rssi = 0;
+
+		ah->stats.avgbrssi = rssi;
+	}
+
+	rxs->signal = ah->noise + rx_stats->rs_rssi;
+}
+EXPORT_SYMBOL(ath9k_cmn_process_rssi);
+
 int ath9k_cmn_get_hw_crypto_keytype(struct sk_buff *skb)
 {
 	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
diff --git a/drivers/net/wireless/ath/ath9k/common.h b/drivers/net/wireless/ath/ath9k/common.h
index eb85e1bdca88..4c449e35bd65 100644
--- a/drivers/net/wireless/ath/ath9k/common.h
+++ b/drivers/net/wireless/ath/ath9k/common.h
@@ -21,6 +21,8 @@
 #include "hw.h"
 #include "hw-ops.h"
 
+#include "common-init.h"
+
 /* Common header for Atheros 802.11n base driver cores */
 
 #define WME_BA_BMP_SIZE         64
@@ -42,6 +44,25 @@
 #define ATH_EP_RND(x, mul) 						\
 	(((x) + ((mul)/2)) / (mul))
 
+bool ath9k_cmn_rx_accept(struct ath_common *common,
+			 struct ieee80211_hdr *hdr,
+			 struct ieee80211_rx_status *rxs,
+			 struct ath_rx_status *rx_stats,
+			 bool *decrypt_error,
+			 unsigned int rxfilter);
+void ath9k_cmn_rx_skb_postprocess(struct ath_common *common,
+				  struct sk_buff *skb,
+				  struct ath_rx_status *rx_stats,
+				  struct ieee80211_rx_status *rxs,
+				  bool decrypt_error);
+int ath9k_cmn_process_rate(struct ath_common *common,
+			   struct ieee80211_hw *hw,
+			   struct ath_rx_status *rx_stats,
+			   struct ieee80211_rx_status *rxs);
+void ath9k_cmn_process_rssi(struct ath_common *common,
+			    struct ieee80211_hw *hw,
+			    struct ath_rx_status *rx_stats,
+			    struct ieee80211_rx_status *rxs);
 int ath9k_cmn_get_hw_crypto_keytype(struct sk_buff *skb);
 struct ath9k_channel *ath9k_cmn_get_channel(struct ieee80211_hw *hw,
 					    struct ath_hw *ah,
diff --git a/drivers/net/wireless/ath/ath9k/debug.c b/drivers/net/wireless/ath/ath9k/debug.c
index ab7264c1d8f7..f8924efdad55 100644
--- a/drivers/net/wireless/ath/ath9k/debug.c
+++ b/drivers/net/wireless/ath/ath9k/debug.c
@@ -135,7 +135,8 @@ static ssize_t read_file_ani(struct file *file, char __user *user_buf,
 	struct ath_softc *sc = file->private_data;
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_hw *ah = sc->sc_ah;
-	unsigned int len = 0, size = 1024;
+	unsigned int len = 0;
+	const unsigned int size = 1024;
 	ssize_t retval = 0;
 	char *buf;
 
@@ -307,13 +308,13 @@ static ssize_t read_file_antenna_diversity(struct file *file,
 	struct ath_antenna_stats *as_main = &sc->debug.stats.ant_stats[ANT_MAIN];
 	struct ath_antenna_stats *as_alt = &sc->debug.stats.ant_stats[ANT_ALT];
 	struct ath_hw_antcomb_conf div_ant_conf;
-	unsigned int len = 0, size = 1024;
+	unsigned int len = 0;
+	const unsigned int size = 1024;
 	ssize_t retval = 0;
 	char *buf;
-	char *lna_conf_str[4] = {"LNA1_MINUS_LNA2",
-				 "LNA2",
-				 "LNA1",
-				 "LNA1_PLUS_LNA2"};
+	static const char *lna_conf_str[4] = {
+		"LNA1_MINUS_LNA2", "LNA2", "LNA1", "LNA1_PLUS_LNA2"
+	};
 
 	buf = kzalloc(size, GFP_KERNEL);
 	if (buf == NULL)
@@ -716,10 +717,13 @@ static ssize_t read_file_queues(struct file *file, char __user *user_buf,
 	struct ath_softc *sc = file->private_data;
 	struct ath_txq *txq;
 	char *buf;
-	unsigned int len = 0, size = 1024;
+	unsigned int len = 0;
+	const unsigned int size = 1024;
 	ssize_t retval = 0;
 	int i;
-	char *qname[4] = {"VO", "VI", "BE", "BK"};
+	static const char *qname[4] = {
+		"VO", "VI", "BE", "BK"
+	};
 
 	buf = kzalloc(size, GFP_KERNEL);
 	if (buf == NULL)
@@ -866,6 +870,12 @@ static ssize_t read_file_reset(struct file *file, char __user *user_buf,
 			 "%17s: %2d\n", "PLL RX Hang",
 			 sc->debug.stats.reset[RESET_TYPE_PLL_HANG]);
 	len += scnprintf(buf + len, sizeof(buf) - len,
+			 "%17s: %2d\n", "MAC Hang",
+			 sc->debug.stats.reset[RESET_TYPE_MAC_HANG]);
+	len += scnprintf(buf + len, sizeof(buf) - len,
+			 "%17s: %2d\n", "Stuck Beacon",
+			 sc->debug.stats.reset[RESET_TYPE_BEACON_STUCK]);
+	len += scnprintf(buf + len, sizeof(buf) - len,
 			 "%17s: %2d\n", "MCI Reset",
 			 sc->debug.stats.reset[RESET_TYPE_MCI]);
 
diff --git a/drivers/net/wireless/ath/ath9k/debug.h b/drivers/net/wireless/ath/ath9k/debug.h
index cc7a025d833e..559a68c2709c 100644
--- a/drivers/net/wireless/ath/ath9k/debug.h
+++ b/drivers/net/wireless/ath/ath9k/debug.h
@@ -18,7 +18,6 @@
 #define DEBUG_H
 
 #include "hw.h"
-#include "rc.h"
 #include "dfs_debug.h"
 
 struct ath_txq;
diff --git a/drivers/net/wireless/ath/ath9k/dfs_debug.h b/drivers/net/wireless/ath/ath9k/dfs_debug.h
index 0a7ddf4c88c9..7936c9126a20 100644
--- a/drivers/net/wireless/ath/ath9k/dfs_debug.h
+++ b/drivers/net/wireless/ath/ath9k/dfs_debug.h
@@ -21,6 +21,8 @@
 
 #include "hw.h"
 
+struct ath_softc;
+
 /**
  * struct ath_dfs_stats - DFS Statistics per wiphy
  * @pulses_total:     pulses reported by HW
diff --git a/drivers/net/wireless/ath/ath9k/hif_usb.c b/drivers/net/wireless/ath/ath9k/hif_usb.c
index 6d5d716adc1b..8e7153b186ed 100644
--- a/drivers/net/wireless/ath/ath9k/hif_usb.c
+++ b/drivers/net/wireless/ath/ath9k/hif_usb.c
@@ -54,6 +54,8 @@ static struct usb_device_id ath9k_hif_usb_ids[] = {
 	  .driver_info = AR9280_USB },  /* SMC Networks */
 	{ USB_DEVICE(0x0411, 0x017f),
 	  .driver_info = AR9280_USB },  /* Sony UWA-BR100 */
+	{ USB_DEVICE(0x0411, 0x0197),
+	  .driver_info = AR9280_USB },  /* Buffalo WLI-UV-AG300P */
 	{ USB_DEVICE(0x04da, 0x3904),
 	  .driver_info = AR9280_USB },
 
diff --git a/drivers/net/wireless/ath/ath9k/htc.h b/drivers/net/wireless/ath/ath9k/htc.h
index 58da3468d1f0..3baf9ceae601 100644
--- a/drivers/net/wireless/ath/ath9k/htc.h
+++ b/drivers/net/wireless/ath/ath9k/htc.h
@@ -262,6 +262,8 @@ enum tid_aggr_state {
 struct ath9k_htc_sta {
 	u8 index;
 	enum tid_aggr_state tid_state[ATH9K_HTC_MAX_TID];
+	struct work_struct rc_update_work;
+	struct ath9k_htc_priv *htc_priv;
 };
 
 #define ATH9K_HTC_RXBUF 256
@@ -275,7 +277,6 @@ struct ath9k_htc_rxbuf {
 };
 
 struct ath9k_htc_rx {
-	int last_rssi; /* FIXME: per-STA */
 	struct list_head rxbuf;
 	spinlock_t rxbuflock;
 };
@@ -486,7 +487,6 @@ struct ath9k_htc_priv {
 	unsigned long op_flags;
 
 	struct ath9k_hw_cal_data caldata;
-	struct ieee80211_supported_band sbands[IEEE80211_NUM_BANDS];
 
 	spinlock_t beacon_lock;
 	struct htc_beacon_config cur_beacon_conf;
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_beacon.c b/drivers/net/wireless/ath/ath9k/htc_drv_beacon.c
index 8b5757734596..a00ddb9e737e 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_beacon.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_beacon.c
@@ -69,7 +69,7 @@ static void ath9k_htc_beacon_config_sta(struct ath9k_htc_priv *priv,
 	struct ath_common *common = ath9k_hw_common(priv->ah);
 	struct ath9k_beacon_state bs;
 	enum ath9k_int imask = 0;
-	int dtimperiod, dtimcount, sleepduration;
+	int dtimperiod, dtimcount;
 	int bmiss_timeout;
 	u32 nexttbtt = 0, intval, tsftu;
 	__be32 htc_imask = 0;
@@ -94,10 +94,6 @@ static void ath9k_htc_beacon_config_sta(struct ath9k_htc_priv *priv,
 	if (dtimcount >= dtimperiod)	/* NB: sanity check */
 		dtimcount = 0;
 
-	sleepduration = intval;
-	if (sleepduration <= 0)
-		sleepduration = intval;
-
 	/*
 	 * Pull nexttbtt forward to reflect the current
 	 * TSF and calculate dtim state for the result.
@@ -128,15 +124,11 @@ static void ath9k_htc_beacon_config_sta(struct ath9k_htc_priv *priv,
 	 * need calculate based	on the beacon interval.  Note that we clamp the
 	 * result to at most 15 beacons.
 	 */
-	if (sleepduration > intval) {
-		bs.bs_bmissthreshold = ATH_DEFAULT_BMISS_LIMIT / 2;
-	} else {
-		bs.bs_bmissthreshold = DIV_ROUND_UP(bmiss_timeout, intval);
-		if (bs.bs_bmissthreshold > 15)
-			bs.bs_bmissthreshold = 15;
-		else if (bs.bs_bmissthreshold <= 0)
-			bs.bs_bmissthreshold = 1;
-	}
+	bs.bs_bmissthreshold = DIV_ROUND_UP(bmiss_timeout, intval);
+	if (bs.bs_bmissthreshold > 15)
+		bs.bs_bmissthreshold = 15;
+	else if (bs.bs_bmissthreshold <= 0)
+		bs.bs_bmissthreshold = 1;
 
 	/*
 	 * Calculate sleep duration. The configuration is given in ms.
@@ -148,7 +140,7 @@ static void ath9k_htc_beacon_config_sta(struct ath9k_htc_priv *priv,
 	 */
 
 	bs.bs_sleepduration = TU_TO_USEC(roundup(IEEE80211_MS_TO_TU(100),
-						 sleepduration));
+						 intval));
 	if (bs.bs_sleepduration > bs.bs_dtimperiod)
 		bs.bs_sleepduration = bs.bs_dtimperiod;
 
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_init.c b/drivers/net/wireless/ath/ath9k/htc_drv_init.c
index f4e1de20d99c..b22fb64403d9 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_init.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_init.c
@@ -34,92 +34,9 @@ static int ath9k_htc_btcoex_enable;
 module_param_named(btcoex_enable, ath9k_htc_btcoex_enable, int, 0444);
 MODULE_PARM_DESC(btcoex_enable, "Enable wifi-BT coexistence");
 
-#define CHAN2G(_freq, _idx)  { \
-	.center_freq = (_freq), \
-	.hw_value = (_idx), \
-	.max_power = 20, \
-}
-
-#define CHAN5G(_freq, _idx) { \
-	.band = IEEE80211_BAND_5GHZ, \
-	.center_freq = (_freq), \
-	.hw_value = (_idx), \
-	.max_power = 20, \
-}
-
-static struct ieee80211_channel ath9k_2ghz_channels[] = {
-	CHAN2G(2412, 0), /* Channel 1 */
-	CHAN2G(2417, 1), /* Channel 2 */
-	CHAN2G(2422, 2), /* Channel 3 */
-	CHAN2G(2427, 3), /* Channel 4 */
-	CHAN2G(2432, 4), /* Channel 5 */
-	CHAN2G(2437, 5), /* Channel 6 */
-	CHAN2G(2442, 6), /* Channel 7 */
-	CHAN2G(2447, 7), /* Channel 8 */
-	CHAN2G(2452, 8), /* Channel 9 */
-	CHAN2G(2457, 9), /* Channel 10 */
-	CHAN2G(2462, 10), /* Channel 11 */
-	CHAN2G(2467, 11), /* Channel 12 */
-	CHAN2G(2472, 12), /* Channel 13 */
-	CHAN2G(2484, 13), /* Channel 14 */
-};
-
-static struct ieee80211_channel ath9k_5ghz_channels[] = {
-	/* _We_ call this UNII 1 */
-	CHAN5G(5180, 14), /* Channel 36 */
-	CHAN5G(5200, 15), /* Channel 40 */
-	CHAN5G(5220, 16), /* Channel 44 */
-	CHAN5G(5240, 17), /* Channel 48 */
-	/* _We_ call this UNII 2 */
-	CHAN5G(5260, 18), /* Channel 52 */
-	CHAN5G(5280, 19), /* Channel 56 */
-	CHAN5G(5300, 20), /* Channel 60 */
-	CHAN5G(5320, 21), /* Channel 64 */
-	/* _We_ call this "Middle band" */
-	CHAN5G(5500, 22), /* Channel 100 */
-	CHAN5G(5520, 23), /* Channel 104 */
-	CHAN5G(5540, 24), /* Channel 108 */
-	CHAN5G(5560, 25), /* Channel 112 */
-	CHAN5G(5580, 26), /* Channel 116 */
-	CHAN5G(5600, 27), /* Channel 120 */
-	CHAN5G(5620, 28), /* Channel 124 */
-	CHAN5G(5640, 29), /* Channel 128 */
-	CHAN5G(5660, 30), /* Channel 132 */
-	CHAN5G(5680, 31), /* Channel 136 */
-	CHAN5G(5700, 32), /* Channel 140 */
-	/* _We_ call this UNII 3 */
-	CHAN5G(5745, 33), /* Channel 149 */
-	CHAN5G(5765, 34), /* Channel 153 */
-	CHAN5G(5785, 35), /* Channel 157 */
-	CHAN5G(5805, 36), /* Channel 161 */
-	CHAN5G(5825, 37), /* Channel 165 */
-};
-
-/* Atheros hardware rate code addition for short premble */
-#define SHPCHECK(__hw_rate, __flags) \
-	((__flags & IEEE80211_RATE_SHORT_PREAMBLE) ? (__hw_rate | 0x04) : 0)
-
-#define RATE(_bitrate, _hw_rate, _flags) {		\
-	.bitrate	= (_bitrate),			\
-	.flags		= (_flags),			\
-	.hw_value	= (_hw_rate),			\
-	.hw_value_short = (SHPCHECK(_hw_rate, _flags))	\
-}
-
-static struct ieee80211_rate ath9k_legacy_rates[] = {
-	RATE(10, 0x1b, 0),
-	RATE(20, 0x1a, IEEE80211_RATE_SHORT_PREAMBLE), /* shortp : 0x1e */
-	RATE(55, 0x19, IEEE80211_RATE_SHORT_PREAMBLE), /* shortp: 0x1d */
-	RATE(110, 0x18, IEEE80211_RATE_SHORT_PREAMBLE), /* short: 0x1c */
-	RATE(60, 0x0b, 0),
-	RATE(90, 0x0f, 0),
-	RATE(120, 0x0a, 0),
-	RATE(180, 0x0e, 0),
-	RATE(240, 0x09, 0),
-	RATE(360, 0x0d, 0),
-	RATE(480, 0x08, 0),
-	RATE(540, 0x0c, 0),
-};
+static int ath9k_ps_enable;
+module_param_named(ps_enable, ath9k_ps_enable, int, 0444);
+MODULE_PARM_DESC(ps_enable, "Enable WLAN PowerSave");
 
 #ifdef CONFIG_MAC80211_LEDS
 static const struct ieee80211_tpt_blink ath9k_htc_tpt_blink[] = {
@@ -339,6 +256,25 @@ static void ath9k_multi_regread(void *hw_priv, u32 *addr,
        }
 }
 
+static void ath9k_regwrite_multi(struct ath_common *common)
+{
+	struct ath9k_htc_priv *priv = (struct ath9k_htc_priv *) common->priv;
+	u32 rsp_status;
+	int r;
+
+	r = ath9k_wmi_cmd(priv->wmi, WMI_REG_WRITE_CMDID,
+			  (u8 *) &priv->wmi->multi_write,
+			  sizeof(struct register_write) * priv->wmi->multi_write_idx,
+			  (u8 *) &rsp_status, sizeof(rsp_status),
+			  100);
+	if (unlikely(r)) {
+		ath_dbg(common, WMI,
+			"REGISTER WRITE FAILED, multi len: %d\n",
+			priv->wmi->multi_write_idx);
+	}
+	priv->wmi->multi_write_idx = 0;
+}
+
 static void ath9k_regwrite_single(void *hw_priv, u32 val, u32 reg_offset)
 {
 	struct ath_hw *ah = (struct ath_hw *) hw_priv;
@@ -365,8 +301,6 @@ static void ath9k_regwrite_buffer(void *hw_priv, u32 val, u32 reg_offset)
 	struct ath_hw *ah = (struct ath_hw *) hw_priv;
 	struct ath_common *common = ath9k_hw_common(ah);
 	struct ath9k_htc_priv *priv = (struct ath9k_htc_priv *) common->priv;
-	u32 rsp_status;
-	int r;
 
 	mutex_lock(&priv->wmi->multi_write_mutex);
 
@@ -379,19 +313,8 @@ static void ath9k_regwrite_buffer(void *hw_priv, u32 val, u32 reg_offset)
 	priv->wmi->multi_write_idx++;
 
 	/* If the buffer is full, send it out. */
-	if (priv->wmi->multi_write_idx == MAX_CMD_NUMBER) {
-		r = ath9k_wmi_cmd(priv->wmi, WMI_REG_WRITE_CMDID,
-			  (u8 *) &priv->wmi->multi_write,
-			  sizeof(struct register_write) * priv->wmi->multi_write_idx,
-			  (u8 *) &rsp_status, sizeof(rsp_status),
-			  100);
-		if (unlikely(r)) {
-			ath_dbg(common, WMI,
-				"REGISTER WRITE FAILED, multi len: %d\n",
-				priv->wmi->multi_write_idx);
-		}
-		priv->wmi->multi_write_idx = 0;
-	}
+	if (priv->wmi->multi_write_idx == MAX_CMD_NUMBER)
+		ath9k_regwrite_multi(common);
 
 	mutex_unlock(&priv->wmi->multi_write_mutex);
 }
@@ -422,26 +345,13 @@ static void ath9k_regwrite_flush(void *hw_priv)
 	struct ath_hw *ah = (struct ath_hw *) hw_priv;
 	struct ath_common *common = ath9k_hw_common(ah);
 	struct ath9k_htc_priv *priv = (struct ath9k_htc_priv *) common->priv;
-	u32 rsp_status;
-	int r;
 
 	atomic_dec(&priv->wmi->mwrite_cnt);
 
 	mutex_lock(&priv->wmi->multi_write_mutex);
 
-	if (priv->wmi->multi_write_idx) {
-		r = ath9k_wmi_cmd(priv->wmi, WMI_REG_WRITE_CMDID,
-			  (u8 *) &priv->wmi->multi_write,
-			  sizeof(struct register_write) * priv->wmi->multi_write_idx,
-			  (u8 *) &rsp_status, sizeof(rsp_status),
-			  100);
-		if (unlikely(r)) {
-			ath_dbg(common, WMI,
-				"REGISTER WRITE FAILED, multi len: %d\n",
-				priv->wmi->multi_write_idx);
-		}
-		priv->wmi->multi_write_idx = 0;
-	}
+	if (priv->wmi->multi_write_idx)
+		ath9k_regwrite_multi(common);
 
 	mutex_unlock(&priv->wmi->multi_write_mutex);
 }
@@ -487,51 +397,6 @@ static const struct ath_bus_ops ath9k_usb_bus_ops = {
 	.eeprom_read = ath_usb_eeprom_read,
 };
 
-static void setup_ht_cap(struct ath9k_htc_priv *priv,
-			 struct ieee80211_sta_ht_cap *ht_info)
-{
-	struct ath_common *common = ath9k_hw_common(priv->ah);
-	u8 tx_streams, rx_streams;
-	int i;
-
-	ht_info->ht_supported = true;
-	ht_info->cap = IEEE80211_HT_CAP_SUP_WIDTH_20_40 |
-		       IEEE80211_HT_CAP_SM_PS |
-		       IEEE80211_HT_CAP_SGI_40 |
-		       IEEE80211_HT_CAP_DSSSCCK40;
-
-	if (priv->ah->caps.hw_caps & ATH9K_HW_CAP_SGI_20)
-		ht_info->cap |= IEEE80211_HT_CAP_SGI_20;
-
-	ht_info->cap |= (1 << IEEE80211_HT_CAP_RX_STBC_SHIFT);
-
-	ht_info->ampdu_factor = IEEE80211_HT_MAX_AMPDU_64K;
-	ht_info->ampdu_density = IEEE80211_HT_MPDU_DENSITY_8;
-
-	memset(&ht_info->mcs, 0, sizeof(ht_info->mcs));
-
-	/* ath9k_htc supports only 1 or 2 stream devices */
-	tx_streams = ath9k_cmn_count_streams(priv->ah->txchainmask, 2);
-	rx_streams = ath9k_cmn_count_streams(priv->ah->rxchainmask, 2);
-
-	ath_dbg(common, CONFIG, "TX streams %d, RX streams: %d\n",
-		tx_streams, rx_streams);
-
-	if (tx_streams >= 2)
-		ht_info->cap |= IEEE80211_HT_CAP_TX_STBC;
-
-	if (tx_streams != rx_streams) {
-		ht_info->mcs.tx_params |= IEEE80211_HT_MCS_TX_RX_DIFF;
-		ht_info->mcs.tx_params |= ((tx_streams - 1) <<
-					   IEEE80211_HT_MCS_TX_MAX_STREAMS_SHIFT);
-	}
-
-	for (i = 0; i < rx_streams; i++)
-		ht_info->mcs.rx_mask[i] = 0xff;
-
-	ht_info->mcs.tx_params |= IEEE80211_HT_MCS_TX_DEFINED;
-}
-
 static int ath9k_init_queues(struct ath9k_htc_priv *priv)
 {
 	struct ath_common *common = ath9k_hw_common(priv->ah);
@@ -576,37 +441,13 @@ err:
 	return -EINVAL;
 }
 
-static void ath9k_init_channels_rates(struct ath9k_htc_priv *priv)
-{
-	if (priv->ah->caps.hw_caps & ATH9K_HW_CAP_2GHZ) {
-		priv->sbands[IEEE80211_BAND_2GHZ].channels =
-			ath9k_2ghz_channels;
-		priv->sbands[IEEE80211_BAND_2GHZ].band = IEEE80211_BAND_2GHZ;
-		priv->sbands[IEEE80211_BAND_2GHZ].n_channels =
-			ARRAY_SIZE(ath9k_2ghz_channels);
-		priv->sbands[IEEE80211_BAND_2GHZ].bitrates = ath9k_legacy_rates;
-		priv->sbands[IEEE80211_BAND_2GHZ].n_bitrates =
-			ARRAY_SIZE(ath9k_legacy_rates);
-	}
-
-	if (priv->ah->caps.hw_caps & ATH9K_HW_CAP_5GHZ) {
-		priv->sbands[IEEE80211_BAND_5GHZ].channels = ath9k_5ghz_channels;
-		priv->sbands[IEEE80211_BAND_5GHZ].band = IEEE80211_BAND_5GHZ;
-		priv->sbands[IEEE80211_BAND_5GHZ].n_channels =
-			ARRAY_SIZE(ath9k_5ghz_channels);
-		priv->sbands[IEEE80211_BAND_5GHZ].bitrates =
-			ath9k_legacy_rates + 4;
-		priv->sbands[IEEE80211_BAND_5GHZ].n_bitrates =
-			ARRAY_SIZE(ath9k_legacy_rates) - 4;
-	}
-}
-
 static void ath9k_init_misc(struct ath9k_htc_priv *priv)
 {
 	struct ath_common *common = ath9k_hw_common(priv->ah);
 
 	memcpy(common->bssidmask, ath_bcast_mac, ETH_ALEN);
 
+	common->last_rssi = ATH_RSSI_DUMMY_MARKER;
 	priv->ah->opmode = NL80211_IFTYPE_STATION;
 }
 
@@ -624,6 +465,7 @@ static int ath9k_init_priv(struct ath9k_htc_priv *priv,
 	if (!ah)
 		return -ENOMEM;
 
+	ah->dev = priv->dev;
 	ah->hw_version.devid = devid;
 	ah->hw_version.usbdev = drv_info;
 	ah->ah_flags |= AH_USE_EEPROM;
@@ -680,8 +522,8 @@ static int ath9k_init_priv(struct ath9k_htc_priv *priv,
 	for (i = 0; i < ATH9K_HTC_MAX_BCN_VIF; i++)
 		priv->cur_beacon_conf.bslot[i] = NULL;
 
+	ath9k_cmn_init_channels_rates(common);
 	ath9k_cmn_init_crypto(ah);
-	ath9k_init_channels_rates(priv);
 	ath9k_init_misc(priv);
 	ath9k_htc_init_btcoex(priv, product);
 
@@ -717,6 +559,7 @@ static const struct ieee80211_iface_combination if_comb = {
 static void ath9k_set_hw_capab(struct ath9k_htc_priv *priv,
 			       struct ieee80211_hw *hw)
 {
+	struct ath_hw *ah = priv->ah;
 	struct ath_common *common = ath9k_hw_common(priv->ah);
 	struct base_eep_header *pBase;
 
@@ -725,12 +568,14 @@ static void ath9k_set_hw_capab(struct ath9k_htc_priv *priv,
 		IEEE80211_HW_SPECTRUM_MGMT |
 		IEEE80211_HW_HAS_RATE_CONTROL |
 		IEEE80211_HW_RX_INCLUDES_FCS |
-		IEEE80211_HW_SUPPORTS_PS |
 		IEEE80211_HW_PS_NULLFUNC_STACK |
 		IEEE80211_HW_REPORTS_TX_ACK_STATUS |
 		IEEE80211_HW_MFP_CAPABLE |
 		IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING;
 
+	if (ath9k_ps_enable)
+		hw->flags |= IEEE80211_HW_SUPPORTS_PS;
+
 	hw->wiphy->interface_modes =
 		BIT(NL80211_IFTYPE_STATION) |
 		BIT(NL80211_IFTYPE_ADHOC) |
@@ -759,19 +604,12 @@ static void ath9k_set_hw_capab(struct ath9k_htc_priv *priv,
 
 	if (priv->ah->caps.hw_caps & ATH9K_HW_CAP_2GHZ)
 		hw->wiphy->bands[IEEE80211_BAND_2GHZ] =
-			&priv->sbands[IEEE80211_BAND_2GHZ];
+			&common->sbands[IEEE80211_BAND_2GHZ];
 	if (priv->ah->caps.hw_caps & ATH9K_HW_CAP_5GHZ)
 		hw->wiphy->bands[IEEE80211_BAND_5GHZ] =
-			&priv->sbands[IEEE80211_BAND_5GHZ];
-
-	if (priv->ah->caps.hw_caps & ATH9K_HW_CAP_HT) {
-		if (priv->ah->caps.hw_caps & ATH9K_HW_CAP_2GHZ)
-			setup_ht_cap(priv,
-				     &priv->sbands[IEEE80211_BAND_2GHZ].ht_cap);
-		if (priv->ah->caps.hw_caps & ATH9K_HW_CAP_5GHZ)
-			setup_ht_cap(priv,
-				     &priv->sbands[IEEE80211_BAND_5GHZ].ht_cap);
-	}
+			&common->sbands[IEEE80211_BAND_5GHZ];
+
+	ath9k_cmn_reload_chainmask(ah);
 
 	pBase = ath9k_htc_get_eeprom_base(priv);
 	if (pBase) {
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_main.c b/drivers/net/wireless/ath/ath9k/htc_drv_main.c
index 608d739d1378..90dad4172b0a 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_main.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_main.c
@@ -1270,18 +1270,50 @@ static void ath9k_htc_configure_filter(struct ieee80211_hw *hw,
 	mutex_unlock(&priv->mutex);
 }
 
+static void ath9k_htc_sta_rc_update_work(struct work_struct *work)
+{
+	struct ath9k_htc_sta *ista =
+	    container_of(work, struct ath9k_htc_sta, rc_update_work);
+	struct ieee80211_sta *sta =
+	    container_of((void *)ista, struct ieee80211_sta, drv_priv);
+	struct ath9k_htc_priv *priv = ista->htc_priv;
+	struct ath_common *common = ath9k_hw_common(priv->ah);
+	struct ath9k_htc_target_rate trate;
+
+	mutex_lock(&priv->mutex);
+	ath9k_htc_ps_wakeup(priv);
+
+	memset(&trate, 0, sizeof(struct ath9k_htc_target_rate));
+	ath9k_htc_setup_rate(priv, sta, &trate);
+	if (!ath9k_htc_send_rate_cmd(priv, &trate))
+		ath_dbg(common, CONFIG,
+			"Supported rates for sta: %pM updated, rate caps: 0x%X\n",
+			sta->addr, be32_to_cpu(trate.capflags));
+	else
+		ath_dbg(common, CONFIG,
+			"Unable to update supported rates for sta: %pM\n",
+			sta->addr);
+
+	ath9k_htc_ps_restore(priv);
+	mutex_unlock(&priv->mutex);
+}
+
 static int ath9k_htc_sta_add(struct ieee80211_hw *hw,
 			     struct ieee80211_vif *vif,
 			     struct ieee80211_sta *sta)
 {
 	struct ath9k_htc_priv *priv = hw->priv;
+	struct ath9k_htc_sta *ista = (struct ath9k_htc_sta *) sta->drv_priv;
 	int ret;
 
 	mutex_lock(&priv->mutex);
 	ath9k_htc_ps_wakeup(priv);
 	ret = ath9k_htc_add_station(priv, vif, sta);
-	if (!ret)
+	if (!ret) {
+		INIT_WORK(&ista->rc_update_work, ath9k_htc_sta_rc_update_work);
+		ista->htc_priv = priv;
 		ath9k_htc_init_rate(priv, sta);
+	}
 	ath9k_htc_ps_restore(priv);
 	mutex_unlock(&priv->mutex);
 
@@ -1293,12 +1325,13 @@ static int ath9k_htc_sta_remove(struct ieee80211_hw *hw,
 				struct ieee80211_sta *sta)
 {
 	struct ath9k_htc_priv *priv = hw->priv;
-	struct ath9k_htc_sta *ista;
+	struct ath9k_htc_sta *ista = (struct ath9k_htc_sta *) sta->drv_priv;
 	int ret;
 
+	cancel_work_sync(&ista->rc_update_work);
+
 	mutex_lock(&priv->mutex);
 	ath9k_htc_ps_wakeup(priv);
-	ista = (struct ath9k_htc_sta *) sta->drv_priv;
 	htc_sta_drain(priv->htc, ista->index);
 	ret = ath9k_htc_remove_station(priv, vif, sta);
 	ath9k_htc_ps_restore(priv);
@@ -1311,28 +1344,12 @@ static void ath9k_htc_sta_rc_update(struct ieee80211_hw *hw,
 				    struct ieee80211_vif *vif,
 				    struct ieee80211_sta *sta, u32 changed)
 {
-	struct ath9k_htc_priv *priv = hw->priv;
-	struct ath_common *common = ath9k_hw_common(priv->ah);
-	struct ath9k_htc_target_rate trate;
-
-	mutex_lock(&priv->mutex);
-	ath9k_htc_ps_wakeup(priv);
+	struct ath9k_htc_sta *ista = (struct ath9k_htc_sta *) sta->drv_priv;
 
-	if (changed & IEEE80211_RC_SUPP_RATES_CHANGED) {
-		memset(&trate, 0, sizeof(struct ath9k_htc_target_rate));
-		ath9k_htc_setup_rate(priv, sta, &trate);
-		if (!ath9k_htc_send_rate_cmd(priv, &trate))
-			ath_dbg(common, CONFIG,
-				"Supported rates for sta: %pM updated, rate caps: 0x%X\n",
-				sta->addr, be32_to_cpu(trate.capflags));
-		else
-			ath_dbg(common, CONFIG,
-				"Unable to update supported rates for sta: %pM\n",
-				sta->addr);
-	}
+	if (!(changed & IEEE80211_RC_SUPP_RATES_CHANGED))
+		return;
 
-	ath9k_htc_ps_restore(priv);
-	mutex_unlock(&priv->mutex);
+	schedule_work(&ista->rc_update_work);
 }
 
 static int ath9k_htc_conf_tx(struct ieee80211_hw *hw,
@@ -1457,6 +1474,7 @@ static void ath9k_htc_bss_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
 
 	if ((vif->type == NL80211_IFTYPE_STATION) && bss_conf->assoc) {
 		common->curaid = bss_conf->aid;
+		common->last_rssi = ATH_RSSI_DUMMY_MARKER;
 		memcpy(common->curbssid, bss_conf->bssid, ETH_ALEN);
 	}
 }
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_txrx.c b/drivers/net/wireless/ath/ath9k/htc_drv_txrx.c
index 12e0f32a4905..47b2bfcd8223 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_txrx.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_txrx.c
@@ -927,43 +927,39 @@ void ath9k_host_rx_init(struct ath9k_htc_priv *priv)
 	ath9k_hw_rxena(priv->ah);
 	ath9k_htc_opmode_init(priv);
 	ath9k_hw_startpcureceive(priv->ah, test_bit(OP_SCANNING, &priv->op_flags));
-	priv->rx.last_rssi = ATH_RSSI_DUMMY_MARKER;
 }
 
-static void ath9k_process_rate(struct ieee80211_hw *hw,
-			       struct ieee80211_rx_status *rxs,
-			       u8 rx_rate, u8 rs_flags)
+static inline void convert_htc_flag(struct ath_rx_status *rx_stats,
+				   struct ath_htc_rx_status *rxstatus)
 {
-	struct ieee80211_supported_band *sband;
-	enum ieee80211_band band;
-	unsigned int i = 0;
-
-	if (rx_rate & 0x80) {
-		/* HT rate */
-		rxs->flag |= RX_FLAG_HT;
-		if (rs_flags & ATH9K_RX_2040)
-			rxs->flag |= RX_FLAG_40MHZ;
-		if (rs_flags & ATH9K_RX_GI)
-			rxs->flag |= RX_FLAG_SHORT_GI;
-		rxs->rate_idx = rx_rate & 0x7f;
-		return;
-	}
-
-	band = hw->conf.chandef.chan->band;
-	sband = hw->wiphy->bands[band];
-
-	for (i = 0; i < sband->n_bitrates; i++) {
-		if (sband->bitrates[i].hw_value == rx_rate) {
-			rxs->rate_idx = i;
-			return;
-		}
-		if (sband->bitrates[i].hw_value_short == rx_rate) {
-			rxs->rate_idx = i;
-			rxs->flag |= RX_FLAG_SHORTPRE;
-			return;
-		}
-	}
+	rx_stats->flag = 0;
+	if (rxstatus->rs_flags & ATH9K_RX_2040)
+		rx_stats->flag |= RX_FLAG_40MHZ;
+	if (rxstatus->rs_flags & ATH9K_RX_GI)
+		rx_stats->flag |= RX_FLAG_SHORT_GI;
+}
 
+static void rx_status_htc_to_ath(struct ath_rx_status *rx_stats,
+				 struct ath_htc_rx_status *rxstatus)
+{
+	rx_stats->rs_datalen	= rxstatus->rs_datalen;
+	rx_stats->rs_status	= rxstatus->rs_status;
+	rx_stats->rs_phyerr	= rxstatus->rs_phyerr;
+	rx_stats->rs_rssi	= rxstatus->rs_rssi;
+	rx_stats->rs_keyix	= rxstatus->rs_keyix;
+	rx_stats->rs_rate	= rxstatus->rs_rate;
+	rx_stats->rs_antenna	= rxstatus->rs_antenna;
+	rx_stats->rs_more	= rxstatus->rs_more;
+
+	memcpy(rx_stats->rs_rssi_ctl, rxstatus->rs_rssi_ctl,
+		sizeof(rx_stats->rs_rssi_ctl));
+	memcpy(rx_stats->rs_rssi_ext, rxstatus->rs_rssi_ext,
+		sizeof(rx_stats->rs_rssi_ext));
+
+	rx_stats->rs_isaggr	= rxstatus->rs_isaggr;
+	rx_stats->rs_moreaggr	= rxstatus->rs_moreaggr;
+	rx_stats->rs_num_delims	= rxstatus->rs_num_delims;
+	convert_htc_flag(rx_stats, rxstatus);
 }
 
 static bool ath9k_rx_prepare(struct ath9k_htc_priv *priv,
@@ -975,10 +971,10 @@ static bool ath9k_rx_prepare(struct ath9k_htc_priv *priv,
 	struct ieee80211_hw *hw = priv->hw;
 	struct sk_buff *skb = rxbuf->skb;
 	struct ath_common *common = ath9k_hw_common(priv->ah);
+	struct ath_hw *ah = common->ah;
 	struct ath_htc_rx_status *rxstatus;
-	int hdrlen, padsize;
-	int last_rssi = ATH_RSSI_DUMMY_MARKER;
-	__le16 fc;
+	struct ath_rx_status rx_stats;
+	bool decrypt_error;
 
 	if (skb->len < HTC_RX_FRAME_HEADER_SIZE) {
 		ath_err(common, "Corrupted RX frame, dropping (len: %d)\n",
@@ -999,103 +995,39 @@ static bool ath9k_rx_prepare(struct ath9k_htc_priv *priv,
 	ath9k_htc_err_stat_rx(priv, rxstatus);
 
 	/* Get the RX status information */
-	memcpy(&rxbuf->rxstatus, rxstatus, HTC_RX_FRAME_HEADER_SIZE);
-	skb_pull(skb, HTC_RX_FRAME_HEADER_SIZE);
-
-	hdr = (struct ieee80211_hdr *)skb->data;
-	fc = hdr->frame_control;
-	hdrlen = ieee80211_get_hdrlen_from_skb(skb);
-
-	padsize = hdrlen & 3;
-	if (padsize && skb->len >= hdrlen+padsize+FCS_LEN) {
-		memmove(skb->data + padsize, skb->data, hdrlen);
-		skb_pull(skb, padsize);
-	}
 
 	memset(rx_status, 0, sizeof(struct ieee80211_rx_status));
 
-	if (rxbuf->rxstatus.rs_status != 0) {
-		if (rxbuf->rxstatus.rs_status & ATH9K_RXERR_CRC)
-			rx_status->flag |= RX_FLAG_FAILED_FCS_CRC;
-		if (rxbuf->rxstatus.rs_status & ATH9K_RXERR_PHY)
-			goto rx_next;
-
-		if (rxbuf->rxstatus.rs_status & ATH9K_RXERR_DECRYPT) {
-			/* FIXME */
-		} else if (rxbuf->rxstatus.rs_status & ATH9K_RXERR_MIC) {
-			if (ieee80211_is_ctl(fc))
-				/*
-				 * Sometimes, we get invalid
-				 * MIC failures on valid control frames.
-				 * Remove these mic errors.
-				 */
-				rxbuf->rxstatus.rs_status &= ~ATH9K_RXERR_MIC;
-			else
-				rx_status->flag |= RX_FLAG_MMIC_ERROR;
-		}
-
-		/*
-		 * Reject error frames with the exception of
-		 * decryption and MIC failures. For monitor mode,
-		 * we also ignore the CRC error.
-		 */
-		if (priv->ah->opmode == NL80211_IFTYPE_MONITOR) {
-			if (rxbuf->rxstatus.rs_status &
-			    ~(ATH9K_RXERR_DECRYPT | ATH9K_RXERR_MIC |
-			      ATH9K_RXERR_CRC))
-				goto rx_next;
-		} else {
-			if (rxbuf->rxstatus.rs_status &
-			    ~(ATH9K_RXERR_DECRYPT | ATH9K_RXERR_MIC)) {
-				goto rx_next;
-			}
-		}
-	}
-
-	if (!(rxbuf->rxstatus.rs_status & ATH9K_RXERR_DECRYPT)) {
-		u8 keyix;
-		keyix = rxbuf->rxstatus.rs_keyix;
-		if (keyix != ATH9K_RXKEYIX_INVALID) {
-			rx_status->flag |= RX_FLAG_DECRYPTED;
-		} else if (ieee80211_has_protected(fc) &&
-			   skb->len >= hdrlen + 4) {
-			keyix = skb->data[hdrlen + 3] >> 6;
-			if (test_bit(keyix, common->keymap))
-				rx_status->flag |= RX_FLAG_DECRYPTED;
-		}
-	}
-
-	ath9k_process_rate(hw, rx_status, rxbuf->rxstatus.rs_rate,
-			   rxbuf->rxstatus.rs_flags);
-
-	if (rxbuf->rxstatus.rs_rssi != ATH9K_RSSI_BAD &&
-	    !rxbuf->rxstatus.rs_moreaggr)
-		ATH_RSSI_LPF(priv->rx.last_rssi,
-			     rxbuf->rxstatus.rs_rssi);
-
-	last_rssi = priv->rx.last_rssi;
+	/* Copy everything from ath_htc_rx_status (HTC_RX_FRAME_HEADER).
+	 * After this, we can drop this part of skb. */
+	rx_status_htc_to_ath(&rx_stats, rxstatus);
+	rx_status->mactime = be64_to_cpu(rxstatus->rs_tstamp);
+	skb_pull(skb, HTC_RX_FRAME_HEADER_SIZE);
 
-	if (ath_is_mybeacon(common, hdr)) {
-		s8 rssi = rxbuf->rxstatus.rs_rssi;
+	/*
+	 * everything but the rate is checked here, the rate check is done
+	 * separately to avoid doing two lookups for a rate for each frame.
+	 */
+	hdr = (struct ieee80211_hdr *)skb->data;
+	if (!ath9k_cmn_rx_accept(common, hdr, rx_status, &rx_stats,
+			&decrypt_error, priv->rxfilter))
+		goto rx_next;
 
-		if (likely(last_rssi != ATH_RSSI_DUMMY_MARKER))
-			rssi = ATH_EP_RND(last_rssi, ATH_RSSI_EP_MULTIPLIER);
+	ath9k_cmn_rx_skb_postprocess(common, skb, &rx_stats,
+				     rx_status, decrypt_error);
 
-		if (rssi < 0)
-			rssi = 0;
+	if (ath9k_cmn_process_rate(common, hw, &rx_stats, rx_status))
+		goto rx_next;
 
-		priv->ah->stats.avgbrssi = rssi;
-	}
+	rx_stats.is_mybeacon = ath_is_mybeacon(common, hdr);
+	ath9k_cmn_process_rssi(common, hw, &rx_stats, rx_status);
 
-	rx_status->mactime = be64_to_cpu(rxbuf->rxstatus.rs_tstamp);
-	rx_status->band = hw->conf.chandef.chan->band;
-	rx_status->freq = hw->conf.chandef.chan->center_freq;
-	rx_status->signal =  rxbuf->rxstatus.rs_rssi + ATH_DEFAULT_NOISE_FLOOR;
-	rx_status->antenna = rxbuf->rxstatus.rs_antenna;
+	rx_status->band = ah->curchan->chan->band;
+	rx_status->freq = ah->curchan->chan->center_freq;
+	rx_status->antenna = rx_stats.rs_antenna;
 	rx_status->flag |= RX_FLAG_MACTIME_END;
 
 	return true;
-
 rx_next:
 	return false;
 }
diff --git a/drivers/net/wireless/ath/ath9k/htc_hst.c b/drivers/net/wireless/ath/ath9k/htc_hst.c
index aac4a406a513..a0ff5b637054 100644
--- a/drivers/net/wireless/ath/ath9k/htc_hst.c
+++ b/drivers/net/wireless/ath/ath9k/htc_hst.c
@@ -358,6 +358,36 @@ ret:
 		kfree_skb(skb);
 }
 
+static void ath9k_htc_fw_panic_report(struct htc_target *htc_handle,
+				      struct sk_buff *skb)
+{
+	uint32_t *pattern = (uint32_t *)skb->data;
+
+	switch (*pattern) {
+	case 0x33221199:
+		{
+		struct htc_panic_bad_vaddr *htc_panic;
+		htc_panic = (struct htc_panic_bad_vaddr *) skb->data;
+		dev_err(htc_handle->dev, "ath: firmware panic! "
+			"exccause: 0x%08x; pc: 0x%08x; badvaddr: 0x%08x.\n",
+			htc_panic->exccause, htc_panic->pc,
+			htc_panic->badvaddr);
+		break;
+		}
+	case 0x33221299:
+		{
+		struct htc_panic_bad_epid *htc_panic;
+		htc_panic = (struct htc_panic_bad_epid *) skb->data;
+		dev_err(htc_handle->dev, "ath: firmware panic! "
+			"bad epid: 0x%08x\n", htc_panic->epid);
+		break;
+		}
+	default:
+		dev_err(htc_handle->dev, "ath: uknown panic pattern!\n");
+		break;
+	}
+}
+
 /*
  * HTC Messages are handled directly here and the obtained SKB
  * is freed.
@@ -379,6 +409,12 @@ void ath9k_htc_rx_msg(struct htc_target *htc_handle,
 	htc_hdr = (struct htc_frame_hdr *) skb->data;
 	epid = htc_hdr->endpoint_id;
 
+	if (epid == 0x99) {
+		ath9k_htc_fw_panic_report(htc_handle, skb);
+		kfree_skb(skb);
+		return;
+	}
+
 	if (epid >= ENDPOINT_MAX) {
 		if (pipe_id != USB_REG_IN_PIPE)
 			dev_kfree_skb_any(skb);
diff --git a/drivers/net/wireless/ath/ath9k/htc_hst.h b/drivers/net/wireless/ath/ath9k/htc_hst.h
index e1ffbb6bd636..06474ccc7696 100644
--- a/drivers/net/wireless/ath/ath9k/htc_hst.h
+++ b/drivers/net/wireless/ath/ath9k/htc_hst.h
@@ -77,6 +77,18 @@ struct htc_config_pipe_msg {
 	u8 credits;
 } __packed;
 
+struct htc_panic_bad_vaddr {
+	__be32 pattern;
+	__be32 exccause;
+	__be32 pc;
+	__be32 badvaddr;
+} __packed;
+
+struct htc_panic_bad_epid {
+	__be32 pattern;
+	__be32 epid;
+} __packed;
+
 struct htc_ep_callbacks {
 	void *priv;
 	void (*tx) (void *, struct sk_buff *, enum htc_endpoint_id, bool txok);
diff --git a/drivers/net/wireless/ath/ath9k/hw.c b/drivers/net/wireless/ath/ath9k/hw.c
index fbf43c05713f..2509c2ff0828 100644
--- a/drivers/net/wireless/ath/ath9k/hw.c
+++ b/drivers/net/wireless/ath/ath9k/hw.c
@@ -23,7 +23,6 @@
 
 #include "hw.h"
 #include "hw-ops.h"
-#include "rc.h"
 #include "ar9003_mac.h"
 #include "ar9003_mci.h"
 #include "ar9003_phy.h"
@@ -1316,7 +1315,7 @@ static bool ath9k_hw_set_reset(struct ath_hw *ah, int type)
 	if (AR_SREV_9300_20_OR_LATER(ah))
 		udelay(50);
 	else if (AR_SREV_9100(ah))
-		udelay(10000);
+		mdelay(10);
 	else
 		udelay(100);
 
@@ -1534,7 +1533,7 @@ EXPORT_SYMBOL(ath9k_hw_check_nav);
 bool ath9k_hw_check_alive(struct ath_hw *ah)
 {
 	int count = 50;
-	u32 reg;
+	u32 reg, last_val;
 
 	if (AR_SREV_9300(ah))
 		return !ath9k_hw_detect_mac_hang(ah);
@@ -1542,9 +1541,13 @@ bool ath9k_hw_check_alive(struct ath_hw *ah)
 	if (AR_SREV_9285_12_OR_LATER(ah))
 		return true;
 
+	last_val = REG_READ(ah, AR_OBS_BUS_1);
 	do {
 		reg = REG_READ(ah, AR_OBS_BUS_1);
+		if (reg != last_val)
+			return true;
 
+		last_val = reg;
 		if ((reg & 0x7E7FFFEF) == 0x00702400)
 			continue;
 
@@ -1556,6 +1559,8 @@ bool ath9k_hw_check_alive(struct ath_hw *ah)
 		default:
 			return true;
 		}
+
+		udelay(1);
 	} while (count-- > 0);
 
 	return false;
@@ -2051,9 +2056,8 @@ static bool ath9k_hw_set_power_awake(struct ath_hw *ah)
 
 	REG_SET_BIT(ah, AR_RTC_FORCE_WAKE,
 		    AR_RTC_FORCE_WAKE_EN);
-
 	if (AR_SREV_9100(ah))
-		udelay(10000);
+		mdelay(10);
 	else
 		udelay(50);
 
diff --git a/drivers/net/wireless/ath/ath9k/init.c b/drivers/net/wireless/ath/ath9k/init.c
index c36de303c8f3..c0a4e866edca 100644
--- a/drivers/net/wireless/ath/ath9k/init.c
+++ b/drivers/net/wireless/ath/ath9k/init.c
@@ -57,112 +57,11 @@ static int ath9k_bt_ant_diversity;
 module_param_named(bt_ant_diversity, ath9k_bt_ant_diversity, int, 0444);
 MODULE_PARM_DESC(bt_ant_diversity, "Enable WLAN/BT RX antenna diversity");
 
-bool is_ath9k_unloaded;
-/* We use the hw_value as an index into our private channel structure */
-
-#define CHAN2G(_freq, _idx)  { \
-	.band = IEEE80211_BAND_2GHZ, \
-	.center_freq = (_freq), \
-	.hw_value = (_idx), \
-	.max_power = 20, \
-}
-
-#define CHAN5G(_freq, _idx) { \
-	.band = IEEE80211_BAND_5GHZ, \
-	.center_freq = (_freq), \
-	.hw_value = (_idx), \
-	.max_power = 20, \
-}
-
-/* Some 2 GHz radios are actually tunable on 2312-2732
- * on 5 MHz steps, we support the channels which we know
- * we have calibration data for all cards though to make
- * this static */
-static const struct ieee80211_channel ath9k_2ghz_chantable[] = {
-	CHAN2G(2412, 0), /* Channel 1 */
-	CHAN2G(2417, 1), /* Channel 2 */
-	CHAN2G(2422, 2), /* Channel 3 */
-	CHAN2G(2427, 3), /* Channel 4 */
-	CHAN2G(2432, 4), /* Channel 5 */
-	CHAN2G(2437, 5), /* Channel 6 */
-	CHAN2G(2442, 6), /* Channel 7 */
-	CHAN2G(2447, 7), /* Channel 8 */
-	CHAN2G(2452, 8), /* Channel 9 */
-	CHAN2G(2457, 9), /* Channel 10 */
-	CHAN2G(2462, 10), /* Channel 11 */
-	CHAN2G(2467, 11), /* Channel 12 */
-	CHAN2G(2472, 12), /* Channel 13 */
-	CHAN2G(2484, 13), /* Channel 14 */
-};
+static int ath9k_ps_enable;
+module_param_named(ps_enable, ath9k_ps_enable, int, 0444);
+MODULE_PARM_DESC(ps_enable, "Enable WLAN PowerSave");
 
-/* Some 5 GHz radios are actually tunable on XXXX-YYYY
- * on 5 MHz steps, we support the channels which we know
- * we have calibration data for all cards though to make
- * this static */
-static const struct ieee80211_channel ath9k_5ghz_chantable[] = {
-	/* _We_ call this UNII 1 */
-	CHAN5G(5180, 14), /* Channel 36 */
-	CHAN5G(5200, 15), /* Channel 40 */
-	CHAN5G(5220, 16), /* Channel 44 */
-	CHAN5G(5240, 17), /* Channel 48 */
-	/* _We_ call this UNII 2 */
-	CHAN5G(5260, 18), /* Channel 52 */
-	CHAN5G(5280, 19), /* Channel 56 */
-	CHAN5G(5300, 20), /* Channel 60 */
-	CHAN5G(5320, 21), /* Channel 64 */
-	/* _We_ call this "Middle band" */
-	CHAN5G(5500, 22), /* Channel 100 */
-	CHAN5G(5520, 23), /* Channel 104 */
-	CHAN5G(5540, 24), /* Channel 108 */
-	CHAN5G(5560, 25), /* Channel 112 */
-	CHAN5G(5580, 26), /* Channel 116 */
-	CHAN5G(5600, 27), /* Channel 120 */
-	CHAN5G(5620, 28), /* Channel 124 */
-	CHAN5G(5640, 29), /* Channel 128 */
-	CHAN5G(5660, 30), /* Channel 132 */
-	CHAN5G(5680, 31), /* Channel 136 */
-	CHAN5G(5700, 32), /* Channel 140 */
-	/* _We_ call this UNII 3 */
-	CHAN5G(5745, 33), /* Channel 149 */
-	CHAN5G(5765, 34), /* Channel 153 */
-	CHAN5G(5785, 35), /* Channel 157 */
-	CHAN5G(5805, 36), /* Channel 161 */
-	CHAN5G(5825, 37), /* Channel 165 */
-};
-
-/* Atheros hardware rate code addition for short premble */
-#define SHPCHECK(__hw_rate, __flags) \
-	((__flags & IEEE80211_RATE_SHORT_PREAMBLE) ? (__hw_rate | 0x04 ) : 0)
-
-#define RATE(_bitrate, _hw_rate, _flags) {              \
-	.bitrate        = (_bitrate),                   \
-	.flags          = (_flags),                     \
-	.hw_value       = (_hw_rate),                   \
-	.hw_value_short = (SHPCHECK(_hw_rate, _flags))  \
-}
-
-static struct ieee80211_rate ath9k_legacy_rates[] = {
-	RATE(10, 0x1b, 0),
-	RATE(20, 0x1a, IEEE80211_RATE_SHORT_PREAMBLE),
-	RATE(55, 0x19, IEEE80211_RATE_SHORT_PREAMBLE),
-	RATE(110, 0x18, IEEE80211_RATE_SHORT_PREAMBLE),
-	RATE(60, 0x0b, (IEEE80211_RATE_SUPPORTS_5MHZ |
-			IEEE80211_RATE_SUPPORTS_10MHZ)),
-	RATE(90, 0x0f, (IEEE80211_RATE_SUPPORTS_5MHZ |
-			IEEE80211_RATE_SUPPORTS_10MHZ)),
-	RATE(120, 0x0a, (IEEE80211_RATE_SUPPORTS_5MHZ |
-			 IEEE80211_RATE_SUPPORTS_10MHZ)),
-	RATE(180, 0x0e, (IEEE80211_RATE_SUPPORTS_5MHZ |
-			 IEEE80211_RATE_SUPPORTS_10MHZ)),
-	RATE(240, 0x09, (IEEE80211_RATE_SUPPORTS_5MHZ |
-			 IEEE80211_RATE_SUPPORTS_10MHZ)),
-	RATE(360, 0x0d, (IEEE80211_RATE_SUPPORTS_5MHZ |
-			 IEEE80211_RATE_SUPPORTS_10MHZ)),
-	RATE(480, 0x08, (IEEE80211_RATE_SUPPORTS_5MHZ |
-			 IEEE80211_RATE_SUPPORTS_10MHZ)),
-	RATE(540, 0x0c, (IEEE80211_RATE_SUPPORTS_5MHZ |
-			 IEEE80211_RATE_SUPPORTS_10MHZ)),
-};
+bool is_ath9k_unloaded;
 
 #ifdef CONFIG_MAC80211_LEDS
 static const struct ieee80211_tpt_blink ath9k_tpt_blink[] = {
@@ -254,64 +153,6 @@ static unsigned int ath9k_reg_rmw(void *hw_priv, u32 reg_offset, u32 set, u32 cl
 /*     Initialization     */
 /**************************/
 
-static void setup_ht_cap(struct ath_softc *sc,
-			 struct ieee80211_sta_ht_cap *ht_info)
-{
-	struct ath_hw *ah = sc->sc_ah;
-	struct ath_common *common = ath9k_hw_common(ah);
-	u8 tx_streams, rx_streams;
-	int i, max_streams;
-
-	ht_info->ht_supported = true;
-	ht_info->cap = IEEE80211_HT_CAP_SUP_WIDTH_20_40 |
-		       IEEE80211_HT_CAP_SM_PS |
-		       IEEE80211_HT_CAP_SGI_40 |
-		       IEEE80211_HT_CAP_DSSSCCK40;
-
-	if (sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_LDPC)
-		ht_info->cap |= IEEE80211_HT_CAP_LDPC_CODING;
-
-	if (sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_SGI_20)
-		ht_info->cap |= IEEE80211_HT_CAP_SGI_20;
-
-	ht_info->ampdu_factor = IEEE80211_HT_MAX_AMPDU_64K;
-	ht_info->ampdu_density = IEEE80211_HT_MPDU_DENSITY_8;
-
-	if (AR_SREV_9330(ah) || AR_SREV_9485(ah) || AR_SREV_9565(ah))
-		max_streams = 1;
-	else if (AR_SREV_9462(ah))
-		max_streams = 2;
-	else if (AR_SREV_9300_20_OR_LATER(ah))
-		max_streams = 3;
-	else
-		max_streams = 2;
-
-	if (AR_SREV_9280_20_OR_LATER(ah)) {
-		if (max_streams >= 2)
-			ht_info->cap |= IEEE80211_HT_CAP_TX_STBC;
-		ht_info->cap |= (1 << IEEE80211_HT_CAP_RX_STBC_SHIFT);
-	}
-
-	/* set up supported mcs set */
-	memset(&ht_info->mcs, 0, sizeof(ht_info->mcs));
-	tx_streams = ath9k_cmn_count_streams(ah->txchainmask, max_streams);
-	rx_streams = ath9k_cmn_count_streams(ah->rxchainmask, max_streams);
-
-	ath_dbg(common, CONFIG, "TX streams %d, RX streams: %d\n",
-		tx_streams, rx_streams);
-
-	if (tx_streams != rx_streams) {
-		ht_info->mcs.tx_params |= IEEE80211_HT_MCS_TX_RX_DIFF;
-		ht_info->mcs.tx_params |= ((tx_streams - 1) <<
-				IEEE80211_HT_MCS_TX_MAX_STREAMS_SHIFT);
-	}
-
-	for (i = 0; i < rx_streams; i++)
-		ht_info->mcs.rx_mask[i] = 0xff;
-
-	ht_info->mcs.tx_params |= IEEE80211_HT_MCS_TX_DEFINED;
-}
-
 static void ath9k_reg_notifier(struct wiphy *wiphy,
 			       struct regulatory_request *request)
 {
@@ -482,51 +323,6 @@ static int ath9k_init_queues(struct ath_softc *sc)
 	return 0;
 }
 
-static int ath9k_init_channels_rates(struct ath_softc *sc)
-{
-	void *channels;
-
-	BUILD_BUG_ON(ARRAY_SIZE(ath9k_2ghz_chantable) +
-		     ARRAY_SIZE(ath9k_5ghz_chantable) !=
-		     ATH9K_NUM_CHANNELS);
-
-	if (sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_2GHZ) {
-		channels = devm_kzalloc(sc->dev,
-			sizeof(ath9k_2ghz_chantable), GFP_KERNEL);
-		if (!channels)
-		    return -ENOMEM;
-
-		memcpy(channels, ath9k_2ghz_chantable,
-		       sizeof(ath9k_2ghz_chantable));
-		sc->sbands[IEEE80211_BAND_2GHZ].channels = channels;
-		sc->sbands[IEEE80211_BAND_2GHZ].band = IEEE80211_BAND_2GHZ;
-		sc->sbands[IEEE80211_BAND_2GHZ].n_channels =
-			ARRAY_SIZE(ath9k_2ghz_chantable);
-		sc->sbands[IEEE80211_BAND_2GHZ].bitrates = ath9k_legacy_rates;
-		sc->sbands[IEEE80211_BAND_2GHZ].n_bitrates =
-			ARRAY_SIZE(ath9k_legacy_rates);
-	}
-
-	if (sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_5GHZ) {
-		channels = devm_kzalloc(sc->dev,
-			sizeof(ath9k_5ghz_chantable), GFP_KERNEL);
-		if (!channels)
-			return -ENOMEM;
-
-		memcpy(channels, ath9k_5ghz_chantable,
-		       sizeof(ath9k_5ghz_chantable));
-		sc->sbands[IEEE80211_BAND_5GHZ].channels = channels;
-		sc->sbands[IEEE80211_BAND_5GHZ].band = IEEE80211_BAND_5GHZ;
-		sc->sbands[IEEE80211_BAND_5GHZ].n_channels =
-			ARRAY_SIZE(ath9k_5ghz_chantable);
-		sc->sbands[IEEE80211_BAND_5GHZ].bitrates =
-			ath9k_legacy_rates + 4;
-		sc->sbands[IEEE80211_BAND_5GHZ].n_bitrates =
-			ARRAY_SIZE(ath9k_legacy_rates) - 4;
-	}
-	return 0;
-}
-
 static void ath9k_init_misc(struct ath_softc *sc)
 {
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
@@ -534,7 +330,7 @@ static void ath9k_init_misc(struct ath_softc *sc)
 
 	setup_timer(&common->ani.timer, ath_ani_calibrate, (unsigned long)sc);
 
-	sc->last_rssi = ATH_RSSI_DUMMY_MARKER;
+	common->last_rssi = ATH_RSSI_DUMMY_MARKER;
 	sc->config.txpowlimit = ATH_TXPOWER_MAX;
 	memcpy(common->bssidmask, ath_bcast_mac, ETH_ALEN);
 	sc->beacon.slottime = ATH9K_SLOT_TIME_9;
@@ -789,7 +585,7 @@ static int ath9k_init_softc(u16 devid, struct ath_softc *sc,
 	if (ret)
 		goto err_btcoex;
 
-	ret = ath9k_init_channels_rates(sc);
+	ret = ath9k_cmn_init_channels_rates(common);
 	if (ret)
 		goto err_btcoex;
 
@@ -819,10 +615,11 @@ static void ath9k_init_band_txpower(struct ath_softc *sc, int band)
 	struct ieee80211_supported_band *sband;
 	struct ieee80211_channel *chan;
 	struct ath_hw *ah = sc->sc_ah;
+	struct ath_common *common = ath9k_hw_common(ah);
 	struct cfg80211_chan_def chandef;
 	int i;
 
-	sband = &sc->sbands[band];
+	sband = &common->sbands[band];
 	for (i = 0; i < sband->n_channels; i++) {
 		chan = &sband->channels[i];
 		ah->curchan = &ah->channels[chan->hw_value];
@@ -845,17 +642,6 @@ static void ath9k_init_txpower_limits(struct ath_softc *sc)
 	ah->curchan = curchan;
 }
 
-void ath9k_reload_chainmask_settings(struct ath_softc *sc)
-{
-	if (!(sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_HT))
-		return;
-
-	if (sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_2GHZ)
-		setup_ht_cap(sc, &sc->sbands[IEEE80211_BAND_2GHZ].ht_cap);
-	if (sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_5GHZ)
-		setup_ht_cap(sc, &sc->sbands[IEEE80211_BAND_5GHZ].ht_cap);
-}
-
 static const struct ieee80211_iface_limit if_limits[] = {
 	{ .max = 2048,	.types = BIT(NL80211_IFTYPE_STATION) |
 				 BIT(NL80211_IFTYPE_P2P_CLIENT) |
@@ -903,13 +689,15 @@ static void ath9k_set_hw_capab(struct ath_softc *sc, struct ieee80211_hw *hw)
 	hw->flags = IEEE80211_HW_RX_INCLUDES_FCS |
 		IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING |
 		IEEE80211_HW_SIGNAL_DBM |
-		IEEE80211_HW_SUPPORTS_PS |
 		IEEE80211_HW_PS_NULLFUNC_STACK |
 		IEEE80211_HW_SPECTRUM_MGMT |
 		IEEE80211_HW_REPORTS_TX_ACK_STATUS |
 		IEEE80211_HW_SUPPORTS_RC_TABLE |
 		IEEE80211_HW_SUPPORTS_HT_CCK_RATES;
 
+	if (ath9k_ps_enable)
+		hw->flags |= IEEE80211_HW_SUPPORTS_PS;
+
 	if (sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_HT) {
 		hw->flags |= IEEE80211_HW_AMPDU_AGGREGATION;
 
@@ -943,6 +731,7 @@ static void ath9k_set_hw_capab(struct ath_softc *sc, struct ieee80211_hw *hw)
 	hw->wiphy->flags |= WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL;
 	hw->wiphy->flags |= WIPHY_FLAG_SUPPORTS_5_10_MHZ;
 	hw->wiphy->flags |= WIPHY_FLAG_HAS_CHANNEL_SWITCH;
+	hw->wiphy->flags |= WIPHY_FLAG_AP_UAPSD;
 
 	hw->queues = 4;
 	hw->max_rates = 4;
@@ -963,13 +752,13 @@ static void ath9k_set_hw_capab(struct ath_softc *sc, struct ieee80211_hw *hw)
 
 	if (sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_2GHZ)
 		hw->wiphy->bands[IEEE80211_BAND_2GHZ] =
-			&sc->sbands[IEEE80211_BAND_2GHZ];
+			&common->sbands[IEEE80211_BAND_2GHZ];
 	if (sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_5GHZ)
 		hw->wiphy->bands[IEEE80211_BAND_5GHZ] =
-			&sc->sbands[IEEE80211_BAND_5GHZ];
+			&common->sbands[IEEE80211_BAND_5GHZ];
 
 	ath9k_init_wow(hw);
-	ath9k_reload_chainmask_settings(sc);
+	ath9k_cmn_reload_chainmask(ah);
 
 	SET_IEEE80211_PERM_ADDR(hw, common->macaddr);
 }
@@ -1100,19 +889,11 @@ static int __init ath9k_init(void)
 {
 	int error;
 
-	/* Register rate control algorithm */
-	error = ath_rate_control_register();
-	if (error != 0) {
-		pr_err("Unable to register rate control algorithm: %d\n",
-		       error);
-		goto err_out;
-	}
-
 	error = ath_pci_init();
 	if (error < 0) {
 		pr_err("No PCI devices found, driver not installed\n");
 		error = -ENODEV;
-		goto err_rate_unregister;
+		goto err_out;
 	}
 
 	error = ath_ahb_init();
@@ -1125,9 +906,6 @@ static int __init ath9k_init(void)
 
  err_pci_exit:
 	ath_pci_exit();
-
- err_rate_unregister:
-	ath_rate_control_unregister();
  err_out:
 	return error;
 }
@@ -1138,7 +916,6 @@ static void __exit ath9k_exit(void)
 	is_ath9k_unloaded = true;
 	ath_ahb_exit();
 	ath_pci_exit();
-	ath_rate_control_unregister();
 	pr_info("%s: Driver unloaded\n", dev_info);
 }
 module_exit(ath9k_exit);
diff --git a/drivers/net/wireless/ath/ath9k/mac.h b/drivers/net/wireless/ath/ath9k/mac.h
index 10271373a0cd..89df634e81f9 100644
--- a/drivers/net/wireless/ath/ath9k/mac.h
+++ b/drivers/net/wireless/ath/ath9k/mac.h
@@ -155,12 +155,8 @@ struct ath_htc_rx_status {
 	u8 rs_status;
 	u8 rs_phyerr;
 	int8_t rs_rssi;
-	int8_t rs_rssi_ctl0;
-	int8_t rs_rssi_ctl1;
-	int8_t rs_rssi_ctl2;
-	int8_t rs_rssi_ext0;
-	int8_t rs_rssi_ext1;
-	int8_t rs_rssi_ext2;
+	int8_t rs_rssi_ctl[3];
+	int8_t rs_rssi_ext[3];
 	u8 rs_keyix;
 	u8 rs_rate;
 	u8 rs_antenna;
@@ -170,6 +166,7 @@ struct ath_htc_rx_status {
 	u8 rs_num_delims;
 	u8 rs_flags;
 	u8 rs_dummy;
+	/* FIXME: evm* never used? */
 	__be32 evm0;
 	__be32 evm1;
 	__be32 evm2;
diff --git a/drivers/net/wireless/ath/ath9k/main.c b/drivers/net/wireless/ath/ath9k/main.c
index 5924f72dd493..42a18037004e 100644
--- a/drivers/net/wireless/ath/ath9k/main.c
+++ b/drivers/net/wireless/ath/ath9k/main.c
@@ -451,7 +451,7 @@ void ath9k_tasklet(unsigned long data)
 		 * interrupts are enabled in the reset routine.
 		 */
 		atomic_inc(&ah->intr_ref_cnt);
-		ath_dbg(common, ANY, "FATAL: Skipping interrupts\n");
+		ath_dbg(common, RESET, "FATAL: Skipping interrupts\n");
 		goto out;
 	}
 
@@ -471,7 +471,7 @@ void ath9k_tasklet(unsigned long data)
 			 * interrupts are enabled in the reset routine.
 			 */
 			atomic_inc(&ah->intr_ref_cnt);
-			ath_dbg(common, ANY,
+			ath_dbg(common, RESET,
 				"BB_WATCHDOG: Skipping interrupts\n");
 			goto out;
 		}
@@ -484,7 +484,7 @@ void ath9k_tasklet(unsigned long data)
 			type = RESET_TYPE_TX_GTT;
 			ath9k_queue_reset(sc, type);
 			atomic_inc(&ah->intr_ref_cnt);
-			ath_dbg(common, ANY,
+			ath_dbg(common, RESET,
 				"GTT: Skipping interrupts\n");
 			goto out;
 		}
@@ -1178,9 +1178,6 @@ static void ath9k_remove_interface(struct ieee80211_hw *hw,
 	if (ath9k_uses_beacons(vif->type))
 		ath9k_beacon_remove_slot(sc, vif);
 
-	if (sc->csa_vif == vif)
-		sc->csa_vif = NULL;
-
 	ath9k_ps_wakeup(sc);
 	ath9k_calculate_summary_state(hw, NULL);
 	ath9k_ps_restore(sc);
@@ -1609,7 +1606,7 @@ static void ath9k_set_assoc_state(struct ath_softc *sc,
 	common->curaid = bss_conf->aid;
 	ath9k_hw_write_associd(sc->sc_ah);
 
-	sc->last_rssi = ATH_RSSI_DUMMY_MARKER;
+	common->last_rssi = ATH_RSSI_DUMMY_MARKER;
 	sc->sc_ah->stats.avgbrssi = ATH_RSSI_DUMMY_MARKER;
 
 	spin_lock_irqsave(&sc->sc_pm_lock, flags);
@@ -1866,7 +1863,7 @@ static void ath9k_set_coverage_class(struct ieee80211_hw *hw, u8 coverage_class)
 
 static bool ath9k_has_tx_pending(struct ath_softc *sc)
 {
-	int i, npend;
+	int i, npend = 0;
 
 	for (i = 0; i < ATH9K_NUM_TX_QUEUES; i++) {
 		if (!ATH_TXQ_SETUP(sc, i))
@@ -2056,7 +2053,7 @@ static int ath9k_set_antenna(struct ieee80211_hw *hw, u32 tx_ant, u32 rx_ant)
 		ah->rxchainmask = fill_chainmask(ah->caps.rx_chainmask, rx_ant);
 
 	ah->txchainmask = fill_chainmask(ah->caps.tx_chainmask, tx_ant);
-	ath9k_reload_chainmask_settings(sc);
+	ath9k_cmn_reload_chainmask(ah);
 
 	return 0;
 }
@@ -2086,13 +2083,8 @@ static void ath9k_channel_switch_beacon(struct ieee80211_hw *hw,
 					struct ieee80211_vif *vif,
 					struct cfg80211_chan_def *chandef)
 {
-	struct ath_softc *sc = hw->priv;
-
-	/* mac80211 does not support CSA in multi-if cases (yet) */
-	if (WARN_ON(sc->csa_vif))
-		return;
-
-	sc->csa_vif = vif;
+	/* depend on vif->csa_active only */
+	return;
 }
 
 struct ieee80211_ops ath9k_ops = {
diff --git a/drivers/net/wireless/ath/ath9k/rc.c b/drivers/net/wireless/ath/ath9k/rc.c
deleted file mode 100644
index d829bb62a3fc..000000000000
--- a/drivers/net/wireless/ath/ath9k/rc.c
+++ /dev/null
@@ -1,1495 +0,0 @@
-/*
- * Copyright (c) 2004 Video54 Technologies, Inc.
- * Copyright (c) 2004-2011 Atheros Communications, Inc.
- *
- * Permission to use, copy, modify, and/or distribute this software for any
- * purpose with or without fee is hereby granted, provided that the above
- * copyright notice and this permission notice appear in all copies.
- *
- * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
- * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
- * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
- * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
- * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
- * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
- * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
- */
-
-#include <linux/slab.h>
-#include <linux/export.h>
-
-#include "ath9k.h"
-
-static const struct ath_rate_table ar5416_11na_ratetable = {
-	68,
-	8, /* MCS start */
-	{
-		[0] = { RC_L_SDT, WLAN_RC_PHY_OFDM, 6000,
-			5400, 0, 12 }, /* 6 Mb */
-		[1] = { RC_L_SDT, WLAN_RC_PHY_OFDM, 9000,
-			7800,  1, 18 }, /* 9 Mb */
-		[2] = { RC_L_SDT, WLAN_RC_PHY_OFDM, 12000,
-			10000, 2, 24 }, /* 12 Mb */
-		[3] = { RC_L_SDT, WLAN_RC_PHY_OFDM, 18000,
-			13900, 3, 36 }, /* 18 Mb */
-		[4] = { RC_L_SDT, WLAN_RC_PHY_OFDM, 24000,
-			17300, 4, 48 }, /* 24 Mb */
-		[5] = { RC_L_SDT, WLAN_RC_PHY_OFDM, 36000,
-			23000, 5, 72 }, /* 36 Mb */
-		[6] = { RC_L_SDT, WLAN_RC_PHY_OFDM, 48000,
-			27400, 6, 96 }, /* 48 Mb */
-		[7] = { RC_L_SDT, WLAN_RC_PHY_OFDM, 54000,
-			29300, 7, 108 }, /* 54 Mb */
-		[8] = { RC_HT_SDT_2040, WLAN_RC_PHY_HT_20_SS, 6500,
-			6400, 0, 0 }, /* 6.5 Mb */
-		[9] = { RC_HT_SDT_20, WLAN_RC_PHY_HT_20_SS, 13000,
-			12700, 1, 1 }, /* 13 Mb */
-		[10] = { RC_HT_SDT_20, WLAN_RC_PHY_HT_20_SS, 19500,
-			18800, 2, 2 }, /* 19.5 Mb */
-		[11] = { RC_HT_SD_20, WLAN_RC_PHY_HT_20_SS, 26000,
-			25000, 3, 3 }, /* 26 Mb */
-		[12] = { RC_HT_SD_20, WLAN_RC_PHY_HT_20_SS, 39000,
-			36700, 4, 4 }, /* 39 Mb */
-		[13] = { RC_HT_S_20, WLAN_RC_PHY_HT_20_SS, 52000,
-			48100, 5, 5 }, /* 52 Mb */
-		[14] = { RC_HT_S_20, WLAN_RC_PHY_HT_20_SS, 58500,
-			53500, 6, 6 }, /* 58.5 Mb */
-		[15] = { RC_HT_S_20, WLAN_RC_PHY_HT_20_SS, 65000,
-			59000, 7, 7 }, /* 65 Mb */
-		[16] = { RC_HT_S_20, WLAN_RC_PHY_HT_20_SS_HGI, 72200,
-			65400, 7, 7 }, /* 75 Mb */
-		[17] = { RC_INVALID, WLAN_RC_PHY_HT_20_DS, 13000,
-			12700, 8, 8 }, /* 13 Mb */
-		[18] = { RC_HT_T_20, WLAN_RC_PHY_HT_20_DS, 26000,
-			24800, 9, 9 }, /* 26 Mb */
-		[19] = { RC_HT_T_20, WLAN_RC_PHY_HT_20_DS, 39000,
-			36600, 10, 10 }, /* 39 Mb */
-		[20] = { RC_HT_DT_20, WLAN_RC_PHY_HT_20_DS, 52000,
-			48100, 11, 11 }, /* 52 Mb */
-		[21] = { RC_HT_DT_20, WLAN_RC_PHY_HT_20_DS, 78000,
-			69500, 12, 12 }, /* 78 Mb */
-		[22] = { RC_HT_DT_20, WLAN_RC_PHY_HT_20_DS, 104000,
-			89500, 13, 13 }, /* 104 Mb */
-		[23] = { RC_HT_DT_20, WLAN_RC_PHY_HT_20_DS, 117000,
-			98900, 14, 14 }, /* 117 Mb */
-		[24] = { RC_HT_DT_20, WLAN_RC_PHY_HT_20_DS, 130000,
-			108300, 15, 15 }, /* 130 Mb */
-		[25] = { RC_HT_DT_20, WLAN_RC_PHY_HT_20_DS_HGI, 144400,
-			120000, 15, 15 }, /* 144.4 Mb */
-		[26] = {  RC_INVALID, WLAN_RC_PHY_HT_20_TS, 19500,
-			17400, 16, 16 }, /* 19.5 Mb */
-		[27] = {  RC_INVALID, WLAN_RC_PHY_HT_20_TS, 39000,
-			35100, 17, 17 }, /* 39 Mb */
-		[28] = {  RC_INVALID, WLAN_RC_PHY_HT_20_TS, 58500,
-			52600, 18, 18 }, /* 58.5 Mb */
-		[29] = {  RC_INVALID, WLAN_RC_PHY_HT_20_TS, 78000,
-			70400, 19, 19 }, /* 78 Mb */
-		[30] = {  RC_INVALID, WLAN_RC_PHY_HT_20_TS, 117000,
-			104900, 20, 20 }, /* 117 Mb */
-		[31] = {  RC_INVALID, WLAN_RC_PHY_HT_20_TS_HGI, 130000,
-			115800, 20, 20 }, /* 130 Mb*/
-		[32] = {  RC_HT_T_20, WLAN_RC_PHY_HT_20_TS, 156000,
-			137200, 21, 21 }, /* 156 Mb */
-		[33] = {  RC_HT_T_20, WLAN_RC_PHY_HT_20_TS_HGI, 173300,
-			151100, 21, 21 }, /* 173.3 Mb */
-		[34] = {  RC_HT_T_20, WLAN_RC_PHY_HT_20_TS, 175500,
-			152800, 22, 22 }, /* 175.5 Mb */
-		[35] = {  RC_HT_T_20, WLAN_RC_PHY_HT_20_TS_HGI, 195000,
-			168400, 22, 22 }, /* 195 Mb*/
-		[36] = {  RC_HT_T_20, WLAN_RC_PHY_HT_20_TS, 195000,
-			168400, 23, 23 }, /* 195 Mb */
-		[37] = {  RC_HT_T_20, WLAN_RC_PHY_HT_20_TS_HGI, 216700,
-			185000, 23, 23 }, /* 216.7 Mb */
-		[38] = { RC_HT_SDT_40, WLAN_RC_PHY_HT_40_SS, 13500,
-			13200, 0, 0 }, /* 13.5 Mb*/
-		[39] = { RC_HT_SDT_40, WLAN_RC_PHY_HT_40_SS, 27500,
-			25900, 1, 1 }, /* 27.0 Mb*/
-		[40] = { RC_HT_SDT_40, WLAN_RC_PHY_HT_40_SS, 40500,
-			38600, 2, 2 }, /* 40.5 Mb*/
-		[41] = { RC_HT_SD_40, WLAN_RC_PHY_HT_40_SS, 54000,
-			49800, 3, 3 }, /* 54 Mb */
-		[42] = { RC_HT_SD_40, WLAN_RC_PHY_HT_40_SS, 81500,
-			72200, 4, 4 }, /* 81 Mb */
-		[43] = { RC_HT_S_40, WLAN_RC_PHY_HT_40_SS, 108000,
-			92900, 5, 5 }, /* 108 Mb */
-		[44] = { RC_HT_S_40, WLAN_RC_PHY_HT_40_SS, 121500,
-			102700, 6, 6 }, /* 121.5 Mb*/
-		[45] = { RC_HT_S_40, WLAN_RC_PHY_HT_40_SS, 135000,
-			112000, 7, 7 }, /* 135 Mb */
-		[46] = { RC_HT_S_40, WLAN_RC_PHY_HT_40_SS_HGI, 150000,
-			122000, 7, 7 }, /* 150 Mb */
-		[47] = { RC_INVALID, WLAN_RC_PHY_HT_40_DS, 27000,
-			25800, 8, 8 }, /* 27 Mb */
-		[48] = { RC_HT_T_40, WLAN_RC_PHY_HT_40_DS, 54000,
-			49800, 9, 9 }, /* 54 Mb */
-		[49] = { RC_HT_T_40, WLAN_RC_PHY_HT_40_DS, 81000,
-			71900, 10, 10 }, /* 81 Mb */
-		[50] = { RC_HT_DT_40, WLAN_RC_PHY_HT_40_DS, 108000,
-			92500, 11, 11 }, /* 108 Mb */
-		[51] = { RC_HT_DT_40, WLAN_RC_PHY_HT_40_DS, 162000,
-			130300, 12, 12 }, /* 162 Mb */
-		[52] = { RC_HT_DT_40, WLAN_RC_PHY_HT_40_DS, 216000,
-			162800, 13, 13 }, /* 216 Mb */
-		[53] = { RC_HT_DT_40, WLAN_RC_PHY_HT_40_DS, 243000,
-			178200, 14, 14 }, /* 243 Mb */
-		[54] = { RC_HT_DT_40, WLAN_RC_PHY_HT_40_DS, 270000,
-			192100, 15, 15 }, /* 270 Mb */
-		[55] = { RC_HT_DT_40, WLAN_RC_PHY_HT_40_DS_HGI, 300000,
-			207000, 15, 15 }, /* 300 Mb */
-		[56] = {  RC_INVALID, WLAN_RC_PHY_HT_40_TS, 40500,
-			36100, 16, 16 }, /* 40.5 Mb */
-		[57] = {  RC_INVALID, WLAN_RC_PHY_HT_40_TS, 81000,
-			72900, 17, 17 }, /* 81 Mb */
-		[58] = {  RC_INVALID, WLAN_RC_PHY_HT_40_TS, 121500,
-			108300, 18, 18 }, /* 121.5 Mb */
-		[59] = {  RC_INVALID, WLAN_RC_PHY_HT_40_TS, 162000,
-			142000, 19, 19 }, /*  162 Mb */
-		[60] = {  RC_INVALID, WLAN_RC_PHY_HT_40_TS, 243000,
-			205100, 20, 20 }, /*  243 Mb */
-		[61] = {  RC_INVALID, WLAN_RC_PHY_HT_40_TS_HGI, 270000,
-			224700, 20, 20 }, /*  270 Mb */
-		[62] = {  RC_HT_T_40, WLAN_RC_PHY_HT_40_TS, 324000,
-			263100, 21, 21 }, /*  324 Mb */
-		[63] = {  RC_HT_T_40, WLAN_RC_PHY_HT_40_TS_HGI, 360000,
-			288000, 21, 21 }, /*  360 Mb */
-		[64] = {  RC_HT_T_40, WLAN_RC_PHY_HT_40_TS, 364500,
-			290700, 22, 22 }, /* 364.5 Mb */
-		[65] = {  RC_HT_T_40, WLAN_RC_PHY_HT_40_TS_HGI, 405000,
-			317200, 22, 22 }, /* 405 Mb */
-		[66] = {  RC_HT_T_40, WLAN_RC_PHY_HT_40_TS, 405000,
-			317200, 23, 23 }, /* 405 Mb */
-		[67] = {  RC_HT_T_40, WLAN_RC_PHY_HT_40_TS_HGI, 450000,
-			346400, 23, 23 }, /* 450 Mb */
-	},
-	50,  /* probe interval */
-	WLAN_RC_HT_FLAG,  /* Phy rates allowed initially */
-};
-
-/* 4ms frame limit not used for NG mode.  The values filled
- * for HT are the 64K max aggregate limit */
-
-static const struct ath_rate_table ar5416_11ng_ratetable = {
-	72,
-	12, /* MCS start */
-	{
-		[0] = { RC_ALL, WLAN_RC_PHY_CCK, 1000,
-			900, 0, 2 }, /* 1 Mb */
-		[1] = { RC_ALL, WLAN_RC_PHY_CCK, 2000,
-			1900, 1, 4 }, /* 2 Mb */
-		[2] = { RC_ALL, WLAN_RC_PHY_CCK, 5500,
-			4900, 2, 11 }, /* 5.5 Mb */
-		[3] = { RC_ALL, WLAN_RC_PHY_CCK, 11000,
-			8100, 3, 22 }, /* 11 Mb */
-		[4] = { RC_INVALID, WLAN_RC_PHY_OFDM, 6000,
-			5400, 4, 12 }, /* 6 Mb */
-		[5] = { RC_INVALID, WLAN_RC_PHY_OFDM, 9000,
-			7800, 5, 18 }, /* 9 Mb */
-		[6] = { RC_L_SDT, WLAN_RC_PHY_OFDM, 12000,
-			10100, 6, 24 }, /* 12 Mb */
-		[7] = { RC_L_SDT, WLAN_RC_PHY_OFDM, 18000,
-			14100, 7, 36 }, /* 18 Mb */
-		[8] = { RC_L_SDT, WLAN_RC_PHY_OFDM, 24000,
-			17700, 8, 48 }, /* 24 Mb */
-		[9] = { RC_L_SDT, WLAN_RC_PHY_OFDM, 36000,
-			23700, 9, 72 }, /* 36 Mb */
-		[10] = { RC_L_SDT, WLAN_RC_PHY_OFDM, 48000,
-			27400, 10, 96 }, /* 48 Mb */
-		[11] = { RC_L_SDT, WLAN_RC_PHY_OFDM, 54000,
-			30900, 11, 108 }, /* 54 Mb */
-		[12] = { RC_INVALID, WLAN_RC_PHY_HT_20_SS, 6500,
-			6400, 0, 0 }, /* 6.5 Mb */
-		[13] = { RC_HT_SDT_20, WLAN_RC_PHY_HT_20_SS, 13000,
-			12700, 1, 1 }, /* 13 Mb */
-		[14] = { RC_HT_SDT_20, WLAN_RC_PHY_HT_20_SS, 19500,
-			18800, 2, 2 }, /* 19.5 Mb*/
-		[15] = { RC_HT_SD_20, WLAN_RC_PHY_HT_20_SS, 26000,
-			25000, 3, 3 }, /* 26 Mb */
-		[16] = { RC_HT_SD_20, WLAN_RC_PHY_HT_20_SS, 39000,
-			36700, 4, 4 }, /* 39 Mb */
-		[17] = { RC_HT_S_20, WLAN_RC_PHY_HT_20_SS, 52000,
-			48100, 5, 5 }, /* 52 Mb */
-		[18] = { RC_HT_S_20, WLAN_RC_PHY_HT_20_SS, 58500,
-			53500, 6, 6 }, /* 58.5 Mb */
-		[19] = { RC_HT_S_20, WLAN_RC_PHY_HT_20_SS, 65000,
-			59000, 7, 7 }, /* 65 Mb */
-		[20] = { RC_HT_S_20, WLAN_RC_PHY_HT_20_SS_HGI, 72200,
-			65400, 7, 7 }, /* 65 Mb*/
-		[21] = { RC_INVALID, WLAN_RC_PHY_HT_20_DS, 13000,
-			12700, 8, 8 }, /* 13 Mb */
-		[22] = { RC_HT_T_20, WLAN_RC_PHY_HT_20_DS, 26000,
-			24800, 9, 9 }, /* 26 Mb */
-		[23] = { RC_HT_T_20, WLAN_RC_PHY_HT_20_DS, 39000,
-			36600, 10, 10 }, /* 39 Mb */
-		[24] = { RC_HT_DT_20, WLAN_RC_PHY_HT_20_DS, 52000,
-			48100, 11, 11 }, /* 52 Mb */
-		[25] = { RC_HT_DT_20, WLAN_RC_PHY_HT_20_DS, 78000,
-			69500, 12, 12 }, /* 78 Mb */
-		[26] = { RC_HT_DT_20, WLAN_RC_PHY_HT_20_DS, 104000,
-			89500, 13, 13 }, /* 104 Mb */
-		[27] = { RC_HT_DT_20, WLAN_RC_PHY_HT_20_DS, 117000,
-			98900, 14, 14 }, /* 117 Mb */
-		[28] = { RC_HT_DT_20, WLAN_RC_PHY_HT_20_DS, 130000,
-			108300, 15, 15 }, /* 130 Mb */
-		[29] = { RC_HT_DT_20, WLAN_RC_PHY_HT_20_DS_HGI, 144400,
-			120000, 15, 15 }, /* 144.4 Mb */
-		[30] = {  RC_INVALID, WLAN_RC_PHY_HT_20_TS, 19500,
-			17400, 16, 16 }, /* 19.5 Mb */
-		[31] = {  RC_INVALID, WLAN_RC_PHY_HT_20_TS, 39000,
-			35100, 17, 17 }, /* 39 Mb */
-		[32] = {  RC_INVALID, WLAN_RC_PHY_HT_20_TS, 58500,
-			52600, 18, 18 }, /* 58.5 Mb */
-		[33] = {  RC_INVALID, WLAN_RC_PHY_HT_20_TS, 78000,
-			70400, 19, 19 }, /* 78 Mb */
-		[34] = {  RC_INVALID, WLAN_RC_PHY_HT_20_TS, 117000,
-			104900, 20, 20 }, /* 117 Mb */
-		[35] = {  RC_INVALID, WLAN_RC_PHY_HT_20_TS_HGI, 130000,
-			115800, 20, 20 }, /* 130 Mb */
-		[36] = {  RC_HT_T_20, WLAN_RC_PHY_HT_20_TS, 156000,
-			137200, 21, 21 }, /* 156 Mb */
-		[37] = {  RC_HT_T_20, WLAN_RC_PHY_HT_20_TS_HGI, 173300,
-			151100, 21, 21 }, /* 173.3 Mb */
-		[38] = {  RC_HT_T_20, WLAN_RC_PHY_HT_20_TS, 175500,
-			152800, 22, 22 }, /* 175.5 Mb */
-		[39] = {  RC_HT_T_20, WLAN_RC_PHY_HT_20_TS_HGI, 195000,
-			168400, 22, 22 }, /* 195 Mb */
-		[40] = {  RC_HT_T_20, WLAN_RC_PHY_HT_20_TS, 195000,
-			168400, 23, 23 }, /* 195 Mb */
-		[41] = {  RC_HT_T_20, WLAN_RC_PHY_HT_20_TS_HGI, 216700,
-			185000, 23, 23 }, /* 216.7 Mb */
-		[42] = { RC_HT_SDT_40, WLAN_RC_PHY_HT_40_SS, 13500,
-			13200, 0, 0 }, /* 13.5 Mb */
-		[43] = { RC_HT_SDT_40, WLAN_RC_PHY_HT_40_SS, 27500,
-			25900, 1, 1 }, /* 27.0 Mb */
-		[44] = { RC_HT_SDT_40, WLAN_RC_PHY_HT_40_SS, 40500,
-			38600, 2, 2 }, /* 40.5 Mb */
-		[45] = { RC_HT_SD_40, WLAN_RC_PHY_HT_40_SS, 54000,
-			49800, 3, 3 }, /* 54 Mb */
-		[46] = { RC_HT_SD_40, WLAN_RC_PHY_HT_40_SS, 81500,
-			72200, 4, 4 }, /* 81 Mb */
-		[47] = { RC_HT_S_40 , WLAN_RC_PHY_HT_40_SS, 108000,
-			92900, 5, 5 }, /* 108 Mb */
-		[48] = { RC_HT_S_40, WLAN_RC_PHY_HT_40_SS, 121500,
-			102700, 6, 6 }, /* 121.5 Mb */
-		[49] = { RC_HT_S_40, WLAN_RC_PHY_HT_40_SS, 135000,
-			112000, 7, 7 }, /* 135 Mb */
-		[50] = { RC_HT_S_40, WLAN_RC_PHY_HT_40_SS_HGI, 150000,
-			122000, 7, 7 }, /* 150 Mb */
-		[51] = { RC_INVALID, WLAN_RC_PHY_HT_40_DS, 27000,
-			25800, 8, 8 }, /* 27 Mb */
-		[52] = { RC_HT_T_40, WLAN_RC_PHY_HT_40_DS, 54000,
-			49800, 9, 9 }, /* 54 Mb */
-		[53] = { RC_HT_T_40, WLAN_RC_PHY_HT_40_DS, 81000,
-			71900, 10, 10 }, /* 81 Mb */
-		[54] = { RC_HT_DT_40, WLAN_RC_PHY_HT_40_DS, 108000,
-			92500, 11, 11 }, /* 108 Mb */
-		[55] = { RC_HT_DT_40, WLAN_RC_PHY_HT_40_DS, 162000,
-			130300, 12, 12 }, /* 162 Mb */
-		[56] = { RC_HT_DT_40, WLAN_RC_PHY_HT_40_DS, 216000,
-			162800, 13, 13 }, /* 216 Mb */
-		[57] = { RC_HT_DT_40, WLAN_RC_PHY_HT_40_DS, 243000,
-			178200, 14, 14 }, /* 243 Mb */
-		[58] = { RC_HT_DT_40, WLAN_RC_PHY_HT_40_DS, 270000,
-			192100, 15, 15 }, /* 270 Mb */
-		[59] = { RC_HT_DT_40, WLAN_RC_PHY_HT_40_DS_HGI, 300000,
-			207000, 15, 15 }, /* 300 Mb */
-		[60] = {  RC_INVALID, WLAN_RC_PHY_HT_40_TS, 40500,
-			36100, 16, 16 }, /* 40.5 Mb */
-		[61] = {  RC_INVALID, WLAN_RC_PHY_HT_40_TS, 81000,
-			72900, 17, 17 }, /* 81 Mb */
-		[62] = {  RC_INVALID, WLAN_RC_PHY_HT_40_TS, 121500,
-			108300, 18, 18 }, /* 121.5 Mb */
-		[63] = {  RC_INVALID, WLAN_RC_PHY_HT_40_TS, 162000,
-			142000, 19, 19 }, /* 162 Mb */
-		[64] = {  RC_INVALID, WLAN_RC_PHY_HT_40_TS, 243000,
-			205100, 20, 20 }, /* 243 Mb */
-		[65] = {  RC_INVALID, WLAN_RC_PHY_HT_40_TS_HGI, 270000,
-			224700, 20, 20 }, /* 270 Mb */
-		[66] = {  RC_HT_T_40, WLAN_RC_PHY_HT_40_TS, 324000,
-			263100, 21, 21 }, /* 324 Mb */
-		[67] = {  RC_HT_T_40, WLAN_RC_PHY_HT_40_TS_HGI, 360000,
-			288000, 21, 21 }, /* 360 Mb */
-		[68] = {  RC_HT_T_40, WLAN_RC_PHY_HT_40_TS, 364500,
-			290700, 22, 22 }, /* 364.5 Mb */
-		[69] = {  RC_HT_T_40, WLAN_RC_PHY_HT_40_TS_HGI, 405000,
-			317200, 22, 22 }, /* 405 Mb */
-		[70] = {  RC_HT_T_40, WLAN_RC_PHY_HT_40_TS, 405000,
-			317200, 23, 23 }, /* 405 Mb */
-		[71] = {  RC_HT_T_40, WLAN_RC_PHY_HT_40_TS_HGI, 450000,
-			346400, 23, 23 }, /* 450 Mb */
-	},
-	50,  /* probe interval */
-	WLAN_RC_HT_FLAG,  /* Phy rates allowed initially */
-};
-
-static const struct ath_rate_table ar5416_11a_ratetable = {
-	8,
-	0,
-	{
-		{ RC_L_SDT, WLAN_RC_PHY_OFDM, 6000, /* 6 Mb */
-			5400, 0, 12},
-		{ RC_L_SDT, WLAN_RC_PHY_OFDM, 9000, /* 9 Mb */
-			7800,  1, 18},
-		{ RC_L_SDT, WLAN_RC_PHY_OFDM, 12000, /* 12 Mb */
-			10000, 2, 24},
-		{ RC_L_SDT, WLAN_RC_PHY_OFDM, 18000, /* 18 Mb */
-			13900, 3, 36},
-		{ RC_L_SDT, WLAN_RC_PHY_OFDM, 24000, /* 24 Mb */
-			17300, 4, 48},
-		{ RC_L_SDT, WLAN_RC_PHY_OFDM, 36000, /* 36 Mb */
-			23000, 5, 72},
-		{ RC_L_SDT, WLAN_RC_PHY_OFDM, 48000, /* 48 Mb */
-			27400, 6, 96},
-		{ RC_L_SDT, WLAN_RC_PHY_OFDM, 54000, /* 54 Mb */
-			29300, 7, 108},
-	},
-	50,  /* probe interval */
-	0,   /* Phy rates allowed initially */
-};
-
-static const struct ath_rate_table ar5416_11g_ratetable = {
-	12,
-	0,
-	{
-		{ RC_L_SDT, WLAN_RC_PHY_CCK, 1000, /* 1 Mb */
-			900, 0, 2},
-		{ RC_L_SDT, WLAN_RC_PHY_CCK, 2000, /* 2 Mb */
-			1900, 1, 4},
-		{ RC_L_SDT, WLAN_RC_PHY_CCK, 5500, /* 5.5 Mb */
-			4900, 2, 11},
-		{ RC_L_SDT, WLAN_RC_PHY_CCK, 11000, /* 11 Mb */
-			8100, 3, 22},
-		{ RC_INVALID, WLAN_RC_PHY_OFDM, 6000, /* 6 Mb */
-			5400, 4, 12},
-		{ RC_INVALID, WLAN_RC_PHY_OFDM, 9000, /* 9 Mb */
-			7800, 5, 18},
-		{ RC_L_SDT, WLAN_RC_PHY_OFDM, 12000, /* 12 Mb */
-			10000, 6, 24},
-		{ RC_L_SDT, WLAN_RC_PHY_OFDM, 18000, /* 18 Mb */
-			13900, 7, 36},
-		{ RC_L_SDT, WLAN_RC_PHY_OFDM, 24000, /* 24 Mb */
-			17300, 8, 48},
-		{ RC_L_SDT, WLAN_RC_PHY_OFDM, 36000, /* 36 Mb */
-			23000, 9, 72},
-		{ RC_L_SDT, WLAN_RC_PHY_OFDM, 48000, /* 48 Mb */
-			27400, 10, 96},
-		{ RC_L_SDT, WLAN_RC_PHY_OFDM, 54000, /* 54 Mb */
-			29300, 11, 108},
-	},
-	50,  /* probe interval */
-	0,   /* Phy rates allowed initially */
-};
-
-static int ath_rc_get_rateindex(struct ath_rate_priv *ath_rc_priv,
-				struct ieee80211_tx_rate *rate)
-{
-	const struct ath_rate_table *rate_table = ath_rc_priv->rate_table;
-	int rix, i, idx = 0;
-
-	if (!(rate->flags & IEEE80211_TX_RC_MCS))
-		return rate->idx;
-
-	for (i = 0; i < ath_rc_priv->max_valid_rate; i++) {
-		idx = ath_rc_priv->valid_rate_index[i];
-
-		if (WLAN_RC_PHY_HT(rate_table->info[idx].phy) &&
-		    rate_table->info[idx].ratecode == rate->idx)
-			break;
-	}
-
-	rix = idx;
-
-	if (rate->flags & IEEE80211_TX_RC_SHORT_GI)
-		rix++;
-
-	return rix;
-}
-
-static void ath_rc_sort_validrates(struct ath_rate_priv *ath_rc_priv)
-{
-	const struct ath_rate_table *rate_table = ath_rc_priv->rate_table;
-	u8 i, j, idx, idx_next;
-
-	for (i = ath_rc_priv->max_valid_rate - 1; i > 0; i--) {
-		for (j = 0; j <= i-1; j++) {
-			idx = ath_rc_priv->valid_rate_index[j];
-			idx_next = ath_rc_priv->valid_rate_index[j+1];
-
-			if (rate_table->info[idx].ratekbps >
-				rate_table->info[idx_next].ratekbps) {
-				ath_rc_priv->valid_rate_index[j] = idx_next;
-				ath_rc_priv->valid_rate_index[j+1] = idx;
-			}
-		}
-	}
-}
-
-static inline
-int ath_rc_get_nextvalid_txrate(const struct ath_rate_table *rate_table,
-				struct ath_rate_priv *ath_rc_priv,
-				u8 cur_valid_txrate,
-				u8 *next_idx)
-{
-	u8 i;
-
-	for (i = 0; i < ath_rc_priv->max_valid_rate - 1; i++) {
-		if (ath_rc_priv->valid_rate_index[i] == cur_valid_txrate) {
-			*next_idx = ath_rc_priv->valid_rate_index[i+1];
-			return 1;
-		}
-	}
-
-	/* No more valid rates */
-	*next_idx = 0;
-
-	return 0;
-}
-
-/* Return true only for single stream */
-
-static int ath_rc_valid_phyrate(u32 phy, u32 capflag, int ignore_cw)
-{
-	if (WLAN_RC_PHY_HT(phy) && !(capflag & WLAN_RC_HT_FLAG))
-		return 0;
-	if (WLAN_RC_PHY_DS(phy) && !(capflag & WLAN_RC_DS_FLAG))
-		return 0;
-	if (WLAN_RC_PHY_TS(phy) && !(capflag & WLAN_RC_TS_FLAG))
-		return 0;
-	if (WLAN_RC_PHY_SGI(phy) && !(capflag & WLAN_RC_SGI_FLAG))
-		return 0;
-	if (!ignore_cw && WLAN_RC_PHY_HT(phy))
-		if (WLAN_RC_PHY_40(phy) && !(capflag & WLAN_RC_40_FLAG))
-			return 0;
-	return 1;
-}
-
-static inline int
-ath_rc_get_lower_rix(struct ath_rate_priv *ath_rc_priv,
-		     u8 cur_valid_txrate, u8 *next_idx)
-{
-	int8_t i;
-
-	for (i = 1; i < ath_rc_priv->max_valid_rate ; i++) {
-		if (ath_rc_priv->valid_rate_index[i] == cur_valid_txrate) {
-			*next_idx = ath_rc_priv->valid_rate_index[i-1];
-			return 1;
-		}
-	}
-
-	return 0;
-}
-
-static u8 ath_rc_init_validrates(struct ath_rate_priv *ath_rc_priv)
-{
-	const struct ath_rate_table *rate_table = ath_rc_priv->rate_table;
-	u8 i, hi = 0;
-
-	for (i = 0; i < rate_table->rate_cnt; i++) {
-		if (rate_table->info[i].rate_flags & RC_LEGACY) {
-			u32 phy = rate_table->info[i].phy;
-			u8 valid_rate_count = 0;
-
-			if (!ath_rc_valid_phyrate(phy, ath_rc_priv->ht_cap, 0))
-				continue;
-
-			valid_rate_count = ath_rc_priv->valid_phy_ratecnt[phy];
-
-			ath_rc_priv->valid_phy_rateidx[phy][valid_rate_count] = i;
-			ath_rc_priv->valid_phy_ratecnt[phy] += 1;
-			ath_rc_priv->valid_rate_index[i] = true;
-			hi = i;
-		}
-	}
-
-	return hi;
-}
-
-static inline bool ath_rc_check_legacy(u8 rate, u8 dot11rate, u16 rate_flags,
-				       u32 phy, u32 capflag)
-{
-	if (rate != dot11rate || WLAN_RC_PHY_HT(phy))
-		return false;
-
-	if ((rate_flags & WLAN_RC_CAP_MODE(capflag)) != WLAN_RC_CAP_MODE(capflag))
-		return false;
-
-	if (!(rate_flags & WLAN_RC_CAP_STREAM(capflag)))
-		return false;
-
-	return true;
-}
-
-static inline bool ath_rc_check_ht(u8 rate, u8 dot11rate, u16 rate_flags,
-				   u32 phy, u32 capflag)
-{
-	if (rate != dot11rate || !WLAN_RC_PHY_HT(phy))
-		return false;
-
-	if (!WLAN_RC_PHY_HT_VALID(rate_flags, capflag))
-		return false;
-
-	if (!(rate_flags & WLAN_RC_CAP_STREAM(capflag)))
-		return false;
-
-	return true;
-}
-
-static u8 ath_rc_setvalid_rates(struct ath_rate_priv *ath_rc_priv, bool legacy)
-{
-	const struct ath_rate_table *rate_table = ath_rc_priv->rate_table;
-	struct ath_rateset *rateset;
-	u32 phy, capflag = ath_rc_priv->ht_cap;
-	u16 rate_flags;
-	u8 i, j, hi = 0, rate, dot11rate, valid_rate_count;
-
-	if (legacy)
-		rateset = &ath_rc_priv->neg_rates;
-	else
-		rateset = &ath_rc_priv->neg_ht_rates;
-
-	for (i = 0; i < rateset->rs_nrates; i++) {
-		for (j = 0; j < rate_table->rate_cnt; j++) {
-			phy = rate_table->info[j].phy;
-			rate_flags = rate_table->info[j].rate_flags;
-			rate = rateset->rs_rates[i];
-			dot11rate = rate_table->info[j].dot11rate;
-
-			if (legacy &&
-			    !ath_rc_check_legacy(rate, dot11rate,
-						 rate_flags, phy, capflag))
-				continue;
-
-			if (!legacy &&
-			    !ath_rc_check_ht(rate, dot11rate,
-					     rate_flags, phy, capflag))
-				continue;
-
-			if (!ath_rc_valid_phyrate(phy, capflag, 0))
-				continue;
-
-			valid_rate_count = ath_rc_priv->valid_phy_ratecnt[phy];
-			ath_rc_priv->valid_phy_rateidx[phy][valid_rate_count] = j;
-			ath_rc_priv->valid_phy_ratecnt[phy] += 1;
-			ath_rc_priv->valid_rate_index[j] = true;
-			hi = max(hi, j);
-		}
-	}
-
-	return hi;
-}
-
-static u8 ath_rc_get_highest_rix(struct ath_rate_priv *ath_rc_priv,
-				 int *is_probing)
-{
-	const struct ath_rate_table *rate_table = ath_rc_priv->rate_table;
-	u32 best_thruput, this_thruput, now_msec;
-	u8 rate, next_rate, best_rate, maxindex, minindex;
-	int8_t index = 0;
-
-	now_msec = jiffies_to_msecs(jiffies);
-	*is_probing = 0;
-	best_thruput = 0;
-	maxindex = ath_rc_priv->max_valid_rate-1;
-	minindex = 0;
-	best_rate = minindex;
-
-	/*
-	 * Try the higher rate first. It will reduce memory moving time
-	 * if we have very good channel characteristics.
-	 */
-	for (index = maxindex; index >= minindex ; index--) {
-		u8 per_thres;
-
-		rate = ath_rc_priv->valid_rate_index[index];
-		if (rate > ath_rc_priv->rate_max_phy)
-			continue;
-
-		/*
-		 * For TCP the average collision rate is around 11%,
-		 * so we ignore PERs less than this.  This is to
-		 * prevent the rate we are currently using (whose
-		 * PER might be in the 10-15 range because of TCP
-		 * collisions) looking worse than the next lower
-		 * rate whose PER has decayed close to 0.  If we
-		 * used to next lower rate, its PER would grow to
-		 * 10-15 and we would be worse off then staying
-		 * at the current rate.
-		 */
-		per_thres = ath_rc_priv->per[rate];
-		if (per_thres < 12)
-			per_thres = 12;
-
-		this_thruput = rate_table->info[rate].user_ratekbps *
-			(100 - per_thres);
-
-		if (best_thruput <= this_thruput) {
-			best_thruput = this_thruput;
-			best_rate    = rate;
-		}
-	}
-
-	rate = best_rate;
-
-	/*
-	 * Must check the actual rate (ratekbps) to account for
-	 * non-monoticity of 11g's rate table
-	 */
-
-	if (rate >= ath_rc_priv->rate_max_phy) {
-		rate = ath_rc_priv->rate_max_phy;
-
-		/* Probe the next allowed phy state */
-		if (ath_rc_get_nextvalid_txrate(rate_table,
-					ath_rc_priv, rate, &next_rate) &&
-		    (now_msec - ath_rc_priv->probe_time >
-		     rate_table->probe_interval) &&
-		    (ath_rc_priv->hw_maxretry_pktcnt >= 1)) {
-			rate = next_rate;
-			ath_rc_priv->probe_rate = rate;
-			ath_rc_priv->probe_time = now_msec;
-			ath_rc_priv->hw_maxretry_pktcnt = 0;
-			*is_probing = 1;
-		}
-	}
-
-	if (rate > (ath_rc_priv->rate_table_size - 1))
-		rate = ath_rc_priv->rate_table_size - 1;
-
-	if (RC_TS_ONLY(rate_table->info[rate].rate_flags) &&
-	    (ath_rc_priv->ht_cap & WLAN_RC_TS_FLAG))
-		return rate;
-
-	if (RC_DS_OR_LATER(rate_table->info[rate].rate_flags) &&
-	    (ath_rc_priv->ht_cap & (WLAN_RC_DS_FLAG | WLAN_RC_TS_FLAG)))
-		return rate;
-
-	if (RC_SS_OR_LEGACY(rate_table->info[rate].rate_flags))
-		return rate;
-
-	/* This should not happen */
-	WARN_ON_ONCE(1);
-
-	rate = ath_rc_priv->valid_rate_index[0];
-
-	return rate;
-}
-
-static void ath_rc_rate_set_series(const struct ath_rate_table *rate_table,
-				   struct ieee80211_tx_rate *rate,
-				   struct ieee80211_tx_rate_control *txrc,
-				   u8 tries, u8 rix, int rtsctsenable)
-{
-	rate->count = tries;
-	rate->idx = rate_table->info[rix].ratecode;
-
-	if (txrc->rts || rtsctsenable)
-		rate->flags |= IEEE80211_TX_RC_USE_RTS_CTS;
-
-	if (WLAN_RC_PHY_HT(rate_table->info[rix].phy)) {
-		rate->flags |= IEEE80211_TX_RC_MCS;
-		if (WLAN_RC_PHY_40(rate_table->info[rix].phy) &&
-		    conf_is_ht40(&txrc->hw->conf))
-			rate->flags |= IEEE80211_TX_RC_40_MHZ_WIDTH;
-		if (WLAN_RC_PHY_SGI(rate_table->info[rix].phy))
-			rate->flags |= IEEE80211_TX_RC_SHORT_GI;
-	}
-}
-
-static void ath_rc_rate_set_rtscts(struct ath_softc *sc,
-				   const struct ath_rate_table *rate_table,
-				   struct ieee80211_tx_info *tx_info)
-{
-	struct ieee80211_bss_conf *bss_conf;
-
-	if (!tx_info->control.vif)
-		return;
-	/*
-	 * For legacy frames, mac80211 takes care of CTS protection.
-	 */
-	if (!(tx_info->control.rates[0].flags & IEEE80211_TX_RC_MCS))
-		return;
-
-	bss_conf = &tx_info->control.vif->bss_conf;
-
-	if (!bss_conf->basic_rates)
-		return;
-
-	/*
-	 * For now, use the lowest allowed basic rate for HT frames.
-	 */
-	tx_info->control.rts_cts_rate_idx = __ffs(bss_conf->basic_rates);
-}
-
-static void ath_get_rate(void *priv, struct ieee80211_sta *sta, void *priv_sta,
-			 struct ieee80211_tx_rate_control *txrc)
-{
-	struct ath_softc *sc = priv;
-	struct ath_rate_priv *ath_rc_priv = priv_sta;
-	const struct ath_rate_table *rate_table;
-	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;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
-	__le16 fc = hdr->frame_control;
-	u8 try_per_rate, i = 0, rix;
-	int is_probe = 0;
-
-	if (rate_control_send_low(sta, priv_sta, txrc))
-		return;
-
-	/*
-	 * For Multi Rate Retry we use a different number of
-	 * retry attempt counts. This ends up looking like this:
-	 *
-	 * MRR[0] = 4
-	 * MRR[1] = 4
-	 * MRR[2] = 4
-	 * MRR[3] = 8
-	 *
-	 */
-	try_per_rate = 4;
-
-	rate_table = ath_rc_priv->rate_table;
-	rix = ath_rc_get_highest_rix(ath_rc_priv, &is_probe);
-
-	if (conf_is_ht(&sc->hw->conf) &&
-	    (sta->ht_cap.cap & IEEE80211_HT_CAP_LDPC_CODING))
-		tx_info->flags |= IEEE80211_TX_CTL_LDPC;
-
-	if (conf_is_ht(&sc->hw->conf) &&
-	    (sta->ht_cap.cap & IEEE80211_HT_CAP_TX_STBC))
-		tx_info->flags |= (1 << IEEE80211_TX_CTL_STBC_SHIFT);
-
-	if (is_probe) {
-		/*
-		 * Set one try for probe rates. For the
-		 * probes don't enable RTS.
-		 */
-		ath_rc_rate_set_series(rate_table, &rates[i++], txrc,
-				       1, rix, 0);
-		/*
-		 * Get the next tried/allowed rate.
-		 * No RTS for the next series after the probe rate.
-		 */
-		ath_rc_get_lower_rix(ath_rc_priv, rix, &rix);
-		ath_rc_rate_set_series(rate_table, &rates[i++], txrc,
-				       try_per_rate, rix, 0);
-
-		tx_info->flags |= IEEE80211_TX_CTL_RATE_CTRL_PROBE;
-	} else {
-		/*
-		 * Set the chosen rate. No RTS for first series entry.
-		 */
-		ath_rc_rate_set_series(rate_table, &rates[i++], txrc,
-				       try_per_rate, rix, 0);
-	}
-
-	for ( ; i < 4; i++) {
-		/*
-		 * Use twice the number of tries for the last MRR segment.
-		 */
-		if (i + 1 == 4)
-			try_per_rate = 8;
-
-		ath_rc_get_lower_rix(ath_rc_priv, rix, &rix);
-
-		/*
-		 * All other rates in the series have RTS enabled.
-		 */
-		ath_rc_rate_set_series(rate_table, &rates[i], txrc,
-				       try_per_rate, rix, 1);
-	}
-
-	/*
-	 * NB:Change rate series to enable aggregation when operating
-	 * at lower MCS rates. When first rate in series is MCS2
-	 * in HT40 @ 2.4GHz, series should look like:
-	 *
-	 * {MCS2, MCS1, MCS0, MCS0}.
-	 *
-	 * When first rate in series is MCS3 in HT20 @ 2.4GHz, series should
-	 * look like:
-	 *
-	 * {MCS3, MCS2, MCS1, MCS1}
-	 *
-	 * So, set fourth rate in series to be same as third one for
-	 * above conditions.
-	 */
-	if ((sc->hw->conf.chandef.chan->band == IEEE80211_BAND_2GHZ) &&
-	    (conf_is_ht(&sc->hw->conf))) {
-		u8 dot11rate = rate_table->info[rix].dot11rate;
-		u8 phy = rate_table->info[rix].phy;
-		if (i == 4 &&
-		    ((dot11rate == 2 && phy == WLAN_RC_PHY_HT_40_SS) ||
-		     (dot11rate == 3 && phy == WLAN_RC_PHY_HT_20_SS))) {
-			rates[3].idx = rates[2].idx;
-			rates[3].flags = rates[2].flags;
-		}
-	}
-
-	/*
-	 * Force hardware to use computed duration for next
-	 * fragment by disabling multi-rate retry, which
-	 * updates duration based on the multi-rate duration table.
-	 *
-	 * FIXME: Fix duration
-	 */
-	if (ieee80211_has_morefrags(fc) ||
-	    (le16_to_cpu(hdr->seq_ctrl) & IEEE80211_SCTL_FRAG)) {
-		rates[1].count = rates[2].count = rates[3].count = 0;
-		rates[1].idx = rates[2].idx = rates[3].idx = 0;
-		rates[0].count = ATH_TXMAXTRY;
-	}
-
-	ath_rc_rate_set_rtscts(sc, rate_table, tx_info);
-}
-
-static void ath_rc_update_per(struct ath_softc *sc,
-			      const struct ath_rate_table *rate_table,
-			      struct ath_rate_priv *ath_rc_priv,
-				  struct ieee80211_tx_info *tx_info,
-			      int tx_rate, int xretries, int retries,
-			      u32 now_msec)
-{
-	int count, n_bad_frames;
-	u8 last_per;
-	static const u32 nretry_to_per_lookup[10] = {
-		100 * 0 / 1,
-		100 * 1 / 4,
-		100 * 1 / 2,
-		100 * 3 / 4,
-		100 * 4 / 5,
-		100 * 5 / 6,
-		100 * 6 / 7,
-		100 * 7 / 8,
-		100 * 8 / 9,
-		100 * 9 / 10
-	};
-
-	last_per = ath_rc_priv->per[tx_rate];
-	n_bad_frames = tx_info->status.ampdu_len - tx_info->status.ampdu_ack_len;
-
-	if (xretries) {
-		if (xretries == 1) {
-			ath_rc_priv->per[tx_rate] += 30;
-			if (ath_rc_priv->per[tx_rate] > 100)
-				ath_rc_priv->per[tx_rate] = 100;
-		} else {
-			/* xretries == 2 */
-			count = ARRAY_SIZE(nretry_to_per_lookup);
-			if (retries >= count)
-				retries = count - 1;
-
-			/* new_PER = 7/8*old_PER + 1/8*(currentPER) */
-			ath_rc_priv->per[tx_rate] =
-				(u8)(last_per - (last_per >> 3) + (100 >> 3));
-		}
-
-		/* xretries == 1 or 2 */
-
-		if (ath_rc_priv->probe_rate == tx_rate)
-			ath_rc_priv->probe_rate = 0;
-
-	} else { /* xretries == 0 */
-		count = ARRAY_SIZE(nretry_to_per_lookup);
-		if (retries >= count)
-			retries = count - 1;
-
-		if (n_bad_frames) {
-			/* new_PER = 7/8*old_PER + 1/8*(currentPER)
-			 * Assuming that n_frames is not 0.  The current PER
-			 * from the retries is 100 * retries / (retries+1),
-			 * since the first retries attempts failed, and the
-			 * next one worked.  For the one that worked,
-			 * n_bad_frames subframes out of n_frames wored,
-			 * so the PER for that part is
-			 * 100 * n_bad_frames / n_frames, and it contributes
-			 * 100 * n_bad_frames / (n_frames * (retries+1)) to
-			 * the above PER.  The expression below is a
-			 * simplified version of the sum of these two terms.
-			 */
-			if (tx_info->status.ampdu_len > 0) {
-				int n_frames, n_bad_tries;
-				u8 cur_per, new_per;
-
-				n_bad_tries = retries * tx_info->status.ampdu_len +
-					n_bad_frames;
-				n_frames = tx_info->status.ampdu_len * (retries + 1);
-				cur_per = (100 * n_bad_tries / n_frames) >> 3;
-				new_per = (u8)(last_per - (last_per >> 3) + cur_per);
-				ath_rc_priv->per[tx_rate] = new_per;
-			}
-		} else {
-			ath_rc_priv->per[tx_rate] =
-				(u8)(last_per - (last_per >> 3) +
-				     (nretry_to_per_lookup[retries] >> 3));
-		}
-
-
-		/*
-		 * If we got at most one retry then increase the max rate if
-		 * this was a probe.  Otherwise, ignore the probe.
-		 */
-		if (ath_rc_priv->probe_rate && ath_rc_priv->probe_rate == tx_rate) {
-			if (retries > 0 || 2 * n_bad_frames > tx_info->status.ampdu_len) {
-				/*
-				 * Since we probed with just a single attempt,
-				 * any retries means the probe failed.  Also,
-				 * if the attempt worked, but more than half
-				 * the subframes were bad then also consider
-				 * the probe a failure.
-				 */
-				ath_rc_priv->probe_rate = 0;
-			} else {
-				u8 probe_rate = 0;
-
-				ath_rc_priv->rate_max_phy =
-					ath_rc_priv->probe_rate;
-				probe_rate = ath_rc_priv->probe_rate;
-
-				if (ath_rc_priv->per[probe_rate] > 30)
-					ath_rc_priv->per[probe_rate] = 20;
-
-				ath_rc_priv->probe_rate = 0;
-
-				/*
-				 * Since this probe succeeded, we allow the next
-				 * probe twice as soon.  This allows the maxRate
-				 * to move up faster if the probes are
-				 * successful.
-				 */
-				ath_rc_priv->probe_time =
-					now_msec - rate_table->probe_interval / 2;
-			}
-		}
-
-		if (retries > 0) {
-			/*
-			 * Don't update anything.  We don't know if
-			 * this was because of collisions or poor signal.
-			 */
-			ath_rc_priv->hw_maxretry_pktcnt = 0;
-		} else {
-			/*
-			 * It worked with no retries. First ignore bogus (small)
-			 * rssi_ack values.
-			 */
-			if (tx_rate == ath_rc_priv->rate_max_phy &&
-			    ath_rc_priv->hw_maxretry_pktcnt < 255) {
-				ath_rc_priv->hw_maxretry_pktcnt++;
-			}
-
-		}
-	}
-}
-
-static void ath_rc_update_ht(struct ath_softc *sc,
-			     struct ath_rate_priv *ath_rc_priv,
-			     struct ieee80211_tx_info *tx_info,
-			     int tx_rate, int xretries, int retries)
-{
-	u32 now_msec = jiffies_to_msecs(jiffies);
-	int rate;
-	u8 last_per;
-	const struct ath_rate_table *rate_table = ath_rc_priv->rate_table;
-	int size = ath_rc_priv->rate_table_size;
-
-	if ((tx_rate < 0) || (tx_rate > rate_table->rate_cnt))
-		return;
-
-	last_per = ath_rc_priv->per[tx_rate];
-
-	/* Update PER first */
-	ath_rc_update_per(sc, rate_table, ath_rc_priv,
-			  tx_info, tx_rate, xretries,
-			  retries, now_msec);
-
-	/*
-	 * If this rate looks bad (high PER) then stop using it for
-	 * a while (except if we are probing).
-	 */
-	if (ath_rc_priv->per[tx_rate] >= 55 && tx_rate > 0 &&
-	    rate_table->info[tx_rate].ratekbps <=
-	    rate_table->info[ath_rc_priv->rate_max_phy].ratekbps) {
-		ath_rc_get_lower_rix(ath_rc_priv, (u8)tx_rate,
-				     &ath_rc_priv->rate_max_phy);
-
-		/* Don't probe for a little while. */
-		ath_rc_priv->probe_time = now_msec;
-	}
-
-	/* Make sure the rates below this have lower PER */
-	/* Monotonicity is kept only for rates below the current rate. */
-	if (ath_rc_priv->per[tx_rate] < last_per) {
-		for (rate = tx_rate - 1; rate >= 0; rate--) {
-
-			if (ath_rc_priv->per[rate] >
-			    ath_rc_priv->per[rate+1]) {
-				ath_rc_priv->per[rate] =
-					ath_rc_priv->per[rate+1];
-			}
-		}
-	}
-
-	/* Maintain monotonicity for rates above the current rate */
-	for (rate = tx_rate; rate < size - 1; rate++) {
-		if (ath_rc_priv->per[rate+1] <
-		    ath_rc_priv->per[rate])
-			ath_rc_priv->per[rate+1] =
-				ath_rc_priv->per[rate];
-	}
-
-	/* Every so often, we reduce the thresholds
-	 * and PER (different for CCK and OFDM). */
-	if (now_msec - ath_rc_priv->per_down_time >=
-	    rate_table->probe_interval) {
-		for (rate = 0; rate < size; rate++) {
-			ath_rc_priv->per[rate] =
-				7 * ath_rc_priv->per[rate] / 8;
-		}
-
-		ath_rc_priv->per_down_time = now_msec;
-	}
-
-	ath_debug_stat_retries(ath_rc_priv, tx_rate, xretries, retries,
-			       ath_rc_priv->per[tx_rate]);
-
-}
-
-static void ath_rc_tx_status(struct ath_softc *sc,
-			     struct ath_rate_priv *ath_rc_priv,
-			     struct sk_buff *skb)
-{
-	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
-	struct ieee80211_tx_rate *rates = tx_info->status.rates;
-	struct ieee80211_tx_rate *rate;
-	int final_ts_idx = 0, xretries = 0, long_retry = 0;
-	u8 flags;
-	u32 i = 0, rix;
-
-	for (i = 0; i < sc->hw->max_rates; i++) {
-		rate = &tx_info->status.rates[i];
-		if (rate->idx < 0 || !rate->count)
-			break;
-
-		final_ts_idx = i;
-		long_retry = rate->count - 1;
-	}
-
-	if (!(tx_info->flags & IEEE80211_TX_STAT_ACK))
-		xretries = 1;
-
-	/*
-	 * If the first rate is not the final index, there
-	 * are intermediate rate failures to be processed.
-	 */
-	if (final_ts_idx != 0) {
-		for (i = 0; i < final_ts_idx ; i++) {
-			if (rates[i].count != 0 && (rates[i].idx >= 0)) {
-				flags = rates[i].flags;
-
-				/* If HT40 and we have switched mode from
-				 * 40 to 20 => don't update */
-
-				if ((flags & IEEE80211_TX_RC_40_MHZ_WIDTH) &&
-				    !(ath_rc_priv->ht_cap & WLAN_RC_40_FLAG))
-					return;
-
-				rix = ath_rc_get_rateindex(ath_rc_priv, &rates[i]);
-				ath_rc_update_ht(sc, ath_rc_priv, tx_info,
-						 rix, xretries ? 1 : 2,
-						 rates[i].count);
-			}
-		}
-	}
-
-	flags = rates[final_ts_idx].flags;
-
-	/* If HT40 and we have switched mode from 40 to 20 => don't update */
-	if ((flags & IEEE80211_TX_RC_40_MHZ_WIDTH) &&
-	    !(ath_rc_priv->ht_cap & WLAN_RC_40_FLAG))
-		return;
-
-	rix = ath_rc_get_rateindex(ath_rc_priv, &rates[final_ts_idx]);
-	ath_rc_update_ht(sc, ath_rc_priv, tx_info, rix, xretries, long_retry);
-	ath_debug_stat_rc(ath_rc_priv, rix);
-}
-
-static const
-struct ath_rate_table *ath_choose_rate_table(struct ath_softc *sc,
-					     enum ieee80211_band band,
-					     bool is_ht)
-{
-	switch(band) {
-	case IEEE80211_BAND_2GHZ:
-		if (is_ht)
-			return &ar5416_11ng_ratetable;
-		return &ar5416_11g_ratetable;
-	case IEEE80211_BAND_5GHZ:
-		if (is_ht)
-			return &ar5416_11na_ratetable;
-		return &ar5416_11a_ratetable;
-	default:
-		return NULL;
-	}
-}
-
-static void ath_rc_init(struct ath_softc *sc,
-			struct ath_rate_priv *ath_rc_priv)
-{
-	const struct ath_rate_table *rate_table = ath_rc_priv->rate_table;
-	struct ath_rateset *rateset = &ath_rc_priv->neg_rates;
-	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
-	u8 i, j, k, hi = 0, hthi = 0;
-
-	ath_rc_priv->rate_table_size = RATE_TABLE_SIZE;
-
-	for (i = 0 ; i < ath_rc_priv->rate_table_size; i++) {
-		ath_rc_priv->per[i] = 0;
-		ath_rc_priv->valid_rate_index[i] = 0;
-	}
-
-	for (i = 0; i < WLAN_RC_PHY_MAX; i++) {
-		for (j = 0; j < RATE_TABLE_SIZE; j++)
-			ath_rc_priv->valid_phy_rateidx[i][j] = 0;
-		ath_rc_priv->valid_phy_ratecnt[i] = 0;
-	}
-
-	if (!rateset->rs_nrates) {
-		hi = ath_rc_init_validrates(ath_rc_priv);
-	} else {
-		hi = ath_rc_setvalid_rates(ath_rc_priv, true);
-
-		if (ath_rc_priv->ht_cap & WLAN_RC_HT_FLAG)
-			hthi = ath_rc_setvalid_rates(ath_rc_priv, false);
-
-		hi = max(hi, hthi);
-	}
-
-	ath_rc_priv->rate_table_size = hi + 1;
-	ath_rc_priv->rate_max_phy = 0;
-	WARN_ON(ath_rc_priv->rate_table_size > RATE_TABLE_SIZE);
-
-	for (i = 0, k = 0; i < WLAN_RC_PHY_MAX; i++) {
-		for (j = 0; j < ath_rc_priv->valid_phy_ratecnt[i]; j++) {
-			ath_rc_priv->valid_rate_index[k++] =
-				ath_rc_priv->valid_phy_rateidx[i][j];
-		}
-
-		if (!ath_rc_valid_phyrate(i, rate_table->initial_ratemax, 1) ||
-		    !ath_rc_priv->valid_phy_ratecnt[i])
-			continue;
-
-		ath_rc_priv->rate_max_phy = ath_rc_priv->valid_phy_rateidx[i][j-1];
-	}
-	WARN_ON(ath_rc_priv->rate_table_size > RATE_TABLE_SIZE);
-	WARN_ON(k > RATE_TABLE_SIZE);
-
-	ath_rc_priv->max_valid_rate = k;
-	ath_rc_sort_validrates(ath_rc_priv);
-	ath_rc_priv->rate_max_phy = (k > 4) ?
-		ath_rc_priv->valid_rate_index[k-4] :
-		ath_rc_priv->valid_rate_index[k-1];
-
-	ath_dbg(common, CONFIG, "RC Initialized with capabilities: 0x%x\n",
-		ath_rc_priv->ht_cap);
-}
-
-static u8 ath_rc_build_ht_caps(struct ath_softc *sc, struct ieee80211_sta *sta)
-{
-	u8 caps = 0;
-
-	if (sta->ht_cap.ht_supported) {
-		caps = WLAN_RC_HT_FLAG;
-		if (sta->ht_cap.mcs.rx_mask[1] && sta->ht_cap.mcs.rx_mask[2])
-			caps |= WLAN_RC_TS_FLAG | WLAN_RC_DS_FLAG;
-		else if (sta->ht_cap.mcs.rx_mask[1])
-			caps |= WLAN_RC_DS_FLAG;
-		if (sta->bandwidth >= IEEE80211_STA_RX_BW_40) {
-			caps |= WLAN_RC_40_FLAG;
-			if (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40)
-				caps |= WLAN_RC_SGI_FLAG;
-		} else {
-			if (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20)
-				caps |= WLAN_RC_SGI_FLAG;
-		}
-	}
-
-	return caps;
-}
-
-static bool ath_tx_aggr_check(struct ath_softc *sc, struct ieee80211_sta *sta,
-			      u8 tidno)
-{
-	struct ath_node *an = (struct ath_node *)sta->drv_priv;
-	struct ath_atx_tid *txtid;
-
-	if (!sta->ht_cap.ht_supported)
-		return false;
-
-	txtid = ATH_AN_2_TID(an, tidno);
-	return !txtid->active;
-}
-
-
-/***********************************/
-/* mac80211 Rate Control callbacks */
-/***********************************/
-
-static void ath_tx_status(void *priv, struct ieee80211_supported_band *sband,
-			  struct ieee80211_sta *sta, void *priv_sta,
-			  struct sk_buff *skb)
-{
-	struct ath_softc *sc = priv;
-	struct ath_rate_priv *ath_rc_priv = priv_sta;
-	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
-	__le16 fc = hdr->frame_control;
-
-	if (!priv_sta || !ieee80211_is_data(fc))
-		return;
-
-	/* This packet was aggregated but doesn't carry status info */
-	if ((tx_info->flags & IEEE80211_TX_CTL_AMPDU) &&
-	    !(tx_info->flags & IEEE80211_TX_STAT_AMPDU))
-		return;
-
-	if (tx_info->flags & IEEE80211_TX_STAT_TX_FILTERED)
-		return;
-
-	ath_rc_tx_status(sc, ath_rc_priv, skb);
-
-	/* Check if aggregation has to be enabled for this tid */
-	if (conf_is_ht(&sc->hw->conf) &&
-	    !(skb->protocol == cpu_to_be16(ETH_P_PAE))) {
-		if (ieee80211_is_data_qos(fc) &&
-		    skb_get_queue_mapping(skb) != IEEE80211_AC_VO) {
-			u8 *qc, tid;
-
-			qc = ieee80211_get_qos_ctl(hdr);
-			tid = qc[0] & 0xf;
-
-			if(ath_tx_aggr_check(sc, sta, tid))
-				ieee80211_start_tx_ba_session(sta, tid, 0);
-		}
-	}
-}
-
-static void ath_rate_init(void *priv, struct ieee80211_supported_band *sband,
-			  struct cfg80211_chan_def *chandef,
-                          struct ieee80211_sta *sta, void *priv_sta)
-{
-	struct ath_softc *sc = priv;
-	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
-	struct ath_rate_priv *ath_rc_priv = priv_sta;
-	int i, j = 0;
-	u32 rate_flags = ieee80211_chandef_rate_flags(&sc->hw->conf.chandef);
-
-	for (i = 0; i < sband->n_bitrates; i++) {
-		if (sta->supp_rates[sband->band] & BIT(i)) {
-			if ((rate_flags & sband->bitrates[i].flags)
-			    != rate_flags)
-				continue;
-
-			ath_rc_priv->neg_rates.rs_rates[j]
-				= (sband->bitrates[i].bitrate * 2) / 10;
-			j++;
-		}
-	}
-	ath_rc_priv->neg_rates.rs_nrates = j;
-
-	if (sta->ht_cap.ht_supported) {
-		for (i = 0, j = 0; i < 77; i++) {
-			if (sta->ht_cap.mcs.rx_mask[i/8] & (1<<(i%8)))
-				ath_rc_priv->neg_ht_rates.rs_rates[j++] = i;
-			if (j == ATH_RATE_MAX)
-				break;
-		}
-		ath_rc_priv->neg_ht_rates.rs_nrates = j;
-	}
-
-	ath_rc_priv->rate_table = ath_choose_rate_table(sc, sband->band,
-							sta->ht_cap.ht_supported);
-	if (!ath_rc_priv->rate_table) {
-		ath_err(common, "No rate table chosen\n");
-		return;
-	}
-
-	ath_rc_priv->ht_cap = ath_rc_build_ht_caps(sc, sta);
-	ath_rc_init(sc, priv_sta);
-}
-
-static void ath_rate_update(void *priv, struct ieee80211_supported_band *sband,
-			    struct cfg80211_chan_def *chandef,
-			    struct ieee80211_sta *sta, void *priv_sta,
-			    u32 changed)
-{
-	struct ath_softc *sc = priv;
-	struct ath_rate_priv *ath_rc_priv = priv_sta;
-
-	if (changed & IEEE80211_RC_BW_CHANGED) {
-		ath_rc_priv->ht_cap = ath_rc_build_ht_caps(sc, sta);
-		ath_rc_init(sc, priv_sta);
-
-		ath_dbg(ath9k_hw_common(sc->sc_ah), CONFIG,
-			"Operating Bandwidth changed to: %d\n",
-			sc->hw->conf.chandef.width);
-	}
-}
-
-#if defined(CONFIG_MAC80211_DEBUGFS) && defined(CONFIG_ATH9K_DEBUGFS)
-
-void ath_debug_stat_rc(struct ath_rate_priv *rc, int final_rate)
-{
-	struct ath_rc_stats *stats;
-
-	stats = &rc->rcstats[final_rate];
-	stats->success++;
-}
-
-void ath_debug_stat_retries(struct ath_rate_priv *rc, int rix,
-			    int xretries, int retries, u8 per)
-{
-	struct ath_rc_stats *stats = &rc->rcstats[rix];
-
-	stats->xretries += xretries;
-	stats->retries += retries;
-	stats->per = per;
-}
-
-static ssize_t read_file_rcstat(struct file *file, char __user *user_buf,
-				size_t count, loff_t *ppos)
-{
-	struct ath_rate_priv *rc = file->private_data;
-	char *buf;
-	unsigned int len = 0, max;
-	int rix;
-	ssize_t retval;
-
-	if (rc->rate_table == NULL)
-		return 0;
-
-	max = 80 + rc->rate_table_size * 1024 + 1;
-	buf = kmalloc(max, GFP_KERNEL);
-	if (buf == NULL)
-		return -ENOMEM;
-
-	len += sprintf(buf, "%6s %6s %6s "
-		       "%10s %10s %10s %10s\n",
-		       "HT", "MCS", "Rate",
-		       "Success", "Retries", "XRetries", "PER");
-
-	for (rix = 0; rix < rc->max_valid_rate; rix++) {
-		u8 i = rc->valid_rate_index[rix];
-		u32 ratekbps = rc->rate_table->info[i].ratekbps;
-		struct ath_rc_stats *stats = &rc->rcstats[i];
-		char mcs[5];
-		char htmode[5];
-		int used_mcs = 0, used_htmode = 0;
-
-		if (WLAN_RC_PHY_HT(rc->rate_table->info[i].phy)) {
-			used_mcs = scnprintf(mcs, 5, "%d",
-					     rc->rate_table->info[i].ratecode);
-
-			if (WLAN_RC_PHY_40(rc->rate_table->info[i].phy))
-				used_htmode = scnprintf(htmode, 5, "HT40");
-			else if (WLAN_RC_PHY_20(rc->rate_table->info[i].phy))
-				used_htmode = scnprintf(htmode, 5, "HT20");
-			else
-				used_htmode = scnprintf(htmode, 5, "????");
-		}
-
-		mcs[used_mcs] = '\0';
-		htmode[used_htmode] = '\0';
-
-		len += scnprintf(buf + len, max - len,
-				 "%6s %6s %3u.%d: "
-				 "%10u %10u %10u %10u\n",
-				 htmode,
-				 mcs,
-				 ratekbps / 1000,
-				 (ratekbps % 1000) / 100,
-				 stats->success,
-				 stats->retries,
-				 stats->xretries,
-				 stats->per);
-	}
-
-	if (len > max)
-		len = max;
-
-	retval = simple_read_from_buffer(user_buf, count, ppos, buf, len);
-	kfree(buf);
-	return retval;
-}
-
-static const struct file_operations fops_rcstat = {
-	.read = read_file_rcstat,
-	.open = simple_open,
-	.owner = THIS_MODULE
-};
-
-static void ath_rate_add_sta_debugfs(void *priv, void *priv_sta,
-				     struct dentry *dir)
-{
-	struct ath_rate_priv *rc = priv_sta;
-	rc->debugfs_rcstats = debugfs_create_file("rc_stats", S_IRUGO,
-						  dir, rc, &fops_rcstat);
-}
-
-static void ath_rate_remove_sta_debugfs(void *priv, void *priv_sta)
-{
-	struct ath_rate_priv *rc = priv_sta;
-	debugfs_remove(rc->debugfs_rcstats);
-}
-
-#endif /* CONFIG_MAC80211_DEBUGFS && CONFIG_ATH9K_DEBUGFS */
-
-static void *ath_rate_alloc(struct ieee80211_hw *hw, struct dentry *debugfsdir)
-{
-	return hw->priv;
-}
-
-static void ath_rate_free(void *priv)
-{
-	return;
-}
-
-static void *ath_rate_alloc_sta(void *priv, struct ieee80211_sta *sta, gfp_t gfp)
-{
-	return kzalloc(sizeof(struct ath_rate_priv), gfp);
-}
-
-static void ath_rate_free_sta(void *priv, struct ieee80211_sta *sta,
-			      void *priv_sta)
-{
-	struct ath_rate_priv *rate_priv = priv_sta;
-	kfree(rate_priv);
-}
-
-static struct rate_control_ops ath_rate_ops = {
-	.module = NULL,
-	.name = "ath9k_rate_control",
-	.tx_status = ath_tx_status,
-	.get_rate = ath_get_rate,
-	.rate_init = ath_rate_init,
-	.rate_update = ath_rate_update,
-	.alloc = ath_rate_alloc,
-	.free = ath_rate_free,
-	.alloc_sta = ath_rate_alloc_sta,
-	.free_sta = ath_rate_free_sta,
-
-#if defined(CONFIG_MAC80211_DEBUGFS) && defined(CONFIG_ATH9K_DEBUGFS)
-	.add_sta_debugfs = ath_rate_add_sta_debugfs,
-	.remove_sta_debugfs = ath_rate_remove_sta_debugfs,
-#endif
-};
-
-int ath_rate_control_register(void)
-{
-	return ieee80211_rate_control_register(&ath_rate_ops);
-}
-
-void ath_rate_control_unregister(void)
-{
-	ieee80211_rate_control_unregister(&ath_rate_ops);
-}
diff --git a/drivers/net/wireless/ath/ath9k/rc.h b/drivers/net/wireless/ath/ath9k/rc.h
deleted file mode 100644
index b9a87383cb43..000000000000
--- a/drivers/net/wireless/ath/ath9k/rc.h
+++ /dev/null
@@ -1,248 +0,0 @@
-/*
- * Copyright (c) 2004 Sam Leffler, Errno Consulting
- * Copyright (c) 2004 Video54 Technologies, Inc.
- * Copyright (c) 2008-2011 Atheros Communications Inc.
- *
- * Permission to use, copy, modify, and/or distribute this software for any
- * purpose with or without fee is hereby granted, provided that the above
- * copyright notice and this permission notice appear in all copies.
- *
- * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
- * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
- * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
- * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
- * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
- * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
- * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
- */
-
-#ifndef RC_H
-#define RC_H
-
-#include "hw.h"
-
-struct ath_softc;
-
-#define ATH_RATE_MAX     30
-#define RATE_TABLE_SIZE  72
-
-#define RC_INVALID	0x0000
-#define RC_LEGACY	0x0001
-#define RC_SS		0x0002
-#define RC_DS		0x0004
-#define RC_TS		0x0008
-#define RC_HT_20	0x0010
-#define RC_HT_40	0x0020
-
-#define RC_STREAM_MASK	0xe
-#define RC_DS_OR_LATER(f)	((((f) & RC_STREAM_MASK) == RC_DS) || \
-				(((f) & RC_STREAM_MASK) == (RC_DS | RC_TS)))
-#define RC_TS_ONLY(f)		(((f) & RC_STREAM_MASK) == RC_TS)
-#define RC_SS_OR_LEGACY(f)	((f) & (RC_SS | RC_LEGACY))
-
-#define RC_HT_2040		(RC_HT_20 | RC_HT_40)
-#define RC_ALL_STREAM		(RC_SS | RC_DS | RC_TS)
-#define RC_L_SD			(RC_LEGACY | RC_SS | RC_DS)
-#define RC_L_SDT		(RC_LEGACY | RC_SS | RC_DS | RC_TS)
-#define RC_HT_S_20		(RC_HT_20 | RC_SS)
-#define RC_HT_D_20		(RC_HT_20 | RC_DS)
-#define RC_HT_T_20		(RC_HT_20 | RC_TS)
-#define RC_HT_S_40		(RC_HT_40 | RC_SS)
-#define RC_HT_D_40		(RC_HT_40 | RC_DS)
-#define RC_HT_T_40		(RC_HT_40 | RC_TS)
-
-#define RC_HT_SD_20		(RC_HT_20 | RC_SS | RC_DS)
-#define RC_HT_DT_20		(RC_HT_20 | RC_DS | RC_TS)
-#define RC_HT_SD_40		(RC_HT_40 | RC_SS | RC_DS)
-#define RC_HT_DT_40		(RC_HT_40 | RC_DS | RC_TS)
-
-#define RC_HT_SD_2040		(RC_HT_2040 | RC_SS | RC_DS)
-#define RC_HT_SDT_2040		(RC_HT_2040 | RC_SS | RC_DS | RC_TS)
-
-#define RC_HT_SDT_20		(RC_HT_20 | RC_SS | RC_DS | RC_TS)
-#define RC_HT_SDT_40		(RC_HT_40 | RC_SS | RC_DS | RC_TS)
-
-#define RC_ALL			(RC_LEGACY | RC_HT_2040 | RC_ALL_STREAM)
-
-enum {
-	WLAN_RC_PHY_OFDM,
-	WLAN_RC_PHY_CCK,
-	WLAN_RC_PHY_HT_20_SS,
-	WLAN_RC_PHY_HT_20_DS,
-	WLAN_RC_PHY_HT_20_TS,
-	WLAN_RC_PHY_HT_40_SS,
-	WLAN_RC_PHY_HT_40_DS,
-	WLAN_RC_PHY_HT_40_TS,
-	WLAN_RC_PHY_HT_20_SS_HGI,
-	WLAN_RC_PHY_HT_20_DS_HGI,
-	WLAN_RC_PHY_HT_20_TS_HGI,
-	WLAN_RC_PHY_HT_40_SS_HGI,
-	WLAN_RC_PHY_HT_40_DS_HGI,
-	WLAN_RC_PHY_HT_40_TS_HGI,
-	WLAN_RC_PHY_MAX
-};
-
-#define WLAN_RC_PHY_DS(_phy)   ((_phy == WLAN_RC_PHY_HT_20_DS)		\
-				|| (_phy == WLAN_RC_PHY_HT_40_DS)	\
-				|| (_phy == WLAN_RC_PHY_HT_20_DS_HGI)	\
-				|| (_phy == WLAN_RC_PHY_HT_40_DS_HGI))
-#define WLAN_RC_PHY_TS(_phy)   ((_phy == WLAN_RC_PHY_HT_20_TS)		\
-				|| (_phy == WLAN_RC_PHY_HT_40_TS)	\
-				|| (_phy == WLAN_RC_PHY_HT_20_TS_HGI)	\
-				|| (_phy == WLAN_RC_PHY_HT_40_TS_HGI))
-#define WLAN_RC_PHY_20(_phy)   ((_phy == WLAN_RC_PHY_HT_20_SS)		\
-				|| (_phy == WLAN_RC_PHY_HT_20_DS)	\
-				|| (_phy == WLAN_RC_PHY_HT_20_TS)	\
-				|| (_phy == WLAN_RC_PHY_HT_20_SS_HGI)	\
-				|| (_phy == WLAN_RC_PHY_HT_20_DS_HGI)	\
-				|| (_phy == WLAN_RC_PHY_HT_20_TS_HGI))
-#define WLAN_RC_PHY_40(_phy)   ((_phy == WLAN_RC_PHY_HT_40_SS)		\
-				|| (_phy == WLAN_RC_PHY_HT_40_DS)	\
-				|| (_phy == WLAN_RC_PHY_HT_40_TS)	\
-				|| (_phy == WLAN_RC_PHY_HT_40_SS_HGI)	\
-				|| (_phy == WLAN_RC_PHY_HT_40_DS_HGI)	\
-				|| (_phy == WLAN_RC_PHY_HT_40_TS_HGI))
-#define WLAN_RC_PHY_SGI(_phy)  ((_phy == WLAN_RC_PHY_HT_20_SS_HGI)      \
-				|| (_phy == WLAN_RC_PHY_HT_20_DS_HGI)   \
-				|| (_phy == WLAN_RC_PHY_HT_20_TS_HGI)   \
-				|| (_phy == WLAN_RC_PHY_HT_40_SS_HGI)   \
-				|| (_phy == WLAN_RC_PHY_HT_40_DS_HGI)   \
-				|| (_phy == WLAN_RC_PHY_HT_40_TS_HGI))
-
-#define WLAN_RC_PHY_HT(_phy)    (_phy >= WLAN_RC_PHY_HT_20_SS)
-
-#define WLAN_RC_CAP_MODE(capflag) (((capflag & WLAN_RC_HT_FLAG) ?	\
-	((capflag & WLAN_RC_40_FLAG) ? RC_HT_40 : RC_HT_20) : RC_LEGACY))
-
-#define WLAN_RC_CAP_STREAM(capflag) (((capflag & WLAN_RC_TS_FLAG) ?	\
-	(RC_TS) : ((capflag & WLAN_RC_DS_FLAG) ? RC_DS : RC_SS)))
-
-/* Return TRUE if flag supports HT20 && client supports HT20 or
- * return TRUE if flag supports HT40 && client supports HT40.
- * This is used becos some rates overlap between HT20/HT40.
- */
-#define WLAN_RC_PHY_HT_VALID(flag, capflag)			\
-	(((flag & RC_HT_20) && !(capflag & WLAN_RC_40_FLAG)) || \
-	 ((flag & RC_HT_40) && (capflag & WLAN_RC_40_FLAG)))
-
-#define WLAN_RC_DS_FLAG         (0x01)
-#define WLAN_RC_TS_FLAG         (0x02)
-#define WLAN_RC_40_FLAG         (0x04)
-#define WLAN_RC_SGI_FLAG        (0x08)
-#define WLAN_RC_HT_FLAG         (0x10)
-
-/**
- * struct ath_rate_table - Rate Control table
- * @rate_cnt: total number of rates for the given wireless mode
- * @mcs_start: MCS rate index offset
- * @rate_flags: Rate Control flags
- * @phy: CCK/OFDM/HT20/HT40
- * @ratekbps: rate in Kbits per second
- * @user_ratekbps: user rate in Kbits per second
- * @ratecode: rate that goes into HW descriptors
- * @dot11rate: value that goes into supported
- * 	rates info element of MLME
- * @ctrl_rate: Index of next lower basic rate, used for duration computation
- * @cw40index: Index of rates having 40MHz channel width
- * @sgi_index: Index of rates having Short Guard Interval
- * @ht_index: high throughput rates having 40MHz channel width and
- * 	Short Guard Interval
- * @probe_interval: interval for rate control to probe for other rates
- * @initial_ratemax: initial ratemax value
- */
-struct ath_rate_table {
-	int rate_cnt;
-	int mcs_start;
-	struct {
-		u16 rate_flags;
-		u8 phy;
-		u32 ratekbps;
-		u32 user_ratekbps;
-		u8 ratecode;
-		u8 dot11rate;
-	} info[RATE_TABLE_SIZE];
-	u32 probe_interval;
-	u8 initial_ratemax;
-};
-
-struct ath_rateset {
-	u8 rs_nrates;
-	u8 rs_rates[ATH_RATE_MAX];
-};
-
-struct ath_rc_stats {
-	u32 success;
-	u32 retries;
-	u32 xretries;
-	u8 per;
-};
-
-/**
- * struct ath_rate_priv - Rate Control priv data
- * @state: RC state
- * @probe_rate: rate we are probing at
- * @probe_time: msec timestamp for last probe
- * @hw_maxretry_pktcnt: num of packets since we got HW max retry error
- * @max_valid_rate: maximum number of valid rate
- * @per_down_time: msec timestamp for last PER down step
- * @valid_phy_ratecnt: valid rate count
- * @rate_max_phy: phy index for the max rate
- * @per: PER for every valid rate in %
- * @probe_interval: interval for ratectrl to probe for other rates
- * @ht_cap: HT capabilities
- * @neg_rates: Negotatied rates
- * @neg_ht_rates: Negotiated HT rates
- */
-struct ath_rate_priv {
-	u8 rate_table_size;
-	u8 probe_rate;
-	u8 hw_maxretry_pktcnt;
-	u8 max_valid_rate;
-	u8 valid_rate_index[RATE_TABLE_SIZE];
-	u8 ht_cap;
-	u8 valid_phy_ratecnt[WLAN_RC_PHY_MAX];
-	u8 valid_phy_rateidx[WLAN_RC_PHY_MAX][RATE_TABLE_SIZE];
-	u8 rate_max_phy;
-	u8 per[RATE_TABLE_SIZE];
-	u32 probe_time;
-	u32 per_down_time;
-	u32 probe_interval;
-	struct ath_rateset neg_rates;
-	struct ath_rateset neg_ht_rates;
-	const struct ath_rate_table *rate_table;
-
-#if defined(CONFIG_MAC80211_DEBUGFS) && defined(CONFIG_ATH9K_DEBUGFS)
-	struct dentry *debugfs_rcstats;
-	struct ath_rc_stats rcstats[RATE_TABLE_SIZE];
-#endif
-};
-
-#if defined(CONFIG_MAC80211_DEBUGFS) && defined(CONFIG_ATH9K_DEBUGFS)
-void ath_debug_stat_rc(struct ath_rate_priv *rc, int final_rate);
-void ath_debug_stat_retries(struct ath_rate_priv *rc, int rix,
-			    int xretries, int retries, u8 per);
-#else
-static inline void ath_debug_stat_rc(struct ath_rate_priv *rc, int final_rate)
-{
-}
-static inline void ath_debug_stat_retries(struct ath_rate_priv *rc, int rix,
-					  int xretries, int retries, u8 per)
-{
-}
-#endif
-
-#ifdef CONFIG_ATH9K_LEGACY_RATE_CONTROL
-int ath_rate_control_register(void);
-void ath_rate_control_unregister(void);
-#else
-static inline int ath_rate_control_register(void)
-{
-	return 0;
-}
-
-static inline void ath_rate_control_unregister(void)
-{
-}
-#endif
-
-#endif /* RC_H */
diff --git a/drivers/net/wireless/ath/ath9k/recv.c b/drivers/net/wireless/ath/ath9k/recv.c
index a0ebdd000fc2..6c9accdb52e4 100644
--- a/drivers/net/wireless/ath/ath9k/recv.c
+++ b/drivers/net/wireless/ath/ath9k/recv.c
@@ -732,11 +732,18 @@ static struct ath_rxbuf *ath_get_next_rx_buf(struct ath_softc *sc,
 			return NULL;
 
 		/*
-		 * mark descriptor as zero-length and set the 'more'
-		 * flag to ensure that both buffers get discarded
+		 * Re-check previous descriptor, in case it has been filled
+		 * in the mean time.
 		 */
-		rs->rs_datalen = 0;
-		rs->rs_more = true;
+		ret = ath9k_hw_rxprocdesc(ah, ds, rs);
+		if (ret == -EINPROGRESS) {
+			/*
+			 * mark descriptor as zero-length and set the 'more'
+			 * flag to ensure that both buffers get discarded
+			 */
+			rs->rs_datalen = 0;
+			rs->rs_more = true;
+		}
 	}
 
 	list_del(&bf->list);
@@ -755,204 +762,6 @@ static struct ath_rxbuf *ath_get_next_rx_buf(struct ath_softc *sc,
 	return bf;
 }
 
-/* Assumes you've already done the endian to CPU conversion */
-static bool ath9k_rx_accept(struct ath_common *common,
-			    struct ieee80211_hdr *hdr,
-			    struct ieee80211_rx_status *rxs,
-			    struct ath_rx_status *rx_stats,
-			    bool *decrypt_error)
-{
-	struct ath_softc *sc = (struct ath_softc *) common->priv;
-	bool is_mc, is_valid_tkip, strip_mic, mic_error;
-	struct ath_hw *ah = common->ah;
-	__le16 fc;
-
-	fc = hdr->frame_control;
-
-	is_mc = !!is_multicast_ether_addr(hdr->addr1);
-	is_valid_tkip = rx_stats->rs_keyix != ATH9K_RXKEYIX_INVALID &&
-		test_bit(rx_stats->rs_keyix, common->tkip_keymap);
-	strip_mic = is_valid_tkip && ieee80211_is_data(fc) &&
-		ieee80211_has_protected(fc) &&
-		!(rx_stats->rs_status &
-		(ATH9K_RXERR_DECRYPT | ATH9K_RXERR_CRC | ATH9K_RXERR_MIC |
-		 ATH9K_RXERR_KEYMISS));
-
-	/*
-	 * Key miss events are only relevant for pairwise keys where the
-	 * descriptor does contain a valid key index. This has been observed
-	 * mostly with CCMP encryption.
-	 */
-	if (rx_stats->rs_keyix == ATH9K_RXKEYIX_INVALID ||
-	    !test_bit(rx_stats->rs_keyix, common->ccmp_keymap))
-		rx_stats->rs_status &= ~ATH9K_RXERR_KEYMISS;
-
-	mic_error = is_valid_tkip && !ieee80211_is_ctl(fc) &&
-		!ieee80211_has_morefrags(fc) &&
-		!(le16_to_cpu(hdr->seq_ctrl) & IEEE80211_SCTL_FRAG) &&
-		(rx_stats->rs_status & ATH9K_RXERR_MIC);
-
-	/*
-	 * The rx_stats->rs_status will not be set until the end of the
-	 * chained descriptors so it can be ignored if rs_more is set. The
-	 * rs_more will be false at the last element of the chained
-	 * descriptors.
-	 */
-	if (rx_stats->rs_status != 0) {
-		u8 status_mask;
-
-		if (rx_stats->rs_status & ATH9K_RXERR_CRC) {
-			rxs->flag |= RX_FLAG_FAILED_FCS_CRC;
-			mic_error = false;
-		}
-
-		if ((rx_stats->rs_status & ATH9K_RXERR_DECRYPT) ||
-		    (!is_mc && (rx_stats->rs_status & ATH9K_RXERR_KEYMISS))) {
-			*decrypt_error = true;
-			mic_error = false;
-		}
-
-		/*
-		 * Reject error frames with the exception of
-		 * decryption and MIC failures. For monitor mode,
-		 * we also ignore the CRC error.
-		 */
-		status_mask = ATH9K_RXERR_DECRYPT | ATH9K_RXERR_MIC |
-			      ATH9K_RXERR_KEYMISS;
-
-		if (ah->is_monitoring && (sc->rx.rxfilter & FIF_FCSFAIL))
-			status_mask |= ATH9K_RXERR_CRC;
-
-		if (rx_stats->rs_status & ~status_mask)
-			return false;
-	}
-
-	/*
-	 * For unicast frames the MIC error bit can have false positives,
-	 * so all MIC error reports need to be validated in software.
-	 * False negatives are not common, so skip software verification
-	 * if the hardware considers the MIC valid.
-	 */
-	if (strip_mic)
-		rxs->flag |= RX_FLAG_MMIC_STRIPPED;
-	else if (is_mc && mic_error)
-		rxs->flag |= RX_FLAG_MMIC_ERROR;
-
-	return true;
-}
-
-static int ath9k_process_rate(struct ath_common *common,
-			      struct ieee80211_hw *hw,
-			      struct ath_rx_status *rx_stats,
-			      struct ieee80211_rx_status *rxs)
-{
-	struct ieee80211_supported_band *sband;
-	enum ieee80211_band band;
-	unsigned int i = 0;
-	struct ath_softc __maybe_unused *sc = common->priv;
-	struct ath_hw *ah = sc->sc_ah;
-
-	band = ah->curchan->chan->band;
-	sband = hw->wiphy->bands[band];
-
-	if (IS_CHAN_QUARTER_RATE(ah->curchan))
-		rxs->flag |= RX_FLAG_5MHZ;
-	else if (IS_CHAN_HALF_RATE(ah->curchan))
-		rxs->flag |= RX_FLAG_10MHZ;
-
-	if (rx_stats->rs_rate & 0x80) {
-		/* HT rate */
-		rxs->flag |= RX_FLAG_HT;
-		rxs->flag |= rx_stats->flag;
-		rxs->rate_idx = rx_stats->rs_rate & 0x7f;
-		return 0;
-	}
-
-	for (i = 0; i < sband->n_bitrates; i++) {
-		if (sband->bitrates[i].hw_value == rx_stats->rs_rate) {
-			rxs->rate_idx = i;
-			return 0;
-		}
-		if (sband->bitrates[i].hw_value_short == rx_stats->rs_rate) {
-			rxs->flag |= RX_FLAG_SHORTPRE;
-			rxs->rate_idx = i;
-			return 0;
-		}
-	}
-
-	/*
-	 * No valid hardware bitrate found -- we should not get here
-	 * because hardware has already validated this frame as OK.
-	 */
-	ath_dbg(common, ANY,
-		"unsupported hw bitrate detected 0x%02x using 1 Mbit\n",
-		rx_stats->rs_rate);
-	RX_STAT_INC(rx_rate_err);
-	return -EINVAL;
-}
-
-static void ath9k_process_rssi(struct ath_common *common,
-			       struct ieee80211_hw *hw,
-			       struct ath_rx_status *rx_stats,
-			       struct ieee80211_rx_status *rxs)
-{
-	struct ath_softc *sc = hw->priv;
-	struct ath_hw *ah = common->ah;
-	int last_rssi;
-	int rssi = rx_stats->rs_rssi;
-	int i, j;
-
-	/*
-	 * RSSI is not available for subframes in an A-MPDU.
-	 */
-	if (rx_stats->rs_moreaggr) {
-		rxs->flag |= RX_FLAG_NO_SIGNAL_VAL;
-		return;
-	}
-
-	/*
-	 * Check if the RSSI for the last subframe in an A-MPDU
-	 * or an unaggregated frame is valid.
-	 */
-	if (rx_stats->rs_rssi == ATH9K_RSSI_BAD) {
-		rxs->flag |= RX_FLAG_NO_SIGNAL_VAL;
-		return;
-	}
-
-	for (i = 0, j = 0; i < ARRAY_SIZE(rx_stats->rs_rssi_ctl); i++) {
-		s8 rssi;
-
-		if (!(ah->rxchainmask & BIT(i)))
-			continue;
-
-		rssi = rx_stats->rs_rssi_ctl[i];
-		if (rssi != ATH9K_RSSI_BAD) {
-		    rxs->chains |= BIT(j);
-		    rxs->chain_signal[j] = ah->noise + rssi;
-		}
-		j++;
-	}
-
-	/*
-	 * Update Beacon RSSI, this is used by ANI.
-	 */
-	if (rx_stats->is_mybeacon &&
-	    ((ah->opmode == NL80211_IFTYPE_STATION) ||
-	     (ah->opmode == NL80211_IFTYPE_ADHOC))) {
-		ATH_RSSI_LPF(sc->last_rssi, rx_stats->rs_rssi);
-		last_rssi = sc->last_rssi;
-
-		if (likely(last_rssi != ATH_RSSI_DUMMY_MARKER))
-			rssi = ATH_EP_RND(last_rssi, ATH_RSSI_EP_MULTIPLIER);
-		if (rssi < 0)
-			rssi = 0;
-
-		ah->stats.avgbrssi = rssi;
-	}
-
-	rxs->signal = ah->noise + rx_stats->rs_rssi;
-}
-
 static void ath9k_process_tsf(struct ath_rx_status *rs,
 			      struct ieee80211_rx_status *rxs,
 			      u64 tsf)
@@ -985,32 +794,32 @@ static int ath9k_rx_skb_preprocess(struct ath_softc *sc,
 	struct ath_common *common = ath9k_hw_common(ah);
 	struct ieee80211_hdr *hdr;
 	bool discard_current = sc->rx.discard_next;
-	int ret = 0;
 
 	/*
 	 * Discard corrupt descriptors which are marked in
 	 * ath_get_next_rx_buf().
 	 */
-	sc->rx.discard_next = rx_stats->rs_more;
 	if (discard_current)
-		return -EINVAL;
+		goto corrupt;
+
+	sc->rx.discard_next = false;
 
 	/*
 	 * Discard zero-length packets.
 	 */
 	if (!rx_stats->rs_datalen) {
 		RX_STAT_INC(rx_len_err);
-		return -EINVAL;
+		goto corrupt;
 	}
 
-        /*
-         * rs_status follows rs_datalen so if rs_datalen is too large
-         * we can take a hint that hardware corrupted it, so ignore
-         * those frames.
-         */
+	/*
+	 * rs_status follows rs_datalen so if rs_datalen is too large
+	 * we can take a hint that hardware corrupted it, so ignore
+	 * those frames.
+	 */
 	if (rx_stats->rs_datalen > (common->rx_bufsize - ah->caps.rx_status_len)) {
 		RX_STAT_INC(rx_len_err);
-		return -EINVAL;
+		goto corrupt;
 	}
 
 	/* Only use status info from the last fragment */
@@ -1024,10 +833,8 @@ static int ath9k_rx_skb_preprocess(struct ath_softc *sc,
 	 * This is different from the other corrupt descriptor
 	 * condition handled above.
 	 */
-	if (rx_stats->rs_status & ATH9K_RXERR_CORRUPT_DESC) {
-		ret = -EINVAL;
-		goto exit;
-	}
+	if (rx_stats->rs_status & ATH9K_RXERR_CORRUPT_DESC)
+		goto corrupt;
 
 	hdr = (struct ieee80211_hdr *) (skb->data + ah->caps.rx_status_len);
 
@@ -1043,18 +850,15 @@ static int ath9k_rx_skb_preprocess(struct ath_softc *sc,
 		if (ath_process_fft(sc, hdr, rx_stats, rx_status->mactime))
 			RX_STAT_INC(rx_spectral);
 
-		ret = -EINVAL;
-		goto exit;
+		return -EINVAL;
 	}
 
 	/*
 	 * everything but the rate is checked here, the rate check is done
 	 * separately to avoid doing two lookups for a rate for each frame.
 	 */
-	if (!ath9k_rx_accept(common, hdr, rx_status, rx_stats, decrypt_error)) {
-		ret = -EINVAL;
-		goto exit;
-	}
+	if (!ath9k_cmn_rx_accept(common, hdr, rx_status, rx_stats, decrypt_error, sc->rx.rxfilter))
+		return -EINVAL;
 
 	if (ath_is_mybeacon(common, hdr)) {
 		RX_STAT_INC(rx_beacons);
@@ -1064,17 +868,21 @@ static int ath9k_rx_skb_preprocess(struct ath_softc *sc,
 	/*
 	 * This shouldn't happen, but have a safety check anyway.
 	 */
-	if (WARN_ON(!ah->curchan)) {
-		ret = -EINVAL;
-		goto exit;
-	}
+	if (WARN_ON(!ah->curchan))
+		return -EINVAL;
 
-	if (ath9k_process_rate(common, hw, rx_stats, rx_status)) {
-		ret =-EINVAL;
-		goto exit;
+	if (ath9k_cmn_process_rate(common, hw, rx_stats, rx_status)) {
+		/*
+		 * No valid hardware bitrate found -- we should not get here
+		 * because hardware has already validated this frame as OK.
+		 */
+		ath_dbg(common, ANY, "unsupported hw bitrate detected 0x%02x using 1 Mbit\n",
+			rx_stats->rs_rate);
+		RX_STAT_INC(rx_rate_err);
+		return -EINVAL;
 	}
 
-	ath9k_process_rssi(common, hw, rx_stats, rx_status);
+	ath9k_cmn_process_rssi(common, hw, rx_stats, rx_status);
 
 	rx_status->band = ah->curchan->chan->band;
 	rx_status->freq = ah->curchan->chan->center_freq;
@@ -1087,60 +895,11 @@ static int ath9k_rx_skb_preprocess(struct ath_softc *sc,
 		sc->rx.num_pkts++;
 #endif
 
-exit:
-	sc->rx.discard_next = false;
-	return ret;
-}
-
-static void ath9k_rx_skb_postprocess(struct ath_common *common,
-				     struct sk_buff *skb,
-				     struct ath_rx_status *rx_stats,
-				     struct ieee80211_rx_status *rxs,
-				     bool decrypt_error)
-{
-	struct ath_hw *ah = common->ah;
-	struct ieee80211_hdr *hdr;
-	int hdrlen, padpos, padsize;
-	u8 keyix;
-	__le16 fc;
-
-	/* see if any padding is done by the hw and remove it */
-	hdr = (struct ieee80211_hdr *) skb->data;
-	hdrlen = ieee80211_get_hdrlen_from_skb(skb);
-	fc = hdr->frame_control;
-	padpos = ieee80211_hdrlen(fc);
-
-	/* The MAC header is padded to have 32-bit boundary if the
-	 * packet payload is non-zero. The general calculation for
-	 * padsize would take into account odd header lengths:
-	 * padsize = (4 - padpos % 4) % 4; However, since only
-	 * even-length headers are used, padding can only be 0 or 2
-	 * bytes and we can optimize this a bit. In addition, we must
-	 * not try to remove padding from short control frames that do
-	 * not have payload. */
-	padsize = padpos & 3;
-	if (padsize && skb->len>=padpos+padsize+FCS_LEN) {
-		memmove(skb->data + padsize, skb->data, padpos);
-		skb_pull(skb, padsize);
-	}
-
-	keyix = rx_stats->rs_keyix;
-
-	if (!(keyix == ATH9K_RXKEYIX_INVALID) && !decrypt_error &&
-	    ieee80211_has_protected(fc)) {
-		rxs->flag |= RX_FLAG_DECRYPTED;
-	} else if (ieee80211_has_protected(fc)
-		   && !decrypt_error && skb->len >= hdrlen + 4) {
-		keyix = skb->data[hdrlen + 3] >> 6;
+	return 0;
 
-		if (test_bit(keyix, common->keymap))
-			rxs->flag |= RX_FLAG_DECRYPTED;
-	}
-	if (ah->sw_mgmt_crypto &&
-	    (rxs->flag & RX_FLAG_DECRYPTED) &&
-	    ieee80211_is_mgmt(fc))
-		/* Use software decrypt for management frames. */
-		rxs->flag &= ~RX_FLAG_DECRYPTED;
+corrupt:
+	sc->rx.discard_next = rx_stats->rs_more;
+	return -EINVAL;
 }
 
 /*
@@ -1292,8 +1051,8 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 			skb_pull(skb, ah->caps.rx_status_len);
 
 		if (!rs.rs_more)
-			ath9k_rx_skb_postprocess(common, hdr_skb, &rs,
-						 rxs, decrypt_error);
+			ath9k_cmn_rx_skb_postprocess(common, hdr_skb, &rs,
+						     rxs, decrypt_error);
 
 		if (rs.rs_more) {
 			RX_STAT_INC(rx_frags);
diff --git a/drivers/net/wireless/ath/ath9k/xmit.c b/drivers/net/wireless/ath/ath9k/xmit.c
index 0a75e2f68c9d..fafacfed44ea 100644
--- a/drivers/net/wireless/ath/ath9k/xmit.c
+++ b/drivers/net/wireless/ath/ath9k/xmit.c
@@ -1040,11 +1040,11 @@ static int ath_max_framelen(int usec, int mcs, bool ht40, bool sgi)
 	int symbols, bits;
 	int bytes = 0;
 
+	usec -= L_STF + L_LTF + L_SIG + HT_SIG + HT_STF + HT_LTF(streams);
 	symbols = sgi ? TIME_SYMBOLS_HALFGI(usec) : TIME_SYMBOLS(usec);
 	bits = symbols * bits_per_symbol[mcs % 8][ht40] * streams;
 	bits -= OFDM_PLCP_BITS;
 	bytes = bits / 8;
-	bytes -= L_STF + L_LTF + L_SIG + HT_SIG + HT_STF + HT_LTF(streams);
 	if (bytes > 65532)
 		bytes = 65532;
 
@@ -1076,6 +1076,7 @@ static void ath_buf_set_rate(struct ath_softc *sc, struct ath_buf *bf,
 			     struct ath_tx_info *info, int len, bool rts)
 {
 	struct ath_hw *ah = sc->sc_ah;
+	struct ath_common *common = ath9k_hw_common(ah);
 	struct sk_buff *skb;
 	struct ieee80211_tx_info *tx_info;
 	struct ieee80211_tx_rate *rates;
@@ -1145,7 +1146,7 @@ static void ath_buf_set_rate(struct ath_softc *sc, struct ath_buf *bf,
 		}
 
 		/* legacy rates */
-		rate = &sc->sbands[tx_info->band].bitrates[rates[i].idx];
+		rate = &common->sbands[tx_info->band].bitrates[rates[i].idx];
 		if ((tx_info->band == IEEE80211_BAND_2GHZ) &&
 		    !(rate->flags & IEEE80211_RATE_ERP_G))
 			phy = WLAN_RC_PHY_CCK;
@@ -1444,14 +1445,16 @@ void ath_tx_aggr_sleep(struct ieee80211_sta *sta, struct ath_softc *sc,
 	for (tidno = 0, tid = &an->tid[tidno];
 	     tidno < IEEE80211_NUM_TIDS; tidno++, tid++) {
 
-		if (!tid->sched)
-			continue;
-
 		ac = tid->ac;
 		txq = ac->txq;
 
 		ath_txq_lock(sc, txq);
 
+		if (!tid->sched) {
+			ath_txq_unlock(sc, txq);
+			continue;
+		}
+
 		buffered = ath_tid_has_buffered(tid);
 
 		tid->sched = false;
@@ -2184,14 +2187,15 @@ int ath_tx_start(struct ieee80211_hw *hw, struct sk_buff *skb,
 		txq->stopped = true;
 	}
 
+	if (txctl->an)
+		tid = ath_get_skb_tid(sc, txctl->an, skb);
+
 	if (info->flags & IEEE80211_TX_CTL_PS_RESPONSE) {
 		ath_txq_unlock(sc, txq);
 		txq = sc->tx.uapsdq;
 		ath_txq_lock(sc, txq);
 	} else if (txctl->an &&
 		   ieee80211_is_data_present(hdr->frame_control)) {
-		tid = ath_get_skb_tid(sc, txctl->an, skb);
-
 		WARN_ON(tid->ac->txq != txctl->txq);
 
 		if (info->flags & IEEE80211_TX_CTL_CLEAR_PS_FILT)
@@ -2566,7 +2570,7 @@ void ath_tx_edma_tasklet(struct ath_softc *sc)
 			sc->beacon.tx_processed = true;
 			sc->beacon.tx_last = !(ts.ts_status & ATH9K_TXERR_MASK);
 
-			ath9k_csa_is_finished(sc);
+			ath9k_csa_update(sc);
 			continue;
 		}
 
diff --git a/drivers/net/wireless/ath/regd.c b/drivers/net/wireless/ath/regd.c
index e5e905910db4..415393dfb6fc 100644
--- a/drivers/net/wireless/ath/regd.c
+++ b/drivers/net/wireless/ath/regd.c
@@ -222,7 +222,7 @@ static const struct ieee80211_regdomain *ath_default_world_regdomain(void)
 static const struct
 ieee80211_regdomain *ath_world_regdomain(struct ath_regulatory *reg)
 {
-	switch (reg->regpair->regDmnEnum) {
+	switch (reg->regpair->reg_domain) {
 	case 0x60:
 	case 0x61:
 	case 0x62:
@@ -431,7 +431,7 @@ static void ath_reg_apply_world_flags(struct wiphy *wiphy,
 				      enum nl80211_reg_initiator initiator,
 				      struct ath_regulatory *reg)
 {
-	switch (reg->regpair->regDmnEnum) {
+	switch (reg->regpair->reg_domain) {
 	case 0x60:
 	case 0x63:
 	case 0x66:
@@ -560,7 +560,7 @@ static bool ath_regd_is_eeprom_valid(struct ath_regulatory *reg)
 			printk(KERN_DEBUG "ath: EEPROM indicates we "
 			       "should expect a direct regpair map\n");
 		for (i = 0; i < ARRAY_SIZE(regDomainPairs); i++)
-			if (regDomainPairs[i].regDmnEnum == rd)
+			if (regDomainPairs[i].reg_domain == rd)
 				return true;
 	}
 	printk(KERN_DEBUG
@@ -617,7 +617,7 @@ ath_get_regpair(int regdmn)
 	if (regdmn == NO_ENUMRD)
 		return NULL;
 	for (i = 0; i < ARRAY_SIZE(regDomainPairs); i++) {
-		if (regDomainPairs[i].regDmnEnum == regdmn)
+		if (regDomainPairs[i].reg_domain == regdmn)
 			return &regDomainPairs[i];
 	}
 	return NULL;
@@ -741,7 +741,7 @@ static int __ath_regd_init(struct ath_regulatory *reg)
 	printk(KERN_DEBUG "ath: Country alpha2 being used: %c%c\n",
 		reg->alpha2[0], reg->alpha2[1]);
 	printk(KERN_DEBUG "ath: Regpair used: 0x%0x\n",
-		reg->regpair->regDmnEnum);
+		reg->regpair->reg_domain);
 
 	return 0;
 }
diff --git a/drivers/net/wireless/ath/wcn36xx/dxe.c b/drivers/net/wireless/ath/wcn36xx/dxe.c
index ee25786b4447..73f12f196f14 100644
--- a/drivers/net/wireless/ath/wcn36xx/dxe.c
+++ b/drivers/net/wireless/ath/wcn36xx/dxe.c
@@ -44,6 +44,14 @@ static void wcn36xx_dxe_write_register(struct wcn36xx *wcn, int addr, int data)
 	writel(data, wcn->mmio + addr);
 }
 
+#define wcn36xx_dxe_write_register_x(wcn, reg, reg_data)		 \
+do {									 \
+	if (wcn->chip_version == WCN36XX_CHIP_3680)			 \
+		wcn36xx_dxe_write_register(wcn, reg ## _3680, reg_data); \
+	else								 \
+		wcn36xx_dxe_write_register(wcn, reg ## _3660, reg_data); \
+} while (0)								 \
+
 static void wcn36xx_dxe_read_register(struct wcn36xx *wcn, int addr, int *data)
 {
 	*data = readl(wcn->mmio + addr);
@@ -680,7 +688,7 @@ int wcn36xx_dxe_init(struct wcn36xx *wcn)
 
 	/* Setting interrupt path */
 	reg_data = WCN36XX_DXE_CCU_INT;
-	wcn36xx_dxe_write_register(wcn, WCN36XX_DXE_REG_CCU_INT, reg_data);
+	wcn36xx_dxe_write_register_x(wcn, WCN36XX_DXE_REG_CCU_INT, reg_data);
 
 	/***************************************/
 	/* Init descriptors for TX LOW channel */
diff --git a/drivers/net/wireless/ath/wcn36xx/dxe.h b/drivers/net/wireless/ath/wcn36xx/dxe.h
index c88562f85de1..35ee7e966bd2 100644
--- a/drivers/net/wireless/ath/wcn36xx/dxe.h
+++ b/drivers/net/wireless/ath/wcn36xx/dxe.h
@@ -28,11 +28,11 @@ H2H_TEST_RX_TX = DMA2
 */
 
 /* DXE registers */
-#define WCN36XX_DXE_MEM_BASE			0x03000000
 #define WCN36XX_DXE_MEM_REG			0x202000
 
 #define WCN36XX_DXE_CCU_INT			0xA0011
-#define WCN36XX_DXE_REG_CCU_INT			0x200b10
+#define WCN36XX_DXE_REG_CCU_INT_3660		0x200b10
+#define WCN36XX_DXE_REG_CCU_INT_3680		0x2050dc
 
 /* TODO This must calculated properly but not hardcoded */
 #define WCN36XX_DXE_CTRL_TX_L			0x328a44
diff --git a/drivers/net/wireless/ath/wcn36xx/hal.h b/drivers/net/wireless/ath/wcn36xx/hal.h
index 3c2ef0c32f72..a1f1127d7808 100644
--- a/drivers/net/wireless/ath/wcn36xx/hal.h
+++ b/drivers/net/wireless/ath/wcn36xx/hal.h
@@ -4384,11 +4384,13 @@ enum place_holder_in_cap_bitmap {
 	MAX_FEATURE_SUPPORTED = 128,
 };
 
+#define WCN36XX_HAL_CAPS_SIZE 4
+
 struct wcn36xx_hal_feat_caps_msg {
 
 	struct wcn36xx_hal_msg_header header;
 
-	u32 feat_caps[4];
+	u32 feat_caps[WCN36XX_HAL_CAPS_SIZE];
 } __packed;
 
 /* status codes to help debug rekey failures */
diff --git a/drivers/net/wireless/ath/wcn36xx/main.c b/drivers/net/wireless/ath/wcn36xx/main.c
index e64a6784079e..4ab5370ab7a6 100644
--- a/drivers/net/wireless/ath/wcn36xx/main.c
+++ b/drivers/net/wireless/ath/wcn36xx/main.c
@@ -17,6 +17,7 @@
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include <linux/module.h>
+#include <linux/firmware.h>
 #include <linux/platform_device.h>
 #include "wcn36xx.h"
 
@@ -177,6 +178,60 @@ static inline u8 get_sta_index(struct ieee80211_vif *vif,
 	       sta_priv->sta_index;
 }
 
+static const char * const wcn36xx_caps_names[] = {
+	"MCC",				/* 0 */
+	"P2P",				/* 1 */
+	"DOT11AC",			/* 2 */
+	"SLM_SESSIONIZATION",		/* 3 */
+	"DOT11AC_OPMODE",		/* 4 */
+	"SAP32STA",			/* 5 */
+	"TDLS",				/* 6 */
+	"P2P_GO_NOA_DECOUPLE_INIT_SCAN",/* 7 */
+	"WLANACTIVE_OFFLOAD",		/* 8 */
+	"BEACON_OFFLOAD",		/* 9 */
+	"SCAN_OFFLOAD",			/* 10 */
+	"ROAM_OFFLOAD",			/* 11 */
+	"BCN_MISS_OFFLOAD",		/* 12 */
+	"STA_POWERSAVE",		/* 13 */
+	"STA_ADVANCED_PWRSAVE",		/* 14 */
+	"AP_UAPSD",			/* 15 */
+	"AP_DFS",			/* 16 */
+	"BLOCKACK",			/* 17 */
+	"PHY_ERR",			/* 18 */
+	"BCN_FILTER",			/* 19 */
+	"RTT",				/* 20 */
+	"RATECTRL",			/* 21 */
+	"WOW"				/* 22 */
+};
+
+static const char *wcn36xx_get_cap_name(enum place_holder_in_cap_bitmap x)
+{
+	if (x >= ARRAY_SIZE(wcn36xx_caps_names))
+		return "UNKNOWN";
+	return wcn36xx_caps_names[x];
+}
+
+static void wcn36xx_feat_caps_info(struct wcn36xx *wcn)
+{
+	int i;
+
+	for (i = 0; i < MAX_FEATURE_SUPPORTED; i++) {
+		if (get_feat_caps(wcn->fw_feat_caps, i))
+			wcn36xx_info("FW Cap %s\n", wcn36xx_get_cap_name(i));
+	}
+}
+
+static void wcn36xx_detect_chip_version(struct wcn36xx *wcn)
+{
+	if (get_feat_caps(wcn->fw_feat_caps, DOT11AC)) {
+		wcn36xx_info("Chip is 3680\n");
+		wcn->chip_version = WCN36XX_CHIP_3680;
+	} else {
+		wcn36xx_info("Chip is 3660\n");
+		wcn->chip_version = WCN36XX_CHIP_3660;
+	}
+}
+
 static int wcn36xx_start(struct ieee80211_hw *hw)
 {
 	struct wcn36xx *wcn = hw->priv;
@@ -223,6 +278,16 @@ static int wcn36xx_start(struct ieee80211_hw *hw)
 		goto out_free_smd_buf;
 	}
 
+	if (!wcn36xx_is_fw_version(wcn, 1, 2, 2, 24)) {
+		ret = wcn36xx_smd_feature_caps_exchange(wcn);
+		if (ret)
+			wcn36xx_warn("Exchange feature caps failed\n");
+		else
+			wcn36xx_feat_caps_info(wcn);
+	}
+
+	wcn36xx_detect_chip_version(wcn);
+
 	/* DMA channel initialization */
 	ret = wcn36xx_dxe_init(wcn);
 	if (ret) {
@@ -232,11 +297,6 @@ static int wcn36xx_start(struct ieee80211_hw *hw)
 
 	wcn36xx_debugfs_init(wcn);
 
-	if (!wcn36xx_is_fw_version(wcn, 1, 2, 2, 24)) {
-		ret = wcn36xx_smd_feature_caps_exchange(wcn);
-		if (ret)
-			wcn36xx_warn("Exchange feature caps failed\n");
-	}
 	INIT_LIST_HEAD(&wcn->vif_list);
 	return 0;
 
@@ -648,6 +708,7 @@ static void wcn36xx_bss_info_changed(struct ieee80211_hw *hw,
 			    bss_conf->enable_beacon);
 
 		if (bss_conf->enable_beacon) {
+			vif_priv->dtim_period = bss_conf->dtim_period;
 			vif_priv->bss_index = 0xff;
 			wcn36xx_smd_config_bss(wcn, vif, NULL,
 					       vif->addr, false);
@@ -992,6 +1053,7 @@ static int wcn36xx_remove(struct platform_device *pdev)
 	struct wcn36xx *wcn = hw->priv;
 	wcn36xx_dbg(WCN36XX_DBG_MAC, "platform remove\n");
 
+	release_firmware(wcn->nv);
 	mutex_destroy(&wcn->hal_mutex);
 
 	ieee80211_unregister_hw(hw);
diff --git a/drivers/net/wireless/ath/wcn36xx/smd.c b/drivers/net/wireless/ath/wcn36xx/smd.c
index 750626b0e22d..7bf0ef8a1f56 100644
--- a/drivers/net/wireless/ath/wcn36xx/smd.c
+++ b/drivers/net/wireless/ath/wcn36xx/smd.c
@@ -195,9 +195,11 @@ static void wcn36xx_smd_set_sta_params(struct wcn36xx *wcn,
 static int wcn36xx_smd_send_and_wait(struct wcn36xx *wcn, size_t len)
 {
 	int ret = 0;
+	unsigned long start;
 	wcn36xx_dbg_dump(WCN36XX_DBG_SMD_DUMP, "HAL >>> ", wcn->hal_buf, len);
 
 	init_completion(&wcn->hal_rsp_compl);
+	start = jiffies;
 	ret = wcn->ctrl_ops->tx(wcn->hal_buf, len);
 	if (ret) {
 		wcn36xx_err("HAL TX failed\n");
@@ -205,10 +207,13 @@ static int wcn36xx_smd_send_and_wait(struct wcn36xx *wcn, size_t len)
 	}
 	if (wait_for_completion_timeout(&wcn->hal_rsp_compl,
 		msecs_to_jiffies(HAL_MSG_TIMEOUT)) <= 0) {
-		wcn36xx_err("Timeout while waiting SMD response\n");
+		wcn36xx_err("Timeout! No SMD response in %dms\n",
+			    HAL_MSG_TIMEOUT);
 		ret = -ETIME;
 		goto out;
 	}
+	wcn36xx_dbg(WCN36XX_DBG_SMD, "SMD command completed in %dms",
+		    jiffies_to_msecs(jiffies - start));
 out:
 	return ret;
 }
@@ -246,21 +251,22 @@ static int wcn36xx_smd_rsp_status_check(void *buf, size_t len)
 
 int wcn36xx_smd_load_nv(struct wcn36xx *wcn)
 {
-	const struct firmware *nv;
 	struct nv_data *nv_d;
 	struct wcn36xx_hal_nv_img_download_req_msg msg_body;
 	int fw_bytes_left;
 	int ret;
 	u16 fm_offset = 0;
 
-	ret = request_firmware(&nv, WLAN_NV_FILE, wcn->dev);
-	if (ret) {
-		wcn36xx_err("Failed to load nv file %s: %d\n",
-			      WLAN_NV_FILE, ret);
-		goto out_free_nv;
+	if (!wcn->nv) {
+		ret = request_firmware(&wcn->nv, WLAN_NV_FILE, wcn->dev);
+		if (ret) {
+			wcn36xx_err("Failed to load nv file %s: %d\n",
+				      WLAN_NV_FILE, ret);
+			goto out;
+		}
 	}
 
-	nv_d = (struct nv_data *)nv->data;
+	nv_d = (struct nv_data *)wcn->nv->data;
 	INIT_HAL_MSG(msg_body, WCN36XX_HAL_DOWNLOAD_NV_REQ);
 
 	msg_body.header.len += WCN36XX_NV_FRAGMENT_SIZE;
@@ -270,7 +276,7 @@ int wcn36xx_smd_load_nv(struct wcn36xx *wcn)
 	mutex_lock(&wcn->hal_mutex);
 
 	do {
-		fw_bytes_left = nv->size - fm_offset - 4;
+		fw_bytes_left = wcn->nv->size - fm_offset - 4;
 		if (fw_bytes_left > WCN36XX_NV_FRAGMENT_SIZE) {
 			msg_body.last_fragment = 0;
 			msg_body.nv_img_buffer_size = WCN36XX_NV_FRAGMENT_SIZE;
@@ -308,10 +314,7 @@ int wcn36xx_smd_load_nv(struct wcn36xx *wcn)
 
 out_unlock:
 	mutex_unlock(&wcn->hal_mutex);
-out_free_nv:
-	release_firmware(nv);
-
-	return ret;
+out:	return ret;
 }
 
 static int wcn36xx_smd_start_rsp(struct wcn36xx *wcn, void *buf, size_t len)
@@ -899,11 +902,12 @@ static int wcn36xx_smd_config_sta_rsp(struct wcn36xx *wcn,
 
 	sta_priv->sta_index = params->sta_index;
 	sta_priv->dpu_desc_index = params->dpu_index;
+	sta_priv->ucast_dpu_sign = params->uc_ucast_sig;
 
 	wcn36xx_dbg(WCN36XX_DBG_HAL,
-		    "hal config sta rsp status %d sta_index %d bssid_index %d p2p %d\n",
+		    "hal config sta rsp status %d sta_index %d bssid_index %d uc_ucast_sig %d p2p %d\n",
 		    params->status, params->sta_index, params->bssid_index,
-		    params->p2p);
+		    params->uc_ucast_sig, params->p2p);
 
 	return 0;
 }
@@ -1118,7 +1122,7 @@ static int wcn36xx_smd_config_bss_rsp(struct wcn36xx *wcn,
 		priv_vif->sta->bss_dpu_desc_index = params->dpu_desc_index;
 	}
 
-	priv_vif->ucast_dpu_signature = params->ucast_dpu_signature;
+	priv_vif->self_ucast_dpu_sign = params->ucast_dpu_signature;
 
 	return 0;
 }
@@ -1637,12 +1641,12 @@ int wcn36xx_smd_keep_alive_req(struct wcn36xx *wcn,
 
 	ret = wcn36xx_smd_send_and_wait(wcn, msg_body.header.len);
 	if (ret) {
-		wcn36xx_err("Sending hal_exit_bmps failed\n");
+		wcn36xx_err("Sending hal_keep_alive failed\n");
 		goto out;
 	}
 	ret = wcn36xx_smd_rsp_status_check(wcn->hal_buf, wcn->hal_rsp_len);
 	if (ret) {
-		wcn36xx_err("hal_exit_bmps response failed err=%d\n", ret);
+		wcn36xx_err("hal_keep_alive response failed err=%d\n", ret);
 		goto out;
 	}
 out:
@@ -1682,8 +1686,7 @@ out:
 	return ret;
 }
 
-static inline void set_feat_caps(u32 *bitmap,
-				 enum place_holder_in_cap_bitmap cap)
+void set_feat_caps(u32 *bitmap, enum place_holder_in_cap_bitmap cap)
 {
 	int arr_idx, bit_idx;
 
@@ -1697,8 +1700,7 @@ static inline void set_feat_caps(u32 *bitmap,
 	bitmap[arr_idx] |= (1 << bit_idx);
 }
 
-static inline int get_feat_caps(u32 *bitmap,
-				enum place_holder_in_cap_bitmap cap)
+int get_feat_caps(u32 *bitmap, enum place_holder_in_cap_bitmap cap)
 {
 	int arr_idx, bit_idx;
 	int ret = 0;
@@ -1714,8 +1716,7 @@ static inline int get_feat_caps(u32 *bitmap,
 	return ret;
 }
 
-static inline void clear_feat_caps(u32 *bitmap,
-				enum place_holder_in_cap_bitmap cap)
+void clear_feat_caps(u32 *bitmap, enum place_holder_in_cap_bitmap cap)
 {
 	int arr_idx, bit_idx;
 
@@ -1731,8 +1732,8 @@ static inline void clear_feat_caps(u32 *bitmap,
 
 int wcn36xx_smd_feature_caps_exchange(struct wcn36xx *wcn)
 {
-	struct wcn36xx_hal_feat_caps_msg msg_body;
-	int ret = 0;
+	struct wcn36xx_hal_feat_caps_msg msg_body, *rsp;
+	int ret = 0, i;
 
 	mutex_lock(&wcn->hal_mutex);
 	INIT_HAL_MSG(msg_body, WCN36XX_HAL_FEATURE_CAPS_EXCHANGE_REQ);
@@ -1746,12 +1747,15 @@ int wcn36xx_smd_feature_caps_exchange(struct wcn36xx *wcn)
 		wcn36xx_err("Sending hal_feature_caps_exchange failed\n");
 		goto out;
 	}
-	ret = wcn36xx_smd_rsp_status_check(wcn->hal_buf, wcn->hal_rsp_len);
-	if (ret) {
-		wcn36xx_err("hal_feature_caps_exchange response failed err=%d\n",
-			    ret);
+	if (wcn->hal_rsp_len != sizeof(*rsp)) {
+		wcn36xx_err("Invalid hal_feature_caps_exchange response");
 		goto out;
 	}
+
+	rsp = (struct wcn36xx_hal_feat_caps_msg *) wcn->hal_buf;
+
+	for (i = 0; i < WCN36XX_HAL_CAPS_SIZE; i++)
+		wcn->fw_feat_caps[i] = rsp->feat_caps[i];
 out:
 	mutex_unlock(&wcn->hal_mutex);
 	return ret;
diff --git a/drivers/net/wireless/ath/wcn36xx/smd.h b/drivers/net/wireless/ath/wcn36xx/smd.h
index e7c39019c6f1..008d03423dbf 100644
--- a/drivers/net/wireless/ath/wcn36xx/smd.h
+++ b/drivers/net/wireless/ath/wcn36xx/smd.h
@@ -24,7 +24,7 @@
 
 #define WCN36XX_HAL_BUF_SIZE				4096
 
-#define HAL_MSG_TIMEOUT 200
+#define HAL_MSG_TIMEOUT 500
 #define WCN36XX_SMSM_WLAN_TX_ENABLE			0x00000400
 #define WCN36XX_SMSM_WLAN_TX_RINGS_EMPTY		0x00000200
 /* The PNO version info be contained in the rsp msg */
@@ -112,6 +112,9 @@ int wcn36xx_smd_keep_alive_req(struct wcn36xx *wcn,
 int wcn36xx_smd_dump_cmd_req(struct wcn36xx *wcn, u32 arg1, u32 arg2,
 			     u32 arg3, u32 arg4, u32 arg5);
 int wcn36xx_smd_feature_caps_exchange(struct wcn36xx *wcn);
+void set_feat_caps(u32 *bitmap, enum place_holder_in_cap_bitmap cap);
+int get_feat_caps(u32 *bitmap, enum place_holder_in_cap_bitmap cap);
+void clear_feat_caps(u32 *bitmap, enum place_holder_in_cap_bitmap cap);
 
 int wcn36xx_smd_add_ba_session(struct wcn36xx *wcn,
 		struct ieee80211_sta *sta,
diff --git a/drivers/net/wireless/ath/wcn36xx/txrx.c b/drivers/net/wireless/ath/wcn36xx/txrx.c
index b2b60e30caaf..32bb26a0db2a 100644
--- a/drivers/net/wireless/ath/wcn36xx/txrx.c
+++ b/drivers/net/wireless/ath/wcn36xx/txrx.c
@@ -57,8 +57,7 @@ int wcn36xx_rx_skb(struct wcn36xx *wcn, struct sk_buff *skb)
 		       RX_FLAG_MMIC_STRIPPED |
 		       RX_FLAG_DECRYPTED;
 
-	wcn36xx_dbg(WCN36XX_DBG_RX, "status.flags=%x status->vendor_radiotap_len=%x\n",
-		    status.flag,  status.vendor_radiotap_len);
+	wcn36xx_dbg(WCN36XX_DBG_RX, "status.flags=%x\n", status.flag);
 
 	memcpy(IEEE80211_SKB_RXCB(skb), &status, sizeof(status));
 
@@ -132,6 +131,7 @@ static void wcn36xx_set_tx_data(struct wcn36xx_tx_bd *bd,
 				   struct ieee80211_vif,
 				   drv_priv);
 
+		bd->dpu_sign = sta_priv->ucast_dpu_sign;
 		if (vif->type == NL80211_IFTYPE_STATION) {
 			bd->sta_index = sta_priv->bss_sta_index;
 			bd->dpu_desc_idx = sta_priv->bss_dpu_desc_index;
@@ -145,10 +145,9 @@ static void wcn36xx_set_tx_data(struct wcn36xx_tx_bd *bd,
 		__vif_priv = get_vif_by_addr(wcn, hdr->addr2);
 		bd->sta_index = __vif_priv->self_sta_index;
 		bd->dpu_desc_idx = __vif_priv->self_dpu_desc_index;
+		bd->dpu_sign = __vif_priv->self_ucast_dpu_sign;
 	}
 
-	bd->dpu_sign = __vif_priv->ucast_dpu_signature;
-
 	if (ieee80211_is_nullfunc(hdr->frame_control) ||
 	   (sta_priv && !sta_priv->is_data_encrypted))
 		bd->dpu_ne = 1;
diff --git a/drivers/net/wireless/ath/wcn36xx/wcn36xx.h b/drivers/net/wireless/ath/wcn36xx/wcn36xx.h
index 8fa5cbace5ab..f0fb81dfd17b 100644
--- a/drivers/net/wireless/ath/wcn36xx/wcn36xx.h
+++ b/drivers/net/wireless/ath/wcn36xx/wcn36xx.h
@@ -125,10 +125,10 @@ struct wcn36xx_vif {
 	enum wcn36xx_power_state pw_state;
 
 	u8 bss_index;
-	u8 ucast_dpu_signature;
 	/* Returned from WCN36XX_HAL_ADD_STA_SELF_RSP */
 	u8 self_sta_index;
 	u8 self_dpu_desc_index;
+	u8 self_ucast_dpu_sign;
 };
 
 /**
@@ -159,6 +159,7 @@ struct wcn36xx_sta {
 	u16 tid;
 	u8 sta_index;
 	u8 dpu_desc_index;
+	u8 ucast_dpu_sign;
 	u8 bss_sta_index;
 	u8 bss_dpu_desc_index;
 	bool is_data_encrypted;
@@ -171,10 +172,14 @@ struct wcn36xx {
 	struct device		*dev;
 	struct list_head	vif_list;
 
+	const struct firmware	*nv;
+
 	u8			fw_revision;
 	u8			fw_version;
 	u8			fw_minor;
 	u8			fw_major;
+	u32			fw_feat_caps[WCN36XX_HAL_CAPS_SIZE];
+	u32			chip_version;
 
 	/* extra byte for the NULL termination */
 	u8			crm_version[WCN36XX_HAL_VERSION_LENGTH + 1];
@@ -222,6 +227,9 @@ struct wcn36xx {
 
 };
 
+#define WCN36XX_CHIP_3660	0
+#define WCN36XX_CHIP_3680	1
+
 static inline bool wcn36xx_is_fw_version(struct wcn36xx *wcn,
 					 u8 major,
 					 u8 minor,
diff --git a/drivers/net/wireless/ath/wil6210/Makefile b/drivers/net/wireless/ath/wil6210/Makefile
index 990dd42ae79e..c7a3465fd02a 100644
--- a/drivers/net/wireless/ath/wil6210/Makefile
+++ b/drivers/net/wireless/ath/wil6210/Makefile
@@ -9,6 +9,7 @@ wil6210-y += wmi.o
 wil6210-y += interrupt.o
 wil6210-y += txrx.o
 wil6210-y += debug.o
+wil6210-y += rx_reorder.o
 wil6210-$(CONFIG_WIL6210_TRACING) += trace.o
 
 # for tracing framework to find trace.h
diff --git a/drivers/net/wireless/ath/wil6210/cfg80211.c b/drivers/net/wireless/ath/wil6210/cfg80211.c
index 5b340769d5bb..743930357061 100644
--- a/drivers/net/wireless/ath/wil6210/cfg80211.c
+++ b/drivers/net/wireless/ath/wil6210/cfg80211.c
@@ -104,41 +104,125 @@ int wil_iftype_nl2wmi(enum nl80211_iftype type)
 	return -EOPNOTSUPP;
 }
 
-static int wil_cfg80211_get_station(struct wiphy *wiphy,
-				    struct net_device *ndev,
-				    u8 *mac, struct station_info *sinfo)
+static int wil_cid_fill_sinfo(struct wil6210_priv *wil, int cid,
+			      struct station_info *sinfo)
 {
-	struct wil6210_priv *wil = wiphy_to_wil(wiphy);
-	int rc;
 	struct wmi_notify_req_cmd cmd = {
-		.cid = 0,
+		.cid = cid,
 		.interval_usec = 0,
 	};
+	struct {
+		struct wil6210_mbox_hdr_wmi wmi;
+		struct wmi_notify_req_done_event evt;
+	} __packed reply;
+	struct wil_net_stats *stats = &wil->sta[cid].stats;
+	int rc;
 
-	if (memcmp(mac, wil->dst_addr[0], ETH_ALEN))
-		return -ENOENT;
-
-	/* WMI_NOTIFY_REQ_DONE_EVENTID handler fills wil->stats.bf_mcs */
 	rc = wmi_call(wil, WMI_NOTIFY_REQ_CMDID, &cmd, sizeof(cmd),
-		      WMI_NOTIFY_REQ_DONE_EVENTID, NULL, 0, 20);
+		      WMI_NOTIFY_REQ_DONE_EVENTID, &reply, sizeof(reply), 20);
 	if (rc)
 		return rc;
 
+	wil_dbg_wmi(wil, "Link status for CID %d: {\n"
+		    "  MCS %d TSF 0x%016llx\n"
+		    "  BF status 0x%08x SNR 0x%08x SQI %d%%\n"
+		    "  Tx Tpt %d goodput %d Rx goodput %d\n"
+		    "  Sectors(rx:tx) my %d:%d peer %d:%d\n""}\n",
+		    cid, le16_to_cpu(reply.evt.bf_mcs),
+		    le64_to_cpu(reply.evt.tsf), reply.evt.status,
+		    le32_to_cpu(reply.evt.snr_val),
+		    reply.evt.sqi,
+		    le32_to_cpu(reply.evt.tx_tpt),
+		    le32_to_cpu(reply.evt.tx_goodput),
+		    le32_to_cpu(reply.evt.rx_goodput),
+		    le16_to_cpu(reply.evt.my_rx_sector),
+		    le16_to_cpu(reply.evt.my_tx_sector),
+		    le16_to_cpu(reply.evt.other_rx_sector),
+		    le16_to_cpu(reply.evt.other_tx_sector));
+
 	sinfo->generation = wil->sinfo_gen;
 
-	sinfo->filled |= STATION_INFO_TX_BITRATE;
+	sinfo->filled = STATION_INFO_RX_BYTES |
+			STATION_INFO_TX_BYTES |
+			STATION_INFO_RX_PACKETS |
+			STATION_INFO_TX_PACKETS |
+			STATION_INFO_RX_BITRATE |
+			STATION_INFO_TX_BITRATE |
+			STATION_INFO_RX_DROP_MISC |
+			STATION_INFO_TX_FAILED;
+
 	sinfo->txrate.flags = RATE_INFO_FLAGS_MCS | RATE_INFO_FLAGS_60G;
-	sinfo->txrate.mcs = wil->stats.bf_mcs;
-	sinfo->filled |= STATION_INFO_RX_BITRATE;
+	sinfo->txrate.mcs = le16_to_cpu(reply.evt.bf_mcs);
 	sinfo->rxrate.flags = RATE_INFO_FLAGS_MCS | RATE_INFO_FLAGS_60G;
-	sinfo->rxrate.mcs = wil->stats.last_mcs_rx;
+	sinfo->rxrate.mcs = stats->last_mcs_rx;
+	sinfo->rx_bytes = stats->rx_bytes;
+	sinfo->rx_packets = stats->rx_packets;
+	sinfo->rx_dropped_misc = stats->rx_dropped;
+	sinfo->tx_bytes = stats->tx_bytes;
+	sinfo->tx_packets = stats->tx_packets;
+	sinfo->tx_failed = stats->tx_errors;
 
 	if (test_bit(wil_status_fwconnected, &wil->status)) {
 		sinfo->filled |= STATION_INFO_SIGNAL;
-		sinfo->signal = 12; /* TODO: provide real value */
+		sinfo->signal = reply.evt.sqi;
 	}
 
-	return 0;
+	return rc;
+}
+
+static int wil_cfg80211_get_station(struct wiphy *wiphy,
+				    struct net_device *ndev,
+				    u8 *mac, struct station_info *sinfo)
+{
+	struct wil6210_priv *wil = wiphy_to_wil(wiphy);
+	int rc;
+
+	int cid = wil_find_cid(wil, mac);
+
+	wil_info(wil, "%s(%pM) CID %d\n", __func__, mac, cid);
+	if (cid < 0)
+		return cid;
+
+	rc = wil_cid_fill_sinfo(wil, cid, sinfo);
+
+	return rc;
+}
+
+/*
+ * Find @idx-th active STA for station dump.
+ */
+static int wil_find_cid_by_idx(struct wil6210_priv *wil, int idx)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(wil->sta); i++) {
+		if (wil->sta[i].status == wil_sta_unused)
+			continue;
+		if (idx == 0)
+			return i;
+		idx--;
+	}
+
+	return -ENOENT;
+}
+
+static int wil_cfg80211_dump_station(struct wiphy *wiphy,
+				     struct net_device *dev, int idx,
+				     u8 *mac, struct station_info *sinfo)
+{
+	struct wil6210_priv *wil = wiphy_to_wil(wiphy);
+	int rc;
+	int cid = wil_find_cid_by_idx(wil, idx);
+
+	if (cid < 0)
+		return -ENOENT;
+
+	memcpy(mac, wil->sta[cid].addr, ETH_ALEN);
+	wil_info(wil, "%s(%pM) CID %d\n", __func__, mac, cid);
+
+	rc = wil_cid_fill_sinfo(wil, cid, sinfo);
+
+	return rc;
 }
 
 static int wil_cfg80211_change_iface(struct wiphy *wiphy,
@@ -352,6 +436,40 @@ static int wil_cfg80211_disconnect(struct wiphy *wiphy,
 	return rc;
 }
 
+static int wil_cfg80211_mgmt_tx(struct wiphy *wiphy,
+				struct wireless_dev *wdev,
+				struct cfg80211_mgmt_tx_params *params,
+				u64 *cookie)
+{
+	const u8 *buf = params->buf;
+	size_t len = params->len;
+	struct wil6210_priv *wil = wiphy_to_wil(wiphy);
+	int rc;
+	struct ieee80211_mgmt *mgmt_frame = (void *)buf;
+	struct wmi_sw_tx_req_cmd *cmd;
+	struct {
+		struct wil6210_mbox_hdr_wmi wmi;
+		struct wmi_sw_tx_complete_event evt;
+	} __packed evt;
+
+	cmd = kmalloc(sizeof(*cmd) + len, GFP_KERNEL);
+	if (!cmd)
+		return -ENOMEM;
+
+	memcpy(cmd->dst_mac, mgmt_frame->da, WMI_MAC_LEN);
+	cmd->len = cpu_to_le16(len);
+	memcpy(cmd->payload, buf, len);
+
+	rc = wmi_call(wil, WMI_SW_TX_REQ_CMDID, cmd, sizeof(*cmd) + len,
+		      WMI_SW_TX_COMPLETE_EVENTID, &evt, sizeof(evt), 2000);
+	if (rc == 0)
+		rc = evt.evt.status;
+
+	kfree(cmd);
+
+	return rc;
+}
+
 static int wil_cfg80211_set_channel(struct wiphy *wiphy,
 				    struct cfg80211_chan_def *chandef)
 {
@@ -402,6 +520,41 @@ static int wil_cfg80211_set_default_key(struct wiphy *wiphy,
 	return 0;
 }
 
+static int wil_remain_on_channel(struct wiphy *wiphy,
+				 struct wireless_dev *wdev,
+				 struct ieee80211_channel *chan,
+				 unsigned int duration,
+				 u64 *cookie)
+{
+	struct wil6210_priv *wil = wiphy_to_wil(wiphy);
+	int rc;
+
+	/* TODO: handle duration */
+	wil_info(wil, "%s(%d, %d ms)\n", __func__, chan->center_freq, duration);
+
+	rc = wmi_set_channel(wil, chan->hw_value);
+	if (rc)
+		return rc;
+
+	rc = wmi_rxon(wil, true);
+
+	return rc;
+}
+
+static int wil_cancel_remain_on_channel(struct wiphy *wiphy,
+					struct wireless_dev *wdev,
+					u64 cookie)
+{
+	struct wil6210_priv *wil = wiphy_to_wil(wiphy);
+	int rc;
+
+	wil_info(wil, "%s()\n", __func__);
+
+	rc = wmi_rxon(wil, false);
+
+	return rc;
+}
+
 static int wil_fix_bcon(struct wil6210_priv *wil,
 			struct cfg80211_beacon_data *bcon)
 {
@@ -504,12 +657,24 @@ static int wil_cfg80211_stop_ap(struct wiphy *wiphy,
 	return rc;
 }
 
+static int wil_cfg80211_del_station(struct wiphy *wiphy,
+				    struct net_device *dev, u8 *mac)
+{
+	struct wil6210_priv *wil = wiphy_to_wil(wiphy);
+	wil6210_disconnect(wil, mac);
+	return 0;
+}
+
 static struct cfg80211_ops wil_cfg80211_ops = {
 	.scan = wil_cfg80211_scan,
 	.connect = wil_cfg80211_connect,
 	.disconnect = wil_cfg80211_disconnect,
 	.change_virtual_intf = wil_cfg80211_change_iface,
 	.get_station = wil_cfg80211_get_station,
+	.dump_station = wil_cfg80211_dump_station,
+	.remain_on_channel = wil_remain_on_channel,
+	.cancel_remain_on_channel = wil_cancel_remain_on_channel,
+	.mgmt_tx = wil_cfg80211_mgmt_tx,
 	.set_monitor_channel = wil_cfg80211_set_channel,
 	.add_key = wil_cfg80211_add_key,
 	.del_key = wil_cfg80211_del_key,
@@ -517,6 +682,7 @@ static struct cfg80211_ops wil_cfg80211_ops = {
 	/* AP mode */
 	.start_ap = wil_cfg80211_start_ap,
 	.stop_ap = wil_cfg80211_stop_ap,
+	.del_station = wil_cfg80211_del_station,
 };
 
 static void wil_wiphy_init(struct wiphy *wiphy)
@@ -542,7 +708,7 @@ static void wil_wiphy_init(struct wiphy *wiphy)
 	wiphy->bands[IEEE80211_BAND_60GHZ] = &wil_band_60ghz;
 
 	/* TODO: figure this out */
-	wiphy->signal_type = CFG80211_SIGNAL_TYPE_MBM;
+	wiphy->signal_type = CFG80211_SIGNAL_TYPE_UNSPEC;
 
 	wiphy->cipher_suites = wil_cipher_suites;
 	wiphy->n_cipher_suites = ARRAY_SIZE(wil_cipher_suites);
diff --git a/drivers/net/wireless/ath/wil6210/debugfs.c b/drivers/net/wireless/ath/wil6210/debugfs.c
index 1caa31992a7e..1d09a4b0a0f4 100644
--- a/drivers/net/wireless/ath/wil6210/debugfs.c
+++ b/drivers/net/wireless/ath/wil6210/debugfs.c
@@ -26,9 +26,11 @@
 /* Nasty hack. Better have per device instances */
 static u32 mem_addr;
 static u32 dbg_txdesc_index;
+static u32 dbg_vring_index; /* 24+ for Rx, 0..23 for Tx */
 
 static void wil_print_vring(struct seq_file *s, struct wil6210_priv *wil,
-			    const char *name, struct vring *vring)
+			    const char *name, struct vring *vring,
+			    char _s, char _h)
 {
 	void __iomem *x = wmi_addr(wil, vring->hwtail);
 
@@ -50,8 +52,8 @@ static void wil_print_vring(struct seq_file *s, struct wil6210_priv *wil,
 			volatile struct vring_tx_desc *d = &vring->va[i].tx;
 			if ((i % 64) == 0 && (i != 0))
 				seq_printf(s, "\n");
-			seq_printf(s, "%s", (d->dma.status & BIT(0)) ?
-					"S" : (vring->ctx[i].skb ? "H" : "h"));
+			seq_printf(s, "%c", (d->dma.status & BIT(0)) ?
+					_s : (vring->ctx[i].skb ? _h : 'h'));
 		}
 		seq_printf(s, "\n");
 	}
@@ -63,14 +65,19 @@ static int wil_vring_debugfs_show(struct seq_file *s, void *data)
 	uint i;
 	struct wil6210_priv *wil = s->private;
 
-	wil_print_vring(s, wil, "rx", &wil->vring_rx);
+	wil_print_vring(s, wil, "rx", &wil->vring_rx, 'S', '_');
 
 	for (i = 0; i < ARRAY_SIZE(wil->vring_tx); i++) {
 		struct vring *vring = &(wil->vring_tx[i]);
 		if (vring->va) {
+			int cid = wil->vring2cid_tid[i][0];
+			int tid = wil->vring2cid_tid[i][1];
 			char name[10];
 			snprintf(name, sizeof(name), "tx_%2d", i);
-			wil_print_vring(s, wil, name, vring);
+
+			seq_printf(s, "\n%pM CID %d TID %d\n",
+				   wil->sta[cid].addr, cid, tid);
+			wil_print_vring(s, wil, name, vring, '_', 'H');
 		}
 	}
 
@@ -390,25 +397,40 @@ static const struct file_operations fops_reset = {
 	.write = wil_write_file_reset,
 	.open  = simple_open,
 };
-/*---------Tx descriptor------------*/
 
+/*---------Tx/Rx descriptor------------*/
 static int wil_txdesc_debugfs_show(struct seq_file *s, void *data)
 {
 	struct wil6210_priv *wil = s->private;
-	struct vring *vring = &(wil->vring_tx[0]);
+	struct vring *vring;
+	bool tx = (dbg_vring_index < WIL6210_MAX_TX_RINGS);
+	if (tx)
+		vring = &(wil->vring_tx[dbg_vring_index]);
+	else
+		vring = &wil->vring_rx;
 
 	if (!vring->va) {
-		seq_printf(s, "No Tx VRING\n");
+		if (tx)
+			seq_printf(s, "No Tx[%2d] VRING\n", dbg_vring_index);
+		else
+			seq_puts(s, "No Rx VRING\n");
 		return 0;
 	}
 
 	if (dbg_txdesc_index < vring->size) {
+		/* use struct vring_tx_desc for Rx as well,
+		 * only field used, .dma.length, is the same
+		 */
 		volatile struct vring_tx_desc *d =
 				&(vring->va[dbg_txdesc_index].tx);
 		volatile u32 *u = (volatile u32 *)d;
 		struct sk_buff *skb = vring->ctx[dbg_txdesc_index].skb;
 
-		seq_printf(s, "Tx[%3d] = {\n", dbg_txdesc_index);
+		if (tx)
+			seq_printf(s, "Tx[%2d][%3d] = {\n", dbg_vring_index,
+				   dbg_txdesc_index);
+		else
+			seq_printf(s, "Rx[%3d] = {\n", dbg_txdesc_index);
 		seq_printf(s, "  MAC = 0x%08x 0x%08x 0x%08x 0x%08x\n",
 			   u[0], u[1], u[2], u[3]);
 		seq_printf(s, "  DMA = 0x%08x 0x%08x 0x%08x 0x%08x\n",
@@ -439,8 +461,13 @@ static int wil_txdesc_debugfs_show(struct seq_file *s, void *data)
 		}
 		seq_printf(s, "}\n");
 	} else {
-		seq_printf(s, "TxDesc index (%d) >= size (%d)\n",
-			   dbg_txdesc_index, vring->size);
+		if (tx)
+			seq_printf(s, "[%2d] TxDesc index (%d) >= size (%d)\n",
+				   dbg_vring_index, dbg_txdesc_index,
+				   vring->size);
+		else
+			seq_printf(s, "RxDesc index (%d) >= size (%d)\n",
+				   dbg_txdesc_index, vring->size);
 	}
 
 	return 0;
@@ -570,6 +597,68 @@ static const struct file_operations fops_temp = {
 	.llseek		= seq_lseek,
 };
 
+/*---------Station matrix------------*/
+static void wil_print_rxtid(struct seq_file *s, struct wil_tid_ampdu_rx *r)
+{
+	int i;
+	u16 index = ((r->head_seq_num - r->ssn) & 0xfff) % r->buf_size;
+	seq_printf(s, "0x%03x [", r->head_seq_num);
+	for (i = 0; i < r->buf_size; i++) {
+		if (i == index)
+			seq_printf(s, "%c", r->reorder_buf[i] ? 'O' : '|');
+		else
+			seq_printf(s, "%c", r->reorder_buf[i] ? '*' : '_');
+	}
+	seq_puts(s, "]\n");
+}
+
+static int wil_sta_debugfs_show(struct seq_file *s, void *data)
+{
+	struct wil6210_priv *wil = s->private;
+	int i, tid;
+
+	for (i = 0; i < ARRAY_SIZE(wil->sta); i++) {
+		struct wil_sta_info *p = &wil->sta[i];
+		char *status = "unknown";
+		switch (p->status) {
+		case wil_sta_unused:
+			status = "unused   ";
+			break;
+		case wil_sta_conn_pending:
+			status = "pending  ";
+			break;
+		case wil_sta_connected:
+			status = "connected";
+			break;
+		}
+		seq_printf(s, "[%d] %pM %s\n", i, p->addr, status);
+
+		if (p->status == wil_sta_connected) {
+			for (tid = 0; tid < WIL_STA_TID_NUM; tid++) {
+				struct wil_tid_ampdu_rx *r = p->tid_rx[tid];
+				if (r) {
+					seq_printf(s, "[%2d] ", tid);
+					wil_print_rxtid(s, r);
+				}
+			}
+		}
+	}
+
+	return 0;
+}
+
+static int wil_sta_seq_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, wil_sta_debugfs_show, inode->i_private);
+}
+
+static const struct file_operations fops_sta = {
+	.open		= wil_sta_seq_open,
+	.release	= single_release,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+};
+
 /*----------------*/
 int wil6210_debugfs_init(struct wil6210_priv *wil)
 {
@@ -581,9 +670,13 @@ int wil6210_debugfs_init(struct wil6210_priv *wil)
 
 	debugfs_create_file("mbox", S_IRUGO, dbg, wil, &fops_mbox);
 	debugfs_create_file("vrings", S_IRUGO, dbg, wil, &fops_vring);
-	debugfs_create_file("txdesc", S_IRUGO, dbg, wil, &fops_txdesc);
-	debugfs_create_u32("txdesc_index", S_IRUGO | S_IWUSR, dbg,
+	debugfs_create_file("stations", S_IRUGO, dbg, wil, &fops_sta);
+	debugfs_create_file("desc", S_IRUGO, dbg, wil, &fops_txdesc);
+	debugfs_create_u32("desc_index", S_IRUGO | S_IWUSR, dbg,
 			   &dbg_txdesc_index);
+	debugfs_create_u32("vring_index", S_IRUGO | S_IWUSR, dbg,
+			   &dbg_vring_index);
+
 	debugfs_create_file("bf", S_IRUGO, dbg, wil, &fops_bf);
 	debugfs_create_file("ssid", S_IRUGO | S_IWUSR, dbg, wil, &fops_ssid);
 	debugfs_create_u32("secure_pcp", S_IRUGO | S_IWUSR, dbg,
diff --git a/drivers/net/wireless/ath/wil6210/main.c b/drivers/net/wireless/ath/wil6210/main.c
index fd30cddd5882..41c362dee032 100644
--- a/drivers/net/wireless/ath/wil6210/main.c
+++ b/drivers/net/wireless/ath/wil6210/main.c
@@ -16,8 +16,10 @@
 
 #include <linux/moduleparam.h>
 #include <linux/if_arp.h>
+#include <linux/etherdevice.h>
 
 #include "wil6210.h"
+#include "txrx.h"
 
 /*
  * Due to a hardware issue,
@@ -52,29 +54,75 @@ void wil_memcpy_toio_32(volatile void __iomem *dst, const void *src,
 		__raw_writel(*s++, d++);
 }
 
-static void _wil6210_disconnect(struct wil6210_priv *wil, void *bssid)
+static void wil_disconnect_cid(struct wil6210_priv *wil, int cid)
 {
 	uint i;
-	struct net_device *ndev = wil_to_ndev(wil);
+	struct wil_sta_info *sta = &wil->sta[cid];
 
-	wil_dbg_misc(wil, "%s()\n", __func__);
+	if (sta->status != wil_sta_unused) {
+		wmi_disconnect_sta(wil, sta->addr, WLAN_REASON_DEAUTH_LEAVING);
+		sta->status = wil_sta_unused;
+	}
+
+	for (i = 0; i < WIL_STA_TID_NUM; i++) {
+		struct wil_tid_ampdu_rx *r = sta->tid_rx[i];
+		sta->tid_rx[i] = NULL;
+		wil_tid_ampdu_rx_free(wil, r);
+	}
+	for (i = 0; i < ARRAY_SIZE(wil->vring_tx); i++) {
+		if (wil->vring2cid_tid[i][0] == cid)
+			wil_vring_fini_tx(wil, i);
+	}
+	memset(&sta->stats, 0, sizeof(sta->stats));
+}
+
+static void _wil6210_disconnect(struct wil6210_priv *wil, void *bssid)
+{
+	int cid = -ENOENT;
+	struct net_device *ndev = wil_to_ndev(wil);
+	struct wireless_dev *wdev = wil->wdev;
 
-	wil_link_off(wil);
-	if (test_bit(wil_status_fwconnected, &wil->status)) {
-		clear_bit(wil_status_fwconnected, &wil->status);
-		cfg80211_disconnected(ndev,
-				      WLAN_STATUS_UNSPECIFIED_FAILURE,
-				      NULL, 0, GFP_KERNEL);
-	} else if (test_bit(wil_status_fwconnecting, &wil->status)) {
-		cfg80211_connect_result(ndev, bssid, NULL, 0, NULL, 0,
-					WLAN_STATUS_UNSPECIFIED_FAILURE,
-					GFP_KERNEL);
+	might_sleep();
+	if (bssid) {
+		cid = wil_find_cid(wil, bssid);
+		wil_dbg_misc(wil, "%s(%pM, CID %d)\n", __func__, bssid, cid);
+	} else {
+		wil_dbg_misc(wil, "%s(all)\n", __func__);
 	}
-	clear_bit(wil_status_fwconnecting, &wil->status);
-	for (i = 0; i < ARRAY_SIZE(wil->vring_tx); i++)
-		wil_vring_fini_tx(wil, i);
 
-	clear_bit(wil_status_dontscan, &wil->status);
+	if (cid >= 0) /* disconnect 1 peer */
+		wil_disconnect_cid(wil, cid);
+	else /* disconnect all */
+		for (cid = 0; cid < WIL6210_MAX_CID; cid++)
+			wil_disconnect_cid(wil, cid);
+
+	/* link state */
+	switch (wdev->iftype) {
+	case NL80211_IFTYPE_STATION:
+	case NL80211_IFTYPE_P2P_CLIENT:
+		wil_link_off(wil);
+		if (test_bit(wil_status_fwconnected, &wil->status)) {
+			clear_bit(wil_status_fwconnected, &wil->status);
+			cfg80211_disconnected(ndev,
+					      WLAN_STATUS_UNSPECIFIED_FAILURE,
+					      NULL, 0, GFP_KERNEL);
+		} else if (test_bit(wil_status_fwconnecting, &wil->status)) {
+			cfg80211_connect_result(ndev, bssid, NULL, 0, NULL, 0,
+						WLAN_STATUS_UNSPECIFIED_FAILURE,
+						GFP_KERNEL);
+		}
+		clear_bit(wil_status_fwconnecting, &wil->status);
+		wil_dbg_misc(wil, "clear_bit(wil_status_dontscan)\n");
+		clear_bit(wil_status_dontscan, &wil->status);
+		break;
+	default:
+		/* AP-like interface and monitor:
+		 * never scan, always connected
+		 */
+		if (bssid)
+			cfg80211_del_sta(ndev, bssid, GFP_KERNEL);
+		break;
+	}
 }
 
 static void wil_disconnect_worker(struct work_struct *work)
@@ -97,12 +145,23 @@ static void wil_connect_timer_fn(ulong x)
 	schedule_work(&wil->disconnect_worker);
 }
 
+static int wil_find_free_vring(struct wil6210_priv *wil)
+{
+	int i;
+	for (i = 0; i < WIL6210_MAX_TX_RINGS; i++) {
+		if (!wil->vring_tx[i].va)
+			return i;
+	}
+	return -EINVAL;
+}
+
 static void wil_connect_worker(struct work_struct *work)
 {
 	int rc;
 	struct wil6210_priv *wil = container_of(work, struct wil6210_priv,
 						connect_worker);
 	int cid = wil->pending_connect_cid;
+	int ringid = wil_find_free_vring(wil);
 
 	if (cid < 0) {
 		wil_err(wil, "No connection pending\n");
@@ -111,16 +170,22 @@ static void wil_connect_worker(struct work_struct *work)
 
 	wil_dbg_wmi(wil, "Configure for connection CID %d\n", cid);
 
-	rc = wil_vring_init_tx(wil, 0, WIL6210_TX_RING_SIZE, cid, 0);
+	rc = wil_vring_init_tx(wil, ringid, WIL6210_TX_RING_SIZE, cid, 0);
 	wil->pending_connect_cid = -1;
-	if (rc == 0)
+	if (rc == 0) {
+		wil->sta[cid].status = wil_sta_connected;
 		wil_link_on(wil);
+	} else {
+		wil->sta[cid].status = wil_sta_unused;
+	}
 }
 
 int wil_priv_init(struct wil6210_priv *wil)
 {
 	wil_dbg_misc(wil, "%s()\n", __func__);
 
+	memset(wil->sta, 0, sizeof(wil->sta));
+
 	mutex_init(&wil->mutex);
 	mutex_init(&wil->wmi_mutex);
 
@@ -370,3 +435,19 @@ int wil_down(struct wil6210_priv *wil)
 
 	return rc;
 }
+
+int wil_find_cid(struct wil6210_priv *wil, const u8 *mac)
+{
+	int i;
+	int rc = -ENOENT;
+
+	for (i = 0; i < ARRAY_SIZE(wil->sta); i++) {
+		if ((wil->sta[i].status != wil_sta_unused) &&
+		    ether_addr_equal(wil->sta[i].addr, mac)) {
+			rc = i;
+			break;
+		}
+	}
+
+	return rc;
+}
diff --git a/drivers/net/wireless/ath/wil6210/rx_reorder.c b/drivers/net/wireless/ath/wil6210/rx_reorder.c
new file mode 100644
index 000000000000..d04629fe053f
--- /dev/null
+++ b/drivers/net/wireless/ath/wil6210/rx_reorder.c
@@ -0,0 +1,177 @@
+#include "wil6210.h"
+#include "txrx.h"
+
+#define SEQ_MODULO 0x1000
+#define SEQ_MASK   0xfff
+
+static inline int seq_less(u16 sq1, u16 sq2)
+{
+	return ((sq1 - sq2) & SEQ_MASK) > (SEQ_MODULO >> 1);
+}
+
+static inline u16 seq_inc(u16 sq)
+{
+	return (sq + 1) & SEQ_MASK;
+}
+
+static inline u16 seq_sub(u16 sq1, u16 sq2)
+{
+	return (sq1 - sq2) & SEQ_MASK;
+}
+
+static inline int reorder_index(struct wil_tid_ampdu_rx *r, u16 seq)
+{
+	return seq_sub(seq, r->ssn) % r->buf_size;
+}
+
+static void wil_release_reorder_frame(struct wil6210_priv *wil,
+				      struct wil_tid_ampdu_rx *r,
+				      int index)
+{
+	struct net_device *ndev = wil_to_ndev(wil);
+	struct sk_buff *skb = r->reorder_buf[index];
+
+	if (!skb)
+		goto no_frame;
+
+	/* release the frame from the reorder ring buffer */
+	r->stored_mpdu_num--;
+	r->reorder_buf[index] = NULL;
+	wil_netif_rx_any(skb, ndev);
+
+no_frame:
+	r->head_seq_num = seq_inc(r->head_seq_num);
+}
+
+static void wil_release_reorder_frames(struct wil6210_priv *wil,
+				       struct wil_tid_ampdu_rx *r,
+				       u16 hseq)
+{
+	int index;
+
+	while (seq_less(r->head_seq_num, hseq)) {
+		index = reorder_index(r, r->head_seq_num);
+		wil_release_reorder_frame(wil, r, index);
+	}
+}
+
+static void wil_reorder_release(struct wil6210_priv *wil,
+				struct wil_tid_ampdu_rx *r)
+{
+	int index = reorder_index(r, r->head_seq_num);
+
+	while (r->reorder_buf[index]) {
+		wil_release_reorder_frame(wil, r, index);
+		index = reorder_index(r, r->head_seq_num);
+	}
+}
+
+void wil_rx_reorder(struct wil6210_priv *wil, struct sk_buff *skb)
+{
+	struct net_device *ndev = wil_to_ndev(wil);
+	struct vring_rx_desc *d = wil_skb_rxdesc(skb);
+	int tid = wil_rxdesc_tid(d);
+	int cid = wil_rxdesc_cid(d);
+	int mid = wil_rxdesc_mid(d);
+	u16 seq = wil_rxdesc_seq(d);
+	struct wil_sta_info *sta = &wil->sta[cid];
+	struct wil_tid_ampdu_rx *r = sta->tid_rx[tid];
+	u16 hseq;
+	int index;
+
+	wil_dbg_txrx(wil, "MID %d CID %d TID %d Seq 0x%03x\n",
+		     mid, cid, tid, seq);
+
+	if (!r) {
+		wil_netif_rx_any(skb, ndev);
+		return;
+	}
+
+	hseq = r->head_seq_num;
+
+	spin_lock(&r->reorder_lock);
+
+	/* frame with out of date sequence number */
+	if (seq_less(seq, r->head_seq_num)) {
+		dev_kfree_skb(skb);
+		goto out;
+	}
+
+	/*
+	 * If frame the sequence number exceeds our buffering window
+	 * size release some previous frames to make room for this one.
+	 */
+	if (!seq_less(seq, r->head_seq_num + r->buf_size)) {
+		hseq = seq_inc(seq_sub(seq, r->buf_size));
+		/* release stored frames up to new head to stack */
+		wil_release_reorder_frames(wil, r, hseq);
+	}
+
+	/* Now the new frame is always in the range of the reordering buffer */
+
+	index = reorder_index(r, seq);
+
+	/* check if we already stored this frame */
+	if (r->reorder_buf[index]) {
+		dev_kfree_skb(skb);
+		goto out;
+	}
+
+	/*
+	 * If the current MPDU is in the right order and nothing else
+	 * is stored we can process it directly, no need to buffer it.
+	 * If it is first but there's something stored, we may be able
+	 * to release frames after this one.
+	 */
+	if (seq == r->head_seq_num && r->stored_mpdu_num == 0) {
+		r->head_seq_num = seq_inc(r->head_seq_num);
+		wil_netif_rx_any(skb, ndev);
+		goto out;
+	}
+
+	/* put the frame in the reordering buffer */
+	r->reorder_buf[index] = skb;
+	r->reorder_time[index] = jiffies;
+	r->stored_mpdu_num++;
+	wil_reorder_release(wil, r);
+
+out:
+	spin_unlock(&r->reorder_lock);
+}
+
+struct wil_tid_ampdu_rx *wil_tid_ampdu_rx_alloc(struct wil6210_priv *wil,
+						int size, u16 ssn)
+{
+	struct wil_tid_ampdu_rx *r = kzalloc(sizeof(*r), GFP_KERNEL);
+	if (!r)
+		return NULL;
+
+	r->reorder_buf =
+		kcalloc(size, sizeof(struct sk_buff *), GFP_KERNEL);
+	r->reorder_time =
+		kcalloc(size, sizeof(unsigned long), GFP_KERNEL);
+	if (!r->reorder_buf || !r->reorder_time) {
+		kfree(r->reorder_buf);
+		kfree(r->reorder_time);
+		kfree(r);
+		return NULL;
+	}
+
+	spin_lock_init(&r->reorder_lock);
+	r->ssn = ssn;
+	r->head_seq_num = ssn;
+	r->buf_size = size;
+	r->stored_mpdu_num = 0;
+	return r;
+}
+
+void wil_tid_ampdu_rx_free(struct wil6210_priv *wil,
+			   struct wil_tid_ampdu_rx *r)
+{
+	if (!r)
+		return;
+	wil_release_reorder_frames(wil, r, r->head_seq_num + r->buf_size);
+	kfree(r->reorder_buf);
+	kfree(r->reorder_time);
+	kfree(r);
+}
diff --git a/drivers/net/wireless/ath/wil6210/txrx.c b/drivers/net/wireless/ath/wil6210/txrx.c
index 0b0975d88b43..092081e209da 100644
--- a/drivers/net/wireless/ath/wil6210/txrx.c
+++ b/drivers/net/wireless/ath/wil6210/txrx.c
@@ -344,6 +344,9 @@ static struct sk_buff *wil_vring_reap_rx(struct wil6210_priv *wil,
 	u16 dmalen;
 	u8 ftype;
 	u8 ds_bits;
+	int cid;
+	struct wil_net_stats *stats;
+
 
 	BUILD_BUG_ON(sizeof(struct vring_rx_desc) > sizeof(skb->cb));
 
@@ -383,8 +386,10 @@ static struct sk_buff *wil_vring_reap_rx(struct wil6210_priv *wil,
 	wil_hex_dump_txrx("Rx ", DUMP_PREFIX_OFFSET, 16, 1,
 			  skb->data, skb_headlen(skb), false);
 
-
-	wil->stats.last_mcs_rx = wil_rxdesc_mcs(d);
+	cid = wil_rxdesc_cid(d);
+	stats = &wil->sta[cid].stats;
+	stats->last_mcs_rx = wil_rxdesc_mcs(d);
+	wil->stats.last_mcs_rx = stats->last_mcs_rx;
 
 	/* use radiotap header only if required */
 	if (ndev->type == ARPHRD_IEEE80211_RADIOTAP)
@@ -472,10 +477,14 @@ static int wil_rx_refill(struct wil6210_priv *wil, int count)
  * Pass Rx packet to the netif. Update statistics.
  * Called in softirq context (NAPI poll).
  */
-static void wil_netif_rx_any(struct sk_buff *skb, struct net_device *ndev)
+void wil_netif_rx_any(struct sk_buff *skb, struct net_device *ndev)
 {
 	int rc;
+	struct wil6210_priv *wil = ndev_to_wil(ndev);
 	unsigned int len = skb->len;
+	struct vring_rx_desc *d = wil_skb_rxdesc(skb);
+	int cid = wil_rxdesc_cid(d);
+	struct wil_net_stats *stats = &wil->sta[cid].stats;
 
 	skb_orphan(skb);
 
@@ -483,10 +492,13 @@ static void wil_netif_rx_any(struct sk_buff *skb, struct net_device *ndev)
 
 	if (likely(rc == NET_RX_SUCCESS)) {
 		ndev->stats.rx_packets++;
+		stats->rx_packets++;
 		ndev->stats.rx_bytes += len;
+		stats->rx_bytes += len;
 
 	} else {
 		ndev->stats.rx_dropped++;
+		stats->rx_dropped++;
 	}
 }
 
@@ -515,12 +527,18 @@ void wil_rx_handle(struct wil6210_priv *wil, int *quota)
 			skb->ip_summed = CHECKSUM_UNNECESSARY;
 			skb->pkt_type = PACKET_OTHERHOST;
 			skb->protocol = htons(ETH_P_802_2);
-
+			wil_netif_rx_any(skb, ndev);
 		} else {
+			struct ethhdr *eth = (void *)skb->data;
+
 			skb->protocol = eth_type_trans(skb, ndev);
+
+			if (is_unicast_ether_addr(eth->h_dest))
+				wil_rx_reorder(wil, skb);
+			else
+				wil_netif_rx_any(skb, ndev);
 		}
 
-		wil_netif_rx_any(skb, ndev);
 	}
 	wil_rx_refill(wil, v->size);
 }
@@ -598,6 +616,9 @@ int wil_vring_init_tx(struct wil6210_priv *wil, int id, int size,
 	if (rc)
 		goto out;
 
+	wil->vring2cid_tid[id][0] = cid;
+	wil->vring2cid_tid[id][1] = tid;
+
 	cmd.vring_cfg.tx_sw_ring.ring_mem_base = cpu_to_le64(vring->pa);
 
 	rc = wmi_call(wil, WMI_VRING_CFG_CMDID, &cmd, sizeof(cmd),
@@ -634,12 +655,83 @@ void wil_vring_fini_tx(struct wil6210_priv *wil, int id)
 static struct vring *wil_find_tx_vring(struct wil6210_priv *wil,
 				       struct sk_buff *skb)
 {
-	struct vring *v = &wil->vring_tx[0];
+	int i;
+	struct ethhdr *eth = (void *)skb->data;
+	int cid = wil_find_cid(wil, eth->h_dest);
+
+	if (cid < 0)
+		return NULL;
+
+	/* TODO: fix for multiple TID */
+	for (i = 0; i < ARRAY_SIZE(wil->vring2cid_tid); i++) {
+		if (wil->vring2cid_tid[i][0] == cid) {
+			struct vring *v = &wil->vring_tx[i];
+			wil_dbg_txrx(wil, "%s(%pM) -> [%d]\n",
+				     __func__, eth->h_dest, i);
+			if (v->va) {
+				return v;
+			} else {
+				wil_dbg_txrx(wil, "vring[%d] not valid\n", i);
+				return NULL;
+			}
+		}
+	}
+
+	return NULL;
+}
+
+static void wil_set_da_for_vring(struct wil6210_priv *wil,
+				 struct sk_buff *skb, int vring_index)
+{
+	struct ethhdr *eth = (void *)skb->data;
+	int cid = wil->vring2cid_tid[vring_index][0];
+	memcpy(eth->h_dest, wil->sta[cid].addr, ETH_ALEN);
+}
+
+static int wil_tx_vring(struct wil6210_priv *wil, struct vring *vring,
+			struct sk_buff *skb);
+/*
+ * Find 1-st vring and return it; set dest address for this vring in skb
+ * duplicate skb and send it to other active vrings
+ */
+static struct vring *wil_tx_bcast(struct wil6210_priv *wil,
+				       struct sk_buff *skb)
+{
+	struct vring *v, *v2;
+	struct sk_buff *skb2;
+	int i;
+
+	/* find 1-st vring */
+	for (i = 0; i < WIL6210_MAX_TX_RINGS; i++) {
+		v = &wil->vring_tx[i];
+		if (v->va)
+			goto found;
+	}
 
-	if (v->va)
-		return v;
+	wil_err(wil, "Tx while no vrings active?\n");
 
 	return NULL;
+
+found:
+	wil_dbg_txrx(wil, "BCAST -> ring %d\n", i);
+	wil_set_da_for_vring(wil, skb, i);
+
+	/* find other active vrings and duplicate skb for each */
+	for (i++; i < WIL6210_MAX_TX_RINGS; i++) {
+		v2 = &wil->vring_tx[i];
+		if (!v2->va)
+			continue;
+		skb2 = skb_copy(skb, GFP_ATOMIC);
+		if (skb2) {
+			wil_dbg_txrx(wil, "BCAST DUP -> ring %d\n", i);
+			wil_set_da_for_vring(wil, skb2, i);
+			wil_tx_vring(wil, v2, skb2);
+		} else {
+			wil_err(wil, "skb_copy failed\n");
+		}
+	}
+
+	return v;
 }
 
 static int wil_tx_desc_map(struct vring_tx_desc *d, dma_addr_t pa, u32 len,
@@ -740,9 +832,6 @@ static int wil_tx_vring(struct wil6210_priv *wil, struct vring *vring,
 	}
 	_d = &(vring->va[i].tx);
 
-	/* FIXME FW can accept only unicast frames for the peer */
-	memcpy(skb->data, wil->dst_addr[vring_index], ETH_ALEN);
-
 	pa = dma_map_single(dev, skb->data,
 			skb_headlen(skb), DMA_TO_DEVICE);
 
@@ -836,6 +925,7 @@ static int wil_tx_vring(struct wil6210_priv *wil, struct vring *vring,
 netdev_tx_t wil_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 {
 	struct wil6210_priv *wil = ndev_to_wil(ndev);
+	struct ethhdr *eth = (void *)skb->data;
 	struct vring *vring;
 	int rc;
 
@@ -854,9 +944,13 @@ netdev_tx_t wil_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 	}
 
 	/* find vring */
-	vring = wil_find_tx_vring(wil, skb);
+	if (is_unicast_ether_addr(eth->h_dest)) {
+		vring = wil_find_tx_vring(wil, skb);
+	} else {
+		vring = wil_tx_bcast(wil, skb);
+	}
 	if (!vring) {
-		wil_err(wil, "No Tx VRING available\n");
+		wil_err(wil, "No Tx VRING found for %pM\n", eth->h_dest);
 		goto drop;
 	}
 	/* set up vring entry */
@@ -892,6 +986,8 @@ int wil_tx_complete(struct wil6210_priv *wil, int ringid)
 	struct device *dev = wil_to_dev(wil);
 	struct vring *vring = &wil->vring_tx[ringid];
 	int done = 0;
+	int cid = wil->vring2cid_tid[ringid][0];
+	struct wil_net_stats *stats = &wil->sta[cid].stats;
 
 	if (!vring->va) {
 		wil_err(wil, "Tx irq[%d]: vring not initialized\n", ringid);
@@ -933,9 +1029,12 @@ int wil_tx_complete(struct wil6210_priv *wil, int ringid)
 		if (skb) {
 			if (d->dma.error == 0) {
 				ndev->stats.tx_packets++;
+				stats->tx_packets++;
 				ndev->stats.tx_bytes += skb->len;
+				stats->tx_bytes += skb->len;
 			} else {
 				ndev->stats.tx_errors++;
+				stats->tx_errors++;
 			}
 
 			dev_kfree_skb_any(skb);
diff --git a/drivers/net/wireless/ath/wil6210/txrx.h b/drivers/net/wireless/ath/wil6210/txrx.h
index b3828279204c..bc5706a4f007 100644
--- a/drivers/net/wireless/ath/wil6210/txrx.h
+++ b/drivers/net/wireless/ath/wil6210/txrx.h
@@ -436,4 +436,11 @@ static inline struct vring_rx_desc *wil_skb_rxdesc(struct sk_buff *skb)
 	return (void *)skb->cb;
 }
 
+void wil_netif_rx_any(struct sk_buff *skb, struct net_device *ndev);
+void wil_rx_reorder(struct wil6210_priv *wil, struct sk_buff *skb);
+struct wil_tid_ampdu_rx *wil_tid_ampdu_rx_alloc(struct wil6210_priv *wil,
+						int size, u16 ssn);
+void wil_tid_ampdu_rx_free(struct wil6210_priv *wil,
+			   struct wil_tid_ampdu_rx *r);
+
 #endif /* WIL6210_TXRX_H */
diff --git a/drivers/net/wireless/ath/wil6210/wil6210.h b/drivers/net/wireless/ath/wil6210/wil6210.h
index 1f91eaf95bbe..980dccc82b32 100644
--- a/drivers/net/wireless/ath/wil6210/wil6210.h
+++ b/drivers/net/wireless/ath/wil6210/wil6210.h
@@ -215,6 +215,46 @@ enum { /* for wil6210_priv.status */
 
 struct pci_dev;
 
+/**
+ * struct tid_ampdu_rx - TID aggregation information (Rx).
+ *
+ * @reorder_buf: buffer to reorder incoming aggregated MPDUs
+ * @reorder_time: jiffies when skb was added
+ * @session_timer: check if peer keeps Tx-ing on the TID (by timeout value)
+ * @reorder_timer: releases expired frames from the reorder buffer.
+ * @last_rx: jiffies of last rx activity
+ * @head_seq_num: head sequence number in reordering buffer.
+ * @stored_mpdu_num: number of MPDUs in reordering buffer
+ * @ssn: Starting Sequence Number expected to be aggregated.
+ * @buf_size: buffer size for incoming A-MPDUs
+ * @timeout: reset timer value (in TUs).
+ * @dialog_token: dialog token for aggregation session
+ * @rcu_head: RCU head used for freeing this struct
+ * @reorder_lock: serializes access to reorder buffer, see below.
+ *
+ * This structure's lifetime is managed by RCU, assignments to
+ * the array holding it must hold the aggregation mutex.
+ *
+ * The @reorder_lock is used to protect the members of this
+ * struct, except for @timeout, @buf_size and @dialog_token,
+ * which are constant across the lifetime of the struct (the
+ * dialog token being used only for debugging).
+ */
+struct wil_tid_ampdu_rx {
+	spinlock_t reorder_lock; /* see above */
+	struct sk_buff **reorder_buf;
+	unsigned long *reorder_time;
+	struct timer_list session_timer;
+	struct timer_list reorder_timer;
+	unsigned long last_rx;
+	u16 head_seq_num;
+	u16 stored_mpdu_num;
+	u16 ssn;
+	u16 buf_size;
+	u16 timeout;
+	u8 dialog_token;
+};
+
 struct wil6210_stats {
 	u64 tsf;
 	u32 snr;
@@ -226,6 +266,42 @@ struct wil6210_stats {
 	u16 peer_tx_sector;
 };
 
+enum wil_sta_status {
+	wil_sta_unused = 0,
+	wil_sta_conn_pending = 1,
+	wil_sta_connected = 2,
+};
+
+#define WIL_STA_TID_NUM (16)
+
+struct wil_net_stats {
+	unsigned long	rx_packets;
+	unsigned long	tx_packets;
+	unsigned long	rx_bytes;
+	unsigned long	tx_bytes;
+	unsigned long	tx_errors;
+	unsigned long	rx_dropped;
+	u16 last_mcs_rx;
+};
+
+/**
+ * struct wil_sta_info - data for peer
+ *
+ * Peer identified by its CID (connection ID)
+ * NIC performs beam forming for each peer;
+ * if no beam forming done, frame exchange is not
+ * possible.
+ */
+struct wil_sta_info {
+	u8 addr[ETH_ALEN];
+	enum wil_sta_status status;
+	struct wil_net_stats stats;
+	/* Rx BACK */
+	struct wil_tid_ampdu_rx *tid_rx[WIL_STA_TID_NUM];
+	unsigned long tid_rx_timer_expired[BITS_TO_LONGS(WIL_STA_TID_NUM)];
+	unsigned long tid_rx_stop_requested[BITS_TO_LONGS(WIL_STA_TID_NUM)];
+};
+
 struct wil6210_priv {
 	struct pci_dev *pdev;
 	int n_msi;
@@ -267,7 +343,8 @@ struct wil6210_priv {
 	/* DMA related */
 	struct vring vring_rx;
 	struct vring vring_tx[WIL6210_MAX_TX_RINGS];
-	u8 dst_addr[WIL6210_MAX_TX_RINGS][ETH_ALEN];
+	u8 vring2cid_tid[WIL6210_MAX_TX_RINGS][2]; /* [0] - CID, [1] - TID */
+	struct wil_sta_info sta[WIL6210_MAX_CID];
 	/* scan */
 	struct cfg80211_scan_request *scan_request;
 
@@ -334,6 +411,7 @@ void wil_link_off(struct wil6210_priv *wil);
 int wil_up(struct wil6210_priv *wil);
 int wil_down(struct wil6210_priv *wil);
 void wil_mbox_ring_le2cpus(struct wil6210_mbox_ring *r);
+int wil_find_cid(struct wil6210_priv *wil, const u8 *mac);
 
 void __iomem *wmi_buffer(struct wil6210_priv *wil, __le32 ptr);
 void __iomem *wmi_addr(struct wil6210_priv *wil, u32 ptr);
@@ -357,7 +435,9 @@ int wmi_echo(struct wil6210_priv *wil);
 int wmi_set_ie(struct wil6210_priv *wil, u8 type, u16 ie_len, const void *ie);
 int wmi_rx_chain_add(struct wil6210_priv *wil, struct vring *vring);
 int wmi_p2p_cfg(struct wil6210_priv *wil, int channel);
+int wmi_rxon(struct wil6210_priv *wil, bool on);
 int wmi_get_temperature(struct wil6210_priv *wil, u32 *t_m, u32 *t_r);
+int wmi_disconnect_sta(struct wil6210_priv *wil, const u8 *mac, u16 reason);
 
 int wil6210_init_irq(struct wil6210_priv *wil, int irq);
 void wil6210_fini_irq(struct wil6210_priv *wil, int irq);
diff --git a/drivers/net/wireless/ath/wil6210/wmi.c b/drivers/net/wireless/ath/wil6210/wmi.c
index 063963ee422a..24eed0963581 100644
--- a/drivers/net/wireless/ath/wil6210/wmi.c
+++ b/drivers/net/wireless/ath/wil6210/wmi.c
@@ -307,14 +307,14 @@ static void wmi_evt_rx_mgmt(struct wil6210_priv *wil, int id, void *d, int len)
 	u32 freq = ieee80211_channel_to_frequency(ch_no,
 			IEEE80211_BAND_60GHZ);
 	struct ieee80211_channel *channel = ieee80211_get_channel(wiphy, freq);
-	/* TODO convert LE to CPU */
-	s32 signal = 0; /* TODO */
+	s32 signal = data->info.sqi;
 	__le16 fc = rx_mgmt_frame->frame_control;
 	u32 d_len = le32_to_cpu(data->info.len);
 	u16 d_status = le16_to_cpu(data->info.status);
 
-	wil_dbg_wmi(wil, "MGMT: channel %d MCS %d SNR %d\n",
-		    data->info.channel, data->info.mcs, data->info.snr);
+	wil_dbg_wmi(wil, "MGMT: channel %d MCS %d SNR %d SQI %d%%\n",
+		    data->info.channel, data->info.mcs, data->info.snr,
+		    data->info.sqi);
 	wil_dbg_wmi(wil, "status 0x%04x len %d fc 0x%04x\n", d_status, d_len,
 		    le16_to_cpu(fc));
 	wil_dbg_wmi(wil, "qid %d mid %d cid %d\n",
@@ -384,6 +384,11 @@ static void wmi_evt_connect(struct wil6210_priv *wil, int id, void *d, int len)
 			evt->assoc_req_len, evt->assoc_resp_len);
 		return;
 	}
+	if (evt->cid >= WIL6210_MAX_CID) {
+		wil_err(wil, "Connect CID invalid : %d\n", evt->cid);
+		return;
+	}
+
 	ch = evt->channel + 1;
 	wil_dbg_wmi(wil, "Connect %pM channel [%d] cid %d\n",
 		    evt->bssid, ch, evt->cid);
@@ -439,7 +444,8 @@ static void wmi_evt_connect(struct wil6210_priv *wil, int id, void *d, int len)
 
 	/* FIXME FW can transmit only ucast frames to peer */
 	/* FIXME real ring_id instead of hard coded 0 */
-	memcpy(wil->dst_addr[0], evt->bssid, ETH_ALEN);
+	memcpy(wil->sta[evt->cid].addr, evt->bssid, ETH_ALEN);
+	wil->sta[evt->cid].status = wil_sta_conn_pending;
 
 	wil->pending_connect_cid = evt->cid;
 	queue_work(wil->wmi_wq_conn, &wil->connect_worker);
@@ -476,11 +482,11 @@ static void wmi_evt_notify(struct wil6210_priv *wil, int id, void *d, int len)
 	wil->stats.peer_rx_sector = le16_to_cpu(evt->other_rx_sector);
 	wil->stats.peer_tx_sector = le16_to_cpu(evt->other_tx_sector);
 	wil_dbg_wmi(wil, "Link status, MCS %d TSF 0x%016llx\n"
-		    "BF status 0x%08x SNR 0x%08x\n"
+		    "BF status 0x%08x SNR 0x%08x SQI %d%%\n"
 		    "Tx Tpt %d goodput %d Rx goodput %d\n"
 		    "Sectors(rx:tx) my %d:%d peer %d:%d\n",
 		    wil->stats.bf_mcs, wil->stats.tsf, evt->status,
-		    wil->stats.snr, le32_to_cpu(evt->tx_tpt),
+		    wil->stats.snr, evt->sqi, le32_to_cpu(evt->tx_tpt),
 		    le32_to_cpu(evt->tx_goodput), le32_to_cpu(evt->rx_goodput),
 		    wil->stats.my_rx_sector, wil->stats.my_tx_sector,
 		    wil->stats.peer_rx_sector, wil->stats.peer_tx_sector);
@@ -499,10 +505,16 @@ static void wmi_evt_eapol_rx(struct wil6210_priv *wil, int id,
 	int sz = eapol_len + ETH_HLEN;
 	struct sk_buff *skb;
 	struct ethhdr *eth;
+	int cid;
+	struct wil_net_stats *stats = NULL;
 
 	wil_dbg_wmi(wil, "EAPOL len %d from %pM\n", eapol_len,
 		    evt->src_mac);
 
+	cid = wil_find_cid(wil, evt->src_mac);
+	if (cid >= 0)
+		stats = &wil->sta[cid].stats;
+
 	if (eapol_len > 196) { /* TODO: revisit size limit */
 		wil_err(wil, "EAPOL too large\n");
 		return;
@@ -513,6 +525,7 @@ static void wmi_evt_eapol_rx(struct wil6210_priv *wil, int id,
 		wil_err(wil, "Failed to allocate skb\n");
 		return;
 	}
+
 	eth = (struct ethhdr *)skb_put(skb, ETH_HLEN);
 	memcpy(eth->h_dest, ndev->dev_addr, ETH_ALEN);
 	memcpy(eth->h_source, evt->src_mac, ETH_ALEN);
@@ -521,9 +534,15 @@ static void wmi_evt_eapol_rx(struct wil6210_priv *wil, int id,
 	skb->protocol = eth_type_trans(skb, ndev);
 	if (likely(netif_rx_ni(skb) == NET_RX_SUCCESS)) {
 		ndev->stats.rx_packets++;
-		ndev->stats.rx_bytes += skb->len;
+		ndev->stats.rx_bytes += sz;
+		if (stats) {
+			stats->rx_packets++;
+			stats->rx_bytes += sz;
+		}
 	} else {
 		ndev->stats.rx_dropped++;
+		if (stats)
+			stats->rx_dropped++;
 	}
 }
 
@@ -552,10 +571,42 @@ static void wmi_evt_ba_status(struct wil6210_priv *wil, int id, void *d,
 			      int len)
 {
 	struct wmi_vring_ba_status_event *evt = d;
+	struct wil_sta_info *sta;
+	uint i, cid;
+
+	/* TODO: use Rx BA status, not Tx one */
 
 	wil_dbg_wmi(wil, "BACK[%d] %s {%d} timeout %d\n",
-		    evt->ringid, evt->status ? "N/A" : "OK", evt->agg_wsize,
-		    __le16_to_cpu(evt->ba_timeout));
+		    evt->ringid,
+		    evt->status == WMI_BA_AGREED ? "OK" : "N/A",
+		    evt->agg_wsize, __le16_to_cpu(evt->ba_timeout));
+
+	if (evt->ringid >= WIL6210_MAX_TX_RINGS) {
+		wil_err(wil, "invalid ring id %d\n", evt->ringid);
+		return;
+	}
+
+	cid = wil->vring2cid_tid[evt->ringid][0];
+	if (cid >= WIL6210_MAX_CID) {
+		wil_err(wil, "invalid CID %d for vring %d\n", cid, evt->ringid);
+		return;
+	}
+
+	sta = &wil->sta[cid];
+	if (sta->status == wil_sta_unused) {
+		wil_err(wil, "CID %d unused\n", cid);
+		return;
+	}
+
+	wil_dbg_wmi(wil, "BACK for CID %d %pM\n", cid, sta->addr);
+	for (i = 0; i < WIL_STA_TID_NUM; i++) {
+		struct wil_tid_ampdu_rx *r = sta->tid_rx[i];
+		sta->tid_rx[i] = NULL;
+		wil_tid_ampdu_rx_free(wil, r);
+		if ((evt->status == WMI_BA_AGREED) && evt->agg_wsize)
+			sta->tid_rx[i] = wil_tid_ampdu_rx_alloc(wil,
+						evt->agg_wsize, 0);
+	}
 }
 
 static const struct {
@@ -893,6 +944,38 @@ int wmi_set_ie(struct wil6210_priv *wil, u8 type, u16 ie_len, const void *ie)
 	return rc;
 }
 
+/**
+ * wmi_rxon - turn radio on/off
+ * @on:		turn on if true, off otherwise
+ *
+ * Only switch radio. Channel should be set separately.
+ * No timeout for rxon - radio turned on forever unless some other call
+ * turns it off
+ */
+int wmi_rxon(struct wil6210_priv *wil, bool on)
+{
+	int rc;
+	struct {
+		struct wil6210_mbox_hdr_wmi wmi;
+		struct wmi_listen_started_event evt;
+	} __packed reply;
+
+	wil_info(wil, "%s(%s)\n", __func__, on ? "on" : "off");
+
+	if (on) {
+		rc = wmi_call(wil, WMI_START_LISTEN_CMDID, NULL, 0,
+			      WMI_LISTEN_STARTED_EVENTID,
+			      &reply, sizeof(reply), 100);
+		if ((rc == 0) && (reply.evt.status != WMI_FW_STATUS_SUCCESS))
+			rc = -EINVAL;
+	} else {
+		rc = wmi_call(wil, WMI_DISCOVERY_STOP_CMDID, NULL, 0,
+			      WMI_DISCOVERY_STOPPED_EVENTID, NULL, 0, 20);
+	}
+
+	return rc;
+}
+
 int wmi_rx_chain_add(struct wil6210_priv *wil, struct vring *vring)
 {
 	struct wireless_dev *wdev = wil->wdev;
@@ -906,6 +989,7 @@ int wmi_rx_chain_add(struct wil6210_priv *wil, struct vring *vring)
 		},
 		.mid = 0, /* TODO - what is it? */
 		.decap_trans_type = WMI_DECAP_TYPE_802_3,
+		.reorder_type = WMI_RX_SW_REORDER,
 	};
 	struct {
 		struct wil6210_mbox_hdr_wmi wmi;
@@ -973,6 +1057,18 @@ int wmi_get_temperature(struct wil6210_priv *wil, u32 *t_m, u32 *t_r)
 	return 0;
 }
 
+int wmi_disconnect_sta(struct wil6210_priv *wil, const u8 *mac, u16 reason)
+{
+	struct wmi_disconnect_sta_cmd cmd = {
+		.disconnect_reason = cpu_to_le16(reason),
+	};
+	memcpy(cmd.dst_mac, mac, ETH_ALEN);
+
+	wil_dbg_wmi(wil, "%s(%pM, reason %d)\n", __func__, mac, reason);
+
+	return wmi_send(wil, WMI_DISCONNECT_STA_CMDID, &cmd, sizeof(cmd));
+}
+
 void wmi_event_flush(struct wil6210_priv *wil)
 {
 	struct pending_wmi_event *evt, *t;
diff --git a/drivers/net/wireless/atmel.c b/drivers/net/wireless/atmel.c
index bf93ea859f2d..1fe41af81a59 100644
--- a/drivers/net/wireless/atmel.c
+++ b/drivers/net/wireless/atmel.c
@@ -67,7 +67,7 @@
 #include <linux/moduleparam.h>
 #include <linux/firmware.h>
 #include <linux/jiffies.h>
-#include <linux/ieee80211.h>
+#include <net/cfg80211.h>
 #include "atmel.h"
 
 #define DRIVER_MAJOR 0
@@ -2273,7 +2273,7 @@ static int atmel_set_freq(struct net_device *dev,
 
 		/* Hack to fall through... */
 		fwrq->e = 0;
-		fwrq->m = ieee80211_freq_to_dsss_chan(f);
+		fwrq->m = ieee80211_frequency_to_channel(f);
 	}
 	/* Setting by channel number */
 	if ((fwrq->m > 1000) || (fwrq->e > 0))
@@ -2434,8 +2434,8 @@ static int atmel_get_range(struct net_device *dev,
 			range->freq[k].i = i; /* List index */
 
 			/* Values in MHz -> * 10^5 * 10 */
-			range->freq[k].m = (ieee80211_dsss_chan_to_freq(i) *
-					    100000);
+			range->freq[k].m = 100000 *
+			 ieee80211_channel_to_frequency(i, IEEE80211_BAND_2GHZ);
 			range->freq[k++].e = 1;
 		}
 		range->num_frequency = k;
diff --git a/drivers/net/wireless/b43/Kconfig b/drivers/net/wireless/b43/Kconfig
index 51ff0b198d0a..088d544ec63f 100644
--- a/drivers/net/wireless/b43/Kconfig
+++ b/drivers/net/wireless/b43/Kconfig
@@ -92,7 +92,7 @@ config B43_SDIO
 # if we can do DMA.
 config B43_BCMA_PIO
 	bool
-	depends on B43_BCMA
+	depends on B43 && B43_BCMA
 	select BCMA_BLOCKIO
 	default y
 
diff --git a/drivers/net/wireless/b43/debugfs.h b/drivers/net/wireless/b43/debugfs.h
index 822aad8842f4..50517b801cb4 100644
--- a/drivers/net/wireless/b43/debugfs.h
+++ b/drivers/net/wireless/b43/debugfs.h
@@ -86,7 +86,7 @@ void b43_debugfs_log_txstat(struct b43_wldev *dev,
 
 static inline bool b43_debug(struct b43_wldev *dev, enum b43_dyndbg feature)
 {
-	return 0;
+	return false;
 }
 
 static inline void b43_debugfs_init(void)
diff --git a/drivers/net/wireless/b43/main.c b/drivers/net/wireless/b43/main.c
index c75237eb55a1..69fc3d65531a 100644
--- a/drivers/net/wireless/b43/main.c
+++ b/drivers/net/wireless/b43/main.c
@@ -1549,7 +1549,7 @@ static void b43_write_beacon_template(struct b43_wldev *dev,
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(dev->wl->current_beacon);
 
 	bcn = (const struct ieee80211_mgmt *)(dev->wl->current_beacon->data);
-	len = min((size_t) dev->wl->current_beacon->len,
+	len = min_t(size_t, dev->wl->current_beacon->len,
 		  0x200 - sizeof(struct b43_plcp_hdr6));
 	rate = ieee80211_get_tx_rate(dev->wl->hw, info)->hw_value;
 
diff --git a/drivers/net/wireless/b43/main.h b/drivers/net/wireless/b43/main.h
index abac25ee958d..f476fc337d64 100644
--- a/drivers/net/wireless/b43/main.h
+++ b/drivers/net/wireless/b43/main.h
@@ -58,41 +58,6 @@ enum b43_verbosity {
 #endif
 };
 
-
-/* Lightweight function to convert a frequency (in Mhz) to a channel number. */
-static inline u8 b43_freq_to_channel_5ghz(int freq)
-{
-	return ((freq - 5000) / 5);
-}
-static inline u8 b43_freq_to_channel_2ghz(int freq)
-{
-	u8 channel;
-
-	if (freq == 2484)
-		channel = 14;
-	else
-		channel = (freq - 2407) / 5;
-
-	return channel;
-}
-
-/* Lightweight function to convert a channel number to a frequency (in Mhz). */
-static inline int b43_channel_to_freq_5ghz(u8 channel)
-{
-	return (5000 + (5 * channel));
-}
-static inline int b43_channel_to_freq_2ghz(u8 channel)
-{
-	int freq;
-
-	if (channel == 14)
-		freq = 2484;
-	else
-		freq = 2407 + (5 * channel);
-
-	return freq;
-}
-
 static inline int b43_is_cck_rate(int rate)
 {
 	return (rate == B43_CCK_RATE_1MB ||
diff --git a/drivers/net/wireless/b43/phy_common.c b/drivers/net/wireless/b43/phy_common.c
index f01676ac481b..dbaa51890198 100644
--- a/drivers/net/wireless/b43/phy_common.c
+++ b/drivers/net/wireless/b43/phy_common.c
@@ -133,9 +133,9 @@ void b43_phy_exit(struct b43_wldev *dev)
 bool b43_has_hardware_pctl(struct b43_wldev *dev)
 {
 	if (!dev->phy.hardware_power_control)
-		return 0;
+		return false;
 	if (!dev->phy.ops->supports_hwpctl)
-		return 0;
+		return false;
 	return dev->phy.ops->supports_hwpctl(dev);
 }
 
diff --git a/drivers/net/wireless/b43/pio.c b/drivers/net/wireless/b43/pio.c
index a73ff8c9deb5..a4ff5e2a42b9 100644
--- a/drivers/net/wireless/b43/pio.c
+++ b/drivers/net/wireless/b43/pio.c
@@ -637,7 +637,7 @@ static bool pio_rx_frame(struct b43_pio_rxqueue *q)
 
 		ctl = b43_piorx_read32(q, B43_PIO8_RXCTL);
 		if (!(ctl & B43_PIO8_RXCTL_FRAMERDY))
-			return 0;
+			return false;
 		b43_piorx_write32(q, B43_PIO8_RXCTL,
 				  B43_PIO8_RXCTL_FRAMERDY);
 		for (i = 0; i < 10; i++) {
@@ -651,7 +651,7 @@ static bool pio_rx_frame(struct b43_pio_rxqueue *q)
 
 		ctl = b43_piorx_read16(q, B43_PIO_RXCTL);
 		if (!(ctl & B43_PIO_RXCTL_FRAMERDY))
-			return 0;
+			return false;
 		b43_piorx_write16(q, B43_PIO_RXCTL,
 				  B43_PIO_RXCTL_FRAMERDY);
 		for (i = 0; i < 10; i++) {
@@ -662,7 +662,7 @@ static bool pio_rx_frame(struct b43_pio_rxqueue *q)
 		}
 	}
 	b43dbg(q->dev->wl, "PIO RX timed out\n");
-	return 1;
+	return true;
 data_ready:
 
 	/* Get the preamble (RX header) */
@@ -759,7 +759,7 @@ data_ready:
 
 	b43_rx(q->dev, skb, rxhdr);
 
-	return 1;
+	return true;
 
 rx_error:
 	if (err_msg)
@@ -769,7 +769,7 @@ rx_error:
 	else
 		b43_piorx_write16(q, B43_PIO_RXCTL, B43_PIO_RXCTL_DATARDY);
 
-	return 1;
+	return true;
 }
 
 void b43_pio_rx(struct b43_pio_rxqueue *q)
diff --git a/drivers/net/wireless/b43/sysfs.c b/drivers/net/wireless/b43/sysfs.c
index 8e8431d4eb0c..3190493bd07f 100644
--- a/drivers/net/wireless/b43/sysfs.c
+++ b/drivers/net/wireless/b43/sysfs.c
@@ -40,7 +40,7 @@ static int get_integer(const char *buf, size_t count)
 
 	if (count == 0)
 		goto out;
-	count = min(count, (size_t) 10);
+	count = min_t(size_t, count, 10);
 	memcpy(tmp, buf, count);
 	ret = simple_strtol(tmp, NULL, 10);
       out:
diff --git a/drivers/net/wireless/b43/xmit.c b/drivers/net/wireless/b43/xmit.c
index 50e5ddb12fb3..31adb8cf0291 100644
--- a/drivers/net/wireless/b43/xmit.c
+++ b/drivers/net/wireless/b43/xmit.c
@@ -337,7 +337,7 @@ int b43_generate_txhdr(struct b43_wldev *dev,
 			/* iv16 */
 			memcpy(txhdr->iv + 10, ((u8 *) wlhdr) + wlhdr_len, 3);
 		} else {
-			iv_len = min((size_t) info->control.hw_key->iv_len,
+			iv_len = min_t(size_t, info->control.hw_key->iv_len,
 				     ARRAY_SIZE(txhdr->iv));
 			memcpy(txhdr->iv, ((u8 *) wlhdr) + wlhdr_len, iv_len);
 		}
@@ -806,7 +806,8 @@ void b43_rx(struct b43_wldev *dev, struct sk_buff *skb, const void *_rxhdr)
 		B43_WARN_ON(1);
 		/* FIXME: We don't really know which value the "chanid" contains.
 		 *        So the following assignment might be wrong. */
-		status.freq = b43_channel_to_freq_5ghz(chanid);
+		status.freq =
+			ieee80211_channel_to_frequency(chanid, status.band);
 		break;
 	case B43_PHYTYPE_G:
 		status.band = IEEE80211_BAND_2GHZ;
@@ -819,13 +820,12 @@ void b43_rx(struct b43_wldev *dev, struct sk_buff *skb, const void *_rxhdr)
 	case B43_PHYTYPE_HT:
 		/* chanid is the SHM channel cookie. Which is the plain
 		 * channel number in b43. */
-		if (chanstat & B43_RX_CHAN_5GHZ) {
+		if (chanstat & B43_RX_CHAN_5GHZ)
 			status.band = IEEE80211_BAND_5GHZ;
-			status.freq = b43_channel_to_freq_5ghz(chanid);
-		} else {
+		else
 			status.band = IEEE80211_BAND_2GHZ;
-			status.freq = b43_channel_to_freq_2ghz(chanid);
-		}
+		status.freq =
+			ieee80211_channel_to_frequency(chanid, status.band);
 		break;
 	default:
 		B43_WARN_ON(1);
diff --git a/drivers/net/wireless/b43legacy/main.c b/drivers/net/wireless/b43legacy/main.c
index 349c77605231..1aec2146a2bf 100644
--- a/drivers/net/wireless/b43legacy/main.c
+++ b/drivers/net/wireless/b43legacy/main.c
@@ -978,7 +978,7 @@ static void b43legacy_write_beacon_template(struct b43legacy_wldev *dev,
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(dev->wl->current_beacon);
 
 	bcn = (const struct ieee80211_mgmt *)(dev->wl->current_beacon->data);
-	len = min((size_t)dev->wl->current_beacon->len,
+	len = min_t(size_t, dev->wl->current_beacon->len,
 		  0x200 - sizeof(struct b43legacy_plcp_hdr6));
 	rate = ieee80211_get_tx_rate(dev->wl->hw, info)->hw_value;
 
@@ -1155,7 +1155,7 @@ static void b43legacy_write_probe_resp_template(struct b43legacy_wldev *dev,
 	b43legacy_write_probe_resp_plcp(dev, 0x350, size,
 					&b43legacy_b_ratetable[3]);
 
-	size = min((size_t)size,
+	size = min_t(size_t, size,
 		   0x200 - sizeof(struct b43legacy_plcp_hdr6));
 	b43legacy_write_template_common(dev, probe_resp_data,
 					size, ram_offset,
diff --git a/drivers/net/wireless/b43legacy/sysfs.c b/drivers/net/wireless/b43legacy/sysfs.c
index 57f8b089767c..2a1da15c913b 100644
--- a/drivers/net/wireless/b43legacy/sysfs.c
+++ b/drivers/net/wireless/b43legacy/sysfs.c
@@ -42,7 +42,7 @@ static int get_integer(const char *buf, size_t count)
 
 	if (count == 0)
 		goto out;
-	count = min(count, (size_t)10);
+	count = min_t(size_t, count, 10);
 	memcpy(tmp, buf, count);
 	ret = simple_strtol(tmp, NULL, 10);
 out:
diff --git a/drivers/net/wireless/b43legacy/xmit.c b/drivers/net/wireless/b43legacy/xmit.c
index 86588c9ff0f2..34bf3f0b729f 100644
--- a/drivers/net/wireless/b43legacy/xmit.c
+++ b/drivers/net/wireless/b43legacy/xmit.c
@@ -254,7 +254,7 @@ static int generate_txhdr_fw3(struct b43legacy_wldev *dev,
 				   B43legacy_TX4_MAC_KEYALG_SHIFT) &
 				   B43legacy_TX4_MAC_KEYALG;
 			wlhdr_len = ieee80211_hdrlen(wlhdr->frame_control);
-			iv_len = min((size_t)info->control.hw_key->iv_len,
+			iv_len = min_t(size_t, info->control.hw_key->iv_len,
 				     ARRAY_SIZE(txhdr->iv));
 			memcpy(txhdr->iv, ((u8 *)wlhdr) + wlhdr_len, iv_len);
 		} else {
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/Makefile b/drivers/net/wireless/brcm80211/brcmfmac/Makefile
index 57cddee03252..1d2ceac3a221 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/Makefile
+++ b/drivers/net/wireless/brcm80211/brcmfmac/Makefile
@@ -24,6 +24,7 @@ ccflags-y += -D__CHECK_ENDIAN__
 obj-$(CONFIG_BRCMFMAC) += brcmfmac.o
 brcmfmac-objs += \
 		wl_cfg80211.o \
+		chip.o \
 		fwil.o \
 		fweh.o \
 		fwsignal.o \
@@ -36,8 +37,7 @@ brcmfmac-objs += \
 		btcoex.o
 brcmfmac-$(CONFIG_BRCMFMAC_SDIO) += \
 		dhd_sdio.o \
-		bcmsdh.o \
-		sdio_chip.o
+		bcmsdh.o
 brcmfmac-$(CONFIG_BRCMFMAC_USB) += \
 		usb.o
 brcmfmac-$(CONFIG_BRCMDBG) += \
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c
index fa35b23bbaa7..4a6508e7e3a1 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c
@@ -43,7 +43,6 @@
 #include "dhd_bus.h"
 #include "dhd_dbg.h"
 #include "sdio_host.h"
-#include "sdio_chip.h"
 
 #define SDIOH_API_ACCESS_RETRY_LIMIT	2
 
@@ -54,6 +53,12 @@
 /* Maximum milliseconds to wait for F2 to come up */
 #define SDIO_WAIT_F2RDY	3000
 
+#define BRCMF_DEFAULT_TXGLOM_SIZE	32  /* max tx frames in glom chain */
+#define BRCMF_DEFAULT_RXGLOM_SIZE	32  /* max rx frames in glom chain */
+
+static int brcmf_sdiod_txglomsz = BRCMF_DEFAULT_TXGLOM_SIZE;
+module_param_named(txglomsz, brcmf_sdiod_txglomsz, int, 0);
+MODULE_PARM_DESC(txglomsz, "maximum tx packet chain size [SDIO]");
 
 static irqreturn_t brcmf_sdiod_oob_irqhandler(int irq, void *dev_id)
 {
@@ -488,7 +493,6 @@ static int brcmf_sdiod_sglist_rw(struct brcmf_sdio_dev *sdiodev, uint fn,
 	struct mmc_request mmc_req;
 	struct mmc_command mmc_cmd;
 	struct mmc_data mmc_dat;
-	struct sg_table st;
 	struct scatterlist *sgl;
 	int ret = 0;
 
@@ -533,16 +537,11 @@ static int brcmf_sdiod_sglist_rw(struct brcmf_sdio_dev *sdiodev, uint fn,
 	pkt_offset = 0;
 	pkt_next = target_list->next;
 
-	if (sg_alloc_table(&st, max_seg_cnt, GFP_KERNEL)) {
-		ret = -ENOMEM;
-		goto exit;
-	}
-
 	memset(&mmc_req, 0, sizeof(struct mmc_request));
 	memset(&mmc_cmd, 0, sizeof(struct mmc_command));
 	memset(&mmc_dat, 0, sizeof(struct mmc_data));
 
-	mmc_dat.sg = st.sgl;
+	mmc_dat.sg = sdiodev->sgtable.sgl;
 	mmc_dat.blksz = func_blk_sz;
 	mmc_dat.flags = write ? MMC_DATA_WRITE : MMC_DATA_READ;
 	mmc_cmd.opcode = SD_IO_RW_EXTENDED;
@@ -558,7 +557,7 @@ static int brcmf_sdiod_sglist_rw(struct brcmf_sdio_dev *sdiodev, uint fn,
 	while (seg_sz) {
 		req_sz = 0;
 		sg_cnt = 0;
-		sgl = st.sgl;
+		sgl = sdiodev->sgtable.sgl;
 		/* prep sg table */
 		while (pkt_next != (struct sk_buff *)target_list) {
 			pkt_data = pkt_next->data + pkt_offset;
@@ -640,7 +639,7 @@ static int brcmf_sdiod_sglist_rw(struct brcmf_sdio_dev *sdiodev, uint fn,
 	}
 
 exit:
-	sg_free_table(&st);
+	sg_init_table(sdiodev->sgtable.sgl, sdiodev->sgtable.orig_nents);
 	while ((pkt_next = __skb_dequeue(&local_list)) != NULL)
 		brcmu_pkt_buf_free_skb(pkt_next);
 
@@ -827,7 +826,7 @@ brcmf_sdiod_ramrw(struct brcmf_sdio_dev *sdiodev, bool write, u32 address,
 		}
 		if (!write)
 			memcpy(data, pkt->data, dsize);
-		skb_trim(pkt, dsize);
+		skb_trim(pkt, 0);
 
 		/* Adjust for next transfer (if any) */
 		size -= dsize;
@@ -864,6 +863,29 @@ int brcmf_sdiod_abort(struct brcmf_sdio_dev *sdiodev, uint fn)
 	return 0;
 }
 
+static void brcmf_sdiod_sgtable_alloc(struct brcmf_sdio_dev *sdiodev)
+{
+	uint nents;
+	int err;
+
+	if (!sdiodev->sg_support)
+		return;
+
+	nents = max_t(uint, BRCMF_DEFAULT_RXGLOM_SIZE, brcmf_sdiod_txglomsz);
+	nents += (nents >> 4) + 1;
+
+	WARN_ON(nents > sdiodev->max_segment_count);
+
+	brcmf_dbg(TRACE, "nents=%d\n", nents);
+	err = sg_alloc_table(&sdiodev->sgtable, nents, GFP_KERNEL);
+	if (err < 0) {
+		brcmf_err("allocation failed: disable scatter-gather");
+		sdiodev->sg_support = false;
+	}
+
+	sdiodev->txglomsz = brcmf_sdiod_txglomsz;
+}
+
 static int brcmf_sdiod_remove(struct brcmf_sdio_dev *sdiodev)
 {
 	if (sdiodev->bus) {
@@ -881,6 +903,7 @@ static int brcmf_sdiod_remove(struct brcmf_sdio_dev *sdiodev)
 	sdio_disable_func(sdiodev->func[1]);
 	sdio_release_host(sdiodev->func[1]);
 
+	sg_free_table(&sdiodev->sgtable);
 	sdiodev->sbwad = 0;
 
 	return 0;
@@ -936,6 +959,11 @@ static int brcmf_sdiod_probe(struct brcmf_sdio_dev *sdiodev)
 					   SG_MAX_SINGLE_ALLOC);
 	sdiodev->max_segment_size = host->max_seg_size;
 
+	/* allocate scatter-gather table. sg support
+	 * will be disabled upon allocation failure.
+	 */
+	brcmf_sdiod_sgtable_alloc(sdiodev);
+
 	/* try to attach to the target device */
 	sdiodev->bus = brcmf_sdio_probe(sdiodev);
 	if (!sdiodev->bus) {
@@ -1073,9 +1101,7 @@ static int brcmf_ops_sdio_suspend(struct device *dev)
 	struct brcmf_sdio_dev *sdiodev = bus_if->bus_priv.sdio;
 	int ret = 0;
 
-	brcmf_dbg(SDIO, "\n");
-
-	atomic_set(&sdiodev->suspend, true);
+	brcmf_dbg(SDIO, "Enter\n");
 
 	sdio_flags = sdio_get_host_pm_caps(sdiodev->func[1]);
 	if (!(sdio_flags & MMC_PM_KEEP_POWER)) {
@@ -1083,9 +1109,12 @@ static int brcmf_ops_sdio_suspend(struct device *dev)
 		return -EINVAL;
 	}
 
+	atomic_set(&sdiodev->suspend, true);
+
 	ret = sdio_set_host_pm_flags(sdiodev->func[1], MMC_PM_KEEP_POWER);
 	if (ret) {
 		brcmf_err("Failed to set pm_flags\n");
+		atomic_set(&sdiodev->suspend, false);
 		return ret;
 	}
 
@@ -1099,6 +1128,7 @@ static int brcmf_ops_sdio_resume(struct device *dev)
 	struct brcmf_bus *bus_if = dev_get_drvdata(dev);
 	struct brcmf_sdio_dev *sdiodev = bus_if->bus_priv.sdio;
 
+	brcmf_dbg(SDIO, "Enter\n");
 	brcmf_sdio_wd_timer(sdiodev->bus, BRCMF_WD_POLL_MS);
 	atomic_set(&sdiodev->suspend, false);
 	return 0;
@@ -1115,11 +1145,12 @@ static struct sdio_driver brcmf_sdmmc_driver = {
 	.remove = brcmf_ops_sdio_remove,
 	.name = BRCMFMAC_SDIO_PDATA_NAME,
 	.id_table = brcmf_sdmmc_ids,
-#ifdef CONFIG_PM_SLEEP
 	.drv = {
+		.owner = THIS_MODULE,
+#ifdef CONFIG_PM_SLEEP
 		.pm = &brcmf_sdio_pm_ops,
-	},
 #endif	/* CONFIG_PM_SLEEP */
+	},
 };
 
 static int brcmf_sdio_pd_probe(struct platform_device *pdev)
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/chip.c b/drivers/net/wireless/brcm80211/brcmfmac/chip.c
new file mode 100644
index 000000000000..a07b95ef9e70
--- /dev/null
+++ b/drivers/net/wireless/brcm80211/brcmfmac/chip.c
@@ -0,0 +1,1029 @@
+/*
+ * Copyright (c) 2014 Broadcom Corporation
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+ * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
+ * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/list.h>
+#include <linux/ssb/ssb_regs.h>
+#include <linux/bcma/bcma.h>
+#include <linux/bcma/bcma_regs.h>
+
+#include <defs.h>
+#include <soc.h>
+#include <brcm_hw_ids.h>
+#include <brcmu_utils.h>
+#include <chipcommon.h>
+#include "dhd_dbg.h"
+#include "chip.h"
+
+/* SOC Interconnect types (aka chip types) */
+#define SOCI_SB		0
+#define SOCI_AI		1
+
+/* PL-368 DMP definitions */
+#define DMP_DESC_TYPE_MSK	0x0000000F
+#define  DMP_DESC_EMPTY		0x00000000
+#define  DMP_DESC_VALID		0x00000001
+#define  DMP_DESC_COMPONENT	0x00000001
+#define  DMP_DESC_MASTER_PORT	0x00000003
+#define  DMP_DESC_ADDRESS	0x00000005
+#define  DMP_DESC_ADDRSIZE_GT32	0x00000008
+#define  DMP_DESC_EOT		0x0000000F
+
+#define DMP_COMP_DESIGNER	0xFFF00000
+#define DMP_COMP_DESIGNER_S	20
+#define DMP_COMP_PARTNUM	0x000FFF00
+#define DMP_COMP_PARTNUM_S	8
+#define DMP_COMP_CLASS		0x000000F0
+#define DMP_COMP_CLASS_S	4
+#define DMP_COMP_REVISION	0xFF000000
+#define DMP_COMP_REVISION_S	24
+#define DMP_COMP_NUM_SWRAP	0x00F80000
+#define DMP_COMP_NUM_SWRAP_S	19
+#define DMP_COMP_NUM_MWRAP	0x0007C000
+#define DMP_COMP_NUM_MWRAP_S	14
+#define DMP_COMP_NUM_SPORT	0x00003E00
+#define DMP_COMP_NUM_SPORT_S	9
+#define DMP_COMP_NUM_MPORT	0x000001F0
+#define DMP_COMP_NUM_MPORT_S	4
+
+#define DMP_MASTER_PORT_UID	0x0000FF00
+#define DMP_MASTER_PORT_UID_S	8
+#define DMP_MASTER_PORT_NUM	0x000000F0
+#define DMP_MASTER_PORT_NUM_S	4
+
+#define DMP_SLAVE_ADDR_BASE	0xFFFFF000
+#define DMP_SLAVE_ADDR_BASE_S	12
+#define DMP_SLAVE_PORT_NUM	0x00000F00
+#define DMP_SLAVE_PORT_NUM_S	8
+#define DMP_SLAVE_TYPE		0x000000C0
+#define DMP_SLAVE_TYPE_S	6
+#define  DMP_SLAVE_TYPE_SLAVE	0
+#define  DMP_SLAVE_TYPE_BRIDGE	1
+#define  DMP_SLAVE_TYPE_SWRAP	2
+#define  DMP_SLAVE_TYPE_MWRAP	3
+#define DMP_SLAVE_SIZE_TYPE	0x00000030
+#define DMP_SLAVE_SIZE_TYPE_S	4
+#define  DMP_SLAVE_SIZE_4K	0
+#define  DMP_SLAVE_SIZE_8K	1
+#define  DMP_SLAVE_SIZE_16K	2
+#define  DMP_SLAVE_SIZE_DESC	3
+
+/* EROM CompIdentB */
+#define CIB_REV_MASK		0xff000000
+#define CIB_REV_SHIFT		24
+
+/* ARM CR4 core specific control flag bits */
+#define ARMCR4_BCMA_IOCTL_CPUHALT	0x0020
+
+/* D11 core specific control flag bits */
+#define D11_BCMA_IOCTL_PHYCLOCKEN	0x0004
+#define D11_BCMA_IOCTL_PHYRESET		0x0008
+
+/* chip core base & ramsize */
+/* bcm4329 */
+/* SDIO device core, ID 0x829 */
+#define BCM4329_CORE_BUS_BASE		0x18011000
+/* internal memory core, ID 0x80e */
+#define BCM4329_CORE_SOCRAM_BASE	0x18003000
+/* ARM Cortex M3 core, ID 0x82a */
+#define BCM4329_CORE_ARM_BASE		0x18002000
+#define BCM4329_RAMSIZE			0x48000
+
+/* bcm43143 */
+/* SDIO device core */
+#define BCM43143_CORE_BUS_BASE		0x18002000
+/* internal memory core */
+#define BCM43143_CORE_SOCRAM_BASE	0x18004000
+/* ARM Cortex M3 core, ID 0x82a */
+#define BCM43143_CORE_ARM_BASE		0x18003000
+#define BCM43143_RAMSIZE		0x70000
+
+#define CORE_SB(base, field) \
+		(base + SBCONFIGOFF + offsetof(struct sbconfig, field))
+#define	SBCOREREV(sbidh) \
+	((((sbidh) & SSB_IDHIGH_RCHI) >> SSB_IDHIGH_RCHI_SHIFT) | \
+	  ((sbidh) & SSB_IDHIGH_RCLO))
+
+struct sbconfig {
+	u32 PAD[2];
+	u32 sbipsflag;	/* initiator port ocp slave flag */
+	u32 PAD[3];
+	u32 sbtpsflag;	/* target port ocp slave flag */
+	u32 PAD[11];
+	u32 sbtmerrloga;	/* (sonics >= 2.3) */
+	u32 PAD;
+	u32 sbtmerrlog;	/* (sonics >= 2.3) */
+	u32 PAD[3];
+	u32 sbadmatch3;	/* address match3 */
+	u32 PAD;
+	u32 sbadmatch2;	/* address match2 */
+	u32 PAD;
+	u32 sbadmatch1;	/* address match1 */
+	u32 PAD[7];
+	u32 sbimstate;	/* initiator agent state */
+	u32 sbintvec;	/* interrupt mask */
+	u32 sbtmstatelow;	/* target state */
+	u32 sbtmstatehigh;	/* target state */
+	u32 sbbwa0;		/* bandwidth allocation table0 */
+	u32 PAD;
+	u32 sbimconfiglow;	/* initiator configuration */
+	u32 sbimconfighigh;	/* initiator configuration */
+	u32 sbadmatch0;	/* address match0 */
+	u32 PAD;
+	u32 sbtmconfiglow;	/* target configuration */
+	u32 sbtmconfighigh;	/* target configuration */
+	u32 sbbconfig;	/* broadcast configuration */
+	u32 PAD;
+	u32 sbbstate;	/* broadcast state */
+	u32 PAD[3];
+	u32 sbactcnfg;	/* activate configuration */
+	u32 PAD[3];
+	u32 sbflagst;	/* current sbflags */
+	u32 PAD[3];
+	u32 sbidlow;		/* identification */
+	u32 sbidhigh;	/* identification */
+};
+
+struct brcmf_core_priv {
+	struct brcmf_core pub;
+	u32 wrapbase;
+	struct list_head list;
+	struct brcmf_chip_priv *chip;
+};
+
+/* ARM CR4 core specific control flag bits */
+#define ARMCR4_BCMA_IOCTL_CPUHALT	0x0020
+
+/* D11 core specific control flag bits */
+#define D11_BCMA_IOCTL_PHYCLOCKEN	0x0004
+#define D11_BCMA_IOCTL_PHYRESET		0x0008
+
+struct brcmf_chip_priv {
+	struct brcmf_chip pub;
+	const struct brcmf_buscore_ops *ops;
+	void *ctx;
+	/* assured first core is chipcommon, second core is buscore */
+	struct list_head cores;
+	u16 num_cores;
+
+	bool (*iscoreup)(struct brcmf_core_priv *core);
+	void (*coredisable)(struct brcmf_core_priv *core, u32 prereset,
+			    u32 reset);
+	void (*resetcore)(struct brcmf_core_priv *core, u32 prereset, u32 reset,
+			  u32 postreset);
+};
+
+static void brcmf_chip_sb_corerev(struct brcmf_chip_priv *ci,
+				  struct brcmf_core *core)
+{
+	u32 regdata;
+
+	regdata = ci->ops->read32(ci->ctx, CORE_SB(core->base, sbidhigh));
+	core->rev = SBCOREREV(regdata);
+}
+
+static bool brcmf_chip_sb_iscoreup(struct brcmf_core_priv *core)
+{
+	struct brcmf_chip_priv *ci;
+	u32 regdata;
+	u32 address;
+
+	ci = core->chip;
+	address = CORE_SB(core->pub.base, sbtmstatelow);
+	regdata = ci->ops->read32(ci->ctx, address);
+	regdata &= (SSB_TMSLOW_RESET | SSB_TMSLOW_REJECT |
+		    SSB_IMSTATE_REJECT | SSB_TMSLOW_CLOCK);
+	return SSB_TMSLOW_CLOCK == regdata;
+}
+
+static bool brcmf_chip_ai_iscoreup(struct brcmf_core_priv *core)
+{
+	struct brcmf_chip_priv *ci;
+	u32 regdata;
+	bool ret;
+
+	ci = core->chip;
+	regdata = ci->ops->read32(ci->ctx, core->wrapbase + BCMA_IOCTL);
+	ret = (regdata & (BCMA_IOCTL_FGC | BCMA_IOCTL_CLK)) == BCMA_IOCTL_CLK;
+
+	regdata = ci->ops->read32(ci->ctx, core->wrapbase + BCMA_RESET_CTL);
+	ret = ret && ((regdata & BCMA_RESET_CTL_RESET) == 0);
+
+	return ret;
+}
+
+static void brcmf_chip_sb_coredisable(struct brcmf_core_priv *core,
+				      u32 prereset, u32 reset)
+{
+	struct brcmf_chip_priv *ci;
+	u32 val, base;
+
+	ci = core->chip;
+	base = core->pub.base;
+	val = ci->ops->read32(ci->ctx, CORE_SB(base, sbtmstatelow));
+	if (val & SSB_TMSLOW_RESET)
+		return;
+
+	val = ci->ops->read32(ci->ctx, CORE_SB(base, sbtmstatelow));
+	if ((val & SSB_TMSLOW_CLOCK) != 0) {
+		/*
+		 * set target reject and spin until busy is clear
+		 * (preserve core-specific bits)
+		 */
+		val = ci->ops->read32(ci->ctx, CORE_SB(base, sbtmstatelow));
+		ci->ops->write32(ci->ctx, CORE_SB(base, sbtmstatelow),
+					 val | SSB_TMSLOW_REJECT);
+
+		val = ci->ops->read32(ci->ctx, CORE_SB(base, sbtmstatelow));
+		udelay(1);
+		SPINWAIT((ci->ops->read32(ci->ctx, CORE_SB(base, sbtmstatehigh))
+			  & SSB_TMSHIGH_BUSY), 100000);
+
+		val = ci->ops->read32(ci->ctx, CORE_SB(base, sbtmstatehigh));
+		if (val & SSB_TMSHIGH_BUSY)
+			brcmf_err("core state still busy\n");
+
+		val = ci->ops->read32(ci->ctx, CORE_SB(base, sbidlow));
+		if (val & SSB_IDLOW_INITIATOR) {
+			val = ci->ops->read32(ci->ctx,
+					      CORE_SB(base, sbimstate));
+			val |= SSB_IMSTATE_REJECT;
+			ci->ops->write32(ci->ctx,
+					 CORE_SB(base, sbimstate), val);
+			val = ci->ops->read32(ci->ctx,
+					      CORE_SB(base, sbimstate));
+			udelay(1);
+			SPINWAIT((ci->ops->read32(ci->ctx,
+						  CORE_SB(base, sbimstate)) &
+				  SSB_IMSTATE_BUSY), 100000);
+		}
+
+		/* set reset and reject while enabling the clocks */
+		val = SSB_TMSLOW_FGC | SSB_TMSLOW_CLOCK |
+		      SSB_TMSLOW_REJECT | SSB_TMSLOW_RESET;
+		ci->ops->write32(ci->ctx, CORE_SB(base, sbtmstatelow), val);
+		val = ci->ops->read32(ci->ctx, CORE_SB(base, sbtmstatelow));
+		udelay(10);
+
+		/* clear the initiator reject bit */
+		val = ci->ops->read32(ci->ctx, CORE_SB(base, sbidlow));
+		if (val & SSB_IDLOW_INITIATOR) {
+			val = ci->ops->read32(ci->ctx,
+					      CORE_SB(base, sbimstate));
+			val &= ~SSB_IMSTATE_REJECT;
+			ci->ops->write32(ci->ctx,
+					 CORE_SB(base, sbimstate), val);
+		}
+	}
+
+	/* leave reset and reject asserted */
+	ci->ops->write32(ci->ctx, CORE_SB(base, sbtmstatelow),
+			 (SSB_TMSLOW_REJECT | SSB_TMSLOW_RESET));
+	udelay(1);
+}
+
+static void brcmf_chip_ai_coredisable(struct brcmf_core_priv *core,
+				      u32 prereset, u32 reset)
+{
+	struct brcmf_chip_priv *ci;
+	u32 regdata;
+
+	ci = core->chip;
+
+	/* if core is already in reset, just return */
+	regdata = ci->ops->read32(ci->ctx, core->wrapbase + BCMA_RESET_CTL);
+	if ((regdata & BCMA_RESET_CTL_RESET) != 0)
+		return;
+
+	/* configure reset */
+	ci->ops->write32(ci->ctx, core->wrapbase + BCMA_IOCTL,
+			 prereset | BCMA_IOCTL_FGC | BCMA_IOCTL_CLK);
+	ci->ops->read32(ci->ctx, core->wrapbase + BCMA_IOCTL);
+
+	/* put in reset */
+	ci->ops->write32(ci->ctx, core->wrapbase + BCMA_RESET_CTL,
+			 BCMA_RESET_CTL_RESET);
+	usleep_range(10, 20);
+
+	/* wait till reset is 1 */
+	SPINWAIT(ci->ops->read32(ci->ctx, core->wrapbase + BCMA_RESET_CTL) !=
+		 BCMA_RESET_CTL_RESET, 300);
+
+	/* in-reset configure */
+	ci->ops->write32(ci->ctx, core->wrapbase + BCMA_IOCTL,
+			 reset | BCMA_IOCTL_FGC | BCMA_IOCTL_CLK);
+	ci->ops->read32(ci->ctx, core->wrapbase + BCMA_IOCTL);
+}
+
+static void brcmf_chip_sb_resetcore(struct brcmf_core_priv *core, u32 prereset,
+				    u32 reset, u32 postreset)
+{
+	struct brcmf_chip_priv *ci;
+	u32 regdata;
+	u32 base;
+
+	ci = core->chip;
+	base = core->pub.base;
+	/*
+	 * Must do the disable sequence first to work for
+	 * arbitrary current core state.
+	 */
+	brcmf_chip_sb_coredisable(core, 0, 0);
+
+	/*
+	 * Now do the initialization sequence.
+	 * set reset while enabling the clock and
+	 * forcing them on throughout the core
+	 */
+	ci->ops->write32(ci->ctx, CORE_SB(base, sbtmstatelow),
+			 SSB_TMSLOW_FGC | SSB_TMSLOW_CLOCK |
+			 SSB_TMSLOW_RESET);
+	regdata = ci->ops->read32(ci->ctx, CORE_SB(base, sbtmstatelow));
+	udelay(1);
+
+	/* clear any serror */
+	regdata = ci->ops->read32(ci->ctx, CORE_SB(base, sbtmstatehigh));
+	if (regdata & SSB_TMSHIGH_SERR)
+		ci->ops->write32(ci->ctx, CORE_SB(base, sbtmstatehigh), 0);
+
+	regdata = ci->ops->read32(ci->ctx, CORE_SB(base, sbimstate));
+	if (regdata & (SSB_IMSTATE_IBE | SSB_IMSTATE_TO)) {
+		regdata &= ~(SSB_IMSTATE_IBE | SSB_IMSTATE_TO);
+		ci->ops->write32(ci->ctx, CORE_SB(base, sbimstate), regdata);
+	}
+
+	/* clear reset and allow it to propagate throughout the core */
+	ci->ops->write32(ci->ctx, CORE_SB(base, sbtmstatelow),
+			 SSB_TMSLOW_FGC | SSB_TMSLOW_CLOCK);
+	regdata = ci->ops->read32(ci->ctx, CORE_SB(base, sbtmstatelow));
+	udelay(1);
+
+	/* leave clock enabled */
+	ci->ops->write32(ci->ctx, CORE_SB(base, sbtmstatelow),
+			 SSB_TMSLOW_CLOCK);
+	regdata = ci->ops->read32(ci->ctx, CORE_SB(base, sbtmstatelow));
+	udelay(1);
+}
+
+static void brcmf_chip_ai_resetcore(struct brcmf_core_priv *core, u32 prereset,
+				    u32 reset, u32 postreset)
+{
+	struct brcmf_chip_priv *ci;
+	int count;
+
+	ci = core->chip;
+
+	/* must disable first to work for arbitrary current core state */
+	brcmf_chip_ai_coredisable(core, prereset, reset);
+
+	count = 0;
+	while (ci->ops->read32(ci->ctx, core->wrapbase + BCMA_RESET_CTL) &
+	       BCMA_RESET_CTL_RESET) {
+		ci->ops->write32(ci->ctx, core->wrapbase + BCMA_RESET_CTL, 0);
+		count++;
+		if (count > 50)
+			break;
+		usleep_range(40, 60);
+	}
+
+	ci->ops->write32(ci->ctx, core->wrapbase + BCMA_IOCTL,
+			 postreset | BCMA_IOCTL_CLK);
+	ci->ops->read32(ci->ctx, core->wrapbase + BCMA_IOCTL);
+}
+
+static char *brcmf_chip_name(uint chipid, char *buf, uint len)
+{
+	const char *fmt;
+
+	fmt = ((chipid > 0xa000) || (chipid < 0x4000)) ? "%d" : "%x";
+	snprintf(buf, len, fmt, chipid);
+	return buf;
+}
+
+static struct brcmf_core *brcmf_chip_add_core(struct brcmf_chip_priv *ci,
+					      u16 coreid, u32 base,
+					      u32 wrapbase)
+{
+	struct brcmf_core_priv *core;
+
+	core = kzalloc(sizeof(*core), GFP_KERNEL);
+	if (!core)
+		return ERR_PTR(-ENOMEM);
+
+	core->pub.id = coreid;
+	core->pub.base = base;
+	core->chip = ci;
+	core->wrapbase = wrapbase;
+
+	list_add_tail(&core->list, &ci->cores);
+	return &core->pub;
+}
+
+#ifdef DEBUG
+/* safety check for chipinfo */
+static int brcmf_chip_cores_check(struct brcmf_chip_priv *ci)
+{
+	struct brcmf_core_priv *core;
+	bool need_socram = false;
+	bool has_socram = false;
+	int idx = 1;
+
+	list_for_each_entry(core, &ci->cores, list) {
+		brcmf_dbg(INFO, " [%-2d] core 0x%x:%-2d base 0x%08x wrap 0x%08x\n",
+			  idx++, core->pub.id, core->pub.rev, core->pub.base,
+			  core->wrapbase);
+
+		switch (core->pub.id) {
+		case BCMA_CORE_ARM_CM3:
+			need_socram = true;
+			break;
+		case BCMA_CORE_INTERNAL_MEM:
+			has_socram = true;
+			break;
+		case BCMA_CORE_ARM_CR4:
+			if (ci->pub.rambase == 0) {
+				brcmf_err("RAM base not provided with ARM CR4 core\n");
+				return -ENOMEM;
+			}
+			break;
+		default:
+			break;
+		}
+	}
+
+	/* check RAM core presence for ARM CM3 core */
+	if (need_socram && !has_socram) {
+		brcmf_err("RAM core not provided with ARM CM3 core\n");
+		return -ENODEV;
+	}
+	return 0;
+}
+#else	/* DEBUG */
+static inline int brcmf_chip_cores_check(struct brcmf_chip_priv *ci)
+{
+	return 0;
+}
+#endif
+
+static void brcmf_chip_get_raminfo(struct brcmf_chip_priv *ci)
+{
+	switch (ci->pub.chip) {
+	case BCM4329_CHIP_ID:
+		ci->pub.ramsize = BCM4329_RAMSIZE;
+		break;
+	case BCM43143_CHIP_ID:
+		ci->pub.ramsize = BCM43143_RAMSIZE;
+		break;
+	case BCM43241_CHIP_ID:
+		ci->pub.ramsize = 0x90000;
+		break;
+	case BCM4330_CHIP_ID:
+		ci->pub.ramsize = 0x48000;
+		break;
+	case BCM4334_CHIP_ID:
+		ci->pub.ramsize = 0x80000;
+		break;
+	case BCM4335_CHIP_ID:
+		ci->pub.ramsize = 0xc0000;
+		ci->pub.rambase = 0x180000;
+		break;
+	case BCM43362_CHIP_ID:
+		ci->pub.ramsize = 0x3c000;
+		break;
+	case BCM4339_CHIP_ID:
+		ci->pub.ramsize = 0xc0000;
+		ci->pub.rambase = 0x180000;
+		break;
+	default:
+		brcmf_err("unknown chip: %s\n", ci->pub.name);
+		break;
+	}
+}
+
+static u32 brcmf_chip_dmp_get_desc(struct brcmf_chip_priv *ci, u32 *eromaddr,
+				   u8 *type)
+{
+	u32 val;
+
+	/* read next descriptor */
+	val = ci->ops->read32(ci->ctx, *eromaddr);
+	*eromaddr += 4;
+
+	if (!type)
+		return val;
+
+	/* determine descriptor type */
+	*type = (val & DMP_DESC_TYPE_MSK);
+	if ((*type & ~DMP_DESC_ADDRSIZE_GT32) == DMP_DESC_ADDRESS)
+		*type = DMP_DESC_ADDRESS;
+
+	return val;
+}
+
+static int brcmf_chip_dmp_get_regaddr(struct brcmf_chip_priv *ci, u32 *eromaddr,
+				      u32 *regbase, u32 *wrapbase)
+{
+	u8 desc;
+	u32 val;
+	u8 mpnum = 0;
+	u8 stype, sztype, wraptype;
+
+	*regbase = 0;
+	*wrapbase = 0;
+
+	val = brcmf_chip_dmp_get_desc(ci, eromaddr, &desc);
+	if (desc == DMP_DESC_MASTER_PORT) {
+		mpnum = (val & DMP_MASTER_PORT_NUM) >> DMP_MASTER_PORT_NUM_S;
+		wraptype = DMP_SLAVE_TYPE_MWRAP;
+	} else if (desc == DMP_DESC_ADDRESS) {
+		/* revert erom address */
+		*eromaddr -= 4;
+		wraptype = DMP_SLAVE_TYPE_SWRAP;
+	} else {
+		*eromaddr -= 4;
+		return -EILSEQ;
+	}
+
+	do {
+		/* locate address descriptor */
+		do {
+			val = brcmf_chip_dmp_get_desc(ci, eromaddr, &desc);
+			/* unexpected table end */
+			if (desc == DMP_DESC_EOT) {
+				*eromaddr -= 4;
+				return -EFAULT;
+			}
+		} while (desc != DMP_DESC_ADDRESS);
+
+		/* skip upper 32-bit address descriptor */
+		if (val & DMP_DESC_ADDRSIZE_GT32)
+			brcmf_chip_dmp_get_desc(ci, eromaddr, NULL);
+
+		sztype = (val & DMP_SLAVE_SIZE_TYPE) >> DMP_SLAVE_SIZE_TYPE_S;
+
+		/* next size descriptor can be skipped */
+		if (sztype == DMP_SLAVE_SIZE_DESC) {
+			val = brcmf_chip_dmp_get_desc(ci, eromaddr, NULL);
+			/* skip upper size descriptor if present */
+			if (val & DMP_DESC_ADDRSIZE_GT32)
+				brcmf_chip_dmp_get_desc(ci, eromaddr, NULL);
+		}
+
+		/* only look for 4K register regions */
+		if (sztype != DMP_SLAVE_SIZE_4K)
+			continue;
+
+		stype = (val & DMP_SLAVE_TYPE) >> DMP_SLAVE_TYPE_S;
+
+		/* only regular slave and wrapper */
+		if (*regbase == 0 && stype == DMP_SLAVE_TYPE_SLAVE)
+			*regbase = val & DMP_SLAVE_ADDR_BASE;
+		if (*wrapbase == 0 && stype == wraptype)
+			*wrapbase = val & DMP_SLAVE_ADDR_BASE;
+	} while (*regbase == 0 || *wrapbase == 0);
+
+	return 0;
+}
+
+static
+int brcmf_chip_dmp_erom_scan(struct brcmf_chip_priv *ci)
+{
+	struct brcmf_core *core;
+	u32 eromaddr;
+	u8 desc_type = 0;
+	u32 val;
+	u16 id;
+	u8 nmp, nsp, nmw, nsw, rev;
+	u32 base, wrap;
+	int err;
+
+	eromaddr = ci->ops->read32(ci->ctx, CORE_CC_REG(SI_ENUM_BASE, eromptr));
+
+	while (desc_type != DMP_DESC_EOT) {
+		val = brcmf_chip_dmp_get_desc(ci, &eromaddr, &desc_type);
+		if (!(val & DMP_DESC_VALID))
+			continue;
+
+		if (desc_type == DMP_DESC_EMPTY)
+			continue;
+
+		/* need a component descriptor */
+		if (desc_type != DMP_DESC_COMPONENT)
+			continue;
+
+		id = (val & DMP_COMP_PARTNUM) >> DMP_COMP_PARTNUM_S;
+
+		/* next descriptor must be component as well */
+		val = brcmf_chip_dmp_get_desc(ci, &eromaddr, &desc_type);
+		if (WARN_ON((val & DMP_DESC_TYPE_MSK) != DMP_DESC_COMPONENT))
+			return -EFAULT;
+
+		/* only look at cores with master port(s) */
+		nmp = (val & DMP_COMP_NUM_MPORT) >> DMP_COMP_NUM_MPORT_S;
+		nsp = (val & DMP_COMP_NUM_SPORT) >> DMP_COMP_NUM_SPORT_S;
+		nmw = (val & DMP_COMP_NUM_MWRAP) >> DMP_COMP_NUM_MWRAP_S;
+		nsw = (val & DMP_COMP_NUM_SWRAP) >> DMP_COMP_NUM_SWRAP_S;
+		rev = (val & DMP_COMP_REVISION) >> DMP_COMP_REVISION_S;
+
+		/* need core with ports */
+		if (nmw + nsw == 0)
+			continue;
+
+		/* try to obtain register address info */
+		err = brcmf_chip_dmp_get_regaddr(ci, &eromaddr, &base, &wrap);
+		if (err)
+			continue;
+
+		/* finally a core to be added */
+		core = brcmf_chip_add_core(ci, id, base, wrap);
+		if (IS_ERR(core))
+			return PTR_ERR(core);
+
+		core->rev = rev;
+	}
+
+	return 0;
+}
+
+static int brcmf_chip_recognition(struct brcmf_chip_priv *ci)
+{
+	struct brcmf_core *core;
+	u32 regdata;
+	u32 socitype;
+
+	/* Get CC core rev
+	 * Chipid is assume to be at offset 0 from SI_ENUM_BASE
+	 * For different chiptypes or old sdio hosts w/o chipcommon,
+	 * other ways of recognition should be added here.
+	 */
+	regdata = ci->ops->read32(ci->ctx, CORE_CC_REG(SI_ENUM_BASE, chipid));
+	ci->pub.chip = regdata & CID_ID_MASK;
+	ci->pub.chiprev = (regdata & CID_REV_MASK) >> CID_REV_SHIFT;
+	socitype = (regdata & CID_TYPE_MASK) >> CID_TYPE_SHIFT;
+
+	brcmf_chip_name(ci->pub.chip, ci->pub.name, sizeof(ci->pub.name));
+	brcmf_dbg(INFO, "found %s chip: BCM%s, rev=%d\n",
+		  socitype == SOCI_SB ? "SB" : "AXI", ci->pub.name,
+		  ci->pub.chiprev);
+
+	if (socitype == SOCI_SB) {
+		if (ci->pub.chip != BCM4329_CHIP_ID) {
+			brcmf_err("SB chip is not supported\n");
+			return -ENODEV;
+		}
+		ci->iscoreup = brcmf_chip_sb_iscoreup;
+		ci->coredisable = brcmf_chip_sb_coredisable;
+		ci->resetcore = brcmf_chip_sb_resetcore;
+
+		core = brcmf_chip_add_core(ci, BCMA_CORE_CHIPCOMMON,
+					   SI_ENUM_BASE, 0);
+		brcmf_chip_sb_corerev(ci, core);
+		core = brcmf_chip_add_core(ci, BCMA_CORE_SDIO_DEV,
+					   BCM4329_CORE_BUS_BASE, 0);
+		brcmf_chip_sb_corerev(ci, core);
+		core = brcmf_chip_add_core(ci, BCMA_CORE_INTERNAL_MEM,
+					   BCM4329_CORE_SOCRAM_BASE, 0);
+		brcmf_chip_sb_corerev(ci, core);
+		core = brcmf_chip_add_core(ci, BCMA_CORE_ARM_CM3,
+					   BCM4329_CORE_ARM_BASE, 0);
+		brcmf_chip_sb_corerev(ci, core);
+
+		core = brcmf_chip_add_core(ci, BCMA_CORE_80211, 0x18001000, 0);
+		brcmf_chip_sb_corerev(ci, core);
+	} else if (socitype == SOCI_AI) {
+		ci->iscoreup = brcmf_chip_ai_iscoreup;
+		ci->coredisable = brcmf_chip_ai_coredisable;
+		ci->resetcore = brcmf_chip_ai_resetcore;
+
+		brcmf_chip_dmp_erom_scan(ci);
+	} else {
+		brcmf_err("chip backplane type %u is not supported\n",
+			  socitype);
+		return -ENODEV;
+	}
+
+	brcmf_chip_get_raminfo(ci);
+
+	return brcmf_chip_cores_check(ci);
+}
+
+static void brcmf_chip_disable_arm(struct brcmf_chip_priv *chip, u16 id)
+{
+	struct brcmf_core *core;
+	struct brcmf_core_priv *cr4;
+	u32 val;
+
+
+	core = brcmf_chip_get_core(&chip->pub, id);
+	if (!core)
+		return;
+
+	switch (id) {
+	case BCMA_CORE_ARM_CM3:
+		brcmf_chip_coredisable(core, 0, 0);
+		break;
+	case BCMA_CORE_ARM_CR4:
+		cr4 = container_of(core, struct brcmf_core_priv, pub);
+
+		/* clear all IOCTL bits except HALT bit */
+		val = chip->ops->read32(chip->ctx, cr4->wrapbase + BCMA_IOCTL);
+		val &= ARMCR4_BCMA_IOCTL_CPUHALT;
+		brcmf_chip_resetcore(core, val, ARMCR4_BCMA_IOCTL_CPUHALT,
+				     ARMCR4_BCMA_IOCTL_CPUHALT);
+		break;
+	default:
+		brcmf_err("unknown id: %u\n", id);
+		break;
+	}
+}
+
+static int brcmf_chip_setup(struct brcmf_chip_priv *chip)
+{
+	struct brcmf_chip *pub;
+	struct brcmf_core_priv *cc;
+	u32 base;
+	u32 val;
+	int ret = 0;
+
+	pub = &chip->pub;
+	cc = list_first_entry(&chip->cores, struct brcmf_core_priv, list);
+	base = cc->pub.base;
+
+	/* get chipcommon capabilites */
+	pub->cc_caps = chip->ops->read32(chip->ctx,
+					 CORE_CC_REG(base, capabilities));
+
+	/* get pmu caps & rev */
+	if (pub->cc_caps & CC_CAP_PMU) {
+		val = chip->ops->read32(chip->ctx,
+					CORE_CC_REG(base, pmucapabilities));
+		pub->pmurev = val & PCAP_REV_MASK;
+		pub->pmucaps = val;
+	}
+
+	brcmf_dbg(INFO, "ccrev=%d, pmurev=%d, pmucaps=0x%x\n",
+		  cc->pub.rev, pub->pmurev, pub->pmucaps);
+
+	/* execute bus core specific setup */
+	if (chip->ops->setup)
+		ret = chip->ops->setup(chip->ctx, pub);
+
+	/*
+	 * Make sure any on-chip ARM is off (in case strapping is wrong),
+	 * or downloaded code was already running.
+	 */
+	brcmf_chip_disable_arm(chip, BCMA_CORE_ARM_CM3);
+	brcmf_chip_disable_arm(chip, BCMA_CORE_ARM_CR4);
+	return ret;
+}
+
+struct brcmf_chip *brcmf_chip_attach(void *ctx,
+				     const struct brcmf_buscore_ops *ops)
+{
+	struct brcmf_chip_priv *chip;
+	int err = 0;
+
+	if (WARN_ON(!ops->read32))
+		err = -EINVAL;
+	if (WARN_ON(!ops->write32))
+		err = -EINVAL;
+	if (WARN_ON(!ops->prepare))
+		err = -EINVAL;
+	if (WARN_ON(!ops->exit_dl))
+		err = -EINVAL;
+	if (err < 0)
+		return ERR_PTR(-EINVAL);
+
+	chip = kzalloc(sizeof(*chip), GFP_KERNEL);
+	if (!chip)
+		return ERR_PTR(-ENOMEM);
+
+	INIT_LIST_HEAD(&chip->cores);
+	chip->num_cores = 0;
+	chip->ops = ops;
+	chip->ctx = ctx;
+
+	err = ops->prepare(ctx);
+	if (err < 0)
+		goto fail;
+
+	err = brcmf_chip_recognition(chip);
+	if (err < 0)
+		goto fail;
+
+	err = brcmf_chip_setup(chip);
+	if (err < 0)
+		goto fail;
+
+	return &chip->pub;
+
+fail:
+	brcmf_chip_detach(&chip->pub);
+	return ERR_PTR(err);
+}
+
+void brcmf_chip_detach(struct brcmf_chip *pub)
+{
+	struct brcmf_chip_priv *chip;
+	struct brcmf_core_priv *core;
+	struct brcmf_core_priv *tmp;
+
+	chip = container_of(pub, struct brcmf_chip_priv, pub);
+	list_for_each_entry_safe(core, tmp, &chip->cores, list) {
+		list_del(&core->list);
+		kfree(core);
+	}
+	kfree(chip);
+}
+
+struct brcmf_core *brcmf_chip_get_core(struct brcmf_chip *pub, u16 coreid)
+{
+	struct brcmf_chip_priv *chip;
+	struct brcmf_core_priv *core;
+
+	chip = container_of(pub, struct brcmf_chip_priv, pub);
+	list_for_each_entry(core, &chip->cores, list)
+		if (core->pub.id == coreid)
+			return &core->pub;
+
+	return NULL;
+}
+
+struct brcmf_core *brcmf_chip_get_chipcommon(struct brcmf_chip *pub)
+{
+	struct brcmf_chip_priv *chip;
+	struct brcmf_core_priv *cc;
+
+	chip = container_of(pub, struct brcmf_chip_priv, pub);
+	cc = list_first_entry(&chip->cores, struct brcmf_core_priv, list);
+	if (WARN_ON(!cc || cc->pub.id != BCMA_CORE_CHIPCOMMON))
+		return brcmf_chip_get_core(pub, BCMA_CORE_CHIPCOMMON);
+	return &cc->pub;
+}
+
+bool brcmf_chip_iscoreup(struct brcmf_core *pub)
+{
+	struct brcmf_core_priv *core;
+
+	core = container_of(pub, struct brcmf_core_priv, pub);
+	return core->chip->iscoreup(core);
+}
+
+void brcmf_chip_coredisable(struct brcmf_core *pub, u32 prereset, u32 reset)
+{
+	struct brcmf_core_priv *core;
+
+	core = container_of(pub, struct brcmf_core_priv, pub);
+	core->chip->coredisable(core, prereset, reset);
+}
+
+void brcmf_chip_resetcore(struct brcmf_core *pub, u32 prereset, u32 reset,
+			  u32 postreset)
+{
+	struct brcmf_core_priv *core;
+
+	core = container_of(pub, struct brcmf_core_priv, pub);
+	core->chip->resetcore(core, prereset, reset, postreset);
+}
+
+static void
+brcmf_chip_cm3_enterdl(struct brcmf_chip_priv *chip)
+{
+	struct brcmf_core *core;
+
+	brcmf_chip_disable_arm(chip, BCMA_CORE_ARM_CM3);
+	core = brcmf_chip_get_core(&chip->pub, BCMA_CORE_80211);
+	brcmf_chip_resetcore(core, D11_BCMA_IOCTL_PHYRESET |
+				   D11_BCMA_IOCTL_PHYCLOCKEN,
+			     D11_BCMA_IOCTL_PHYCLOCKEN,
+			     D11_BCMA_IOCTL_PHYCLOCKEN);
+	core = brcmf_chip_get_core(&chip->pub, BCMA_CORE_INTERNAL_MEM);
+	brcmf_chip_resetcore(core, 0, 0, 0);
+}
+
+static bool brcmf_chip_cm3_exitdl(struct brcmf_chip_priv *chip)
+{
+	struct brcmf_core *core;
+
+	core = brcmf_chip_get_core(&chip->pub, BCMA_CORE_INTERNAL_MEM);
+	if (!brcmf_chip_iscoreup(core)) {
+		brcmf_err("SOCRAM core is down after reset?\n");
+		return false;
+	}
+
+	chip->ops->exit_dl(chip->ctx, &chip->pub, 0);
+
+	core = brcmf_chip_get_core(&chip->pub, BCMA_CORE_ARM_CM3);
+	brcmf_chip_resetcore(core, 0, 0, 0);
+
+	return true;
+}
+
+static inline void
+brcmf_chip_cr4_enterdl(struct brcmf_chip_priv *chip)
+{
+	struct brcmf_core *core;
+
+	brcmf_chip_disable_arm(chip, BCMA_CORE_ARM_CR4);
+
+	core = brcmf_chip_get_core(&chip->pub, BCMA_CORE_80211);
+	brcmf_chip_resetcore(core, D11_BCMA_IOCTL_PHYRESET |
+				   D11_BCMA_IOCTL_PHYCLOCKEN,
+			     D11_BCMA_IOCTL_PHYCLOCKEN,
+			     D11_BCMA_IOCTL_PHYCLOCKEN);
+}
+
+static bool brcmf_chip_cr4_exitdl(struct brcmf_chip_priv *chip, u32 rstvec)
+{
+	struct brcmf_core *core;
+
+	chip->ops->exit_dl(chip->ctx, &chip->pub, rstvec);
+
+	/* restore ARM */
+	core = brcmf_chip_get_core(&chip->pub, BCMA_CORE_ARM_CR4);
+	brcmf_chip_resetcore(core, ARMCR4_BCMA_IOCTL_CPUHALT, 0, 0);
+
+	return true;
+}
+
+void brcmf_chip_enter_download(struct brcmf_chip *pub)
+{
+	struct brcmf_chip_priv *chip;
+	struct brcmf_core *arm;
+
+	brcmf_dbg(TRACE, "Enter\n");
+
+	chip = container_of(pub, struct brcmf_chip_priv, pub);
+	arm = brcmf_chip_get_core(pub, BCMA_CORE_ARM_CR4);
+	if (arm) {
+		brcmf_chip_cr4_enterdl(chip);
+		return;
+	}
+
+	brcmf_chip_cm3_enterdl(chip);
+}
+
+bool brcmf_chip_exit_download(struct brcmf_chip *pub, u32 rstvec)
+{
+	struct brcmf_chip_priv *chip;
+	struct brcmf_core *arm;
+
+	brcmf_dbg(TRACE, "Enter\n");
+
+	chip = container_of(pub, struct brcmf_chip_priv, pub);
+	arm = brcmf_chip_get_core(pub, BCMA_CORE_ARM_CR4);
+	if (arm)
+		return brcmf_chip_cr4_exitdl(chip, rstvec);
+
+	return brcmf_chip_cm3_exitdl(chip);
+}
+
+bool brcmf_chip_sr_capable(struct brcmf_chip *pub)
+{
+	u32 base, addr, reg, pmu_cc3_mask = ~0;
+	struct brcmf_chip_priv *chip;
+
+	brcmf_dbg(TRACE, "Enter\n");
+
+	/* old chips with PMU version less than 17 don't support save restore */
+	if (pub->pmurev < 17)
+		return false;
+
+	base = brcmf_chip_get_chipcommon(pub)->base;
+	chip = container_of(pub, struct brcmf_chip_priv, pub);
+
+	switch (pub->chip) {
+	case BCM43241_CHIP_ID:
+	case BCM4335_CHIP_ID:
+	case BCM4339_CHIP_ID:
+		/* read PMU chipcontrol register 3 */
+		addr = CORE_CC_REG(base, chipcontrol_addr);
+		chip->ops->write32(chip->ctx, addr, 3);
+		addr = CORE_CC_REG(base, chipcontrol_data);
+		reg = chip->ops->read32(chip->ctx, addr);
+		return (reg & pmu_cc3_mask) != 0;
+	default:
+		addr = CORE_CC_REG(base, pmucapabilities_ext);
+		reg = chip->ops->read32(chip->ctx, addr);
+		if ((reg & PCAPEXT_SR_SUPPORTED_MASK) == 0)
+			return false;
+
+		addr = CORE_CC_REG(base, retention_ctl);
+		reg = chip->ops->read32(chip->ctx, addr);
+		return (reg & (PMU_RCTL_MACPHY_DISABLE_MASK |
+			       PMU_RCTL_LOGIC_DISABLE_MASK)) == 0;
+	}
+}
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/chip.h b/drivers/net/wireless/brcm80211/brcmfmac/chip.h
new file mode 100644
index 000000000000..c32908da90c8
--- /dev/null
+++ b/drivers/net/wireless/brcm80211/brcmfmac/chip.h
@@ -0,0 +1,91 @@
+/*
+ * Copyright (c) 2014 Broadcom Corporation
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+ * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
+ * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#ifndef BRCMF_CHIP_H
+#define BRCMF_CHIP_H
+
+#include <linux/types.h>
+
+#define CORE_CC_REG(base, field) \
+		(base + offsetof(struct chipcregs, field))
+
+/**
+ * struct brcmf_chip - chip level information.
+ *
+ * @chip: chip identifier.
+ * @chiprev: chip revision.
+ * @cc_caps: chipcommon core capabilities.
+ * @pmucaps: PMU capabilities.
+ * @pmurev: PMU revision.
+ * @rambase: RAM base address (only applicable for ARM CR4 chips).
+ * @ramsize: amount of RAM on chip.
+ * @name: string representation of the chip identifier.
+ */
+struct brcmf_chip {
+	u32 chip;
+	u32 chiprev;
+	u32 cc_caps;
+	u32 pmucaps;
+	u32 pmurev;
+	u32 rambase;
+	u32 ramsize;
+	char name[8];
+};
+
+/**
+ * struct brcmf_core - core related information.
+ *
+ * @id: core identifier.
+ * @rev: core revision.
+ * @base: base address of core register space.
+ */
+struct brcmf_core {
+	u16 id;
+	u16 rev;
+	u32 base;
+};
+
+/**
+ * struct brcmf_buscore_ops - buscore specific callbacks.
+ *
+ * @read32: read 32-bit value over bus.
+ * @write32: write 32-bit value over bus.
+ * @prepare: prepare bus for core configuration.
+ * @setup: bus-specific core setup.
+ * @exit_dl: exit download state.
+ *	The callback should use the provided @rstvec when non-zero.
+ */
+struct brcmf_buscore_ops {
+	u32 (*read32)(void *ctx, u32 addr);
+	void (*write32)(void *ctx, u32 addr, u32 value);
+	int (*prepare)(void *ctx);
+	int (*setup)(void *ctx, struct brcmf_chip *chip);
+	void (*exit_dl)(void *ctx, struct brcmf_chip *chip, u32 rstvec);
+};
+
+struct brcmf_chip *brcmf_chip_attach(void *ctx,
+				     const struct brcmf_buscore_ops *ops);
+void brcmf_chip_detach(struct brcmf_chip *chip);
+struct brcmf_core *brcmf_chip_get_core(struct brcmf_chip *chip, u16 coreid);
+struct brcmf_core *brcmf_chip_get_chipcommon(struct brcmf_chip *chip);
+bool brcmf_chip_iscoreup(struct brcmf_core *core);
+void brcmf_chip_coredisable(struct brcmf_core *core, u32 prereset, u32 reset);
+void brcmf_chip_resetcore(struct brcmf_core *core, u32 prereset, u32 reset,
+			  u32 postreset);
+void brcmf_chip_enter_download(struct brcmf_chip *ci);
+bool brcmf_chip_exit_download(struct brcmf_chip *ci, u32 rstvec);
+bool brcmf_chip_sr_capable(struct brcmf_chip *pub);
+
+#endif /* BRCMF_AXIDMP_H */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_linux.c b/drivers/net/wireless/brcm80211/brcmfmac/dhd_linux.c
index d4d966beb840..7d28cd385092 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_linux.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_linux.c
@@ -1040,12 +1040,12 @@ void brcmf_detach(struct device *dev)
 
 	brcmf_cfg80211_detach(drvr->config);
 
+	brcmf_fws_deinit(drvr);
+
 	brcmf_bus_detach(drvr);
 
 	brcmf_proto_detach(drvr);
 
-	brcmf_fws_deinit(drvr);
-
 	brcmf_debugfs_detach(drvr);
 	bus_if->drvr = NULL;
 	kfree(drvr);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c b/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
index 3e991897d7ca..a111b6fbbeba 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
@@ -23,6 +23,7 @@
 #include <linux/interrupt.h>
 #include <linux/sched.h>
 #include <linux/mmc/sdio.h>
+#include <linux/mmc/sdio_ids.h>
 #include <linux/mmc/sdio_func.h>
 #include <linux/mmc/card.h>
 #include <linux/semaphore.h>
@@ -40,7 +41,7 @@
 #include <brcm_hw_ids.h>
 #include <soc.h>
 #include "sdio_host.h"
-#include "sdio_chip.h"
+#include "chip.h"
 #include "nvram.h"
 
 #define DCMD_RESP_TIMEOUT  2000	/* In milli second */
@@ -112,8 +113,6 @@ struct rte_console {
 #define BRCMF_TXBOUND	20	/* Default for max tx frames in
 				 one scheduling */
 
-#define BRCMF_DEFAULT_TXGLOM_SIZE	32  /* max tx frames in glom chain */
-
 #define BRCMF_TXMINMAX	1	/* Max tx frames if rx still pending */
 
 #define MEMBLOCK	2048	/* Block size used for downloading
@@ -156,6 +155,33 @@ struct rte_console {
 /* manfid tuple length, include tuple, link bytes */
 #define SBSDIO_CIS_MANFID_TUPLE_LEN	6
 
+#define CORE_BUS_REG(base, field) \
+		(base + offsetof(struct sdpcmd_regs, field))
+
+/* SDIO function 1 register CHIPCLKCSR */
+/* Force ALP request to backplane */
+#define SBSDIO_FORCE_ALP		0x01
+/* Force HT request to backplane */
+#define SBSDIO_FORCE_HT			0x02
+/* Force ILP request to backplane */
+#define SBSDIO_FORCE_ILP		0x04
+/* Make ALP ready (power up xtal) */
+#define SBSDIO_ALP_AVAIL_REQ		0x08
+/* Make HT ready (power up PLL) */
+#define SBSDIO_HT_AVAIL_REQ		0x10
+/* Squelch clock requests from HW */
+#define SBSDIO_FORCE_HW_CLKREQ_OFF	0x20
+/* Status: ALP is ready */
+#define SBSDIO_ALP_AVAIL		0x40
+/* Status: HT is ready */
+#define SBSDIO_HT_AVAIL			0x80
+#define SBSDIO_AVBITS		(SBSDIO_HT_AVAIL | SBSDIO_ALP_AVAIL)
+#define SBSDIO_ALPAV(regval)	((regval) & SBSDIO_AVBITS)
+#define SBSDIO_HTAV(regval)	(((regval) & SBSDIO_AVBITS) == SBSDIO_AVBITS)
+#define SBSDIO_ALPONLY(regval)	(SBSDIO_ALPAV(regval) && !SBSDIO_HTAV(regval))
+#define SBSDIO_CLKAV(regval, alponly) \
+	(SBSDIO_ALPAV(regval) && (alponly ? 1 : SBSDIO_HTAV(regval)))
+
 /* intstatus */
 #define I_SMB_SW0	(1 << 0)	/* To SB Mail S/W interrupt 0 */
 #define I_SMB_SW1	(1 << 1)	/* To SB Mail S/W interrupt 1 */
@@ -276,7 +302,6 @@ struct rte_console {
 /* Flags for SDH calls */
 #define F2SYNC	(SDIO_REQ_4BYTE | SDIO_REQ_FIXED)
 
-#define BRCMF_IDLE_IMMEDIATE	(-1)	/* Enter idle immediately */
 #define BRCMF_IDLE_ACTIVE	0	/* Do not request any SD clock change
 					 * when idle
 					 */
@@ -457,7 +482,6 @@ struct brcmf_sdio {
 
 	u8 tx_hdrlen;		/* sdio bus header length for tx packet */
 	bool txglom;		/* host tx glomming enable flag */
-	struct sk_buff *txglom_sgpad;	/* scatter-gather padding buffer */
 	u16 head_align;		/* buffer pointer alignment */
 	u16 sgentry_align;	/* scatter-gather buffer alignment */
 };
@@ -484,16 +508,58 @@ static const uint max_roundup = 512;
 
 #define ALIGNMENT  4
 
-static int brcmf_sdio_txglomsz = BRCMF_DEFAULT_TXGLOM_SIZE;
-module_param_named(txglomsz, brcmf_sdio_txglomsz, int, 0);
-MODULE_PARM_DESC(txglomsz, "maximum tx packet chain size [SDIO]");
-
 enum brcmf_sdio_frmtype {
 	BRCMF_SDIO_FT_NORMAL,
 	BRCMF_SDIO_FT_SUPER,
 	BRCMF_SDIO_FT_SUB,
 };
 
+#define SDIOD_DRVSTR_KEY(chip, pmu)     (((chip) << 16) | (pmu))
+
+/* SDIO Pad drive strength to select value mappings */
+struct sdiod_drive_str {
+	u8 strength;	/* Pad Drive Strength in mA */
+	u8 sel;		/* Chip-specific select value */
+};
+
+/* SDIO Drive Strength to sel value table for PMU Rev 11 (1.8V) */
+static const struct sdiod_drive_str sdiod_drvstr_tab1_1v8[] = {
+	{32, 0x6},
+	{26, 0x7},
+	{22, 0x4},
+	{16, 0x5},
+	{12, 0x2},
+	{8, 0x3},
+	{4, 0x0},
+	{0, 0x1}
+};
+
+/* SDIO Drive Strength to sel value table for PMU Rev 13 (1.8v) */
+static const struct sdiod_drive_str sdiod_drive_strength_tab5_1v8[] = {
+	{6, 0x7},
+	{5, 0x6},
+	{4, 0x5},
+	{3, 0x4},
+	{2, 0x2},
+	{1, 0x1},
+	{0, 0x0}
+};
+
+/* SDIO Drive Strength to sel value table for PMU Rev 17 (1.8v) */
+static const struct sdiod_drive_str sdiod_drvstr_tab6_1v8[] = {
+	{3, 0x3},
+	{2, 0x2},
+	{1, 0x1},
+	{0, 0x0} };
+
+/* SDIO Drive Strength to sel value table for 43143 PMU Rev 17 (3.3V) */
+static const struct sdiod_drive_str sdiod_drvstr_tab2_3v3[] = {
+	{16, 0x7},
+	{12, 0x5},
+	{8,  0x3},
+	{4,  0x1}
+};
+
 #define BCM43143_FIRMWARE_NAME		"brcm/brcmfmac43143-sdio.bin"
 #define BCM43143_NVRAM_NAME		"brcm/brcmfmac43143-sdio.txt"
 #define BCM43241B0_FIRMWARE_NAME	"brcm/brcmfmac43241b0-sdio.bin"
@@ -619,27 +685,24 @@ static bool data_ok(struct brcmf_sdio *bus)
  * Reads a register in the SDIO hardware block. This block occupies a series of
  * adresses on the 32 bit backplane bus.
  */
-static int
-r_sdreg32(struct brcmf_sdio *bus, u32 *regvar, u32 offset)
+static int r_sdreg32(struct brcmf_sdio *bus, u32 *regvar, u32 offset)
 {
-	u8 idx = brcmf_sdio_chip_getinfidx(bus->ci, BCMA_CORE_SDIO_DEV);
+	struct brcmf_core *core;
 	int ret;
 
-	*regvar = brcmf_sdiod_regrl(bus->sdiodev,
-				    bus->ci->c_inf[idx].base + offset, &ret);
+	core = brcmf_chip_get_core(bus->ci, BCMA_CORE_SDIO_DEV);
+	*regvar = brcmf_sdiod_regrl(bus->sdiodev, core->base + offset, &ret);
 
 	return ret;
 }
 
-static int
-w_sdreg32(struct brcmf_sdio *bus, u32 regval, u32 reg_offset)
+static int w_sdreg32(struct brcmf_sdio *bus, u32 regval, u32 reg_offset)
 {
-	u8 idx = brcmf_sdio_chip_getinfidx(bus->ci, BCMA_CORE_SDIO_DEV);
+	struct brcmf_core *core;
 	int ret;
 
-	brcmf_sdiod_regwl(bus->sdiodev,
-			  bus->ci->c_inf[idx].base + reg_offset,
-			  regval, &ret);
+	core = brcmf_chip_get_core(bus->ci, BCMA_CORE_SDIO_DEV);
+	brcmf_sdiod_regwl(bus->sdiodev, core->base + reg_offset, regval, &ret);
 
 	return ret;
 }
@@ -700,8 +763,6 @@ brcmf_sdio_kso_control(struct brcmf_sdio *bus, bool on)
 	return err;
 }
 
-#define PKT_AVAILABLE()		(intstatus & I_HMB_FRAME_IND)
-
 #define HOSTINTMASK		(I_HMB_SW_MASK | I_CHIPACTIVE)
 
 /* Turn backplane clock on or off */
@@ -800,7 +861,6 @@ static int brcmf_sdio_htclk(struct brcmf_sdio *bus, bool on, bool pendok)
 		}
 #endif				/* defined (DEBUG) */
 
-		bus->activity = true;
 	} else {
 		clkreq = 0;
 
@@ -900,8 +960,8 @@ static int
 brcmf_sdio_bus_sleep(struct brcmf_sdio *bus, bool sleep, bool pendok)
 {
 	int err = 0;
-	brcmf_dbg(TRACE, "Enter\n");
-	brcmf_dbg(SDIO, "request %s currently %s\n",
+
+	brcmf_dbg(SDIO, "Enter: request %s currently %s\n",
 		  (sleep ? "SLEEP" : "WAKE"),
 		  (bus->sleeping ? "SLEEP" : "WAKE"));
 
@@ -953,6 +1013,86 @@ end:
 
 }
 
+#ifdef DEBUG
+static inline bool brcmf_sdio_valid_shared_address(u32 addr)
+{
+	return !(addr == 0 || ((~addr >> 16) & 0xffff) == (addr & 0xffff));
+}
+
+static int brcmf_sdio_readshared(struct brcmf_sdio *bus,
+				 struct sdpcm_shared *sh)
+{
+	u32 addr;
+	int rv;
+	u32 shaddr = 0;
+	struct sdpcm_shared_le sh_le;
+	__le32 addr_le;
+
+	shaddr = bus->ci->rambase + bus->ramsize - 4;
+
+	/*
+	 * Read last word in socram to determine
+	 * address of sdpcm_shared structure
+	 */
+	sdio_claim_host(bus->sdiodev->func[1]);
+	brcmf_sdio_bus_sleep(bus, false, false);
+	rv = brcmf_sdiod_ramrw(bus->sdiodev, false, shaddr, (u8 *)&addr_le, 4);
+	sdio_release_host(bus->sdiodev->func[1]);
+	if (rv < 0)
+		return rv;
+
+	addr = le32_to_cpu(addr_le);
+
+	brcmf_dbg(SDIO, "sdpcm_shared address 0x%08X\n", addr);
+
+	/*
+	 * Check if addr is valid.
+	 * NVRAM length at the end of memory should have been overwritten.
+	 */
+	if (!brcmf_sdio_valid_shared_address(addr)) {
+			brcmf_err("invalid sdpcm_shared address 0x%08X\n",
+				  addr);
+			return -EINVAL;
+	}
+
+	/* Read hndrte_shared structure */
+	rv = brcmf_sdiod_ramrw(bus->sdiodev, false, addr, (u8 *)&sh_le,
+			       sizeof(struct sdpcm_shared_le));
+	if (rv < 0)
+		return rv;
+
+	/* Endianness */
+	sh->flags = le32_to_cpu(sh_le.flags);
+	sh->trap_addr = le32_to_cpu(sh_le.trap_addr);
+	sh->assert_exp_addr = le32_to_cpu(sh_le.assert_exp_addr);
+	sh->assert_file_addr = le32_to_cpu(sh_le.assert_file_addr);
+	sh->assert_line = le32_to_cpu(sh_le.assert_line);
+	sh->console_addr = le32_to_cpu(sh_le.console_addr);
+	sh->msgtrace_addr = le32_to_cpu(sh_le.msgtrace_addr);
+
+	if ((sh->flags & SDPCM_SHARED_VERSION_MASK) > SDPCM_SHARED_VERSION) {
+		brcmf_err("sdpcm shared version unsupported: dhd %d dongle %d\n",
+			  SDPCM_SHARED_VERSION,
+			  sh->flags & SDPCM_SHARED_VERSION_MASK);
+		return -EPROTO;
+	}
+
+	return 0;
+}
+
+static void brcmf_sdio_get_console_addr(struct brcmf_sdio *bus)
+{
+	struct sdpcm_shared sh;
+
+	if (brcmf_sdio_readshared(bus, &sh) == 0)
+		bus->console_addr = sh.console_addr;
+}
+#else
+static void brcmf_sdio_get_console_addr(struct brcmf_sdio *bus)
+{
+}
+#endif /* DEBUG */
+
 static u32 brcmf_sdio_hostmail(struct brcmf_sdio *bus)
 {
 	u32 intstatus = 0;
@@ -996,6 +1136,12 @@ static u32 brcmf_sdio_hostmail(struct brcmf_sdio *bus)
 		else
 			brcmf_dbg(SDIO, "Dongle ready, protocol version %d\n",
 				  bus->sdpcm_ver);
+
+		/*
+		 * Retrieve console state address now that firmware should have
+		 * updated it.
+		 */
+		brcmf_sdio_get_console_addr(bus);
 	}
 
 	/*
@@ -1084,6 +1230,28 @@ static void brcmf_sdio_rxfail(struct brcmf_sdio *bus, bool abort, bool rtx)
 	bus->cur_read.len = 0;
 }
 
+static void brcmf_sdio_txfail(struct brcmf_sdio *bus)
+{
+	struct brcmf_sdio_dev *sdiodev = bus->sdiodev;
+	u8 i, hi, lo;
+
+	/* On failure, abort the command and terminate the frame */
+	brcmf_err("sdio error, abort command and terminate frame\n");
+	bus->sdcnt.tx_sderrs++;
+
+	brcmf_sdiod_abort(sdiodev, SDIO_FUNC_2);
+	brcmf_sdiod_regwb(sdiodev, SBSDIO_FUNC1_FRAMECTRL, SFC_WF_TERM, NULL);
+	bus->sdcnt.f1regdata++;
+
+	for (i = 0; i < 3; i++) {
+		hi = brcmf_sdiod_regrb(sdiodev, SBSDIO_FUNC1_WFRAMEBCHI, NULL);
+		lo = brcmf_sdiod_regrb(sdiodev, SBSDIO_FUNC1_WFRAMEBCLO, NULL);
+		bus->sdcnt.f1regdata += 2;
+		if ((hi == 0) && (lo == 0))
+			break;
+	}
+}
+
 /* return total length of buffer chain */
 static uint brcmf_sdio_glom_len(struct brcmf_sdio *bus)
 {
@@ -1944,9 +2112,8 @@ static int brcmf_sdio_txpkt_prep_sg(struct brcmf_sdio *bus,
 	if (lastfrm && chain_pad)
 		tail_pad += blksize - chain_pad;
 	if (skb_tailroom(pkt) < tail_pad && pkt->len > blksize) {
-		pkt_pad = bus->txglom_sgpad;
-		if (pkt_pad == NULL)
-			  brcmu_pkt_buf_get_skb(tail_pad + tail_chop);
+		pkt_pad = brcmu_pkt_buf_get_skb(tail_pad + tail_chop +
+						bus->head_align);
 		if (pkt_pad == NULL)
 			return -ENOMEM;
 		ret = brcmf_sdio_txpkt_hdalign(bus, pkt_pad);
@@ -1955,8 +2122,9 @@ static int brcmf_sdio_txpkt_prep_sg(struct brcmf_sdio *bus,
 		memcpy(pkt_pad->data,
 		       pkt->data + pkt->len - tail_chop,
 		       tail_chop);
-		*(u32 *)(pkt_pad->cb) = ALIGN_SKB_FLAG + tail_chop;
+		*(u16 *)(pkt_pad->cb) = ALIGN_SKB_FLAG + tail_chop;
 		skb_trim(pkt, pkt->len - tail_chop);
+		skb_trim(pkt_pad, tail_pad + tail_chop);
 		__skb_queue_after(pktq, pkt, pkt_pad);
 	} else {
 		ntail = pkt->data_len + tail_pad -
@@ -2002,7 +2170,7 @@ brcmf_sdio_txpkt_prep(struct brcmf_sdio *bus, struct sk_buff_head *pktq,
 		 * already properly aligned and does not
 		 * need an sdpcm header.
 		 */
-		if (*(u32 *)(pkt_next->cb) & ALIGN_SKB_FLAG)
+		if (*(u16 *)(pkt_next->cb) & ALIGN_SKB_FLAG)
 			continue;
 
 		/* align packet data pointer */
@@ -2011,7 +2179,7 @@ brcmf_sdio_txpkt_prep(struct brcmf_sdio *bus, struct sk_buff_head *pktq,
 			return ret;
 		head_pad = (u16)ret;
 		if (head_pad)
-			memset(pkt_next->data, 0, head_pad + bus->tx_hdrlen);
+			memset(pkt_next->data + bus->tx_hdrlen, 0, head_pad);
 
 		total_len += pkt_next->len;
 
@@ -2036,10 +2204,10 @@ brcmf_sdio_txpkt_prep(struct brcmf_sdio *bus, struct sk_buff_head *pktq,
 		if (BRCMF_BYTES_ON() &&
 		    ((BRCMF_CTL_ON() && chan == SDPCM_CONTROL_CHANNEL) ||
 		     (BRCMF_DATA_ON() && chan != SDPCM_CONTROL_CHANNEL)))
-			brcmf_dbg_hex_dump(true, pkt_next, hd_info.len,
+			brcmf_dbg_hex_dump(true, pkt_next->data, hd_info.len,
 					   "Tx Frame:\n");
 		else if (BRCMF_HDRS_ON())
-			brcmf_dbg_hex_dump(true, pkt_next,
+			brcmf_dbg_hex_dump(true, pkt_next->data,
 					   head_pad + bus->tx_hdrlen,
 					   "Tx Header:\n");
 	}
@@ -2066,11 +2234,11 @@ brcmf_sdio_txpkt_postp(struct brcmf_sdio *bus, struct sk_buff_head *pktq)
 	u8 *hdr;
 	u32 dat_offset;
 	u16 tail_pad;
-	u32 dummy_flags, chop_len;
+	u16 dummy_flags, chop_len;
 	struct sk_buff *pkt_next, *tmp, *pkt_prev;
 
 	skb_queue_walk_safe(pktq, pkt_next, tmp) {
-		dummy_flags = *(u32 *)(pkt_next->cb);
+		dummy_flags = *(u16 *)(pkt_next->cb);
 		if (dummy_flags & ALIGN_SKB_FLAG) {
 			chop_len = dummy_flags & ALIGN_SKB_CHOP_LEN_MASK;
 			if (chop_len) {
@@ -2099,7 +2267,6 @@ static int brcmf_sdio_txpkt(struct brcmf_sdio *bus, struct sk_buff_head *pktq,
 			    uint chan)
 {
 	int ret;
-	int i;
 	struct sk_buff *pkt_next, *tmp;
 
 	brcmf_dbg(TRACE, "Enter\n");
@@ -2112,28 +2279,9 @@ static int brcmf_sdio_txpkt(struct brcmf_sdio *bus, struct sk_buff_head *pktq,
 	ret = brcmf_sdiod_send_pkt(bus->sdiodev, pktq);
 	bus->sdcnt.f2txdata++;
 
-	if (ret < 0) {
-		/* On failure, abort the command and terminate the frame */
-		brcmf_dbg(INFO, "sdio error %d, abort command and terminate frame\n",
-			  ret);
-		bus->sdcnt.tx_sderrs++;
+	if (ret < 0)
+		brcmf_sdio_txfail(bus);
 
-		brcmf_sdiod_abort(bus->sdiodev, SDIO_FUNC_2);
-		brcmf_sdiod_regwb(bus->sdiodev, SBSDIO_FUNC1_FRAMECTRL,
-				  SFC_WF_TERM, NULL);
-		bus->sdcnt.f1regdata++;
-
-		for (i = 0; i < 3; i++) {
-			u8 hi, lo;
-			hi = brcmf_sdiod_regrb(bus->sdiodev,
-					       SBSDIO_FUNC1_WFRAMEBCHI, NULL);
-			lo = brcmf_sdiod_regrb(bus->sdiodev,
-					       SBSDIO_FUNC1_WFRAMEBCLO, NULL);
-			bus->sdcnt.f1regdata += 2;
-			if ((hi == 0) && (lo == 0))
-				break;
-		}
-	}
 	sdio_release_host(bus->sdiodev->func[1]);
 
 done:
@@ -2166,7 +2314,7 @@ static uint brcmf_sdio_sendfromq(struct brcmf_sdio *bus, uint maxframes)
 		__skb_queue_head_init(&pktq);
 		if (bus->txglom)
 			pkt_num = min_t(u8, bus->tx_max - bus->tx_seq,
-					brcmf_sdio_txglomsz);
+					bus->sdiodev->txglomsz);
 		pkt_num = min_t(u32, pkt_num,
 				brcmu_pktq_mlen(&bus->txq, ~bus->flowcontrol));
 		spin_lock_bh(&bus->txqlock);
@@ -2185,7 +2333,7 @@ static uint brcmf_sdio_sendfromq(struct brcmf_sdio *bus, uint maxframes)
 		cnt += i;
 
 		/* In poll mode, need to check for other events */
-		if (!bus->intr && cnt) {
+		if (!bus->intr) {
 			/* Check device status, signal pending interrupt */
 			sdio_claim_host(bus->sdiodev->func[1]);
 			ret = r_sdreg32(bus, &intstatus,
@@ -2291,21 +2439,29 @@ static inline void brcmf_sdio_clrintr(struct brcmf_sdio *bus)
 	}
 }
 
+static void atomic_orr(int val, atomic_t *v)
+{
+	int old_val;
+
+	old_val = atomic_read(v);
+	while (atomic_cmpxchg(v, old_val, val | old_val) != old_val)
+		old_val = atomic_read(v);
+}
+
 static int brcmf_sdio_intr_rstatus(struct brcmf_sdio *bus)
 {
-	u8 idx;
+	struct brcmf_core *buscore;
 	u32 addr;
 	unsigned long val;
-	int n, ret;
+	int ret;
 
-	idx = brcmf_sdio_chip_getinfidx(bus->ci, BCMA_CORE_SDIO_DEV);
-	addr = bus->ci->c_inf[idx].base +
-	       offsetof(struct sdpcmd_regs, intstatus);
+	buscore = brcmf_chip_get_core(bus->ci, BCMA_CORE_SDIO_DEV);
+	addr = buscore->base + offsetof(struct sdpcmd_regs, intstatus);
 
 	val = brcmf_sdiod_regrl(bus->sdiodev, addr, &ret);
 	bus->sdcnt.f1regdata++;
 	if (ret != 0)
-		val = 0;
+		return ret;
 
 	val &= bus->hostintmask;
 	atomic_set(&bus->fcstate, !!(val & I_HMB_FC_STATE));
@@ -2314,13 +2470,7 @@ static int brcmf_sdio_intr_rstatus(struct brcmf_sdio *bus)
 	if (val) {
 		brcmf_sdiod_regwl(bus->sdiodev, addr, val, &ret);
 		bus->sdcnt.f1regdata++;
-	}
-
-	if (ret) {
-		atomic_set(&bus->intstatus, 0);
-	} else if (val) {
-		for_each_set_bit(n, &val, 32)
-			set_bit(n, (unsigned long *)&bus->intstatus.counter);
+		atomic_orr(val, &bus->intstatus);
 	}
 
 	return ret;
@@ -2330,10 +2480,9 @@ static void brcmf_sdio_dpc(struct brcmf_sdio *bus)
 {
 	u32 newstatus = 0;
 	unsigned long intstatus;
-	uint rxlimit = bus->rxbound;	/* Rx frames to read before resched */
 	uint txlimit = bus->txbound;	/* Tx frames to send before resched */
-	uint framecnt = 0;	/* Temporary counter of tx/rx frames */
-	int err = 0, n;
+	uint framecnt;			/* Temporary counter of tx/rx frames */
+	int err = 0;
 
 	brcmf_dbg(TRACE, "Enter\n");
 
@@ -2430,58 +2579,30 @@ static void brcmf_sdio_dpc(struct brcmf_sdio *bus)
 		intstatus &= ~I_HMB_FRAME_IND;
 
 	/* On frame indication, read available frames */
-	if (PKT_AVAILABLE() && bus->clkstate == CLK_AVAIL) {
-		framecnt = brcmf_sdio_readframes(bus, rxlimit);
+	if ((intstatus & I_HMB_FRAME_IND) && (bus->clkstate == CLK_AVAIL)) {
+		brcmf_sdio_readframes(bus, bus->rxbound);
 		if (!bus->rxpending)
 			intstatus &= ~I_HMB_FRAME_IND;
-		rxlimit -= min(framecnt, rxlimit);
 	}
 
 	/* Keep still-pending events for next scheduling */
-	if (intstatus) {
-		for_each_set_bit(n, &intstatus, 32)
-			set_bit(n, (unsigned long *)&bus->intstatus.counter);
-	}
+	if (intstatus)
+		atomic_orr(intstatus, &bus->intstatus);
 
 	brcmf_sdio_clrintr(bus);
 
 	if (data_ok(bus) && bus->ctrl_frame_stat &&
-		(bus->clkstate == CLK_AVAIL)) {
-		int i;
+	    (bus->clkstate == CLK_AVAIL)) {
 
 		sdio_claim_host(bus->sdiodev->func[1]);
 		err = brcmf_sdiod_send_buf(bus->sdiodev, bus->ctrl_frame_buf,
 					   (u32)bus->ctrl_frame_len);
 
-		if (err < 0) {
-			/* On failure, abort the command and
-				terminate the frame */
-			brcmf_dbg(INFO, "sdio error %d, abort command and terminate frame\n",
-				  err);
-			bus->sdcnt.tx_sderrs++;
-
-			brcmf_sdiod_abort(bus->sdiodev, SDIO_FUNC_2);
-
-			brcmf_sdiod_regwb(bus->sdiodev, SBSDIO_FUNC1_FRAMECTRL,
-					  SFC_WF_TERM, &err);
-			bus->sdcnt.f1regdata++;
-
-			for (i = 0; i < 3; i++) {
-				u8 hi, lo;
-				hi = brcmf_sdiod_regrb(bus->sdiodev,
-						       SBSDIO_FUNC1_WFRAMEBCHI,
-						       &err);
-				lo = brcmf_sdiod_regrb(bus->sdiodev,
-						       SBSDIO_FUNC1_WFRAMEBCLO,
-						       &err);
-				bus->sdcnt.f1regdata += 2;
-				if ((hi == 0) && (lo == 0))
-					break;
-			}
-
-		} else {
+		if (err < 0)
+			brcmf_sdio_txfail(bus);
+		else
 			bus->tx_seq = (bus->tx_seq + 1) % SDPCM_SEQ_WRAP;
-		}
+
 		sdio_release_host(bus->sdiodev->func[1]);
 		bus->ctrl_frame_stat = false;
 		brcmf_sdio_wait_event_wakeup(bus);
@@ -2492,8 +2613,7 @@ static void brcmf_sdio_dpc(struct brcmf_sdio *bus)
 		 && data_ok(bus)) {
 		framecnt = bus->rxpending ? min(txlimit, bus->txminmax) :
 					    txlimit;
-		framecnt = brcmf_sdio_sendfromq(bus, framecnt);
-		txlimit -= framecnt;
+		brcmf_sdio_sendfromq(bus, framecnt);
 	}
 
 	if (!brcmf_bus_ready(bus->sdiodev->bus_if) || (err != 0)) {
@@ -2503,19 +2623,9 @@ static void brcmf_sdio_dpc(struct brcmf_sdio *bus)
 		   atomic_read(&bus->ipend) > 0 ||
 		   (!atomic_read(&bus->fcstate) &&
 		    brcmu_pktq_mlen(&bus->txq, ~bus->flowcontrol) &&
-		    data_ok(bus)) || PKT_AVAILABLE()) {
+		    data_ok(bus))) {
 		atomic_inc(&bus->dpc_tskcnt);
 	}
-
-	/* If we're done for now, turn off clock request. */
-	if ((bus->clkstate != CLK_PENDING)
-	    && bus->idletime == BRCMF_IDLE_IMMEDIATE) {
-		bus->activity = false;
-		brcmf_dbg(SDIO, "idle state\n");
-		sdio_claim_host(bus->sdiodev->func[1]);
-		brcmf_sdio_bus_sleep(bus, true, false);
-		sdio_release_host(bus->sdiodev->func[1]);
-	}
 }
 
 static struct pktq *brcmf_sdio_bus_gettxq(struct device *dev)
@@ -2530,15 +2640,13 @@ static struct pktq *brcmf_sdio_bus_gettxq(struct device *dev)
 static int brcmf_sdio_bus_txdata(struct device *dev, struct sk_buff *pkt)
 {
 	int ret = -EBADE;
-	uint datalen, prec;
+	uint prec;
 	struct brcmf_bus *bus_if = dev_get_drvdata(dev);
 	struct brcmf_sdio_dev *sdiodev = bus_if->bus_priv.sdio;
 	struct brcmf_sdio *bus = sdiodev->bus;
 	ulong flags;
 
-	brcmf_dbg(TRACE, "Enter\n");
-
-	datalen = pkt->len;
+	brcmf_dbg(TRACE, "Enter: pkt: data %p len %d\n", pkt->data, pkt->len);
 
 	/* Add space for the header */
 	skb_push(pkt, bus->tx_hdrlen);
@@ -2553,6 +2661,8 @@ static int brcmf_sdio_bus_txdata(struct device *dev, struct sk_buff *pkt)
 
 	/* Priority based enq */
 	spin_lock_irqsave(&bus->txqlock, flags);
+	/* reset bus_flags in packet cb */
+	*(u16 *)(pkt->cb) = 0;
 	if (!brcmf_c_prec_enq(bus->sdiodev->dev, &bus->txq, pkt, prec)) {
 		skb_pull(pkt, bus->tx_hdrlen);
 		brcmf_err("out of bus->txq !!!\n");
@@ -2662,38 +2772,15 @@ break2:
 
 static int brcmf_sdio_tx_frame(struct brcmf_sdio *bus, u8 *frame, u16 len)
 {
-	int i;
 	int ret;
 
 	bus->ctrl_frame_stat = false;
 	ret = brcmf_sdiod_send_buf(bus->sdiodev, frame, len);
 
-	if (ret < 0) {
-		/* On failure, abort the command and terminate the frame */
-		brcmf_dbg(INFO, "sdio error %d, abort command and terminate frame\n",
-			  ret);
-		bus->sdcnt.tx_sderrs++;
-
-		brcmf_sdiod_abort(bus->sdiodev, SDIO_FUNC_2);
-
-		brcmf_sdiod_regwb(bus->sdiodev, SBSDIO_FUNC1_FRAMECTRL,
-				  SFC_WF_TERM, NULL);
-		bus->sdcnt.f1regdata++;
-
-		for (i = 0; i < 3; i++) {
-			u8 hi, lo;
-			hi = brcmf_sdiod_regrb(bus->sdiodev,
-					       SBSDIO_FUNC1_WFRAMEBCHI, NULL);
-			lo = brcmf_sdiod_regrb(bus->sdiodev,
-					       SBSDIO_FUNC1_WFRAMEBCLO, NULL);
-			bus->sdcnt.f1regdata += 2;
-			if (hi == 0 && lo == 0)
-				break;
-		}
-		return ret;
-	}
-
-	bus->tx_seq = (bus->tx_seq + 1) % SDPCM_SEQ_WRAP;
+	if (ret < 0)
+		brcmf_sdio_txfail(bus);
+	else
+		bus->tx_seq = (bus->tx_seq + 1) % SDPCM_SEQ_WRAP;
 
 	return ret;
 }
@@ -2792,15 +2879,6 @@ brcmf_sdio_bus_txctl(struct device *dev, unsigned char *msg, uint msglen)
 		} while (ret < 0 && retries++ < TXRETRIES);
 	}
 
-	if ((bus->idletime == BRCMF_IDLE_IMMEDIATE) &&
-	    atomic_read(&bus->dpc_tskcnt) == 0) {
-		bus->activity = false;
-		sdio_claim_host(bus->sdiodev->func[1]);
-		brcmf_dbg(INFO, "idle\n");
-		brcmf_sdio_clkctl(bus, CLK_NONE, true);
-		sdio_release_host(bus->sdiodev->func[1]);
-	}
-
 	if (ret)
 		bus->sdcnt.tx_ctlerrs++;
 	else
@@ -2810,72 +2888,6 @@ brcmf_sdio_bus_txctl(struct device *dev, unsigned char *msg, uint msglen)
 }
 
 #ifdef DEBUG
-static inline bool brcmf_sdio_valid_shared_address(u32 addr)
-{
-	return !(addr == 0 || ((~addr >> 16) & 0xffff) == (addr & 0xffff));
-}
-
-static int brcmf_sdio_readshared(struct brcmf_sdio *bus,
-				 struct sdpcm_shared *sh)
-{
-	u32 addr;
-	int rv;
-	u32 shaddr = 0;
-	struct sdpcm_shared_le sh_le;
-	__le32 addr_le;
-
-	shaddr = bus->ci->rambase + bus->ramsize - 4;
-
-	/*
-	 * Read last word in socram to determine
-	 * address of sdpcm_shared structure
-	 */
-	sdio_claim_host(bus->sdiodev->func[1]);
-	brcmf_sdio_bus_sleep(bus, false, false);
-	rv = brcmf_sdiod_ramrw(bus->sdiodev, false, shaddr, (u8 *)&addr_le, 4);
-	sdio_release_host(bus->sdiodev->func[1]);
-	if (rv < 0)
-		return rv;
-
-	addr = le32_to_cpu(addr_le);
-
-	brcmf_dbg(SDIO, "sdpcm_shared address 0x%08X\n", addr);
-
-	/*
-	 * Check if addr is valid.
-	 * NVRAM length at the end of memory should have been overwritten.
-	 */
-	if (!brcmf_sdio_valid_shared_address(addr)) {
-			brcmf_err("invalid sdpcm_shared address 0x%08X\n",
-				  addr);
-			return -EINVAL;
-	}
-
-	/* Read hndrte_shared structure */
-	rv = brcmf_sdiod_ramrw(bus->sdiodev, false, addr, (u8 *)&sh_le,
-			       sizeof(struct sdpcm_shared_le));
-	if (rv < 0)
-		return rv;
-
-	/* Endianness */
-	sh->flags = le32_to_cpu(sh_le.flags);
-	sh->trap_addr = le32_to_cpu(sh_le.trap_addr);
-	sh->assert_exp_addr = le32_to_cpu(sh_le.assert_exp_addr);
-	sh->assert_file_addr = le32_to_cpu(sh_le.assert_file_addr);
-	sh->assert_line = le32_to_cpu(sh_le.assert_line);
-	sh->console_addr = le32_to_cpu(sh_le.console_addr);
-	sh->msgtrace_addr = le32_to_cpu(sh_le.msgtrace_addr);
-
-	if ((sh->flags & SDPCM_SHARED_VERSION_MASK) > SDPCM_SHARED_VERSION) {
-		brcmf_err("sdpcm shared version unsupported: dhd %d dongle %d\n",
-			  SDPCM_SHARED_VERSION,
-			  sh->flags & SDPCM_SHARED_VERSION_MASK);
-		return -EPROTO;
-	}
-
-	return 0;
-}
-
 static int brcmf_sdio_dump_console(struct brcmf_sdio *bus,
 				   struct sdpcm_shared *sh, char __user *data,
 				   size_t count)
@@ -3105,6 +3117,8 @@ static void brcmf_sdio_debugfs_create(struct brcmf_sdio *bus)
 	debugfs_create_file("forensics", S_IRUGO, dentry, bus,
 			    &brcmf_sdio_forensic_ops);
 	brcmf_debugfs_create_sdio_count(drvr, &bus->sdcnt);
+	debugfs_create_u32("console_interval", 0644, dentry,
+			   &bus->console_interval);
 }
 #else
 static int brcmf_sdio_checkdied(struct brcmf_sdio *bus)
@@ -3223,32 +3237,17 @@ static int brcmf_sdio_download_code_file(struct brcmf_sdio *bus,
 					 const struct firmware *fw)
 {
 	int err;
-	int offset;
-	int address;
-	int len;
 
 	brcmf_dbg(TRACE, "Enter\n");
 
-	err = 0;
-	offset = 0;
-	address = bus->ci->rambase;
-	while (offset < fw->size) {
-		len = ((offset + MEMBLOCK) < fw->size) ? MEMBLOCK :
-		      fw->size - offset;
-		err = brcmf_sdiod_ramrw(bus->sdiodev, true, address,
-					(u8 *)&fw->data[offset], len);
-		if (err) {
-			brcmf_err("error %d on writing %d membytes at 0x%08x\n",
-				  err, len, address);
-			return err;
-		}
-		offset += len;
-		address += len;
-	}
-	if (!err)
-		if (!brcmf_sdio_verifymemory(bus->sdiodev, bus->ci->rambase,
-					     (u8 *)fw->data, fw->size))
-			err = -EIO;
+	err = brcmf_sdiod_ramrw(bus->sdiodev, true, bus->ci->rambase,
+				(u8 *)fw->data, fw->size);
+	if (err)
+		brcmf_err("error %d on writing %d membytes at 0x%08x\n",
+			  err, (int)fw->size, bus->ci->rambase);
+	else if (!brcmf_sdio_verifymemory(bus->sdiodev, bus->ci->rambase,
+					  (u8 *)fw->data, fw->size))
+		err = -EIO;
 
 	return err;
 }
@@ -3291,7 +3290,7 @@ static int brcmf_sdio_download_firmware(struct brcmf_sdio *bus)
 	brcmf_sdio_clkctl(bus, CLK_AVAIL, false);
 
 	/* Keep arm in reset */
-	brcmf_sdio_chip_enter_download(bus->sdiodev, bus->ci);
+	brcmf_chip_enter_download(bus->ci);
 
 	fw = brcmf_sdio_get_fw(bus, BRCMF_FIRMWARE_BIN);
 	if (fw == NULL) {
@@ -3323,7 +3322,7 @@ static int brcmf_sdio_download_firmware(struct brcmf_sdio *bus)
 	}
 
 	/* Take arm out of reset */
-	if (!brcmf_sdio_chip_exit_download(bus->sdiodev, bus->ci, rstvec)) {
+	if (!brcmf_chip_exit_download(bus->ci, rstvec)) {
 		brcmf_err("error getting out of ARM core reset\n");
 		goto err;
 	}
@@ -3338,40 +3337,6 @@ err:
 	return bcmerror;
 }
 
-static bool brcmf_sdio_sr_capable(struct brcmf_sdio *bus)
-{
-	u32 addr, reg, pmu_cc3_mask = ~0;
-	int err;
-
-	brcmf_dbg(TRACE, "Enter\n");
-
-	/* old chips with PMU version less than 17 don't support save restore */
-	if (bus->ci->pmurev < 17)
-		return false;
-
-	switch (bus->ci->chip) {
-	case BCM43241_CHIP_ID:
-	case BCM4335_CHIP_ID:
-	case BCM4339_CHIP_ID:
-		/* read PMU chipcontrol register 3 */
-		addr = CORE_CC_REG(bus->ci->c_inf[0].base, chipcontrol_addr);
-		brcmf_sdiod_regwl(bus->sdiodev, addr, 3, NULL);
-		addr = CORE_CC_REG(bus->ci->c_inf[0].base, chipcontrol_data);
-		reg = brcmf_sdiod_regrl(bus->sdiodev, addr, NULL);
-		return (reg & pmu_cc3_mask) != 0;
-	default:
-		addr = CORE_CC_REG(bus->ci->c_inf[0].base, pmucapabilities_ext);
-		reg = brcmf_sdiod_regrl(bus->sdiodev, addr, &err);
-		if ((reg & PCAPEXT_SR_SUPPORTED_MASK) == 0)
-			return false;
-
-		addr = CORE_CC_REG(bus->ci->c_inf[0].base, retention_ctl);
-		reg = brcmf_sdiod_regrl(bus->sdiodev, addr, NULL);
-		return (reg & (PMU_RCTL_MACPHY_DISABLE_MASK |
-			       PMU_RCTL_LOGIC_DISABLE_MASK)) == 0;
-	}
-}
-
 static void brcmf_sdio_sr_init(struct brcmf_sdio *bus)
 {
 	int err = 0;
@@ -3423,7 +3388,7 @@ static int brcmf_sdio_kso_init(struct brcmf_sdio *bus)
 	brcmf_dbg(TRACE, "Enter\n");
 
 	/* KSO bit added in SDIO core rev 12 */
-	if (bus->ci->c_inf[1].rev < 12)
+	if (brcmf_chip_get_core(bus->ci, BCMA_CORE_SDIO_DEV)->rev < 12)
 		return 0;
 
 	val = brcmf_sdiod_regrb(bus->sdiodev, SBSDIO_FUNC1_SLEEPCSR, &err);
@@ -3454,15 +3419,13 @@ static int brcmf_sdio_bus_preinit(struct device *dev)
 	struct brcmf_sdio *bus = sdiodev->bus;
 	uint pad_size;
 	u32 value;
-	u8 idx;
 	int err;
 
 	/* the commands below use the terms tx and rx from
 	 * a device perspective, ie. bus:txglom affects the
 	 * bus transfers from device to host.
 	 */
-	idx = brcmf_sdio_chip_getinfidx(bus->ci, BCMA_CORE_SDIO_DEV);
-	if (bus->ci->c_inf[idx].rev < 12) {
+	if (brcmf_chip_get_core(bus->ci, BCMA_CORE_SDIO_DEV)->rev < 12) {
 		/* for sdio core rev < 12, disable txgloming */
 		value = 0;
 		err = brcmf_iovar_data_set(dev, "bus:txglom", &value,
@@ -3486,10 +3449,6 @@ static int brcmf_sdio_bus_preinit(struct device *dev)
 		bus->txglom = false;
 		value = 1;
 		pad_size = bus->sdiodev->func[2]->cur_blksize << 1;
-		bus->txglom_sgpad = brcmu_pkt_buf_get_skb(pad_size);
-		if (!bus->txglom_sgpad)
-			brcmf_err("allocating txglom padding skb failed, reduced performance\n");
-
 		err = brcmf_iovar_data_set(bus->sdiodev->dev, "bus:rxglom",
 					   &value, sizeof(u32));
 		if (err < 0) {
@@ -3573,7 +3532,7 @@ static int brcmf_sdio_bus_init(struct device *dev)
 		ret = -ENODEV;
 	}
 
-	if (brcmf_sdio_sr_capable(bus)) {
+	if (brcmf_chip_sr_capable(bus->ci)) {
 		brcmf_sdio_sr_init(bus);
 	} else {
 		/* Restore previous clock setting */
@@ -3717,11 +3676,175 @@ static void brcmf_sdio_dataworker(struct work_struct *work)
 					      datawork);
 
 	while (atomic_read(&bus->dpc_tskcnt)) {
+		atomic_set(&bus->dpc_tskcnt, 0);
 		brcmf_sdio_dpc(bus);
-		atomic_dec(&bus->dpc_tskcnt);
 	}
 }
 
+static void
+brcmf_sdio_drivestrengthinit(struct brcmf_sdio_dev *sdiodev,
+			     struct brcmf_chip *ci, u32 drivestrength)
+{
+	const struct sdiod_drive_str *str_tab = NULL;
+	u32 str_mask;
+	u32 str_shift;
+	u32 base;
+	u32 i;
+	u32 drivestrength_sel = 0;
+	u32 cc_data_temp;
+	u32 addr;
+
+	if (!(ci->cc_caps & CC_CAP_PMU))
+		return;
+
+	switch (SDIOD_DRVSTR_KEY(ci->chip, ci->pmurev)) {
+	case SDIOD_DRVSTR_KEY(BCM4330_CHIP_ID, 12):
+		str_tab = sdiod_drvstr_tab1_1v8;
+		str_mask = 0x00003800;
+		str_shift = 11;
+		break;
+	case SDIOD_DRVSTR_KEY(BCM4334_CHIP_ID, 17):
+		str_tab = sdiod_drvstr_tab6_1v8;
+		str_mask = 0x00001800;
+		str_shift = 11;
+		break;
+	case SDIOD_DRVSTR_KEY(BCM43143_CHIP_ID, 17):
+		/* note: 43143 does not support tristate */
+		i = ARRAY_SIZE(sdiod_drvstr_tab2_3v3) - 1;
+		if (drivestrength >= sdiod_drvstr_tab2_3v3[i].strength) {
+			str_tab = sdiod_drvstr_tab2_3v3;
+			str_mask = 0x00000007;
+			str_shift = 0;
+		} else
+			brcmf_err("Invalid SDIO Drive strength for chip %s, strength=%d\n",
+				  ci->name, drivestrength);
+		break;
+	case SDIOD_DRVSTR_KEY(BCM43362_CHIP_ID, 13):
+		str_tab = sdiod_drive_strength_tab5_1v8;
+		str_mask = 0x00003800;
+		str_shift = 11;
+		break;
+	default:
+		brcmf_err("No SDIO Drive strength init done for chip %s rev %d pmurev %d\n",
+			  ci->name, ci->chiprev, ci->pmurev);
+		break;
+	}
+
+	if (str_tab != NULL) {
+		for (i = 0; str_tab[i].strength != 0; i++) {
+			if (drivestrength >= str_tab[i].strength) {
+				drivestrength_sel = str_tab[i].sel;
+				break;
+			}
+		}
+		base = brcmf_chip_get_chipcommon(ci)->base;
+		addr = CORE_CC_REG(base, chipcontrol_addr);
+		brcmf_sdiod_regwl(sdiodev, addr, 1, NULL);
+		cc_data_temp = brcmf_sdiod_regrl(sdiodev, addr, NULL);
+		cc_data_temp &= ~str_mask;
+		drivestrength_sel <<= str_shift;
+		cc_data_temp |= drivestrength_sel;
+		brcmf_sdiod_regwl(sdiodev, addr, cc_data_temp, NULL);
+
+		brcmf_dbg(INFO, "SDIO: %d mA (req=%d mA) drive strength selected, set to 0x%08x\n",
+			  str_tab[i].strength, drivestrength, cc_data_temp);
+	}
+}
+
+static int brcmf_sdio_buscoreprep(void *ctx)
+{
+	struct brcmf_sdio_dev *sdiodev = ctx;
+	int err = 0;
+	u8 clkval, clkset;
+
+	/* Try forcing SDIO core to do ALPAvail request only */
+	clkset = SBSDIO_FORCE_HW_CLKREQ_OFF | SBSDIO_ALP_AVAIL_REQ;
+	brcmf_sdiod_regwb(sdiodev, SBSDIO_FUNC1_CHIPCLKCSR, clkset, &err);
+	if (err) {
+		brcmf_err("error writing for HT off\n");
+		return err;
+	}
+
+	/* If register supported, wait for ALPAvail and then force ALP */
+	/* This may take up to 15 milliseconds */
+	clkval = brcmf_sdiod_regrb(sdiodev,
+				   SBSDIO_FUNC1_CHIPCLKCSR, NULL);
+
+	if ((clkval & ~SBSDIO_AVBITS) != clkset) {
+		brcmf_err("ChipClkCSR access: wrote 0x%02x read 0x%02x\n",
+			  clkset, clkval);
+		return -EACCES;
+	}
+
+	SPINWAIT(((clkval = brcmf_sdiod_regrb(sdiodev,
+					      SBSDIO_FUNC1_CHIPCLKCSR, NULL)),
+			!SBSDIO_ALPAV(clkval)),
+			PMU_MAX_TRANSITION_DLY);
+	if (!SBSDIO_ALPAV(clkval)) {
+		brcmf_err("timeout on ALPAV wait, clkval 0x%02x\n",
+			  clkval);
+		return -EBUSY;
+	}
+
+	clkset = SBSDIO_FORCE_HW_CLKREQ_OFF | SBSDIO_FORCE_ALP;
+	brcmf_sdiod_regwb(sdiodev, SBSDIO_FUNC1_CHIPCLKCSR, clkset, &err);
+	udelay(65);
+
+	/* Also, disable the extra SDIO pull-ups */
+	brcmf_sdiod_regwb(sdiodev, SBSDIO_FUNC1_SDIOPULLUP, 0, NULL);
+
+	return 0;
+}
+
+static void brcmf_sdio_buscore_exitdl(void *ctx, struct brcmf_chip *chip,
+				      u32 rstvec)
+{
+	struct brcmf_sdio_dev *sdiodev = ctx;
+	struct brcmf_core *core;
+	u32 reg_addr;
+
+	/* clear all interrupts */
+	core = brcmf_chip_get_core(chip, BCMA_CORE_SDIO_DEV);
+	reg_addr = core->base + offsetof(struct sdpcmd_regs, intstatus);
+	brcmf_sdiod_regwl(sdiodev, reg_addr, 0xFFFFFFFF, NULL);
+
+	if (rstvec)
+		/* Write reset vector to address 0 */
+		brcmf_sdiod_ramrw(sdiodev, true, 0, (void *)&rstvec,
+				  sizeof(rstvec));
+}
+
+static u32 brcmf_sdio_buscore_read32(void *ctx, u32 addr)
+{
+	struct brcmf_sdio_dev *sdiodev = ctx;
+	u32 val, rev;
+
+	val = brcmf_sdiod_regrl(sdiodev, addr, NULL);
+	if (sdiodev->func[0]->device == SDIO_DEVICE_ID_BROADCOM_4335_4339 &&
+	    addr == CORE_CC_REG(SI_ENUM_BASE, chipid)) {
+		rev = (val & CID_REV_MASK) >> CID_REV_SHIFT;
+		if (rev >= 2) {
+			val &= ~CID_ID_MASK;
+			val |= BCM4339_CHIP_ID;
+		}
+	}
+	return val;
+}
+
+static void brcmf_sdio_buscore_write32(void *ctx, u32 addr, u32 val)
+{
+	struct brcmf_sdio_dev *sdiodev = ctx;
+
+	brcmf_sdiod_regwl(sdiodev, addr, val, NULL);
+}
+
+static const struct brcmf_buscore_ops brcmf_sdio_buscore_ops = {
+	.prepare = brcmf_sdio_buscoreprep,
+	.exit_dl = brcmf_sdio_buscore_exitdl,
+	.read32 = brcmf_sdio_buscore_read32,
+	.write32 = brcmf_sdio_buscore_write32,
+};
+
 static bool
 brcmf_sdio_probe_attach(struct brcmf_sdio *bus)
 {
@@ -3737,7 +3860,7 @@ brcmf_sdio_probe_attach(struct brcmf_sdio *bus)
 		 brcmf_sdiod_regrl(bus->sdiodev, SI_ENUM_BASE, NULL));
 
 	/*
-	 * Force PLL off until brcmf_sdio_chip_attach()
+	 * Force PLL off until brcmf_chip_attach()
 	 * programs PLL control regs
 	 */
 
@@ -3758,8 +3881,10 @@ brcmf_sdio_probe_attach(struct brcmf_sdio *bus)
 	 */
 	brcmf_bus_change_state(bus->sdiodev->bus_if, BRCMF_BUS_DOWN);
 
-	if (brcmf_sdio_chip_attach(bus->sdiodev, &bus->ci)) {
-		brcmf_err("brcmf_sdio_chip_attach failed!\n");
+	bus->ci = brcmf_chip_attach(bus->sdiodev, &brcmf_sdio_buscore_ops);
+	if (IS_ERR(bus->ci)) {
+		brcmf_err("brcmf_chip_attach failed!\n");
+		bus->ci = NULL;
 		goto fail;
 	}
 
@@ -3772,7 +3897,7 @@ brcmf_sdio_probe_attach(struct brcmf_sdio *bus)
 		drivestrength = bus->sdiodev->pdata->drive_strength;
 	else
 		drivestrength = DEFAULT_SDIO_DRIVE_STRENGTH;
-	brcmf_sdio_chip_drivestrengthinit(bus->sdiodev, bus->ci, drivestrength);
+	brcmf_sdio_drivestrengthinit(bus->sdiodev, bus->ci, drivestrength);
 
 	/* Get info on the SOCRAM cores... */
 	bus->ramsize = bus->ci->ramsize;
@@ -3795,24 +3920,18 @@ brcmf_sdio_probe_attach(struct brcmf_sdio *bus)
 		goto fail;
 
 	/* set PMUControl so a backplane reset does PMU state reload */
-	reg_addr = CORE_CC_REG(bus->ci->c_inf[0].base,
+	reg_addr = CORE_CC_REG(brcmf_chip_get_chipcommon(bus->ci)->base,
 			       pmucontrol);
-	reg_val = brcmf_sdiod_regrl(bus->sdiodev,
-				    reg_addr,
-				    &err);
+	reg_val = brcmf_sdiod_regrl(bus->sdiodev, reg_addr, &err);
 	if (err)
 		goto fail;
 
 	reg_val |= (BCMA_CC_PMU_CTL_RES_RELOAD << BCMA_CC_PMU_CTL_RES_SHIFT);
 
-	brcmf_sdiod_regwl(bus->sdiodev,
-			  reg_addr,
-			  reg_val,
-			  &err);
+	brcmf_sdiod_regwl(bus->sdiodev, reg_addr, reg_val, &err);
 	if (err)
 		goto fail;
 
-
 	sdio_release_host(bus->sdiodev->func[1]);
 
 	brcmu_pktq_init(&bus->txq, (PRIOMASK + 1), TXQLEN);
@@ -4027,14 +4146,14 @@ void brcmf_sdio_remove(struct brcmf_sdio *bus)
 		/* De-register interrupt handler */
 		brcmf_sdiod_intr_unregister(bus->sdiodev);
 
-		cancel_work_sync(&bus->datawork);
-		if (bus->brcmf_wq)
-			destroy_workqueue(bus->brcmf_wq);
-
 		if (bus->sdiodev->bus_if->drvr) {
 			brcmf_detach(bus->sdiodev->dev);
 		}
 
+		cancel_work_sync(&bus->datawork);
+		if (bus->brcmf_wq)
+			destroy_workqueue(bus->brcmf_wq);
+
 		if (bus->ci) {
 			if (bus->sdiodev->bus_if->state == BRCMF_BUS_DOWN) {
 				sdio_claim_host(bus->sdiodev->func[1]);
@@ -4045,15 +4164,13 @@ void brcmf_sdio_remove(struct brcmf_sdio *bus)
 				 * all necessary cores.
 				 */
 				msleep(20);
-				brcmf_sdio_chip_enter_download(bus->sdiodev,
-							       bus->ci);
+				brcmf_chip_enter_download(bus->ci);
 				brcmf_sdio_clkctl(bus, CLK_NONE, false);
 				sdio_release_host(bus->sdiodev->func[1]);
 			}
-			brcmf_sdio_chip_detach(&bus->ci);
+			brcmf_chip_detach(bus->ci);
 		}
 
-		brcmu_pkt_buf_free_skb(bus->txglom_sgpad);
 		kfree(bus->rxbuf);
 		kfree(bus->hdrbuf);
 		kfree(bus);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/fwil.c b/drivers/net/wireless/brcm80211/brcmfmac/fwil.c
index 22adbe311d20..59a5af5bf994 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/fwil.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/fwil.c
@@ -124,7 +124,8 @@ brcmf_fil_cmd_int_get(struct brcmf_if *ifp, u32 cmd, u32 *data)
 }
 
 static u32
-brcmf_create_iovar(char *name, char *data, u32 datalen, char *buf, u32 buflen)
+brcmf_create_iovar(char *name, const char *data, u32 datalen,
+		   char *buf, u32 buflen)
 {
 	u32 len;
 
@@ -144,7 +145,7 @@ brcmf_create_iovar(char *name, char *data, u32 datalen, char *buf, u32 buflen)
 
 
 s32
-brcmf_fil_iovar_data_set(struct brcmf_if *ifp, char *name, void *data,
+brcmf_fil_iovar_data_set(struct brcmf_if *ifp, char *name, const void *data,
 			 u32 len)
 {
 	struct brcmf_pub *drvr = ifp->drvr;
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/fwil.h b/drivers/net/wireless/brcm80211/brcmfmac/fwil.h
index 77eae86e55c2..a30be683f4a1 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/fwil.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/fwil.h
@@ -83,7 +83,7 @@ s32 brcmf_fil_cmd_data_get(struct brcmf_if *ifp, u32 cmd, void *data, u32 len);
 s32 brcmf_fil_cmd_int_set(struct brcmf_if *ifp, u32 cmd, u32 data);
 s32 brcmf_fil_cmd_int_get(struct brcmf_if *ifp, u32 cmd, u32 *data);
 
-s32 brcmf_fil_iovar_data_set(struct brcmf_if *ifp, char *name, void *data,
+s32 brcmf_fil_iovar_data_set(struct brcmf_if *ifp, char *name, const void *data,
 			     u32 len);
 s32 brcmf_fil_iovar_data_get(struct brcmf_if *ifp, char *name, void *data,
 			     u32 len);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.c b/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.c
deleted file mode 100644
index 82bf3c5d3cdc..000000000000
--- a/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.c
+++ /dev/null
@@ -1,972 +0,0 @@
-/*
- * Copyright (c) 2011 Broadcom Corporation
- *
- * Permission to use, copy, modify, and/or distribute this software for any
- * purpose with or without fee is hereby granted, provided that the above
- * copyright notice and this permission notice appear in all copies.
- *
- * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
- * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
- * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
- * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
- * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
- * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
- * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
- */
-/* ***** SDIO interface chip backplane handle functions ***** */
-
-#include <linux/types.h>
-#include <linux/netdevice.h>
-#include <linux/mmc/card.h>
-#include <linux/mmc/sdio_func.h>
-#include <linux/mmc/sdio_ids.h>
-#include <linux/ssb/ssb_regs.h>
-#include <linux/bcma/bcma.h>
-
-#include <chipcommon.h>
-#include <brcm_hw_ids.h>
-#include <brcmu_wifi.h>
-#include <brcmu_utils.h>
-#include <soc.h>
-#include "dhd_dbg.h"
-#include "sdio_host.h"
-#include "sdio_chip.h"
-
-/* chip core base & ramsize */
-/* bcm4329 */
-/* SDIO device core, ID 0x829 */
-#define BCM4329_CORE_BUS_BASE		0x18011000
-/* internal memory core, ID 0x80e */
-#define BCM4329_CORE_SOCRAM_BASE	0x18003000
-/* ARM Cortex M3 core, ID 0x82a */
-#define BCM4329_CORE_ARM_BASE		0x18002000
-#define BCM4329_RAMSIZE			0x48000
-
-/* bcm43143 */
-/* SDIO device core */
-#define BCM43143_CORE_BUS_BASE		0x18002000
-/* internal memory core */
-#define BCM43143_CORE_SOCRAM_BASE	0x18004000
-/* ARM Cortex M3 core, ID 0x82a */
-#define BCM43143_CORE_ARM_BASE		0x18003000
-#define BCM43143_RAMSIZE		0x70000
-
-/* All D11 cores, ID 0x812 */
-#define BCM43xx_CORE_D11_BASE		0x18001000
-
-#define	SBCOREREV(sbidh) \
-	((((sbidh) & SSB_IDHIGH_RCHI) >> SSB_IDHIGH_RCHI_SHIFT) | \
-	  ((sbidh) & SSB_IDHIGH_RCLO))
-
-/* SOC Interconnect types (aka chip types) */
-#define SOCI_SB		0
-#define SOCI_AI		1
-
-/* EROM CompIdentB */
-#define CIB_REV_MASK		0xff000000
-#define CIB_REV_SHIFT		24
-
-/* ARM CR4 core specific control flag bits */
-#define ARMCR4_BCMA_IOCTL_CPUHALT	0x0020
-
-/* D11 core specific control flag bits */
-#define D11_BCMA_IOCTL_PHYCLOCKEN	0x0004
-#define D11_BCMA_IOCTL_PHYRESET		0x0008
-
-#define SDIOD_DRVSTR_KEY(chip, pmu)     (((chip) << 16) | (pmu))
-/* SDIO Pad drive strength to select value mappings */
-struct sdiod_drive_str {
-	u8 strength;	/* Pad Drive Strength in mA */
-	u8 sel;		/* Chip-specific select value */
-};
-/* SDIO Drive Strength to sel value table for PMU Rev 11 (1.8V) */
-static const struct sdiod_drive_str sdiod_drvstr_tab1_1v8[] = {
-	{32, 0x6},
-	{26, 0x7},
-	{22, 0x4},
-	{16, 0x5},
-	{12, 0x2},
-	{8, 0x3},
-	{4, 0x0},
-	{0, 0x1}
-};
-
-/* SDIO Drive Strength to sel value table for PMU Rev 13 (1.8v) */
-static const struct sdiod_drive_str sdiod_drive_strength_tab5_1v8[] = {
-        {6, 0x7},
-        {5, 0x6},
-        {4, 0x5},
-        {3, 0x4},
-        {2, 0x2},
-        {1, 0x1},
-        {0, 0x0}
-};
-
-/* SDIO Drive Strength to sel value table for PMU Rev 17 (1.8v) */
-static const struct sdiod_drive_str sdiod_drvstr_tab6_1v8[] = {
-	{3, 0x3},
-	{2, 0x2},
-	{1, 0x1},
-	{0, 0x0} };
-
-/* SDIO Drive Strength to sel value table for 43143 PMU Rev 17 (3.3V) */
-static const struct sdiod_drive_str sdiod_drvstr_tab2_3v3[] = {
-	{16, 0x7},
-	{12, 0x5},
-	{8,  0x3},
-	{4,  0x1}
-};
-
-u8
-brcmf_sdio_chip_getinfidx(struct brcmf_chip *ci, u16 coreid)
-{
-	u8 idx;
-
-	for (idx = 0; idx < BRCMF_MAX_CORENUM; idx++)
-		if (coreid == ci->c_inf[idx].id)
-			return idx;
-
-	return BRCMF_MAX_CORENUM;
-}
-
-static u32
-brcmf_sdio_sb_corerev(struct brcmf_sdio_dev *sdiodev,
-		      struct brcmf_chip *ci, u16 coreid)
-{
-	u32 regdata;
-	u8 idx;
-
-	idx = brcmf_sdio_chip_getinfidx(ci, coreid);
-
-	regdata = brcmf_sdiod_regrl(sdiodev,
-				    CORE_SB(ci->c_inf[idx].base, sbidhigh),
-				    NULL);
-	return SBCOREREV(regdata);
-}
-
-static u32
-brcmf_sdio_ai_corerev(struct brcmf_sdio_dev *sdiodev,
-		      struct brcmf_chip *ci, u16 coreid)
-{
-	u8 idx;
-
-	idx = brcmf_sdio_chip_getinfidx(ci, coreid);
-
-	return (ci->c_inf[idx].cib & CIB_REV_MASK) >> CIB_REV_SHIFT;
-}
-
-static bool
-brcmf_sdio_sb_iscoreup(struct brcmf_sdio_dev *sdiodev,
-		       struct brcmf_chip *ci, u16 coreid)
-{
-	u32 regdata;
-	u8 idx;
-
-	idx = brcmf_sdio_chip_getinfidx(ci, coreid);
-	if (idx == BRCMF_MAX_CORENUM)
-		return false;
-
-	regdata = brcmf_sdiod_regrl(sdiodev,
-				    CORE_SB(ci->c_inf[idx].base, sbtmstatelow),
-				    NULL);
-	regdata &= (SSB_TMSLOW_RESET | SSB_TMSLOW_REJECT |
-		    SSB_IMSTATE_REJECT | SSB_TMSLOW_CLOCK);
-	return (SSB_TMSLOW_CLOCK == regdata);
-}
-
-static bool
-brcmf_sdio_ai_iscoreup(struct brcmf_sdio_dev *sdiodev,
-		       struct brcmf_chip *ci, u16 coreid)
-{
-	u32 regdata;
-	u8 idx;
-	bool ret;
-
-	idx = brcmf_sdio_chip_getinfidx(ci, coreid);
-	if (idx == BRCMF_MAX_CORENUM)
-		return false;
-
-	regdata = brcmf_sdiod_regrl(sdiodev, ci->c_inf[idx].wrapbase+BCMA_IOCTL,
-				    NULL);
-	ret = (regdata & (BCMA_IOCTL_FGC | BCMA_IOCTL_CLK)) == BCMA_IOCTL_CLK;
-
-	regdata = brcmf_sdiod_regrl(sdiodev,
-				    ci->c_inf[idx].wrapbase+BCMA_RESET_CTL,
-				    NULL);
-	ret = ret && ((regdata & BCMA_RESET_CTL_RESET) == 0);
-
-	return ret;
-}
-
-static void
-brcmf_sdio_sb_coredisable(struct brcmf_sdio_dev *sdiodev,
-			  struct brcmf_chip *ci, u16 coreid, u32 pre_resetbits,
-			  u32 in_resetbits)
-{
-	u32 regdata, base;
-	u8 idx;
-
-	idx = brcmf_sdio_chip_getinfidx(ci, coreid);
-	base = ci->c_inf[idx].base;
-
-	regdata = brcmf_sdiod_regrl(sdiodev, CORE_SB(base, sbtmstatelow), NULL);
-	if (regdata & SSB_TMSLOW_RESET)
-		return;
-
-	regdata = brcmf_sdiod_regrl(sdiodev, CORE_SB(base, sbtmstatelow), NULL);
-	if ((regdata & SSB_TMSLOW_CLOCK) != 0) {
-		/*
-		 * set target reject and spin until busy is clear
-		 * (preserve core-specific bits)
-		 */
-		regdata = brcmf_sdiod_regrl(sdiodev,
-					    CORE_SB(base, sbtmstatelow), NULL);
-		brcmf_sdiod_regwl(sdiodev, CORE_SB(base, sbtmstatelow),
-				  regdata | SSB_TMSLOW_REJECT, NULL);
-
-		regdata = brcmf_sdiod_regrl(sdiodev,
-					    CORE_SB(base, sbtmstatelow), NULL);
-		udelay(1);
-		SPINWAIT((brcmf_sdiod_regrl(sdiodev,
-					    CORE_SB(base, sbtmstatehigh),
-					    NULL) &
-			  SSB_TMSHIGH_BUSY), 100000);
-
-		regdata = brcmf_sdiod_regrl(sdiodev,
-					    CORE_SB(base, sbtmstatehigh),
-					    NULL);
-		if (regdata & SSB_TMSHIGH_BUSY)
-			brcmf_err("core state still busy\n");
-
-		regdata = brcmf_sdiod_regrl(sdiodev, CORE_SB(base, sbidlow),
-					    NULL);
-		if (regdata & SSB_IDLOW_INITIATOR) {
-			regdata = brcmf_sdiod_regrl(sdiodev,
-						    CORE_SB(base, sbimstate),
-						    NULL);
-			regdata |= SSB_IMSTATE_REJECT;
-			brcmf_sdiod_regwl(sdiodev, CORE_SB(base, sbimstate),
-					  regdata, NULL);
-			regdata = brcmf_sdiod_regrl(sdiodev,
-						    CORE_SB(base, sbimstate),
-						    NULL);
-			udelay(1);
-			SPINWAIT((brcmf_sdiod_regrl(sdiodev,
-						    CORE_SB(base, sbimstate),
-						    NULL) &
-				  SSB_IMSTATE_BUSY), 100000);
-		}
-
-		/* set reset and reject while enabling the clocks */
-		regdata = SSB_TMSLOW_FGC | SSB_TMSLOW_CLOCK |
-			  SSB_TMSLOW_REJECT | SSB_TMSLOW_RESET;
-		brcmf_sdiod_regwl(sdiodev, CORE_SB(base, sbtmstatelow),
-				  regdata, NULL);
-		regdata = brcmf_sdiod_regrl(sdiodev,
-					    CORE_SB(base, sbtmstatelow), NULL);
-		udelay(10);
-
-		/* clear the initiator reject bit */
-		regdata = brcmf_sdiod_regrl(sdiodev, CORE_SB(base, sbidlow),
-					    NULL);
-		if (regdata & SSB_IDLOW_INITIATOR) {
-			regdata = brcmf_sdiod_regrl(sdiodev,
-						    CORE_SB(base, sbimstate),
-						    NULL);
-			regdata &= ~SSB_IMSTATE_REJECT;
-			brcmf_sdiod_regwl(sdiodev, CORE_SB(base, sbimstate),
-					  regdata, NULL);
-		}
-	}
-
-	/* leave reset and reject asserted */
-	brcmf_sdiod_regwl(sdiodev, CORE_SB(base, sbtmstatelow),
-			  (SSB_TMSLOW_REJECT | SSB_TMSLOW_RESET), NULL);
-	udelay(1);
-}
-
-static void
-brcmf_sdio_ai_coredisable(struct brcmf_sdio_dev *sdiodev,
-			  struct brcmf_chip *ci, u16 coreid, u32 pre_resetbits,
-			  u32 in_resetbits)
-{
-	u8 idx;
-	u32 regdata;
-	u32 wrapbase;
-
-	idx = brcmf_sdio_chip_getinfidx(ci, coreid);
-	if (idx == BRCMF_MAX_CORENUM)
-		return;
-
-	wrapbase = ci->c_inf[idx].wrapbase;
-
-	/* if core is already in reset, just return */
-	regdata = brcmf_sdiod_regrl(sdiodev, wrapbase + BCMA_RESET_CTL, NULL);
-	if ((regdata & BCMA_RESET_CTL_RESET) != 0)
-		return;
-
-	/* configure reset */
-	brcmf_sdiod_regwl(sdiodev, wrapbase + BCMA_IOCTL, pre_resetbits |
-			  BCMA_IOCTL_FGC | BCMA_IOCTL_CLK, NULL);
-	regdata = brcmf_sdiod_regrl(sdiodev, wrapbase + BCMA_IOCTL, NULL);
-
-	/* put in reset */
-	brcmf_sdiod_regwl(sdiodev, wrapbase + BCMA_RESET_CTL,
-			  BCMA_RESET_CTL_RESET, NULL);
-	usleep_range(10, 20);
-
-	/* wait till reset is 1 */
-	SPINWAIT(brcmf_sdiod_regrl(sdiodev, wrapbase + BCMA_RESET_CTL, NULL) !=
-		 BCMA_RESET_CTL_RESET, 300);
-
-	/* post reset configure */
-	brcmf_sdiod_regwl(sdiodev, wrapbase + BCMA_IOCTL, pre_resetbits |
-			  BCMA_IOCTL_FGC | BCMA_IOCTL_CLK, NULL);
-	regdata = brcmf_sdiod_regrl(sdiodev, wrapbase + BCMA_IOCTL, NULL);
-}
-
-static void
-brcmf_sdio_sb_resetcore(struct brcmf_sdio_dev *sdiodev,
-			struct brcmf_chip *ci, u16 coreid,  u32 pre_resetbits,
-			u32 in_resetbits, u32 post_resetbits)
-{
-	u32 regdata;
-	u8 idx;
-
-	idx = brcmf_sdio_chip_getinfidx(ci, coreid);
-	if (idx == BRCMF_MAX_CORENUM)
-		return;
-
-	/*
-	 * Must do the disable sequence first to work for
-	 * arbitrary current core state.
-	 */
-	brcmf_sdio_sb_coredisable(sdiodev, ci, coreid, pre_resetbits,
-				  in_resetbits);
-
-	/*
-	 * Now do the initialization sequence.
-	 * set reset while enabling the clock and
-	 * forcing them on throughout the core
-	 */
-	brcmf_sdiod_regwl(sdiodev,
-			  CORE_SB(ci->c_inf[idx].base, sbtmstatelow),
-			  SSB_TMSLOW_FGC | SSB_TMSLOW_CLOCK | SSB_TMSLOW_RESET,
-			  NULL);
-	regdata = brcmf_sdiod_regrl(sdiodev,
-				    CORE_SB(ci->c_inf[idx].base, sbtmstatelow),
-				    NULL);
-	udelay(1);
-
-	/* clear any serror */
-	regdata = brcmf_sdiod_regrl(sdiodev,
-				    CORE_SB(ci->c_inf[idx].base, sbtmstatehigh),
-				    NULL);
-	if (regdata & SSB_TMSHIGH_SERR)
-		brcmf_sdiod_regwl(sdiodev,
-				  CORE_SB(ci->c_inf[idx].base, sbtmstatehigh),
-				  0, NULL);
-
-	regdata = brcmf_sdiod_regrl(sdiodev,
-				    CORE_SB(ci->c_inf[idx].base, sbimstate),
-				    NULL);
-	if (regdata & (SSB_IMSTATE_IBE | SSB_IMSTATE_TO))
-		brcmf_sdiod_regwl(sdiodev,
-				  CORE_SB(ci->c_inf[idx].base, sbimstate),
-				  regdata & ~(SSB_IMSTATE_IBE | SSB_IMSTATE_TO),
-				  NULL);
-
-	/* clear reset and allow it to propagate throughout the core */
-	brcmf_sdiod_regwl(sdiodev, CORE_SB(ci->c_inf[idx].base, sbtmstatelow),
-			  SSB_TMSLOW_FGC | SSB_TMSLOW_CLOCK, NULL);
-	regdata = brcmf_sdiod_regrl(sdiodev,
-				    CORE_SB(ci->c_inf[idx].base, sbtmstatelow),
-				    NULL);
-	udelay(1);
-
-	/* leave clock enabled */
-	brcmf_sdiod_regwl(sdiodev, CORE_SB(ci->c_inf[idx].base, sbtmstatelow),
-			  SSB_TMSLOW_CLOCK, NULL);
-	regdata = brcmf_sdiod_regrl(sdiodev,
-				    CORE_SB(ci->c_inf[idx].base, sbtmstatelow),
-				    NULL);
-	udelay(1);
-}
-
-static void
-brcmf_sdio_ai_resetcore(struct brcmf_sdio_dev *sdiodev,
-			struct brcmf_chip *ci, u16 coreid, u32 pre_resetbits,
-			u32 in_resetbits, u32 post_resetbits)
-{
-	u8 idx;
-	u32 regdata;
-	u32 wrapbase;
-
-	idx = brcmf_sdio_chip_getinfidx(ci, coreid);
-	if (idx == BRCMF_MAX_CORENUM)
-		return;
-
-	wrapbase = ci->c_inf[idx].wrapbase;
-
-	/* must disable first to work for arbitrary current core state */
-	brcmf_sdio_ai_coredisable(sdiodev, ci, coreid, pre_resetbits,
-				  in_resetbits);
-
-	while (brcmf_sdiod_regrl(sdiodev, wrapbase + BCMA_RESET_CTL, NULL) &
-	       BCMA_RESET_CTL_RESET) {
-		brcmf_sdiod_regwl(sdiodev, wrapbase + BCMA_RESET_CTL, 0, NULL);
-		usleep_range(40, 60);
-	}
-
-	brcmf_sdiod_regwl(sdiodev, wrapbase + BCMA_IOCTL, post_resetbits |
-			  BCMA_IOCTL_CLK, NULL);
-	regdata = brcmf_sdiod_regrl(sdiodev, wrapbase + BCMA_IOCTL, NULL);
-}
-
-#ifdef DEBUG
-/* safety check for chipinfo */
-static int brcmf_sdio_chip_cichk(struct brcmf_chip *ci)
-{
-	u8 core_idx;
-
-	/* check RAM core presence for ARM CM3 core */
-	core_idx = brcmf_sdio_chip_getinfidx(ci, BCMA_CORE_ARM_CM3);
-	if (BRCMF_MAX_CORENUM != core_idx) {
-		core_idx = brcmf_sdio_chip_getinfidx(ci,
-						     BCMA_CORE_INTERNAL_MEM);
-		if (BRCMF_MAX_CORENUM == core_idx) {
-			brcmf_err("RAM core not provided with ARM CM3 core\n");
-			return -ENODEV;
-		}
-	}
-
-	/* check RAM base for ARM CR4 core */
-	core_idx = brcmf_sdio_chip_getinfidx(ci, BCMA_CORE_ARM_CR4);
-	if (BRCMF_MAX_CORENUM != core_idx) {
-		if (ci->rambase == 0) {
-			brcmf_err("RAM base not provided with ARM CR4 core\n");
-			return -ENOMEM;
-		}
-	}
-
-	return 0;
-}
-#else	/* DEBUG */
-static inline int brcmf_sdio_chip_cichk(struct brcmf_chip *ci)
-{
-	return 0;
-}
-#endif
-
-static int brcmf_sdio_chip_recognition(struct brcmf_sdio_dev *sdiodev,
-				       struct brcmf_chip *ci)
-{
-	u32 regdata;
-	u32 socitype;
-
-	/* Get CC core rev
-	 * Chipid is assume to be at offset 0 from SI_ENUM_BASE
-	 * For different chiptypes or old sdio hosts w/o chipcommon,
-	 * other ways of recognition should be added here.
-	 */
-	regdata = brcmf_sdiod_regrl(sdiodev,
-				    CORE_CC_REG(SI_ENUM_BASE, chipid),
-				    NULL);
-	ci->chip = regdata & CID_ID_MASK;
-	ci->chiprev = (regdata & CID_REV_MASK) >> CID_REV_SHIFT;
-	if (sdiodev->func[0]->device == SDIO_DEVICE_ID_BROADCOM_4335_4339 &&
-	    ci->chiprev >= 2)
-		ci->chip = BCM4339_CHIP_ID;
-	socitype = (regdata & CID_TYPE_MASK) >> CID_TYPE_SHIFT;
-
-	brcmf_dbg(INFO, "found %s chip: id=0x%x, rev=%d\n",
-		  socitype == SOCI_SB ? "SB" : "AXI", ci->chip, ci->chiprev);
-
-	if (socitype == SOCI_SB) {
-		if (ci->chip != BCM4329_CHIP_ID) {
-			brcmf_err("SB chip is not supported\n");
-			return -ENODEV;
-		}
-		ci->iscoreup = brcmf_sdio_sb_iscoreup;
-		ci->corerev = brcmf_sdio_sb_corerev;
-		ci->coredisable = brcmf_sdio_sb_coredisable;
-		ci->resetcore = brcmf_sdio_sb_resetcore;
-
-		ci->c_inf[0].id = BCMA_CORE_CHIPCOMMON;
-		ci->c_inf[0].base = SI_ENUM_BASE;
-		ci->c_inf[1].id = BCMA_CORE_SDIO_DEV;
-		ci->c_inf[1].base = BCM4329_CORE_BUS_BASE;
-		ci->c_inf[2].id = BCMA_CORE_INTERNAL_MEM;
-		ci->c_inf[2].base = BCM4329_CORE_SOCRAM_BASE;
-		ci->c_inf[3].id = BCMA_CORE_ARM_CM3;
-		ci->c_inf[3].base = BCM4329_CORE_ARM_BASE;
-		ci->c_inf[4].id = BCMA_CORE_80211;
-		ci->c_inf[4].base = BCM43xx_CORE_D11_BASE;
-		ci->ramsize = BCM4329_RAMSIZE;
-	} else if (socitype == SOCI_AI) {
-		ci->iscoreup = brcmf_sdio_ai_iscoreup;
-		ci->corerev = brcmf_sdio_ai_corerev;
-		ci->coredisable = brcmf_sdio_ai_coredisable;
-		ci->resetcore = brcmf_sdio_ai_resetcore;
-
-		ci->c_inf[0].id = BCMA_CORE_CHIPCOMMON;
-		ci->c_inf[0].base = SI_ENUM_BASE;
-
-		/* Address of cores for new chips should be added here */
-		switch (ci->chip) {
-		case BCM43143_CHIP_ID:
-			ci->c_inf[0].wrapbase = ci->c_inf[0].base + 0x00100000;
-			ci->c_inf[0].cib = 0x2b000000;
-			ci->c_inf[1].id = BCMA_CORE_SDIO_DEV;
-			ci->c_inf[1].base = BCM43143_CORE_BUS_BASE;
-			ci->c_inf[1].wrapbase = ci->c_inf[1].base + 0x00100000;
-			ci->c_inf[1].cib = 0x18000000;
-			ci->c_inf[2].id = BCMA_CORE_INTERNAL_MEM;
-			ci->c_inf[2].base = BCM43143_CORE_SOCRAM_BASE;
-			ci->c_inf[2].wrapbase = ci->c_inf[2].base + 0x00100000;
-			ci->c_inf[2].cib = 0x14000000;
-			ci->c_inf[3].id = BCMA_CORE_ARM_CM3;
-			ci->c_inf[3].base = BCM43143_CORE_ARM_BASE;
-			ci->c_inf[3].wrapbase = ci->c_inf[3].base + 0x00100000;
-			ci->c_inf[3].cib = 0x07000000;
-			ci->c_inf[4].id = BCMA_CORE_80211;
-			ci->c_inf[4].base = BCM43xx_CORE_D11_BASE;
-			ci->c_inf[4].wrapbase = ci->c_inf[4].base + 0x00100000;
-			ci->ramsize = BCM43143_RAMSIZE;
-			break;
-		case BCM43241_CHIP_ID:
-			ci->c_inf[0].wrapbase = 0x18100000;
-			ci->c_inf[0].cib = 0x2a084411;
-			ci->c_inf[1].id = BCMA_CORE_SDIO_DEV;
-			ci->c_inf[1].base = 0x18002000;
-			ci->c_inf[1].wrapbase = 0x18102000;
-			ci->c_inf[1].cib = 0x0e004211;
-			ci->c_inf[2].id = BCMA_CORE_INTERNAL_MEM;
-			ci->c_inf[2].base = 0x18004000;
-			ci->c_inf[2].wrapbase = 0x18104000;
-			ci->c_inf[2].cib = 0x14080401;
-			ci->c_inf[3].id = BCMA_CORE_ARM_CM3;
-			ci->c_inf[3].base = 0x18003000;
-			ci->c_inf[3].wrapbase = 0x18103000;
-			ci->c_inf[3].cib = 0x07004211;
-			ci->c_inf[4].id = BCMA_CORE_80211;
-			ci->c_inf[4].base = BCM43xx_CORE_D11_BASE;
-			ci->c_inf[4].wrapbase = ci->c_inf[4].base + 0x00100000;
-			ci->ramsize = 0x90000;
-			break;
-		case BCM4330_CHIP_ID:
-			ci->c_inf[0].wrapbase = 0x18100000;
-			ci->c_inf[0].cib = 0x27004211;
-			ci->c_inf[1].id = BCMA_CORE_SDIO_DEV;
-			ci->c_inf[1].base = 0x18002000;
-			ci->c_inf[1].wrapbase = 0x18102000;
-			ci->c_inf[1].cib = 0x07004211;
-			ci->c_inf[2].id = BCMA_CORE_INTERNAL_MEM;
-			ci->c_inf[2].base = 0x18004000;
-			ci->c_inf[2].wrapbase = 0x18104000;
-			ci->c_inf[2].cib = 0x0d080401;
-			ci->c_inf[3].id = BCMA_CORE_ARM_CM3;
-			ci->c_inf[3].base = 0x18003000;
-			ci->c_inf[3].wrapbase = 0x18103000;
-			ci->c_inf[3].cib = 0x03004211;
-			ci->c_inf[4].id = BCMA_CORE_80211;
-			ci->c_inf[4].base = BCM43xx_CORE_D11_BASE;
-			ci->c_inf[4].wrapbase = ci->c_inf[4].base + 0x00100000;
-			ci->ramsize = 0x48000;
-			break;
-		case BCM4334_CHIP_ID:
-			ci->c_inf[0].wrapbase = 0x18100000;
-			ci->c_inf[0].cib = 0x29004211;
-			ci->c_inf[1].id = BCMA_CORE_SDIO_DEV;
-			ci->c_inf[1].base = 0x18002000;
-			ci->c_inf[1].wrapbase = 0x18102000;
-			ci->c_inf[1].cib = 0x0d004211;
-			ci->c_inf[2].id = BCMA_CORE_INTERNAL_MEM;
-			ci->c_inf[2].base = 0x18004000;
-			ci->c_inf[2].wrapbase = 0x18104000;
-			ci->c_inf[2].cib = 0x13080401;
-			ci->c_inf[3].id = BCMA_CORE_ARM_CM3;
-			ci->c_inf[3].base = 0x18003000;
-			ci->c_inf[3].wrapbase = 0x18103000;
-			ci->c_inf[3].cib = 0x07004211;
-			ci->c_inf[4].id = BCMA_CORE_80211;
-			ci->c_inf[4].base = BCM43xx_CORE_D11_BASE;
-			ci->c_inf[4].wrapbase = ci->c_inf[4].base + 0x00100000;
-			ci->ramsize = 0x80000;
-			break;
-		case BCM4335_CHIP_ID:
-			ci->c_inf[0].wrapbase = 0x18100000;
-			ci->c_inf[0].cib = 0x2b084411;
-			ci->c_inf[1].id = BCMA_CORE_SDIO_DEV;
-			ci->c_inf[1].base = 0x18005000;
-			ci->c_inf[1].wrapbase = 0x18105000;
-			ci->c_inf[1].cib = 0x0f004211;
-			ci->c_inf[2].id = BCMA_CORE_ARM_CR4;
-			ci->c_inf[2].base = 0x18002000;
-			ci->c_inf[2].wrapbase = 0x18102000;
-			ci->c_inf[2].cib = 0x01084411;
-			ci->c_inf[3].id = BCMA_CORE_80211;
-			ci->c_inf[3].base = BCM43xx_CORE_D11_BASE;
-			ci->c_inf[3].wrapbase = ci->c_inf[3].base + 0x00100000;
-			ci->ramsize = 0xc0000;
-			ci->rambase = 0x180000;
-			break;
-		case BCM43362_CHIP_ID:
-			ci->c_inf[0].wrapbase = 0x18100000;
-			ci->c_inf[0].cib = 0x27004211;
-			ci->c_inf[1].id = BCMA_CORE_SDIO_DEV;
-			ci->c_inf[1].base = 0x18002000;
-			ci->c_inf[1].wrapbase = 0x18102000;
-			ci->c_inf[1].cib = 0x0a004211;
-			ci->c_inf[2].id = BCMA_CORE_INTERNAL_MEM;
-			ci->c_inf[2].base = 0x18004000;
-			ci->c_inf[2].wrapbase = 0x18104000;
-			ci->c_inf[2].cib = 0x08080401;
-			ci->c_inf[3].id = BCMA_CORE_ARM_CM3;
-			ci->c_inf[3].base = 0x18003000;
-			ci->c_inf[3].wrapbase = 0x18103000;
-			ci->c_inf[3].cib = 0x03004211;
-			ci->c_inf[4].id = BCMA_CORE_80211;
-			ci->c_inf[4].base = BCM43xx_CORE_D11_BASE;
-			ci->c_inf[4].wrapbase = ci->c_inf[4].base + 0x00100000;
-			ci->ramsize = 0x3C000;
-			break;
-		case BCM4339_CHIP_ID:
-			ci->c_inf[0].wrapbase = 0x18100000;
-			ci->c_inf[0].cib = 0x2e084411;
-			ci->c_inf[1].id = BCMA_CORE_SDIO_DEV;
-			ci->c_inf[1].base = 0x18005000;
-			ci->c_inf[1].wrapbase = 0x18105000;
-			ci->c_inf[1].cib = 0x15004211;
-			ci->c_inf[2].id = BCMA_CORE_ARM_CR4;
-			ci->c_inf[2].base = 0x18002000;
-			ci->c_inf[2].wrapbase = 0x18102000;
-			ci->c_inf[2].cib = 0x04084411;
-			ci->c_inf[3].id = BCMA_CORE_80211;
-			ci->c_inf[3].base = BCM43xx_CORE_D11_BASE;
-			ci->c_inf[3].wrapbase = ci->c_inf[3].base + 0x00100000;
-			ci->ramsize = 0xc0000;
-			ci->rambase = 0x180000;
-			break;
-		default:
-			brcmf_err("AXI chip is not supported\n");
-			return -ENODEV;
-		}
-	} else {
-		brcmf_err("chip backplane type %u is not supported\n",
-			  socitype);
-		return -ENODEV;
-	}
-
-	return brcmf_sdio_chip_cichk(ci);
-}
-
-static int
-brcmf_sdio_chip_buscoreprep(struct brcmf_sdio_dev *sdiodev)
-{
-	int err = 0;
-	u8 clkval, clkset;
-
-	/* Try forcing SDIO core to do ALPAvail request only */
-	clkset = SBSDIO_FORCE_HW_CLKREQ_OFF | SBSDIO_ALP_AVAIL_REQ;
-	brcmf_sdiod_regwb(sdiodev, SBSDIO_FUNC1_CHIPCLKCSR, clkset, &err);
-	if (err) {
-		brcmf_err("error writing for HT off\n");
-		return err;
-	}
-
-	/* If register supported, wait for ALPAvail and then force ALP */
-	/* This may take up to 15 milliseconds */
-	clkval = brcmf_sdiod_regrb(sdiodev,
-				   SBSDIO_FUNC1_CHIPCLKCSR, NULL);
-
-	if ((clkval & ~SBSDIO_AVBITS) != clkset) {
-		brcmf_err("ChipClkCSR access: wrote 0x%02x read 0x%02x\n",
-			  clkset, clkval);
-		return -EACCES;
-	}
-
-	SPINWAIT(((clkval = brcmf_sdiod_regrb(sdiodev,
-					      SBSDIO_FUNC1_CHIPCLKCSR, NULL)),
-			!SBSDIO_ALPAV(clkval)),
-			PMU_MAX_TRANSITION_DLY);
-	if (!SBSDIO_ALPAV(clkval)) {
-		brcmf_err("timeout on ALPAV wait, clkval 0x%02x\n",
-			  clkval);
-		return -EBUSY;
-	}
-
-	clkset = SBSDIO_FORCE_HW_CLKREQ_OFF | SBSDIO_FORCE_ALP;
-	brcmf_sdiod_regwb(sdiodev, SBSDIO_FUNC1_CHIPCLKCSR, clkset, &err);
-	udelay(65);
-
-	/* Also, disable the extra SDIO pull-ups */
-	brcmf_sdiod_regwb(sdiodev, SBSDIO_FUNC1_SDIOPULLUP, 0, NULL);
-
-	return 0;
-}
-
-static void
-brcmf_sdio_chip_buscoresetup(struct brcmf_sdio_dev *sdiodev,
-			     struct brcmf_chip *ci)
-{
-	u32 base = ci->c_inf[0].base;
-
-	/* get chipcommon rev */
-	ci->c_inf[0].rev = ci->corerev(sdiodev, ci, ci->c_inf[0].id);
-
-	/* get chipcommon capabilites */
-	ci->c_inf[0].caps = brcmf_sdiod_regrl(sdiodev,
-					      CORE_CC_REG(base, capabilities),
-					      NULL);
-
-	/* get pmu caps & rev */
-	if (ci->c_inf[0].caps & CC_CAP_PMU) {
-		ci->pmucaps =
-			brcmf_sdiod_regrl(sdiodev,
-					  CORE_CC_REG(base, pmucapabilities),
-					  NULL);
-		ci->pmurev = ci->pmucaps & PCAP_REV_MASK;
-	}
-
-	ci->c_inf[1].rev = ci->corerev(sdiodev, ci, ci->c_inf[1].id);
-
-	brcmf_dbg(INFO, "ccrev=%d, pmurev=%d, buscore rev/type=%d/0x%x\n",
-		  ci->c_inf[0].rev, ci->pmurev,
-		  ci->c_inf[1].rev, ci->c_inf[1].id);
-
-	/*
-	 * Make sure any on-chip ARM is off (in case strapping is wrong),
-	 * or downloaded code was already running.
-	 */
-	ci->coredisable(sdiodev, ci, BCMA_CORE_ARM_CM3, 0, 0);
-}
-
-int brcmf_sdio_chip_attach(struct brcmf_sdio_dev *sdiodev,
-			   struct brcmf_chip **ci_ptr)
-{
-	int ret;
-	struct brcmf_chip *ci;
-
-	brcmf_dbg(TRACE, "Enter\n");
-
-	ci = kzalloc(sizeof(*ci), GFP_ATOMIC);
-	if (!ci)
-		return -ENOMEM;
-
-	ret = brcmf_sdio_chip_buscoreprep(sdiodev);
-	if (ret != 0)
-		goto err;
-
-	ret = brcmf_sdio_chip_recognition(sdiodev, ci);
-	if (ret != 0)
-		goto err;
-
-	brcmf_sdio_chip_buscoresetup(sdiodev, ci);
-
-	brcmf_sdiod_regwl(sdiodev, CORE_CC_REG(ci->c_inf[0].base, gpiopullup),
-			  0, NULL);
-	brcmf_sdiod_regwl(sdiodev, CORE_CC_REG(ci->c_inf[0].base, gpiopulldown),
-			  0, NULL);
-
-	*ci_ptr = ci;
-	return 0;
-
-err:
-	kfree(ci);
-	return ret;
-}
-
-void
-brcmf_sdio_chip_detach(struct brcmf_chip **ci_ptr)
-{
-	brcmf_dbg(TRACE, "Enter\n");
-
-	kfree(*ci_ptr);
-	*ci_ptr = NULL;
-}
-
-static char *brcmf_sdio_chip_name(uint chipid, char *buf, uint len)
-{
-	const char *fmt;
-
-	fmt = ((chipid > 0xa000) || (chipid < 0x4000)) ? "%d" : "%x";
-	snprintf(buf, len, fmt, chipid);
-	return buf;
-}
-
-void
-brcmf_sdio_chip_drivestrengthinit(struct brcmf_sdio_dev *sdiodev,
-				  struct brcmf_chip *ci, u32 drivestrength)
-{
-	const struct sdiod_drive_str *str_tab = NULL;
-	u32 str_mask;
-	u32 str_shift;
-	char chn[8];
-	u32 base = ci->c_inf[0].base;
-	u32 i;
-	u32 drivestrength_sel = 0;
-	u32 cc_data_temp;
-	u32 addr;
-
-	if (!(ci->c_inf[0].caps & CC_CAP_PMU))
-		return;
-
-	switch (SDIOD_DRVSTR_KEY(ci->chip, ci->pmurev)) {
-	case SDIOD_DRVSTR_KEY(BCM4330_CHIP_ID, 12):
-		str_tab = sdiod_drvstr_tab1_1v8;
-		str_mask = 0x00003800;
-		str_shift = 11;
-		break;
-	case SDIOD_DRVSTR_KEY(BCM4334_CHIP_ID, 17):
-		str_tab = sdiod_drvstr_tab6_1v8;
-		str_mask = 0x00001800;
-		str_shift = 11;
-		break;
-	case SDIOD_DRVSTR_KEY(BCM43143_CHIP_ID, 17):
-		/* note: 43143 does not support tristate */
-		i = ARRAY_SIZE(sdiod_drvstr_tab2_3v3) - 1;
-		if (drivestrength >= sdiod_drvstr_tab2_3v3[i].strength) {
-			str_tab = sdiod_drvstr_tab2_3v3;
-			str_mask = 0x00000007;
-			str_shift = 0;
-		} else
-			brcmf_err("Invalid SDIO Drive strength for chip %s, strength=%d\n",
-				  brcmf_sdio_chip_name(ci->chip, chn, 8),
-				  drivestrength);
-		break;
-	case SDIOD_DRVSTR_KEY(BCM43362_CHIP_ID, 13):
-		str_tab = sdiod_drive_strength_tab5_1v8;
-		str_mask = 0x00003800;
-		str_shift = 11;
-		break;
-	default:
-		brcmf_err("No SDIO Drive strength init done for chip %s rev %d pmurev %d\n",
-			  brcmf_sdio_chip_name(ci->chip, chn, 8),
-			  ci->chiprev, ci->pmurev);
-		break;
-	}
-
-	if (str_tab != NULL) {
-		for (i = 0; str_tab[i].strength != 0; i++) {
-			if (drivestrength >= str_tab[i].strength) {
-				drivestrength_sel = str_tab[i].sel;
-				break;
-			}
-		}
-		addr = CORE_CC_REG(base, chipcontrol_addr);
-		brcmf_sdiod_regwl(sdiodev, addr, 1, NULL);
-		cc_data_temp = brcmf_sdiod_regrl(sdiodev, addr, NULL);
-		cc_data_temp &= ~str_mask;
-		drivestrength_sel <<= str_shift;
-		cc_data_temp |= drivestrength_sel;
-		brcmf_sdiod_regwl(sdiodev, addr, cc_data_temp, NULL);
-
-		brcmf_dbg(INFO, "SDIO: %d mA (req=%d mA) drive strength selected, set to 0x%08x\n",
-			  str_tab[i].strength, drivestrength, cc_data_temp);
-	}
-}
-
-static void
-brcmf_sdio_chip_cm3_enterdl(struct brcmf_sdio_dev *sdiodev,
-			    struct brcmf_chip *ci)
-{
-	ci->coredisable(sdiodev, ci, BCMA_CORE_ARM_CM3, 0, 0);
-	ci->resetcore(sdiodev, ci, BCMA_CORE_80211,
-		      D11_BCMA_IOCTL_PHYRESET | D11_BCMA_IOCTL_PHYCLOCKEN,
-		      D11_BCMA_IOCTL_PHYCLOCKEN, D11_BCMA_IOCTL_PHYCLOCKEN);
-	ci->resetcore(sdiodev, ci, BCMA_CORE_INTERNAL_MEM, 0, 0, 0);
-}
-
-static bool brcmf_sdio_chip_cm3_exitdl(struct brcmf_sdio_dev *sdiodev,
-				       struct brcmf_chip *ci)
-{
-	u8 core_idx;
-	u32 reg_addr;
-
-	if (!ci->iscoreup(sdiodev, ci, BCMA_CORE_INTERNAL_MEM)) {
-		brcmf_err("SOCRAM core is down after reset?\n");
-		return false;
-	}
-
-	/* clear all interrupts */
-	core_idx = brcmf_sdio_chip_getinfidx(ci, BCMA_CORE_SDIO_DEV);
-	reg_addr = ci->c_inf[core_idx].base;
-	reg_addr += offsetof(struct sdpcmd_regs, intstatus);
-	brcmf_sdiod_regwl(sdiodev, reg_addr, 0xFFFFFFFF, NULL);
-
-	ci->resetcore(sdiodev, ci, BCMA_CORE_ARM_CM3, 0, 0, 0);
-
-	return true;
-}
-
-static inline void
-brcmf_sdio_chip_cr4_enterdl(struct brcmf_sdio_dev *sdiodev,
-			    struct brcmf_chip *ci)
-{
-	u8 idx;
-	u32 regdata;
-	u32 wrapbase;
-	idx = brcmf_sdio_chip_getinfidx(ci, BCMA_CORE_ARM_CR4);
-
-	if (idx == BRCMF_MAX_CORENUM)
-		return;
-
-	wrapbase = ci->c_inf[idx].wrapbase;
-	regdata = brcmf_sdiod_regrl(sdiodev, wrapbase + BCMA_IOCTL, NULL);
-	regdata &= ARMCR4_BCMA_IOCTL_CPUHALT;
-	ci->resetcore(sdiodev, ci, BCMA_CORE_ARM_CR4, regdata,
-		      ARMCR4_BCMA_IOCTL_CPUHALT, ARMCR4_BCMA_IOCTL_CPUHALT);
-	ci->resetcore(sdiodev, ci, BCMA_CORE_80211,
-		      D11_BCMA_IOCTL_PHYRESET | D11_BCMA_IOCTL_PHYCLOCKEN,
-		      D11_BCMA_IOCTL_PHYCLOCKEN, D11_BCMA_IOCTL_PHYCLOCKEN);
-}
-
-static bool brcmf_sdio_chip_cr4_exitdl(struct brcmf_sdio_dev *sdiodev,
-				       struct brcmf_chip *ci, u32 rstvec)
-{
-	u8 core_idx;
-	u32 reg_addr;
-
-	/* clear all interrupts */
-	core_idx = brcmf_sdio_chip_getinfidx(ci, BCMA_CORE_SDIO_DEV);
-	reg_addr = ci->c_inf[core_idx].base;
-	reg_addr += offsetof(struct sdpcmd_regs, intstatus);
-	brcmf_sdiod_regwl(sdiodev, reg_addr, 0xFFFFFFFF, NULL);
-
-	/* Write reset vector to address 0 */
-	brcmf_sdiod_ramrw(sdiodev, true, 0, (void *)&rstvec,
-			  sizeof(rstvec));
-
-	/* restore ARM */
-	ci->resetcore(sdiodev, ci, BCMA_CORE_ARM_CR4, ARMCR4_BCMA_IOCTL_CPUHALT,
-		      0, 0);
-
-	return true;
-}
-
-void brcmf_sdio_chip_enter_download(struct brcmf_sdio_dev *sdiodev,
-				    struct brcmf_chip *ci)
-{
-	u8 arm_core_idx;
-
-	arm_core_idx = brcmf_sdio_chip_getinfidx(ci, BCMA_CORE_ARM_CM3);
-	if (BRCMF_MAX_CORENUM != arm_core_idx) {
-		brcmf_sdio_chip_cm3_enterdl(sdiodev, ci);
-		return;
-	}
-
-	brcmf_sdio_chip_cr4_enterdl(sdiodev, ci);
-}
-
-bool brcmf_sdio_chip_exit_download(struct brcmf_sdio_dev *sdiodev,
-				   struct brcmf_chip *ci, u32 rstvec)
-{
-	u8 arm_core_idx;
-
-	arm_core_idx = brcmf_sdio_chip_getinfidx(ci, BCMA_CORE_ARM_CM3);
-	if (BRCMF_MAX_CORENUM != arm_core_idx)
-		return brcmf_sdio_chip_cm3_exitdl(sdiodev, ci);
-
-	return brcmf_sdio_chip_cr4_exitdl(sdiodev, ci, rstvec);
-}
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.h b/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.h
deleted file mode 100644
index fb0614329ede..000000000000
--- a/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.h
+++ /dev/null
@@ -1,231 +0,0 @@
-/*
- * Copyright (c) 2011 Broadcom Corporation
- *
- * Permission to use, copy, modify, and/or distribute this software for any
- * purpose with or without fee is hereby granted, provided that the above
- * copyright notice and this permission notice appear in all copies.
- *
- * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
- * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
- * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
- * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
- * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
- * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
- * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
- */
-
-#ifndef _BRCMFMAC_SDIO_CHIP_H_
-#define _BRCMFMAC_SDIO_CHIP_H_
-
-/*
- * Core reg address translation.
- * Both macro's returns a 32 bits byte address on the backplane bus.
- */
-#define CORE_CC_REG(base, field) \
-		(base + offsetof(struct chipcregs, field))
-#define CORE_BUS_REG(base, field) \
-		(base + offsetof(struct sdpcmd_regs, field))
-#define CORE_SB(base, field) \
-		(base + SBCONFIGOFF + offsetof(struct sbconfig, field))
-
-/* SDIO function 1 register CHIPCLKCSR */
-/* Force ALP request to backplane */
-#define SBSDIO_FORCE_ALP		0x01
-/* Force HT request to backplane */
-#define SBSDIO_FORCE_HT			0x02
-/* Force ILP request to backplane */
-#define SBSDIO_FORCE_ILP		0x04
-/* Make ALP ready (power up xtal) */
-#define SBSDIO_ALP_AVAIL_REQ		0x08
-/* Make HT ready (power up PLL) */
-#define SBSDIO_HT_AVAIL_REQ		0x10
-/* Squelch clock requests from HW */
-#define SBSDIO_FORCE_HW_CLKREQ_OFF	0x20
-/* Status: ALP is ready */
-#define SBSDIO_ALP_AVAIL		0x40
-/* Status: HT is ready */
-#define SBSDIO_HT_AVAIL			0x80
-#define SBSDIO_AVBITS		(SBSDIO_HT_AVAIL | SBSDIO_ALP_AVAIL)
-#define SBSDIO_ALPAV(regval)	((regval) & SBSDIO_AVBITS)
-#define SBSDIO_HTAV(regval)	(((regval) & SBSDIO_AVBITS) == SBSDIO_AVBITS)
-#define SBSDIO_ALPONLY(regval)	(SBSDIO_ALPAV(regval) && !SBSDIO_HTAV(regval))
-#define SBSDIO_CLKAV(regval, alponly) \
-	(SBSDIO_ALPAV(regval) && (alponly ? 1 : SBSDIO_HTAV(regval)))
-
-#define BRCMF_MAX_CORENUM	6
-
-struct brcmf_core {
-	u16 id;
-	u16 rev;
-	u32 base;
-	u32 wrapbase;
-	u32 caps;
-	u32 cib;
-};
-
-struct brcmf_chip {
-	u32 chip;
-	u32 chiprev;
-	/* core info */
-	/* always put chipcommon core at 0, bus core at 1 */
-	struct brcmf_core c_inf[BRCMF_MAX_CORENUM];
-	u32 pmurev;
-	u32 pmucaps;
-	u32 ramsize;
-	u32 rambase;
-	u32 rst_vec;	/* reset vertor for ARM CR4 core */
-
-	bool (*iscoreup)(struct brcmf_sdio_dev *sdiodev, struct brcmf_chip *ci,
-			 u16 coreid);
-	u32 (*corerev)(struct brcmf_sdio_dev *sdiodev, struct brcmf_chip *ci,
-			 u16 coreid);
-	void (*coredisable)(struct brcmf_sdio_dev *sdiodev,
-			struct brcmf_chip *ci, u16 coreid, u32 pre_resetbits,
-			u32 in_resetbits);
-	void (*resetcore)(struct brcmf_sdio_dev *sdiodev,
-			struct brcmf_chip *ci, u16 coreid, u32 pre_resetbits,
-			u32 in_resetbits, u32 post_resetbits);
-};
-
-struct sbconfig {
-	u32 PAD[2];
-	u32 sbipsflag;	/* initiator port ocp slave flag */
-	u32 PAD[3];
-	u32 sbtpsflag;	/* target port ocp slave flag */
-	u32 PAD[11];
-	u32 sbtmerrloga;	/* (sonics >= 2.3) */
-	u32 PAD;
-	u32 sbtmerrlog;	/* (sonics >= 2.3) */
-	u32 PAD[3];
-	u32 sbadmatch3;	/* address match3 */
-	u32 PAD;
-	u32 sbadmatch2;	/* address match2 */
-	u32 PAD;
-	u32 sbadmatch1;	/* address match1 */
-	u32 PAD[7];
-	u32 sbimstate;	/* initiator agent state */
-	u32 sbintvec;	/* interrupt mask */
-	u32 sbtmstatelow;	/* target state */
-	u32 sbtmstatehigh;	/* target state */
-	u32 sbbwa0;		/* bandwidth allocation table0 */
-	u32 PAD;
-	u32 sbimconfiglow;	/* initiator configuration */
-	u32 sbimconfighigh;	/* initiator configuration */
-	u32 sbadmatch0;	/* address match0 */
-	u32 PAD;
-	u32 sbtmconfiglow;	/* target configuration */
-	u32 sbtmconfighigh;	/* target configuration */
-	u32 sbbconfig;	/* broadcast configuration */
-	u32 PAD;
-	u32 sbbstate;	/* broadcast state */
-	u32 PAD[3];
-	u32 sbactcnfg;	/* activate configuration */
-	u32 PAD[3];
-	u32 sbflagst;	/* current sbflags */
-	u32 PAD[3];
-	u32 sbidlow;		/* identification */
-	u32 sbidhigh;	/* identification */
-};
-
-/* sdio core registers */
-struct sdpcmd_regs {
-	u32 corecontrol;		/* 0x00, rev8 */
-	u32 corestatus;			/* rev8 */
-	u32 PAD[1];
-	u32 biststatus;			/* rev8 */
-
-	/* PCMCIA access */
-	u16 pcmciamesportaladdr;	/* 0x010, rev8 */
-	u16 PAD[1];
-	u16 pcmciamesportalmask;	/* rev8 */
-	u16 PAD[1];
-	u16 pcmciawrframebc;		/* rev8 */
-	u16 PAD[1];
-	u16 pcmciaunderflowtimer;	/* rev8 */
-	u16 PAD[1];
-
-	/* interrupt */
-	u32 intstatus;			/* 0x020, rev8 */
-	u32 hostintmask;		/* rev8 */
-	u32 intmask;			/* rev8 */
-	u32 sbintstatus;		/* rev8 */
-	u32 sbintmask;			/* rev8 */
-	u32 funcintmask;		/* rev4 */
-	u32 PAD[2];
-	u32 tosbmailbox;		/* 0x040, rev8 */
-	u32 tohostmailbox;		/* rev8 */
-	u32 tosbmailboxdata;		/* rev8 */
-	u32 tohostmailboxdata;		/* rev8 */
-
-	/* synchronized access to registers in SDIO clock domain */
-	u32 sdioaccess;			/* 0x050, rev8 */
-	u32 PAD[3];
-
-	/* PCMCIA frame control */
-	u8 pcmciaframectrl;		/* 0x060, rev8 */
-	u8 PAD[3];
-	u8 pcmciawatermark;		/* rev8 */
-	u8 PAD[155];
-
-	/* interrupt batching control */
-	u32 intrcvlazy;			/* 0x100, rev8 */
-	u32 PAD[3];
-
-	/* counters */
-	u32 cmd52rd;			/* 0x110, rev8 */
-	u32 cmd52wr;			/* rev8 */
-	u32 cmd53rd;			/* rev8 */
-	u32 cmd53wr;			/* rev8 */
-	u32 abort;			/* rev8 */
-	u32 datacrcerror;		/* rev8 */
-	u32 rdoutofsync;		/* rev8 */
-	u32 wroutofsync;		/* rev8 */
-	u32 writebusy;			/* rev8 */
-	u32 readwait;			/* rev8 */
-	u32 readterm;			/* rev8 */
-	u32 writeterm;			/* rev8 */
-	u32 PAD[40];
-	u32 clockctlstatus;		/* rev8 */
-	u32 PAD[7];
-
-	u32 PAD[128];			/* DMA engines */
-
-	/* SDIO/PCMCIA CIS region */
-	char cis[512];			/* 0x400-0x5ff, rev6 */
-
-	/* PCMCIA function control registers */
-	char pcmciafcr[256];		/* 0x600-6ff, rev6 */
-	u16 PAD[55];
-
-	/* PCMCIA backplane access */
-	u16 backplanecsr;		/* 0x76E, rev6 */
-	u16 backplaneaddr0;		/* rev6 */
-	u16 backplaneaddr1;		/* rev6 */
-	u16 backplaneaddr2;		/* rev6 */
-	u16 backplaneaddr3;		/* rev6 */
-	u16 backplanedata0;		/* rev6 */
-	u16 backplanedata1;		/* rev6 */
-	u16 backplanedata2;		/* rev6 */
-	u16 backplanedata3;		/* rev6 */
-	u16 PAD[31];
-
-	/* sprom "size" & "blank" info */
-	u16 spromstatus;		/* 0x7BE, rev2 */
-	u32 PAD[464];
-
-	u16 PAD[0x80];
-};
-
-int brcmf_sdio_chip_attach(struct brcmf_sdio_dev *sdiodev,
-			   struct brcmf_chip **ci_ptr);
-void brcmf_sdio_chip_detach(struct brcmf_chip **ci_ptr);
-void brcmf_sdio_chip_drivestrengthinit(struct brcmf_sdio_dev *sdiodev,
-				       struct brcmf_chip *ci,
-				       u32 drivestrength);
-u8 brcmf_sdio_chip_getinfidx(struct brcmf_chip *ci, u16 coreid);
-void brcmf_sdio_chip_enter_download(struct brcmf_sdio_dev *sdiodev,
-				    struct brcmf_chip *ci);
-bool brcmf_sdio_chip_exit_download(struct brcmf_sdio_dev *sdiodev,
-				   struct brcmf_chip *ci, u32 rstvec);
-
-#endif		/* _BRCMFMAC_SDIO_CHIP_H_ */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/sdio_host.h b/drivers/net/wireless/brcm80211/brcmfmac/sdio_host.h
index 092e9c824992..3deab7959a0d 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/sdio_host.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/sdio_host.h
@@ -180,6 +180,97 @@ struct brcmf_sdio_dev {
 	uint max_request_size;
 	ushort max_segment_count;
 	uint max_segment_size;
+	uint txglomsz;
+	struct sg_table sgtable;
+};
+
+/* sdio core registers */
+struct sdpcmd_regs {
+	u32 corecontrol;		/* 0x00, rev8 */
+	u32 corestatus;			/* rev8 */
+	u32 PAD[1];
+	u32 biststatus;			/* rev8 */
+
+	/* PCMCIA access */
+	u16 pcmciamesportaladdr;	/* 0x010, rev8 */
+	u16 PAD[1];
+	u16 pcmciamesportalmask;	/* rev8 */
+	u16 PAD[1];
+	u16 pcmciawrframebc;		/* rev8 */
+	u16 PAD[1];
+	u16 pcmciaunderflowtimer;	/* rev8 */
+	u16 PAD[1];
+
+	/* interrupt */
+	u32 intstatus;			/* 0x020, rev8 */
+	u32 hostintmask;		/* rev8 */
+	u32 intmask;			/* rev8 */
+	u32 sbintstatus;		/* rev8 */
+	u32 sbintmask;			/* rev8 */
+	u32 funcintmask;		/* rev4 */
+	u32 PAD[2];
+	u32 tosbmailbox;		/* 0x040, rev8 */
+	u32 tohostmailbox;		/* rev8 */
+	u32 tosbmailboxdata;		/* rev8 */
+	u32 tohostmailboxdata;		/* rev8 */
+
+	/* synchronized access to registers in SDIO clock domain */
+	u32 sdioaccess;			/* 0x050, rev8 */
+	u32 PAD[3];
+
+	/* PCMCIA frame control */
+	u8 pcmciaframectrl;		/* 0x060, rev8 */
+	u8 PAD[3];
+	u8 pcmciawatermark;		/* rev8 */
+	u8 PAD[155];
+
+	/* interrupt batching control */
+	u32 intrcvlazy;			/* 0x100, rev8 */
+	u32 PAD[3];
+
+	/* counters */
+	u32 cmd52rd;			/* 0x110, rev8 */
+	u32 cmd52wr;			/* rev8 */
+	u32 cmd53rd;			/* rev8 */
+	u32 cmd53wr;			/* rev8 */
+	u32 abort;			/* rev8 */
+	u32 datacrcerror;		/* rev8 */
+	u32 rdoutofsync;		/* rev8 */
+	u32 wroutofsync;		/* rev8 */
+	u32 writebusy;			/* rev8 */
+	u32 readwait;			/* rev8 */
+	u32 readterm;			/* rev8 */
+	u32 writeterm;			/* rev8 */
+	u32 PAD[40];
+	u32 clockctlstatus;		/* rev8 */
+	u32 PAD[7];
+
+	u32 PAD[128];			/* DMA engines */
+
+	/* SDIO/PCMCIA CIS region */
+	char cis[512];			/* 0x400-0x5ff, rev6 */
+
+	/* PCMCIA function control registers */
+	char pcmciafcr[256];		/* 0x600-6ff, rev6 */
+	u16 PAD[55];
+
+	/* PCMCIA backplane access */
+	u16 backplanecsr;		/* 0x76E, rev6 */
+	u16 backplaneaddr0;		/* rev6 */
+	u16 backplaneaddr1;		/* rev6 */
+	u16 backplaneaddr2;		/* rev6 */
+	u16 backplaneaddr3;		/* rev6 */
+	u16 backplanedata0;		/* rev6 */
+	u16 backplanedata1;		/* rev6 */
+	u16 backplanedata2;		/* rev6 */
+	u16 backplanedata3;		/* rev6 */
+	u16 PAD[31];
+
+	/* sprom "size" & "blank" info */
+	u16 spromstatus;		/* 0x7BE, rev2 */
+	u32 PAD[464];
+
+	u16 PAD[0x80];
 };
 
 /* Register/deregister interrupt handler. */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c b/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
index d7718a5fa2f0..00bd1e16c3ce 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
@@ -18,6 +18,7 @@
 
 #include <linux/kernel.h>
 #include <linux/etherdevice.h>
+#include <linux/module.h>
 #include <net/cfg80211.h>
 #include <net/netlink.h>
 
@@ -251,6 +252,10 @@ struct parsed_vndr_ies {
 	struct parsed_vndr_ie_info ie_info[VNDR_IE_PARSE_LIMIT];
 };
 
+static int brcmf_roamoff;
+module_param_named(roamoff, brcmf_roamoff, int, S_IRUSR);
+MODULE_PARM_DESC(roamoff, "do not use internal roaming engine");
+
 /* Quarter dBm units to mW
  * Table starts at QDBM_OFFSET, so the first entry is mW for qdBm=153
  * Table is offset so the last entry is largest mW value that fits in
@@ -351,13 +356,11 @@ u16 channel_to_chanspec(struct brcmu_d11inf *d11inf,
  * triples, returning a pointer to the substring whose first element
  * matches tag
  */
-struct brcmf_tlv *brcmf_parse_tlvs(void *buf, int buflen, uint key)
+const struct brcmf_tlv *
+brcmf_parse_tlvs(const void *buf, int buflen, uint key)
 {
-	struct brcmf_tlv *elt;
-	int totlen;
-
-	elt = (struct brcmf_tlv *)buf;
-	totlen = buflen;
+	const struct brcmf_tlv *elt = buf;
+	int totlen = buflen;
 
 	/* find tagged parameter */
 	while (totlen >= TLV_HDR_LEN) {
@@ -378,8 +381,8 @@ struct brcmf_tlv *brcmf_parse_tlvs(void *buf, int buflen, uint key)
  * not update the tlvs buffer pointer/length.
  */
 static bool
-brcmf_tlv_has_ie(u8 *ie, u8 **tlvs, u32 *tlvs_len,
-		 u8 *oui, u32 oui_len, u8 type)
+brcmf_tlv_has_ie(const u8 *ie, const u8 **tlvs, u32 *tlvs_len,
+		 const u8 *oui, u32 oui_len, u8 type)
 {
 	/* If the contents match the OUI and the type */
 	if (ie[TLV_LEN_OFF] >= oui_len + 1 &&
@@ -401,12 +404,12 @@ brcmf_tlv_has_ie(u8 *ie, u8 **tlvs, u32 *tlvs_len,
 }
 
 static struct brcmf_vs_tlv *
-brcmf_find_wpaie(u8 *parse, u32 len)
+brcmf_find_wpaie(const u8 *parse, u32 len)
 {
-	struct brcmf_tlv *ie;
+	const struct brcmf_tlv *ie;
 
 	while ((ie = brcmf_parse_tlvs(parse, len, WLAN_EID_VENDOR_SPECIFIC))) {
-		if (brcmf_tlv_has_ie((u8 *)ie, &parse, &len,
+		if (brcmf_tlv_has_ie((const u8 *)ie, &parse, &len,
 				     WPA_OUI, TLV_OUI_LEN, WPA_OUI_TYPE))
 			return (struct brcmf_vs_tlv *)ie;
 	}
@@ -414,9 +417,9 @@ brcmf_find_wpaie(u8 *parse, u32 len)
 }
 
 static struct brcmf_vs_tlv *
-brcmf_find_wpsie(u8 *parse, u32 len)
+brcmf_find_wpsie(const u8 *parse, u32 len)
 {
-	struct brcmf_tlv *ie;
+	const struct brcmf_tlv *ie;
 
 	while ((ie = brcmf_parse_tlvs(parse, len, WLAN_EID_VENDOR_SPECIFIC))) {
 		if (brcmf_tlv_has_ie((u8 *)ie, &parse, &len,
@@ -1562,9 +1565,9 @@ brcmf_cfg80211_connect(struct wiphy *wiphy, struct net_device *ndev,
 	struct ieee80211_channel *chan = sme->channel;
 	struct brcmf_join_params join_params;
 	size_t join_params_size;
-	struct brcmf_tlv *rsn_ie;
-	struct brcmf_vs_tlv *wpa_ie;
-	void *ie;
+	const struct brcmf_tlv *rsn_ie;
+	const struct brcmf_vs_tlv *wpa_ie;
+	const void *ie;
 	u32 ie_len;
 	struct brcmf_ext_join_params_le *ext_join_params;
 	u16 chanspec;
@@ -1591,7 +1594,8 @@ brcmf_cfg80211_connect(struct wiphy *wiphy, struct net_device *ndev,
 			ie_len = wpa_ie->len + TLV_HDR_LEN;
 		} else {
 			/* find the RSN_IE */
-			rsn_ie = brcmf_parse_tlvs((u8 *)sme->ie, sme->ie_len,
+			rsn_ie = brcmf_parse_tlvs((const u8 *)sme->ie,
+						  sme->ie_len,
 						  WLAN_EID_RSN);
 			if (rsn_ie) {
 				ie = rsn_ie;
@@ -1981,7 +1985,9 @@ brcmf_cfg80211_add_key(struct wiphy *wiphy, struct net_device *ndev,
 	if (!check_vif_up(ifp->vif))
 		return -EIO;
 
-	if (mac_addr) {
+	if (mac_addr &&
+		(params->cipher != WLAN_CIPHER_SUITE_WEP40) &&
+		(params->cipher != WLAN_CIPHER_SUITE_WEP104)) {
 		brcmf_dbg(TRACE, "Exit");
 		return brcmf_add_keyext(wiphy, ndev, key_idx, mac_addr, params);
 	}
@@ -2164,6 +2170,8 @@ brcmf_cfg80211_get_station(struct wiphy *wiphy, struct net_device *ndev,
 	s32 err = 0;
 	u8 *bssid = profile->bssid;
 	struct brcmf_sta_info_le sta_info_le;
+	u32 beacon_period;
+	u32 dtim_period;
 
 	brcmf_dbg(TRACE, "Enter, MAC %pM\n", mac);
 	if (!check_vif_up(ifp->vif))
@@ -2218,6 +2226,30 @@ brcmf_cfg80211_get_station(struct wiphy *wiphy, struct net_device *ndev,
 				sinfo->signal = rssi;
 				brcmf_dbg(CONN, "RSSI %d dBm\n", rssi);
 			}
+			err = brcmf_fil_cmd_int_get(ifp, BRCMF_C_GET_BCNPRD,
+						    &beacon_period);
+			if (err) {
+				brcmf_err("Could not get beacon period (%d)\n",
+					  err);
+				goto done;
+			} else {
+				sinfo->bss_param.beacon_interval =
+					beacon_period;
+				brcmf_dbg(CONN, "Beacon peroid %d\n",
+					  beacon_period);
+			}
+			err = brcmf_fil_cmd_int_get(ifp, BRCMF_C_GET_DTIMPRD,
+						    &dtim_period);
+			if (err) {
+				brcmf_err("Could not get DTIM period (%d)\n",
+					  err);
+				goto done;
+			} else {
+				sinfo->bss_param.dtim_period = dtim_period;
+				brcmf_dbg(CONN, "DTIM peroid %d\n",
+					  dtim_period);
+			}
+			sinfo->filled |= STATION_INFO_BSS_PARAM;
 		}
 	} else
 		err = -EPERM;
@@ -2455,7 +2487,7 @@ static s32 brcmf_update_bss_info(struct brcmf_cfg80211_info *cfg,
 	struct brcmf_cfg80211_profile *profile = ndev_to_prof(ifp->ndev);
 	struct brcmf_bss_info_le *bi;
 	struct brcmf_ssid *ssid;
-	struct brcmf_tlv *tim;
+	const struct brcmf_tlv *tim;
 	u16 beacon_interval;
 	u8 dtim_period;
 	size_t ie_len;
@@ -3220,8 +3252,9 @@ static bool brcmf_valid_wpa_oui(u8 *oui, bool is_rsn_ie)
 }
 
 static s32
-brcmf_configure_wpaie(struct net_device *ndev, struct brcmf_vs_tlv *wpa_ie,
-		     bool is_rsn_ie)
+brcmf_configure_wpaie(struct net_device *ndev,
+		      const struct brcmf_vs_tlv *wpa_ie,
+		      bool is_rsn_ie)
 {
 	struct brcmf_if *ifp = netdev_priv(ndev);
 	u32 auth = 0; /* d11 open authentication */
@@ -3707,11 +3740,11 @@ brcmf_cfg80211_start_ap(struct wiphy *wiphy, struct net_device *ndev,
 	s32 ie_offset;
 	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
 	struct brcmf_if *ifp = netdev_priv(ndev);
-	struct brcmf_tlv *ssid_ie;
+	const struct brcmf_tlv *ssid_ie;
 	struct brcmf_ssid_le ssid_le;
 	s32 err = -EPERM;
-	struct brcmf_tlv *rsn_ie;
-	struct brcmf_vs_tlv *wpa_ie;
+	const struct brcmf_tlv *rsn_ie;
+	const struct brcmf_vs_tlv *wpa_ie;
 	struct brcmf_join_params join_params;
 	enum nl80211_iftype dev_role;
 	struct brcmf_fil_bss_enable_le bss_enable;
@@ -4416,7 +4449,9 @@ static bool brcmf_is_linkdown(const struct brcmf_event_msg *e)
 	u32 event = e->event_code;
 	u16 flags = e->flags;
 
-	if (event == BRCMF_E_LINK && (!(flags & BRCMF_EVENT_MSG_LINK))) {
+	if ((event == BRCMF_E_DEAUTH) || (event == BRCMF_E_DEAUTH_IND) ||
+	    (event == BRCMF_E_DISASSOC_IND) ||
+	    ((event == BRCMF_E_LINK) && (!(flags & BRCMF_EVENT_MSG_LINK)))) {
 		brcmf_dbg(CONN, "Processing link down\n");
 		return true;
 	}
@@ -4658,16 +4693,19 @@ brcmf_notify_connect_status(struct brcmf_if *ifp,
 	struct brcmf_cfg80211_info *cfg = ifp->drvr->config;
 	struct net_device *ndev = ifp->ndev;
 	struct brcmf_cfg80211_profile *profile = &ifp->vif->profile;
+	struct ieee80211_channel *chan;
 	s32 err = 0;
+	u16 reason;
 
 	if (ifp->vif->mode == WL_MODE_AP) {
 		err = brcmf_notify_connect_status_ap(cfg, ndev, e, data);
 	} else if (brcmf_is_linkup(e)) {
 		brcmf_dbg(CONN, "Linkup\n");
 		if (brcmf_is_ibssmode(ifp->vif)) {
+			chan = ieee80211_get_channel(cfg->wiphy, cfg->channel);
 			memcpy(profile->bssid, e->addr, ETH_ALEN);
 			wl_inform_ibss(cfg, ndev, e->addr);
-			cfg80211_ibss_joined(ndev, e->addr, GFP_KERNEL);
+			cfg80211_ibss_joined(ndev, e->addr, chan, GFP_KERNEL);
 			clear_bit(BRCMF_VIF_STATUS_CONNECTING,
 				  &ifp->vif->sme_state);
 			set_bit(BRCMF_VIF_STATUS_CONNECTED,
@@ -4679,9 +4717,15 @@ brcmf_notify_connect_status(struct brcmf_if *ifp,
 		if (!brcmf_is_ibssmode(ifp->vif)) {
 			brcmf_bss_connect_done(cfg, ndev, e, false);
 			if (test_and_clear_bit(BRCMF_VIF_STATUS_CONNECTED,
-					       &ifp->vif->sme_state))
-				cfg80211_disconnected(ndev, 0, NULL, 0,
+					       &ifp->vif->sme_state)) {
+				reason = 0;
+				if (((e->event_code == BRCMF_E_DEAUTH_IND) ||
+				     (e->event_code == BRCMF_E_DISASSOC_IND)) &&
+				    (e->reason != WLAN_REASON_UNSPECIFIED))
+					reason = e->reason;
+				cfg80211_disconnected(ndev, reason, NULL, 0,
 						      GFP_KERNEL);
+			}
 		}
 		brcmf_link_down(ifp->vif);
 		brcmf_init_prof(ndev_to_prof(ndev));
@@ -4875,11 +4919,8 @@ static s32 wl_init_priv(struct brcmf_cfg80211_info *cfg)
 
 	cfg->scan_request = NULL;
 	cfg->pwr_save = true;
-	cfg->roam_on = true;	/* roam on & off switch.
-				 we enable roam per default */
-	cfg->active_scan = true;	/* we do active scan for
-				 specific scan per default */
-	cfg->dongle_up = false;	/* dongle is not up yet */
+	cfg->active_scan = true;	/* we do active scan per default */
+	cfg->dongle_up = false;		/* dongle is not up yet */
 	err = brcmf_init_priv_mem(cfg);
 	if (err)
 		return err;
@@ -4999,7 +5040,7 @@ void brcmf_cfg80211_detach(struct brcmf_cfg80211_info *cfg)
 }
 
 static s32
-brcmf_dongle_roam(struct brcmf_if *ifp, u32 roamvar, u32 bcn_timeout)
+brcmf_dongle_roam(struct brcmf_if *ifp, u32 bcn_timeout)
 {
 	s32 err = 0;
 	__le32 roamtrigger[2];
@@ -5009,7 +5050,7 @@ brcmf_dongle_roam(struct brcmf_if *ifp, u32 roamvar, u32 bcn_timeout)
 	 * Setup timeout if Beacons are lost and roam is
 	 * off to report link down
 	 */
-	if (roamvar) {
+	if (brcmf_roamoff) {
 		err = brcmf_fil_iovar_int_set(ifp, "bcn_timeout", bcn_timeout);
 		if (err) {
 			brcmf_err("bcn_timeout error (%d)\n", err);
@@ -5021,8 +5062,9 @@ brcmf_dongle_roam(struct brcmf_if *ifp, u32 roamvar, u32 bcn_timeout)
 	 * Enable/Disable built-in roaming to allow supplicant
 	 * to take care of roaming
 	 */
-	brcmf_dbg(INFO, "Internal Roaming = %s\n", roamvar ? "Off" : "On");
-	err = brcmf_fil_iovar_int_set(ifp, "roam_off", roamvar);
+	brcmf_dbg(INFO, "Internal Roaming = %s\n",
+		  brcmf_roamoff ? "Off" : "On");
+	err = brcmf_fil_iovar_int_set(ifp, "roam_off", !!(brcmf_roamoff));
 	if (err) {
 		brcmf_err("roam_off error (%d)\n", err);
 		goto dongle_rom_out;
@@ -5164,9 +5206,6 @@ static s32 brcmf_construct_reginfo(struct brcmf_cfg80211_info *cfg,
 				ieee80211_channel_to_frequency(ch.chnum, band);
 			band_chan_arr[index].hw_value = ch.chnum;
 
-			brcmf_err("channel %d: f=%d bw=%d sb=%d\n",
-				  ch.chnum, band_chan_arr[index].center_freq,
-				  ch.bw, ch.sb);
 			if (ch.bw == BRCMU_CHAN_BW_40) {
 				/* assuming the order is HT20, HT40 Upper,
 				 * HT40 lower from chanspecs
@@ -5267,6 +5306,8 @@ static s32 brcmf_update_wiphybands(struct brcmf_cfg80211_info *cfg)
 	u32 band_list[3];
 	u32 nmode;
 	u32 bw_cap[2] = { 0, 0 };
+	u32 rxchain;
+	u32 nchain;
 	s8 phy;
 	s32 err;
 	u32 nband;
@@ -5303,6 +5344,16 @@ static s32 brcmf_update_wiphybands(struct brcmf_cfg80211_info *cfg)
 	brcmf_dbg(INFO, "nmode=%d, bw_cap=(%d, %d)\n", nmode,
 		  bw_cap[IEEE80211_BAND_2GHZ], bw_cap[IEEE80211_BAND_5GHZ]);
 
+	err = brcmf_fil_iovar_int_get(ifp, "rxchain", &rxchain);
+	if (err) {
+		brcmf_err("rxchain error (%d)\n", err);
+		nchain = 1;
+	} else {
+		for (nchain = 0; rxchain; nchain++)
+			rxchain = rxchain & (rxchain - 1);
+	}
+	brcmf_dbg(INFO, "nchain=%d\n", nchain);
+
 	err = brcmf_construct_reginfo(cfg, bw_cap);
 	if (err) {
 		brcmf_err("brcmf_construct_reginfo failed (%d)\n", err);
@@ -5331,10 +5382,7 @@ static s32 brcmf_update_wiphybands(struct brcmf_cfg80211_info *cfg)
 		band->ht_cap.ht_supported = true;
 		band->ht_cap.ampdu_factor = IEEE80211_HT_MAX_AMPDU_64K;
 		band->ht_cap.ampdu_density = IEEE80211_HT_MPDU_DENSITY_16;
-		/* An HT shall support all EQM rates for one spatial
-		 * stream
-		 */
-		band->ht_cap.mcs.rx_mask[0] = 0xff;
+		memset(band->ht_cap.mcs.rx_mask, 0xff, nchain);
 		band->ht_cap.mcs.tx_params = IEEE80211_HT_MCS_TX_DEFINED;
 		bands[band->band] = band;
 	}
@@ -5381,7 +5429,7 @@ static s32 brcmf_config_dongle(struct brcmf_cfg80211_info *cfg)
 	brcmf_dbg(INFO, "power save set to %s\n",
 		  (power_mode ? "enabled" : "disabled"));
 
-	err = brcmf_dongle_roam(ifp, (cfg->roam_on ? 0 : 1), WL_BEACON_TIMEOUT);
+	err = brcmf_dongle_roam(ifp, WL_BEACON_TIMEOUT);
 	if (err)
 		goto default_conf_out;
 	err = brcmf_cfg80211_change_iface(wdev->wiphy, ndev, wdev->iftype,
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.h b/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.h
index 2dc6a074e8ed..5715bb0708cf 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.h
@@ -402,7 +402,6 @@ struct brcmf_cfg80211_info {
 	bool ibss_starter;
 	bool pwr_save;
 	bool dongle_up;
-	bool roam_on;
 	bool scan_tried;
 	u8 *dcmd_buf;
 	u8 *extra_buf;
@@ -491,7 +490,8 @@ void brcmf_free_vif(struct brcmf_cfg80211_vif *vif);
 s32 brcmf_vif_set_mgmt_ie(struct brcmf_cfg80211_vif *vif, s32 pktflag,
 			  const u8 *vndr_ie_buf, u32 vndr_ie_len);
 s32 brcmf_vif_clear_mgmt_ies(struct brcmf_cfg80211_vif *vif);
-struct brcmf_tlv *brcmf_parse_tlvs(void *buf, int buflen, uint key);
+const struct brcmf_tlv *
+brcmf_parse_tlvs(const void *buf, int buflen, uint key);
 u16 channel_to_chanspec(struct brcmu_d11inf *d11inf,
 			struct ieee80211_channel *ch);
 u32 wl_get_vif_state_all(struct brcmf_cfg80211_info *cfg, unsigned long state);
diff --git a/drivers/net/wireless/cw1200/fwio.c b/drivers/net/wireless/cw1200/fwio.c
index 5a9ffd3a6a6c..e23d67e0bfe6 100644
--- a/drivers/net/wireless/cw1200/fwio.c
+++ b/drivers/net/wireless/cw1200/fwio.c
@@ -202,8 +202,8 @@ static int cw1200_load_firmware_cw1200(struct cw1200_common *priv)
 		}
 
 		/* calculate the block size */
-		tx_size = block_size = min((size_t)(firmware->size - put),
-			(size_t)DOWNLOAD_BLOCK_SIZE);
+		tx_size = block_size = min_t(size_t, firmware->size - put,
+					DOWNLOAD_BLOCK_SIZE);
 
 		memcpy(buf, &firmware->data[put], block_size);
 		if (block_size < DOWNLOAD_BLOCK_SIZE) {
diff --git a/drivers/net/wireless/hostap/hostap_ap.c b/drivers/net/wireless/hostap/hostap_ap.c
index d36e252d2ccb..596525528f50 100644
--- a/drivers/net/wireless/hostap/hostap_ap.c
+++ b/drivers/net/wireless/hostap/hostap_ap.c
@@ -147,7 +147,7 @@ static void ap_free_sta(struct ap_data *ap, struct sta_info *sta)
 
 	if (!sta->ap && sta->u.sta.challenge)
 		kfree(sta->u.sta.challenge);
-	del_timer(&sta->timer);
+	del_timer_sync(&sta->timer);
 #endif /* PRISM2_NO_KERNEL_IEEE80211_MGMT */
 
 	kfree(sta);
diff --git a/drivers/net/wireless/hostap/hostap_cs.c b/drivers/net/wireless/hostap/hostap_cs.c
index 9f825f2620da..b6ec51923b20 100644
--- a/drivers/net/wireless/hostap/hostap_cs.c
+++ b/drivers/net/wireless/hostap/hostap_cs.c
@@ -677,6 +677,8 @@ static const struct pcmcia_device_id hostap_cs_ids[] = {
 	PCMCIA_DEVICE_PROD_ID12(
 		"ZoomAir 11Mbps High", "Rate wireless Networking",
 		0x273fe3db, 0x32a1eaee),
+	PCMCIA_DEVICE_PROD_ID12("NETGEAR MA401 Wireless PC", "Card",
+		0xa37434e9, 0x9762e8f1),
 	PCMCIA_DEVICE_PROD_ID123(
 		"Pretec", "CompactWLAN Card 802.11b", "2.5",
 		0x1cadd3e5, 0xe697636c, 0x7a5bfcf1),
diff --git a/drivers/net/wireless/hostap/hostap_proc.c b/drivers/net/wireless/hostap/hostap_proc.c
index aa7ad3a7a69b..4e5c0f8c9496 100644
--- a/drivers/net/wireless/hostap/hostap_proc.c
+++ b/drivers/net/wireless/hostap/hostap_proc.c
@@ -496,7 +496,7 @@ void hostap_init_proc(local_info_t *local)
 
 void hostap_remove_proc(local_info_t *local)
 {
-	remove_proc_subtree(local->ddev->name, hostap_proc);
+	proc_remove(local->proc);
 }
 
 
diff --git a/drivers/net/wireless/ipw2x00/ipw2100.c b/drivers/net/wireless/ipw2x00/ipw2100.c
index 3aba49259ef1..dfc6dfc56d52 100644
--- a/drivers/net/wireless/ipw2x00/ipw2100.c
+++ b/drivers/net/wireless/ipw2x00/ipw2100.c
@@ -7065,7 +7065,7 @@ static int ipw2100_wx_set_nick(struct net_device *dev,
 	if (wrqu->data.length > IW_ESSID_MAX_SIZE)
 		return -E2BIG;
 
-	wrqu->data.length = min((size_t) wrqu->data.length, sizeof(priv->nick));
+	wrqu->data.length = min_t(size_t, wrqu->data.length, sizeof(priv->nick));
 	memset(priv->nick, 0, sizeof(priv->nick));
 	memcpy(priv->nick, extra, wrqu->data.length);
 
diff --git a/drivers/net/wireless/ipw2x00/ipw2200.c b/drivers/net/wireless/ipw2x00/ipw2200.c
index 139326065bd9..c5aa404069f3 100644
--- a/drivers/net/wireless/ipw2x00/ipw2200.c
+++ b/drivers/net/wireless/ipw2x00/ipw2200.c
@@ -9169,7 +9169,7 @@ static int ipw_wx_set_nick(struct net_device *dev,
 	if (wrqu->data.length > IW_ESSID_MAX_SIZE)
 		return -E2BIG;
 	mutex_lock(&priv->mutex);
-	wrqu->data.length = min((size_t) wrqu->data.length, sizeof(priv->nick));
+	wrqu->data.length = min_t(size_t, wrqu->data.length, sizeof(priv->nick));
 	memset(priv->nick, 0, sizeof(priv->nick));
 	memcpy(priv->nick, extra, wrqu->data.length);
 	IPW_DEBUG_TRACE("<<\n");
diff --git a/drivers/net/wireless/iwlegacy/3945-mac.c b/drivers/net/wireless/iwlegacy/3945-mac.c
index 0487461ae4da..dc1d20cf64ee 100644
--- a/drivers/net/wireless/iwlegacy/3945-mac.c
+++ b/drivers/net/wireless/iwlegacy/3945-mac.c
@@ -1248,14 +1248,7 @@ il3945_rx_handle(struct il_priv *il)
 		len = le32_to_cpu(pkt->len_n_flags) & IL_RX_FRAME_SIZE_MSK;
 		len += sizeof(u32);	/* account for status word */
 
-		/* Reclaim a command buffer only if this packet is a response
-		 *   to a (driver-originated) command.
-		 * If the packet (e.g. Rx frame) originated from uCode,
-		 *   there is no command buffer to reclaim.
-		 * Ucode should set SEQ_RX_FRAME bit if ucode-originated,
-		 *   but apparently a few don't get set; catch them here. */
-		reclaim = !(pkt->hdr.sequence & SEQ_RX_FRAME) &&
-		    pkt->hdr.cmd != N_STATS && pkt->hdr.cmd != C_TX;
+		reclaim = il_need_reclaim(il, pkt);
 
 		/* Based on type of command response or notification,
 		 *   handle those that need handling via function in
@@ -1495,12 +1488,14 @@ il3945_irq_tasklet(struct il_priv *il)
 	if (inta & CSR_INT_BIT_WAKEUP) {
 		D_ISR("Wakeup interrupt\n");
 		il_rx_queue_update_write_ptr(il, &il->rxq);
+
+		spin_lock_irqsave(&il->lock, flags);
 		il_txq_update_write_ptr(il, &il->txq[0]);
 		il_txq_update_write_ptr(il, &il->txq[1]);
 		il_txq_update_write_ptr(il, &il->txq[2]);
 		il_txq_update_write_ptr(il, &il->txq[3]);
 		il_txq_update_write_ptr(il, &il->txq[4]);
-		il_txq_update_write_ptr(il, &il->txq[5]);
+		spin_unlock_irqrestore(&il->lock, flags);
 
 		il->isr_stats.wakeup++;
 		handled |= CSR_INT_BIT_WAKEUP;
diff --git a/drivers/net/wireless/iwlegacy/3945-rs.c b/drivers/net/wireless/iwlegacy/3945-rs.c
index 9a45f6f626f6..76b0729ade17 100644
--- a/drivers/net/wireless/iwlegacy/3945-rs.c
+++ b/drivers/net/wireless/iwlegacy/3945-rs.c
@@ -891,8 +891,7 @@ il3945_rs_rate_init_stub(void *il_r, struct ieee80211_supported_band *sband,
 {
 }
 
-static struct rate_control_ops rs_ops = {
-	.module = NULL,
+static const struct rate_control_ops rs_ops = {
 	.name = RS_NAME,
 	.tx_status = il3945_rs_tx_status,
 	.get_rate = il3945_rs_get_rate,
diff --git a/drivers/net/wireless/iwlegacy/4965-mac.c b/drivers/net/wireless/iwlegacy/4965-mac.c
index 43f488a8cda2..888ad5c74639 100644
--- a/drivers/net/wireless/iwlegacy/4965-mac.c
+++ b/drivers/net/wireless/iwlegacy/4965-mac.c
@@ -92,7 +92,6 @@ il4965_check_abort_status(struct il_priv *il, u8 frame_count, u32 status)
  * EEPROM
  */
 struct il_mod_params il4965_mod_params = {
-	.amsdu_size_8K = 1,
 	.restart_fw = 1,
 	/* the rest are 0 by default */
 };
@@ -4274,17 +4273,7 @@ il4965_rx_handle(struct il_priv *il)
 		len = le32_to_cpu(pkt->len_n_flags) & IL_RX_FRAME_SIZE_MSK;
 		len += sizeof(u32);	/* account for status word */
 
-		/* Reclaim a command buffer only if this packet is a response
-		 *   to a (driver-originated) command.
-		 * If the packet (e.g. Rx frame) originated from uCode,
-		 *   there is no command buffer to reclaim.
-		 * Ucode should set SEQ_RX_FRAME bit if ucode-originated,
-		 *   but apparently a few don't get set; catch them here. */
-		reclaim = !(pkt->hdr.sequence & SEQ_RX_FRAME) &&
-		    (pkt->hdr.cmd != N_RX_PHY) && (pkt->hdr.cmd != N_RX) &&
-		    (pkt->hdr.cmd != N_RX_MPDU) &&
-		    (pkt->hdr.cmd != N_COMPRESSED_BA) &&
-		    (pkt->hdr.cmd != N_STATS) && (pkt->hdr.cmd != C_TX);
+		reclaim = il_need_reclaim(il, pkt);
 
 		/* Based on type of command response or notification,
 		 *   handle those that need handling via function in
@@ -6876,6 +6865,6 @@ module_param_named(11n_disable, il4965_mod_params.disable_11n, int, S_IRUGO);
 MODULE_PARM_DESC(11n_disable, "disable 11n functionality");
 module_param_named(amsdu_size_8K, il4965_mod_params.amsdu_size_8K, int,
 		   S_IRUGO);
-MODULE_PARM_DESC(amsdu_size_8K, "enable 8K amsdu size");
+MODULE_PARM_DESC(amsdu_size_8K, "enable 8K amsdu size (default 0 [disabled])");
 module_param_named(fw_restart, il4965_mod_params.restart_fw, int, S_IRUGO);
 MODULE_PARM_DESC(fw_restart, "restart firmware in case of error");
diff --git a/drivers/net/wireless/iwlegacy/4965-rs.c b/drivers/net/wireless/iwlegacy/4965-rs.c
index 4d5e33259ca8..eaaeea19d8c5 100644
--- a/drivers/net/wireless/iwlegacy/4965-rs.c
+++ b/drivers/net/wireless/iwlegacy/4965-rs.c
@@ -2807,8 +2807,7 @@ il4965_rs_rate_init_stub(void *il_r, struct ieee80211_supported_band *sband,
 {
 }
 
-static struct rate_control_ops rs_4965_ops = {
-	.module = NULL,
+static const struct rate_control_ops rs_4965_ops = {
 	.name = IL4965_RS_NAME,
 	.tx_status = il4965_rs_tx_status,
 	.get_rate = il4965_rs_get_rate,
diff --git a/drivers/net/wireless/iwlegacy/commands.h b/drivers/net/wireless/iwlegacy/commands.h
index 048421511988..dd744135c956 100644
--- a/drivers/net/wireless/iwlegacy/commands.h
+++ b/drivers/net/wireless/iwlegacy/commands.h
@@ -2270,7 +2270,8 @@ struct il_spectrum_notification {
  */
 #define IL_POWER_VEC_SIZE 5
 
-#define IL_POWER_DRIVER_ALLOW_SLEEP_MSK	cpu_to_le16(BIT(0))
+#define IL_POWER_DRIVER_ALLOW_SLEEP_MSK		cpu_to_le16(BIT(0))
+#define IL_POWER_SLEEP_OVER_DTIM_MSK		cpu_to_le16(BIT(2))
 #define IL_POWER_PCI_PM_MSK			cpu_to_le16(BIT(3))
 
 struct il3945_powertable_cmd {
diff --git a/drivers/net/wireless/iwlegacy/common.c b/drivers/net/wireless/iwlegacy/common.c
index 02e8233ccf29..4f42174d9994 100644
--- a/drivers/net/wireless/iwlegacy/common.c
+++ b/drivers/net/wireless/iwlegacy/common.c
@@ -1078,29 +1078,82 @@ EXPORT_SYMBOL(il_get_channel_info);
  * Setting power level allows the card to go to sleep when not busy.
  *
  * We calculate a sleep command based on the required latency, which
- * we get from mac80211. In order to handle thermal throttling, we can
- * also use pre-defined power levels.
+ * we get from mac80211.
  */
 
-/*
- * This defines the old power levels. They are still used by default
- * (level 1) and for thermal throttle (levels 3 through 5)
- */
-
-struct il_power_vec_entry {
-	struct il_powertable_cmd cmd;
-	u8 no_dtim;		/* number of skip dtim */
-};
+#define SLP_VEC(X0, X1, X2, X3, X4) { \
+		cpu_to_le32(X0), \
+		cpu_to_le32(X1), \
+		cpu_to_le32(X2), \
+		cpu_to_le32(X3), \
+		cpu_to_le32(X4)  \
+}
 
 static void
-il_power_sleep_cam_cmd(struct il_priv *il, struct il_powertable_cmd *cmd)
+il_build_powertable_cmd(struct il_priv *il, struct il_powertable_cmd *cmd)
 {
+	const __le32 interval[3][IL_POWER_VEC_SIZE] = {
+		SLP_VEC(2, 2, 4, 6, 0xFF),
+		SLP_VEC(2, 4, 7, 10, 10),
+		SLP_VEC(4, 7, 10, 10, 0xFF)
+	};
+	int i, dtim_period, no_dtim;
+	u32 max_sleep;
+	bool skip;
+
 	memset(cmd, 0, sizeof(*cmd));
 
 	if (il->power_data.pci_pm)
 		cmd->flags |= IL_POWER_PCI_PM_MSK;
 
-	D_POWER("Sleep command for CAM\n");
+	/* if no Power Save, we are done */
+	if (il->power_data.ps_disabled)
+		return;
+
+	cmd->flags = IL_POWER_DRIVER_ALLOW_SLEEP_MSK;
+	cmd->keep_alive_seconds = 0;
+	cmd->debug_flags = 0;
+	cmd->rx_data_timeout = cpu_to_le32(25 * 1024);
+	cmd->tx_data_timeout = cpu_to_le32(25 * 1024);
+	cmd->keep_alive_beacons = 0;
+
+	dtim_period = il->vif ? il->vif->bss_conf.dtim_period : 0;
+
+	if (dtim_period <= 2) {
+		memcpy(cmd->sleep_interval, interval[0], sizeof(interval[0]));
+		no_dtim = 2;
+	} else if (dtim_period <= 10) {
+		memcpy(cmd->sleep_interval, interval[1], sizeof(interval[1]));
+		no_dtim = 2;
+	} else {
+		memcpy(cmd->sleep_interval, interval[2], sizeof(interval[2]));
+		no_dtim = 0;
+	}
+
+	if (dtim_period == 0) {
+		dtim_period = 1;
+		skip = false;
+	} else {
+		skip = !!no_dtim;
+	}
+
+	if (skip) {
+		__le32 tmp = cmd->sleep_interval[IL_POWER_VEC_SIZE - 1];
+
+		max_sleep = le32_to_cpu(tmp);
+		if (max_sleep == 0xFF)
+			max_sleep = dtim_period * (skip + 1);
+		else if (max_sleep >  dtim_period)
+			max_sleep = (max_sleep / dtim_period) * dtim_period;
+		cmd->flags |= IL_POWER_SLEEP_OVER_DTIM_MSK;
+	} else {
+		max_sleep = dtim_period;
+		cmd->flags &= ~IL_POWER_SLEEP_OVER_DTIM_MSK;
+	}
+
+	for (i = 0; i < IL_POWER_VEC_SIZE; i++)
+		if (le32_to_cpu(cmd->sleep_interval[i]) > max_sleep)
+			cmd->sleep_interval[i] = cpu_to_le32(max_sleep);
 }
 
 static int
@@ -1173,7 +1226,8 @@ il_power_update_mode(struct il_priv *il, bool force)
 {
 	struct il_powertable_cmd cmd;
 
-	il_power_sleep_cam_cmd(il, &cmd);
+	il_build_powertable_cmd(il, &cmd);
+
 	return il_power_set_mode(il, &cmd, force);
 }
 EXPORT_SYMBOL(il_power_update_mode);
@@ -5081,6 +5135,7 @@ set_ch_out:
 	}
 
 	if (changed & (IEEE80211_CONF_CHANGE_PS | IEEE80211_CONF_CHANGE_IDLE)) {
+		il->power_data.ps_disabled = !(conf->flags & IEEE80211_CONF_PS);
 		ret = il_power_update_mode(il, false);
 		if (ret)
 			D_MAC80211("Error setting sleep level\n");
diff --git a/drivers/net/wireless/iwlegacy/common.h b/drivers/net/wireless/iwlegacy/common.h
index ad123d66ab6c..dfb13c70efe8 100644
--- a/drivers/net/wireless/iwlegacy/common.h
+++ b/drivers/net/wireless/iwlegacy/common.h
@@ -1123,6 +1123,7 @@ struct il_power_mgr {
 	struct il_powertable_cmd sleep_cmd_next;
 	int debug_sleep_level_override;
 	bool pci_pm;
+	bool ps_disabled;
 };
 
 struct il_priv {
@@ -1597,7 +1598,7 @@ struct il_mod_params {
 	int disable_hw_scan;	/* def: 0 = use h/w scan */
 	int num_of_queues;	/* def: HW dependent */
 	int disable_11n;	/* def: 0 = 11n capabilities enabled */
-	int amsdu_size_8K;	/* def: 1 = enable 8K amsdu size */
+	int amsdu_size_8K;	/* def: 0 = disable 8K amsdu size */
 	int antenna;		/* def: 0 = both antennas (use diversity) */
 	int restart_fw;		/* def: 1 = restart firmware */
 };
@@ -1978,6 +1979,20 @@ void il_wr_prph(struct il_priv *il, u32 addr, u32 val);
 u32 il_read_targ_mem(struct il_priv *il, u32 addr);
 void il_write_targ_mem(struct il_priv *il, u32 addr, u32 val);
 
+static inline bool il_need_reclaim(struct il_priv *il, struct il_rx_pkt *pkt)
+{
+	/* Reclaim a command buffer only if this packet is a response
+	 * to a (driver-originated) command. If the packet (e.g. Rx frame)
+	 * originated from uCode, there is no command buffer to reclaim.
+	 * Ucode should set SEQ_RX_FRAME bit if ucode-originated, but
+	 * apparently a few don't get set; catch them here.
+	 */
+	return !(pkt->hdr.sequence & SEQ_RX_FRAME) &&
+	       pkt->hdr.cmd != N_STATS && pkt->hdr.cmd != C_TX &&
+	       pkt->hdr.cmd != N_RX_PHY && pkt->hdr.cmd != N_RX &&
+	       pkt->hdr.cmd != N_RX_MPDU && pkt->hdr.cmd != N_COMPRESSED_BA;
+}
+
 static inline void
 _il_write8(struct il_priv *il, u32 ofs, u8 val)
 {
diff --git a/drivers/net/wireless/iwlwifi/Kconfig b/drivers/net/wireless/iwlwifi/Kconfig
index 3eb2102ce236..74b3b4de7bb7 100644
--- a/drivers/net/wireless/iwlwifi/Kconfig
+++ b/drivers/net/wireless/iwlwifi/Kconfig
@@ -68,6 +68,19 @@ config IWLWIFI_OPMODE_MODULAR
 comment "WARNING: iwlwifi is useless without IWLDVM or IWLMVM"
 	depends on IWLWIFI && IWLDVM=n && IWLMVM=n
 
+config IWLWIFI_BCAST_FILTERING
+	bool "Enable broadcast filtering"
+	depends on IWLMVM
+	help
+	  Say Y here to enable default bcast filtering configuration.
+
+	  Enabling broadcast filtering will drop any incoming wireless
+	  broadcast frames, except some very specific predefined
+	  patterns (e.g. incoming arp requests).
+
+	  If unsure, don't enable this option, as some programs might
+	  expect incoming broadcasts for their normal operations.
+
 menu "Debugging Options"
 	depends on IWLWIFI
 
@@ -111,6 +124,7 @@ config IWLWIFI_DEBUG_EXPERIMENTAL_UCODE
 	  Enable use of experimental ucode for testing and debugging.
 
 config IWLWIFI_DEVICE_TRACING
+
 	bool "iwlwifi device access tracing"
 	depends on IWLWIFI
 	depends on EVENT_TRACING
diff --git a/drivers/net/wireless/iwlwifi/Makefile b/drivers/net/wireless/iwlwifi/Makefile
index 1fa64429bcc2..3d32f4120174 100644
--- a/drivers/net/wireless/iwlwifi/Makefile
+++ b/drivers/net/wireless/iwlwifi/Makefile
@@ -8,7 +8,7 @@ iwlwifi-objs		+= iwl-eeprom-read.o iwl-eeprom-parse.o
 iwlwifi-objs		+= iwl-phy-db.o iwl-nvm-parse.o
 iwlwifi-objs		+= pcie/drv.o pcie/rx.o pcie/tx.o pcie/trans.o
 iwlwifi-$(CONFIG_IWLDVM) += iwl-1000.o iwl-2000.o iwl-5000.o iwl-6000.o
-iwlwifi-$(CONFIG_IWLMVM) += iwl-7000.o
+iwlwifi-$(CONFIG_IWLMVM) += iwl-7000.o iwl-8000.o
 
 iwlwifi-objs += $(iwlwifi-m)
 
diff --git a/drivers/net/wireless/iwlwifi/dvm/agn.h b/drivers/net/wireless/iwlwifi/dvm/agn.h
index 562772d85102..c160dad03037 100644
--- a/drivers/net/wireless/iwlwifi/dvm/agn.h
+++ b/drivers/net/wireless/iwlwifi/dvm/agn.h
@@ -109,7 +109,7 @@ extern const struct iwl_dvm_cfg iwl_dvm_6030_cfg;
 
 struct iwl_ucode_capabilities;
 
-extern struct ieee80211_ops iwlagn_hw_ops;
+extern const struct ieee80211_ops iwlagn_hw_ops;
 
 static inline void iwl_set_calib_hdr(struct iwl_calib_hdr *hdr, u8 cmd)
 {
@@ -480,7 +480,7 @@ do {									\
 } while (0)
 #endif				/* CONFIG_IWLWIFI_DEBUG */
 
-extern const char *iwl_dvm_cmd_strings[REPLY_MAX];
+extern const char *const iwl_dvm_cmd_strings[REPLY_MAX];
 
 static inline const char *iwl_dvm_get_cmd_string(u8 cmd)
 {
diff --git a/drivers/net/wireless/iwlwifi/dvm/devices.c b/drivers/net/wireless/iwlwifi/dvm/devices.c
index 7b140e487deb..758c54eeb206 100644
--- a/drivers/net/wireless/iwlwifi/dvm/devices.c
+++ b/drivers/net/wireless/iwlwifi/dvm/devices.c
@@ -317,7 +317,7 @@ static const struct iwl_sensitivity_ranges iwl5000_sensitivity = {
 	.nrg_th_cca = 62,
 };
 
-static struct iwl_sensitivity_ranges iwl5150_sensitivity = {
+static const struct iwl_sensitivity_ranges iwl5150_sensitivity = {
 	.min_nrg_cck = 95,
 	.auto_corr_min_ofdm = 90,
 	.auto_corr_min_ofdm_mrc = 170,
diff --git a/drivers/net/wireless/iwlwifi/dvm/mac80211.c b/drivers/net/wireless/iwlwifi/dvm/mac80211.c
index 73086c1629ca..dd55c9cf7ba8 100644
--- a/drivers/net/wireless/iwlwifi/dvm/mac80211.c
+++ b/drivers/net/wireless/iwlwifi/dvm/mac80211.c
@@ -1582,7 +1582,7 @@ static void iwlagn_mac_sta_notify(struct ieee80211_hw *hw,
 	IWL_DEBUG_MAC80211(priv, "leave\n");
 }
 
-struct ieee80211_ops iwlagn_hw_ops = {
+const struct ieee80211_ops iwlagn_hw_ops = {
 	.tx = iwlagn_mac_tx,
 	.start = iwlagn_mac_start,
 	.stop = iwlagn_mac_stop,
diff --git a/drivers/net/wireless/iwlwifi/dvm/rs.c b/drivers/net/wireless/iwlwifi/dvm/rs.c
index 0977d93b529d..aa773a2da4ab 100644
--- a/drivers/net/wireless/iwlwifi/dvm/rs.c
+++ b/drivers/net/wireless/iwlwifi/dvm/rs.c
@@ -176,46 +176,46 @@ static void rs_dbgfs_set_mcs(struct iwl_lq_sta *lq_sta,
  * (2.4 GHz) band.
  */
 
-static s32 expected_tpt_legacy[IWL_RATE_COUNT] = {
+static const u16 expected_tpt_legacy[IWL_RATE_COUNT] = {
 	7, 13, 35, 58, 40, 57, 72, 98, 121, 154, 177, 186, 0
 };
 
-static s32 expected_tpt_siso20MHz[4][IWL_RATE_COUNT] = {
+static const u16 expected_tpt_siso20MHz[4][IWL_RATE_COUNT] = {
 	{0, 0, 0, 0, 42, 0,  76, 102, 124, 159, 183, 193, 202}, /* Norm */
 	{0, 0, 0, 0, 46, 0,  82, 110, 132, 168, 192, 202, 210}, /* SGI */
 	{0, 0, 0, 0, 47, 0,  91, 133, 171, 242, 305, 334, 362}, /* AGG */
 	{0, 0, 0, 0, 52, 0, 101, 145, 187, 264, 330, 361, 390}, /* AGG+SGI */
 };
 
-static s32 expected_tpt_siso40MHz[4][IWL_RATE_COUNT] = {
+static const u16 expected_tpt_siso40MHz[4][IWL_RATE_COUNT] = {
 	{0, 0, 0, 0,  77, 0, 127, 160, 184, 220, 242, 250, 257}, /* Norm */
 	{0, 0, 0, 0,  83, 0, 135, 169, 193, 229, 250, 257, 264}, /* SGI */
 	{0, 0, 0, 0,  94, 0, 177, 249, 313, 423, 512, 550, 586}, /* AGG */
 	{0, 0, 0, 0, 104, 0, 193, 270, 338, 454, 545, 584, 620}, /* AGG+SGI */
 };
 
-static s32 expected_tpt_mimo2_20MHz[4][IWL_RATE_COUNT] = {
+static const u16 expected_tpt_mimo2_20MHz[4][IWL_RATE_COUNT] = {
 	{0, 0, 0, 0,  74, 0, 123, 155, 179, 214, 236, 244, 251}, /* Norm */
 	{0, 0, 0, 0,  81, 0, 131, 164, 188, 223, 243, 251, 257}, /* SGI */
 	{0, 0, 0, 0,  89, 0, 167, 235, 296, 402, 488, 526, 560}, /* AGG */
 	{0, 0, 0, 0,  97, 0, 182, 255, 320, 431, 520, 558, 593}, /* AGG+SGI*/
 };
 
-static s32 expected_tpt_mimo2_40MHz[4][IWL_RATE_COUNT] = {
+static const u16 expected_tpt_mimo2_40MHz[4][IWL_RATE_COUNT] = {
 	{0, 0, 0, 0, 123, 0, 182, 214, 235, 264, 279, 285, 289}, /* Norm */
 	{0, 0, 0, 0, 131, 0, 191, 222, 242, 270, 284, 289, 293}, /* SGI */
 	{0, 0, 0, 0, 171, 0, 305, 410, 496, 634, 731, 771, 805}, /* AGG */
 	{0, 0, 0, 0, 186, 0, 329, 439, 527, 667, 764, 803, 838}, /* AGG+SGI */
 };
 
-static s32 expected_tpt_mimo3_20MHz[4][IWL_RATE_COUNT] = {
+static const u16 expected_tpt_mimo3_20MHz[4][IWL_RATE_COUNT] = {
 	{0, 0, 0, 0,  99, 0, 153, 186, 208, 239, 256, 263, 268}, /* Norm */
 	{0, 0, 0, 0, 106, 0, 162, 194, 215, 246, 262, 268, 273}, /* SGI */
 	{0, 0, 0, 0, 134, 0, 249, 346, 431, 574, 685, 732, 775}, /* AGG */
 	{0, 0, 0, 0, 148, 0, 272, 376, 465, 614, 727, 775, 818}, /* AGG+SGI */
 };
 
-static s32 expected_tpt_mimo3_40MHz[4][IWL_RATE_COUNT] = {
+static const u16 expected_tpt_mimo3_40MHz[4][IWL_RATE_COUNT] = {
 	{0, 0, 0, 0, 152, 0, 211, 239, 255, 279,  290,  294,  297}, /* Norm */
 	{0, 0, 0, 0, 160, 0, 219, 245, 261, 284,  294,  297,  300}, /* SGI */
 	{0, 0, 0, 0, 254, 0, 443, 584, 695, 868,  984, 1030, 1070}, /* AGG */
@@ -1111,7 +1111,7 @@ static void rs_set_expected_tpt_table(struct iwl_lq_sta *lq_sta,
 				      struct iwl_scale_tbl_info *tbl)
 {
 	/* Used to choose among HT tables */
-	s32 (*ht_tbl_pointer)[IWL_RATE_COUNT];
+	const u16 (*ht_tbl_pointer)[IWL_RATE_COUNT];
 
 	/* Check for invalid LQ type */
 	if (WARN_ON_ONCE(!is_legacy(tbl->lq_type) && !is_Ht(tbl->lq_type))) {
@@ -1173,9 +1173,8 @@ static s32 rs_get_best_rate(struct iwl_priv *priv,
 	    &(lq_sta->lq_info[lq_sta->active_tbl]);
 	s32 active_sr = active_tbl->win[index].success_ratio;
 	s32 active_tpt = active_tbl->expected_tpt[index];
-
 	/* expected "search" throughput */
-	s32 *tpt_tbl = tbl->expected_tpt;
+	const u16 *tpt_tbl = tbl->expected_tpt;
 
 	s32 new_rate, high, low, start_hi;
 	u16 high_low;
@@ -3319,8 +3318,8 @@ static void rs_rate_init_stub(void *priv_r, struct ieee80211_supported_band *sba
 			      struct ieee80211_sta *sta, void *priv_sta)
 {
 }
-static struct rate_control_ops rs_ops = {
-	.module = NULL,
+
+static const struct rate_control_ops rs_ops = {
 	.name = RS_NAME,
 	.tx_status = rs_tx_status,
 	.get_rate = rs_get_rate,
diff --git a/drivers/net/wireless/iwlwifi/dvm/rs.h b/drivers/net/wireless/iwlwifi/dvm/rs.h
index bdd5644a400b..f6bd25cad203 100644
--- a/drivers/net/wireless/iwlwifi/dvm/rs.h
+++ b/drivers/net/wireless/iwlwifi/dvm/rs.h
@@ -315,7 +315,7 @@ struct iwl_scale_tbl_info {
 	u8 is_dup;	/* 1 = duplicated data streams */
 	u8 action;	/* change modulation; IWL_[LEGACY/SISO/MIMO]_SWITCH_* */
 	u8 max_search;	/* maximun number of tables we can search */
-	s32 *expected_tpt;	/* throughput metrics; expected_tpt_G, etc. */
+	const u16 *expected_tpt;	/* throughput metrics; expected_tpt_G, etc. */
 	u32 current_rate;  /* rate_n_flags, uCode API format */
 	struct iwl_rate_scale_data win[IWL_RATE_COUNT]; /* rate histories */
 };
diff --git a/drivers/net/wireless/iwlwifi/dvm/rx.c b/drivers/net/wireless/iwlwifi/dvm/rx.c
index 7a1bc1c547e1..cd8377346aff 100644
--- a/drivers/net/wireless/iwlwifi/dvm/rx.c
+++ b/drivers/net/wireless/iwlwifi/dvm/rx.c
@@ -39,7 +39,7 @@
 
 #define IWL_CMD_ENTRY(x) [x] = #x
 
-const char *iwl_dvm_cmd_strings[REPLY_MAX] = {
+const char *const iwl_dvm_cmd_strings[REPLY_MAX] = {
 	IWL_CMD_ENTRY(REPLY_ALIVE),
 	IWL_CMD_ENTRY(REPLY_ERROR),
 	IWL_CMD_ENTRY(REPLY_ECHO),
diff --git a/drivers/net/wireless/iwlwifi/iwl-7000.c b/drivers/net/wireless/iwlwifi/iwl-7000.c
index 2a59da2ff87a..fbd262ffa497 100644
--- a/drivers/net/wireless/iwlwifi/iwl-7000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-7000.c
@@ -71,8 +71,8 @@
 #define IWL3160_UCODE_API_MAX	8
 
 /* Oldest version we won't warn about */
-#define IWL7260_UCODE_API_OK	7
-#define IWL3160_UCODE_API_OK	7
+#define IWL7260_UCODE_API_OK	8
+#define IWL3160_UCODE_API_OK	8
 
 /* Lowest firmware API version supported */
 #define IWL7260_UCODE_API_MIN	7
@@ -95,6 +95,8 @@
 #define IWL7265_FW_PRE "iwlwifi-7265-"
 #define IWL7265_MODULE_FIRMWARE(api) IWL7265_FW_PRE __stringify(api) ".ucode"
 
+#define NVM_HW_SECTION_NUM_FAMILY_7000		0
+
 static const struct iwl_base_params iwl7000_base_params = {
 	.eeprom_size = OTP_LOW_IMAGE_SIZE,
 	.num_of_queues = IWLAGN_NUM_QUEUES,
@@ -120,7 +122,8 @@ static const struct iwl_ht_params iwl7000_ht_params = {
 	.max_inst_size = IWL60_RTC_INST_SIZE,			\
 	.max_data_size = IWL60_RTC_DATA_SIZE,			\
 	.base_params = &iwl7000_base_params,			\
-	.led_mode = IWL_LED_RF_STATE
+	.led_mode = IWL_LED_RF_STATE,				\
+	.nvm_hw_section_num = NVM_HW_SECTION_NUM_FAMILY_7000
 
 
 const struct iwl_cfg iwl7260_2ac_cfg = {
@@ -194,6 +197,17 @@ const struct iwl_cfg iwl3160_n_cfg = {
 	.host_interrupt_operation_mode = true,
 };
 
+static const struct iwl_pwr_tx_backoff iwl7265_pwr_tx_backoffs[] = {
+	{.pwr = 1600, .backoff = 0},
+	{.pwr = 1300, .backoff = 467},
+	{.pwr = 900,  .backoff = 1900},
+	{.pwr = 800, .backoff = 2630},
+	{.pwr = 700, .backoff = 3720},
+	{.pwr = 600, .backoff = 5550},
+	{.pwr = 500, .backoff = 9350},
+	{0},
+};
+
 const struct iwl_cfg iwl7265_2ac_cfg = {
 	.name = "Intel(R) Dual Band Wireless AC 7265",
 	.fw_name_pre = IWL7265_FW_PRE,
@@ -201,6 +215,7 @@ const struct iwl_cfg iwl7265_2ac_cfg = {
 	.ht_params = &iwl7000_ht_params,
 	.nvm_ver = IWL7265_NVM_VERSION,
 	.nvm_calib_ver = IWL7265_TX_POWER_VERSION,
+	.pwr_tx_backoffs = iwl7265_pwr_tx_backoffs,
 };
 
 const struct iwl_cfg iwl7265_2n_cfg = {
@@ -210,6 +225,7 @@ const struct iwl_cfg iwl7265_2n_cfg = {
 	.ht_params = &iwl7000_ht_params,
 	.nvm_ver = IWL7265_NVM_VERSION,
 	.nvm_calib_ver = IWL7265_TX_POWER_VERSION,
+	.pwr_tx_backoffs = iwl7265_pwr_tx_backoffs,
 };
 
 const struct iwl_cfg iwl7265_n_cfg = {
@@ -219,6 +235,7 @@ const struct iwl_cfg iwl7265_n_cfg = {
 	.ht_params = &iwl7000_ht_params,
 	.nvm_ver = IWL7265_NVM_VERSION,
 	.nvm_calib_ver = IWL7265_TX_POWER_VERSION,
+	.pwr_tx_backoffs = iwl7265_pwr_tx_backoffs,
 };
 
 MODULE_FIRMWARE(IWL7260_MODULE_FIRMWARE(IWL7260_UCODE_API_OK));
diff --git a/drivers/net/wireless/iwlwifi/iwl-8000.c b/drivers/net/wireless/iwlwifi/iwl-8000.c
new file mode 100644
index 000000000000..f5bd82b88592
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-8000.c
@@ -0,0 +1,132 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ * redistributing this file, you may do so under either license.
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2014 Intel 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
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called COPYING.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2014 Intel Corporation. All rights reserved.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+
+#include <linux/module.h>
+#include <linux/stringify.h>
+#include "iwl-config.h"
+#include "iwl-agn-hw.h"
+
+/* Highest firmware API version supported */
+#define IWL8000_UCODE_API_MAX	8
+
+/* Oldest version we won't warn about */
+#define IWL8000_UCODE_API_OK	8
+
+/* Lowest firmware API version supported */
+#define IWL8000_UCODE_API_MIN	8
+
+/* NVM versions */
+#define IWL8000_NVM_VERSION		0x0a1d
+#define IWL8000_TX_POWER_VERSION	0xffff /* meaningless */
+
+#define IWL8000_FW_PRE "iwlwifi-8000-"
+#define IWL8000_MODULE_FIRMWARE(api) IWL8000_FW_PRE __stringify(api) ".ucode"
+
+#define NVM_HW_SECTION_NUM_FAMILY_8000		10
+
+static const struct iwl_base_params iwl8000_base_params = {
+	.eeprom_size = OTP_LOW_IMAGE_SIZE,
+	.num_of_queues = IWLAGN_NUM_QUEUES,
+	.pll_cfg_val = 0,
+	.shadow_ram_support = true,
+	.led_compensation = 57,
+	.wd_timeout = IWL_LONG_WD_TIMEOUT,
+	.max_event_log_size = 512,
+	.shadow_reg_enable = true,
+	.pcie_l1_allowed = true,
+};
+
+static const struct iwl_ht_params iwl8000_ht_params = {
+	.ht40_bands = BIT(IEEE80211_BAND_2GHZ) | BIT(IEEE80211_BAND_5GHZ),
+};
+
+#define IWL_DEVICE_8000						\
+	.ucode_api_max = IWL8000_UCODE_API_MAX,			\
+	.ucode_api_ok = IWL8000_UCODE_API_OK,			\
+	.ucode_api_min = IWL8000_UCODE_API_MIN,			\
+	.device_family = IWL_DEVICE_FAMILY_8000,		\
+	.max_inst_size = IWL60_RTC_INST_SIZE,			\
+	.max_data_size = IWL60_RTC_DATA_SIZE,			\
+	.base_params = &iwl8000_base_params,			\
+	.led_mode = IWL_LED_RF_STATE,				\
+	.nvm_hw_section_num = NVM_HW_SECTION_NUM_FAMILY_8000
+
+const struct iwl_cfg iwl8260_2ac_cfg = {
+	.name = "Intel(R) Dual Band Wireless AC 8260",
+	.fw_name_pre = IWL8000_FW_PRE,
+	IWL_DEVICE_8000,
+	.ht_params = &iwl8000_ht_params,
+	.nvm_ver = IWL8000_NVM_VERSION,
+	.nvm_calib_ver = IWL8000_TX_POWER_VERSION,
+};
+
+const struct iwl_cfg iwl8260_n_cfg = {
+	.name = "Intel(R) Dual Band Wireless-AC 8260",
+	.fw_name_pre = IWL8000_FW_PRE,
+	IWL_DEVICE_8000,
+	.ht_params = &iwl8000_ht_params,
+	.nvm_ver = IWL8000_NVM_VERSION,
+	.nvm_calib_ver = IWL8000_TX_POWER_VERSION,
+};
+
+MODULE_FIRMWARE(IWL8000_MODULE_FIRMWARE(IWL8000_UCODE_API_OK));
diff --git a/drivers/net/wireless/iwlwifi/iwl-config.h b/drivers/net/wireless/iwlwifi/iwl-config.h
index 1ced525157dc..13ec56607d10 100644
--- a/drivers/net/wireless/iwlwifi/iwl-config.h
+++ b/drivers/net/wireless/iwlwifi/iwl-config.h
@@ -84,6 +84,7 @@ enum iwl_device_family {
 	IWL_DEVICE_FAMILY_6050,
 	IWL_DEVICE_FAMILY_6150,
 	IWL_DEVICE_FAMILY_7000,
+	IWL_DEVICE_FAMILY_8000,
 };
 
 /*
@@ -192,6 +193,15 @@ struct iwl_eeprom_params {
 	bool enhanced_txpower;
 };
 
+/* Tx-backoff power threshold
+ * @pwr: The power limit in mw
+ * @backoff: The tx-backoff in uSec
+ */
+struct iwl_pwr_tx_backoff {
+	u32 pwr;
+	u32 backoff;
+};
+
 /**
  * struct iwl_cfg
  * @name: Offical name of the device
@@ -217,6 +227,9 @@ struct iwl_eeprom_params {
  * @high_temp: Is this NIC is designated to be in high temperature.
  * @host_interrupt_operation_mode: device needs host interrupt operation
  *	mode set
+ * @d0i3: device uses d0i3 instead of d3
+ * @nvm_hw_section_num: the ID of the HW NVM section
+ * @pwr_tx_backoffs: translation table between power limits and backoffs
  *
  * We enable the driver to be backward compatible wrt. hardware features.
  * API differences in uCode shouldn't be handled here but through TLVs
@@ -247,6 +260,9 @@ struct iwl_cfg {
 	const bool internal_wimax_coex;
 	const bool host_interrupt_operation_mode;
 	bool high_temp;
+	bool d0i3;
+	u8   nvm_hw_section_num;
+	const struct iwl_pwr_tx_backoff *pwr_tx_backoffs;
 };
 
 /*
@@ -307,6 +323,8 @@ extern const struct iwl_cfg iwl3160_n_cfg;
 extern const struct iwl_cfg iwl7265_2ac_cfg;
 extern const struct iwl_cfg iwl7265_2n_cfg;
 extern const struct iwl_cfg iwl7265_n_cfg;
+extern const struct iwl_cfg iwl8260_2ac_cfg;
+extern const struct iwl_cfg iwl8260_n_cfg;
 #endif /* CONFIG_IWLMVM */
 
 #endif /* __IWL_CONFIG_H__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-csr.h b/drivers/net/wireless/iwlwifi/iwl-csr.h
index 9d325516c42d..f13dec9ad9c9 100644
--- a/drivers/net/wireless/iwlwifi/iwl-csr.h
+++ b/drivers/net/wireless/iwlwifi/iwl-csr.h
@@ -395,38 +395,6 @@
 #define CSR_DRAM_INT_TBL_ENABLE		(1 << 31)
 #define CSR_DRAM_INIT_TBL_WRAP_CHECK	(1 << 27)
 
-/* SECURE boot registers */
-#define CSR_SECURE_BOOT_CONFIG_ADDR	(0x100)
-enum secure_boot_config_reg {
-	CSR_SECURE_BOOT_CONFIG_INSPECTOR_BURNED_IN_OTP	= 0x00000001,
-	CSR_SECURE_BOOT_CONFIG_INSPECTOR_NOT_REQ	= 0x00000002,
-};
-
-#define CSR_SECURE_BOOT_CPU1_STATUS_ADDR	(0x100)
-#define CSR_SECURE_BOOT_CPU2_STATUS_ADDR	(0x100)
-enum secure_boot_status_reg {
-	CSR_SECURE_BOOT_CPU_STATUS_VERF_STATUS		= 0x00000003,
-	CSR_SECURE_BOOT_CPU_STATUS_VERF_COMPLETED	= 0x00000002,
-	CSR_SECURE_BOOT_CPU_STATUS_VERF_SUCCESS		= 0x00000004,
-	CSR_SECURE_BOOT_CPU_STATUS_VERF_FAIL		= 0x00000008,
-	CSR_SECURE_BOOT_CPU_STATUS_SIGN_VERF_FAIL	= 0x00000010,
-};
-
-#define CSR_UCODE_LOAD_STATUS_ADDR	(0x100)
-enum secure_load_status_reg {
-	CSR_CPU_STATUS_LOADING_STARTED			= 0x00000001,
-	CSR_CPU_STATUS_LOADING_COMPLETED		= 0x00000002,
-	CSR_CPU_STATUS_NUM_OF_LAST_COMPLETED		= 0x000000F8,
-	CSR_CPU_STATUS_NUM_OF_LAST_LOADED_BLOCK		= 0x0000FF00,
-};
-
-#define CSR_SECURE_INSPECTOR_CODE_ADDR	(0x100)
-#define CSR_SECURE_INSPECTOR_DATA_ADDR	(0x100)
-
-#define CSR_SECURE_TIME_OUT	(100)
-
-#define FH_TCSR_0_REG0 (0x1D00)
-
 /*
  * HBUS (Host-side Bus)
  *
diff --git a/drivers/net/wireless/iwlwifi/iwl-debug.h b/drivers/net/wireless/iwlwifi/iwl-debug.h
index a75aac986a23..c8cbdbe15924 100644
--- a/drivers/net/wireless/iwlwifi/iwl-debug.h
+++ b/drivers/net/wireless/iwlwifi/iwl-debug.h
@@ -126,6 +126,7 @@ do {                                            			\
 /* 0x00000F00 - 0x00000100 */
 #define IWL_DL_POWER		0x00000100
 #define IWL_DL_TEMP		0x00000200
+#define IWL_DL_RPM		0x00000400
 #define IWL_DL_SCAN		0x00000800
 /* 0x0000F000 - 0x00001000 */
 #define IWL_DL_ASSOC		0x00001000
@@ -189,5 +190,6 @@ do {                                            			\
 #define IWL_DEBUG_RADIO(p, f, a...)	IWL_DEBUG(p, IWL_DL_RADIO, f, ## a)
 #define IWL_DEBUG_POWER(p, f, a...)	IWL_DEBUG(p, IWL_DL_POWER, f, ## a)
 #define IWL_DEBUG_11H(p, f, a...)	IWL_DEBUG(p, IWL_DL_11H, f, ## a)
+#define IWL_DEBUG_RPM(p, f, a...)	IWL_DEBUG(p, IWL_DL_RPM, f, ## a)
 
 #endif
diff --git a/drivers/net/wireless/iwlwifi/iwl-drv.c b/drivers/net/wireless/iwlwifi/iwl-drv.c
index 75103554cd63..0a3e841b44a9 100644
--- a/drivers/net/wireless/iwlwifi/iwl-drv.c
+++ b/drivers/net/wireless/iwlwifi/iwl-drv.c
@@ -128,7 +128,7 @@ struct iwl_drv {
 	const struct iwl_cfg *cfg;
 
 	int fw_index;                   /* firmware we're trying to load */
-	char firmware_name[25];         /* name of firmware file to load */
+	char firmware_name[32];         /* name of firmware file to load */
 
 	struct completion request_firmware_complete;
 
@@ -237,7 +237,8 @@ static int iwl_request_firmware(struct iwl_drv *drv, bool first)
 		return -ENOENT;
 	}
 
-	sprintf(drv->firmware_name, "%s%s%s", name_pre, tag, ".ucode");
+	snprintf(drv->firmware_name, sizeof(drv->firmware_name), "%s%s.ucode",
+		 name_pre, tag);
 
 	IWL_DEBUG_INFO(drv, "attempting to load firmware %s'%s'\n",
 		       (drv->fw_index == UCODE_EXPERIMENTAL_INDEX)
@@ -403,6 +404,38 @@ static int iwl_set_default_calib(struct iwl_drv *drv, const u8 *data)
 	return 0;
 }
 
+static int iwl_set_ucode_api_flags(struct iwl_drv *drv, const u8 *data,
+				   struct iwl_ucode_capabilities *capa)
+{
+	const struct iwl_ucode_api *ucode_api = (void *)data;
+	u32 api_index = le32_to_cpu(ucode_api->api_index);
+
+	if (api_index >= IWL_API_ARRAY_SIZE) {
+		IWL_ERR(drv, "api_index larger than supported by driver\n");
+		return -EINVAL;
+	}
+
+	capa->api[api_index] = le32_to_cpu(ucode_api->api_flags);
+
+	return 0;
+}
+
+static int iwl_set_ucode_capabilities(struct iwl_drv *drv, const u8 *data,
+				      struct iwl_ucode_capabilities *capa)
+{
+	const struct iwl_ucode_capa *ucode_capa = (void *)data;
+	u32 api_index = le32_to_cpu(ucode_capa->api_index);
+
+	if (api_index >= IWL_CAPABILITIES_ARRAY_SIZE) {
+		IWL_ERR(drv, "api_index larger than supported by driver\n");
+		return -EINVAL;
+	}
+
+	capa->capa[api_index] = le32_to_cpu(ucode_capa->api_capa);
+
+	return 0;
+}
+
 static int iwl_parse_v1_v2_firmware(struct iwl_drv *drv,
 				    const struct firmware *ucode_raw,
 				    struct iwl_firmware_pieces *pieces)
@@ -637,6 +670,18 @@ static int iwl_parse_tlv_firmware(struct iwl_drv *drv,
 			 */
 			capa->flags = le32_to_cpup((__le32 *)tlv_data);
 			break;
+		case IWL_UCODE_TLV_API_CHANGES_SET:
+			if (tlv_len != sizeof(struct iwl_ucode_api))
+				goto invalid_tlv_len;
+			if (iwl_set_ucode_api_flags(drv, tlv_data, capa))
+				goto tlv_error;
+			break;
+		case IWL_UCODE_TLV_ENABLED_CAPABILITIES:
+			if (tlv_len != sizeof(struct iwl_ucode_capa))
+				goto invalid_tlv_len;
+			if (iwl_set_ucode_capabilities(drv, tlv_data, capa))
+				goto tlv_error;
+			break;
 		case IWL_UCODE_TLV_INIT_EVTLOG_PTR:
 			if (tlv_len != sizeof(u32))
 				goto invalid_tlv_len;
@@ -727,6 +772,12 @@ static int iwl_parse_tlv_firmware(struct iwl_drv *drv,
 			if (tlv_len != sizeof(u32))
 				goto invalid_tlv_len;
 			drv->fw.phy_config = le32_to_cpup((__le32 *)tlv_data);
+			drv->fw.valid_tx_ant = (drv->fw.phy_config &
+						FW_PHY_CFG_TX_CHAIN) >>
+						FW_PHY_CFG_TX_CHAIN_POS;
+			drv->fw.valid_rx_ant = (drv->fw.phy_config &
+						FW_PHY_CFG_RX_CHAIN) >>
+						FW_PHY_CFG_RX_CHAIN_POS;
 			break;
 		 case IWL_UCODE_TLV_SECURE_SEC_RT:
 			iwl_store_ucode_sec(pieces, tlv_data, IWL_UCODE_REGULAR,
@@ -1300,8 +1351,7 @@ MODULE_PARM_DESC(antenna_coupling,
 
 module_param_named(wd_disable, iwlwifi_mod_params.wd_disable, int, S_IRUGO);
 MODULE_PARM_DESC(wd_disable,
-		"Disable stuck queue watchdog timer 0=system default, "
-		"1=disable, 2=enable (default: 0)");
+		"Disable stuck queue watchdog timer 0=system default, 1=disable (default: 1)");
 
 module_param_named(nvm_file, iwlwifi_mod_params.nvm_file, charp, S_IRUGO);
 MODULE_PARM_DESC(nvm_file, "NVM file name");
diff --git a/drivers/net/wireless/iwlwifi/iwl-drv.h b/drivers/net/wireless/iwlwifi/iwl-drv.h
index 592c01e11013..3c72cb710b0c 100644
--- a/drivers/net/wireless/iwlwifi/iwl-drv.h
+++ b/drivers/net/wireless/iwlwifi/iwl-drv.h
@@ -70,6 +70,20 @@
 #define DRV_COPYRIGHT	"Copyright(c) 2003- 2014 Intel Corporation"
 #define DRV_AUTHOR     "<ilw@linux.intel.com>"
 
+/* radio config bits (actual values from NVM definition) */
+#define NVM_RF_CFG_DASH_MSK(x)   (x & 0x3)         /* bits 0-1   */
+#define NVM_RF_CFG_STEP_MSK(x)   ((x >> 2)  & 0x3) /* bits 2-3   */
+#define NVM_RF_CFG_TYPE_MSK(x)   ((x >> 4)  & 0x3) /* bits 4-5   */
+#define NVM_RF_CFG_PNUM_MSK(x)   ((x >> 6)  & 0x3) /* bits 6-7   */
+#define NVM_RF_CFG_TX_ANT_MSK(x) ((x >> 8)  & 0xF) /* bits 8-11  */
+#define NVM_RF_CFG_RX_ANT_MSK(x) ((x >> 12) & 0xF) /* bits 12-15 */
+
+#define NVM_RF_CFG_FLAVOR_MSK_FAMILY_8000(x)   (x & 0xF)
+#define NVM_RF_CFG_DASH_MSK_FAMILY_8000(x)   ((x >> 4) & 0xF)
+#define NVM_RF_CFG_STEP_MSK_FAMILY_8000(x)   ((x >> 8) & 0xF)
+#define NVM_RF_CFG_TYPE_MSK_FAMILY_8000(x)   ((x >> 12) & 0xFFF)
+#define NVM_RF_CFG_TX_ANT_MSK_FAMILY_8000(x) ((x >> 24) & 0xF)
+#define NVM_RF_CFG_RX_ANT_MSK_FAMILY_8000(x) ((x >> 28) & 0xF)
 
 /**
  * DOC: Driver system flows - drv component
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.h b/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.h
index e3c7deafabe6..f0548b8a64b0 100644
--- a/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.h
+++ b/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.h
@@ -81,16 +81,17 @@ struct iwl_nvm_data {
 	bool sku_cap_band_24GHz_enable;
 	bool sku_cap_band_52GHz_enable;
 	bool sku_cap_11n_enable;
+	bool sku_cap_11ac_enable;
 	bool sku_cap_amt_enable;
 	bool sku_cap_ipan_enable;
 
-	u8 radio_cfg_type;
+	u16 radio_cfg_type;
 	u8 radio_cfg_step;
 	u8 radio_cfg_dash;
 	u8 radio_cfg_pnum;
 	u8 valid_tx_ant, valid_rx_ant;
 
-	u16 nvm_version;
+	u32 nvm_version;
 	s8 max_tx_pwr_half_dbm;
 
 	struct ieee80211_supported_band bands[IEEE80211_NUM_BANDS];
diff --git a/drivers/net/wireless/iwlwifi/iwl-fw-file.h b/drivers/net/wireless/iwlwifi/iwl-fw-file.h
index 88e2d6eb569f..b45e576a4b57 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fw-file.h
+++ b/drivers/net/wireless/iwlwifi/iwl-fw-file.h
@@ -126,6 +126,8 @@ enum iwl_ucode_tlv_type {
 	IWL_UCODE_TLV_SECURE_SEC_WOWLAN	= 26,
 	IWL_UCODE_TLV_NUM_OF_CPU	= 27,
 	IWL_UCODE_TLV_CSCHEME		= 28,
+	IWL_UCODE_TLV_API_CHANGES_SET	= 29,
+	IWL_UCODE_TLV_ENABLED_CAPABILITIES	= 30,
 };
 
 struct iwl_ucode_tlv {
@@ -158,4 +160,19 @@ struct iwl_tlv_ucode_header {
 	u8 data[0];
 };
 
+/*
+ * ucode TLVs
+ *
+ * ability to get extension for: flags & capabilities from ucode binaries files
+ */
+struct iwl_ucode_api {
+	__le32 api_index;
+	__le32 api_flags;
+} __packed;
+
+struct iwl_ucode_capa {
+	__le32 api_index;
+	__le32 api_capa;
+} __packed;
+
 #endif  /* __iwl_fw_file_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-fw.h b/drivers/net/wireless/iwlwifi/iwl-fw.h
index 5f1493c44097..f04ff871dc6d 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fw.h
+++ b/drivers/net/wireless/iwlwifi/iwl-fw.h
@@ -92,9 +92,11 @@
  * @IWL_UCODE_TLV_FLAGS_STA_KEY_CMD: new ADD_STA and ADD_STA_KEY command API
  * @IWL_UCODE_TLV_FLAGS_DEVICE_PS_CMD: support device wide power command
  *	containing CAM (Continuous Active Mode) indication.
- * @IWL_UCODE_TLV_FLAGS_P2P_PS: P2P client power save is supported (only on a
- *	single bound interface).
+ * @IWL_UCODE_TLV_FLAGS_P2P_BSS_PS_DCM: support power save on BSS station and
+ *	P2P client interfaces simultaneously if they are in different bindings.
  * @IWL_UCODE_TLV_FLAGS_P2P_PS_UAPSD: P2P client supports uAPSD power save
+ * @IWL_UCODE_TLV_FLAGS_BCAST_FILTERING: uCode supports broadcast filtering.
+ * @IWL_UCODE_TLV_FLAGS_GO_UAPSD: AP/GO interfaces support uAPSD clients
  */
 enum iwl_ucode_tlv_flag {
 	IWL_UCODE_TLV_FLAGS_PAN			= BIT(0),
@@ -116,9 +118,11 @@ enum iwl_ucode_tlv_flag {
 	IWL_UCODE_TLV_FLAGS_SCHED_SCAN		= BIT(17),
 	IWL_UCODE_TLV_FLAGS_STA_KEY_CMD		= BIT(19),
 	IWL_UCODE_TLV_FLAGS_DEVICE_PS_CMD	= BIT(20),
-	IWL_UCODE_TLV_FLAGS_P2P_PS		= BIT(21),
+	IWL_UCODE_TLV_FLAGS_BSS_P2P_PS_DCM	= BIT(22),
 	IWL_UCODE_TLV_FLAGS_UAPSD_SUPPORT	= BIT(24),
 	IWL_UCODE_TLV_FLAGS_P2P_PS_UAPSD	= BIT(26),
+	IWL_UCODE_TLV_FLAGS_BCAST_FILTERING	= BIT(29),
+	IWL_UCODE_TLV_FLAGS_GO_UAPSD		= BIT(30),
 };
 
 /* The default calibrate table size if not specified by firmware file */
@@ -160,13 +164,16 @@ enum iwl_ucode_sec {
  * For 16.0 uCode and above, there is no differentiation between sections,
  * just an offset to the HW address.
  */
-#define IWL_UCODE_SECTION_MAX 6
-#define IWL_UCODE_FIRST_SECTION_OF_SECOND_CPU	(IWL_UCODE_SECTION_MAX/2)
+#define IWL_UCODE_SECTION_MAX 12
+#define IWL_API_ARRAY_SIZE	1
+#define IWL_CAPABILITIES_ARRAY_SIZE	1
 
 struct iwl_ucode_capabilities {
 	u32 max_probe_length;
 	u32 standard_phy_calibration_size;
 	u32 flags;
+	u32 api[IWL_API_ARRAY_SIZE];
+	u32 capa[IWL_CAPABILITIES_ARRAY_SIZE];
 };
 
 /* one for each uCode image (inst/data, init/runtime/wowlan) */
@@ -285,22 +292,12 @@ struct iwl_fw {
 
 	struct iwl_tlv_calib_ctrl default_calib[IWL_UCODE_TYPE_MAX];
 	u32 phy_config;
+	u8 valid_tx_ant;
+	u8 valid_rx_ant;
 
 	bool mvm_fw;
 
 	struct ieee80211_cipher_scheme cs[IWL_UCODE_MAX_CS];
 };
 
-static inline u8 iwl_fw_valid_tx_ant(const struct iwl_fw *fw)
-{
-	return (fw->phy_config & FW_PHY_CFG_TX_CHAIN) >>
-		FW_PHY_CFG_TX_CHAIN_POS;
-}
-
-static inline u8 iwl_fw_valid_rx_ant(const struct iwl_fw *fw)
-{
-	return (fw->phy_config & FW_PHY_CFG_RX_CHAIN) >>
-		FW_PHY_CFG_RX_CHAIN_POS;
-}
-
 #endif  /* __iwl_fw_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-io.c b/drivers/net/wireless/iwlwifi/iwl-io.c
index f98175a0d35b..07372f2b0250 100644
--- a/drivers/net/wireless/iwlwifi/iwl-io.c
+++ b/drivers/net/wireless/iwlwifi/iwl-io.c
@@ -130,6 +130,21 @@ void iwl_write_prph(struct iwl_trans *trans, u32 ofs, u32 val)
 }
 IWL_EXPORT_SYMBOL(iwl_write_prph);
 
+int iwl_poll_prph_bit(struct iwl_trans *trans, u32 addr,
+		      u32 bits, u32 mask, int timeout)
+{
+	int t = 0;
+
+	do {
+		if ((iwl_read_prph(trans, addr) & mask) == (bits & mask))
+			return t;
+		udelay(IWL_POLL_INTERVAL);
+		t += IWL_POLL_INTERVAL;
+	} while (t < timeout);
+
+	return -ETIMEDOUT;
+}
+
 void iwl_set_bits_prph(struct iwl_trans *trans, u32 ofs, u32 mask)
 {
 	unsigned long flags;
diff --git a/drivers/net/wireless/iwlwifi/iwl-io.h b/drivers/net/wireless/iwlwifi/iwl-io.h
index c339c1bed080..9e81b23d738b 100644
--- a/drivers/net/wireless/iwlwifi/iwl-io.h
+++ b/drivers/net/wireless/iwlwifi/iwl-io.h
@@ -72,6 +72,8 @@ void iwl_write_direct32(struct iwl_trans *trans, u32 reg, u32 value);
 
 u32 iwl_read_prph(struct iwl_trans *trans, u32 ofs);
 void iwl_write_prph(struct iwl_trans *trans, u32 ofs, u32 val);
+int iwl_poll_prph_bit(struct iwl_trans *trans, u32 addr,
+		      u32 bits, u32 mask, int timeout);
 void iwl_set_bits_prph(struct iwl_trans *trans, u32 ofs, u32 mask);
 void iwl_set_bits_mask_prph(struct iwl_trans *trans, u32 ofs,
 			    u32 bits, u32 mask);
diff --git a/drivers/net/wireless/iwlwifi/iwl-modparams.h b/drivers/net/wireless/iwlwifi/iwl-modparams.h
index b29075c3da8e..d994317db85b 100644
--- a/drivers/net/wireless/iwlwifi/iwl-modparams.h
+++ b/drivers/net/wireless/iwlwifi/iwl-modparams.h
@@ -96,7 +96,7 @@ enum iwl_disable_11n {
  *	use IWL_[DIS,EN]ABLE_HT_* constants
  * @amsdu_size_8K: enable 8K amsdu size, default = 0
  * @restart_fw: restart firmware, default = 1
- * @wd_disable: enable stuck queue check, default = 0
+ * @wd_disable: disable stuck queue check, default = 1
  * @bt_coex_active: enable bt coex, default = true
  * @led_mode: system default, default = 0
  * @power_save: disable power save, default = false
diff --git a/drivers/net/wireless/iwlwifi/iwl-nvm-parse.c b/drivers/net/wireless/iwlwifi/iwl-nvm-parse.c
index 725e954d8475..2f962ec0b750 100644
--- a/drivers/net/wireless/iwlwifi/iwl-nvm-parse.c
+++ b/drivers/net/wireless/iwlwifi/iwl-nvm-parse.c
@@ -71,7 +71,7 @@ enum wkp_nvm_offsets {
 	/* NVM HW-Section offset (in words) definitions */
 	HW_ADDR = 0x15,
 
-/* NVM SW-Section offset (in words) definitions */
+	/* NVM SW-Section offset (in words) definitions */
 	NVM_SW_SECTION = 0x1C0,
 	NVM_VERSION = 0,
 	RADIO_CFG = 1,
@@ -79,11 +79,32 @@ enum wkp_nvm_offsets {
 	N_HW_ADDRS = 3,
 	NVM_CHANNELS = 0x1E0 - NVM_SW_SECTION,
 
-/* NVM calibration section offset (in words) definitions */
+	/* NVM calibration section offset (in words) definitions */
 	NVM_CALIB_SECTION = 0x2B8,
 	XTAL_CALIB = 0x316 - NVM_CALIB_SECTION
 };
 
+enum family_8000_nvm_offsets {
+	/* NVM HW-Section offset (in words) definitions */
+	HW_ADDR0_FAMILY_8000 = 0x12,
+	HW_ADDR1_FAMILY_8000 = 0x16,
+	MAC_ADDRESS_OVERRIDE_FAMILY_8000 = 1,
+
+	/* NVM SW-Section offset (in words) definitions */
+	NVM_SW_SECTION_FAMILY_8000 = 0x1C0,
+	NVM_VERSION_FAMILY_8000 = 0,
+	RADIO_CFG_FAMILY_8000 = 2,
+	SKU_FAMILY_8000 = 4,
+	N_HW_ADDRS_FAMILY_8000 = 5,
+
+	/* NVM REGULATORY -Section offset (in words) definitions */
+	NVM_CHANNELS_FAMILY_8000 = 0,
+
+	/* NVM calibration section offset (in words) definitions */
+	NVM_CALIB_SECTION_FAMILY_8000 = 0x2B8,
+	XTAL_CALIB_FAMILY_8000 = 0x316 - NVM_CALIB_SECTION_FAMILY_8000
+};
+
 /* SKU Capabilities (actual values from NVM definition) */
 enum nvm_sku_bits {
 	NVM_SKU_CAP_BAND_24GHZ	= BIT(0),
@@ -92,14 +113,6 @@ enum nvm_sku_bits {
 	NVM_SKU_CAP_11AC_ENABLE	= BIT(3),
 };
 
-/* radio config bits (actual values from NVM definition) */
-#define NVM_RF_CFG_DASH_MSK(x)   (x & 0x3)         /* bits 0-1   */
-#define NVM_RF_CFG_STEP_MSK(x)   ((x >> 2)  & 0x3) /* bits 2-3   */
-#define NVM_RF_CFG_TYPE_MSK(x)   ((x >> 4)  & 0x3) /* bits 4-5   */
-#define NVM_RF_CFG_PNUM_MSK(x)   ((x >> 6)  & 0x3) /* bits 6-7   */
-#define NVM_RF_CFG_TX_ANT_MSK(x) ((x >> 8)  & 0xF) /* bits 8-11  */
-#define NVM_RF_CFG_RX_ANT_MSK(x) ((x >> 12) & 0xF) /* bits 12-15 */
-
 /*
  * These are the channel numbers in the order that they are stored in the NVM
  */
@@ -112,7 +125,17 @@ static const u8 iwl_nvm_channels[] = {
 	149, 153, 157, 161, 165
 };
 
+static const u8 iwl_nvm_channels_family_8000[] = {
+	/* 2.4 GHz */
+	1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13,
+	/* 5 GHz */
+	36, 40, 44, 48, 52, 56, 60, 64, 68, 72, 76, 80, 84, 88, 92,
+	96, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 144,
+	149, 153, 157, 161, 165, 169, 173, 177, 181
+};
+
 #define IWL_NUM_CHANNELS	ARRAY_SIZE(iwl_nvm_channels)
+#define IWL_NUM_CHANNELS_FAMILY_8000	ARRAY_SIZE(iwl_nvm_channels_family_8000)
 #define NUM_2GHZ_CHANNELS	14
 #define FIRST_2GHZ_HT_MINUS	5
 #define LAST_2GHZ_HT_PLUS	9
@@ -179,8 +202,18 @@ static int iwl_init_channel_map(struct device *dev, const struct iwl_cfg *cfg,
 	struct ieee80211_channel *channel;
 	u16 ch_flags;
 	bool is_5ghz;
+	int num_of_ch;
+	const u8 *nvm_chan;
+
+	if (cfg->device_family != IWL_DEVICE_FAMILY_8000) {
+		num_of_ch = IWL_NUM_CHANNELS;
+		nvm_chan = &iwl_nvm_channels[0];
+	} else {
+		num_of_ch = IWL_NUM_CHANNELS_FAMILY_8000;
+		nvm_chan = &iwl_nvm_channels_family_8000[0];
+	}
 
-	for (ch_idx = 0; ch_idx < IWL_NUM_CHANNELS; ch_idx++) {
+	for (ch_idx = 0; ch_idx < num_of_ch; ch_idx++) {
 		ch_flags = __le16_to_cpup(nvm_ch_flags + ch_idx);
 
 		if (ch_idx >= NUM_2GHZ_CHANNELS &&
@@ -190,7 +223,7 @@ static int iwl_init_channel_map(struct device *dev, const struct iwl_cfg *cfg,
 		if (!(ch_flags & NVM_CHANNEL_VALID)) {
 			IWL_DEBUG_EEPROM(dev,
 					 "Ch. %d Flags %x [%sGHz] - No traffic\n",
-					 iwl_nvm_channels[ch_idx],
+					 nvm_chan[ch_idx],
 					 ch_flags,
 					 (ch_idx >= NUM_2GHZ_CHANNELS) ?
 					 "5.2" : "2.4");
@@ -200,7 +233,7 @@ static int iwl_init_channel_map(struct device *dev, const struct iwl_cfg *cfg,
 		channel = &data->channels[n_channels];
 		n_channels++;
 
-		channel->hw_value = iwl_nvm_channels[ch_idx];
+		channel->hw_value = nvm_chan[ch_idx];
 		channel->band = (ch_idx < NUM_2GHZ_CHANNELS) ?
 				IEEE80211_BAND_2GHZ : IEEE80211_BAND_5GHZ;
 		channel->center_freq =
@@ -211,11 +244,11 @@ static int iwl_init_channel_map(struct device *dev, const struct iwl_cfg *cfg,
 		channel->flags = IEEE80211_CHAN_NO_HT40;
 		if (ch_idx < NUM_2GHZ_CHANNELS &&
 		    (ch_flags & NVM_CHANNEL_40MHZ)) {
-			if (iwl_nvm_channels[ch_idx] <= LAST_2GHZ_HT_PLUS)
+			if (nvm_chan[ch_idx] <= LAST_2GHZ_HT_PLUS)
 				channel->flags &= ~IEEE80211_CHAN_NO_HT40PLUS;
-			if (iwl_nvm_channels[ch_idx] >= FIRST_2GHZ_HT_MINUS)
+			if (nvm_chan[ch_idx] >= FIRST_2GHZ_HT_MINUS)
 				channel->flags &= ~IEEE80211_CHAN_NO_HT40MINUS;
-		} else if (iwl_nvm_channels[ch_idx] <= LAST_5GHZ_HT &&
+		} else if (nvm_chan[ch_idx] <= LAST_5GHZ_HT &&
 			   (ch_flags & NVM_CHANNEL_40MHZ)) {
 			if ((ch_idx - NUM_2GHZ_CHANNELS) % 2 == 0)
 				channel->flags &= ~IEEE80211_CHAN_NO_HT40PLUS;
@@ -307,14 +340,23 @@ static void iwl_init_vht_hw_capab(const struct iwl_cfg *cfg,
 }
 
 static void iwl_init_sbands(struct device *dev, const struct iwl_cfg *cfg,
-			    struct iwl_nvm_data *data, const __le16 *nvm_sw,
-			    bool enable_vht, u8 tx_chains, u8 rx_chains)
+			    struct iwl_nvm_data *data,
+			    const __le16 *ch_section, bool enable_vht,
+			    u8 tx_chains, u8 rx_chains)
 {
-	int n_channels = iwl_init_channel_map(dev, cfg, data,
-			&nvm_sw[NVM_CHANNELS]);
+	int n_channels;
 	int n_used = 0;
 	struct ieee80211_supported_band *sband;
 
+	if (cfg->device_family != IWL_DEVICE_FAMILY_8000)
+		n_channels = iwl_init_channel_map(
+				dev, cfg, data,
+				&ch_section[NVM_CHANNELS]);
+	else
+		n_channels = iwl_init_channel_map(
+				dev, cfg, data,
+				&ch_section[NVM_CHANNELS_FAMILY_8000]);
+
 	sband = &data->bands[IEEE80211_BAND_2GHZ];
 	sband->band = IEEE80211_BAND_2GHZ;
 	sband->bitrates = &iwl_cfg80211_rates[RATES_24_OFFS];
@@ -340,73 +382,152 @@ static void iwl_init_sbands(struct device *dev, const struct iwl_cfg *cfg,
 			    n_used, n_channels);
 }
 
+static int iwl_get_sku(const struct iwl_cfg *cfg,
+		       const __le16 *nvm_sw)
+{
+	if (cfg->device_family != IWL_DEVICE_FAMILY_8000)
+		return le16_to_cpup(nvm_sw + SKU);
+	else
+		return le32_to_cpup((__le32 *)(nvm_sw + SKU_FAMILY_8000));
+}
+
+static int iwl_get_nvm_version(const struct iwl_cfg *cfg,
+			       const __le16 *nvm_sw)
+{
+	if (cfg->device_family != IWL_DEVICE_FAMILY_8000)
+		return le16_to_cpup(nvm_sw + NVM_VERSION);
+	else
+		return le32_to_cpup((__le32 *)(nvm_sw +
+					       NVM_VERSION_FAMILY_8000));
+}
+
+static int iwl_get_radio_cfg(const struct iwl_cfg *cfg,
+			     const __le16 *nvm_sw)
+{
+	if (cfg->device_family != IWL_DEVICE_FAMILY_8000)
+		return le16_to_cpup(nvm_sw + RADIO_CFG);
+	else
+		return le32_to_cpup((__le32 *)(nvm_sw + RADIO_CFG_FAMILY_8000));
+}
+
+#define N_HW_ADDRS_MASK_FAMILY_8000	0xF
+static int iwl_get_n_hw_addrs(const struct iwl_cfg *cfg,
+			      const __le16 *nvm_sw)
+{
+	if (cfg->device_family != IWL_DEVICE_FAMILY_8000)
+		return le16_to_cpup(nvm_sw + N_HW_ADDRS);
+	else
+		return le32_to_cpup((__le32 *)(nvm_sw + N_HW_ADDRS_FAMILY_8000))
+		       & N_HW_ADDRS_MASK_FAMILY_8000;
+}
+
+static void iwl_set_radio_cfg(const struct iwl_cfg *cfg,
+			      struct iwl_nvm_data *data,
+			      u32 radio_cfg)
+{
+	if (cfg->device_family != IWL_DEVICE_FAMILY_8000) {
+		data->radio_cfg_type = NVM_RF_CFG_TYPE_MSK(radio_cfg);
+		data->radio_cfg_step = NVM_RF_CFG_STEP_MSK(radio_cfg);
+		data->radio_cfg_dash = NVM_RF_CFG_DASH_MSK(radio_cfg);
+		data->radio_cfg_pnum = NVM_RF_CFG_PNUM_MSK(radio_cfg);
+		return;
+	}
+
+	/* set the radio configuration for family 8000 */
+	data->radio_cfg_type = NVM_RF_CFG_TYPE_MSK_FAMILY_8000(radio_cfg);
+	data->radio_cfg_step = NVM_RF_CFG_STEP_MSK_FAMILY_8000(radio_cfg);
+	data->radio_cfg_dash = NVM_RF_CFG_DASH_MSK_FAMILY_8000(radio_cfg);
+	data->radio_cfg_pnum = NVM_RF_CFG_FLAVOR_MSK_FAMILY_8000(radio_cfg);
+}
+
+static void iwl_set_hw_address(const struct iwl_cfg *cfg,
+			       struct iwl_nvm_data *data,
+			       const __le16 *nvm_sec)
+{
+	u8 hw_addr[ETH_ALEN];
+
+	if (cfg->device_family != IWL_DEVICE_FAMILY_8000)
+		memcpy(hw_addr, nvm_sec + HW_ADDR, ETH_ALEN);
+	else
+		memcpy(hw_addr, nvm_sec + MAC_ADDRESS_OVERRIDE_FAMILY_8000,
+		       ETH_ALEN);
+
+	/* The byte order is little endian 16 bit, meaning 214365 */
+	data->hw_addr[0] = hw_addr[1];
+	data->hw_addr[1] = hw_addr[0];
+	data->hw_addr[2] = hw_addr[3];
+	data->hw_addr[3] = hw_addr[2];
+	data->hw_addr[4] = hw_addr[5];
+	data->hw_addr[5] = hw_addr[4];
+}
+
 struct iwl_nvm_data *
 iwl_parse_nvm_data(struct device *dev, const struct iwl_cfg *cfg,
 		   const __le16 *nvm_hw, const __le16 *nvm_sw,
-		   const __le16 *nvm_calib, u8 tx_chains, u8 rx_chains)
+		   const __le16 *nvm_calib, const __le16 *regulatory,
+		   const __le16 *mac_override, u8 tx_chains, u8 rx_chains)
 {
 	struct iwl_nvm_data *data;
-	u8 hw_addr[ETH_ALEN];
-	u16 radio_cfg, sku;
-
-	data = kzalloc(sizeof(*data) +
-		       sizeof(struct ieee80211_channel) * IWL_NUM_CHANNELS,
-		       GFP_KERNEL);
+	u32 sku;
+	u32 radio_cfg;
+
+	if (cfg->device_family != IWL_DEVICE_FAMILY_8000)
+		data = kzalloc(sizeof(*data) +
+			       sizeof(struct ieee80211_channel) *
+			       IWL_NUM_CHANNELS,
+			       GFP_KERNEL);
+	else
+		data = kzalloc(sizeof(*data) +
+			       sizeof(struct ieee80211_channel) *
+			       IWL_NUM_CHANNELS_FAMILY_8000,
+			       GFP_KERNEL);
 	if (!data)
 		return NULL;
 
-	data->nvm_version = le16_to_cpup(nvm_sw + NVM_VERSION);
+	data->nvm_version = iwl_get_nvm_version(cfg, nvm_sw);
 
-	radio_cfg = le16_to_cpup(nvm_sw + RADIO_CFG);
-	data->radio_cfg_type = NVM_RF_CFG_TYPE_MSK(radio_cfg);
-	data->radio_cfg_step = NVM_RF_CFG_STEP_MSK(radio_cfg);
-	data->radio_cfg_dash = NVM_RF_CFG_DASH_MSK(radio_cfg);
-	data->radio_cfg_pnum = NVM_RF_CFG_PNUM_MSK(radio_cfg);
-	data->valid_tx_ant = NVM_RF_CFG_TX_ANT_MSK(radio_cfg);
-	data->valid_rx_ant = NVM_RF_CFG_RX_ANT_MSK(radio_cfg);
+	radio_cfg = iwl_get_radio_cfg(cfg, nvm_sw);
+	iwl_set_radio_cfg(cfg, data, radio_cfg);
 
-	sku = le16_to_cpup(nvm_sw + SKU);
+	sku = iwl_get_sku(cfg, nvm_sw);
 	data->sku_cap_band_24GHz_enable = sku & NVM_SKU_CAP_BAND_24GHZ;
 	data->sku_cap_band_52GHz_enable = sku & NVM_SKU_CAP_BAND_52GHZ;
 	data->sku_cap_11n_enable = sku & NVM_SKU_CAP_11N_ENABLE;
+	data->sku_cap_11ac_enable = sku & NVM_SKU_CAP_11AC_ENABLE;
 	if (iwlwifi_mod_params.disable_11n & IWL_DISABLE_HT_ALL)
 		data->sku_cap_11n_enable = false;
 
-	/* check overrides (some devices have wrong NVM) */
-	if (cfg->valid_tx_ant)
-		data->valid_tx_ant = cfg->valid_tx_ant;
-	if (cfg->valid_rx_ant)
-		data->valid_rx_ant = cfg->valid_rx_ant;
+	data->n_hw_addrs = iwl_get_n_hw_addrs(cfg, nvm_sw);
 
-	if (!data->valid_tx_ant || !data->valid_rx_ant) {
-		IWL_ERR_DEV(dev, "invalid antennas (0x%x, 0x%x)\n",
-			    data->valid_tx_ant, data->valid_rx_ant);
-		kfree(data);
-		return NULL;
+	if (cfg->device_family != IWL_DEVICE_FAMILY_8000) {
+		/* Checking for required sections */
+		if (!nvm_calib) {
+			IWL_ERR_DEV(dev,
+				    "Can't parse empty Calib NVM sections\n");
+			kfree(data);
+			return NULL;
+		}
+		/* in family 8000 Xtal calibration values moved to OTP */
+		data->xtal_calib[0] = *(nvm_calib + XTAL_CALIB);
+		data->xtal_calib[1] = *(nvm_calib + XTAL_CALIB + 1);
 	}
 
-	data->n_hw_addrs = le16_to_cpup(nvm_sw + N_HW_ADDRS);
+	if (cfg->device_family != IWL_DEVICE_FAMILY_8000) {
+		iwl_set_hw_address(cfg, data, nvm_hw);
 
-	data->xtal_calib[0] = *(nvm_calib + XTAL_CALIB);
-	data->xtal_calib[1] = *(nvm_calib + XTAL_CALIB + 1);
+		iwl_init_sbands(dev, cfg, data, nvm_sw,
+				sku & NVM_SKU_CAP_11AC_ENABLE, tx_chains,
+				rx_chains);
+	} else {
+		/* MAC address in family 8000 */
+		iwl_set_hw_address(cfg, data, mac_override);
 
-	/* The byte order is little endian 16 bit, meaning 214365 */
-	memcpy(hw_addr, nvm_hw + HW_ADDR, ETH_ALEN);
-	data->hw_addr[0] = hw_addr[1];
-	data->hw_addr[1] = hw_addr[0];
-	data->hw_addr[2] = hw_addr[3];
-	data->hw_addr[3] = hw_addr[2];
-	data->hw_addr[4] = hw_addr[5];
-	data->hw_addr[5] = hw_addr[4];
-
-	iwl_init_sbands(dev, cfg, data, nvm_sw, sku & NVM_SKU_CAP_11AC_ENABLE,
-			tx_chains, rx_chains);
+		iwl_init_sbands(dev, cfg, data, regulatory,
+				sku & NVM_SKU_CAP_11AC_ENABLE, tx_chains,
+				rx_chains);
+	}
 
-	data->calib_version = 255;   /* TODO:
-					this value will prevent some checks from
-					failing, we need to check if this
-					field is still needed, and if it does,
-					where is it in the NVM*/
+	data->calib_version = 255;
 
 	return data;
 }
diff --git a/drivers/net/wireless/iwlwifi/iwl-nvm-parse.h b/drivers/net/wireless/iwlwifi/iwl-nvm-parse.h
index 0c4399aba8c6..c9c45a39d212 100644
--- a/drivers/net/wireless/iwlwifi/iwl-nvm-parse.h
+++ b/drivers/net/wireless/iwlwifi/iwl-nvm-parse.h
@@ -75,6 +75,7 @@
 struct iwl_nvm_data *
 iwl_parse_nvm_data(struct device *dev, const struct iwl_cfg *cfg,
 		   const __le16 *nvm_hw, const __le16 *nvm_sw,
-		   const __le16 *nvm_calib, u8 tx_chains, u8 rx_chains);
+		   const __le16 *nvm_calib, const __le16 *regulatory,
+		   const __le16 *mac_override, u8 tx_chains, u8 rx_chains);
 
 #endif /* __iwl_nvm_parse_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-op-mode.h b/drivers/net/wireless/iwlwifi/iwl-op-mode.h
index b5be51f3cd3d..5d78207040b0 100644
--- a/drivers/net/wireless/iwlwifi/iwl-op-mode.h
+++ b/drivers/net/wireless/iwlwifi/iwl-op-mode.h
@@ -131,6 +131,8 @@ struct iwl_cfg;
  * @nic_config: configure NIC, called before firmware is started.
  *	May sleep
  * @wimax_active: invoked when WiMax becomes active. May sleep
+ * @enter_d0i3: configure the fw to enter d0i3. May sleep.
+ * @exit_d0i3: configure the fw to exit d0i3. May sleep.
  */
 struct iwl_op_mode_ops {
 	struct iwl_op_mode *(*start)(struct iwl_trans *trans,
@@ -148,6 +150,8 @@ struct iwl_op_mode_ops {
 	void (*cmd_queue_full)(struct iwl_op_mode *op_mode);
 	void (*nic_config)(struct iwl_op_mode *op_mode);
 	void (*wimax_active)(struct iwl_op_mode *op_mode);
+	int (*enter_d0i3)(struct iwl_op_mode *op_mode);
+	int (*exit_d0i3)(struct iwl_op_mode *op_mode);
 };
 
 int iwl_opmode_register(const char *name, const struct iwl_op_mode_ops *ops);
@@ -155,7 +159,7 @@ void iwl_opmode_deregister(const char *name);
 
 /**
  * struct iwl_op_mode - operational mode
- * @ops - pointer to its own ops
+ * @ops: pointer to its own ops
  *
  * This holds an implementation of the mac80211 / fw API.
  */
@@ -226,4 +230,22 @@ static inline void iwl_op_mode_wimax_active(struct iwl_op_mode *op_mode)
 	op_mode->ops->wimax_active(op_mode);
 }
 
+static inline int iwl_op_mode_enter_d0i3(struct iwl_op_mode *op_mode)
+{
+	might_sleep();
+
+	if (!op_mode->ops->enter_d0i3)
+		return 0;
+	return op_mode->ops->enter_d0i3(op_mode);
+}
+
+static inline int iwl_op_mode_exit_d0i3(struct iwl_op_mode *op_mode)
+{
+	might_sleep();
+
+	if (!op_mode->ops->exit_d0i3)
+		return 0;
+	return op_mode->ops->exit_d0i3(op_mode);
+}
+
 #endif /* __iwl_op_mode_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-phy-db.c b/drivers/net/wireless/iwlwifi/iwl-phy-db.c
index fa77d63a277a..b761ac4822a3 100644
--- a/drivers/net/wireless/iwlwifi/iwl-phy-db.c
+++ b/drivers/net/wireless/iwlwifi/iwl-phy-db.c
@@ -72,7 +72,7 @@
 #include "iwl-trans.h"
 
 #define CHANNEL_NUM_SIZE	4	/* num of channels in calib_ch size */
-#define IWL_NUM_PAPD_CH_GROUPS	4
+#define IWL_NUM_PAPD_CH_GROUPS	7
 #define IWL_NUM_TXP_CH_GROUPS	9
 
 struct iwl_phy_db_entry {
@@ -383,7 +383,7 @@ static int iwl_phy_db_send_all_channel_groups(
 		if (!entry)
 			return -EINVAL;
 
-		if (WARN_ON_ONCE(!entry->size))
+		if (!entry->size)
 			continue;
 
 		/* Send the requested PHY DB section */
diff --git a/drivers/net/wireless/iwlwifi/iwl-prph.h b/drivers/net/wireless/iwlwifi/iwl-prph.h
index 100bd0d79681..9c90186d1744 100644
--- a/drivers/net/wireless/iwlwifi/iwl-prph.h
+++ b/drivers/net/wireless/iwlwifi/iwl-prph.h
@@ -105,6 +105,13 @@
 /* Device NMI register */
 #define DEVICE_SET_NMI_REG 0x00a01c30
 
+/*
+ * Device reset for family 8000
+ * write to bit 24 in order to reset the CPU
+*/
+#define RELEASE_CPU_RESET		(0x300C)
+#define RELEASE_CPU_RESET_BIT		BIT(24)
+
 /*****************************************************************************
  *                        7000/3000 series SHR DTS addresses                 *
  *****************************************************************************/
@@ -281,4 +288,43 @@ static inline unsigned int SCD_QUEUE_STATUS_BITS(unsigned int chnl)
 #define OSC_CLK				(0xa04068)
 #define OSC_CLK_FORCE_CONTROL		(0x8)
 
+/* SECURE boot registers */
+#define LMPM_SECURE_BOOT_CONFIG_ADDR	(0x100)
+enum secure_boot_config_reg {
+	LMPM_SECURE_BOOT_CONFIG_INSPECTOR_BURNED_IN_OTP	= 0x00000001,
+	LMPM_SECURE_BOOT_CONFIG_INSPECTOR_NOT_REQ	= 0x00000002,
+};
+
+#define LMPM_SECURE_BOOT_CPU1_STATUS_ADDR	(0x1E30)
+#define LMPM_SECURE_BOOT_CPU2_STATUS_ADDR	(0x1E34)
+enum secure_boot_status_reg {
+	LMPM_SECURE_BOOT_CPU_STATUS_VERF_STATUS		= 0x00000001,
+	LMPM_SECURE_BOOT_CPU_STATUS_VERF_COMPLETED	= 0x00000002,
+	LMPM_SECURE_BOOT_CPU_STATUS_VERF_SUCCESS	= 0x00000004,
+	LMPM_SECURE_BOOT_CPU_STATUS_VERF_FAIL		= 0x00000008,
+	LMPM_SECURE_BOOT_CPU_STATUS_SIGN_VERF_FAIL	= 0x00000010,
+	LMPM_SECURE_BOOT_STATUS_SUCCESS			= 0x00000003,
+};
+
+#define CSR_UCODE_LOAD_STATUS_ADDR	(0x1E70)
+enum secure_load_status_reg {
+	LMPM_CPU_UCODE_LOADING_STARTED			= 0x00000001,
+	LMPM_CPU_HDRS_LOADING_COMPLETED			= 0x00000003,
+	LMPM_CPU_UCODE_LOADING_COMPLETED		= 0x00000007,
+	LMPM_CPU_STATUS_NUM_OF_LAST_COMPLETED		= 0x000000F8,
+	LMPM_CPU_STATUS_NUM_OF_LAST_LOADED_BLOCK	= 0x0000FF00,
+};
+
+#define LMPM_SECURE_INSPECTOR_CODE_ADDR	(0x1E38)
+#define LMPM_SECURE_INSPECTOR_DATA_ADDR	(0x1E3C)
+#define LMPM_SECURE_UCODE_LOAD_CPU1_HDR_ADDR	(0x1E78)
+#define LMPM_SECURE_UCODE_LOAD_CPU2_HDR_ADDR	(0x1E7C)
+
+#define LMPM_SECURE_INSPECTOR_CODE_MEM_SPACE	(0x400000)
+#define LMPM_SECURE_INSPECTOR_DATA_MEM_SPACE	(0x402000)
+#define LMPM_SECURE_CPU1_HDR_MEM_SPACE		(0x420000)
+#define LMPM_SECURE_CPU2_HDR_MEM_SPACE		(0x420400)
+
+#define LMPM_SECURE_TIME_OUT	(100)
+
 #endif				/* __iwl_prph_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-trans.h b/drivers/net/wireless/iwlwifi/iwl-trans.h
index 1f065cf4a4ba..8cdb0dd618a6 100644
--- a/drivers/net/wireless/iwlwifi/iwl-trans.h
+++ b/drivers/net/wireless/iwlwifi/iwl-trans.h
@@ -193,12 +193,23 @@ static inline u32 iwl_rx_packet_payload_len(const struct iwl_rx_packet *pkt)
  * @CMD_ASYNC: Return right away and don't wait for the response
  * @CMD_WANT_SKB: valid only with CMD_SYNC. The caller needs the buffer of the
  *	response. The caller needs to call iwl_free_resp when done.
+ * @CMD_HIGH_PRIO: The command is high priority - it goes to the front of the
+ *	command queue, but after other high priority commands. valid only
+ *	with CMD_ASYNC.
+ * @CMD_SEND_IN_IDLE: The command should be sent even when the trans is idle.
+ * @CMD_MAKE_TRANS_IDLE: The command response should mark the trans as idle.
+ * @CMD_WAKE_UP_TRANS: The command response should wake up the trans
+ *	(i.e. mark it as non-idle).
  */
 enum CMD_MODE {
 	CMD_SYNC		= 0,
 	CMD_ASYNC		= BIT(0),
 	CMD_WANT_SKB		= BIT(1),
 	CMD_SEND_IN_RFKILL	= BIT(2),
+	CMD_HIGH_PRIO		= BIT(3),
+	CMD_SEND_IN_IDLE	= BIT(4),
+	CMD_MAKE_TRANS_IDLE	= BIT(5),
+	CMD_WAKE_UP_TRANS	= BIT(6),
 };
 
 #define DEF_CMD_PAYLOAD_SIZE 320
@@ -335,6 +346,9 @@ enum iwl_d3_status {
  * @STATUS_INT_ENABLED: interrupts are enabled
  * @STATUS_RFKILL: the HW RFkill switch is in KILL position
  * @STATUS_FW_ERROR: the fw is in error state
+ * @STATUS_TRANS_GOING_IDLE: shutting down the trans, only special commands
+ *	are sent
+ * @STATUS_TRANS_IDLE: the trans is idle - general commands are not to be sent
  */
 enum iwl_trans_status {
 	STATUS_SYNC_HCMD_ACTIVE,
@@ -343,6 +357,8 @@ enum iwl_trans_status {
 	STATUS_INT_ENABLED,
 	STATUS_RFKILL,
 	STATUS_FW_ERROR,
+	STATUS_TRANS_GOING_IDLE,
+	STATUS_TRANS_IDLE,
 };
 
 /**
@@ -377,7 +393,7 @@ struct iwl_trans_config {
 	bool rx_buf_size_8k;
 	bool bc_table_dword;
 	unsigned int queue_watchdog_timeout;
-	const char **command_names;
+	const char *const *command_names;
 };
 
 struct iwl_trans;
@@ -443,6 +459,11 @@ struct iwl_trans;
  * @release_nic_access: let the NIC go to sleep. The "flags" parameter
  *	must be the same one that was sent before to the grab_nic_access.
  * @set_bits_mask - set SRAM register according to value and mask.
+ * @ref: grab a reference to the transport/FW layers, disallowing
+ *	certain low power states
+ * @unref: release a reference previously taken with @ref. Note that
+ *	initially the reference count is 1, making an initial @unref
+ *	necessary to allow low power states.
  */
 struct iwl_trans_ops {
 
@@ -489,6 +510,8 @@ struct iwl_trans_ops {
 				   unsigned long *flags);
 	void (*set_bits_mask)(struct iwl_trans *trans, u32 reg, u32 mask,
 			      u32 value);
+	void (*ref)(struct iwl_trans *trans);
+	void (*unref)(struct iwl_trans *trans);
 };
 
 /**
@@ -523,6 +546,7 @@ enum iwl_trans_state {
  *	starting the firmware, used for tracing
  * @rx_mpdu_cmd_hdr_size: used for tracing, amount of data before the
  *	start of the 802.11 header in the @rx_mpdu_cmd
+ * @dflt_pwr_limit: default power limit fetched from the platform (ACPI)
  */
 struct iwl_trans {
 	const struct iwl_trans_ops *ops;
@@ -551,6 +575,8 @@ struct iwl_trans {
 	struct lockdep_map sync_cmd_lockdep_map;
 #endif
 
+	u64 dflt_pwr_limit;
+
 	/* pointer to trans specific struct */
 	/*Ensure that this pointer will always be aligned to sizeof pointer */
 	char trans_specific[0] __aligned(sizeof(void *));
@@ -627,6 +653,18 @@ static inline int iwl_trans_d3_resume(struct iwl_trans *trans,
 	return trans->ops->d3_resume(trans, status, test);
 }
 
+static inline void iwl_trans_ref(struct iwl_trans *trans)
+{
+	if (trans->ops->ref)
+		trans->ops->ref(trans);
+}
+
+static inline void iwl_trans_unref(struct iwl_trans *trans)
+{
+	if (trans->ops->unref)
+		trans->ops->unref(trans);
+}
+
 static inline int iwl_trans_send_cmd(struct iwl_trans *trans,
 				     struct iwl_host_cmd *cmd)
 {
diff --git a/drivers/net/wireless/iwlwifi/mvm/Makefile b/drivers/net/wireless/iwlwifi/mvm/Makefile
index f98ec2b23898..41d390fd2ac8 100644
--- a/drivers/net/wireless/iwlwifi/mvm/Makefile
+++ b/drivers/net/wireless/iwlwifi/mvm/Makefile
@@ -2,7 +2,7 @@ obj-$(CONFIG_IWLMVM)   += iwlmvm.o
 iwlmvm-y += fw.o mac80211.o nvm.o ops.o phy-ctxt.o mac-ctxt.o
 iwlmvm-y += utils.o rx.o tx.o binding.o quota.o sta.o sf.o
 iwlmvm-y += scan.o time-event.o rs.o
-iwlmvm-y += power.o power_legacy.o bt-coex.o
+iwlmvm-y += power.o bt-coex.o
 iwlmvm-y += led.o tt.o
 iwlmvm-$(CONFIG_IWLWIFI_DEBUGFS) += debugfs.o debugfs-vif.o
 iwlmvm-$(CONFIG_PM_SLEEP) += d3.o
diff --git a/drivers/net/wireless/iwlwifi/mvm/bt-coex.c b/drivers/net/wireless/iwlwifi/mvm/bt-coex.c
index 18a895a949d4..2aa3ee93c68e 100644
--- a/drivers/net/wireless/iwlwifi/mvm/bt-coex.c
+++ b/drivers/net/wireless/iwlwifi/mvm/bt-coex.c
@@ -378,7 +378,6 @@ int iwl_send_bt_init_conf(struct iwl_mvm *mvm)
 
 	flags = iwlwifi_mod_params.bt_coex_active ?
 			BT_COEX_NW : BT_COEX_DISABLE;
-	flags |= BT_CH_PRIMARY_EN | BT_CH_SECONDARY_EN | BT_SYNC_2_BT_DISABLE;
 	bt_cmd->flags = cpu_to_le32(flags);
 
 	bt_cmd->valid_bit_msk = cpu_to_le32(BT_VALID_ENABLE |
@@ -399,6 +398,9 @@ int iwl_send_bt_init_conf(struct iwl_mvm *mvm)
 					    BT_VALID_TXRX_MAX_FREQ_0 |
 					    BT_VALID_SYNC_TO_SCO);
 
+	if (IWL_MVM_BT_COEX_SYNC2SCO)
+		bt_cmd->flags |= cpu_to_le32(BT_COEX_SYNC2SCO);
+
 	if (mvm->cfg->bt_shared_single_ant)
 		memcpy(&bt_cmd->decision_lut, iwl_single_shared_ant,
 		       sizeof(iwl_single_shared_ant));
@@ -489,8 +491,7 @@ static int iwl_mvm_bt_udpate_ctrl_kill_msk(struct iwl_mvm *mvm,
 	return ret;
 }
 
-static int iwl_mvm_bt_coex_reduced_txp(struct iwl_mvm *mvm, u8 sta_id,
-				       bool enable)
+int iwl_mvm_bt_coex_reduced_txp(struct iwl_mvm *mvm, u8 sta_id, bool enable)
 {
 	struct iwl_bt_coex_cmd *bt_cmd;
 	/* Send ASYNC since this can be sent from an atomic context */
@@ -500,25 +501,16 @@ static int iwl_mvm_bt_coex_reduced_txp(struct iwl_mvm *mvm, u8 sta_id,
 		.dataflags = { IWL_HCMD_DFL_DUP, },
 		.flags = CMD_ASYNC,
 	};
-
-	struct ieee80211_sta *sta;
 	struct iwl_mvm_sta *mvmsta;
 	int ret;
 
-	if (sta_id == IWL_MVM_STATION_COUNT)
-		return 0;
-
-	sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[sta_id],
-					lockdep_is_held(&mvm->mutex));
-
-	/* This can happen if the station has been removed right now */
-	if (IS_ERR_OR_NULL(sta))
+	mvmsta = iwl_mvm_sta_from_staid_protected(mvm, sta_id);
+	if (!mvmsta)
 		return 0;
 
-	mvmsta = iwl_mvm_sta_from_mac80211(sta);
-
 	/* nothing to do */
-	if (mvmsta->bt_reduced_txpower == enable)
+	if (mvmsta->bt_reduced_txpower_dbg ||
+	    mvmsta->bt_reduced_txpower == enable)
 		return 0;
 
 	bt_cmd = kzalloc(sizeof(*bt_cmd), GFP_ATOMIC);
@@ -552,6 +544,7 @@ struct iwl_bt_iterator_data {
 	bool reduced_tx_power;
 	struct ieee80211_chanctx_conf *primary;
 	struct ieee80211_chanctx_conf *secondary;
+	bool primary_ll;
 };
 
 static inline
@@ -577,72 +570,113 @@ static void iwl_mvm_bt_notif_iterator(void *_data, u8 *mac,
 	struct iwl_mvm *mvm = data->mvm;
 	struct ieee80211_chanctx_conf *chanctx_conf;
 	enum ieee80211_smps_mode smps_mode;
+	u32 bt_activity_grading;
 	int ave_rssi;
 
 	lockdep_assert_held(&mvm->mutex);
 
-	if (vif->type != NL80211_IFTYPE_STATION &&
-	    vif->type != NL80211_IFTYPE_AP)
-		return;
+	switch (vif->type) {
+	case NL80211_IFTYPE_STATION:
+		/* default smps_mode for BSS / P2P client is AUTOMATIC */
+		smps_mode = IEEE80211_SMPS_AUTOMATIC;
+		data->num_bss_ifaces++;
 
-	smps_mode = IEEE80211_SMPS_AUTOMATIC;
+		/*
+		 * Count unassoc BSSes, relax SMSP constraints
+		 * and disable reduced Tx Power
+		 */
+		if (!vif->bss_conf.assoc) {
+			iwl_mvm_update_smps(mvm, vif, IWL_MVM_SMPS_REQ_BT_COEX,
+					    smps_mode);
+			if (iwl_mvm_bt_coex_reduced_txp(mvm,
+							mvmvif->ap_sta_id,
+							false))
+				IWL_ERR(mvm, "Couldn't send BT_CONFIG cmd\n");
+			return;
+		}
+		break;
+	case NL80211_IFTYPE_AP:
+		/* default smps_mode for AP / GO is OFF */
+		smps_mode = IEEE80211_SMPS_OFF;
+		if (!mvmvif->ap_ibss_active) {
+			iwl_mvm_update_smps(mvm, vif, IWL_MVM_SMPS_REQ_BT_COEX,
+					    smps_mode);
+			return;
+		}
+
+		/* the Ack / Cts kill mask must be default if AP / GO */
+		data->reduced_tx_power = false;
+		break;
+	default:
+		return;
+	}
 
 	chanctx_conf = rcu_dereference(vif->chanctx_conf);
 
 	/* If channel context is invalid or not on 2.4GHz .. */
 	if ((!chanctx_conf ||
 	     chanctx_conf->def.chan->band != IEEE80211_BAND_2GHZ)) {
-		/* ... and it is an associated STATION, relax constraints */
-		if (vif->type == NL80211_IFTYPE_STATION && vif->bss_conf.assoc)
-			iwl_mvm_update_smps(mvm, vif, IWL_MVM_SMPS_REQ_BT_COEX,
-					    smps_mode);
-		iwl_mvm_bt_coex_enable_rssi_event(mvm, vif, false, 0);
+		/* ... relax constraints and disable rssi events */
+		iwl_mvm_update_smps(mvm, vif, IWL_MVM_SMPS_REQ_BT_COEX,
+				    smps_mode);
+		if (vif->type == NL80211_IFTYPE_STATION)
+			iwl_mvm_bt_coex_enable_rssi_event(mvm, vif, false, 0);
 		return;
 	}
 
-	/* SoftAP / GO will always be primary */
-	if (vif->type == NL80211_IFTYPE_AP) {
-		if (!mvmvif->ap_ibss_active)
-			return;
+	bt_activity_grading = le32_to_cpu(data->notif->bt_activity_grading);
+	if (bt_activity_grading >= BT_HIGH_TRAFFIC)
+		smps_mode = IEEE80211_SMPS_STATIC;
+	else if (bt_activity_grading >= BT_LOW_TRAFFIC)
+		smps_mode = vif->type == NL80211_IFTYPE_AP ?
+				IEEE80211_SMPS_OFF :
+				IEEE80211_SMPS_DYNAMIC;
+	IWL_DEBUG_COEX(data->mvm,
+		       "mac %d: bt_status %d bt_activity_grading %d smps_req %d\n",
+		       mvmvif->id, data->notif->bt_status, bt_activity_grading,
+		       smps_mode);
 
-		/* the Ack / Cts kill mask must be default if AP / GO */
-		data->reduced_tx_power = false;
+	iwl_mvm_update_smps(mvm, vif, IWL_MVM_SMPS_REQ_BT_COEX, smps_mode);
 
-		if (chanctx_conf == data->primary)
-			return;
+	/* low latency is always primary */
+	if (iwl_mvm_vif_low_latency(mvmvif)) {
+		data->primary_ll = true;
 
-		/* downgrade the current primary no matter what its type is */
 		data->secondary = data->primary;
 		data->primary = chanctx_conf;
-		return;
 	}
 
-	data->num_bss_ifaces++;
+	if (vif->type == NL80211_IFTYPE_AP) {
+		if (!mvmvif->ap_ibss_active)
+			return;
 
-	/* we are now a STA / P2P Client, and take associated ones only */
-	if (!vif->bss_conf.assoc)
+		if (chanctx_conf == data->primary)
+			return;
+
+		if (!data->primary_ll) {
+			/*
+			 * downgrade the current primary no matter what its
+			 * type is.
+			 */
+			data->secondary = data->primary;
+			data->primary = chanctx_conf;
+		} else {
+			/* there is low latency vif - we will be secondary */
+			data->secondary = chanctx_conf;
+		}
 		return;
+	}
 
-	/* STA / P2P Client, try to be primary if first vif */
+	/*
+	 * STA / P2P Client, try to be primary if first vif. If we are in low
+	 * latency mode, we are already in primary and just don't do much
+	 */
 	if (!data->primary || data->primary == chanctx_conf)
 		data->primary = chanctx_conf;
 	else if (!data->secondary)
 		/* if secondary is not NULL, it might be a GO */
 		data->secondary = chanctx_conf;
 
-	if (le32_to_cpu(data->notif->bt_activity_grading) >= BT_HIGH_TRAFFIC)
-		smps_mode = IEEE80211_SMPS_STATIC;
-	else if (le32_to_cpu(data->notif->bt_activity_grading) >=
-		 BT_LOW_TRAFFIC)
-		smps_mode = IEEE80211_SMPS_DYNAMIC;
-
-	IWL_DEBUG_COEX(data->mvm,
-		       "mac %d: bt_status %d bt_activity_grading %d smps_req %d\n",
-		       mvmvif->id,  data->notif->bt_status,
-		       data->notif->bt_activity_grading, smps_mode);
-
-	iwl_mvm_update_smps(mvm, vif, IWL_MVM_SMPS_REQ_BT_COEX, smps_mode);
-
 	/* don't reduce the Tx power if in loose scheme */
 	if (iwl_get_coex_type(mvm, vif) == BT_COEX_LOOSE_LUT ||
 	    mvm->cfg->bt_shared_single_ant) {
diff --git a/drivers/net/wireless/iwlwifi/mvm/constants.h b/drivers/net/wireless/iwlwifi/mvm/constants.h
index 036857698565..2d133b1b2dde 100644
--- a/drivers/net/wireless/iwlwifi/mvm/constants.h
+++ b/drivers/net/wireless/iwlwifi/mvm/constants.h
@@ -78,5 +78,9 @@
 #define IWL_MVM_PS_SNOOZE_INTERVAL		25
 #define IWL_MVM_PS_SNOOZE_WINDOW		50
 #define IWL_MVM_WOWLAN_PS_SNOOZE_WINDOW		25
+#define IWL_MVM_LOWLAT_QUOTA_MIN_PERCENT	64
+#define IWL_MVM_LOWLAT_SINGLE_BINDING_MAXDUR	24 /* TU */
+#define IWL_MVM_LOWLAT_DUAL_BINDING_MAXDUR	24 /* TU */
+#define IWL_MVM_BT_COEX_SYNC2SCO		1
 
 #endif /* __MVM_CONSTANTS_H */
diff --git a/drivers/net/wireless/iwlwifi/mvm/d3.c b/drivers/net/wireless/iwlwifi/mvm/d3.c
index f36a7ee0267f..b956e2f0b631 100644
--- a/drivers/net/wireless/iwlwifi/mvm/d3.c
+++ b/drivers/net/wireless/iwlwifi/mvm/d3.c
@@ -963,7 +963,6 @@ static int __iwl_mvm_suspend(struct ieee80211_hw *hw,
 	};
 	int ret, i;
 	int len __maybe_unused;
-	u8 old_aux_sta_id, old_ap_sta_id = IWL_MVM_STATION_COUNT;
 
 	if (!wowlan) {
 		/*
@@ -980,8 +979,6 @@ static int __iwl_mvm_suspend(struct ieee80211_hw *hw,
 
 	mutex_lock(&mvm->mutex);
 
-	old_aux_sta_id = mvm->aux_sta.sta_id;
-
 	/* see if there's only a single BSS vif and it's associated */
 	ieee80211_iterate_active_interfaces_atomic(
 		mvm->hw, IEEE80211_IFACE_ITER_NORMAL,
@@ -1067,16 +1064,6 @@ static int __iwl_mvm_suspend(struct ieee80211_hw *hw,
 	iwl_trans_stop_device(mvm->trans);
 
 	/*
-	 * The D3 firmware still hardcodes the AP station ID for the
-	 * BSS we're associated with as 0. Store the real STA ID here
-	 * and assign 0. When we leave this function, we'll restore
-	 * the original value for the resume code.
-	 */
-	old_ap_sta_id = mvm_ap_sta->sta_id;
-	mvm_ap_sta->sta_id = 0;
-	mvmvif->ap_sta_id = 0;
-
-	/*
 	 * Set the HW restart bit -- this is mostly true as we're
 	 * going to load new firmware and reprogram that, though
 	 * the reprogramming is going to be manual to avoid adding
@@ -1096,16 +1083,6 @@ static int __iwl_mvm_suspend(struct ieee80211_hw *hw,
 	mvm->ptk_ivlen = 0;
 	mvm->ptk_icvlen = 0;
 
-	/*
-	 * The D3 firmware still hardcodes the AP station ID for the
-	 * BSS we're associated with as 0. As a result, we have to move
-	 * the auxiliary station to ID 1 so the ID 0 remains free for
-	 * the AP station for later.
-	 * We set the sta_id to 1 here, and reset it to its previous
-	 * value (that we stored above) later.
-	 */
-	mvm->aux_sta.sta_id = 1;
-
 	ret = iwl_mvm_load_d3_fw(mvm);
 	if (ret)
 		goto out;
@@ -1191,11 +1168,11 @@ static int __iwl_mvm_suspend(struct ieee80211_hw *hw,
 	if (ret)
 		goto out;
 
-	ret = iwl_mvm_power_update_device_mode(mvm);
+	ret = iwl_mvm_power_update_device(mvm);
 	if (ret)
 		goto out;
 
-	ret = iwl_mvm_power_update_mode(mvm, vif);
+	ret = iwl_mvm_power_update_mac(mvm, vif);
 	if (ret)
 		goto out;
 
@@ -1222,10 +1199,6 @@ static int __iwl_mvm_suspend(struct ieee80211_hw *hw,
 
 	iwl_trans_d3_suspend(mvm->trans, test);
  out:
-	mvm->aux_sta.sta_id = old_aux_sta_id;
-	mvm_ap_sta->sta_id = old_ap_sta_id;
-	mvmvif->ap_sta_id = old_ap_sta_id;
-
 	if (ret < 0)
 		ieee80211_restart_hw(mvm->hw);
  out_noreset:
diff --git a/drivers/net/wireless/iwlwifi/mvm/debugfs-vif.c b/drivers/net/wireless/iwlwifi/mvm/debugfs-vif.c
index 0e29cd83a06a..f64e972191eb 100644
--- a/drivers/net/wireless/iwlwifi/mvm/debugfs-vif.c
+++ b/drivers/net/wireless/iwlwifi/mvm/debugfs-vif.c
@@ -185,7 +185,7 @@ static ssize_t iwl_dbgfs_pm_params_write(struct ieee80211_vif *vif, char *buf,
 
 	mutex_lock(&mvm->mutex);
 	iwl_dbgfs_update_pm(mvm, vif, param, val);
-	ret = iwl_mvm_power_update_mode(mvm, vif);
+	ret = iwl_mvm_power_update_mac(mvm, vif);
 	mutex_unlock(&mvm->mutex);
 
 	return ret ?: count;
@@ -202,7 +202,7 @@ static ssize_t iwl_dbgfs_pm_params_read(struct file *file,
 	int bufsz = sizeof(buf);
 	int pos;
 
-	pos = iwl_mvm_power_dbgfs_read(mvm, vif, buf, bufsz);
+	pos = iwl_mvm_power_mac_dbgfs_read(mvm, vif, buf, bufsz);
 
 	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
 }
@@ -225,6 +225,29 @@ static ssize_t iwl_dbgfs_mac_params_read(struct file *file,
 
 	ap_sta_id = mvmvif->ap_sta_id;
 
+	switch (ieee80211_vif_type_p2p(vif)) {
+	case NL80211_IFTYPE_ADHOC:
+		pos += scnprintf(buf+pos, bufsz-pos, "type: ibss\n");
+		break;
+	case NL80211_IFTYPE_STATION:
+		pos += scnprintf(buf+pos, bufsz-pos, "type: bss\n");
+		break;
+	case NL80211_IFTYPE_AP:
+		pos += scnprintf(buf+pos, bufsz-pos, "type: ap\n");
+		break;
+	case NL80211_IFTYPE_P2P_CLIENT:
+		pos += scnprintf(buf+pos, bufsz-pos, "type: p2p client\n");
+		break;
+	case NL80211_IFTYPE_P2P_GO:
+		pos += scnprintf(buf+pos, bufsz-pos, "type: p2p go\n");
+		break;
+	case NL80211_IFTYPE_P2P_DEVICE:
+		pos += scnprintf(buf+pos, bufsz-pos, "type: p2p dev\n");
+		break;
+	default:
+		break;
+	}
+
 	pos += scnprintf(buf+pos, bufsz-pos, "mac id/color: %d / %d\n",
 			 mvmvif->id, mvmvif->color);
 	pos += scnprintf(buf+pos, bufsz-pos, "bssid: %pM\n",
@@ -249,9 +272,10 @@ static ssize_t iwl_dbgfs_mac_params_read(struct file *file,
 			struct iwl_mvm_sta *mvm_sta = (void *)sta->drv_priv;
 
 			pos += scnprintf(buf+pos, bufsz-pos,
-					 "ap_sta_id %d - reduced Tx power %d\n",
+					 "ap_sta_id %d - reduced Tx power %d force %d\n",
 					 ap_sta_id,
-					 mvm_sta->bt_reduced_txpower);
+					 mvm_sta->bt_reduced_txpower,
+					 mvm_sta->bt_reduced_txpower_dbg);
 		}
 	}
 
@@ -269,6 +293,36 @@ static ssize_t iwl_dbgfs_mac_params_read(struct file *file,
 	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
 }
 
+static ssize_t iwl_dbgfs_reduced_txp_write(struct ieee80211_vif *vif,
+					   char *buf, size_t count,
+					   loff_t *ppos)
+{
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+	struct iwl_mvm *mvm = mvmvif->mvm;
+	struct iwl_mvm_sta *mvmsta;
+	bool reduced_tx_power;
+	int ret;
+
+	if (mvmvif->ap_sta_id >= ARRAY_SIZE(mvm->fw_id_to_mac_id))
+		return -ENOTCONN;
+
+	if (strtobool(buf, &reduced_tx_power) != 0)
+		return -EINVAL;
+
+	mutex_lock(&mvm->mutex);
+
+	mvmsta = iwl_mvm_sta_from_staid_protected(mvm, mvmvif->ap_sta_id);
+	mvmsta->bt_reduced_txpower_dbg = false;
+	ret = iwl_mvm_bt_coex_reduced_txp(mvm, mvmvif->ap_sta_id,
+					  reduced_tx_power);
+	if (!ret)
+		mvmsta->bt_reduced_txpower_dbg = true;
+
+	mutex_unlock(&mvm->mutex);
+
+	return ret ? : count;
+}
+
 static void iwl_dbgfs_update_bf(struct ieee80211_vif *vif,
 				enum iwl_dbgfs_bf_mask param, int value)
 {
@@ -403,9 +457,9 @@ static ssize_t iwl_dbgfs_bf_params_write(struct ieee80211_vif *vif, char *buf,
 	mutex_lock(&mvm->mutex);
 	iwl_dbgfs_update_bf(vif, param, value);
 	if (param == MVM_DEBUGFS_BF_ENABLE_BEACON_FILTER && !value)
-		ret = iwl_mvm_disable_beacon_filter(mvm, vif);
+		ret = iwl_mvm_disable_beacon_filter(mvm, vif, CMD_SYNC);
 	else
-		ret = iwl_mvm_enable_beacon_filter(mvm, vif);
+		ret = iwl_mvm_enable_beacon_filter(mvm, vif, CMD_SYNC);
 	mutex_unlock(&mvm->mutex);
 
 	return ret ?: count;
@@ -460,6 +514,41 @@ static ssize_t iwl_dbgfs_bf_params_read(struct file *file,
 	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
 }
 
+static ssize_t iwl_dbgfs_low_latency_write(struct ieee80211_vif *vif, char *buf,
+					   size_t count, loff_t *ppos)
+{
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+	struct iwl_mvm *mvm = mvmvif->mvm;
+	u8 value;
+	int ret;
+
+	ret = kstrtou8(buf, 0, &value);
+	if (ret)
+		return ret;
+	if (value > 1)
+		return -EINVAL;
+
+	mutex_lock(&mvm->mutex);
+	iwl_mvm_update_low_latency(mvm, vif, value);
+	mutex_unlock(&mvm->mutex);
+
+	return count;
+}
+
+static ssize_t iwl_dbgfs_low_latency_read(struct file *file,
+					  char __user *user_buf,
+					  size_t count, loff_t *ppos)
+{
+	struct ieee80211_vif *vif = file->private_data;
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+	char buf[3];
+
+	buf[0] = mvmvif->low_latency ? '1' : '0';
+	buf[1] = '\n';
+	buf[2] = '\0';
+	return simple_read_from_buffer(user_buf, count, ppos, buf, sizeof(buf));
+}
+
 #define MVM_DEBUGFS_WRITE_FILE_OPS(name, bufsz) \
 	_MVM_DEBUGFS_WRITE_FILE_OPS(name, bufsz, struct ieee80211_vif)
 #define MVM_DEBUGFS_READ_WRITE_FILE_OPS(name, bufsz) \
@@ -473,6 +562,8 @@ static ssize_t iwl_dbgfs_bf_params_read(struct file *file,
 MVM_DEBUGFS_READ_FILE_OPS(mac_params);
 MVM_DEBUGFS_READ_WRITE_FILE_OPS(pm_params, 32);
 MVM_DEBUGFS_READ_WRITE_FILE_OPS(bf_params, 256);
+MVM_DEBUGFS_READ_WRITE_FILE_OPS(low_latency, 10);
+MVM_DEBUGFS_WRITE_FILE_OPS(reduced_txp, 10);
 
 void iwl_mvm_vif_dbgfs_register(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 {
@@ -496,15 +587,18 @@ void iwl_mvm_vif_dbgfs_register(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 		return;
 	}
 
-	if (iwlmvm_mod_params.power_scheme != IWL_POWER_SCHEME_CAM &&
+	if ((mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_PM_CMD_SUPPORT) &&
+	    iwlmvm_mod_params.power_scheme != IWL_POWER_SCHEME_CAM &&
 	    ((vif->type == NL80211_IFTYPE_STATION && !vif->p2p) ||
 	     (vif->type == NL80211_IFTYPE_STATION && vif->p2p &&
-	      mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_P2P_PS)))
+	      mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_BSS_P2P_PS_DCM)))
 		MVM_DEBUGFS_ADD_FILE_VIF(pm_params, mvmvif->dbgfs_dir, S_IWUSR |
 					 S_IRUSR);
 
-	MVM_DEBUGFS_ADD_FILE_VIF(mac_params, mvmvif->dbgfs_dir,
-				 S_IRUSR);
+	MVM_DEBUGFS_ADD_FILE_VIF(mac_params, mvmvif->dbgfs_dir, S_IRUSR);
+	MVM_DEBUGFS_ADD_FILE_VIF(reduced_txp, mvmvif->dbgfs_dir, S_IWUSR);
+	MVM_DEBUGFS_ADD_FILE_VIF(low_latency, mvmvif->dbgfs_dir,
+				 S_IRUSR | S_IWUSR);
 
 	if (vif->type == NL80211_IFTYPE_STATION && !vif->p2p &&
 	    mvmvif == mvm->bf_allowed_vif)
diff --git a/drivers/net/wireless/iwlwifi/mvm/debugfs.c b/drivers/net/wireless/iwlwifi/mvm/debugfs.c
index 369d4c90e669..e0ff43ed2482 100644
--- a/drivers/net/wireless/iwlwifi/mvm/debugfs.c
+++ b/drivers/net/wireless/iwlwifi/mvm/debugfs.c
@@ -90,7 +90,7 @@ static ssize_t iwl_dbgfs_tx_flush_write(struct iwl_mvm *mvm, char *buf,
 static ssize_t iwl_dbgfs_sta_drain_write(struct iwl_mvm *mvm, char *buf,
 					 size_t count, loff_t *ppos)
 {
-	struct ieee80211_sta *sta;
+	struct iwl_mvm_sta *mvmsta;
 	int sta_id, drain, ret;
 
 	if (!mvm->ucode_loaded || mvm->cur_ucode != IWL_UCODE_REGULAR)
@@ -105,13 +105,12 @@ static ssize_t iwl_dbgfs_sta_drain_write(struct iwl_mvm *mvm, char *buf,
 
 	mutex_lock(&mvm->mutex);
 
-	sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[sta_id],
-					lockdep_is_held(&mvm->mutex));
-	if (IS_ERR_OR_NULL(sta))
+	mvmsta = iwl_mvm_sta_from_staid_protected(mvm, sta_id);
+
+	if (!mvmsta)
 		ret = -ENOENT;
 	else
-		ret = iwl_mvm_drain_sta(mvm, (void *)sta->drv_priv, drain) ? :
-			count;
+		ret = iwl_mvm_drain_sta(mvm, mvmsta, drain) ? : count;
 
 	mutex_unlock(&mvm->mutex);
 
@@ -251,7 +250,7 @@ static ssize_t iwl_dbgfs_disable_power_off_write(struct iwl_mvm *mvm, char *buf,
 	}
 
 	mutex_lock(&mvm->mutex);
-	ret = iwl_mvm_power_update_device_mode(mvm);
+	ret = iwl_mvm_power_update_device(mvm);
 	mutex_unlock(&mvm->mutex);
 
 	return ret ?: count;
@@ -532,6 +531,76 @@ static ssize_t iwl_dbgfs_fw_rx_stats_read(struct file *file,
 }
 #undef PRINT_STAT_LE32
 
+static ssize_t iwl_dbgfs_frame_stats_read(struct iwl_mvm *mvm,
+					  char __user *user_buf, size_t count,
+					  loff_t *ppos,
+					  struct iwl_mvm_frame_stats *stats)
+{
+	char *buff;
+	int pos = 0, idx, i;
+	int ret;
+	size_t bufsz = 1024;
+
+	buff = kmalloc(bufsz, GFP_KERNEL);
+	if (!buff)
+		return -ENOMEM;
+
+	spin_lock_bh(&mvm->drv_stats_lock);
+	pos += scnprintf(buff + pos, bufsz - pos,
+			 "Legacy/HT/VHT\t:\t%d/%d/%d\n",
+			 stats->legacy_frames,
+			 stats->ht_frames,
+			 stats->vht_frames);
+	pos += scnprintf(buff + pos, bufsz - pos, "20/40/80\t:\t%d/%d/%d\n",
+			 stats->bw_20_frames,
+			 stats->bw_40_frames,
+			 stats->bw_80_frames);
+	pos += scnprintf(buff + pos, bufsz - pos, "NGI/SGI\t\t:\t%d/%d\n",
+			 stats->ngi_frames,
+			 stats->sgi_frames);
+	pos += scnprintf(buff + pos, bufsz - pos, "SISO/MIMO2\t:\t%d/%d\n",
+			 stats->siso_frames,
+			 stats->mimo2_frames);
+	pos += scnprintf(buff + pos, bufsz - pos, "FAIL/SCSS\t:\t%d/%d\n",
+			 stats->fail_frames,
+			 stats->success_frames);
+	pos += scnprintf(buff + pos, bufsz - pos, "MPDUs agg\t:\t%d\n",
+			 stats->agg_frames);
+	pos += scnprintf(buff + pos, bufsz - pos, "A-MPDUs\t\t:\t%d\n",
+			 stats->ampdu_count);
+	pos += scnprintf(buff + pos, bufsz - pos, "Avg MPDUs/A-MPDU:\t%d\n",
+			 stats->ampdu_count > 0 ?
+			 (stats->agg_frames / stats->ampdu_count) : 0);
+
+	pos += scnprintf(buff + pos, bufsz - pos, "Last Rates\n");
+
+	idx = stats->last_frame_idx - 1;
+	for (i = 0; i < ARRAY_SIZE(stats->last_rates); i++) {
+		idx = (idx + 1) % ARRAY_SIZE(stats->last_rates);
+		if (stats->last_rates[idx] == 0)
+			continue;
+		pos += scnprintf(buff + pos, bufsz - pos, "Rate[%d]: ",
+				 (int)(ARRAY_SIZE(stats->last_rates) - i));
+		pos += rs_pretty_print_rate(buff + pos, stats->last_rates[idx]);
+	}
+	spin_unlock_bh(&mvm->drv_stats_lock);
+
+	ret = simple_read_from_buffer(user_buf, count, ppos, buff, pos);
+	kfree(buff);
+
+	return ret;
+}
+
+static ssize_t iwl_dbgfs_drv_rx_stats_read(struct file *file,
+					   char __user *user_buf, size_t count,
+					   loff_t *ppos)
+{
+	struct iwl_mvm *mvm = file->private_data;
+
+	return iwl_dbgfs_frame_stats_read(mvm, user_buf, count, ppos,
+					  &mvm->drv_rx_stats);
+}
+
 static ssize_t iwl_dbgfs_fw_restart_write(struct iwl_mvm *mvm, char *buf,
 					  size_t count, loff_t *ppos)
 {
@@ -592,7 +661,7 @@ iwl_dbgfs_scan_ant_rxchain_write(struct iwl_mvm *mvm, char *buf,
 		return -EINVAL;
 	if (scan_rx_ant > ANT_ABC)
 		return -EINVAL;
-	if (scan_rx_ant & ~iwl_fw_valid_rx_ant(mvm->fw))
+	if (scan_rx_ant & ~mvm->fw->valid_rx_ant)
 		return -EINVAL;
 
 	mvm->scan_rx_ant = scan_rx_ant;
@@ -600,6 +669,187 @@ iwl_dbgfs_scan_ant_rxchain_write(struct iwl_mvm *mvm, char *buf,
 	return count;
 }
 
+#define ADD_TEXT(...) pos += scnprintf(buf + pos, bufsz - pos, __VA_ARGS__)
+#ifdef CONFIG_IWLWIFI_BCAST_FILTERING
+static ssize_t iwl_dbgfs_bcast_filters_read(struct file *file,
+					    char __user *user_buf,
+					    size_t count, loff_t *ppos)
+{
+	struct iwl_mvm *mvm = file->private_data;
+	struct iwl_bcast_filter_cmd cmd;
+	const struct iwl_fw_bcast_filter *filter;
+	char *buf;
+	int bufsz = 1024;
+	int i, j, pos = 0;
+	ssize_t ret;
+
+	buf = kzalloc(bufsz, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	mutex_lock(&mvm->mutex);
+	if (!iwl_mvm_bcast_filter_build_cmd(mvm, &cmd)) {
+		ADD_TEXT("None\n");
+		mutex_unlock(&mvm->mutex);
+		goto out;
+	}
+	mutex_unlock(&mvm->mutex);
+
+	for (i = 0; cmd.filters[i].attrs[0].mask; i++) {
+		filter = &cmd.filters[i];
+
+		ADD_TEXT("Filter [%d]:\n", i);
+		ADD_TEXT("\tDiscard=%d\n", filter->discard);
+		ADD_TEXT("\tFrame Type: %s\n",
+			 filter->frame_type ? "IPv4" : "Generic");
+
+		for (j = 0; j < ARRAY_SIZE(filter->attrs); j++) {
+			const struct iwl_fw_bcast_filter_attr *attr;
+
+			attr = &filter->attrs[j];
+			if (!attr->mask)
+				break;
+
+			ADD_TEXT("\tAttr [%d]: offset=%d (from %s), mask=0x%x, value=0x%x reserved=0x%x\n",
+				 j, attr->offset,
+				 attr->offset_type ? "IP End" :
+						     "Payload Start",
+				 be32_to_cpu(attr->mask),
+				 be32_to_cpu(attr->val),
+				 le16_to_cpu(attr->reserved1));
+		}
+	}
+out:
+	ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+	kfree(buf);
+	return ret;
+}
+
+static ssize_t iwl_dbgfs_bcast_filters_write(struct iwl_mvm *mvm, char *buf,
+					     size_t count, loff_t *ppos)
+{
+	int pos, next_pos;
+	struct iwl_fw_bcast_filter filter = {};
+	struct iwl_bcast_filter_cmd cmd;
+	u32 filter_id, attr_id, mask, value;
+	int err = 0;
+
+	if (sscanf(buf, "%d %hhi %hhi %n", &filter_id, &filter.discard,
+		   &filter.frame_type, &pos) != 3)
+		return -EINVAL;
+
+	if (filter_id >= ARRAY_SIZE(mvm->dbgfs_bcast_filtering.cmd.filters) ||
+	    filter.frame_type > BCAST_FILTER_FRAME_TYPE_IPV4)
+		return -EINVAL;
+
+	for (attr_id = 0; attr_id < ARRAY_SIZE(filter.attrs);
+	     attr_id++) {
+		struct iwl_fw_bcast_filter_attr *attr =
+				&filter.attrs[attr_id];
+
+		if (pos >= count)
+			break;
+
+		if (sscanf(&buf[pos], "%hhi %hhi %i %i %n",
+			   &attr->offset, &attr->offset_type,
+			   &mask, &value, &next_pos) != 4)
+			return -EINVAL;
+
+		attr->mask = cpu_to_be32(mask);
+		attr->val = cpu_to_be32(value);
+		if (mask)
+			filter.num_attrs++;
+
+		pos += next_pos;
+	}
+
+	mutex_lock(&mvm->mutex);
+	memcpy(&mvm->dbgfs_bcast_filtering.cmd.filters[filter_id],
+	       &filter, sizeof(filter));
+
+	/* send updated bcast filtering configuration */
+	if (mvm->dbgfs_bcast_filtering.override &&
+	    iwl_mvm_bcast_filter_build_cmd(mvm, &cmd))
+		err = iwl_mvm_send_cmd_pdu(mvm, BCAST_FILTER_CMD, CMD_SYNC,
+					   sizeof(cmd), &cmd);
+	mutex_unlock(&mvm->mutex);
+
+	return err ?: count;
+}
+
+static ssize_t iwl_dbgfs_bcast_filters_macs_read(struct file *file,
+						 char __user *user_buf,
+						 size_t count, loff_t *ppos)
+{
+	struct iwl_mvm *mvm = file->private_data;
+	struct iwl_bcast_filter_cmd cmd;
+	char *buf;
+	int bufsz = 1024;
+	int i, pos = 0;
+	ssize_t ret;
+
+	buf = kzalloc(bufsz, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	mutex_lock(&mvm->mutex);
+	if (!iwl_mvm_bcast_filter_build_cmd(mvm, &cmd)) {
+		ADD_TEXT("None\n");
+		mutex_unlock(&mvm->mutex);
+		goto out;
+	}
+	mutex_unlock(&mvm->mutex);
+
+	for (i = 0; i < ARRAY_SIZE(cmd.macs); i++) {
+		const struct iwl_fw_bcast_mac *mac = &cmd.macs[i];
+
+		ADD_TEXT("Mac [%d]: discard=%d attached_filters=0x%x\n",
+			 i, mac->default_discard, mac->attached_filters);
+	}
+out:
+	ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+	kfree(buf);
+	return ret;
+}
+
+static ssize_t iwl_dbgfs_bcast_filters_macs_write(struct iwl_mvm *mvm,
+						  char *buf, size_t count,
+						  loff_t *ppos)
+{
+	struct iwl_bcast_filter_cmd cmd;
+	struct iwl_fw_bcast_mac mac = {};
+	u32 mac_id, attached_filters;
+	int err = 0;
+
+	if (!mvm->bcast_filters)
+		return -ENOENT;
+
+	if (sscanf(buf, "%d %hhi %i", &mac_id, &mac.default_discard,
+		   &attached_filters) != 3)
+		return -EINVAL;
+
+	if (mac_id >= ARRAY_SIZE(cmd.macs) ||
+	    mac.default_discard > 1 ||
+	    attached_filters >= BIT(ARRAY_SIZE(cmd.filters)))
+		return -EINVAL;
+
+	mac.attached_filters = cpu_to_le16(attached_filters);
+
+	mutex_lock(&mvm->mutex);
+	memcpy(&mvm->dbgfs_bcast_filtering.cmd.macs[mac_id],
+	       &mac, sizeof(mac));
+
+	/* send updated bcast filtering configuration */
+	if (mvm->dbgfs_bcast_filtering.override &&
+	    iwl_mvm_bcast_filter_build_cmd(mvm, &cmd))
+		err = iwl_mvm_send_cmd_pdu(mvm, BCAST_FILTER_CMD, CMD_SYNC,
+					   sizeof(cmd), &cmd);
+	mutex_unlock(&mvm->mutex);
+
+	return err ?: count;
+}
+#endif
+
 #ifdef CONFIG_PM_SLEEP
 static ssize_t iwl_dbgfs_d3_sram_write(struct iwl_mvm *mvm, char *buf,
 				       size_t count, loff_t *ppos)
@@ -658,15 +908,117 @@ static ssize_t iwl_dbgfs_d3_sram_read(struct file *file, char __user *user_buf,
 }
 #endif
 
+#define PRINT_MVM_REF(ref) do {					\
+	if (test_bit(ref, mvm->ref_bitmap))			\
+		pos += scnprintf(buf + pos, bufsz - pos,	\
+				 "\t(0x%lx) %s\n",		\
+				 BIT(ref), #ref);		\
+} while (0)
+
+static ssize_t iwl_dbgfs_d0i3_refs_read(struct file *file,
+					char __user *user_buf,
+					size_t count, loff_t *ppos)
+{
+	struct iwl_mvm *mvm = file->private_data;
+	int pos = 0;
+	char buf[256];
+	const size_t bufsz = sizeof(buf);
+
+	pos += scnprintf(buf + pos, bufsz - pos, "taken mvm refs: 0x%lx\n",
+			 mvm->ref_bitmap[0]);
+
+	PRINT_MVM_REF(IWL_MVM_REF_UCODE_DOWN);
+	PRINT_MVM_REF(IWL_MVM_REF_SCAN);
+	PRINT_MVM_REF(IWL_MVM_REF_ROC);
+	PRINT_MVM_REF(IWL_MVM_REF_P2P_CLIENT);
+	PRINT_MVM_REF(IWL_MVM_REF_AP_IBSS);
+	PRINT_MVM_REF(IWL_MVM_REF_USER);
+
+	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+}
+
+static ssize_t iwl_dbgfs_d0i3_refs_write(struct iwl_mvm *mvm, char *buf,
+					 size_t count, loff_t *ppos)
+{
+	unsigned long value;
+	int ret;
+	bool taken;
+
+	ret = kstrtoul(buf, 10, &value);
+	if (ret < 0)
+		return ret;
+
+	mutex_lock(&mvm->mutex);
+
+	taken = test_bit(IWL_MVM_REF_USER, mvm->ref_bitmap);
+	if (value == 1 && !taken)
+		iwl_mvm_ref(mvm, IWL_MVM_REF_USER);
+	else if (value == 0 && taken)
+		iwl_mvm_unref(mvm, IWL_MVM_REF_USER);
+	else
+		ret = -EINVAL;
+
+	mutex_unlock(&mvm->mutex);
+
+	if (ret < 0)
+		return ret;
+	return count;
+}
+
 #define MVM_DEBUGFS_WRITE_FILE_OPS(name, bufsz) \
 	_MVM_DEBUGFS_WRITE_FILE_OPS(name, bufsz, struct iwl_mvm)
 #define MVM_DEBUGFS_READ_WRITE_FILE_OPS(name, bufsz) \
 	_MVM_DEBUGFS_READ_WRITE_FILE_OPS(name, bufsz, struct iwl_mvm)
-#define MVM_DEBUGFS_ADD_FILE(name, parent, mode) do {			\
-		if (!debugfs_create_file(#name, mode, parent, mvm,	\
+#define MVM_DEBUGFS_ADD_FILE_ALIAS(alias, name, parent, mode) do {	\
+		if (!debugfs_create_file(alias, mode, parent, mvm,	\
 					 &iwl_dbgfs_##name##_ops))	\
 			goto err;					\
 	} while (0)
+#define MVM_DEBUGFS_ADD_FILE(name, parent, mode) \
+	MVM_DEBUGFS_ADD_FILE_ALIAS(#name, name, parent, mode)
+
+static ssize_t
+iwl_dbgfs_prph_reg_read(struct file *file,
+			char __user *user_buf,
+			size_t count, loff_t *ppos)
+{
+	struct iwl_mvm *mvm = file->private_data;
+	int pos = 0;
+	char buf[32];
+	const size_t bufsz = sizeof(buf);
+
+	if (!mvm->dbgfs_prph_reg_addr)
+		return -EINVAL;
+
+	pos += scnprintf(buf + pos, bufsz - pos, "Reg 0x%x: (0x%x)\n",
+		mvm->dbgfs_prph_reg_addr,
+		iwl_read_prph(mvm->trans, mvm->dbgfs_prph_reg_addr));
+
+	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+}
+
+static ssize_t
+iwl_dbgfs_prph_reg_write(struct iwl_mvm *mvm, char *buf,
+			 size_t count, loff_t *ppos)
+{
+	u8 args;
+	u32 value;
+
+	args = sscanf(buf, "%i %i", &mvm->dbgfs_prph_reg_addr, &value);
+	/* if we only want to set the reg address - nothing more to do */
+	if (args == 1)
+		goto out;
+
+	/* otherwise, make sure we have both address and value */
+	if (args != 2)
+		return -EINVAL;
+
+	iwl_write_prph(mvm->trans, mvm->dbgfs_prph_reg_addr, value);
+out:
+	return count;
+}
+
+MVM_DEBUGFS_READ_WRITE_FILE_OPS(prph_reg, 64);
 
 /* Device wide debugfs entries */
 MVM_DEBUGFS_WRITE_FILE_OPS(tx_flush, 16);
@@ -677,9 +1029,16 @@ MVM_DEBUGFS_READ_FILE_OPS(bt_notif);
 MVM_DEBUGFS_READ_FILE_OPS(bt_cmd);
 MVM_DEBUGFS_READ_WRITE_FILE_OPS(disable_power_off, 64);
 MVM_DEBUGFS_READ_FILE_OPS(fw_rx_stats);
+MVM_DEBUGFS_READ_FILE_OPS(drv_rx_stats);
 MVM_DEBUGFS_WRITE_FILE_OPS(fw_restart, 10);
 MVM_DEBUGFS_WRITE_FILE_OPS(fw_nmi, 10);
 MVM_DEBUGFS_READ_WRITE_FILE_OPS(scan_ant_rxchain, 8);
+MVM_DEBUGFS_READ_WRITE_FILE_OPS(d0i3_refs, 8);
+
+#ifdef CONFIG_IWLWIFI_BCAST_FILTERING
+MVM_DEBUGFS_READ_WRITE_FILE_OPS(bcast_filters, 256);
+MVM_DEBUGFS_READ_WRITE_FILE_OPS(bcast_filters_macs, 256);
+#endif
 
 #ifdef CONFIG_PM_SLEEP
 MVM_DEBUGFS_READ_WRITE_FILE_OPS(d3_sram, 8);
@@ -687,6 +1046,7 @@ MVM_DEBUGFS_READ_WRITE_FILE_OPS(d3_sram, 8);
 
 int iwl_mvm_dbgfs_register(struct iwl_mvm *mvm, struct dentry *dbgfs_dir)
 {
+	struct dentry *bcast_dir __maybe_unused;
 	char buf[100];
 
 	mvm->debugfs_dir = dbgfs_dir;
@@ -701,10 +1061,33 @@ int iwl_mvm_dbgfs_register(struct iwl_mvm *mvm, struct dentry *dbgfs_dir)
 		MVM_DEBUGFS_ADD_FILE(disable_power_off, mvm->debugfs_dir,
 				     S_IRUSR | S_IWUSR);
 	MVM_DEBUGFS_ADD_FILE(fw_rx_stats, mvm->debugfs_dir, S_IRUSR);
+	MVM_DEBUGFS_ADD_FILE(drv_rx_stats, mvm->debugfs_dir, S_IRUSR);
 	MVM_DEBUGFS_ADD_FILE(fw_restart, mvm->debugfs_dir, S_IWUSR);
 	MVM_DEBUGFS_ADD_FILE(fw_nmi, mvm->debugfs_dir, S_IWUSR);
 	MVM_DEBUGFS_ADD_FILE(scan_ant_rxchain, mvm->debugfs_dir,
 			     S_IWUSR | S_IRUSR);
+	MVM_DEBUGFS_ADD_FILE(prph_reg, mvm->debugfs_dir, S_IWUSR | S_IRUSR);
+	MVM_DEBUGFS_ADD_FILE(d0i3_refs, mvm->debugfs_dir, S_IRUSR | S_IWUSR);
+
+#ifdef CONFIG_IWLWIFI_BCAST_FILTERING
+	if (mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_BCAST_FILTERING) {
+		bcast_dir = debugfs_create_dir("bcast_filtering",
+					       mvm->debugfs_dir);
+		if (!bcast_dir)
+			goto err;
+
+		if (!debugfs_create_bool("override", S_IRUSR | S_IWUSR,
+				bcast_dir,
+				&mvm->dbgfs_bcast_filtering.override))
+			goto err;
+
+		MVM_DEBUGFS_ADD_FILE_ALIAS("filters", bcast_filters,
+					   bcast_dir, S_IWUSR | S_IRUSR);
+		MVM_DEBUGFS_ADD_FILE_ALIAS("macs", bcast_filters_macs,
+					   bcast_dir, S_IWUSR | S_IRUSR);
+	}
+#endif
+
 #ifdef CONFIG_PM_SLEEP
 	MVM_DEBUGFS_ADD_FILE(d3_sram, mvm->debugfs_dir, S_IRUSR | S_IWUSR);
 	MVM_DEBUGFS_ADD_FILE(d3_test, mvm->debugfs_dir, S_IRUSR);
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-bt-coex.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-bt-coex.h
index 1b4e54d416b0..20b723d6270f 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-bt-coex.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-bt-coex.h
@@ -70,37 +70,24 @@
 
 /**
  * enum iwl_bt_coex_flags - flags for BT_COEX command
- * @BT_CH_PRIMARY_EN:
- * @BT_CH_SECONDARY_EN:
- * @BT_NOTIF_COEX_OFF:
  * @BT_COEX_MODE_POS:
  * @BT_COEX_MODE_MSK:
  * @BT_COEX_DISABLE:
  * @BT_COEX_2W:
  * @BT_COEX_3W:
  * @BT_COEX_NW:
- * @BT_USE_DEFAULTS:
- * @BT_SYNC_2_BT_DISABLE:
- * @BT_COEX_CORUNNING_TBL_EN:
+ * @BT_COEX_SYNC2SCO:
  *
  * The COEX_MODE must be set for each command. Even if it is not changed.
  */
 enum iwl_bt_coex_flags {
-	BT_CH_PRIMARY_EN		= BIT(0),
-	BT_CH_SECONDARY_EN		= BIT(1),
-	BT_NOTIF_COEX_OFF		= BIT(2),
 	BT_COEX_MODE_POS		= 3,
 	BT_COEX_MODE_MSK		= BITS(3) << BT_COEX_MODE_POS,
 	BT_COEX_DISABLE			= 0x0 << BT_COEX_MODE_POS,
 	BT_COEX_2W			= 0x1 << BT_COEX_MODE_POS,
 	BT_COEX_3W			= 0x2 << BT_COEX_MODE_POS,
 	BT_COEX_NW			= 0x3 << BT_COEX_MODE_POS,
-	BT_USE_DEFAULTS			= BIT(6),
-	BT_SYNC_2_BT_DISABLE		= BIT(7),
-	BT_COEX_CORUNNING_TBL_EN	= BIT(8),
-	BT_COEX_MPLUT_TBL_EN		= BIT(9),
-	/* Bit 10 is reserved */
-	BT_COEX_WF_PRIO_BOOST_CHECK_EN	= BIT(11),
+	BT_COEX_SYNC2SCO		= BIT(7),
 };
 
 /*
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-d3.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-d3.h
index 8415ff312d0e..521997669c99 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-d3.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-d3.h
@@ -231,8 +231,12 @@ enum iwl_wowlan_wakeup_filters {
 	IWL_WOWLAN_WAKEUP_RF_KILL_DEASSERT		= BIT(8),
 	IWL_WOWLAN_WAKEUP_REMOTE_LINK_LOSS		= BIT(9),
 	IWL_WOWLAN_WAKEUP_REMOTE_SIGNATURE_TABLE	= BIT(10),
-	/* BIT(11) reserved */
+	IWL_WOWLAN_WAKEUP_REMOTE_TCP_EXTERNAL		= BIT(11),
 	IWL_WOWLAN_WAKEUP_REMOTE_WAKEUP_PACKET		= BIT(12),
+	IWL_WOWLAN_WAKEUP_IOAC_MAGIC_PACKET		= BIT(13),
+	IWL_WOWLAN_WAKEUP_HOST_TIMER			= BIT(14),
+	IWL_WOWLAN_WAKEUP_RX_FRAME			= BIT(15),
+	IWL_WOWLAN_WAKEUP_BCN_FILTERING			= BIT(16),
 }; /* WOWLAN_WAKEUP_FILTER_API_E_VER_4 */
 
 struct iwl_wowlan_config_cmd {
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-power.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-power.h
index 884c08725308..cbbcd8e284e4 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-power.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-power.h
@@ -301,54 +301,65 @@ struct iwl_beacon_filter_cmd {
 
 /* Beacon filtering and beacon abort */
 #define IWL_BF_ENERGY_DELTA_DEFAULT 5
+#define IWL_BF_ENERGY_DELTA_D0I3 20
 #define IWL_BF_ENERGY_DELTA_MAX 255
 #define IWL_BF_ENERGY_DELTA_MIN 0
 
 #define IWL_BF_ROAMING_ENERGY_DELTA_DEFAULT 1
+#define IWL_BF_ROAMING_ENERGY_DELTA_D0I3 20
 #define IWL_BF_ROAMING_ENERGY_DELTA_MAX 255
 #define IWL_BF_ROAMING_ENERGY_DELTA_MIN 0
 
 #define IWL_BF_ROAMING_STATE_DEFAULT 72
+#define IWL_BF_ROAMING_STATE_D0I3 72
 #define IWL_BF_ROAMING_STATE_MAX 255
 #define IWL_BF_ROAMING_STATE_MIN 0
 
 #define IWL_BF_TEMP_THRESHOLD_DEFAULT 112
+#define IWL_BF_TEMP_THRESHOLD_D0I3 112
 #define IWL_BF_TEMP_THRESHOLD_MAX 255
 #define IWL_BF_TEMP_THRESHOLD_MIN 0
 
 #define IWL_BF_TEMP_FAST_FILTER_DEFAULT 1
+#define IWL_BF_TEMP_FAST_FILTER_D0I3 1
 #define IWL_BF_TEMP_FAST_FILTER_MAX 255
 #define IWL_BF_TEMP_FAST_FILTER_MIN 0
 
 #define IWL_BF_TEMP_SLOW_FILTER_DEFAULT 5
+#define IWL_BF_TEMP_SLOW_FILTER_D0I3 5
 #define IWL_BF_TEMP_SLOW_FILTER_MAX 255
 #define IWL_BF_TEMP_SLOW_FILTER_MIN 0
 
 #define IWL_BF_ENABLE_BEACON_FILTER_DEFAULT 1
 
 #define IWL_BF_DEBUG_FLAG_DEFAULT 0
+#define IWL_BF_DEBUG_FLAG_D0I3 0
 
 #define IWL_BF_ESCAPE_TIMER_DEFAULT 50
+#define IWL_BF_ESCAPE_TIMER_D0I3 1024
 #define IWL_BF_ESCAPE_TIMER_MAX 1024
 #define IWL_BF_ESCAPE_TIMER_MIN 0
 
 #define IWL_BA_ESCAPE_TIMER_DEFAULT 6
+#define IWL_BA_ESCAPE_TIMER_D0I3 6
 #define IWL_BA_ESCAPE_TIMER_D3 9
 #define IWL_BA_ESCAPE_TIMER_MAX 1024
 #define IWL_BA_ESCAPE_TIMER_MIN 0
 
 #define IWL_BA_ENABLE_BEACON_ABORT_DEFAULT 1
 
-#define IWL_BF_CMD_CONFIG_DEFAULTS					     \
-	.bf_energy_delta = cpu_to_le32(IWL_BF_ENERGY_DELTA_DEFAULT),	     \
-	.bf_roaming_energy_delta =					     \
-		cpu_to_le32(IWL_BF_ROAMING_ENERGY_DELTA_DEFAULT),	     \
-	.bf_roaming_state = cpu_to_le32(IWL_BF_ROAMING_STATE_DEFAULT),	     \
-	.bf_temp_threshold = cpu_to_le32(IWL_BF_TEMP_THRESHOLD_DEFAULT),     \
-	.bf_temp_fast_filter = cpu_to_le32(IWL_BF_TEMP_FAST_FILTER_DEFAULT), \
-	.bf_temp_slow_filter = cpu_to_le32(IWL_BF_TEMP_SLOW_FILTER_DEFAULT), \
-	.bf_debug_flag = cpu_to_le32(IWL_BF_DEBUG_FLAG_DEFAULT),	     \
-	.bf_escape_timer = cpu_to_le32(IWL_BF_ESCAPE_TIMER_DEFAULT),	     \
-	.ba_escape_timer = cpu_to_le32(IWL_BA_ESCAPE_TIMER_DEFAULT)
+#define IWL_BF_CMD_CONFIG(mode)					     \
+	.bf_energy_delta = cpu_to_le32(IWL_BF_ENERGY_DELTA ## mode),	      \
+	.bf_roaming_energy_delta =					      \
+		cpu_to_le32(IWL_BF_ROAMING_ENERGY_DELTA ## mode),	      \
+	.bf_roaming_state = cpu_to_le32(IWL_BF_ROAMING_STATE ## mode),	      \
+	.bf_temp_threshold = cpu_to_le32(IWL_BF_TEMP_THRESHOLD ## mode),      \
+	.bf_temp_fast_filter = cpu_to_le32(IWL_BF_TEMP_FAST_FILTER ## mode),  \
+	.bf_temp_slow_filter = cpu_to_le32(IWL_BF_TEMP_SLOW_FILTER ## mode),  \
+	.bf_debug_flag = cpu_to_le32(IWL_BF_DEBUG_FLAG ## mode),	      \
+	.bf_escape_timer = cpu_to_le32(IWL_BF_ESCAPE_TIMER ## mode),	      \
+	.ba_escape_timer = cpu_to_le32(IWL_BA_ESCAPE_TIMER ## mode)
 
+#define IWL_BF_CMD_CONFIG_DEFAULTS IWL_BF_CMD_CONFIG(_DEFAULT)
+#define IWL_BF_CMD_CONFIG_D0I3 IWL_BF_CMD_CONFIG(_D0I3)
 #endif
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-rs.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-rs.h
index 85057219cc43..39148b5bb332 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-rs.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-rs.h
@@ -257,7 +257,8 @@ enum {
 
 /* Bit 17-18: (0) SS, (1) SS*2 */
 #define RATE_MCS_STBC_POS		17
-#define RATE_MCS_STBC_MSK		(1 << RATE_MCS_STBC_POS)
+#define RATE_MCS_HT_STBC_MSK		(3 << RATE_MCS_STBC_POS)
+#define RATE_MCS_VHT_STBC_MSK		(1 << RATE_MCS_STBC_POS)
 
 /* Bit 19: (0) Beamforming is off, (1) Beamforming is on */
 #define RATE_MCS_BF_POS			19
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-sta.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-sta.h
index 1b60fdff6a56..d63647867262 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-sta.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-sta.h
@@ -199,11 +199,14 @@ enum iwl_sta_modify_flag {
  * @STA_SLEEP_STATE_AWAKE:
  * @STA_SLEEP_STATE_PS_POLL:
  * @STA_SLEEP_STATE_UAPSD:
+ * @STA_SLEEP_STATE_MOREDATA: set more-data bit on
+ *	(last) released frame
  */
 enum iwl_sta_sleep_flag {
-	STA_SLEEP_STATE_AWAKE	= 0,
-	STA_SLEEP_STATE_PS_POLL	= BIT(0),
-	STA_SLEEP_STATE_UAPSD	= BIT(1),
+	STA_SLEEP_STATE_AWAKE		= 0,
+	STA_SLEEP_STATE_PS_POLL		= BIT(0),
+	STA_SLEEP_STATE_UAPSD		= BIT(1),
+	STA_SLEEP_STATE_MOREDATA	= BIT(2),
 };
 
 /* STA ID and color bits definitions */
@@ -318,13 +321,15 @@ struct iwl_mvm_add_sta_cmd_v5 {
 } __packed; /* ADD_STA_CMD_API_S_VER_5 */
 
 /**
- * struct iwl_mvm_add_sta_cmd_v6 - Add / modify a station
- * VER_6 of this command is quite similar to VER_5 except
+ * struct iwl_mvm_add_sta_cmd_v7 - Add / modify a station
+ * VER_7 of this command is quite similar to VER_5 except
  * exclusion of all fields related to the security key installation.
+ * It only differs from VER_6 by the "awake_acs" field that is
+ * reserved and ignored in VER_6.
  */
-struct iwl_mvm_add_sta_cmd_v6 {
+struct iwl_mvm_add_sta_cmd_v7 {
 	u8 add_modify;
-	u8 reserved1;
+	u8 awake_acs;
 	__le16 tid_disable_tx;
 	__le32 mac_id_n_color;
 	u8 addr[ETH_ALEN];	/* _STA_ID_MODIFY_INFO_API_S_VER_1 */
@@ -342,7 +347,7 @@ struct iwl_mvm_add_sta_cmd_v6 {
 	__le16 assoc_id;
 	__le16 beamform_flags;
 	__le32 tfd_queue_msk;
-} __packed; /* ADD_STA_CMD_API_S_VER_6 */
+} __packed; /* ADD_STA_CMD_API_S_VER_7 */
 
 /**
  * struct iwl_mvm_add_sta_key_cmd - add/modify sta key
@@ -432,5 +437,15 @@ struct iwl_mvm_wep_key_cmd {
 	struct iwl_mvm_wep_key wep_key[0];
 } __packed; /* SEC_CURR_WEP_KEY_CMD_API_S_VER_2 */
 
+/**
+ * struct iwl_mvm_eosp_notification - EOSP notification from firmware
+ * @remain_frame_count: # of frames remaining, non-zero if SP was cut
+ *	short by GO absence
+ * @sta_id: station ID
+ */
+struct iwl_mvm_eosp_notification {
+	__le32 remain_frame_count;
+	__le32 sta_id;
+} __packed; /* UAPSD_EOSP_NTFY_API_S_VER_1 */
 
 #endif /* __fw_api_sta_h__ */
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api.h b/drivers/net/wireless/iwlwifi/mvm/fw-api.h
index 989d7dbdca6c..807fa525cafe 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api.h
@@ -163,6 +163,7 @@ enum {
 	TX_ANT_CONFIGURATION_CMD = 0x98,
 	BT_CONFIG = 0x9b,
 	STATISTICS_NOTIFICATION = 0x9d,
+	EOSP_NOTIFICATION = 0x9e,
 	REDUCE_TX_POWER_CMD = 0x9f,
 
 	/* RF-KILL commands and notifications */
@@ -190,6 +191,7 @@ enum {
 	REPLY_DEBUG_CMD = 0xf0,
 	DEBUG_LOG_MSG = 0xf7,
 
+	BCAST_FILTER_CMD = 0xcf,
 	MCAST_FILTER_CMD = 0xd0,
 
 	/* D3 commands/notifications */
@@ -197,6 +199,7 @@ enum {
 	PROT_OFFLOAD_CONFIG_CMD = 0xd4,
 	OFFLOADS_QUERY_CMD = 0xd5,
 	REMOTE_WAKE_CONFIG_CMD = 0xd6,
+	D0I3_END_CMD = 0xed,
 
 	/* for WoWLAN in particular */
 	WOWLAN_PATTERNS = 0xe0,
@@ -313,14 +316,12 @@ enum {
 
 /* Section types for NVM_ACCESS_CMD */
 enum {
-	NVM_SECTION_TYPE_HW = 0,
-	NVM_SECTION_TYPE_SW,
-	NVM_SECTION_TYPE_PAPD,
-	NVM_SECTION_TYPE_BT,
-	NVM_SECTION_TYPE_CALIBRATION,
-	NVM_SECTION_TYPE_PRODUCTION,
-	NVM_SECTION_TYPE_POST_FCS_CALIB,
-	NVM_NUM_OF_SECTIONS,
+	NVM_SECTION_TYPE_SW = 1,
+	NVM_SECTION_TYPE_REGULATORY = 3,
+	NVM_SECTION_TYPE_CALIBRATION = 4,
+	NVM_SECTION_TYPE_PRODUCTION = 5,
+	NVM_SECTION_TYPE_MAC_OVERRIDE = 11,
+	NVM_MAX_NUM_SECTIONS = 12,
 };
 
 /**
@@ -412,6 +413,35 @@ struct mvm_alive_resp {
 	__le32 scd_base_ptr;		/* SRAM address for SCD */
 } __packed; /* ALIVE_RES_API_S_VER_1 */
 
+struct mvm_alive_resp_ver2 {
+	__le16 status;
+	__le16 flags;
+	u8 ucode_minor;
+	u8 ucode_major;
+	__le16 id;
+	u8 api_minor;
+	u8 api_major;
+	u8 ver_subtype;
+	u8 ver_type;
+	u8 mac;
+	u8 opt;
+	__le16 reserved2;
+	__le32 timestamp;
+	__le32 error_event_table_ptr;	/* SRAM address for error log */
+	__le32 log_event_table_ptr;	/* SRAM address for LMAC event log */
+	__le32 cpu_register_ptr;
+	__le32 dbgm_config_ptr;
+	__le32 alive_counter_ptr;
+	__le32 scd_base_ptr;		/* SRAM address for SCD */
+	__le32 st_fwrd_addr;		/* pointer to Store and forward */
+	__le32 st_fwrd_size;
+	u8 umac_minor;			/* UMAC version: minor */
+	u8 umac_major;			/* UMAC version: major */
+	__le16 umac_id;			/* UMAC version: id */
+	__le32 error_info_addr;		/* SRAM address for UMAC error log */
+	__le32 dbg_print_buff_addr;
+} __packed; /* ALIVE_RES_API_S_VER_2 */
+
 /* Error response/notification */
 enum {
 	FW_ERR_UNKNOWN_CMD = 0x0,
@@ -682,6 +712,7 @@ enum {
 	TE_V2_NOTIF_HOST_FRAG_END = BIT(5),
 	TE_V2_NOTIF_INTERNAL_FRAG_START = BIT(6),
 	TE_V2_NOTIF_INTERNAL_FRAG_END = BIT(7),
+	T2_V2_START_IMMEDIATELY = BIT(11),
 
 	TE_V2_NOTIF_MSK = 0xff,
 
@@ -1159,6 +1190,90 @@ struct iwl_mcast_filter_cmd {
 	u8 addr_list[0];
 } __packed; /* MCAST_FILTERING_CMD_API_S_VER_1 */
 
+#define MAX_BCAST_FILTERS 8
+#define MAX_BCAST_FILTER_ATTRS 2
+
+/**
+ * enum iwl_mvm_bcast_filter_attr_offset - written by fw for each Rx packet
+ * @BCAST_FILTER_OFFSET_PAYLOAD_START: offset is from payload start.
+ * @BCAST_FILTER_OFFSET_IP_END: offset is from ip header end (i.e.
+ *	start of ip payload).
+ */
+enum iwl_mvm_bcast_filter_attr_offset {
+	BCAST_FILTER_OFFSET_PAYLOAD_START = 0,
+	BCAST_FILTER_OFFSET_IP_END = 1,
+};
+
+/**
+ * struct iwl_fw_bcast_filter_attr - broadcast filter attribute
+ * @offset_type:	&enum iwl_mvm_bcast_filter_attr_offset.
+ * @offset:	starting offset of this pattern.
+ * @val:		value to match - big endian (MSB is the first
+ *		byte to match from offset pos).
+ * @mask:	mask to match (big endian).
+ */
+struct iwl_fw_bcast_filter_attr {
+	u8 offset_type;
+	u8 offset;
+	__le16 reserved1;
+	__be32 val;
+	__be32 mask;
+} __packed; /* BCAST_FILTER_ATT_S_VER_1 */
+
+/**
+ * enum iwl_mvm_bcast_filter_frame_type - filter frame type
+ * @BCAST_FILTER_FRAME_TYPE_ALL: consider all frames.
+ * @BCAST_FILTER_FRAME_TYPE_IPV4: consider only ipv4 frames
+ */
+enum iwl_mvm_bcast_filter_frame_type {
+	BCAST_FILTER_FRAME_TYPE_ALL = 0,
+	BCAST_FILTER_FRAME_TYPE_IPV4 = 1,
+};
+
+/**
+ * struct iwl_fw_bcast_filter - broadcast filter
+ * @discard: discard frame (1) or let it pass (0).
+ * @frame_type: &enum iwl_mvm_bcast_filter_frame_type.
+ * @num_attrs: number of valid attributes in this filter.
+ * @attrs: attributes of this filter. a filter is considered matched
+ *	only when all its attributes are matched (i.e. AND relationship)
+ */
+struct iwl_fw_bcast_filter {
+	u8 discard;
+	u8 frame_type;
+	u8 num_attrs;
+	u8 reserved1;
+	struct iwl_fw_bcast_filter_attr attrs[MAX_BCAST_FILTER_ATTRS];
+} __packed; /* BCAST_FILTER_S_VER_1 */
+
+/**
+ * struct iwl_fw_bcast_mac - per-mac broadcast filtering configuration.
+ * @default_discard: default action for this mac (discard (1) / pass (0)).
+ * @attached_filters: bitmap of relevant filters for this mac.
+ */
+struct iwl_fw_bcast_mac {
+	u8 default_discard;
+	u8 reserved1;
+	__le16 attached_filters;
+} __packed; /* BCAST_MAC_CONTEXT_S_VER_1 */
+
+/**
+ * struct iwl_bcast_filter_cmd - broadcast filtering configuration
+ * @disable: enable (0) / disable (1)
+ * @max_bcast_filters: max number of filters (MAX_BCAST_FILTERS)
+ * @max_macs: max number of macs (NUM_MAC_INDEX_DRIVER)
+ * @filters: broadcast filters
+ * @macs: broadcast filtering configuration per-mac
+ */
+struct iwl_bcast_filter_cmd {
+	u8 disable;
+	u8 max_bcast_filters;
+	u8 max_macs;
+	u8 reserved1;
+	struct iwl_fw_bcast_filter filters[MAX_BCAST_FILTERS];
+	struct iwl_fw_bcast_mac macs[NUM_MAC_INDEX_DRIVER];
+} __packed; /* BCAST_FILTERING_HCMD_API_S_VER_1 */
+
 struct mvm_statistics_dbg {
 	__le32 burst_check;
 	__le32 burst_count;
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw.c b/drivers/net/wireless/iwlwifi/mvm/fw.c
index c03d39541f9e..7ce20062f32d 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw.c
+++ b/drivers/net/wireless/iwlwifi/mvm/fw.c
@@ -110,18 +110,48 @@ static bool iwl_alive_fn(struct iwl_notif_wait_data *notif_wait,
 		container_of(notif_wait, struct iwl_mvm, notif_wait);
 	struct iwl_mvm_alive_data *alive_data = data;
 	struct mvm_alive_resp *palive;
-
-	palive = (void *)pkt->data;
-
-	mvm->error_event_table = le32_to_cpu(palive->error_event_table_ptr);
-	mvm->log_event_table = le32_to_cpu(palive->log_event_table_ptr);
-	alive_data->scd_base_addr = le32_to_cpu(palive->scd_base_ptr);
-
-	alive_data->valid = le16_to_cpu(palive->status) == IWL_ALIVE_STATUS_OK;
-	IWL_DEBUG_FW(mvm,
-		     "Alive ucode status 0x%04x revision 0x%01X 0x%01X flags 0x%01X\n",
-		     le16_to_cpu(palive->status), palive->ver_type,
-		     palive->ver_subtype, palive->flags);
+	struct mvm_alive_resp_ver2 *palive2;
+
+	if (iwl_rx_packet_payload_len(pkt) == sizeof(*palive)) {
+		palive = (void *)pkt->data;
+
+		mvm->support_umac_log = false;
+		mvm->error_event_table =
+			le32_to_cpu(palive->error_event_table_ptr);
+		mvm->log_event_table = le32_to_cpu(palive->log_event_table_ptr);
+		alive_data->scd_base_addr = le32_to_cpu(palive->scd_base_ptr);
+
+		alive_data->valid = le16_to_cpu(palive->status) ==
+				    IWL_ALIVE_STATUS_OK;
+		IWL_DEBUG_FW(mvm,
+			     "Alive VER1 ucode status 0x%04x revision 0x%01X 0x%01X flags 0x%01X\n",
+			     le16_to_cpu(palive->status), palive->ver_type,
+			     palive->ver_subtype, palive->flags);
+	} else {
+		palive2 = (void *)pkt->data;
+
+		mvm->error_event_table =
+			le32_to_cpu(palive2->error_event_table_ptr);
+		mvm->log_event_table =
+			le32_to_cpu(palive2->log_event_table_ptr);
+		alive_data->scd_base_addr = le32_to_cpu(palive2->scd_base_ptr);
+		mvm->umac_error_event_table =
+			le32_to_cpu(palive2->error_info_addr);
+
+		alive_data->valid = le16_to_cpu(palive2->status) ==
+				    IWL_ALIVE_STATUS_OK;
+		if (mvm->umac_error_event_table)
+			mvm->support_umac_log = true;
+
+		IWL_DEBUG_FW(mvm,
+			     "Alive VER2 ucode status 0x%04x revision 0x%01X 0x%01X flags 0x%01X\n",
+			     le16_to_cpu(palive2->status), palive2->ver_type,
+			     palive2->ver_subtype, palive2->flags);
+
+		IWL_DEBUG_FW(mvm,
+			     "UMAC version: Major - 0x%x, Minor - 0x%x\n",
+			     palive2->umac_major, palive2->umac_minor);
+	}
 
 	return true;
 }
@@ -292,7 +322,7 @@ int iwl_run_init_mvm_ucode(struct iwl_mvm *mvm, bool read_nvm)
 	}
 
 	/* Send TX valid antennas before triggering calibrations */
-	ret = iwl_send_tx_ant_cfg(mvm, iwl_fw_valid_tx_ant(mvm->fw));
+	ret = iwl_send_tx_ant_cfg(mvm, mvm->fw->valid_tx_ant);
 	if (ret)
 		goto error;
 
@@ -328,8 +358,6 @@ out:
 					GFP_KERNEL);
 		if (!mvm->nvm_data)
 			return -ENOMEM;
-		mvm->nvm_data->valid_rx_ant = 1;
-		mvm->nvm_data->valid_tx_ant = 1;
 		mvm->nvm_data->bands[0].channels = mvm->nvm_data->channels;
 		mvm->nvm_data->bands[0].n_channels = 1;
 		mvm->nvm_data->bands[0].n_bitrates = 1;
@@ -341,8 +369,6 @@ out:
 	return ret;
 }
 
-#define UCODE_CALIB_TIMEOUT	(2*HZ)
-
 int iwl_mvm_up(struct iwl_mvm *mvm)
 {
 	int ret, i;
@@ -394,7 +420,7 @@ int iwl_mvm_up(struct iwl_mvm *mvm)
 	if (ret)
 		IWL_ERR(mvm, "Failed to initialize Smart Fifo\n");
 
-	ret = iwl_send_tx_ant_cfg(mvm, iwl_fw_valid_tx_ant(mvm->fw));
+	ret = iwl_send_tx_ant_cfg(mvm, mvm->fw->valid_tx_ant);
 	if (ret)
 		goto error;
 
@@ -439,10 +465,23 @@ int iwl_mvm_up(struct iwl_mvm *mvm)
 			goto error;
 	}
 
-	ret = iwl_mvm_power_update_device_mode(mvm);
+	/* Initialize tx backoffs to the minimal possible */
+	iwl_mvm_tt_tx_backoff(mvm, 0);
+
+	if (!(mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_PM_CMD_SUPPORT)) {
+		ret = iwl_power_legacy_set_cam_mode(mvm);
+		if (ret)
+			goto error;
+	}
+
+	ret = iwl_mvm_power_update_device(mvm);
 	if (ret)
 		goto error;
 
+	/* allow FW/transport low power modes if not during restart */
+	if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status))
+		iwl_mvm_unref(mvm, IWL_MVM_REF_UCODE_DOWN);
+
 	IWL_DEBUG_INFO(mvm, "RT uCode started.\n");
 	return 0;
  error:
@@ -466,7 +505,7 @@ int iwl_mvm_load_d3_fw(struct iwl_mvm *mvm)
 		goto error;
 	}
 
-	ret = iwl_send_tx_ant_cfg(mvm, iwl_fw_valid_tx_ant(mvm->fw));
+	ret = iwl_send_tx_ant_cfg(mvm, mvm->fw->valid_tx_ant);
 	if (ret)
 		goto error;
 
diff --git a/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c b/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c
index ba723d50939a..9ccec10bba16 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c
+++ b/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c
@@ -90,6 +90,7 @@ static void iwl_mvm_mac_tsf_id_iter(void *_data, u8 *mac,
 {
 	struct iwl_mvm_mac_iface_iterator_data *data = _data;
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+	u16 min_bi;
 
 	/* Skip the interface for which we are trying to assign a tsf_id  */
 	if (vif == data->vif)
@@ -114,42 +115,57 @@ static void iwl_mvm_mac_tsf_id_iter(void *_data, u8 *mac,
 	switch (data->vif->type) {
 	case NL80211_IFTYPE_STATION:
 		/*
-		 * The new interface is client, so if the existing one
-		 * we're iterating is an AP, and both interfaces have the
-		 * same beacon interval, the same TSF should be used to
-		 * avoid drift between the new client and existing AP,
-		 * the existing AP will get drift updates from the new
-		 * client context in this case
+		 * The new interface is a client, so if the one we're iterating
+		 * is an AP, and the beacon interval of the AP is a multiple or
+		 * divisor of the beacon interval of the client, the same TSF
+		 * should be used to avoid drift between the new client and
+		 * existing AP. The existing AP will get drift updates from the
+		 * new client context in this case.
 		 */
-		if (vif->type == NL80211_IFTYPE_AP) {
-			if (data->preferred_tsf == NUM_TSF_IDS &&
-			    test_bit(mvmvif->tsf_id, data->available_tsf_ids) &&
-			    (vif->bss_conf.beacon_int ==
-			     data->vif->bss_conf.beacon_int)) {
-				data->preferred_tsf = mvmvif->tsf_id;
-				return;
-			}
+		if (vif->type != NL80211_IFTYPE_AP ||
+		    data->preferred_tsf != NUM_TSF_IDS ||
+		    !test_bit(mvmvif->tsf_id, data->available_tsf_ids))
+			break;
+
+		min_bi = min(data->vif->bss_conf.beacon_int,
+			     vif->bss_conf.beacon_int);
+
+		if (!min_bi)
+			break;
+
+		if ((data->vif->bss_conf.beacon_int -
+		     vif->bss_conf.beacon_int) % min_bi == 0) {
+			data->preferred_tsf = mvmvif->tsf_id;
+			return;
 		}
 		break;
+
 	case NL80211_IFTYPE_AP:
 		/*
-		 * The new interface is AP/GO, so in case both interfaces
-		 * have the same beacon interval, it should get drift
-		 * updates from an existing client or use the same
-		 * TSF as an existing GO. There's no drift between
-		 * TSFs internally but if they used different TSFs
-		 * then a new client MAC could update one of them
-		 * and cause drift that way.
+		 * The new interface is AP/GO, so if its beacon interval is a
+		 * multiple or a divisor of the beacon interval of an existing
+		 * interface, it should get drift updates from an existing
+		 * client or use the same TSF as an existing GO. There's no
+		 * drift between TSFs internally but if they used different
+		 * TSFs then a new client MAC could update one of them and
+		 * cause drift that way.
 		 */
-		if (vif->type == NL80211_IFTYPE_STATION ||
-		    vif->type == NL80211_IFTYPE_AP) {
-			if (data->preferred_tsf == NUM_TSF_IDS &&
-			    test_bit(mvmvif->tsf_id, data->available_tsf_ids) &&
-			    (vif->bss_conf.beacon_int ==
-			     data->vif->bss_conf.beacon_int)) {
-				data->preferred_tsf = mvmvif->tsf_id;
-				return;
-			}
+		if ((vif->type != NL80211_IFTYPE_AP &&
+		     vif->type != NL80211_IFTYPE_STATION) ||
+		    data->preferred_tsf != NUM_TSF_IDS ||
+		    !test_bit(mvmvif->tsf_id, data->available_tsf_ids))
+			break;
+
+		min_bi = min(data->vif->bss_conf.beacon_int,
+			     vif->bss_conf.beacon_int);
+
+		if (!min_bi)
+			break;
+
+		if ((data->vif->bss_conf.beacon_int -
+		     vif->bss_conf.beacon_int) % min_bi == 0) {
+			data->preferred_tsf = mvmvif->tsf_id;
+			return;
 		}
 		break;
 	default:
@@ -936,7 +952,7 @@ static int iwl_mvm_mac_ctxt_send_beacon(struct iwl_mvm *mvm,
 					     TX_CMD_FLG_TSF);
 
 	mvm->mgmt_last_antenna_idx =
-		iwl_mvm_next_antenna(mvm, iwl_fw_valid_tx_ant(mvm->fw),
+		iwl_mvm_next_antenna(mvm, mvm->fw->valid_tx_ant,
 				     mvm->mgmt_last_antenna_idx);
 
 	beacon_cmd.tx.rate_n_flags =
diff --git a/drivers/net/wireless/iwlwifi/mvm/mac80211.c b/drivers/net/wireless/iwlwifi/mvm/mac80211.c
index c35b8661b395..c2ab6a3318cb 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mac80211.c
+++ b/drivers/net/wireless/iwlwifi/mvm/mac80211.c
@@ -66,7 +66,9 @@
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
 #include <linux/ip.h>
+#include <linux/if_arp.h>
 #include <net/mac80211.h>
+#include <net/ieee80211_radiotap.h>
 #include <net/tcp.h>
 
 #include "iwl-op-mode.h"
@@ -128,6 +130,117 @@ static const struct wiphy_wowlan_tcp_support iwl_mvm_wowlan_tcp_support = {
 };
 #endif
 
+#ifdef CONFIG_IWLWIFI_BCAST_FILTERING
+/*
+ * Use the reserved field to indicate magic values.
+ * these values will only be used internally by the driver,
+ * and won't make it to the fw (reserved will be 0).
+ * BC_FILTER_MAGIC_IP - configure the val of this attribute to
+ *	be the vif's ip address. in case there is not a single
+ *	ip address (0, or more than 1), this attribute will
+ *	be skipped.
+ * BC_FILTER_MAGIC_MAC - set the val of this attribute to
+ *	the LSB bytes of the vif's mac address
+ */
+enum {
+	BC_FILTER_MAGIC_NONE = 0,
+	BC_FILTER_MAGIC_IP,
+	BC_FILTER_MAGIC_MAC,
+};
+
+static const struct iwl_fw_bcast_filter iwl_mvm_default_bcast_filters[] = {
+	{
+		/* arp */
+		.discard = 0,
+		.frame_type = BCAST_FILTER_FRAME_TYPE_ALL,
+		.attrs = {
+			{
+				/* frame type - arp, hw type - ethernet */
+				.offset_type =
+					BCAST_FILTER_OFFSET_PAYLOAD_START,
+				.offset = sizeof(rfc1042_header),
+				.val = cpu_to_be32(0x08060001),
+				.mask = cpu_to_be32(0xffffffff),
+			},
+			{
+				/* arp dest ip */
+				.offset_type =
+					BCAST_FILTER_OFFSET_PAYLOAD_START,
+				.offset = sizeof(rfc1042_header) + 2 +
+					  sizeof(struct arphdr) +
+					  ETH_ALEN + sizeof(__be32) +
+					  ETH_ALEN,
+				.mask = cpu_to_be32(0xffffffff),
+				/* mark it as special field */
+				.reserved1 = cpu_to_le16(BC_FILTER_MAGIC_IP),
+			},
+		},
+	},
+	{
+		/* dhcp offer bcast */
+		.discard = 0,
+		.frame_type = BCAST_FILTER_FRAME_TYPE_IPV4,
+		.attrs = {
+			{
+				/* udp dest port - 68 (bootp client)*/
+				.offset_type = BCAST_FILTER_OFFSET_IP_END,
+				.offset = offsetof(struct udphdr, dest),
+				.val = cpu_to_be32(0x00440000),
+				.mask = cpu_to_be32(0xffff0000),
+			},
+			{
+				/* dhcp - lsb bytes of client hw address */
+				.offset_type = BCAST_FILTER_OFFSET_IP_END,
+				.offset = 38,
+				.mask = cpu_to_be32(0xffffffff),
+				/* mark it as special field */
+				.reserved1 = cpu_to_le16(BC_FILTER_MAGIC_MAC),
+			},
+		},
+	},
+	/* last filter must be empty */
+	{},
+};
+#endif
+
+void iwl_mvm_ref(struct iwl_mvm *mvm, enum iwl_mvm_ref_type ref_type)
+{
+	if (!mvm->trans->cfg->d0i3)
+		return;
+
+	IWL_DEBUG_RPM(mvm, "Take mvm reference - type %d\n", ref_type);
+	WARN_ON(test_and_set_bit(ref_type, mvm->ref_bitmap));
+	iwl_trans_ref(mvm->trans);
+}
+
+void iwl_mvm_unref(struct iwl_mvm *mvm, enum iwl_mvm_ref_type ref_type)
+{
+	if (!mvm->trans->cfg->d0i3)
+		return;
+
+	IWL_DEBUG_RPM(mvm, "Leave mvm reference - type %d\n", ref_type);
+	WARN_ON(!test_and_clear_bit(ref_type, mvm->ref_bitmap));
+	iwl_trans_unref(mvm->trans);
+}
+
+static void
+iwl_mvm_unref_all_except(struct iwl_mvm *mvm, enum iwl_mvm_ref_type ref)
+{
+	int i;
+
+	if (!mvm->trans->cfg->d0i3)
+		return;
+
+	for_each_set_bit(i, mvm->ref_bitmap, IWL_MVM_REF_COUNT) {
+		if (ref == i)
+			continue;
+
+		IWL_DEBUG_RPM(mvm, "Cleanup: remove mvm ref type %d\n", i);
+		clear_bit(i, mvm->ref_bitmap);
+		iwl_trans_unref(mvm->trans);
+	}
+}
+
 static void iwl_mvm_reset_phy_ctxts(struct iwl_mvm *mvm)
 {
 	int i;
@@ -168,6 +281,9 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 
 	hw->queues = mvm->first_agg_queue;
 	hw->offchannel_tx_hw_queue = IWL_MVM_OFFCHANNEL_QUEUE;
+	hw->radiotap_mcs_details |= IEEE80211_RADIOTAP_MCS_HAVE_FEC |
+				    IEEE80211_RADIOTAP_MCS_HAVE_STBC;
+	hw->radiotap_vht_details |= IEEE80211_RADIOTAP_VHT_KNOWN_STBC;
 	hw->rate_control_algorithm = "iwl-mvm-rs";
 
 	/*
@@ -203,6 +319,9 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 	hw->wiphy->regulatory_flags |= REGULATORY_CUSTOM_REG |
 				       REGULATORY_DISABLE_BEACON_HINTS;
 
+	if (mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_GO_UAPSD)
+		hw->wiphy->flags |= WIPHY_FLAG_AP_UAPSD;
+
 	hw->wiphy->iface_combinations = iwl_mvm_iface_combinations;
 	hw->wiphy->n_iface_combinations =
 		ARRAY_SIZE(iwl_mvm_iface_combinations);
@@ -289,6 +408,11 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 	}
 #endif
 
+#ifdef CONFIG_IWLWIFI_BCAST_FILTERING
+	/* assign default bcast filtering configuration */
+	mvm->bcast_filters = iwl_mvm_default_bcast_filters;
+#endif
+
 	ret = iwl_mvm_leds_init(mvm);
 	if (ret)
 		return ret;
@@ -305,6 +429,9 @@ static void iwl_mvm_mac_tx(struct ieee80211_hw *hw,
 			   struct sk_buff *skb)
 {
 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
+	struct ieee80211_sta *sta = control->sta;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct ieee80211_hdr *hdr = (void *)skb->data;
 
 	if (iwl_mvm_is_radio_killed(mvm)) {
 		IWL_DEBUG_DROP(mvm, "Dropping - RF/CT KILL\n");
@@ -315,8 +442,16 @@ static void iwl_mvm_mac_tx(struct ieee80211_hw *hw,
 	    !test_bit(IWL_MVM_STATUS_ROC_RUNNING, &mvm->status))
 		goto drop;
 
-	if (control->sta) {
-		if (iwl_mvm_tx_skb(mvm, skb, control->sta))
+	/* treat non-bufferable MMPDUs as broadcast if sta is sleeping */
+	if (unlikely(info->flags & IEEE80211_TX_CTL_NO_PS_BUFFER &&
+		     ieee80211_is_mgmt(hdr->frame_control) &&
+		     !ieee80211_is_deauth(hdr->frame_control) &&
+		     !ieee80211_is_disassoc(hdr->frame_control) &&
+		     !ieee80211_is_action(hdr->frame_control)))
+		sta = NULL;
+
+	if (sta) {
+		if (iwl_mvm_tx_skb(mvm, skb, sta))
 			goto drop;
 		return;
 	}
@@ -434,6 +569,7 @@ static void iwl_mvm_restart_cleanup(struct iwl_mvm *mvm)
 		iwl_mvm_cleanup_iterator, mvm);
 
 	mvm->p2p_device_vif = NULL;
+	mvm->d0i3_ap_sta_id = IWL_MVM_STATION_COUNT;
 
 	iwl_mvm_reset_phy_ctxts(mvm);
 	memset(mvm->fw_key_table, 0, sizeof(mvm->fw_key_table));
@@ -441,6 +577,10 @@ static void iwl_mvm_restart_cleanup(struct iwl_mvm *mvm)
 
 	ieee80211_wake_queues(mvm->hw);
 
+	/* cleanup all stale references (scan, roc), but keep the
+	 * ucode_down ref until reconfig is complete */
+	iwl_mvm_unref_all_except(mvm, IWL_MVM_REF_UCODE_DOWN);
+
 	mvm->vif_count = 0;
 	mvm->rx_ba_sessions = 0;
 }
@@ -475,6 +615,9 @@ static void iwl_mvm_mac_restart_complete(struct ieee80211_hw *hw)
 		IWL_ERR(mvm, "Failed to update quotas after restart (%d)\n",
 			ret);
 
+	/* allow transport/FW low power modes */
+	iwl_mvm_unref(mvm, IWL_MVM_REF_UCODE_DOWN);
+
 	mutex_unlock(&mvm->mutex);
 }
 
@@ -482,9 +625,14 @@ static void iwl_mvm_mac_stop(struct ieee80211_hw *hw)
 {
 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
 
+	flush_work(&mvm->d0i3_exit_work);
 	flush_work(&mvm->async_handlers_wk);
 
 	mutex_lock(&mvm->mutex);
+
+	/* disallow low power states when the FW is down */
+	iwl_mvm_ref(mvm, IWL_MVM_REF_UCODE_DOWN);
+
 	/* async_handlers_wk is now blocked */
 
 	/*
@@ -510,14 +658,6 @@ static void iwl_mvm_mac_stop(struct ieee80211_hw *hw)
 	cancel_work_sync(&mvm->async_handlers_wk);
 }
 
-static void iwl_mvm_power_update_iterator(void *data, u8 *mac,
-					  struct ieee80211_vif *vif)
-{
-	struct iwl_mvm *mvm = data;
-
-	iwl_mvm_power_update_mode(mvm, vif);
-}
-
 static struct iwl_mvm_phy_ctxt *iwl_mvm_get_free_phy_ctxt(struct iwl_mvm *mvm)
 {
 	u16 i;
@@ -585,7 +725,8 @@ static int iwl_mvm_mac_add_interface(struct ieee80211_hw *hw,
 	    vif->type == NL80211_IFTYPE_ADHOC) {
 		u32 qmask = iwl_mvm_mac_get_queues_mask(mvm, vif);
 		ret = iwl_mvm_allocate_int_sta(mvm, &mvmvif->bcast_sta,
-					       qmask);
+					       qmask,
+					       ieee80211_vif_type_p2p(vif));
 		if (ret) {
 			IWL_ERR(mvm, "Failed to allocate bcast sta\n");
 			goto out_release;
@@ -599,10 +740,12 @@ static int iwl_mvm_mac_add_interface(struct ieee80211_hw *hw,
 	if (ret)
 		goto out_release;
 
-	iwl_mvm_power_disable(mvm, vif);
+	ret = iwl_mvm_power_update_mac(mvm, vif);
+	if (ret)
+		goto out_release;
 
 	/* beacon filtering */
-	ret = iwl_mvm_disable_beacon_filter(mvm, vif);
+	ret = iwl_mvm_disable_beacon_filter(mvm, vif, CMD_SYNC);
 	if (ret)
 		goto out_remove_mac;
 
@@ -661,11 +804,6 @@ static int iwl_mvm_mac_add_interface(struct ieee80211_hw *hw,
 	if (vif->type != NL80211_IFTYPE_P2P_DEVICE)
 		mvm->vif_count--;
 
-	/* TODO: remove this when legacy PM will be discarded */
-	ieee80211_iterate_active_interfaces(
-		mvm->hw, IEEE80211_IFACE_ITER_NORMAL,
-		iwl_mvm_power_update_iterator, mvm);
-
 	iwl_mvm_mac_ctxt_release(mvm, vif);
  out_unlock:
 	mutex_unlock(&mvm->mutex);
@@ -754,11 +892,7 @@ static void iwl_mvm_mac_remove_interface(struct ieee80211_hw *hw,
 	if (mvm->vif_count && vif->type != NL80211_IFTYPE_P2P_DEVICE)
 		mvm->vif_count--;
 
-	/* TODO: remove this when legacy PM will be discarded */
-	ieee80211_iterate_active_interfaces(
-		mvm->hw, IEEE80211_IFACE_ITER_NORMAL,
-		iwl_mvm_power_update_iterator, mvm);
-
+	iwl_mvm_power_update_mac(mvm, vif);
 	iwl_mvm_mac_ctxt_remove(mvm, vif);
 
 out_release:
@@ -876,6 +1010,156 @@ out:
 	*total_flags = 0;
 }
 
+#ifdef CONFIG_IWLWIFI_BCAST_FILTERING
+struct iwl_bcast_iter_data {
+	struct iwl_mvm *mvm;
+	struct iwl_bcast_filter_cmd *cmd;
+	u8 current_filter;
+};
+
+static void
+iwl_mvm_set_bcast_filter(struct ieee80211_vif *vif,
+			 const struct iwl_fw_bcast_filter *in_filter,
+			 struct iwl_fw_bcast_filter *out_filter)
+{
+	struct iwl_fw_bcast_filter_attr *attr;
+	int i;
+
+	memcpy(out_filter, in_filter, sizeof(*out_filter));
+
+	for (i = 0; i < ARRAY_SIZE(out_filter->attrs); i++) {
+		attr = &out_filter->attrs[i];
+
+		if (!attr->mask)
+			break;
+
+		switch (attr->reserved1) {
+		case cpu_to_le16(BC_FILTER_MAGIC_IP):
+			if (vif->bss_conf.arp_addr_cnt != 1) {
+				attr->mask = 0;
+				continue;
+			}
+
+			attr->val = vif->bss_conf.arp_addr_list[0];
+			break;
+		case cpu_to_le16(BC_FILTER_MAGIC_MAC):
+			attr->val = *(__be32 *)&vif->addr[2];
+			break;
+		default:
+			break;
+		}
+		attr->reserved1 = 0;
+		out_filter->num_attrs++;
+	}
+}
+
+static void iwl_mvm_bcast_filter_iterator(void *_data, u8 *mac,
+					  struct ieee80211_vif *vif)
+{
+	struct iwl_bcast_iter_data *data = _data;
+	struct iwl_mvm *mvm = data->mvm;
+	struct iwl_bcast_filter_cmd *cmd = data->cmd;
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+	struct iwl_fw_bcast_mac *bcast_mac;
+	int i;
+
+	if (WARN_ON(mvmvif->id >= ARRAY_SIZE(cmd->macs)))
+		return;
+
+	bcast_mac = &cmd->macs[mvmvif->id];
+
+	/* enable filtering only for associated stations */
+	if (vif->type != NL80211_IFTYPE_STATION || !vif->bss_conf.assoc)
+		return;
+
+	bcast_mac->default_discard = 1;
+
+	/* copy all configured filters */
+	for (i = 0; mvm->bcast_filters[i].attrs[0].mask; i++) {
+		/*
+		 * Make sure we don't exceed our filters limit.
+		 * if there is still a valid filter to be configured,
+		 * be on the safe side and just allow bcast for this mac.
+		 */
+		if (WARN_ON_ONCE(data->current_filter >=
+				 ARRAY_SIZE(cmd->filters))) {
+			bcast_mac->default_discard = 0;
+			bcast_mac->attached_filters = 0;
+			break;
+		}
+
+		iwl_mvm_set_bcast_filter(vif,
+					 &mvm->bcast_filters[i],
+					 &cmd->filters[data->current_filter]);
+
+		/* skip current filter if it contains no attributes */
+		if (!cmd->filters[data->current_filter].num_attrs)
+			continue;
+
+		/* attach the filter to current mac */
+		bcast_mac->attached_filters |=
+				cpu_to_le16(BIT(data->current_filter));
+
+		data->current_filter++;
+	}
+}
+
+bool iwl_mvm_bcast_filter_build_cmd(struct iwl_mvm *mvm,
+				    struct iwl_bcast_filter_cmd *cmd)
+{
+	struct iwl_bcast_iter_data iter_data = {
+		.mvm = mvm,
+		.cmd = cmd,
+	};
+
+	memset(cmd, 0, sizeof(*cmd));
+	cmd->max_bcast_filters = ARRAY_SIZE(cmd->filters);
+	cmd->max_macs = ARRAY_SIZE(cmd->macs);
+
+#ifdef CONFIG_IWLWIFI_DEBUGFS
+	/* use debugfs filters/macs if override is configured */
+	if (mvm->dbgfs_bcast_filtering.override) {
+		memcpy(cmd->filters, &mvm->dbgfs_bcast_filtering.cmd.filters,
+		       sizeof(cmd->filters));
+		memcpy(cmd->macs, &mvm->dbgfs_bcast_filtering.cmd.macs,
+		       sizeof(cmd->macs));
+		return true;
+	}
+#endif
+
+	/* if no filters are configured, do nothing */
+	if (!mvm->bcast_filters)
+		return false;
+
+	/* configure and attach these filters for each associated sta vif */
+	ieee80211_iterate_active_interfaces(
+		mvm->hw, IEEE80211_IFACE_ITER_NORMAL,
+		iwl_mvm_bcast_filter_iterator, &iter_data);
+
+	return true;
+}
+static int iwl_mvm_configure_bcast_filter(struct iwl_mvm *mvm,
+					  struct ieee80211_vif *vif)
+{
+	struct iwl_bcast_filter_cmd cmd;
+
+	if (!(mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_BCAST_FILTERING))
+		return 0;
+
+	if (!iwl_mvm_bcast_filter_build_cmd(mvm, &cmd))
+		return 0;
+
+	return iwl_mvm_send_cmd_pdu(mvm, BCAST_FILTER_CMD, CMD_SYNC,
+				    sizeof(cmd), &cmd);
+}
+#else
+static inline int iwl_mvm_configure_bcast_filter(struct iwl_mvm *mvm,
+						 struct ieee80211_vif *vif)
+{
+	return 0;
+}
+#endif
+
 static void iwl_mvm_bss_info_changed_station(struct iwl_mvm *mvm,
 					     struct ieee80211_vif *vif,
 					     struct ieee80211_bss_conf *bss_conf,
@@ -928,6 +1212,8 @@ static void iwl_mvm_bss_info_changed_station(struct iwl_mvm *mvm,
 
 			iwl_mvm_sf_update(mvm, vif, false);
 			iwl_mvm_power_vif_assoc(mvm, vif);
+			if (vif->p2p)
+				iwl_mvm_ref(mvm, IWL_MVM_REF_P2P_CLIENT);
 		} else if (mvmvif->ap_sta_id != IWL_MVM_STATION_COUNT) {
 			/*
 			 * If update fails - SF might be running in associated
@@ -940,27 +1226,25 @@ static void iwl_mvm_bss_info_changed_station(struct iwl_mvm *mvm,
 			ret = iwl_mvm_rm_sta_id(mvm, vif, mvmvif->ap_sta_id);
 			if (ret)
 				IWL_ERR(mvm, "failed to remove AP station\n");
+
+			if (mvm->d0i3_ap_sta_id == mvmvif->ap_sta_id)
+				mvm->d0i3_ap_sta_id = IWL_MVM_STATION_COUNT;
 			mvmvif->ap_sta_id = IWL_MVM_STATION_COUNT;
 			/* remove quota for this interface */
 			ret = iwl_mvm_update_quotas(mvm, NULL);
 			if (ret)
 				IWL_ERR(mvm, "failed to update quotas\n");
+
+			if (vif->p2p)
+				iwl_mvm_unref(mvm, IWL_MVM_REF_P2P_CLIENT);
 		}
 
 		iwl_mvm_recalc_multicast(mvm);
+		iwl_mvm_configure_bcast_filter(mvm, vif);
 
 		/* reset rssi values */
 		mvmvif->bf_data.ave_beacon_signal = 0;
 
-		if (!(mvm->fw->ucode_capa.flags &
-					IWL_UCODE_TLV_FLAGS_PM_CMD_SUPPORT)) {
-			/* Workaround for FW bug, otherwise FW disables device
-			 * power save upon disassociation
-			 */
-			ret = iwl_mvm_power_update_mode(mvm, vif);
-			if (ret)
-				IWL_ERR(mvm, "failed to update power mode\n");
-		}
 		iwl_mvm_bt_coex_vif_change(mvm);
 		iwl_mvm_update_smps(mvm, vif, IWL_MVM_SMPS_REQ_TT,
 				    IEEE80211_SMPS_AUTOMATIC);
@@ -973,7 +1257,7 @@ static void iwl_mvm_bss_info_changed_station(struct iwl_mvm *mvm,
 					  &mvmvif->time_event_data);
 	} else if (changes & (BSS_CHANGED_PS | BSS_CHANGED_P2P_PS |
 			      BSS_CHANGED_QOS)) {
-		ret = iwl_mvm_power_update_mode(mvm, vif);
+		ret = iwl_mvm_power_update_mac(mvm, vif);
 		if (ret)
 			IWL_ERR(mvm, "failed to update power mode\n");
 	}
@@ -987,10 +1271,15 @@ static void iwl_mvm_bss_info_changed_station(struct iwl_mvm *mvm,
 		IWL_DEBUG_MAC80211(mvm, "cqm info_changed");
 		/* reset cqm events tracking */
 		mvmvif->bf_data.last_cqm_event = 0;
-		ret = iwl_mvm_update_beacon_filter(mvm, vif);
+		ret = iwl_mvm_update_beacon_filter(mvm, vif, false, CMD_SYNC);
 		if (ret)
 			IWL_ERR(mvm, "failed to update CQM thresholds\n");
 	}
+
+	if (changes & BSS_CHANGED_ARP_FILTER) {
+		IWL_DEBUG_MAC80211(mvm, "arp filter changed");
+		iwl_mvm_configure_bcast_filter(mvm, vif);
+	}
 }
 
 static int iwl_mvm_start_ap_ibss(struct ieee80211_hw *hw,
@@ -1024,8 +1313,6 @@ static int iwl_mvm_start_ap_ibss(struct ieee80211_hw *hw,
 	if (ret)
 		goto out_remove;
 
-	mvmvif->ap_ibss_active = true;
-
 	/* Send the bcast station. At this stage the TBTT and DTIM time events
 	 * are added and applied to the scheduler */
 	ret = iwl_mvm_send_bcast_sta(mvm, vif, &mvmvif->bcast_sta);
@@ -1036,8 +1323,7 @@ static int iwl_mvm_start_ap_ibss(struct ieee80211_hw *hw,
 	mvmvif->ap_ibss_active = true;
 
 	/* power updated needs to be done before quotas */
-	mvm->bound_vif_cnt++;
-	iwl_mvm_power_update_binding(mvm, vif, true);
+	iwl_mvm_power_update_mac(mvm, vif);
 
 	ret = iwl_mvm_update_quotas(mvm, vif);
 	if (ret)
@@ -1047,14 +1333,15 @@ static int iwl_mvm_start_ap_ibss(struct ieee80211_hw *hw,
 	if (vif->p2p && mvm->p2p_device_vif)
 		iwl_mvm_mac_ctxt_changed(mvm, mvm->p2p_device_vif);
 
+	iwl_mvm_ref(mvm, IWL_MVM_REF_AP_IBSS);
+
 	iwl_mvm_bt_coex_vif_change(mvm);
 
 	mutex_unlock(&mvm->mutex);
 	return 0;
 
 out_quota_failed:
-	mvm->bound_vif_cnt--;
-	iwl_mvm_power_update_binding(mvm, vif, false);
+	iwl_mvm_power_update_mac(mvm, vif);
 	mvmvif->ap_ibss_active = false;
 	iwl_mvm_send_rm_bcast_sta(mvm, &mvmvif->bcast_sta);
 out_unbind:
@@ -1080,6 +1367,8 @@ static void iwl_mvm_stop_ap_ibss(struct ieee80211_hw *hw,
 
 	iwl_mvm_bt_coex_vif_change(mvm);
 
+	iwl_mvm_unref(mvm, IWL_MVM_REF_AP_IBSS);
+
 	/* Need to update the P2P Device MAC (only GO, IBSS is single vif) */
 	if (vif->p2p && mvm->p2p_device_vif)
 		iwl_mvm_mac_ctxt_changed(mvm, mvm->p2p_device_vif);
@@ -1088,8 +1377,7 @@ static void iwl_mvm_stop_ap_ibss(struct ieee80211_hw *hw,
 	iwl_mvm_send_rm_bcast_sta(mvm, &mvmvif->bcast_sta);
 	iwl_mvm_binding_remove_vif(mvm, vif);
 
-	mvm->bound_vif_cnt--;
-	iwl_mvm_power_update_binding(mvm, vif, false);
+	iwl_mvm_power_update_mac(mvm, vif);
 
 	iwl_mvm_mac_ctxt_remove(mvm, vif);
 
@@ -1103,26 +1391,20 @@ iwl_mvm_bss_info_changed_ap_ibss(struct iwl_mvm *mvm,
 				 u32 changes)
 {
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
-	enum ieee80211_bss_change ht_change = BSS_CHANGED_ERP_CTS_PROT |
-					      BSS_CHANGED_HT |
-					      BSS_CHANGED_BANDWIDTH;
-	int ret;
 
 	/* Changes will be applied when the AP/IBSS is started */
 	if (!mvmvif->ap_ibss_active)
 		return;
 
-	if (changes & ht_change) {
-		ret = iwl_mvm_mac_ctxt_changed(mvm, vif);
-		if (ret)
-			IWL_ERR(mvm, "failed to update MAC %pM\n", vif->addr);
-	}
+	if (changes & (BSS_CHANGED_ERP_CTS_PROT | BSS_CHANGED_HT |
+		       BSS_CHANGED_BANDWIDTH) &&
+	    iwl_mvm_mac_ctxt_changed(mvm, vif))
+		IWL_ERR(mvm, "failed to update MAC %pM\n", vif->addr);
 
 	/* Need to send a new beacon template to the FW */
-	if (changes & BSS_CHANGED_BEACON) {
-		if (iwl_mvm_mac_ctxt_beacon_changed(mvm, vif))
-			IWL_WARN(mvm, "Failed updating beacon data\n");
-	}
+	if (changes & BSS_CHANGED_BEACON &&
+	    iwl_mvm_mac_ctxt_beacon_changed(mvm, vif))
+		IWL_WARN(mvm, "Failed updating beacon data\n");
 }
 
 static void iwl_mvm_bss_info_changed(struct ieee80211_hw *hw,
@@ -1155,6 +1437,8 @@ static int iwl_mvm_mac_hw_scan(struct ieee80211_hw *hw,
 			       struct cfg80211_scan_request *req)
 {
 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
+	struct iwl_notification_wait wait_scan_done;
+	static const u8 scan_done_notif[] = { SCAN_OFFLOAD_COMPLETE, };
 	int ret;
 
 	if (req->n_channels == 0 || req->n_channels > MAX_NUM_SCAN_CHANNELS)
@@ -1162,11 +1446,38 @@ static int iwl_mvm_mac_hw_scan(struct ieee80211_hw *hw,
 
 	mutex_lock(&mvm->mutex);
 
-	if (mvm->scan_status == IWL_MVM_SCAN_NONE)
-		ret = iwl_mvm_scan_request(mvm, vif, req);
-	else
+	switch (mvm->scan_status) {
+	case IWL_MVM_SCAN_SCHED:
+		iwl_init_notification_wait(&mvm->notif_wait, &wait_scan_done,
+					   scan_done_notif,
+					   ARRAY_SIZE(scan_done_notif),
+					   NULL, NULL);
+		iwl_mvm_sched_scan_stop(mvm);
+		ret = iwl_wait_notification(&mvm->notif_wait,
+					    &wait_scan_done, HZ);
+		if (ret) {
+			ret = -EBUSY;
+			goto out;
+		}
+		/* iwl_mvm_rx_scan_offload_complete_notif() will be called
+		 * soon but will not reset the scan status as it won't be
+		 * IWL_MVM_SCAN_SCHED any more since we queue the next scan
+		 * immediately (below)
+		 */
+		break;
+	case IWL_MVM_SCAN_NONE:
+		break;
+	default:
 		ret = -EBUSY;
+		goto out;
+	}
 
+	iwl_mvm_ref(mvm, IWL_MVM_REF_SCAN);
+
+	ret = iwl_mvm_scan_request(mvm, vif, req);
+	if (ret)
+		iwl_mvm_unref(mvm, IWL_MVM_REF_SCAN);
+out:
 	mutex_unlock(&mvm->mutex);
 
 	return ret;
@@ -1186,20 +1497,32 @@ static void iwl_mvm_mac_cancel_hw_scan(struct ieee80211_hw *hw,
 
 static void
 iwl_mvm_mac_allow_buffered_frames(struct ieee80211_hw *hw,
-				  struct ieee80211_sta *sta, u16 tid,
+				  struct ieee80211_sta *sta, u16 tids,
 				  int num_frames,
 				  enum ieee80211_frame_release_type reason,
 				  bool more_data)
 {
 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
 
-	/* TODO: how do we tell the fw to send frames for a specific TID */
+	/* Called when we need to transmit (a) frame(s) from mac80211 */
 
-	/*
-	 * The fw will send EOSP notification when the last frame will be
-	 * transmitted.
-	 */
-	iwl_mvm_sta_modify_sleep_tx_count(mvm, sta, reason, num_frames);
+	iwl_mvm_sta_modify_sleep_tx_count(mvm, sta, reason, num_frames,
+					  tids, more_data, false);
+}
+
+static void
+iwl_mvm_mac_release_buffered_frames(struct ieee80211_hw *hw,
+				    struct ieee80211_sta *sta, u16 tids,
+				    int num_frames,
+				    enum ieee80211_frame_release_type reason,
+				    bool more_data)
+{
+	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
+
+	/* Called when we need to transmit (a) frame(s) from agg queue */
+
+	iwl_mvm_sta_modify_sleep_tx_count(mvm, sta, reason, num_frames,
+					  tids, more_data, true);
 }
 
 static void iwl_mvm_mac_sta_notify(struct ieee80211_hw *hw,
@@ -1209,11 +1532,25 @@ static void iwl_mvm_mac_sta_notify(struct ieee80211_hw *hw,
 {
 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
 	struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
+	int tid;
 
 	switch (cmd) {
 	case STA_NOTIFY_SLEEP:
 		if (atomic_read(&mvm->pending_frames[mvmsta->sta_id]) > 0)
 			ieee80211_sta_block_awake(hw, sta, true);
+		spin_lock_bh(&mvmsta->lock);
+		for (tid = 0; tid < IWL_MAX_TID_COUNT; tid++) {
+			struct iwl_mvm_tid_data *tid_data;
+
+			tid_data = &mvmsta->tid_data[tid];
+			if (tid_data->state != IWL_AGG_ON &&
+			    tid_data->state != IWL_EMPTYING_HW_QUEUE_DELBA)
+				continue;
+			if (iwl_mvm_tid_queued(tid_data) == 0)
+				continue;
+			ieee80211_sta_set_buffered(sta, tid, true);
+		}
+		spin_unlock_bh(&mvmsta->lock);
 		/*
 		 * The fw updates the STA to be asleep. Tx packets on the Tx
 		 * queues to this station will not be transmitted. The fw will
@@ -1304,12 +1641,12 @@ static int iwl_mvm_mac_sta_state(struct ieee80211_hw *hw,
 	} else if (old_state == IEEE80211_STA_ASSOC &&
 		   new_state == IEEE80211_STA_AUTHORIZED) {
 		/* enable beacon filtering */
-		WARN_ON(iwl_mvm_enable_beacon_filter(mvm, vif));
+		WARN_ON(iwl_mvm_enable_beacon_filter(mvm, vif, CMD_SYNC));
 		ret = 0;
 	} else if (old_state == IEEE80211_STA_AUTHORIZED &&
 		   new_state == IEEE80211_STA_ASSOC) {
 		/* disable beacon filtering */
-		WARN_ON(iwl_mvm_disable_beacon_filter(mvm, vif));
+		WARN_ON(iwl_mvm_disable_beacon_filter(mvm, vif, CMD_SYNC));
 		ret = 0;
 	} else if (old_state == IEEE80211_STA_ASSOC &&
 		   new_state == IEEE80211_STA_AUTH) {
@@ -1428,14 +1765,16 @@ out:
 	return ret;
 }
 
-static void iwl_mvm_mac_sched_scan_stop(struct ieee80211_hw *hw,
-					struct ieee80211_vif *vif)
+static int iwl_mvm_mac_sched_scan_stop(struct ieee80211_hw *hw,
+				       struct ieee80211_vif *vif)
 {
 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
 
 	mutex_lock(&mvm->mutex);
 	iwl_mvm_sched_scan_stop(mvm);
 	mutex_unlock(&mvm->mutex);
+
+	return 0;
 }
 
 static int iwl_mvm_mac_set_key(struct ieee80211_hw *hw,
@@ -1773,8 +2112,7 @@ static int iwl_mvm_assign_vif_chanctx(struct ieee80211_hw *hw,
 	 * Power state must be updated before quotas,
 	 * otherwise fw will complain.
 	 */
-	mvm->bound_vif_cnt++;
-	iwl_mvm_power_update_binding(mvm, vif, true);
+	iwl_mvm_power_update_mac(mvm, vif);
 
 	/* Setting the quota at this stage is only required for monitor
 	 * interfaces. For the other types, the bss_info changed flow
@@ -1791,8 +2129,7 @@ static int iwl_mvm_assign_vif_chanctx(struct ieee80211_hw *hw,
 
  out_remove_binding:
 	iwl_mvm_binding_remove_vif(mvm, vif);
-	mvm->bound_vif_cnt--;
-	iwl_mvm_power_update_binding(mvm, vif, false);
+	iwl_mvm_power_update_mac(mvm, vif);
  out_unlock:
 	mutex_unlock(&mvm->mutex);
 	if (ret)
@@ -1824,8 +2161,7 @@ static void iwl_mvm_unassign_vif_chanctx(struct ieee80211_hw *hw,
 	}
 
 	iwl_mvm_binding_remove_vif(mvm, vif);
-	mvm->bound_vif_cnt--;
-	iwl_mvm_power_update_binding(mvm, vif, false);
+	iwl_mvm_power_update_mac(mvm, vif);
 
 out_unlock:
 	mvmvif->phy_ctxt = NULL;
@@ -1892,8 +2228,9 @@ static int __iwl_mvm_mac_testmode_cmd(struct iwl_mvm *mvm,
 			return -EINVAL;
 
 		if (nla_get_u32(tb[IWL_MVM_TM_ATTR_BEACON_FILTER_STATE]))
-			return iwl_mvm_enable_beacon_filter(mvm, vif);
-		return iwl_mvm_disable_beacon_filter(mvm, vif);
+			return iwl_mvm_enable_beacon_filter(mvm, vif,
+							    CMD_SYNC);
+		return iwl_mvm_disable_beacon_filter(mvm, vif, CMD_SYNC);
 	}
 
 	return -EOPNOTSUPP;
@@ -1914,7 +2251,7 @@ static int iwl_mvm_mac_testmode_cmd(struct ieee80211_hw *hw,
 }
 #endif
 
-struct ieee80211_ops iwl_mvm_hw_ops = {
+const struct ieee80211_ops iwl_mvm_hw_ops = {
 	.tx = iwl_mvm_mac_tx,
 	.ampdu_action = iwl_mvm_mac_ampdu_action,
 	.start = iwl_mvm_mac_start,
@@ -1932,6 +2269,7 @@ struct ieee80211_ops iwl_mvm_hw_ops = {
 	.sta_state = iwl_mvm_mac_sta_state,
 	.sta_notify = iwl_mvm_mac_sta_notify,
 	.allow_buffered_frames = iwl_mvm_mac_allow_buffered_frames,
+	.release_buffered_frames = iwl_mvm_mac_release_buffered_frames,
 	.set_rts_threshold = iwl_mvm_mac_set_rts_threshold,
 	.sta_rc_update = iwl_mvm_sta_rc_update,
 	.conf_tx = iwl_mvm_mac_conf_tx,
diff --git a/drivers/net/wireless/iwlwifi/mvm/mvm.h b/drivers/net/wireless/iwlwifi/mvm/mvm.h
index 2b0ba1fc3c82..302cf779c172 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mvm.h
+++ b/drivers/net/wireless/iwlwifi/mvm/mvm.h
@@ -91,9 +91,7 @@ enum iwl_mvm_tx_fifo {
 	IWL_MVM_TX_FIFO_MCAST = 5,
 };
 
-extern struct ieee80211_ops iwl_mvm_hw_ops;
-extern const struct iwl_mvm_power_ops pm_legacy_ops;
-extern const struct iwl_mvm_power_ops pm_mac_ops;
+extern const struct ieee80211_ops iwl_mvm_hw_ops;
 
 /**
  * struct iwl_mvm_mod_params - module parameters for iwlmvm
@@ -159,20 +157,6 @@ enum iwl_power_scheme {
 					 IEEE80211_WMM_IE_STA_QOSINFO_AC_BE)
 #define IWL_UAPSD_MAX_SP		IEEE80211_WMM_IE_STA_QOSINFO_SP_2
 
-struct iwl_mvm_power_ops {
-	int (*power_update_mode)(struct iwl_mvm *mvm,
-				 struct ieee80211_vif *vif);
-	int (*power_update_device_mode)(struct iwl_mvm *mvm);
-	int (*power_disable)(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
-	void (*power_update_binding)(struct iwl_mvm *mvm,
-				     struct ieee80211_vif *vif, bool assign);
-#ifdef CONFIG_IWLWIFI_DEBUGFS
-	int (*power_dbgfs_read)(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
-				char *buf, int bufsz);
-#endif
-};
-
-
 #ifdef CONFIG_IWLWIFI_DEBUGFS
 enum iwl_dbgfs_pm_mask {
 	MVM_DEBUGFS_PM_KEEP_ALIVE = BIT(0),
@@ -239,6 +223,17 @@ enum iwl_mvm_smps_type_request {
 	NUM_IWL_MVM_SMPS_REQ,
 };
 
+enum iwl_mvm_ref_type {
+	IWL_MVM_REF_UCODE_DOWN,
+	IWL_MVM_REF_SCAN,
+	IWL_MVM_REF_ROC,
+	IWL_MVM_REF_P2P_CLIENT,
+	IWL_MVM_REF_AP_IBSS,
+	IWL_MVM_REF_USER,
+
+	IWL_MVM_REF_COUNT,
+};
+
 /**
 * struct iwl_mvm_vif_bf_data - beacon filtering related data
 * @bf_enabled: indicates if beacon filtering is enabled
@@ -269,7 +264,9 @@ struct iwl_mvm_vif_bf_data {
  * @ap_ibss_active: indicates that AP/IBSS is configured and that the interface
  *	should get quota etc.
  * @monitor_active: indicates that monitor context is configured, and that the
- * interface should get quota etc.
+ *	interface should get quota etc.
+ * @low_latency: indicates that this interface is in low-latency mode
+ *	(VMACLowLatencyMode)
  * @queue_params: QoS params for this MAC
  * @bcast_sta: station used for broadcast packets. Used by the following
  *  vifs: P2P_DEVICE, GO and AP.
@@ -285,6 +282,7 @@ struct iwl_mvm_vif {
 	bool uploaded;
 	bool ap_ibss_active;
 	bool monitor_active;
+	bool low_latency;
 	struct iwl_mvm_vif_bf_data bf_data;
 
 	u32 ap_beacon_time;
@@ -333,14 +331,13 @@ struct iwl_mvm_vif {
 	struct dentry *dbgfs_slink;
 	struct iwl_dbgfs_pm dbgfs_pm;
 	struct iwl_dbgfs_bf dbgfs_bf;
+	struct iwl_mac_power_cmd mac_pwr_cmd;
 #endif
 
 	enum ieee80211_smps_mode smps_requests[NUM_IWL_MVM_SMPS_REQ];
 
 	/* FW identified misbehaving AP */
 	u8 uapsd_misbehaving_bssid[ETH_ALEN];
-
-	bool pm_prevented;
 };
 
 static inline struct iwl_mvm_vif *
@@ -415,6 +412,7 @@ struct iwl_tt_params {
  * @ct_kill_exit: worker to exit thermal kill
  * @dynamic_smps: Is thermal throttling enabled dynamic_smps?
  * @tx_backoff: The current thremal throttling tx backoff in uSec.
+ * @min_backoff: The minimal tx backoff due to power restrictions
  * @params: Parameters to configure the thermal throttling algorithm.
  * @throttle: Is thermal throttling is active?
  */
@@ -422,10 +420,33 @@ struct iwl_mvm_tt_mgmt {
 	struct delayed_work ct_kill_exit;
 	bool dynamic_smps;
 	u32 tx_backoff;
+	u32 min_backoff;
 	const struct iwl_tt_params *params;
 	bool throttle;
 };
 
+#define IWL_MVM_NUM_LAST_FRAMES_UCODE_RATES 8
+
+struct iwl_mvm_frame_stats {
+	u32 legacy_frames;
+	u32 ht_frames;
+	u32 vht_frames;
+	u32 bw_20_frames;
+	u32 bw_40_frames;
+	u32 bw_80_frames;
+	u32 bw_160_frames;
+	u32 sgi_frames;
+	u32 ngi_frames;
+	u32 siso_frames;
+	u32 mimo2_frames;
+	u32 agg_frames;
+	u32 ampdu_count;
+	u32 success_frames;
+	u32 fail_frames;
+	u32 last_rates[IWL_MVM_NUM_LAST_FRAMES_UCODE_RATES];
+	int last_frame_idx;
+};
+
 struct iwl_mvm {
 	/* for logger access */
 	struct device *dev;
@@ -457,6 +478,8 @@ struct iwl_mvm {
 	bool init_ucode_complete;
 	u32 error_event_table;
 	u32 log_event_table;
+	u32 umac_error_event_table;
+	bool support_umac_log;
 
 	u32 ampdu_ref;
 
@@ -470,7 +493,7 @@ struct iwl_mvm {
 
 	struct iwl_nvm_data *nvm_data;
 	/* NVM sections */
-	struct iwl_nvm_section nvm_sections[NVM_NUM_OF_SECTIONS];
+	struct iwl_nvm_section nvm_sections[NVM_MAX_NUM_SECTIONS];
 
 	/* EEPROM MAC addresses */
 	struct mac_address addresses[IWL_MVM_MAX_ADDRESSES];
@@ -494,6 +517,17 @@ struct iwl_mvm {
 	/* rx chain antennas set through debugfs for the scan command */
 	u8 scan_rx_ant;
 
+#ifdef CONFIG_IWLWIFI_BCAST_FILTERING
+	/* broadcast filters to configure for each associated station */
+	const struct iwl_fw_bcast_filter *bcast_filters;
+#ifdef CONFIG_IWLWIFI_DEBUGFS
+	struct {
+		u32 override; /* u32 for debugfs_create_bool */
+		struct iwl_bcast_filter_cmd cmd;
+	} dbgfs_bcast_filtering;
+#endif
+#endif
+
 	/* Internal station */
 	struct iwl_mvm_int_sta aux_sta;
 
@@ -506,6 +540,7 @@ struct iwl_mvm {
 #ifdef CONFIG_IWLWIFI_DEBUGFS
 	struct dentry *debugfs_dir;
 	u32 dbgfs_sram_offset, dbgfs_sram_len;
+	u32 dbgfs_prph_reg_addr;
 	bool disable_power_off;
 	bool disable_power_off_d3;
 
@@ -513,6 +548,9 @@ struct iwl_mvm {
 	struct debugfs_blob_wrapper nvm_sw_blob;
 	struct debugfs_blob_wrapper nvm_calib_blob;
 	struct debugfs_blob_wrapper nvm_prod_blob;
+
+	struct iwl_mvm_frame_stats drv_rx_stats;
+	spinlock_t drv_stats_lock;
 #endif
 
 	struct iwl_mvm_phy_ctxt phy_ctxts[NUM_PHY_CTX];
@@ -526,6 +564,9 @@ struct iwl_mvm {
 	 */
 	unsigned long fw_key_table[BITS_TO_LONGS(STA_KEY_MAX_NUM)];
 
+	/* A bitmap of reference types taken by the driver. */
+	unsigned long ref_bitmap[BITS_TO_LONGS(IWL_MVM_REF_COUNT)];
+
 	u8 vif_count;
 
 	/* -1 for always, 0 for never, >0 for that many times */
@@ -548,6 +589,10 @@ struct iwl_mvm {
 #endif
 #endif
 
+	/* d0i3 */
+	u8 d0i3_ap_sta_id;
+	struct work_struct d0i3_exit_work;
+
 	/* BT-Coex */
 	u8 bt_kill_msk;
 	struct iwl_bt_coex_profile_notif last_bt_notif;
@@ -557,8 +602,6 @@ struct iwl_mvm {
 	struct iwl_mvm_tt_mgmt thermal_throttle;
 	s32 temperature;	/* Celsius */
 
-	const struct iwl_mvm_power_ops *pm_ops;
-
 #ifdef CONFIG_NL80211_TESTMODE
 	u32 noa_duration;
 	struct ieee80211_vif *noa_vif;
@@ -569,10 +612,10 @@ struct iwl_mvm {
 	u8 first_agg_queue;
 	u8 last_agg_queue;
 
-	u8 bound_vif_cnt;
-
 	/* Indicate if device power save is allowed */
-	bool ps_prevented;
+	bool ps_disabled;
+	/* Indicate if device power management is allowed */
+	bool pm_disabled;
 };
 
 /* Extract MVM priv from op_mode and _hw */
@@ -595,6 +638,24 @@ static inline bool iwl_mvm_is_radio_killed(struct iwl_mvm *mvm)
 	       test_bit(IWL_MVM_STATUS_HW_CTKILL, &mvm->status);
 }
 
+static inline struct iwl_mvm_sta *
+iwl_mvm_sta_from_staid_protected(struct iwl_mvm *mvm, u8 sta_id)
+{
+	struct ieee80211_sta *sta;
+
+	if (sta_id >= ARRAY_SIZE(mvm->fw_id_to_mac_id))
+		return NULL;
+
+	sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[sta_id],
+					lockdep_is_held(&mvm->mutex));
+
+	/* This can happen if the station has been removed right now */
+	if (IS_ERR_OR_NULL(sta))
+		return NULL;
+
+	return iwl_mvm_sta_from_mac80211(sta);
+}
+
 extern const u8 iwl_mvm_ac_to_tx_fifo[];
 
 struct iwl_rate_info {
@@ -661,6 +722,8 @@ int iwl_mvm_up(struct iwl_mvm *mvm);
 int iwl_mvm_load_d3_fw(struct iwl_mvm *mvm);
 
 int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm);
+bool iwl_mvm_bcast_filter_build_cmd(struct iwl_mvm *mvm,
+				    struct iwl_bcast_filter_cmd *cmd);
 
 /*
  * FW notifications / CMD responses handlers
@@ -772,49 +835,24 @@ iwl_mvm_vif_dbgfs_clean(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 
 /* rate scaling */
 int iwl_mvm_send_lq_cmd(struct iwl_mvm *mvm, struct iwl_lq_cmd *lq, bool init);
+void iwl_mvm_update_frame_stats(struct iwl_mvm *mvm,
+				struct iwl_mvm_frame_stats *stats,
+				u32 rate, bool agg);
+int rs_pretty_print_rate(char *buf, const u32 rate);
 
-/* power managment */
-static inline int iwl_mvm_power_update_mode(struct iwl_mvm *mvm,
-					    struct ieee80211_vif *vif)
-{
-	return mvm->pm_ops->power_update_mode(mvm, vif);
-}
-
-static inline int iwl_mvm_power_disable(struct iwl_mvm *mvm,
-					struct ieee80211_vif *vif)
-{
-	return mvm->pm_ops->power_disable(mvm, vif);
-}
+/* power management */
+int iwl_power_legacy_set_cam_mode(struct iwl_mvm *mvm);
 
-static inline int iwl_mvm_power_update_device_mode(struct iwl_mvm *mvm)
-{
-	if (mvm->pm_ops->power_update_device_mode)
-		return mvm->pm_ops->power_update_device_mode(mvm);
-	return 0;
-}
-
-static inline void iwl_mvm_power_update_binding(struct iwl_mvm *mvm,
-						struct ieee80211_vif *vif,
-						bool assign)
-{
-	if (mvm->pm_ops->power_update_binding)
-		mvm->pm_ops->power_update_binding(mvm, vif, assign);
-}
+int iwl_mvm_power_update_device(struct iwl_mvm *mvm);
+int iwl_mvm_power_update_mac(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
+int iwl_mvm_power_mac_dbgfs_read(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
+				 char *buf, int bufsz);
 
 void iwl_mvm_power_vif_assoc(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
 int iwl_mvm_power_uapsd_misbehaving_ap_notif(struct iwl_mvm *mvm,
 					     struct iwl_rx_cmd_buffer *rxb,
 					     struct iwl_device_cmd *cmd);
 
-#ifdef CONFIG_IWLWIFI_DEBUGFS
-static inline int iwl_mvm_power_dbgfs_read(struct iwl_mvm *mvm,
-					    struct ieee80211_vif *vif,
-					    char *buf, int bufsz)
-{
-	return mvm->pm_ops->power_dbgfs_read(mvm, vif, buf, bufsz);
-}
-#endif
-
 int iwl_mvm_leds_init(struct iwl_mvm *mvm);
 void iwl_mvm_leds_exit(struct iwl_mvm *mvm);
 
@@ -841,6 +879,10 @@ iwl_mvm_set_last_nonqos_seq(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 }
 #endif
 
+/* D0i3 */
+void iwl_mvm_ref(struct iwl_mvm *mvm, enum iwl_mvm_ref_type ref_type);
+void iwl_mvm_unref(struct iwl_mvm *mvm, enum iwl_mvm_ref_type ref_type);
+
 /* BT Coex */
 int iwl_send_bt_prio_tbl(struct iwl_mvm *mvm);
 int iwl_send_bt_init_conf(struct iwl_mvm *mvm);
@@ -854,6 +896,7 @@ u16 iwl_mvm_bt_coex_agg_time_limit(struct iwl_mvm *mvm,
 				   struct ieee80211_sta *sta);
 bool iwl_mvm_bt_coex_is_mimo_allowed(struct iwl_mvm *mvm,
 				     struct ieee80211_sta *sta);
+int iwl_mvm_bt_coex_reduced_txp(struct iwl_mvm *mvm, u8 sta_id, bool enable);
 
 enum iwl_bt_kill_msk {
 	BT_KILL_MSK_DEFAULT,
@@ -875,25 +918,51 @@ iwl_mvm_beacon_filter_debugfs_parameters(struct ieee80211_vif *vif,
 					 struct iwl_beacon_filter_cmd *cmd)
 {}
 #endif
+int iwl_mvm_update_d0i3_power_mode(struct iwl_mvm *mvm,
+				   struct ieee80211_vif *vif,
+				   bool enable, u32 flags);
 int iwl_mvm_enable_beacon_filter(struct iwl_mvm *mvm,
-				 struct ieee80211_vif *vif);
+				 struct ieee80211_vif *vif,
+				 u32 flags);
 int iwl_mvm_disable_beacon_filter(struct iwl_mvm *mvm,
-				  struct ieee80211_vif *vif);
-int iwl_mvm_beacon_filter_send_cmd(struct iwl_mvm *mvm,
-				   struct iwl_beacon_filter_cmd *cmd);
+				  struct ieee80211_vif *vif,
+				  u32 flags);
 int iwl_mvm_update_beacon_abort(struct iwl_mvm *mvm,
 				struct ieee80211_vif *vif, bool enable);
 int iwl_mvm_update_beacon_filter(struct iwl_mvm *mvm,
-				  struct ieee80211_vif *vif);
+				 struct ieee80211_vif *vif,
+				 bool force,
+				 u32 flags);
 
 /* SMPS */
 void iwl_mvm_update_smps(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 				enum iwl_mvm_smps_type_request req_type,
 				enum ieee80211_smps_mode smps_request);
 
+/* Low latency */
+int iwl_mvm_update_low_latency(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
+			       bool value);
+/* get VMACLowLatencyMode */
+static inline bool iwl_mvm_vif_low_latency(struct iwl_mvm_vif *mvmvif)
+{
+	/*
+	 * should this consider associated/active/... state?
+	 *
+	 * Normally low-latency should only be active on interfaces
+	 * that are active, but at least with debugfs it can also be
+	 * enabled on interfaces that aren't active. However, when
+	 * interface aren't active then they aren't added into the
+	 * binding, so this has no real impact. For now, just return
+	 * the current desired low-latency state.
+	 */
+
+	return mvmvif->low_latency;
+}
+
 /* 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);
-void iwl_mvm_tt_initialize(struct iwl_mvm *mvm);
+void iwl_mvm_tt_initialize(struct iwl_mvm *mvm, u32 min_backoff);
 void iwl_mvm_tt_exit(struct iwl_mvm *mvm);
 void iwl_mvm_set_hw_ctkill_state(struct iwl_mvm *mvm, bool state);
 
diff --git a/drivers/net/wireless/iwlwifi/mvm/nvm.c b/drivers/net/wireless/iwlwifi/mvm/nvm.c
index 35b71af78d02..cf2d09f53782 100644
--- a/drivers/net/wireless/iwlwifi/mvm/nvm.c
+++ b/drivers/net/wireless/iwlwifi/mvm/nvm.c
@@ -67,14 +67,6 @@
 #include "iwl-eeprom-read.h"
 #include "iwl-nvm-parse.h"
 
-/* list of NVM sections we are allowed/need to read */
-static const int nvm_to_read[] = {
-	NVM_SECTION_TYPE_HW,
-	NVM_SECTION_TYPE_SW,
-	NVM_SECTION_TYPE_CALIBRATION,
-	NVM_SECTION_TYPE_PRODUCTION,
-};
-
 /* Default NVM size to read */
 #define IWL_NVM_DEFAULT_CHUNK_SIZE (2*1024)
 #define IWL_MAX_NVM_SECTION_SIZE 7000
@@ -236,24 +228,39 @@ static struct iwl_nvm_data *
 iwl_parse_nvm_sections(struct iwl_mvm *mvm)
 {
 	struct iwl_nvm_section *sections = mvm->nvm_sections;
-	const __le16 *hw, *sw, *calib;
+	const __le16 *hw, *sw, *calib, *regulatory, *mac_override;
 
 	/* Checking for required sections */
-	if (!mvm->nvm_sections[NVM_SECTION_TYPE_SW].data ||
-	    !mvm->nvm_sections[NVM_SECTION_TYPE_HW].data) {
-		IWL_ERR(mvm, "Can't parse empty NVM sections\n");
-		return NULL;
+	if (mvm->trans->cfg->device_family != IWL_DEVICE_FAMILY_8000) {
+		if (!mvm->nvm_sections[NVM_SECTION_TYPE_SW].data ||
+		    !mvm->nvm_sections[mvm->cfg->nvm_hw_section_num].data) {
+			IWL_ERR(mvm, "Can't parse empty NVM sections\n");
+			return NULL;
+		}
+	} else {
+		if (!mvm->nvm_sections[NVM_SECTION_TYPE_SW].data ||
+		    !mvm->nvm_sections[NVM_SECTION_TYPE_MAC_OVERRIDE].data ||
+		    !mvm->nvm_sections[NVM_SECTION_TYPE_REGULATORY].data) {
+			IWL_ERR(mvm,
+				"Can't parse empty family 8000 NVM sections\n");
+			return NULL;
+		}
 	}
 
 	if (WARN_ON(!mvm->cfg))
 		return NULL;
 
-	hw = (const __le16 *)sections[NVM_SECTION_TYPE_HW].data;
+	hw = (const __le16 *)sections[mvm->cfg->nvm_hw_section_num].data;
 	sw = (const __le16 *)sections[NVM_SECTION_TYPE_SW].data;
 	calib = (const __le16 *)sections[NVM_SECTION_TYPE_CALIBRATION].data;
+	regulatory = (const __le16 *)sections[NVM_SECTION_TYPE_REGULATORY].data;
+	mac_override =
+		(const __le16 *)sections[NVM_SECTION_TYPE_MAC_OVERRIDE].data;
+
 	return iwl_parse_nvm_data(mvm->trans->dev, mvm->cfg, hw, sw, calib,
-				  iwl_fw_valid_tx_ant(mvm->fw),
-				  iwl_fw_valid_rx_ant(mvm->fw));
+				  regulatory, mac_override,
+				  mvm->fw->valid_tx_ant,
+				  mvm->fw->valid_rx_ant);
 }
 
 #define MAX_NVM_FILE_LEN	16384
@@ -293,6 +300,8 @@ static int iwl_mvm_read_external_nvm(struct iwl_mvm *mvm)
 
 #define NVM_WORD1_LEN(x) (8 * (x & 0x03FF))
 #define NVM_WORD2_ID(x) (x >> 12)
+#define NVM_WORD2_LEN_FAMILY_8000(x) (2 * ((x & 0xFF) << 8 | x >> 8))
+#define NVM_WORD1_ID_FAMILY_8000(x) (x >> 4)
 
 	IWL_DEBUG_EEPROM(mvm->trans->dev, "Read from external NVM\n");
 
@@ -343,8 +352,16 @@ static int iwl_mvm_read_external_nvm(struct iwl_mvm *mvm)
 			break;
 		}
 
-		section_size = 2 * NVM_WORD1_LEN(le16_to_cpu(file_sec->word1));
-		section_id = NVM_WORD2_ID(le16_to_cpu(file_sec->word2));
+		if (mvm->trans->cfg->device_family != IWL_DEVICE_FAMILY_8000) {
+			section_size =
+				2 * NVM_WORD1_LEN(le16_to_cpu(file_sec->word1));
+			section_id = NVM_WORD2_ID(le16_to_cpu(file_sec->word2));
+		} else {
+			section_size = 2 * NVM_WORD2_LEN_FAMILY_8000(
+						le16_to_cpu(file_sec->word2));
+			section_id = NVM_WORD1_ID_FAMILY_8000(
+						le16_to_cpu(file_sec->word1));
+		}
 
 		if (section_size > IWL_MAX_NVM_SECTION_SIZE) {
 			IWL_ERR(mvm, "ERROR - section too large (%d)\n",
@@ -367,7 +384,7 @@ static int iwl_mvm_read_external_nvm(struct iwl_mvm *mvm)
 			break;
 		}
 
-		if (WARN(section_id >= NVM_NUM_OF_SECTIONS,
+		if (WARN(section_id >= NVM_MAX_NUM_SECTIONS,
 			 "Invalid NVM section ID %d\n", section_id)) {
 			ret = -EINVAL;
 			break;
@@ -414,6 +431,11 @@ int iwl_nvm_init(struct iwl_mvm *mvm)
 {
 	int ret, i, section;
 	u8 *nvm_buffer, *temp;
+	int nvm_to_read[NVM_MAX_NUM_SECTIONS];
+	int num_of_sections_to_read;
+
+	if (WARN_ON_ONCE(mvm->cfg->nvm_hw_section_num >= NVM_MAX_NUM_SECTIONS))
+		return -EINVAL;
 
 	/* load external NVM if configured */
 	if (iwlwifi_mod_params.nvm_file) {
@@ -422,6 +444,22 @@ int iwl_nvm_init(struct iwl_mvm *mvm)
 		if (ret)
 			return ret;
 	} else {
+		/* list of NVM sections we are allowed/need to read */
+		if (mvm->trans->cfg->device_family != IWL_DEVICE_FAMILY_8000) {
+			nvm_to_read[0] = mvm->cfg->nvm_hw_section_num;
+			nvm_to_read[1] = NVM_SECTION_TYPE_SW;
+			nvm_to_read[2] = NVM_SECTION_TYPE_CALIBRATION;
+			nvm_to_read[3] = NVM_SECTION_TYPE_PRODUCTION;
+			num_of_sections_to_read = 4;
+		} else {
+			nvm_to_read[0] = NVM_SECTION_TYPE_SW;
+			nvm_to_read[1] = NVM_SECTION_TYPE_CALIBRATION;
+			nvm_to_read[2] = NVM_SECTION_TYPE_PRODUCTION;
+			nvm_to_read[3] = NVM_SECTION_TYPE_REGULATORY;
+			nvm_to_read[4] = NVM_SECTION_TYPE_MAC_OVERRIDE;
+			num_of_sections_to_read = 5;
+		}
+
 		/* Read From FW NVM */
 		IWL_DEBUG_EEPROM(mvm->trans->dev, "Read from NVM\n");
 
@@ -430,7 +468,7 @@ int iwl_nvm_init(struct iwl_mvm *mvm)
 				     GFP_KERNEL);
 		if (!nvm_buffer)
 			return -ENOMEM;
-		for (i = 0; i < ARRAY_SIZE(nvm_to_read); i++) {
+		for (i = 0; i < num_of_sections_to_read; i++) {
 			section = nvm_to_read[i];
 			/* we override the constness for initial read */
 			ret = iwl_nvm_read_section(mvm, section, nvm_buffer);
@@ -446,10 +484,6 @@ int iwl_nvm_init(struct iwl_mvm *mvm)
 
 #ifdef CONFIG_IWLWIFI_DEBUGFS
 			switch (section) {
-			case NVM_SECTION_TYPE_HW:
-				mvm->nvm_hw_blob.data = temp;
-				mvm->nvm_hw_blob.size = ret;
-				break;
 			case NVM_SECTION_TYPE_SW:
 				mvm->nvm_sw_blob.data = temp;
 				mvm->nvm_sw_blob.size  = ret;
@@ -463,6 +497,11 @@ int iwl_nvm_init(struct iwl_mvm *mvm)
 				mvm->nvm_prod_blob.size  = ret;
 				break;
 			default:
+				if (section == mvm->cfg->nvm_hw_section_num) {
+					mvm->nvm_hw_blob.data = temp;
+					mvm->nvm_hw_blob.size = ret;
+					break;
+				}
 				WARN(1, "section: %d", section);
 			}
 #endif
diff --git a/drivers/net/wireless/iwlwifi/mvm/ops.c b/drivers/net/wireless/iwlwifi/mvm/ops.c
index a3d43de342d7..ae347fb16a5d 100644
--- a/drivers/net/wireless/iwlwifi/mvm/ops.c
+++ b/drivers/net/wireless/iwlwifi/mvm/ops.c
@@ -185,9 +185,10 @@ static void iwl_mvm_nic_config(struct iwl_op_mode *op_mode)
 	 * (PCIe power is lost before PERST# is asserted), causing ME FW
 	 * to lose ownership and not being able to obtain it back.
 	 */
-	iwl_set_bits_mask_prph(mvm->trans, APMG_PS_CTRL_REG,
-			       APMG_PS_CTRL_EARLY_PWR_OFF_RESET_DIS,
-			       ~APMG_PS_CTRL_EARLY_PWR_OFF_RESET_DIS);
+	if (mvm->trans->cfg->device_family != IWL_DEVICE_FAMILY_8000)
+		iwl_set_bits_mask_prph(mvm->trans, APMG_PS_CTRL_REG,
+				       APMG_PS_CTRL_EARLY_PWR_OFF_RESET_DIS,
+				       ~APMG_PS_CTRL_EARLY_PWR_OFF_RESET_DIS);
 }
 
 struct iwl_rx_handlers {
@@ -222,10 +223,12 @@ static const struct iwl_rx_handlers iwl_mvm_rx_handlers[] = {
 
 	RX_HANDLER(TIME_EVENT_NOTIFICATION, iwl_mvm_rx_time_event_notif, false),
 
+	RX_HANDLER(EOSP_NOTIFICATION, iwl_mvm_rx_eosp_notif, false),
+
 	RX_HANDLER(SCAN_REQUEST_CMD, iwl_mvm_rx_scan_response, false),
 	RX_HANDLER(SCAN_COMPLETE_NOTIFICATION, iwl_mvm_rx_scan_complete, false),
 	RX_HANDLER(SCAN_OFFLOAD_COMPLETE,
-		   iwl_mvm_rx_scan_offload_complete_notif, false),
+		   iwl_mvm_rx_scan_offload_complete_notif, true),
 	RX_HANDLER(MATCH_FOUND_NOTIFICATION, iwl_mvm_rx_sched_scan_results,
 		   false),
 
@@ -242,7 +245,7 @@ static const struct iwl_rx_handlers iwl_mvm_rx_handlers[] = {
 #undef RX_HANDLER
 #define CMD(x) [x] = #x
 
-static const char *iwl_mvm_cmd_strings[REPLY_MAX] = {
+static const char *const iwl_mvm_cmd_strings[REPLY_MAX] = {
 	CMD(MVM_ALIVE),
 	CMD(REPLY_ERROR),
 	CMD(INIT_COMPLETE_NOTIF),
@@ -284,9 +287,11 @@ static const char *iwl_mvm_cmd_strings[REPLY_MAX] = {
 	CMD(BEACON_NOTIFICATION),
 	CMD(BEACON_TEMPLATE_CMD),
 	CMD(STATISTICS_NOTIFICATION),
+	CMD(EOSP_NOTIFICATION),
 	CMD(REDUCE_TX_POWER_CMD),
 	CMD(TX_ANT_CONFIGURATION_CMD),
 	CMD(D3_CONFIG_CMD),
+	CMD(D0I3_END_CMD),
 	CMD(PROT_OFFLOAD_CONFIG_CMD),
 	CMD(OFFLOADS_QUERY_CMD),
 	CMD(REMOTE_WAKE_CONFIG_CMD),
@@ -309,6 +314,7 @@ static const char *iwl_mvm_cmd_strings[REPLY_MAX] = {
 	CMD(BT_PROFILE_NOTIFICATION),
 	CMD(BT_CONFIG),
 	CMD(MCAST_FILTER_CMD),
+	CMD(BCAST_FILTER_CMD),
 	CMD(REPLY_SF_CFG_CMD),
 	CMD(REPLY_BEACON_FILTERING_CMD),
 	CMD(REPLY_THERMAL_MNG_BACKOFF),
@@ -320,6 +326,24 @@ static const char *iwl_mvm_cmd_strings[REPLY_MAX] = {
 
 /* this forward declaration can avoid to export the function */
 static void iwl_mvm_async_handlers_wk(struct work_struct *wk);
+static void iwl_mvm_d0i3_exit_work(struct work_struct *wk);
+
+static u32 calc_min_backoff(struct iwl_trans *trans, const struct iwl_cfg *cfg)
+{
+	const struct iwl_pwr_tx_backoff *pwr_tx_backoff = cfg->pwr_tx_backoffs;
+
+	if (!pwr_tx_backoff)
+		return 0;
+
+	while (pwr_tx_backoff->pwr) {
+		if (trans->dflt_pwr_limit >= pwr_tx_backoff->pwr)
+			return pwr_tx_backoff->backoff;
+
+		pwr_tx_backoff++;
+	}
+
+	return 0;
+}
 
 static struct iwl_op_mode *
 iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
@@ -333,6 +357,14 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 		TX_CMD,
 	};
 	int err, scan_size;
+	u32 min_backoff;
+
+	/*
+	 * We use IWL_MVM_STATION_COUNT to check the validity of the station
+	 * index all over the driver - check that its value corresponds to the
+	 * array size.
+	 */
+	BUILD_BUG_ON(ARRAY_SIZE(mvm->fw_id_to_mac_id) != IWL_MVM_STATION_COUNT);
 
 	/********************************
 	 * 1. Allocating and configuring HW data
@@ -373,6 +405,7 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 	INIT_WORK(&mvm->async_handlers_wk, iwl_mvm_async_handlers_wk);
 	INIT_WORK(&mvm->roc_done_wk, iwl_mvm_roc_done_wk);
 	INIT_WORK(&mvm->sta_drained_wk, iwl_mvm_sta_drained_wk);
+	INIT_WORK(&mvm->d0i3_exit_work, iwl_mvm_d0i3_exit_work);
 
 	SET_IEEE80211_DEV(mvm->hw, mvm->trans->dev);
 
@@ -421,7 +454,8 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 	IWL_INFO(mvm, "Detected %s, REV=0x%X\n",
 		 mvm->cfg->name, mvm->trans->hw_rev);
 
-	iwl_mvm_tt_initialize(mvm);
+	min_backoff = calc_min_backoff(trans, cfg);
+	iwl_mvm_tt_initialize(mvm, min_backoff);
 
 	/*
 	 * If the NVM exists in an external file,
@@ -462,13 +496,11 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 	if (err)
 		goto out_unregister;
 
-	if (mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_PM_CMD_SUPPORT)
-		mvm->pm_ops = &pm_mac_ops;
-	else
-		mvm->pm_ops = &pm_legacy_ops;
-
 	memset(&mvm->rx_stats, 0, sizeof(struct mvm_statistics_rx));
 
+	/* rpm starts with a taken ref. only set the appropriate bit here. */
+	set_bit(IWL_MVM_REF_UCODE_DOWN, mvm->ref_bitmap);
+
 	return op_mode;
 
  out_unregister:
@@ -508,7 +540,7 @@ static void iwl_op_mode_mvm_stop(struct iwl_op_mode *op_mode)
 	mvm->phy_db = NULL;
 
 	iwl_free_nvm_data(mvm->nvm_data);
-	for (i = 0; i < NVM_NUM_OF_SECTIONS; i++)
+	for (i = 0; i < NVM_MAX_NUM_SECTIONS; i++)
 		kfree(mvm->nvm_sections[i].data);
 
 	ieee80211_free_hw(mvm->hw);
@@ -703,6 +735,29 @@ static void iwl_mvm_nic_restart(struct iwl_mvm *mvm)
 	iwl_abort_notification_waits(&mvm->notif_wait);
 
 	/*
+	 * This is a bit racy, but worst case we tell mac80211 about
+	 * a stopped/aborted scan when that was already done which
+	 * is not a problem. It is necessary to abort any os scan
+	 * here because mac80211 requires having the scan cleared
+	 * before restarting.
+	 * We'll reset the scan_status to NONE in restart cleanup in
+	 * the next start() call from mac80211. If restart isn't called
+	 * (no fw restart) scan status will stay busy.
+	 */
+	switch (mvm->scan_status) {
+	case IWL_MVM_SCAN_NONE:
+		break;
+	case IWL_MVM_SCAN_OS:
+		ieee80211_scan_completed(mvm->hw, true);
+		break;
+	case IWL_MVM_SCAN_SCHED:
+		/* Sched scan will be restarted by mac80211 in restart_hw. */
+		if (!mvm->restart_fw)
+			ieee80211_sched_scan_stopped(mvm->hw);
+		break;
+	}
+
+	/*
 	 * If we're restarting already, don't cycle restarts.
 	 * If INIT fw asserted, it will likely fail again.
 	 * If WoWLAN fw asserted, don't restart either, mac80211
@@ -733,25 +788,8 @@ static void iwl_mvm_nic_restart(struct iwl_mvm *mvm)
 		INIT_WORK(&reprobe->work, iwl_mvm_reprobe_wk);
 		schedule_work(&reprobe->work);
 	} else if (mvm->cur_ucode == IWL_UCODE_REGULAR && mvm->restart_fw) {
-		/*
-		 * This is a bit racy, but worst case we tell mac80211 about
-		 * a stopped/aborted (sched) scan when that was already done
-		 * which is not a problem. It is necessary to abort any scan
-		 * here because mac80211 requires having the scan cleared
-		 * before restarting.
-		 * We'll reset the scan_status to NONE in restart cleanup in
-		 * the next start() call from mac80211.
-		 */
-		switch (mvm->scan_status) {
-		case IWL_MVM_SCAN_NONE:
-			break;
-		case IWL_MVM_SCAN_OS:
-			ieee80211_scan_completed(mvm->hw, true);
-			break;
-		case IWL_MVM_SCAN_SCHED:
-			ieee80211_sched_scan_stopped(mvm->hw);
-			break;
-		}
+		/* don't let the transport/FW power down */
+		iwl_mvm_ref(mvm, IWL_MVM_REF_UCODE_DOWN);
 
 		if (mvm->restart_fw > 0)
 			mvm->restart_fw--;
@@ -778,6 +816,163 @@ static void iwl_mvm_cmd_queue_full(struct iwl_op_mode *op_mode)
 	iwl_mvm_nic_restart(mvm);
 }
 
+struct iwl_d0i3_iter_data {
+	struct iwl_mvm *mvm;
+	u8 ap_sta_id;
+	u8 vif_count;
+};
+
+static void iwl_mvm_enter_d0i3_iterator(void *_data, u8 *mac,
+					struct ieee80211_vif *vif)
+{
+	struct iwl_d0i3_iter_data *data = _data;
+	struct iwl_mvm *mvm = data->mvm;
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+	u32 flags = CMD_ASYNC | CMD_HIGH_PRIO | CMD_SEND_IN_IDLE;
+
+	IWL_DEBUG_RPM(mvm, "entering D0i3 - vif %pM\n", vif->addr);
+	if (vif->type != NL80211_IFTYPE_STATION ||
+	    !vif->bss_conf.assoc)
+		return;
+
+	iwl_mvm_update_d0i3_power_mode(mvm, vif, true, flags);
+
+	/*
+	 * on init/association, mvm already configures POWER_TABLE_CMD
+	 * and REPLY_MCAST_FILTER_CMD, so currently don't
+	 * reconfigure them (we might want to use different
+	 * params later on, though).
+	 */
+	data->ap_sta_id = mvmvif->ap_sta_id;
+	data->vif_count++;
+}
+
+static int iwl_mvm_enter_d0i3(struct iwl_op_mode *op_mode)
+{
+	struct iwl_mvm *mvm = IWL_OP_MODE_GET_MVM(op_mode);
+	u32 flags = CMD_ASYNC | CMD_HIGH_PRIO | CMD_SEND_IN_IDLE;
+	int ret;
+	struct iwl_d0i3_iter_data d0i3_iter_data = {
+		.mvm = mvm,
+	};
+	struct iwl_wowlan_config_cmd wowlan_config_cmd = {
+		.wakeup_filter = cpu_to_le32(IWL_WOWLAN_WAKEUP_RX_FRAME |
+					     IWL_WOWLAN_WAKEUP_BEACON_MISS |
+					     IWL_WOWLAN_WAKEUP_LINK_CHANGE |
+					     IWL_WOWLAN_WAKEUP_BCN_FILTERING),
+	};
+	struct iwl_d3_manager_config d3_cfg_cmd = {
+		.min_sleep_time = cpu_to_le32(1000),
+	};
+
+	IWL_DEBUG_RPM(mvm, "MVM entering D0i3\n");
+
+	ieee80211_iterate_active_interfaces_atomic(mvm->hw,
+						   IEEE80211_IFACE_ITER_NORMAL,
+						   iwl_mvm_enter_d0i3_iterator,
+						   &d0i3_iter_data);
+	if (d0i3_iter_data.vif_count == 1) {
+		mvm->d0i3_ap_sta_id = d0i3_iter_data.ap_sta_id;
+	} else {
+		WARN_ON_ONCE(d0i3_iter_data.vif_count > 1);
+		mvm->d0i3_ap_sta_id = IWL_MVM_STATION_COUNT;
+	}
+
+	ret = iwl_mvm_send_cmd_pdu(mvm, WOWLAN_CONFIGURATION, flags,
+				   sizeof(wowlan_config_cmd),
+				   &wowlan_config_cmd);
+	if (ret)
+		return ret;
+
+	return iwl_mvm_send_cmd_pdu(mvm, D3_CONFIG_CMD,
+				    flags | CMD_MAKE_TRANS_IDLE,
+				    sizeof(d3_cfg_cmd), &d3_cfg_cmd);
+}
+
+static void iwl_mvm_exit_d0i3_iterator(void *_data, u8 *mac,
+				       struct ieee80211_vif *vif)
+{
+	struct iwl_mvm *mvm = _data;
+	u32 flags = CMD_ASYNC | CMD_HIGH_PRIO;
+
+	IWL_DEBUG_RPM(mvm, "exiting D0i3 - vif %pM\n", vif->addr);
+	if (vif->type != NL80211_IFTYPE_STATION ||
+	    !vif->bss_conf.assoc)
+		return;
+
+	iwl_mvm_update_d0i3_power_mode(mvm, vif, false, flags);
+}
+
+static void iwl_mvm_d0i3_disconnect_iter(void *data, u8 *mac,
+					 struct ieee80211_vif *vif)
+{
+	struct iwl_mvm *mvm = data;
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+
+	if (vif->type == NL80211_IFTYPE_STATION && vif->bss_conf.assoc &&
+	    mvm->d0i3_ap_sta_id == mvmvif->ap_sta_id)
+		ieee80211_connection_loss(vif);
+}
+
+static void iwl_mvm_d0i3_exit_work(struct work_struct *wk)
+{
+	struct iwl_mvm *mvm = container_of(wk, struct iwl_mvm, d0i3_exit_work);
+	struct iwl_host_cmd get_status_cmd = {
+		.id = WOWLAN_GET_STATUSES,
+		.flags = CMD_SYNC | CMD_HIGH_PRIO | CMD_WANT_SKB,
+	};
+	struct iwl_wowlan_status_v6 *status;
+	int ret;
+	u32 disconnection_reasons, wakeup_reasons;
+
+	mutex_lock(&mvm->mutex);
+	ret = iwl_mvm_send_cmd(mvm, &get_status_cmd);
+	if (ret)
+		goto out;
+
+	if (!get_status_cmd.resp_pkt)
+		goto out;
+
+	status = (void *)get_status_cmd.resp_pkt->data;
+	wakeup_reasons = le32_to_cpu(status->wakeup_reasons);
+
+	IWL_DEBUG_RPM(mvm, "wakeup reasons: 0x%x\n", wakeup_reasons);
+
+	disconnection_reasons =
+		IWL_WOWLAN_WAKEUP_BY_DISCONNECTION_ON_MISSED_BEACON |
+		IWL_WOWLAN_WAKEUP_BY_DISCONNECTION_ON_DEAUTH;
+	if (wakeup_reasons & disconnection_reasons)
+		ieee80211_iterate_active_interfaces(
+			mvm->hw, IEEE80211_IFACE_ITER_NORMAL,
+			iwl_mvm_d0i3_disconnect_iter, mvm);
+
+	iwl_free_resp(&get_status_cmd);
+out:
+	mutex_unlock(&mvm->mutex);
+}
+
+static int iwl_mvm_exit_d0i3(struct iwl_op_mode *op_mode)
+{
+	struct iwl_mvm *mvm = IWL_OP_MODE_GET_MVM(op_mode);
+	u32 flags = CMD_ASYNC | CMD_HIGH_PRIO | CMD_SEND_IN_IDLE |
+		    CMD_WAKE_UP_TRANS;
+	int ret;
+
+	IWL_DEBUG_RPM(mvm, "MVM exiting D0i3\n");
+
+	ret = iwl_mvm_send_cmd_pdu(mvm, D0I3_END_CMD, flags, 0, NULL);
+	if (ret)
+		goto out;
+
+	ieee80211_iterate_active_interfaces_atomic(mvm->hw,
+						   IEEE80211_IFACE_ITER_NORMAL,
+						   iwl_mvm_exit_d0i3_iterator,
+						   mvm);
+out:
+	schedule_work(&mvm->d0i3_exit_work);
+	return ret;
+}
+
 static const struct iwl_op_mode_ops iwl_mvm_ops = {
 	.start = iwl_op_mode_mvm_start,
 	.stop = iwl_op_mode_mvm_stop,
@@ -789,4 +984,6 @@ static const struct iwl_op_mode_ops iwl_mvm_ops = {
 	.nic_error = iwl_mvm_nic_error,
 	.cmd_queue_full = iwl_mvm_cmd_queue_full,
 	.nic_config = iwl_mvm_nic_config,
+	.enter_d0i3 = iwl_mvm_enter_d0i3,
+	.exit_d0i3 = iwl_mvm_exit_d0i3,
 };
diff --git a/drivers/net/wireless/iwlwifi/mvm/phy-ctxt.c b/drivers/net/wireless/iwlwifi/mvm/phy-ctxt.c
index b7268c0b3333..237efe0ac1c4 100644
--- a/drivers/net/wireless/iwlwifi/mvm/phy-ctxt.c
+++ b/drivers/net/wireless/iwlwifi/mvm/phy-ctxt.c
@@ -156,13 +156,13 @@ static void iwl_mvm_phy_ctxt_cmd_data(struct iwl_mvm *mvm,
 	idle_cnt = chains_static;
 	active_cnt = chains_dynamic;
 
-	cmd->rxchain_info = cpu_to_le32(iwl_fw_valid_rx_ant(mvm->fw) <<
+	cmd->rxchain_info = cpu_to_le32(mvm->fw->valid_rx_ant <<
 					PHY_RX_CHAIN_VALID_POS);
 	cmd->rxchain_info |= cpu_to_le32(idle_cnt << PHY_RX_CHAIN_CNT_POS);
 	cmd->rxchain_info |= cpu_to_le32(active_cnt <<
 					 PHY_RX_CHAIN_MIMO_CNT_POS);
 
-	cmd->txchain_info = cpu_to_le32(iwl_fw_valid_tx_ant(mvm->fw));
+	cmd->txchain_info = cpu_to_le32(mvm->fw->valid_tx_ant);
 }
 
 /*
diff --git a/drivers/net/wireless/iwlwifi/mvm/power.c b/drivers/net/wireless/iwlwifi/mvm/power.c
index d9eab3b7bb9f..def6ec5173b9 100644
--- a/drivers/net/wireless/iwlwifi/mvm/power.c
+++ b/drivers/net/wireless/iwlwifi/mvm/power.c
@@ -74,39 +74,36 @@
 
 #define POWER_KEEP_ALIVE_PERIOD_SEC    25
 
+static
 int iwl_mvm_beacon_filter_send_cmd(struct iwl_mvm *mvm,
-				   struct iwl_beacon_filter_cmd *cmd)
+				   struct iwl_beacon_filter_cmd *cmd,
+				   u32 flags)
 {
-	int ret;
-
-	ret = iwl_mvm_send_cmd_pdu(mvm, REPLY_BEACON_FILTERING_CMD, CMD_SYNC,
-				   sizeof(struct iwl_beacon_filter_cmd), cmd);
-
-	if (!ret) {
-		IWL_DEBUG_POWER(mvm, "ba_enable_beacon_abort is: %d\n",
-				le32_to_cpu(cmd->ba_enable_beacon_abort));
-		IWL_DEBUG_POWER(mvm, "ba_escape_timer is: %d\n",
-				le32_to_cpu(cmd->ba_escape_timer));
-		IWL_DEBUG_POWER(mvm, "bf_debug_flag is: %d\n",
-				le32_to_cpu(cmd->bf_debug_flag));
-		IWL_DEBUG_POWER(mvm, "bf_enable_beacon_filter is: %d\n",
-				le32_to_cpu(cmd->bf_enable_beacon_filter));
-		IWL_DEBUG_POWER(mvm, "bf_energy_delta is: %d\n",
-				le32_to_cpu(cmd->bf_energy_delta));
-		IWL_DEBUG_POWER(mvm, "bf_escape_timer is: %d\n",
-				le32_to_cpu(cmd->bf_escape_timer));
-		IWL_DEBUG_POWER(mvm, "bf_roaming_energy_delta is: %d\n",
-				le32_to_cpu(cmd->bf_roaming_energy_delta));
-		IWL_DEBUG_POWER(mvm, "bf_roaming_state is: %d\n",
-				le32_to_cpu(cmd->bf_roaming_state));
-		IWL_DEBUG_POWER(mvm, "bf_temp_threshold is: %d\n",
-				le32_to_cpu(cmd->bf_temp_threshold));
-		IWL_DEBUG_POWER(mvm, "bf_temp_fast_filter is: %d\n",
-				le32_to_cpu(cmd->bf_temp_fast_filter));
-		IWL_DEBUG_POWER(mvm, "bf_temp_slow_filter is: %d\n",
-				le32_to_cpu(cmd->bf_temp_slow_filter));
-	}
-	return ret;
+	IWL_DEBUG_POWER(mvm, "ba_enable_beacon_abort is: %d\n",
+			le32_to_cpu(cmd->ba_enable_beacon_abort));
+	IWL_DEBUG_POWER(mvm, "ba_escape_timer is: %d\n",
+			le32_to_cpu(cmd->ba_escape_timer));
+	IWL_DEBUG_POWER(mvm, "bf_debug_flag is: %d\n",
+			le32_to_cpu(cmd->bf_debug_flag));
+	IWL_DEBUG_POWER(mvm, "bf_enable_beacon_filter is: %d\n",
+			le32_to_cpu(cmd->bf_enable_beacon_filter));
+	IWL_DEBUG_POWER(mvm, "bf_energy_delta is: %d\n",
+			le32_to_cpu(cmd->bf_energy_delta));
+	IWL_DEBUG_POWER(mvm, "bf_escape_timer is: %d\n",
+			le32_to_cpu(cmd->bf_escape_timer));
+	IWL_DEBUG_POWER(mvm, "bf_roaming_energy_delta is: %d\n",
+			le32_to_cpu(cmd->bf_roaming_energy_delta));
+	IWL_DEBUG_POWER(mvm, "bf_roaming_state is: %d\n",
+			le32_to_cpu(cmd->bf_roaming_state));
+	IWL_DEBUG_POWER(mvm, "bf_temp_threshold is: %d\n",
+			le32_to_cpu(cmd->bf_temp_threshold));
+	IWL_DEBUG_POWER(mvm, "bf_temp_fast_filter is: %d\n",
+			le32_to_cpu(cmd->bf_temp_fast_filter));
+	IWL_DEBUG_POWER(mvm, "bf_temp_slow_filter is: %d\n",
+			le32_to_cpu(cmd->bf_temp_slow_filter));
+
+	return iwl_mvm_send_cmd_pdu(mvm, REPLY_BEACON_FILTERING_CMD, flags,
+				    sizeof(struct iwl_beacon_filter_cmd), cmd);
 }
 
 static
@@ -145,7 +142,7 @@ int iwl_mvm_update_beacon_abort(struct iwl_mvm *mvm,
 	mvmvif->bf_data.ba_enabled = enable;
 	iwl_mvm_beacon_filter_set_cqm_params(mvm, vif, &cmd);
 	iwl_mvm_beacon_filter_debugfs_parameters(vif, &cmd);
-	return iwl_mvm_beacon_filter_send_cmd(mvm, &cmd);
+	return iwl_mvm_beacon_filter_send_cmd(mvm, &cmd, CMD_SYNC);
 }
 
 static void iwl_mvm_power_log(struct iwl_mvm *mvm,
@@ -301,8 +298,7 @@ static void iwl_mvm_power_build_cmd(struct iwl_mvm *mvm,
 	keep_alive = DIV_ROUND_UP(keep_alive, MSEC_PER_SEC);
 	cmd->keep_alive_seconds = cpu_to_le16(keep_alive);
 
-	if (iwlmvm_mod_params.power_scheme == IWL_POWER_SCHEME_CAM ||
-	    mvm->ps_prevented)
+	if (mvm->ps_disabled)
 		return;
 
 	cmd->flags |= cpu_to_le16(POWER_FLAGS_POWER_SAVE_ENA_MSK);
@@ -312,7 +308,8 @@ static void iwl_mvm_power_build_cmd(struct iwl_mvm *mvm,
 	    mvmvif->dbgfs_pm.disable_power_off)
 		cmd->flags &= cpu_to_le16(~POWER_FLAGS_POWER_SAVE_ENA_MSK);
 #endif
-	if (!vif->bss_conf.ps || mvmvif->pm_prevented)
+	if (!vif->bss_conf.ps || iwl_mvm_vif_low_latency(mvmvif) ||
+	    mvm->pm_disabled)
 		return;
 
 	cmd->flags |= cpu_to_le16(POWER_FLAGS_POWER_MANAGEMENT_ENA_MSK);
@@ -419,72 +416,44 @@ static void iwl_mvm_power_build_cmd(struct iwl_mvm *mvm,
 #endif /* CONFIG_IWLWIFI_DEBUGFS */
 }
 
-static int iwl_mvm_power_mac_update_mode(struct iwl_mvm *mvm,
+static int iwl_mvm_power_send_cmd(struct iwl_mvm *mvm,
 					 struct ieee80211_vif *vif)
 {
-	int ret;
-	bool ba_enable;
 	struct iwl_mac_power_cmd cmd = {};
 
 	if (vif->type != NL80211_IFTYPE_STATION)
 		return 0;
 
 	if (vif->p2p &&
-	    !(mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_P2P_PS))
+	    !(mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_BSS_P2P_PS_DCM))
 		return 0;
 
 	iwl_mvm_power_build_cmd(mvm, vif, &cmd);
 	iwl_mvm_power_log(mvm, &cmd);
-
-	ret = iwl_mvm_send_cmd_pdu(mvm, MAC_PM_POWER_TABLE, CMD_SYNC,
-				   sizeof(cmd), &cmd);
-	if (ret)
-		return ret;
-
-	ba_enable = !!(cmd.flags &
-		       cpu_to_le16(POWER_FLAGS_POWER_MANAGEMENT_ENA_MSK));
-
-	return iwl_mvm_update_beacon_abort(mvm, vif, ba_enable);
-}
-
-static int iwl_mvm_power_mac_disable(struct iwl_mvm *mvm,
-				     struct ieee80211_vif *vif)
-{
-	struct iwl_mac_power_cmd cmd = {};
-	struct iwl_mvm_vif *mvmvif __maybe_unused =
-		iwl_mvm_vif_from_mac80211(vif);
-
-	if (vif->type != NL80211_IFTYPE_STATION || vif->p2p)
-		return 0;
-
-	cmd.id_and_color = cpu_to_le32(FW_CMD_ID_AND_COLOR(mvmvif->id,
-							   mvmvif->color));
-
-	if (iwlmvm_mod_params.power_scheme != IWL_POWER_SCHEME_CAM)
-		cmd.flags |= cpu_to_le16(POWER_FLAGS_POWER_SAVE_ENA_MSK);
-
 #ifdef CONFIG_IWLWIFI_DEBUGFS
-	if (mvmvif->dbgfs_pm.mask & MVM_DEBUGFS_PM_DISABLE_POWER_OFF &&
-	    mvmvif->dbgfs_pm.disable_power_off)
-		cmd.flags &= cpu_to_le16(~POWER_FLAGS_POWER_SAVE_ENA_MSK);
+	memcpy(&iwl_mvm_vif_from_mac80211(vif)->mac_pwr_cmd, &cmd, sizeof(cmd));
 #endif
-	iwl_mvm_power_log(mvm, &cmd);
 
-	return iwl_mvm_send_cmd_pdu(mvm, MAC_PM_POWER_TABLE, CMD_ASYNC,
+	return iwl_mvm_send_cmd_pdu(mvm, MAC_PM_POWER_TABLE, CMD_SYNC,
 				    sizeof(cmd), &cmd);
 }
 
-static int _iwl_mvm_power_update_device(struct iwl_mvm *mvm, bool force_disable)
+int iwl_mvm_power_update_device(struct iwl_mvm *mvm)
 {
 	struct iwl_device_power_cmd cmd = {
 		.flags = cpu_to_le16(DEVICE_POWER_FLAGS_POWER_SAVE_ENA_MSK),
 	};
 
+	if (!(mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_PM_CMD_SUPPORT))
+		return 0;
+
 	if (!(mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_DEVICE_PS_CMD))
 		return 0;
 
-	if (iwlmvm_mod_params.power_scheme == IWL_POWER_SCHEME_CAM ||
-	    force_disable)
+	if (iwlmvm_mod_params.power_scheme == IWL_POWER_SCHEME_CAM)
+		mvm->ps_disabled = true;
+
+	if (mvm->ps_disabled)
 		cmd.flags |= cpu_to_le16(DEVICE_POWER_FLAGS_CAM_MSK);
 
 #ifdef CONFIG_IWLWIFI_DEBUGFS
@@ -501,11 +470,6 @@ static int _iwl_mvm_power_update_device(struct iwl_mvm *mvm, bool force_disable)
 				    &cmd);
 }
 
-static int iwl_mvm_power_update_device(struct iwl_mvm *mvm)
-{
-	return _iwl_mvm_power_update_device(mvm, false);
-}
-
 void iwl_mvm_power_vif_assoc(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 {
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
@@ -544,44 +508,169 @@ int iwl_mvm_power_uapsd_misbehaving_ap_notif(struct iwl_mvm *mvm,
 	return 0;
 }
 
-static void iwl_mvm_power_binding_iterator(void *_data, u8 *mac,
-					   struct ieee80211_vif *vif)
+struct iwl_power_constraint {
+	struct ieee80211_vif *bf_vif;
+	struct ieee80211_vif *bss_vif;
+	u16 bss_phyctx_id;
+	u16 p2p_phyctx_id;
+	bool pm_disabled;
+	bool ps_disabled;
+	struct iwl_mvm *mvm;
+};
+
+static void iwl_mvm_power_iterator(void *_data, u8 *mac,
+				   struct ieee80211_vif *vif)
 {
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
-	struct iwl_mvm *mvm = _data;
-	int ret;
+	struct iwl_power_constraint *power_iterator = _data;
+	struct iwl_mvm *mvm = power_iterator->mvm;
+
+	switch (ieee80211_vif_type_p2p(vif)) {
+	case NL80211_IFTYPE_P2P_DEVICE:
+		break;
+
+	case NL80211_IFTYPE_P2P_GO:
+	case NL80211_IFTYPE_AP:
+		/* no BSS power mgmt if we have an active AP */
+		if (mvmvif->ap_ibss_active)
+			power_iterator->pm_disabled = true;
+		break;
+
+	case NL80211_IFTYPE_MONITOR:
+		/* no BSS power mgmt and no device power save */
+		power_iterator->pm_disabled = true;
+		power_iterator->ps_disabled = true;
+		break;
+
+	case NL80211_IFTYPE_P2P_CLIENT:
+		if (mvmvif->phy_ctxt)
+			power_iterator->p2p_phyctx_id = mvmvif->phy_ctxt->id;
+
+		IWL_DEBUG_POWER(mvm, "p2p: p2p_id=%d, bss_id=%d\n",
+				power_iterator->p2p_phyctx_id,
+				power_iterator->bss_phyctx_id);
+		if (!(mvm->fw->ucode_capa.flags &
+		      IWL_UCODE_TLV_FLAGS_BSS_P2P_PS_DCM)) {
+			/* no BSS power mgmt if we have a P2P client*/
+			power_iterator->pm_disabled = true;
+		} else if (power_iterator->p2p_phyctx_id < MAX_PHYS &&
+			   power_iterator->bss_phyctx_id < MAX_PHYS &&
+			   power_iterator->p2p_phyctx_id ==
+			   power_iterator->bss_phyctx_id) {
+			power_iterator->pm_disabled = true;
+		}
+		break;
+
+	case NL80211_IFTYPE_STATION:
+		if (mvmvif->phy_ctxt)
+			power_iterator->bss_phyctx_id = mvmvif->phy_ctxt->id;
+
+		/* we should have only one BSS vif */
+		WARN_ON(power_iterator->bss_vif);
+		power_iterator->bss_vif = vif;
+
+		if (mvmvif->bf_data.bf_enabled &&
+		    !WARN_ON(power_iterator->bf_vif))
+			power_iterator->bf_vif = vif;
+
+		IWL_DEBUG_POWER(mvm, "bss: p2p_id=%d, bss_id=%d\n",
+				power_iterator->p2p_phyctx_id,
+				power_iterator->bss_phyctx_id);
+		if (mvm->fw->ucode_capa.flags &
+		    IWL_UCODE_TLV_FLAGS_BSS_P2P_PS_DCM &&
+			(power_iterator->p2p_phyctx_id < MAX_PHYS &&
+			 power_iterator->bss_phyctx_id < MAX_PHYS &&
+			 power_iterator->p2p_phyctx_id ==
+			 power_iterator->bss_phyctx_id))
+			power_iterator->pm_disabled = true;
+		break;
+
+	default:
+		break;
+	}
+}
 
-	mvmvif->pm_prevented = (mvm->bound_vif_cnt <= 1) ? false : true;
+static void
+iwl_mvm_power_get_global_constraint(struct iwl_mvm *mvm,
+				    struct iwl_power_constraint *constraint)
+{
+	lockdep_assert_held(&mvm->mutex);
+
+	if (iwlmvm_mod_params.power_scheme == IWL_POWER_SCHEME_CAM) {
+		constraint->pm_disabled = true;
+		constraint->ps_disabled = true;
+	}
 
-	ret = iwl_mvm_power_mac_update_mode(mvm, vif);
-	WARN_ONCE(ret, "Failed to update power parameters on a specific vif\n");
+	ieee80211_iterate_active_interfaces_atomic(mvm->hw,
+					    IEEE80211_IFACE_ITER_NORMAL,
+					    iwl_mvm_power_iterator, constraint);
 }
 
-static void _iwl_mvm_power_update_binding(struct iwl_mvm *mvm,
-					  struct ieee80211_vif *vif,
-					  bool assign)
+int iwl_mvm_power_update_mac(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 {
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+	struct iwl_power_constraint constraint = {
+		    .p2p_phyctx_id = MAX_PHYS,
+		    .bss_phyctx_id = MAX_PHYS,
+		    .mvm = mvm,
+	};
+	bool ba_enable;
+	int ret;
+
+	lockdep_assert_held(&mvm->mutex);
+
+	if (!(mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_PM_CMD_SUPPORT))
+		return 0;
+
+	iwl_mvm_power_get_global_constraint(mvm, &constraint);
+	mvm->ps_disabled = constraint.ps_disabled;
+	mvm->pm_disabled = constraint.pm_disabled;
+
+	/* don't update device power state unless we add / remove monitor */
 	if (vif->type == NL80211_IFTYPE_MONITOR) {
-		int ret = _iwl_mvm_power_update_device(mvm, assign);
-		mvm->ps_prevented = assign;
-		WARN_ONCE(ret, "Failed to update power device state\n");
+		ret = iwl_mvm_power_update_device(mvm);
+		if (ret)
+			return ret;
 	}
 
-	ieee80211_iterate_active_interfaces(mvm->hw,
-					    IEEE80211_IFACE_ITER_NORMAL,
-					    iwl_mvm_power_binding_iterator,
-					    mvm);
+	ret = iwl_mvm_power_send_cmd(mvm, vif);
+	if (ret)
+		return ret;
+
+	if (constraint.bss_vif && vif != constraint.bss_vif) {
+		ret = iwl_mvm_power_send_cmd(mvm, constraint.bss_vif);
+		if (ret)
+			return ret;
+	}
+
+	if (!constraint.bf_vif)
+		return 0;
+
+	vif = constraint.bf_vif;
+	mvmvif = iwl_mvm_vif_from_mac80211(vif);
+
+	ba_enable = !(constraint.pm_disabled || constraint.ps_disabled ||
+		      !vif->bss_conf.ps || iwl_mvm_vif_low_latency(mvmvif));
+
+	return iwl_mvm_update_beacon_abort(mvm, constraint.bf_vif, ba_enable);
 }
 
 #ifdef CONFIG_IWLWIFI_DEBUGFS
-static int iwl_mvm_power_mac_dbgfs_read(struct iwl_mvm *mvm,
-					struct ieee80211_vif *vif, char *buf,
-					int bufsz)
+int iwl_mvm_power_mac_dbgfs_read(struct iwl_mvm *mvm,
+				 struct ieee80211_vif *vif, char *buf,
+				 int bufsz)
 {
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 	struct iwl_mac_power_cmd cmd = {};
 	int pos = 0;
 
-	iwl_mvm_power_build_cmd(mvm, vif, &cmd);
+	if (WARN_ON(!(mvm->fw->ucode_capa.flags &
+		      IWL_UCODE_TLV_FLAGS_PM_CMD_SUPPORT)))
+		return 0;
+
+	mutex_lock(&mvm->mutex);
+	memcpy(&cmd, &mvmvif->mac_pwr_cmd, sizeof(cmd));
+	mutex_unlock(&mvm->mutex);
 
 	if (!(mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_DEVICE_PS_CMD))
 		pos += scnprintf(buf+pos, bufsz-pos, "disable_power_off = %d\n",
@@ -685,32 +774,46 @@ iwl_mvm_beacon_filter_debugfs_parameters(struct ieee80211_vif *vif,
 }
 #endif
 
-int iwl_mvm_enable_beacon_filter(struct iwl_mvm *mvm,
-				 struct ieee80211_vif *vif)
+static int _iwl_mvm_enable_beacon_filter(struct iwl_mvm *mvm,
+					 struct ieee80211_vif *vif,
+					 struct iwl_beacon_filter_cmd *cmd,
+					 u32 cmd_flags,
+					 bool d0i3)
 {
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
-	struct iwl_beacon_filter_cmd cmd = {
-		IWL_BF_CMD_CONFIG_DEFAULTS,
-		.bf_enable_beacon_filter = cpu_to_le32(1),
-	};
 	int ret;
 
 	if (mvmvif != mvm->bf_allowed_vif ||
 	    vif->type != NL80211_IFTYPE_STATION || vif->p2p)
 		return 0;
 
-	iwl_mvm_beacon_filter_set_cqm_params(mvm, vif, &cmd);
-	iwl_mvm_beacon_filter_debugfs_parameters(vif, &cmd);
-	ret = iwl_mvm_beacon_filter_send_cmd(mvm, &cmd);
+	iwl_mvm_beacon_filter_set_cqm_params(mvm, vif, cmd);
+	if (!d0i3)
+		iwl_mvm_beacon_filter_debugfs_parameters(vif, cmd);
+	ret = iwl_mvm_beacon_filter_send_cmd(mvm, cmd, cmd_flags);
 
-	if (!ret)
+	/* don't change bf_enabled in case of temporary d0i3 configuration */
+	if (!ret && !d0i3)
 		mvmvif->bf_data.bf_enabled = true;
 
 	return ret;
 }
 
+int iwl_mvm_enable_beacon_filter(struct iwl_mvm *mvm,
+				 struct ieee80211_vif *vif,
+				 u32 flags)
+{
+	struct iwl_beacon_filter_cmd cmd = {
+		IWL_BF_CMD_CONFIG_DEFAULTS,
+		.bf_enable_beacon_filter = cpu_to_le32(1),
+	};
+
+	return _iwl_mvm_enable_beacon_filter(mvm, vif, &cmd, flags, false);
+}
+
 int iwl_mvm_disable_beacon_filter(struct iwl_mvm *mvm,
-				  struct ieee80211_vif *vif)
+				  struct ieee80211_vif *vif,
+				  u32 flags)
 {
 	struct iwl_beacon_filter_cmd cmd = {};
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
@@ -720,7 +823,7 @@ int iwl_mvm_disable_beacon_filter(struct iwl_mvm *mvm,
 	    vif->type != NL80211_IFTYPE_STATION || vif->p2p)
 		return 0;
 
-	ret = iwl_mvm_beacon_filter_send_cmd(mvm, &cmd);
+	ret = iwl_mvm_beacon_filter_send_cmd(mvm, &cmd, flags);
 
 	if (!ret)
 		mvmvif->bf_data.bf_enabled = false;
@@ -728,23 +831,89 @@ int iwl_mvm_disable_beacon_filter(struct iwl_mvm *mvm,
 	return ret;
 }
 
-int iwl_mvm_update_beacon_filter(struct iwl_mvm *mvm,
-				 struct ieee80211_vif *vif)
+int iwl_mvm_update_d0i3_power_mode(struct iwl_mvm *mvm,
+				   struct ieee80211_vif *vif,
+				   bool enable, u32 flags)
 {
+	int ret;
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+	struct iwl_mac_power_cmd cmd = {};
 
-	if (!mvmvif->bf_data.bf_enabled)
+	if (vif->type != NL80211_IFTYPE_STATION || vif->p2p)
 		return 0;
 
-	return iwl_mvm_enable_beacon_filter(mvm, vif);
-}
+	if (!vif->bss_conf.assoc)
+		return 0;
 
-const struct iwl_mvm_power_ops pm_mac_ops = {
-	.power_update_mode = iwl_mvm_power_mac_update_mode,
-	.power_update_device_mode = iwl_mvm_power_update_device,
-	.power_disable = iwl_mvm_power_mac_disable,
-	.power_update_binding = _iwl_mvm_power_update_binding,
+	iwl_mvm_power_build_cmd(mvm, vif, &cmd);
+	if (enable) {
+		/* configure skip over dtim up to 300 msec */
+		int dtimper = mvm->hw->conf.ps_dtim_period ?: 1;
+		int dtimper_msec = dtimper * vif->bss_conf.beacon_int;
+
+		if (WARN_ON(!dtimper_msec))
+			return 0;
+
+		cmd.flags |=
+			cpu_to_le16(POWER_FLAGS_SKIP_OVER_DTIM_MSK);
+		cmd.skip_dtim_periods = 300 / dtimper_msec;
+	}
+	iwl_mvm_power_log(mvm, &cmd);
 #ifdef CONFIG_IWLWIFI_DEBUGFS
-	.power_dbgfs_read = iwl_mvm_power_mac_dbgfs_read,
+	memcpy(&mvmvif->mac_pwr_cmd, &cmd, sizeof(cmd));
 #endif
-};
+	ret = iwl_mvm_send_cmd_pdu(mvm, MAC_PM_POWER_TABLE, flags,
+				   sizeof(cmd), &cmd);
+	if (ret)
+		return ret;
+
+	/* configure beacon filtering */
+	if (mvmvif != mvm->bf_allowed_vif)
+		return 0;
+
+	if (enable) {
+		struct iwl_beacon_filter_cmd cmd_bf = {
+			IWL_BF_CMD_CONFIG_D0I3,
+			.bf_enable_beacon_filter = cpu_to_le32(1),
+		};
+		ret = _iwl_mvm_enable_beacon_filter(mvm, vif, &cmd_bf,
+						    flags, true);
+	} else {
+		if (mvmvif->bf_data.bf_enabled)
+			ret = iwl_mvm_enable_beacon_filter(mvm, vif, flags);
+		else
+			ret = iwl_mvm_disable_beacon_filter(mvm, vif, flags);
+	}
+
+	return ret;
+}
+
+int iwl_mvm_update_beacon_filter(struct iwl_mvm *mvm,
+				 struct ieee80211_vif *vif,
+				 bool force,
+				 u32 flags)
+{
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+
+	if (mvmvif != mvm->bf_allowed_vif)
+		return 0;
+
+	if (!mvmvif->bf_data.bf_enabled) {
+		/* disable beacon filtering explicitly if force is true */
+		if (force)
+			return iwl_mvm_disable_beacon_filter(mvm, vif, flags);
+		return 0;
+	}
+
+	return iwl_mvm_enable_beacon_filter(mvm, vif, flags);
+}
+
+int iwl_power_legacy_set_cam_mode(struct iwl_mvm *mvm)
+{
+	struct iwl_powertable_cmd cmd = {
+		.keep_alive_seconds = POWER_KEEP_ALIVE_PERIOD_SEC,
+	};
+
+	return iwl_mvm_send_cmd_pdu(mvm, POWER_TABLE_CMD, CMD_SYNC,
+				    sizeof(cmd), &cmd);
+}
diff --git a/drivers/net/wireless/iwlwifi/mvm/power_legacy.c b/drivers/net/wireless/iwlwifi/mvm/power_legacy.c
deleted file mode 100644
index ef712ae5bc62..000000000000
--- a/drivers/net/wireless/iwlwifi/mvm/power_legacy.c
+++ /dev/null
@@ -1,319 +0,0 @@
-/******************************************************************************
- *
- * This file is provided under a dual BSD/GPLv2 license.  When using or
- * redistributing this file, you may do so under either license.
- *
- * GPL LICENSE SUMMARY
- *
- * Copyright(c) 2012 - 2014 Intel 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
- *
- * The full GNU General Public License is included in this distribution
- * in the file called COPYING.
- *
- * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
- * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
- *
- * BSD LICENSE
- *
- * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- *  * Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- *  * Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- *  * Neither the name Intel Corporation nor the names of its
- *    contributors may be used to endorse or promote products derived
- *    from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
- * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
- * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
- * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
- * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
- * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- *****************************************************************************/
-
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/slab.h>
-#include <linux/init.h>
-
-#include <net/mac80211.h>
-
-#include "iwl-debug.h"
-#include "mvm.h"
-#include "iwl-modparams.h"
-#include "fw-api-power.h"
-
-#define POWER_KEEP_ALIVE_PERIOD_SEC    25
-
-static void iwl_mvm_power_log(struct iwl_mvm *mvm,
-			      struct iwl_powertable_cmd *cmd)
-{
-	IWL_DEBUG_POWER(mvm,
-			"Sending power table command for power level %d, flags = 0x%X\n",
-			iwlmvm_mod_params.power_scheme,
-			le16_to_cpu(cmd->flags));
-	IWL_DEBUG_POWER(mvm, "Keep alive = %u sec\n", cmd->keep_alive_seconds);
-
-	if (cmd->flags & cpu_to_le16(POWER_FLAGS_POWER_MANAGEMENT_ENA_MSK)) {
-		IWL_DEBUG_POWER(mvm, "Rx timeout = %u usec\n",
-				le32_to_cpu(cmd->rx_data_timeout));
-		IWL_DEBUG_POWER(mvm, "Tx timeout = %u usec\n",
-				le32_to_cpu(cmd->tx_data_timeout));
-		if (cmd->flags & cpu_to_le16(POWER_FLAGS_SKIP_OVER_DTIM_MSK))
-			IWL_DEBUG_POWER(mvm, "DTIM periods to skip = %u\n",
-					le32_to_cpu(cmd->skip_dtim_periods));
-		if (cmd->flags & cpu_to_le16(POWER_FLAGS_LPRX_ENA_MSK))
-			IWL_DEBUG_POWER(mvm, "LP RX RSSI threshold = %u\n",
-					le32_to_cpu(cmd->lprx_rssi_threshold));
-	}
-}
-
-static void iwl_mvm_power_build_cmd(struct iwl_mvm *mvm,
-				    struct ieee80211_vif *vif,
-				    struct iwl_powertable_cmd *cmd)
-{
-	struct ieee80211_hw *hw = mvm->hw;
-	struct ieee80211_chanctx_conf *chanctx_conf;
-	struct ieee80211_channel *chan;
-	int dtimper, dtimper_msec;
-	int keep_alive;
-	bool radar_detect = false;
-	struct iwl_mvm_vif *mvmvif __maybe_unused =
-		iwl_mvm_vif_from_mac80211(vif);
-
-	/*
-	 * Regardless of power management state the driver must set
-	 * keep alive period. FW will use it for sending keep alive NDPs
-	 * immediately after association.
-	 */
-	cmd->keep_alive_seconds = POWER_KEEP_ALIVE_PERIOD_SEC;
-
-	if (iwlmvm_mod_params.power_scheme == IWL_POWER_SCHEME_CAM)
-		return;
-
-	cmd->flags |= cpu_to_le16(POWER_FLAGS_POWER_SAVE_ENA_MSK);
-	if (!vif->bss_conf.assoc)
-		cmd->flags |= cpu_to_le16(POWER_FLAGS_POWER_MANAGEMENT_ENA_MSK);
-
-#ifdef CONFIG_IWLWIFI_DEBUGFS
-	if (mvmvif->dbgfs_pm.mask & MVM_DEBUGFS_PM_DISABLE_POWER_OFF &&
-	    mvmvif->dbgfs_pm.disable_power_off)
-		cmd->flags &= cpu_to_le16(~POWER_FLAGS_POWER_SAVE_ENA_MSK);
-#endif
-	if (!vif->bss_conf.ps)
-		return;
-
-	cmd->flags |= cpu_to_le16(POWER_FLAGS_POWER_MANAGEMENT_ENA_MSK);
-
-	if (vif->bss_conf.beacon_rate &&
-	    (vif->bss_conf.beacon_rate->bitrate == 10 ||
-	     vif->bss_conf.beacon_rate->bitrate == 60)) {
-		cmd->flags |= cpu_to_le16(POWER_FLAGS_LPRX_ENA_MSK);
-		cmd->lprx_rssi_threshold =
-			cpu_to_le32(POWER_LPRX_RSSI_THRESHOLD);
-	}
-
-	dtimper = hw->conf.ps_dtim_period ?: 1;
-
-	/* Check if radar detection is required on current channel */
-	rcu_read_lock();
-	chanctx_conf = rcu_dereference(vif->chanctx_conf);
-	WARN_ON(!chanctx_conf);
-	if (chanctx_conf) {
-		chan = chanctx_conf->def.chan;
-		radar_detect = chan->flags & IEEE80211_CHAN_RADAR;
-	}
-	rcu_read_unlock();
-
-	/* Check skip over DTIM conditions */
-	if (!radar_detect && (dtimper <= 10) &&
-	    (iwlmvm_mod_params.power_scheme == IWL_POWER_SCHEME_LP ||
-	     mvm->cur_ucode == IWL_UCODE_WOWLAN)) {
-		cmd->flags |= cpu_to_le16(POWER_FLAGS_SKIP_OVER_DTIM_MSK);
-		cmd->skip_dtim_periods = cpu_to_le32(3);
-	}
-
-	/* Check that keep alive period is at least 3 * DTIM */
-	dtimper_msec = dtimper * vif->bss_conf.beacon_int;
-	keep_alive = max_t(int, 3 * dtimper_msec,
-			   MSEC_PER_SEC * cmd->keep_alive_seconds);
-	keep_alive = DIV_ROUND_UP(keep_alive, MSEC_PER_SEC);
-	cmd->keep_alive_seconds = keep_alive;
-
-	if (mvm->cur_ucode != IWL_UCODE_WOWLAN) {
-		cmd->rx_data_timeout = cpu_to_le32(100 * USEC_PER_MSEC);
-		cmd->tx_data_timeout = cpu_to_le32(100 * USEC_PER_MSEC);
-	} else {
-		cmd->rx_data_timeout = cpu_to_le32(10 * USEC_PER_MSEC);
-		cmd->tx_data_timeout = cpu_to_le32(10 * USEC_PER_MSEC);
-	}
-
-#ifdef CONFIG_IWLWIFI_DEBUGFS
-	if (mvmvif->dbgfs_pm.mask & MVM_DEBUGFS_PM_KEEP_ALIVE)
-		cmd->keep_alive_seconds = mvmvif->dbgfs_pm.keep_alive_seconds;
-	if (mvmvif->dbgfs_pm.mask & MVM_DEBUGFS_PM_SKIP_OVER_DTIM) {
-		if (mvmvif->dbgfs_pm.skip_over_dtim)
-			cmd->flags |=
-				cpu_to_le16(POWER_FLAGS_SKIP_OVER_DTIM_MSK);
-		else
-			cmd->flags &=
-				cpu_to_le16(~POWER_FLAGS_SKIP_OVER_DTIM_MSK);
-	}
-	if (mvmvif->dbgfs_pm.mask & MVM_DEBUGFS_PM_RX_DATA_TIMEOUT)
-		cmd->rx_data_timeout =
-			cpu_to_le32(mvmvif->dbgfs_pm.rx_data_timeout);
-	if (mvmvif->dbgfs_pm.mask & MVM_DEBUGFS_PM_TX_DATA_TIMEOUT)
-		cmd->tx_data_timeout =
-			cpu_to_le32(mvmvif->dbgfs_pm.tx_data_timeout);
-	if (mvmvif->dbgfs_pm.mask & MVM_DEBUGFS_PM_SKIP_DTIM_PERIODS)
-		cmd->skip_dtim_periods =
-			cpu_to_le32(mvmvif->dbgfs_pm.skip_dtim_periods);
-	if (mvmvif->dbgfs_pm.mask & MVM_DEBUGFS_PM_LPRX_ENA) {
-		if (mvmvif->dbgfs_pm.lprx_ena)
-			cmd->flags |= cpu_to_le16(POWER_FLAGS_LPRX_ENA_MSK);
-		else
-			cmd->flags &= cpu_to_le16(~POWER_FLAGS_LPRX_ENA_MSK);
-	}
-	if (mvmvif->dbgfs_pm.mask & MVM_DEBUGFS_PM_LPRX_RSSI_THRESHOLD)
-		cmd->lprx_rssi_threshold =
-			cpu_to_le32(mvmvif->dbgfs_pm.lprx_rssi_threshold);
-#endif /* CONFIG_IWLWIFI_DEBUGFS */
-}
-
-static int iwl_mvm_power_legacy_update_mode(struct iwl_mvm *mvm,
-					    struct ieee80211_vif *vif)
-{
-	int ret;
-	bool ba_enable;
-	struct iwl_powertable_cmd cmd = {};
-
-	if (vif->type != NL80211_IFTYPE_STATION || vif->p2p)
-		return 0;
-
-	/*
-	 * TODO: The following vif_count verification is temporary condition.
-	 * Avoid power mode update if more than one interface is currently
-	 * active. Remove this condition when FW will support power management
-	 * on multiple MACs.
-	 */
-	IWL_DEBUG_POWER(mvm, "Currently %d interfaces active\n",
-			mvm->vif_count);
-	if (mvm->vif_count > 1)
-		return 0;
-
-	iwl_mvm_power_build_cmd(mvm, vif, &cmd);
-	iwl_mvm_power_log(mvm, &cmd);
-
-	ret = iwl_mvm_send_cmd_pdu(mvm, POWER_TABLE_CMD, CMD_SYNC,
-				   sizeof(cmd), &cmd);
-	if (ret)
-		return ret;
-
-	ba_enable = !!(cmd.flags &
-		       cpu_to_le16(POWER_FLAGS_POWER_MANAGEMENT_ENA_MSK));
-
-	return iwl_mvm_update_beacon_abort(mvm, vif, ba_enable);
-}
-
-static int iwl_mvm_power_legacy_disable(struct iwl_mvm *mvm,
-					struct ieee80211_vif *vif)
-{
-	struct iwl_powertable_cmd cmd = {};
-	struct iwl_mvm_vif *mvmvif __maybe_unused =
-		iwl_mvm_vif_from_mac80211(vif);
-
-	if (vif->type != NL80211_IFTYPE_STATION || vif->p2p)
-		return 0;
-
-	if (iwlmvm_mod_params.power_scheme != IWL_POWER_SCHEME_CAM)
-		cmd.flags |= cpu_to_le16(POWER_FLAGS_POWER_SAVE_ENA_MSK);
-
-#ifdef CONFIG_IWLWIFI_DEBUGFS
-	if (mvmvif->dbgfs_pm.mask & MVM_DEBUGFS_PM_DISABLE_POWER_OFF &&
-	    mvmvif->dbgfs_pm.disable_power_off)
-		cmd.flags &= cpu_to_le16(~POWER_FLAGS_POWER_SAVE_ENA_MSK);
-#endif
-	iwl_mvm_power_log(mvm, &cmd);
-
-	return iwl_mvm_send_cmd_pdu(mvm, POWER_TABLE_CMD, CMD_ASYNC,
-				    sizeof(cmd), &cmd);
-}
-
-#ifdef CONFIG_IWLWIFI_DEBUGFS
-static int iwl_mvm_power_legacy_dbgfs_read(struct iwl_mvm *mvm,
-					   struct ieee80211_vif *vif, char *buf,
-					   int bufsz)
-{
-	struct iwl_powertable_cmd cmd = {};
-	int pos = 0;
-
-	iwl_mvm_power_build_cmd(mvm, vif, &cmd);
-
-	pos += scnprintf(buf+pos, bufsz-pos, "disable_power_off = %d\n",
-			 (cmd.flags &
-			 cpu_to_le16(POWER_FLAGS_POWER_SAVE_ENA_MSK)) ?
-			 0 : 1);
-	pos += scnprintf(buf+pos, bufsz-pos, "skip_dtim_periods = %d\n",
-			 le32_to_cpu(cmd.skip_dtim_periods));
-	pos += scnprintf(buf+pos, bufsz-pos, "power_scheme = %d\n",
-			 iwlmvm_mod_params.power_scheme);
-	pos += scnprintf(buf+pos, bufsz-pos, "flags = 0x%x\n",
-			 le16_to_cpu(cmd.flags));
-	pos += scnprintf(buf+pos, bufsz-pos, "keep_alive = %d\n",
-			 cmd.keep_alive_seconds);
-
-	if (cmd.flags & cpu_to_le16(POWER_FLAGS_POWER_MANAGEMENT_ENA_MSK)) {
-		pos += scnprintf(buf+pos, bufsz-pos, "skip_over_dtim = %d\n",
-				 (cmd.flags &
-				 cpu_to_le16(POWER_FLAGS_SKIP_OVER_DTIM_MSK)) ?
-				 1 : 0);
-		pos += scnprintf(buf+pos, bufsz-pos, "rx_data_timeout = %d\n",
-				 le32_to_cpu(cmd.rx_data_timeout));
-		pos += scnprintf(buf+pos, bufsz-pos, "tx_data_timeout = %d\n",
-				 le32_to_cpu(cmd.tx_data_timeout));
-		if (cmd.flags & cpu_to_le16(POWER_FLAGS_LPRX_ENA_MSK))
-			pos += scnprintf(buf+pos, bufsz-pos,
-					 "lprx_rssi_threshold = %d\n",
-					 le32_to_cpu(cmd.lprx_rssi_threshold));
-	}
-	return pos;
-}
-#endif
-
-const struct iwl_mvm_power_ops pm_legacy_ops = {
-	.power_update_mode = iwl_mvm_power_legacy_update_mode,
-	.power_disable = iwl_mvm_power_legacy_disable,
-#ifdef CONFIG_IWLWIFI_DEBUGFS
-	.power_dbgfs_read = iwl_mvm_power_legacy_dbgfs_read,
-#endif
-};
diff --git a/drivers/net/wireless/iwlwifi/mvm/quota.c b/drivers/net/wireless/iwlwifi/mvm/quota.c
index ce5db6c4ef7e..06d8429be1fb 100644
--- a/drivers/net/wireless/iwlwifi/mvm/quota.c
+++ b/drivers/net/wireless/iwlwifi/mvm/quota.c
@@ -65,9 +65,14 @@
 #include "fw-api.h"
 #include "mvm.h"
 
+#define QUOTA_100	IWL_MVM_MAX_QUOTA
+#define QUOTA_LOWLAT_MIN ((QUOTA_100 * IWL_MVM_LOWLAT_QUOTA_MIN_PERCENT) / 100)
+
 struct iwl_mvm_quota_iterator_data {
 	int n_interfaces[MAX_BINDINGS];
 	int colors[MAX_BINDINGS];
+	int low_latency[MAX_BINDINGS];
+	int n_low_latency_bindings;
 	struct ieee80211_vif *new_vif;
 };
 
@@ -107,22 +112,29 @@ static void iwl_mvm_quota_iterator(void *_data, u8 *mac,
 	switch (vif->type) {
 	case NL80211_IFTYPE_STATION:
 		if (vif->bss_conf.assoc)
-			data->n_interfaces[id]++;
-		break;
+			break;
+		return;
 	case NL80211_IFTYPE_AP:
 	case NL80211_IFTYPE_ADHOC:
 		if (mvmvif->ap_ibss_active)
-			data->n_interfaces[id]++;
-		break;
+			break;
+		return;
 	case NL80211_IFTYPE_MONITOR:
 		if (mvmvif->monitor_active)
-			data->n_interfaces[id]++;
-		break;
+			break;
+		return;
 	case NL80211_IFTYPE_P2P_DEVICE:
-		break;
+		return;
 	default:
 		WARN_ON_ONCE(1);
-		break;
+		return;
+	}
+
+	data->n_interfaces[id]++;
+
+	if (iwl_mvm_vif_low_latency(mvmvif) && !data->low_latency[id]) {
+		data->n_low_latency_bindings++;
+		data->low_latency[id] = true;
 	}
 }
 
@@ -162,12 +174,13 @@ static void iwl_mvm_adjust_quota_for_noa(struct iwl_mvm *mvm,
 int iwl_mvm_update_quotas(struct iwl_mvm *mvm, struct ieee80211_vif *newvif)
 {
 	struct iwl_time_quota_cmd cmd = {};
-	int i, idx, ret, num_active_macs, quota, quota_rem;
+	int i, idx, ret, num_active_macs, quota, quota_rem, n_non_lowlat;
 	struct iwl_mvm_quota_iterator_data data = {
 		.n_interfaces = {},
 		.colors = { -1, -1, -1, -1 },
 		.new_vif = newvif,
 	};
+	u32 ll_max_duration;
 
 	lockdep_assert_held(&mvm->mutex);
 
@@ -186,6 +199,21 @@ int iwl_mvm_update_quotas(struct iwl_mvm *mvm, struct ieee80211_vif *newvif)
 		iwl_mvm_quota_iterator(&data, newvif->addr, newvif);
 	}
 
+	switch (data.n_low_latency_bindings) {
+	case 0: /* no low latency - use default */
+		ll_max_duration = 0;
+		break;
+	case 1: /* SingleBindingLowLatencyMode */
+		ll_max_duration = IWL_MVM_LOWLAT_SINGLE_BINDING_MAXDUR;
+		break;
+	case 2: /* DualBindingLowLatencyMode */
+		ll_max_duration = IWL_MVM_LOWLAT_DUAL_BINDING_MAXDUR;
+		break;
+	default: /* MultiBindingLowLatencyMode */
+		ll_max_duration = 0;
+		break;
+	}
+
 	/*
 	 * The FW's scheduling session consists of
 	 * IWL_MVM_MAX_QUOTA fragments. Divide these fragments
@@ -197,11 +225,39 @@ int iwl_mvm_update_quotas(struct iwl_mvm *mvm, struct ieee80211_vif *newvif)
 		num_active_macs += data.n_interfaces[i];
 	}
 
-	quota = 0;
-	quota_rem = 0;
-	if (num_active_macs) {
-		quota = IWL_MVM_MAX_QUOTA / num_active_macs;
-		quota_rem = IWL_MVM_MAX_QUOTA % num_active_macs;
+	n_non_lowlat = num_active_macs;
+
+	if (data.n_low_latency_bindings == 1) {
+		for (i = 0; i < MAX_BINDINGS; i++) {
+			if (data.low_latency[i]) {
+				n_non_lowlat -= data.n_interfaces[i];
+				break;
+			}
+		}
+	}
+
+	if (data.n_low_latency_bindings == 1 && n_non_lowlat) {
+		/*
+		 * Reserve quota for the low latency binding in case that
+		 * there are several data bindings but only a single
+		 * low latency one. Split the rest of the quota equally
+		 * between the other data interfaces.
+		 */
+		quota = (QUOTA_100 - QUOTA_LOWLAT_MIN) / n_non_lowlat;
+		quota_rem = QUOTA_100 - n_non_lowlat * quota -
+			    QUOTA_LOWLAT_MIN;
+	} else if (num_active_macs) {
+		/*
+		 * There are 0 or more than 1 low latency bindings, or all the
+		 * data interfaces belong to the single low latency binding.
+		 * Split the quota equally between the data interfaces.
+		 */
+		quota = QUOTA_100 / num_active_macs;
+		quota_rem = QUOTA_100 % num_active_macs;
+	} else {
+		/* values don't really matter - won't be used */
+		quota = 0;
+		quota_rem = 0;
 	}
 
 	for (idx = 0, i = 0; i < MAX_BINDINGS; i++) {
@@ -211,19 +267,42 @@ int iwl_mvm_update_quotas(struct iwl_mvm *mvm, struct ieee80211_vif *newvif)
 		cmd.quotas[idx].id_and_color =
 			cpu_to_le32(FW_CMD_ID_AND_COLOR(i, data.colors[i]));
 
-		if (data.n_interfaces[i] <= 0) {
+		if (data.n_interfaces[i] <= 0)
 			cmd.quotas[idx].quota = cpu_to_le32(0);
-			cmd.quotas[idx].max_duration = cpu_to_le32(0);
-		} else {
+		else if (data.n_low_latency_bindings == 1 && n_non_lowlat &&
+			 data.low_latency[i])
+			/*
+			 * There is more than one binding, but only one of the
+			 * bindings is in low latency. For this case, allocate
+			 * the minimal required quota for the low latency
+			 * binding.
+			 */
+			cmd.quotas[idx].quota = cpu_to_le32(QUOTA_LOWLAT_MIN);
+
+		else
 			cmd.quotas[idx].quota =
 				cpu_to_le32(quota * data.n_interfaces[i]);
+
+		WARN_ONCE(le32_to_cpu(cmd.quotas[idx].quota) > QUOTA_100,
+			  "Binding=%d, quota=%u > max=%u\n",
+			  idx, le32_to_cpu(cmd.quotas[idx].quota), QUOTA_100);
+
+		if (data.n_interfaces[i] && !data.low_latency[i])
+			cmd.quotas[idx].max_duration =
+				cpu_to_le32(ll_max_duration);
+		else
 			cmd.quotas[idx].max_duration = cpu_to_le32(0);
-		}
+
 		idx++;
 	}
 
-	/* Give the remainder of the session to the first binding */
-	le32_add_cpu(&cmd.quotas[0].quota, quota_rem);
+	/* Give the remainder of the session to the first data binding */
+	for (i = 0; i < MAX_BINDINGS; i++) {
+		if (le32_to_cpu(cmd.quotas[i].quota) != 0) {
+			le32_add_cpu(&cmd.quotas[i].quota, quota_rem);
+			break;
+		}
+	}
 
 	iwl_mvm_adjust_quota_for_noa(mvm, &cmd);
 
diff --git a/drivers/net/wireless/iwlwifi/mvm/rs.c b/drivers/net/wireless/iwlwifi/mvm/rs.c
index 6abf74e1351f..399709f2be2e 100644
--- a/drivers/net/wireless/iwlwifi/mvm/rs.c
+++ b/drivers/net/wireless/iwlwifi/mvm/rs.c
@@ -166,7 +166,7 @@ static bool rs_mimo_allow(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 	if (sta->smps_mode == IEEE80211_SMPS_STATIC)
 		return false;
 
-	if (num_of_ant(iwl_fw_valid_tx_ant(mvm->fw)) < 2)
+	if (num_of_ant(mvm->fw->valid_tx_ant) < 2)
 		return false;
 
 	if (!iwl_mvm_bt_coex_is_mimo_allowed(mvm, sta))
@@ -380,49 +380,49 @@ static void rs_stay_in_table(struct iwl_lq_sta *lq_sta, bool force_search);
  * (2.4 GHz) band.
  */
 
-static s32 expected_tpt_legacy[IWL_RATE_COUNT] = {
+static const u16 expected_tpt_legacy[IWL_RATE_COUNT] = {
 	7, 13, 35, 58, 40, 57, 72, 98, 121, 154, 177, 186, 0, 0, 0
 };
 
 /* Expected TpT tables. 4 indexes:
  * 0 - NGI, 1 - SGI, 2 - AGG+NGI, 3 - AGG+SGI
  */
-static s32 expected_tpt_siso_20MHz[4][IWL_RATE_COUNT] = {
+static const u16 expected_tpt_siso_20MHz[4][IWL_RATE_COUNT] = {
 	{0, 0, 0, 0, 42, 0,  76, 102, 124, 159, 183, 193, 202, 216, 0},
 	{0, 0, 0, 0, 46, 0,  82, 110, 132, 168, 192, 202, 210, 225, 0},
 	{0, 0, 0, 0, 49, 0,  97, 145, 192, 285, 375, 420, 464, 551, 0},
 	{0, 0, 0, 0, 54, 0, 108, 160, 213, 315, 415, 465, 513, 608, 0},
 };
 
-static s32 expected_tpt_siso_40MHz[4][IWL_RATE_COUNT] = {
+static const u16 expected_tpt_siso_40MHz[4][IWL_RATE_COUNT] = {
 	{0, 0, 0, 0,  77, 0, 127, 160, 184, 220, 242, 250,  257,  269,  275},
 	{0, 0, 0, 0,  83, 0, 135, 169, 193, 229, 250, 257,  264,  275,  280},
 	{0, 0, 0, 0, 101, 0, 199, 295, 389, 570, 744, 828,  911, 1070, 1173},
 	{0, 0, 0, 0, 112, 0, 220, 326, 429, 629, 819, 912, 1000, 1173, 1284},
 };
 
-static s32 expected_tpt_siso_80MHz[4][IWL_RATE_COUNT] = {
+static const u16 expected_tpt_siso_80MHz[4][IWL_RATE_COUNT] = {
 	{0, 0, 0, 0, 130, 0, 191, 223, 244,  273,  288,  294,  298,  305,  308},
 	{0, 0, 0, 0, 138, 0, 200, 231, 251,  279,  293,  298,  302,  308,  312},
 	{0, 0, 0, 0, 217, 0, 429, 634, 834, 1220, 1585, 1760, 1931, 2258, 2466},
 	{0, 0, 0, 0, 241, 0, 475, 701, 921, 1343, 1741, 1931, 2117, 2468, 2691},
 };
 
-static s32 expected_tpt_mimo2_20MHz[4][IWL_RATE_COUNT] = {
+static const u16 expected_tpt_mimo2_20MHz[4][IWL_RATE_COUNT] = {
 	{0, 0, 0, 0,  74, 0, 123, 155, 179, 213, 235, 243, 250,  261, 0},
 	{0, 0, 0, 0,  81, 0, 131, 164, 187, 221, 242, 250, 256,  267, 0},
 	{0, 0, 0, 0,  98, 0, 193, 286, 375, 550, 718, 799, 878, 1032, 0},
 	{0, 0, 0, 0, 109, 0, 214, 316, 414, 607, 790, 879, 965, 1132, 0},
 };
 
-static s32 expected_tpt_mimo2_40MHz[4][IWL_RATE_COUNT] = {
+static const u16 expected_tpt_mimo2_40MHz[4][IWL_RATE_COUNT] = {
 	{0, 0, 0, 0, 123, 0, 182, 214, 235,  264,  279,  285,  289,  296,  300},
 	{0, 0, 0, 0, 131, 0, 191, 222, 242,  270,  284,  289,  293,  300,  303},
 	{0, 0, 0, 0, 200, 0, 390, 571, 741, 1067, 1365, 1505, 1640, 1894, 2053},
 	{0, 0, 0, 0, 221, 0, 430, 630, 816, 1169, 1490, 1641, 1784, 2053, 2221},
 };
 
-static s32 expected_tpt_mimo2_80MHz[4][IWL_RATE_COUNT] = {
+static const u16 expected_tpt_mimo2_80MHz[4][IWL_RATE_COUNT] = {
 	{0, 0, 0, 0, 182, 0, 240,  264,  278,  299,  308,  311,  313,  317,  319},
 	{0, 0, 0, 0, 190, 0, 247,  269,  282,  302,  310,  313,  315,  319,  320},
 	{0, 0, 0, 0, 428, 0, 833, 1215, 1577, 2254, 2863, 3147, 3418, 3913, 4219},
@@ -905,7 +905,7 @@ static void rs_get_lower_rate_down_column(struct iwl_lq_sta *lq_sta,
 
 		rate->bw = RATE_MCS_CHAN_WIDTH_20;
 
-		WARN_ON_ONCE(rate->index < IWL_RATE_MCS_0_INDEX &&
+		WARN_ON_ONCE(rate->index < IWL_RATE_MCS_0_INDEX ||
 			     rate->index > IWL_RATE_MCS_9_INDEX);
 
 		rate->index = rs_ht_to_legacy[rate->index];
@@ -917,7 +917,7 @@ static void rs_get_lower_rate_down_column(struct iwl_lq_sta *lq_sta,
 
 
 	if (num_of_ant(rate->ant) > 1)
-		rate->ant = first_antenna(iwl_fw_valid_tx_ant(mvm->fw));
+		rate->ant = first_antenna(mvm->fw->valid_tx_ant);
 
 	/* Relevant in both switching to SISO or Legacy */
 	rate->sgi = false;
@@ -1169,12 +1169,12 @@ static void rs_set_stay_in_table(struct iwl_mvm *mvm, u8 is_legacy,
 	lq_sta->visited_columns = 0;
 }
 
-static s32 *rs_get_expected_tpt_table(struct iwl_lq_sta *lq_sta,
+static const u16 *rs_get_expected_tpt_table(struct iwl_lq_sta *lq_sta,
 				      const struct rs_tx_column *column,
 				      u32 bw)
 {
 	/* Used to choose among HT tables */
-	s32 (*ht_tbl_pointer)[IWL_RATE_COUNT];
+	const u16 (*ht_tbl_pointer)[IWL_RATE_COUNT];
 
 	if (WARN_ON_ONCE(column->mode != RS_LEGACY &&
 			 column->mode != RS_SISO &&
@@ -1262,9 +1262,8 @@ static s32 rs_get_best_rate(struct iwl_mvm *mvm,
 	    &(lq_sta->lq_info[lq_sta->active_tbl]);
 	s32 active_sr = active_tbl->win[index].success_ratio;
 	s32 active_tpt = active_tbl->expected_tpt[index];
-
 	/* expected "search" throughput */
-	s32 *tpt_tbl = tbl->expected_tpt;
+	const u16 *tpt_tbl = tbl->expected_tpt;
 
 	s32 new_rate, high, low, start_hi;
 	u16 high_low;
@@ -1478,8 +1477,8 @@ static enum rs_column rs_get_next_column(struct iwl_mvm *mvm,
 	const struct rs_tx_column *curr_col = &rs_tx_columns[tbl->column];
 	const struct rs_tx_column *next_col;
 	allow_column_func_t allow_func;
-	u8 valid_ants = iwl_fw_valid_tx_ant(mvm->fw);
-	s32 *expected_tpt_tbl;
+	u8 valid_ants = mvm->fw->valid_tx_ant;
+	const u16 *expected_tpt_tbl;
 	s32 tpt, max_expected_tpt;
 
 	for (i = 0; i < MAX_NEXT_COLUMNS; i++) {
@@ -2090,7 +2089,7 @@ static void rs_initialize_lq(struct iwl_mvm *mvm,
 
 	i = lq_sta->last_txrate_idx;
 
-	valid_tx_ant = iwl_fw_valid_tx_ant(mvm->fw);
+	valid_tx_ant = mvm->fw->valid_tx_ant;
 
 	if (!lq_sta->search_better_tbl)
 		active_tbl = lq_sta->active_tbl;
@@ -2241,6 +2240,73 @@ static void rs_vht_set_enabled_rates(struct ieee80211_sta *sta,
 	}
 }
 
+#ifdef CONFIG_IWLWIFI_DEBUGFS
+static void iwl_mvm_reset_frame_stats(struct iwl_mvm *mvm,
+				      struct iwl_mvm_frame_stats *stats)
+{
+	spin_lock_bh(&mvm->drv_stats_lock);
+	memset(stats, 0, sizeof(*stats));
+	spin_unlock_bh(&mvm->drv_stats_lock);
+}
+
+void iwl_mvm_update_frame_stats(struct iwl_mvm *mvm,
+				struct iwl_mvm_frame_stats *stats,
+				u32 rate, bool agg)
+{
+	u8 nss = 0, mcs = 0;
+
+	spin_lock(&mvm->drv_stats_lock);
+
+	if (agg)
+		stats->agg_frames++;
+
+	stats->success_frames++;
+
+	switch (rate & RATE_MCS_CHAN_WIDTH_MSK) {
+	case RATE_MCS_CHAN_WIDTH_20:
+		stats->bw_20_frames++;
+		break;
+	case RATE_MCS_CHAN_WIDTH_40:
+		stats->bw_40_frames++;
+		break;
+	case RATE_MCS_CHAN_WIDTH_80:
+		stats->bw_80_frames++;
+		break;
+	default:
+		WARN_ONCE(1, "bad BW. rate 0x%x", rate);
+	}
+
+	if (rate & RATE_MCS_HT_MSK) {
+		stats->ht_frames++;
+		mcs = rate & RATE_HT_MCS_RATE_CODE_MSK;
+		nss = ((rate & RATE_HT_MCS_NSS_MSK) >> RATE_HT_MCS_NSS_POS) + 1;
+	} else if (rate & RATE_MCS_VHT_MSK) {
+		stats->vht_frames++;
+		mcs = rate & RATE_VHT_MCS_RATE_CODE_MSK;
+		nss = ((rate & RATE_VHT_MCS_NSS_MSK) >>
+		       RATE_VHT_MCS_NSS_POS) + 1;
+	} else {
+		stats->legacy_frames++;
+	}
+
+	if (nss == 1)
+		stats->siso_frames++;
+	else if (nss == 2)
+		stats->mimo2_frames++;
+
+	if (rate & RATE_MCS_SGI_MSK)
+		stats->sgi_frames++;
+	else
+		stats->ngi_frames++;
+
+	stats->last_rates[stats->last_frame_idx] = rate;
+	stats->last_frame_idx = (stats->last_frame_idx + 1) %
+		ARRAY_SIZE(stats->last_rates);
+
+	spin_unlock(&mvm->drv_stats_lock);
+}
+#endif
+
 /*
  * Called after adding a new station to initialize rate scaling
  */
@@ -2320,7 +2386,7 @@ void iwl_mvm_rs_rate_init(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 
 	/* These values will be overridden later */
 	lq_sta->lq.single_stream_ant_msk =
-		first_antenna(iwl_fw_valid_tx_ant(mvm->fw));
+		first_antenna(mvm->fw->valid_tx_ant);
 	lq_sta->lq.dual_stream_ant_msk = ANT_AB;
 
 	/* as default allow aggregation for all tids */
@@ -2335,7 +2401,9 @@ void iwl_mvm_rs_rate_init(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 #ifdef CONFIG_MAC80211_DEBUGFS
 	lq_sta->dbg_fixed_rate = 0;
 #endif
-
+#ifdef CONFIG_IWLWIFI_DEBUGFS
+	iwl_mvm_reset_frame_stats(mvm, &mvm->drv_rx_stats);
+#endif
 	rs_initialize_lq(mvm, sta, lq_sta, band, init);
 }
 
@@ -2446,7 +2514,7 @@ static void rs_build_rates_table(struct iwl_mvm *mvm,
 
 	memcpy(&rate, initial_rate, sizeof(rate));
 
-	valid_tx_ant = iwl_fw_valid_tx_ant(mvm->fw);
+	valid_tx_ant = mvm->fw->valid_tx_ant;
 
 	if (is_siso(&rate)) {
 		num_rates = RS_INITIAL_SISO_NUM_RATES;
@@ -2547,7 +2615,7 @@ static void rs_free_sta(void *mvm_r, struct ieee80211_sta *sta,
 }
 
 #ifdef CONFIG_MAC80211_DEBUGFS
-static int rs_pretty_print_rate(char *buf, const u32 rate)
+int rs_pretty_print_rate(char *buf, const u32 rate)
 {
 
 	char *type, *bw;
@@ -2596,7 +2664,7 @@ static int rs_pretty_print_rate(char *buf, const u32 rate)
 	return sprintf(buf, "%s | ANT: %s BW: %s MCS: %d NSS: %d %s%s%s%s%s\n",
 		       type, rs_pretty_ant(ant), bw, mcs, nss,
 		       (rate & RATE_MCS_SGI_MSK) ? "SGI " : "NGI ",
-		       (rate & RATE_MCS_STBC_MSK) ? "STBC " : "",
+		       (rate & RATE_MCS_HT_STBC_MSK) ? "STBC " : "",
 		       (rate & RATE_MCS_LDPC_MSK) ? "LDPC " : "",
 		       (rate & RATE_MCS_BF_MSK) ? "BF " : "",
 		       (rate & RATE_MCS_ZLF_MSK) ? "ZLF " : "");
@@ -2677,9 +2745,9 @@ static ssize_t rs_sta_dbgfs_scale_table_read(struct file *file,
 	desc += sprintf(buff+desc, "fixed rate 0x%X\n",
 			lq_sta->dbg_fixed_rate);
 	desc += sprintf(buff+desc, "valid_tx_ant %s%s%s\n",
-	    (iwl_fw_valid_tx_ant(mvm->fw) & ANT_A) ? "ANT_A," : "",
-	    (iwl_fw_valid_tx_ant(mvm->fw) & ANT_B) ? "ANT_B," : "",
-	    (iwl_fw_valid_tx_ant(mvm->fw) & ANT_C) ? "ANT_C" : "");
+	    (mvm->fw->valid_tx_ant & ANT_A) ? "ANT_A," : "",
+	    (mvm->fw->valid_tx_ant & ANT_B) ? "ANT_B," : "",
+	    (mvm->fw->valid_tx_ant & ANT_C) ? "ANT_C" : "");
 	desc += sprintf(buff+desc, "lq type %s\n",
 			(is_legacy(rate)) ? "legacy" :
 			is_vht(rate) ? "VHT" : "HT");
@@ -2815,8 +2883,8 @@ static void rs_rate_init_stub(void *mvm_r,
 			      struct ieee80211_sta *sta, void *mvm_sta)
 {
 }
-static struct rate_control_ops rs_mvm_ops = {
-	.module = NULL,
+
+static const struct rate_control_ops rs_mvm_ops = {
 	.name = RS_NAME,
 	.tx_status = rs_tx_status,
 	.get_rate = rs_get_rate,
diff --git a/drivers/net/wireless/iwlwifi/mvm/rs.h b/drivers/net/wireless/iwlwifi/mvm/rs.h
index 7bc6404f6986..3332b396011e 100644
--- a/drivers/net/wireless/iwlwifi/mvm/rs.h
+++ b/drivers/net/wireless/iwlwifi/mvm/rs.h
@@ -277,7 +277,7 @@ enum rs_column {
 struct iwl_scale_tbl_info {
 	struct rs_rate rate;
 	enum rs_column column;
-	s32 *expected_tpt;	/* throughput metrics; expected_tpt_G, etc. */
+	const u16 *expected_tpt;	/* throughput metrics; expected_tpt_G, etc. */
 	struct iwl_rate_scale_data win[IWL_RATE_COUNT]; /* rate histories */
 };
 
diff --git a/drivers/net/wireless/iwlwifi/mvm/rx.c b/drivers/net/wireless/iwlwifi/mvm/rx.c
index a85b60f7e67e..6061553a5e44 100644
--- a/drivers/net/wireless/iwlwifi/mvm/rx.c
+++ b/drivers/net/wireless/iwlwifi/mvm/rx.c
@@ -77,6 +77,15 @@ int iwl_mvm_rx_rx_phy_cmd(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
 
 	memcpy(&mvm->last_phy_info, pkt->data, sizeof(mvm->last_phy_info));
 	mvm->ampdu_ref++;
+
+#ifdef CONFIG_IWLWIFI_DEBUGFS
+	if (mvm->last_phy_info.phy_flags & cpu_to_le16(RX_RES_PHY_FLAGS_AGG)) {
+		spin_lock(&mvm->drv_stats_lock);
+		mvm->drv_rx_stats.ampdu_count++;
+		spin_unlock(&mvm->drv_stats_lock);
+	}
+#endif
+
 	return 0;
 }
 
@@ -129,22 +138,16 @@ static void iwl_mvm_calc_rssi(struct iwl_mvm *mvm,
 			      struct ieee80211_rx_status *rx_status)
 {
 	int rssi_a, rssi_b, rssi_a_dbm, rssi_b_dbm, max_rssi_dbm;
-	int rssi_all_band_a, rssi_all_band_b;
-	u32 agc_a, agc_b, max_agc;
+	u32 agc_a, agc_b;
 	u32 val;
 
 	val = le32_to_cpu(phy_info->non_cfg_phy[IWL_RX_INFO_AGC_IDX]);
 	agc_a = (val & IWL_OFDM_AGC_A_MSK) >> IWL_OFDM_AGC_A_POS;
 	agc_b = (val & IWL_OFDM_AGC_B_MSK) >> IWL_OFDM_AGC_B_POS;
-	max_agc = max_t(u32, agc_a, agc_b);
 
 	val = le32_to_cpu(phy_info->non_cfg_phy[IWL_RX_INFO_RSSI_AB_IDX]);
 	rssi_a = (val & IWL_OFDM_RSSI_INBAND_A_MSK) >> IWL_OFDM_RSSI_A_POS;
 	rssi_b = (val & IWL_OFDM_RSSI_INBAND_B_MSK) >> IWL_OFDM_RSSI_B_POS;
-	rssi_all_band_a = (val & IWL_OFDM_RSSI_ALLBAND_A_MSK) >>
-				IWL_OFDM_RSSI_ALLBAND_A_POS;
-	rssi_all_band_b = (val & IWL_OFDM_RSSI_ALLBAND_B_MSK) >>
-				IWL_OFDM_RSSI_ALLBAND_B_POS;
 
 	/*
 	 * dBm = rssi dB - agc dB - constant.
@@ -364,31 +367,43 @@ int iwl_mvm_rx_rx_mpdu(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
 		rx_status.flag |= RX_FLAG_40MHZ;
 		break;
 	case RATE_MCS_CHAN_WIDTH_80:
-		rx_status.flag |= RX_FLAG_80MHZ;
+		rx_status.vht_flag |= RX_VHT_FLAG_80MHZ;
 		break;
 	case RATE_MCS_CHAN_WIDTH_160:
-		rx_status.flag |= RX_FLAG_160MHZ;
+		rx_status.vht_flag |= RX_VHT_FLAG_160MHZ;
 		break;
 	}
 	if (rate_n_flags & RATE_MCS_SGI_MSK)
 		rx_status.flag |= RX_FLAG_SHORT_GI;
 	if (rate_n_flags & RATE_HT_MCS_GF_MSK)
 		rx_status.flag |= RX_FLAG_HT_GF;
+	if (rate_n_flags & RATE_MCS_LDPC_MSK)
+		rx_status.flag |= RX_FLAG_LDPC;
 	if (rate_n_flags & RATE_MCS_HT_MSK) {
+		u8 stbc = (rate_n_flags & RATE_MCS_HT_STBC_MSK) >>
+				RATE_MCS_STBC_POS;
 		rx_status.flag |= RX_FLAG_HT;
 		rx_status.rate_idx = rate_n_flags & RATE_HT_MCS_INDEX_MSK;
+		rx_status.flag |= stbc << RX_FLAG_STBC_SHIFT;
 	} else if (rate_n_flags & RATE_MCS_VHT_MSK) {
+		u8 stbc = (rate_n_flags & RATE_MCS_VHT_STBC_MSK) >>
+				RATE_MCS_STBC_POS;
 		rx_status.vht_nss =
 			((rate_n_flags & RATE_VHT_MCS_NSS_MSK) >>
 						RATE_VHT_MCS_NSS_POS) + 1;
 		rx_status.rate_idx = rate_n_flags & RATE_VHT_MCS_RATE_CODE_MSK;
 		rx_status.flag |= RX_FLAG_VHT;
+		rx_status.flag |= stbc << RX_FLAG_STBC_SHIFT;
 	} else {
 		rx_status.rate_idx =
 			iwl_mvm_legacy_rate_to_mac80211_idx(rate_n_flags,
 							    rx_status.band);
 	}
 
+#ifdef CONFIG_IWLWIFI_DEBUGFS
+	iwl_mvm_update_frame_stats(mvm, &mvm->drv_rx_stats, rate_n_flags,
+				   rx_status.flag & RX_FLAG_AMPDU_DETAILS);
+#endif
 	iwl_mvm_pass_packet_to_mac80211(mvm, hdr, len, ampdu_status,
 					rxb, &rx_status);
 	return 0;
diff --git a/drivers/net/wireless/iwlwifi/mvm/scan.c b/drivers/net/wireless/iwlwifi/mvm/scan.c
index 742afc429c94..713efd71efe2 100644
--- a/drivers/net/wireless/iwlwifi/mvm/scan.c
+++ b/drivers/net/wireless/iwlwifi/mvm/scan.c
@@ -82,7 +82,7 @@ static inline __le16 iwl_mvm_scan_rx_chain(struct iwl_mvm *mvm)
 	if (mvm->scan_rx_ant != ANT_NONE)
 		rx_ant = mvm->scan_rx_ant;
 	else
-		rx_ant = iwl_fw_valid_rx_ant(mvm->fw);
+		rx_ant = mvm->fw->valid_rx_ant;
 	rx_chain = rx_ant << PHY_RX_CHAIN_VALID_POS;
 	rx_chain |= rx_ant << PHY_RX_CHAIN_FORCE_MIMO_SEL_POS;
 	rx_chain |= rx_ant << PHY_RX_CHAIN_FORCE_SEL_POS;
@@ -124,7 +124,7 @@ iwl_mvm_scan_rate_n_flags(struct iwl_mvm *mvm, enum ieee80211_band band,
 	u32 tx_ant;
 
 	mvm->scan_last_antenna_idx =
-		iwl_mvm_next_antenna(mvm, iwl_fw_valid_tx_ant(mvm->fw),
+		iwl_mvm_next_antenna(mvm, mvm->fw->valid_tx_ant,
 				     mvm->scan_last_antenna_idx);
 	tx_ant = BIT(mvm->scan_last_antenna_idx) << RATE_MCS_ANT_POS;
 
@@ -408,6 +408,8 @@ int iwl_mvm_rx_scan_complete(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
 	mvm->scan_status = IWL_MVM_SCAN_NONE;
 	ieee80211_scan_completed(mvm->hw, notif->status != SCAN_COMP_STATUS_OK);
 
+	iwl_mvm_unref(mvm, IWL_MVM_REF_SCAN);
+
 	return 0;
 }
 
@@ -476,6 +478,7 @@ void iwl_mvm_cancel_scan(struct iwl_mvm *mvm)
 
 	if (iwl_mvm_is_radio_killed(mvm)) {
 		ieee80211_scan_completed(mvm->hw, true);
+		iwl_mvm_unref(mvm, IWL_MVM_REF_SCAN);
 		mvm->scan_status = IWL_MVM_SCAN_NONE;
 		return;
 	}
@@ -488,7 +491,7 @@ void iwl_mvm_cancel_scan(struct iwl_mvm *mvm)
 	ret = iwl_mvm_send_cmd_pdu(mvm, SCAN_ABORT_CMD, CMD_SYNC, 0, NULL);
 	if (ret) {
 		IWL_ERR(mvm, "Couldn't send SCAN_ABORT_CMD: %d\n", ret);
-		/* mac80211's state will be cleaned in the fw_restart flow */
+		/* mac80211's state will be cleaned in the nic_restart flow */
 		goto out_remove_notif;
 	}
 
@@ -509,11 +512,16 @@ int iwl_mvm_rx_scan_offload_complete_notif(struct iwl_mvm *mvm,
 	struct iwl_rx_packet *pkt = rxb_addr(rxb);
 	struct iwl_scan_offload_complete *scan_notif = (void *)pkt->data;
 
+	/* scan status must be locked for proper checking */
+	lockdep_assert_held(&mvm->mutex);
+
 	IWL_DEBUG_SCAN(mvm, "Scheduled scan completed, status %s\n",
 		       scan_notif->status == IWL_SCAN_OFFLOAD_COMPLETED ?
 		       "completed" : "aborted");
 
-	mvm->scan_status = IWL_MVM_SCAN_NONE;
+	/* might already be something else again, don't reset if so */
+	if (mvm->scan_status == IWL_MVM_SCAN_SCHED)
+		mvm->scan_status = IWL_MVM_SCAN_NONE;
 	ieee80211_sched_scan_stopped(mvm->hw);
 
 	return 0;
@@ -596,6 +604,9 @@ static void iwl_scan_offload_build_ssid(struct cfg80211_sched_scan_request *req,
 	 * config match list.
 	 */
 	for (i = 0; i < req->n_match_sets && i < PROBE_OPTION_MAX; i++) {
+		/* skip empty SSID matchsets */
+		if (!req->match_sets[i].ssid.ssid_len)
+			continue;
 		scan->direct_scan[i].id = WLAN_EID_SSID;
 		scan->direct_scan[i].len = req->match_sets[i].ssid.ssid_len;
 		memcpy(scan->direct_scan[i].ssid, req->match_sets[i].ssid.ssid,
diff --git a/drivers/net/wireless/iwlwifi/mvm/sta.c b/drivers/net/wireless/iwlwifi/mvm/sta.c
index 3397f59cd4e4..2677d1c0e1a1 100644
--- a/drivers/net/wireless/iwlwifi/mvm/sta.c
+++ b/drivers/net/wireless/iwlwifi/mvm/sta.c
@@ -66,27 +66,27 @@
 #include "sta.h"
 #include "rs.h"
 
-static void iwl_mvm_add_sta_cmd_v6_to_v5(struct iwl_mvm_add_sta_cmd_v6 *cmd_v6,
+static void iwl_mvm_add_sta_cmd_v7_to_v5(struct iwl_mvm_add_sta_cmd_v7 *cmd_v7,
 					 struct iwl_mvm_add_sta_cmd_v5 *cmd_v5)
 {
 	memset(cmd_v5, 0, sizeof(*cmd_v5));
 
-	cmd_v5->add_modify = cmd_v6->add_modify;
-	cmd_v5->tid_disable_tx = cmd_v6->tid_disable_tx;
-	cmd_v5->mac_id_n_color = cmd_v6->mac_id_n_color;
-	memcpy(cmd_v5->addr, cmd_v6->addr, ETH_ALEN);
-	cmd_v5->sta_id = cmd_v6->sta_id;
-	cmd_v5->modify_mask = cmd_v6->modify_mask;
-	cmd_v5->station_flags = cmd_v6->station_flags;
-	cmd_v5->station_flags_msk = cmd_v6->station_flags_msk;
-	cmd_v5->add_immediate_ba_tid = cmd_v6->add_immediate_ba_tid;
-	cmd_v5->remove_immediate_ba_tid = cmd_v6->remove_immediate_ba_tid;
-	cmd_v5->add_immediate_ba_ssn = cmd_v6->add_immediate_ba_ssn;
-	cmd_v5->sleep_tx_count = cmd_v6->sleep_tx_count;
-	cmd_v5->sleep_state_flags = cmd_v6->sleep_state_flags;
-	cmd_v5->assoc_id = cmd_v6->assoc_id;
-	cmd_v5->beamform_flags = cmd_v6->beamform_flags;
-	cmd_v5->tfd_queue_msk = cmd_v6->tfd_queue_msk;
+	cmd_v5->add_modify = cmd_v7->add_modify;
+	cmd_v5->tid_disable_tx = cmd_v7->tid_disable_tx;
+	cmd_v5->mac_id_n_color = cmd_v7->mac_id_n_color;
+	memcpy(cmd_v5->addr, cmd_v7->addr, ETH_ALEN);
+	cmd_v5->sta_id = cmd_v7->sta_id;
+	cmd_v5->modify_mask = cmd_v7->modify_mask;
+	cmd_v5->station_flags = cmd_v7->station_flags;
+	cmd_v5->station_flags_msk = cmd_v7->station_flags_msk;
+	cmd_v5->add_immediate_ba_tid = cmd_v7->add_immediate_ba_tid;
+	cmd_v5->remove_immediate_ba_tid = cmd_v7->remove_immediate_ba_tid;
+	cmd_v5->add_immediate_ba_ssn = cmd_v7->add_immediate_ba_ssn;
+	cmd_v5->sleep_tx_count = cmd_v7->sleep_tx_count;
+	cmd_v5->sleep_state_flags = cmd_v7->sleep_state_flags;
+	cmd_v5->assoc_id = cmd_v7->assoc_id;
+	cmd_v5->beamform_flags = cmd_v7->beamform_flags;
+	cmd_v5->tfd_queue_msk = cmd_v7->tfd_queue_msk;
 }
 
 static void
@@ -110,7 +110,7 @@ iwl_mvm_add_sta_key_to_add_sta_cmd_v5(struct iwl_mvm_add_sta_key_cmd *key_cmd,
 }
 
 static int iwl_mvm_send_add_sta_cmd_status(struct iwl_mvm *mvm,
-					   struct iwl_mvm_add_sta_cmd_v6 *cmd,
+					   struct iwl_mvm_add_sta_cmd_v7 *cmd,
 					   int *status)
 {
 	struct iwl_mvm_add_sta_cmd_v5 cmd_v5;
@@ -119,14 +119,14 @@ static int iwl_mvm_send_add_sta_cmd_status(struct iwl_mvm *mvm,
 		return iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA, sizeof(*cmd),
 						   cmd, status);
 
-	iwl_mvm_add_sta_cmd_v6_to_v5(cmd, &cmd_v5);
+	iwl_mvm_add_sta_cmd_v7_to_v5(cmd, &cmd_v5);
 
 	return iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA, sizeof(cmd_v5),
 					   &cmd_v5, status);
 }
 
 static int iwl_mvm_send_add_sta_cmd(struct iwl_mvm *mvm, u32 flags,
-				    struct iwl_mvm_add_sta_cmd_v6 *cmd)
+				    struct iwl_mvm_add_sta_cmd_v7 *cmd)
 {
 	struct iwl_mvm_add_sta_cmd_v5 cmd_v5;
 
@@ -134,7 +134,7 @@ static int iwl_mvm_send_add_sta_cmd(struct iwl_mvm *mvm, u32 flags,
 		return iwl_mvm_send_cmd_pdu(mvm, ADD_STA, flags,
 					    sizeof(*cmd), cmd);
 
-	iwl_mvm_add_sta_cmd_v6_to_v5(cmd, &cmd_v5);
+	iwl_mvm_add_sta_cmd_v7_to_v5(cmd, &cmd_v5);
 
 	return iwl_mvm_send_cmd_pdu(mvm, ADD_STA, flags, sizeof(cmd_v5),
 				    &cmd_v5);
@@ -175,19 +175,30 @@ static int iwl_mvm_send_add_sta_key_cmd(struct iwl_mvm *mvm,
 				    &sta_cmd);
 }
 
-static int iwl_mvm_find_free_sta_id(struct iwl_mvm *mvm)
+static int iwl_mvm_find_free_sta_id(struct iwl_mvm *mvm,
+				    enum nl80211_iftype iftype)
 {
 	int sta_id;
+	u32 reserved_ids = 0;
 
+	BUILD_BUG_ON(IWL_MVM_STATION_COUNT > 32);
 	WARN_ON_ONCE(test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status));
 
 	lockdep_assert_held(&mvm->mutex);
 
+	/* d0i3/d3 assumes the AP's sta_id (of sta vif) is 0. reserve it. */
+	if (iftype != NL80211_IFTYPE_STATION)
+		reserved_ids = BIT(0);
+
 	/* Don't take rcu_read_lock() since we are protected by mvm->mutex */
-	for (sta_id = 0; sta_id < IWL_MVM_STATION_COUNT; sta_id++)
+	for (sta_id = 0; sta_id < IWL_MVM_STATION_COUNT; sta_id++) {
+		if (BIT(sta_id) & reserved_ids)
+			continue;
+
 		if (!rcu_dereference_protected(mvm->fw_id_to_mac_id[sta_id],
 					       lockdep_is_held(&mvm->mutex)))
 			return sta_id;
+	}
 	return IWL_MVM_STATION_COUNT;
 }
 
@@ -196,7 +207,7 @@ int iwl_mvm_sta_send_to_fw(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 			   bool update)
 {
 	struct iwl_mvm_sta *mvm_sta = (void *)sta->drv_priv;
-	struct iwl_mvm_add_sta_cmd_v6 add_sta_cmd;
+	struct iwl_mvm_add_sta_cmd_v7 add_sta_cmd;
 	int ret;
 	u32 status;
 	u32 agg_size = 0, mpdu_dens = 0;
@@ -312,7 +323,8 @@ int iwl_mvm_add_sta(struct iwl_mvm *mvm,
 	lockdep_assert_held(&mvm->mutex);
 
 	if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status))
-		sta_id = iwl_mvm_find_free_sta_id(mvm);
+		sta_id = iwl_mvm_find_free_sta_id(mvm,
+						  ieee80211_vif_type_p2p(vif));
 	else
 		sta_id = mvm_sta->sta_id;
 
@@ -368,7 +380,7 @@ int iwl_mvm_update_sta(struct iwl_mvm *mvm,
 int iwl_mvm_drain_sta(struct iwl_mvm *mvm, struct iwl_mvm_sta *mvmsta,
 		      bool drain)
 {
-	struct iwl_mvm_add_sta_cmd_v6 cmd = {};
+	struct iwl_mvm_add_sta_cmd_v7 cmd = {};
 	int ret;
 	u32 status;
 
@@ -522,6 +534,10 @@ int iwl_mvm_rm_sta(struct iwl_mvm *mvm,
 
 		/* unassoc - go ahead - remove the AP STA now */
 		mvmvif->ap_sta_id = IWL_MVM_STATION_COUNT;
+
+		/* clear d0i3_ap_sta_id if no longer relevant */
+		if (mvm->d0i3_ap_sta_id == mvm_sta->sta_id)
+			mvm->d0i3_ap_sta_id = IWL_MVM_STATION_COUNT;
 	}
 
 	/*
@@ -560,10 +576,10 @@ int iwl_mvm_rm_sta_id(struct iwl_mvm *mvm,
 }
 
 int iwl_mvm_allocate_int_sta(struct iwl_mvm *mvm, struct iwl_mvm_int_sta *sta,
-			     u32 qmask)
+			     u32 qmask, enum nl80211_iftype iftype)
 {
 	if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status)) {
-		sta->sta_id = iwl_mvm_find_free_sta_id(mvm);
+		sta->sta_id = iwl_mvm_find_free_sta_id(mvm, iftype);
 		if (WARN_ON_ONCE(sta->sta_id == IWL_MVM_STATION_COUNT))
 			return -ENOSPC;
 	}
@@ -587,13 +603,13 @@ static int iwl_mvm_add_int_sta_common(struct iwl_mvm *mvm,
 				      const u8 *addr,
 				      u16 mac_id, u16 color)
 {
-	struct iwl_mvm_add_sta_cmd_v6 cmd;
+	struct iwl_mvm_add_sta_cmd_v7 cmd;
 	int ret;
 	u32 status;
 
 	lockdep_assert_held(&mvm->mutex);
 
-	memset(&cmd, 0, sizeof(struct iwl_mvm_add_sta_cmd_v6));
+	memset(&cmd, 0, sizeof(struct iwl_mvm_add_sta_cmd_v7));
 	cmd.sta_id = sta->sta_id;
 	cmd.mac_id_n_color = cpu_to_le32(FW_CMD_ID_AND_COLOR(mac_id,
 							     color));
@@ -627,7 +643,8 @@ int iwl_mvm_add_aux_sta(struct iwl_mvm *mvm)
 	lockdep_assert_held(&mvm->mutex);
 
 	/* Add the aux station, but without any queues */
-	ret = iwl_mvm_allocate_int_sta(mvm, &mvm->aux_sta, 0);
+	ret = iwl_mvm_allocate_int_sta(mvm, &mvm->aux_sta, 0,
+				       NL80211_IFTYPE_UNSPECIFIED);
 	if (ret)
 		return ret;
 
@@ -699,7 +716,8 @@ int iwl_mvm_add_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 	lockdep_assert_held(&mvm->mutex);
 
 	qmask = iwl_mvm_mac_get_queues_mask(mvm, vif);
-	ret = iwl_mvm_allocate_int_sta(mvm, bsta, qmask);
+	ret = iwl_mvm_allocate_int_sta(mvm, bsta, qmask,
+				       ieee80211_vif_type_p2p(vif));
 	if (ret)
 		return ret;
 
@@ -735,7 +753,7 @@ int iwl_mvm_sta_rx_agg(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 		       int tid, u16 ssn, bool start)
 {
 	struct iwl_mvm_sta *mvm_sta = (void *)sta->drv_priv;
-	struct iwl_mvm_add_sta_cmd_v6 cmd = {};
+	struct iwl_mvm_add_sta_cmd_v7 cmd = {};
 	int ret;
 	u32 status;
 
@@ -794,7 +812,7 @@ static int iwl_mvm_sta_tx_agg(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 			      int tid, u8 queue, bool start)
 {
 	struct iwl_mvm_sta *mvm_sta = (void *)sta->drv_priv;
-	struct iwl_mvm_add_sta_cmd_v6 cmd = {};
+	struct iwl_mvm_add_sta_cmd_v7 cmd = {};
 	int ret;
 	u32 status;
 
@@ -833,7 +851,7 @@ static int iwl_mvm_sta_tx_agg(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 	return ret;
 }
 
-static const u8 tid_to_ac[] = {
+static const u8 tid_to_mac80211_ac[] = {
 	IEEE80211_AC_BE,
 	IEEE80211_AC_BK,
 	IEEE80211_AC_BK,
@@ -844,6 +862,17 @@ static const u8 tid_to_ac[] = {
 	IEEE80211_AC_VO,
 };
 
+static const u8 tid_to_ucode_ac[] = {
+	AC_BE,
+	AC_BK,
+	AC_BK,
+	AC_BE,
+	AC_VI,
+	AC_VI,
+	AC_VO,
+	AC_VO,
+};
+
 int iwl_mvm_sta_tx_agg_start(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 			     struct ieee80211_sta *sta, u16 tid, u16 *ssn)
 {
@@ -874,7 +903,7 @@ int iwl_mvm_sta_tx_agg_start(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 	}
 
 	/* the new tx queue is still connected to the same mac80211 queue */
-	mvm->queue_to_mac80211[txq_id] = vif->hw_queue[tid_to_ac[tid]];
+	mvm->queue_to_mac80211[txq_id] = vif->hw_queue[tid_to_mac80211_ac[tid]];
 
 	spin_lock_bh(&mvmsta->lock);
 	tid_data = &mvmsta->tid_data[tid];
@@ -916,7 +945,7 @@ int iwl_mvm_sta_tx_agg_oper(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 	tid_data->ssn = 0xffff;
 	spin_unlock_bh(&mvmsta->lock);
 
-	fifo = iwl_mvm_ac_to_tx_fifo[tid_to_ac[tid]];
+	fifo = iwl_mvm_ac_to_tx_fifo[tid_to_mac80211_ac[tid]];
 
 	ret = iwl_mvm_sta_tx_agg(mvm, sta, tid, queue, true);
 	if (ret)
@@ -1411,7 +1440,7 @@ void iwl_mvm_sta_modify_ps_wake(struct iwl_mvm *mvm,
 				struct ieee80211_sta *sta)
 {
 	struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
-	struct iwl_mvm_add_sta_cmd_v6 cmd = {
+	struct iwl_mvm_add_sta_cmd_v7 cmd = {
 		.add_modify = STA_MODE_MODIFY,
 		.sta_id = mvmsta->sta_id,
 		.station_flags_msk = cpu_to_le32(STA_FLG_PS),
@@ -1427,28 +1456,102 @@ void iwl_mvm_sta_modify_ps_wake(struct iwl_mvm *mvm,
 void iwl_mvm_sta_modify_sleep_tx_count(struct iwl_mvm *mvm,
 				       struct ieee80211_sta *sta,
 				       enum ieee80211_frame_release_type reason,
-				       u16 cnt)
+				       u16 cnt, u16 tids, bool more_data,
+				       bool agg)
 {
-	u16 sleep_state_flags =
-		(reason == IEEE80211_FRAME_RELEASE_UAPSD) ?
-			STA_SLEEP_STATE_UAPSD : STA_SLEEP_STATE_PS_POLL;
 	struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
-	struct iwl_mvm_add_sta_cmd_v6 cmd = {
+	struct iwl_mvm_add_sta_cmd_v7 cmd = {
 		.add_modify = STA_MODE_MODIFY,
 		.sta_id = mvmsta->sta_id,
 		.modify_mask = STA_MODIFY_SLEEPING_STA_TX_COUNT,
 		.sleep_tx_count = cpu_to_le16(cnt),
 		.mac_id_n_color = cpu_to_le32(mvmsta->mac_id_n_color),
-		/*
-		 * Same modify mask for sleep_tx_count and sleep_state_flags so
-		 * we must set the sleep_state_flags too.
-		 */
-		.sleep_state_flags = cpu_to_le16(sleep_state_flags),
 	};
-	int ret;
+	int tid, ret;
+	unsigned long _tids = tids;
+
+	/* convert TIDs to ACs - we don't support TSPEC so that's OK
+	 * Note that this field is reserved and unused by firmware not
+	 * supporting GO uAPSD, so it's safe to always do this.
+	 */
+	for_each_set_bit(tid, &_tids, IWL_MAX_TID_COUNT)
+		cmd.awake_acs |= BIT(tid_to_ucode_ac[tid]);
+
+	/* If we're releasing frames from aggregation queues then check if the
+	 * all queues combined that we're releasing frames from have
+	 *  - more frames than the service period, in which case more_data
+	 *    needs to be set
+	 *  - fewer than 'cnt' frames, in which case we need to adjust the
+	 *    firmware command (but do that unconditionally)
+	 */
+	if (agg) {
+		int remaining = cnt;
+
+		spin_lock_bh(&mvmsta->lock);
+		for_each_set_bit(tid, &_tids, IWL_MAX_TID_COUNT) {
+			struct iwl_mvm_tid_data *tid_data;
+			u16 n_queued;
+
+			tid_data = &mvmsta->tid_data[tid];
+			if (WARN(tid_data->state != IWL_AGG_ON &&
+				 tid_data->state != IWL_EMPTYING_HW_QUEUE_DELBA,
+				 "TID %d state is %d\n",
+				 tid, tid_data->state)) {
+				spin_unlock_bh(&mvmsta->lock);
+				ieee80211_sta_eosp(sta);
+				return;
+			}
+
+			n_queued = iwl_mvm_tid_queued(tid_data);
+			if (n_queued > remaining) {
+				more_data = true;
+				remaining = 0;
+				break;
+			}
+			remaining -= n_queued;
+		}
+		spin_unlock_bh(&mvmsta->lock);
+
+		cmd.sleep_tx_count = cpu_to_le16(cnt - remaining);
+		if (WARN_ON(cnt - remaining == 0)) {
+			ieee80211_sta_eosp(sta);
+			return;
+		}
+	}
+
+	/* Note: this is ignored by firmware not supporting GO uAPSD */
+	if (more_data)
+		cmd.sleep_state_flags |= cpu_to_le16(STA_SLEEP_STATE_MOREDATA);
+
+	if (reason == IEEE80211_FRAME_RELEASE_PSPOLL) {
+		mvmsta->next_status_eosp = true;
+		cmd.sleep_state_flags |= cpu_to_le16(STA_SLEEP_STATE_PS_POLL);
+	} else {
+		cmd.sleep_state_flags |= cpu_to_le16(STA_SLEEP_STATE_UAPSD);
+	}
 
-	/* TODO: somehow the fw doesn't seem to take PS_POLL into account */
 	ret = iwl_mvm_send_add_sta_cmd(mvm, CMD_ASYNC, &cmd);
 	if (ret)
 		IWL_ERR(mvm, "Failed to send ADD_STA command (%d)\n", ret);
 }
+
+int iwl_mvm_rx_eosp_notif(struct iwl_mvm *mvm,
+			  struct iwl_rx_cmd_buffer *rxb,
+			  struct iwl_device_cmd *cmd)
+{
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	struct iwl_mvm_eosp_notification *notif = (void *)pkt->data;
+	struct ieee80211_sta *sta;
+	u32 sta_id = le32_to_cpu(notif->sta_id);
+
+	if (WARN_ON_ONCE(sta_id >= IWL_MVM_STATION_COUNT))
+		return 0;
+
+	rcu_read_lock();
+	sta = rcu_dereference(mvm->fw_id_to_mac_id[sta_id]);
+	if (!IS_ERR_OR_NULL(sta))
+		ieee80211_sta_eosp(sta);
+	rcu_read_unlock();
+
+	return 0;
+}
diff --git a/drivers/net/wireless/iwlwifi/mvm/sta.h b/drivers/net/wireless/iwlwifi/mvm/sta.h
index 4968d0237dc5..2ed84c421481 100644
--- a/drivers/net/wireless/iwlwifi/mvm/sta.h
+++ b/drivers/net/wireless/iwlwifi/mvm/sta.h
@@ -195,24 +195,33 @@ struct iwl_mvm;
 /**
  * DOC: AP mode - PS
  *
- * When a station is asleep, the fw will set it as "asleep". All the
- * non-aggregation frames to that station will be dropped by the fw
- * (%TX_STATUS_FAIL_DEST_PS failure code).
+ * When a station is asleep, the fw will set it as "asleep". All frames on
+ * shared queues (i.e. non-aggregation queues) to that station will be dropped
+ * by the fw (%TX_STATUS_FAIL_DEST_PS failure code).
+ *
  * AMPDUs are in a separate queue that is stopped by the fw. We just need to
- * let mac80211 know how many frames we have in these queues so that it can
+ * let mac80211 know when there are frames in these queues so that it can
  * properly handle trigger frames.
- * When the a trigger frame is received, mac80211 tells the driver to send
- * frames from the AMPDU queues or AC queue depending on which queue are
- * delivery-enabled and what TID has frames to transmit (Note that mac80211 has
- * all the knowledege since all the non-agg frames are buffered / filtered, and
- * the driver tells mac80211 about agg frames). The driver needs to tell the fw
- * to let frames out even if the station is asleep. This is done by
- * %iwl_mvm_sta_modify_sleep_tx_count.
- * When we receive a frame from that station with PM bit unset, the
- * driver needs to let the fw know that this station isn't alseep any more.
- * This is done by %iwl_mvm_sta_modify_ps_wake.
- *
- * TODO - EOSP handling
+ *
+ * When a trigger frame is received, mac80211 tells the driver to send frames
+ * from the AMPDU queues or sends frames to non-aggregation queues itself,
+ * depending on which ACs are delivery-enabled and what TID has frames to
+ * transmit. Note that mac80211 has all the knowledege since all the non-agg
+ * frames are buffered / filtered, and the driver tells mac80211 about agg
+ * frames). The driver needs to tell the fw to let frames out even if the
+ * station is asleep. This is done by %iwl_mvm_sta_modify_sleep_tx_count.
+ *
+ * When we receive a frame from that station with PM bit unset, the driver
+ * needs to let the fw know that this station isn't asleep any more. This is
+ * done by %iwl_mvm_sta_modify_ps_wake in response to mac80211 signalling the
+ * station's wakeup.
+ *
+ * For a GO, the Service Period might be cut short due to an absence period
+ * of the GO. In this (and all other cases) the firmware notifies us with the
+ * EOSP_NOTIFICATION, and we notify mac80211 of that. Further frames that we
+ * already sent to the device will be rejected again.
+ *
+ * See also "AP support for powersaving clients" in mac80211.h.
  */
 
 /**
@@ -261,6 +270,12 @@ struct iwl_mvm_tid_data {
 	u16 ssn;
 };
 
+static inline u16 iwl_mvm_tid_queued(struct iwl_mvm_tid_data *tid_data)
+{
+	return ieee80211_sn_sub(IEEE80211_SEQ_TO_SN(tid_data->seq_number),
+				tid_data->next_reclaimed);
+}
+
 /**
  * struct iwl_mvm_sta - representation of a station in the driver
  * @sta_id: the index of the station in the fw (will be replaced by id_n_color)
@@ -269,7 +284,11 @@ struct iwl_mvm_tid_data {
  * @tid_disable_agg: bitmap: if bit(tid) is set, the fw won't send ampdus for
  *	tid.
  * @max_agg_bufsize: the maximal size of the AGG buffer for this station
+ * @bt_reduced_txpower_dbg: debug mode in which %bt_reduced_txpower is forced
+ *	by debugfs.
  * @bt_reduced_txpower: is reduced tx power enabled for this station
+ * @next_status_eosp: the next reclaimed packet is a PS-Poll response and
+ *	we need to signal the EOSP
  * @lock: lock to protect the whole struct. Since %tid_data is access from Tx
  * and from Tx response flow, it needs a spinlock.
  * @tid_data: per tid data. Look at %iwl_mvm_tid_data.
@@ -287,7 +306,9 @@ struct iwl_mvm_sta {
 	u32 mac_id_n_color;
 	u16 tid_disable_agg;
 	u8 max_agg_bufsize;
+	bool bt_reduced_txpower_dbg;
 	bool bt_reduced_txpower;
+	bool next_status_eosp;
 	spinlock_t lock;
 	struct iwl_mvm_tid_data tid_data[IWL_MAX_TID_COUNT];
 	struct iwl_lq_sta lq_sta;
@@ -345,6 +366,10 @@ void iwl_mvm_update_tkip_key(struct iwl_mvm *mvm,
 			     struct ieee80211_sta *sta, u32 iv32,
 			     u16 *phase1key);
 
+int iwl_mvm_rx_eosp_notif(struct iwl_mvm *mvm,
+			  struct iwl_rx_cmd_buffer *rxb,
+			  struct iwl_device_cmd *cmd);
+
 /* AMPDU */
 int iwl_mvm_sta_rx_agg(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 		       int tid, u16 ssn, bool start);
@@ -359,7 +384,7 @@ int iwl_mvm_sta_tx_agg_flush(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 
 int iwl_mvm_add_aux_sta(struct iwl_mvm *mvm);
 int iwl_mvm_allocate_int_sta(struct iwl_mvm *mvm, struct iwl_mvm_int_sta *sta,
-			     u32 qmask);
+			     u32 qmask, enum nl80211_iftype iftype);
 void iwl_mvm_dealloc_int_sta(struct iwl_mvm *mvm,
 			     struct iwl_mvm_int_sta *sta);
 int iwl_mvm_send_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
@@ -375,7 +400,8 @@ void iwl_mvm_sta_modify_ps_wake(struct iwl_mvm *mvm,
 void iwl_mvm_sta_modify_sleep_tx_count(struct iwl_mvm *mvm,
 				       struct ieee80211_sta *sta,
 				       enum ieee80211_frame_release_type reason,
-				       u16 cnt);
+				       u16 cnt, u16 tids, bool more_data,
+				       bool agg);
 int iwl_mvm_drain_sta(struct iwl_mvm *mvm, struct iwl_mvm_sta *mvmsta,
 		      bool drain);
 
diff --git a/drivers/net/wireless/iwlwifi/mvm/time-event.c b/drivers/net/wireless/iwlwifi/mvm/time-event.c
index b4c2abaa297b..61331245ad93 100644
--- a/drivers/net/wireless/iwlwifi/mvm/time-event.c
+++ b/drivers/net/wireless/iwlwifi/mvm/time-event.c
@@ -126,6 +126,7 @@ static void iwl_mvm_roc_finished(struct iwl_mvm *mvm)
 	 * in iwl_mvm_te_handle_notif).
 	 */
 	clear_bit(IWL_MVM_STATUS_ROC_RUNNING, &mvm->status);
+	iwl_mvm_unref(mvm, IWL_MVM_REF_ROC);
 
 	/*
 	 * Of course, our status bit is just as racy as mac80211, so in
@@ -210,6 +211,7 @@ static void iwl_mvm_te_handle_notif(struct iwl_mvm *mvm,
 
 		if (te_data->vif->type == NL80211_IFTYPE_P2P_DEVICE) {
 			set_bit(IWL_MVM_STATUS_ROC_RUNNING, &mvm->status);
+			iwl_mvm_ref(mvm, IWL_MVM_REF_ROC);
 			ieee80211_ready_on_channel(mvm->hw);
 		}
 	} else {
@@ -436,7 +438,8 @@ void iwl_mvm_protect_session(struct iwl_mvm *mvm,
 	time_cmd.duration = cpu_to_le32(duration);
 	time_cmd.repeat = 1;
 	time_cmd.policy = cpu_to_le16(TE_V2_NOTIF_HOST_EVENT_START |
-				      TE_V2_NOTIF_HOST_EVENT_END);
+				      TE_V2_NOTIF_HOST_EVENT_END |
+				      T2_V2_START_IMMEDIATELY);
 
 	iwl_mvm_time_event_send_add(mvm, vif, te_data, &time_cmd);
 }
@@ -551,7 +554,8 @@ int iwl_mvm_start_p2p_roc(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 	time_cmd.duration = cpu_to_le32(MSEC_TO_TU(duration));
 	time_cmd.repeat = 1;
 	time_cmd.policy = cpu_to_le16(TE_V2_NOTIF_HOST_EVENT_START |
-				      TE_V2_NOTIF_HOST_EVENT_END);
+				      TE_V2_NOTIF_HOST_EVENT_END |
+				      T2_V2_START_IMMEDIATELY);
 
 	return iwl_mvm_time_event_send_add(mvm, vif, te_data, &time_cmd);
 }
diff --git a/drivers/net/wireless/iwlwifi/mvm/tt.c b/drivers/net/wireless/iwlwifi/mvm/tt.c
index 3afa6b6bf835..7a99fa361954 100644
--- a/drivers/net/wireless/iwlwifi/mvm/tt.c
+++ b/drivers/net/wireless/iwlwifi/mvm/tt.c
@@ -403,7 +403,7 @@ static void iwl_mvm_tt_tx_protection(struct iwl_mvm *mvm, bool enable)
 	}
 }
 
-static void iwl_mvm_tt_tx_backoff(struct iwl_mvm *mvm, u32 backoff)
+void iwl_mvm_tt_tx_backoff(struct iwl_mvm *mvm, u32 backoff)
 {
 	struct iwl_host_cmd cmd = {
 		.id = REPLY_THERMAL_MNG_BACKOFF,
@@ -412,6 +412,8 @@ static void iwl_mvm_tt_tx_backoff(struct iwl_mvm *mvm, u32 backoff)
 		.flags = CMD_SYNC,
 	};
 
+	backoff = max(backoff, mvm->thermal_throttle.min_backoff);
+
 	if (iwl_mvm_send_cmd(mvm, &cmd) == 0) {
 		IWL_DEBUG_TEMP(mvm, "Set Thermal Tx backoff to: %u\n",
 			       backoff);
@@ -534,7 +536,7 @@ static const struct iwl_tt_params iwl7000_high_temp_tt_params = {
 	.support_tx_backoff = true,
 };
 
-void iwl_mvm_tt_initialize(struct iwl_mvm *mvm)
+void iwl_mvm_tt_initialize(struct iwl_mvm *mvm, u32 min_backoff)
 {
 	struct iwl_mvm_tt_mgmt *tt = &mvm->thermal_throttle;
 
@@ -546,6 +548,7 @@ void iwl_mvm_tt_initialize(struct iwl_mvm *mvm)
 		tt->params = &iwl7000_tt_params;
 
 	tt->throttle = false;
+	tt->min_backoff = min_backoff;
 	INIT_DELAYED_WORK(&tt->ct_kill_exit, check_exit_ctkill);
 }
 
diff --git a/drivers/net/wireless/iwlwifi/mvm/tx.c b/drivers/net/wireless/iwlwifi/mvm/tx.c
index 76ee486039d7..0ba96654d2c3 100644
--- a/drivers/net/wireless/iwlwifi/mvm/tx.c
+++ b/drivers/net/wireless/iwlwifi/mvm/tx.c
@@ -122,7 +122,7 @@ static void iwl_mvm_set_tx_cmd(struct iwl_mvm *mvm, struct sk_buff *skb,
 		 * it
 		 */
 		WARN_ON_ONCE(info->flags & IEEE80211_TX_CTL_AMPDU);
-	} else if (skb->protocol == cpu_to_be16(ETH_P_PAE)) {
+	} else if (info->control.flags & IEEE80211_TX_CTRL_PORT_CTRL_PROTO) {
 		tx_cmd->pm_frame_timeout = cpu_to_le16(2);
 	} else {
 		tx_cmd->pm_frame_timeout = 0;
@@ -207,7 +207,7 @@ static void iwl_mvm_set_tx_cmd_rate(struct iwl_mvm *mvm,
 	rate_plcp = iwl_mvm_mac80211_idx_to_hwrate(rate_idx);
 
 	mvm->mgmt_last_antenna_idx =
-		iwl_mvm_next_antenna(mvm, iwl_fw_valid_tx_ant(mvm->fw),
+		iwl_mvm_next_antenna(mvm, mvm->fw->valid_tx_ant,
 				     mvm->mgmt_last_antenna_idx);
 	rate_flags = BIT(mvm->mgmt_last_antenna_idx) << RATE_MCS_ANT_POS;
 
@@ -377,6 +377,13 @@ int iwl_mvm_tx_skb(struct iwl_mvm *mvm, struct sk_buff *skb,
 	tx_cmd = (struct iwl_tx_cmd *)dev_cmd->payload;
 	/* From now on, we cannot access info->control */
 
+	/*
+	 * we handle that entirely ourselves -- for uAPSD the firmware
+	 * will always send a notification, and for PS-Poll responses
+	 * we'll notify mac80211 when getting frame status
+	 */
+	info->flags &= ~IEEE80211_TX_STATUS_EOSP;
+
 	spin_lock(&mvmsta->lock);
 
 	if (ieee80211_is_data_qos(fc) && !ieee80211_is_qos_nullfunc(fc)) {
@@ -437,6 +444,17 @@ static void iwl_mvm_check_ratid_empty(struct iwl_mvm *mvm,
 
 	lockdep_assert_held(&mvmsta->lock);
 
+	if ((tid_data->state == IWL_AGG_ON ||
+	     tid_data->state == IWL_EMPTYING_HW_QUEUE_DELBA) &&
+	    iwl_mvm_tid_queued(tid_data) == 0) {
+		/*
+		 * Now that this aggregation queue is empty tell mac80211 so it
+		 * knows we no longer have frames buffered for the station on
+		 * this TID (for the TIM bitmap calculation.)
+		 */
+		ieee80211_sta_set_buffered(sta, tid, false);
+	}
+
 	if (tid_data->ssn != tid_data->next_reclaimed)
 		return;
 
@@ -680,6 +698,11 @@ static void iwl_mvm_rx_tx_cmd_single(struct iwl_mvm *mvm,
 			iwl_mvm_check_ratid_empty(mvm, sta, tid);
 			spin_unlock_bh(&mvmsta->lock);
 		}
+
+		if (mvmsta->next_status_eosp) {
+			mvmsta->next_status_eosp = false;
+			ieee80211_sta_eosp(sta);
+		}
 	} else {
 		mvmsta = NULL;
 	}
diff --git a/drivers/net/wireless/iwlwifi/mvm/utils.c b/drivers/net/wireless/iwlwifi/mvm/utils.c
index 86989df69356..bbfe529d7b64 100644
--- a/drivers/net/wireless/iwlwifi/mvm/utils.c
+++ b/drivers/net/wireless/iwlwifi/mvm/utils.c
@@ -289,8 +289,8 @@ u8 iwl_mvm_next_antenna(struct iwl_mvm *mvm, u8 valid, u8 last_idx)
 	return last_idx;
 }
 
-static struct {
-	char *name;
+static const struct {
+	const char *name;
 	u8 num;
 } advanced_lookup[] = {
 	{ "NMI_INTERRUPT_WDG", 0x34 },
@@ -376,9 +376,67 @@ struct iwl_error_event_table {
 	u32 flow_handler;	/* FH read/write pointers, RX credit */
 } __packed;
 
+/*
+ * UMAC error struct - relevant starting from family 8000 chip.
+ * Note: This structure is read from the device with IO accesses,
+ * and the reading already does the endian conversion. As it is
+ * read with u32-sized accesses, any members with a different size
+ * need to be ordered correctly though!
+ */
+struct iwl_umac_error_event_table {
+	u32 valid;		/* (nonzero) valid, (0) log is empty */
+	u32 error_id;		/* type of error */
+	u32 pc;			/* program counter */
+	u32 blink1;		/* branch link */
+	u32 blink2;		/* branch link */
+	u32 ilink1;		/* interrupt link */
+	u32 ilink2;		/* interrupt link */
+	u32 data1;		/* error-specific data */
+	u32 data2;		/* error-specific data */
+	u32 line;		/* source code line of error */
+	u32 umac_ver;		/* umac version */
+} __packed;
+
 #define ERROR_START_OFFSET  (1 * sizeof(u32))
 #define ERROR_ELEM_SIZE     (7 * sizeof(u32))
 
+static void iwl_mvm_dump_umac_error_log(struct iwl_mvm *mvm)
+{
+	struct iwl_trans *trans = mvm->trans;
+	struct iwl_umac_error_event_table table;
+	u32 base;
+
+	base = mvm->umac_error_event_table;
+
+	if (base < 0x800000 || base >= 0x80C000) {
+		IWL_ERR(mvm,
+			"Not valid error log pointer 0x%08X for %s uCode\n",
+			base,
+			(mvm->cur_ucode == IWL_UCODE_INIT)
+					? "Init" : "RT");
+		return;
+	}
+
+	iwl_trans_read_mem_bytes(trans, base, &table, sizeof(table));
+
+	if (ERROR_START_OFFSET <= table.valid * ERROR_ELEM_SIZE) {
+		IWL_ERR(trans, "Start IWL Error Log Dump:\n");
+		IWL_ERR(trans, "Status: 0x%08lX, count: %d\n",
+			mvm->status, table.valid);
+	}
+
+	IWL_ERR(mvm, "0x%08X | %-28s\n", table.error_id,
+		desc_lookup(table.error_id));
+	IWL_ERR(mvm, "0x%08X | umac uPc\n", table.pc);
+	IWL_ERR(mvm, "0x%08X | umac branchlink1\n", table.blink1);
+	IWL_ERR(mvm, "0x%08X | umac branchlink2\n", table.blink2);
+	IWL_ERR(mvm, "0x%08X | umac interruptlink1\n", table.ilink1);
+	IWL_ERR(mvm, "0x%08X | umac interruptlink2\n", table.ilink2);
+	IWL_ERR(mvm, "0x%08X | umac data1\n", table.data1);
+	IWL_ERR(mvm, "0x%08X | umac data2\n", table.data2);
+	IWL_ERR(mvm, "0x%08X | umac version\n", table.umac_ver);
+}
+
 void iwl_mvm_dump_nic_error_log(struct iwl_mvm *mvm)
 {
 	struct iwl_trans *trans = mvm->trans;
@@ -394,7 +452,7 @@ void iwl_mvm_dump_nic_error_log(struct iwl_mvm *mvm)
 			base = mvm->fw->inst_errlog_ptr;
 	}
 
-	if (base < 0x800000 || base >= 0x80C000) {
+	if (base < 0x800000) {
 		IWL_ERR(mvm,
 			"Not valid error log pointer 0x%08X for %s uCode\n",
 			base,
@@ -453,13 +511,17 @@ void iwl_mvm_dump_nic_error_log(struct iwl_mvm *mvm)
 	IWL_ERR(mvm, "0x%08X | lmpm_pmg_sel\n", table.lmpm_pmg_sel);
 	IWL_ERR(mvm, "0x%08X | timestamp\n", table.u_timestamp);
 	IWL_ERR(mvm, "0x%08X | flow_handler\n", table.flow_handler);
+
+	if (mvm->support_umac_log)
+		iwl_mvm_dump_umac_error_log(mvm);
 }
 
 void iwl_mvm_dump_sram(struct iwl_mvm *mvm)
 {
 	const struct fw_img *img;
 	int ofs, len = 0;
-	u8 *buf;
+	int i;
+	__le32 *buf;
 
 	if (!mvm->ucode_loaded)
 		return;
@@ -473,7 +535,12 @@ void iwl_mvm_dump_sram(struct iwl_mvm *mvm)
 		return;
 
 	iwl_trans_read_mem_bytes(mvm->trans, ofs, buf, len);
-	iwl_print_hex_error(mvm->trans, buf, len);
+	len = len >> 2;
+	for (i = 0; i < len; i++) {
+		IWL_ERR(mvm, "0x%08X\n", le32_to_cpu(buf[i]));
+		/* Add a small delay to let syslog catch up */
+		udelay(10);
+	}
 
 	kfree(buf);
 }
@@ -516,15 +583,20 @@ void iwl_mvm_update_smps(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 			 enum ieee80211_smps_mode smps_request)
 {
 	struct iwl_mvm_vif *mvmvif;
-	enum ieee80211_smps_mode smps_mode = IEEE80211_SMPS_AUTOMATIC;
+	enum ieee80211_smps_mode smps_mode;
 	int i;
 
 	lockdep_assert_held(&mvm->mutex);
 
 	/* SMPS is irrelevant for NICs that don't have at least 2 RX antenna */
-	if (num_of_ant(iwl_fw_valid_rx_ant(mvm->fw)) == 1)
+	if (num_of_ant(mvm->fw->valid_rx_ant) == 1)
 		return;
 
+	if (vif->type == NL80211_IFTYPE_AP)
+		smps_mode = IEEE80211_SMPS_OFF;
+	else
+		smps_mode = IEEE80211_SMPS_AUTOMATIC;
+
 	mvmvif = iwl_mvm_vif_from_mac80211(vif);
 	mvmvif->smps_requests[req_type] = smps_request;
 	for (i = 0; i < NUM_IWL_MVM_SMPS_REQ; i++) {
@@ -538,3 +610,22 @@ void iwl_mvm_update_smps(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 
 	ieee80211_request_smps(vif, smps_mode);
 }
+
+int iwl_mvm_update_low_latency(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
+			       bool value)
+{
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+	int res;
+
+	lockdep_assert_held(&mvm->mutex);
+
+	mvmvif->low_latency = value;
+
+	res = iwl_mvm_update_quotas(mvm, NULL);
+	if (res)
+		return res;
+
+	iwl_mvm_bt_coex_vif_change(mvm);
+
+	return iwl_mvm_power_update_mac(mvm, vif);
+}
diff --git a/drivers/net/wireless/iwlwifi/pcie/drv.c b/drivers/net/wireless/iwlwifi/pcie/drv.c
index 3872ead75488..1f97631a82e4 100644
--- a/drivers/net/wireless/iwlwifi/pcie/drv.c
+++ b/drivers/net/wireless/iwlwifi/pcie/drv.c
@@ -66,6 +66,7 @@
 #include <linux/module.h>
 #include <linux/pci.h>
 #include <linux/pci-aspm.h>
+#include <linux/acpi.h>
 
 #include "iwl-trans.h"
 #include "iwl-drv.h"
@@ -389,12 +390,91 @@ static DEFINE_PCI_DEVICE_TABLE(iwl_hw_card_ids) = {
 	{IWL_PCI_DEVICE(0x095A, 0x5590, iwl7265_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x095B, 0x5290, iwl7265_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x095A, 0x5490, iwl7265_2ac_cfg)},
+
+/* 8000 Series */
+	{IWL_PCI_DEVICE(0x24F3, 0x0010, iwl8260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24F4, 0x0030, iwl8260_2ac_cfg)},
 #endif /* CONFIG_IWLMVM */
 
 	{0}
 };
 MODULE_DEVICE_TABLE(pci, iwl_hw_card_ids);
 
+#ifdef CONFIG_ACPI
+#define SPL_METHOD		"SPLC"
+#define SPL_DOMAINTYPE_MODULE	BIT(0)
+#define SPL_DOMAINTYPE_WIFI	BIT(1)
+#define SPL_DOMAINTYPE_WIGIG	BIT(2)
+#define SPL_DOMAINTYPE_RFEM	BIT(3)
+
+static u64 splx_get_pwr_limit(struct iwl_trans *trans, union acpi_object *splx)
+{
+	union acpi_object *limits, *domain_type, *power_limit;
+
+	if (splx->type != ACPI_TYPE_PACKAGE ||
+	    splx->package.count != 2 ||
+	    splx->package.elements[0].type != ACPI_TYPE_INTEGER ||
+	    splx->package.elements[0].integer.value != 0) {
+		IWL_ERR(trans, "Unsupported splx structure");
+		return 0;
+	}
+
+	limits = &splx->package.elements[1];
+	if (limits->type != ACPI_TYPE_PACKAGE ||
+	    limits->package.count < 2 ||
+	    limits->package.elements[0].type != ACPI_TYPE_INTEGER ||
+	    limits->package.elements[1].type != ACPI_TYPE_INTEGER) {
+		IWL_ERR(trans, "Invalid limits element");
+		return 0;
+	}
+
+	domain_type = &limits->package.elements[0];
+	power_limit = &limits->package.elements[1];
+	if (!(domain_type->integer.value & SPL_DOMAINTYPE_WIFI)) {
+		IWL_DEBUG_INFO(trans, "WiFi power is not limited");
+		return 0;
+	}
+
+	return power_limit->integer.value;
+}
+
+static void set_dflt_pwr_limit(struct iwl_trans *trans, struct pci_dev *pdev)
+{
+	acpi_handle pxsx_handle;
+	acpi_handle handle;
+	struct acpi_buffer splx = {ACPI_ALLOCATE_BUFFER, NULL};
+	acpi_status status;
+
+	pxsx_handle = ACPI_HANDLE(&pdev->dev);
+	if (!pxsx_handle) {
+		IWL_ERR(trans, "Could not retrieve root port ACPI handle");
+		return;
+	}
+
+	/* Get the method's handle */
+	status = acpi_get_handle(pxsx_handle, (acpi_string)SPL_METHOD, &handle);
+	if (ACPI_FAILURE(status)) {
+		IWL_DEBUG_INFO(trans, "SPL method not found");
+		return;
+	}
+
+	/* Call SPLC with no arguments */
+	status = acpi_evaluate_object(handle, NULL, NULL, &splx);
+	if (ACPI_FAILURE(status)) {
+		IWL_ERR(trans, "SPLC invocation failed (0x%x)", status);
+		return;
+	}
+
+	trans->dflt_pwr_limit = splx_get_pwr_limit(trans, splx.pointer);
+	IWL_DEBUG_INFO(trans, "Default power limit set to %lld",
+		       trans->dflt_pwr_limit);
+	kfree(splx.pointer);
+}
+
+#else /* CONFIG_ACPI */
+static void set_dflt_pwr_limit(struct iwl_trans *trans, struct pci_dev *pdev) {}
+#endif
+
 /* PCI registers */
 #define PCI_CFG_RETRY_TIMEOUT	0x041
 
@@ -419,6 +499,8 @@ static int iwl_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		goto out_free_trans;
 	}
 
+	set_dflt_pwr_limit(iwl_trans, pdev);
+
 	/* register transport layer debugfs here */
 	ret = iwl_trans_dbgfs_register(iwl_trans, iwl_trans->dbgfs_dir);
 	if (ret)
diff --git a/drivers/net/wireless/iwlwifi/pcie/internal.h b/drivers/net/wireless/iwlwifi/pcie/internal.h
index e851f26fd44c..3120bc5bb12d 100644
--- a/drivers/net/wireless/iwlwifi/pcie/internal.h
+++ b/drivers/net/wireless/iwlwifi/pcie/internal.h
@@ -304,7 +304,7 @@ struct iwl_trans_pcie {
 	bool bc_table_dword;
 	u32 rx_page_order;
 
-	const char **command_names;
+	const char *const *command_names;
 
 	/* queue watchdog */
 	unsigned long wd_timeout;
diff --git a/drivers/net/wireless/iwlwifi/pcie/rx.c b/drivers/net/wireless/iwlwifi/pcie/rx.c
index 08c23d497a02..cf49f6ce0ff8 100644
--- a/drivers/net/wireless/iwlwifi/pcie/rx.c
+++ b/drivers/net/wireless/iwlwifi/pcie/rx.c
@@ -155,37 +155,26 @@ static void iwl_pcie_rxq_inc_wr_ptr(struct iwl_trans *trans,
 	if (rxq->need_update == 0)
 		goto exit_unlock;
 
-	if (trans->cfg->base_params->shadow_reg_enable) {
-		/* shadow register enabled */
-		/* Device expects a multiple of 8 */
-		rxq->write_actual = (rxq->write & ~0x7);
-		iwl_write32(trans, FH_RSCSR_CHNL0_WPTR, rxq->write_actual);
-	} else {
-		/* If power-saving is in use, make sure device is awake */
-		if (test_bit(STATUS_TPOWER_PMI, &trans->status)) {
-			reg = iwl_read32(trans, CSR_UCODE_DRV_GP1);
-
-			if (reg & CSR_UCODE_DRV_GP1_BIT_MAC_SLEEP) {
-				IWL_DEBUG_INFO(trans,
-					"Rx queue requesting wakeup,"
-					" GP1 = 0x%x\n", reg);
-				iwl_set_bit(trans, CSR_GP_CNTRL,
-					CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
-				goto exit_unlock;
-			}
-
-			rxq->write_actual = (rxq->write & ~0x7);
-			iwl_write_direct32(trans, FH_RSCSR_CHNL0_WPTR,
-					   rxq->write_actual);
-
-		/* Else device is assumed to be awake */
-		} else {
-			/* Device expects a multiple of 8 */
-			rxq->write_actual = (rxq->write & ~0x7);
-			iwl_write_direct32(trans, FH_RSCSR_CHNL0_WPTR,
-					   rxq->write_actual);
+	/*
+	 * explicitly wake up the NIC if:
+	 * 1. shadow registers aren't enabled
+	 * 2. there is a chance that the NIC is asleep
+	 */
+	if (!trans->cfg->base_params->shadow_reg_enable &&
+	    test_bit(STATUS_TPOWER_PMI, &trans->status)) {
+		reg = iwl_read32(trans, CSR_UCODE_DRV_GP1);
+
+		if (reg & CSR_UCODE_DRV_GP1_BIT_MAC_SLEEP) {
+			IWL_DEBUG_INFO(trans, "Rx queue requesting wakeup, GP1 = 0x%x\n",
+				       reg);
+			iwl_set_bit(trans, CSR_GP_CNTRL,
+				    CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
+			goto exit_unlock;
 		}
 	}
+
+	rxq->write_actual = round_down(rxq->write, 8);
+	iwl_write32(trans, FH_RSCSR_CHNL0_WPTR, rxq->write_actual);
 	rxq->need_update = 0;
 
  exit_unlock:
@@ -802,10 +791,9 @@ static void iwl_pcie_irq_handle_error(struct iwl_trans *trans)
 
 static u32 iwl_pcie_int_cause_non_ict(struct iwl_trans *trans)
 {
-	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	u32 inta;
 
-	lockdep_assert_held(&trans_pcie->irq_lock);
+	lockdep_assert_held(&IWL_TRANS_GET_PCIE_TRANS(trans)->irq_lock);
 
 	trace_iwlwifi_dev_irq(trans->dev);
 
diff --git a/drivers/net/wireless/iwlwifi/pcie/trans.c b/drivers/net/wireless/iwlwifi/pcie/trans.c
index f9507807b486..84d471299e5a 100644
--- a/drivers/net/wireless/iwlwifi/pcie/trans.c
+++ b/drivers/net/wireless/iwlwifi/pcie/trans.c
@@ -89,6 +89,7 @@ static void iwl_pcie_set_pwr(struct iwl_trans *trans, bool vaux)
 
 /* PCI registers */
 #define PCI_CFG_RETRY_TIMEOUT	0x041
+#define CPU1_CPU2_SEPARATOR_SECTION	0xFFFFCCCC
 
 static void iwl_pcie_apm_config(struct iwl_trans *trans)
 {
@@ -132,8 +133,9 @@ static int iwl_pcie_apm_init(struct iwl_trans *trans)
 	 */
 
 	/* Disable L0S exit timer (platform NMI Work/Around) */
-	iwl_set_bit(trans, CSR_GIO_CHICKEN_BITS,
-		    CSR_GIO_CHICKEN_BITS_REG_BIT_DIS_L0S_EXIT_TIMER);
+	if (trans->cfg->device_family != IWL_DEVICE_FAMILY_8000)
+		iwl_set_bit(trans, CSR_GIO_CHICKEN_BITS,
+			    CSR_GIO_CHICKEN_BITS_REG_BIT_DIS_L0S_EXIT_TIMER);
 
 	/*
 	 * Disable L0s without affecting L1;
@@ -203,19 +205,23 @@ static int iwl_pcie_apm_init(struct iwl_trans *trans)
 	/*
 	 * Enable DMA clock and wait for it to stabilize.
 	 *
-	 * Write to "CLK_EN_REG"; "1" bits enable clocks, while "0" bits
-	 * do not disable clocks.  This preserves any hardware bits already
-	 * set by default in "CLK_CTRL_REG" after reset.
+	 * Write to "CLK_EN_REG"; "1" bits enable clocks, while "0"
+	 * bits do not disable clocks.  This preserves any hardware
+	 * bits already set by default in "CLK_CTRL_REG" after reset.
 	 */
-	iwl_write_prph(trans, APMG_CLK_EN_REG, APMG_CLK_VAL_DMA_CLK_RQT);
-	udelay(20);
+	if (trans->cfg->device_family != IWL_DEVICE_FAMILY_8000) {
+		iwl_write_prph(trans, APMG_CLK_EN_REG,
+			       APMG_CLK_VAL_DMA_CLK_RQT);
+		udelay(20);
 
-	/* Disable L1-Active */
-	iwl_set_bits_prph(trans, APMG_PCIDEV_STT_REG,
-			  APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
+		/* Disable L1-Active */
+		iwl_set_bits_prph(trans, APMG_PCIDEV_STT_REG,
+				  APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
 
-	/* Clear the interrupt in APMG if the NIC is in RFKILL */
-	iwl_write_prph(trans, APMG_RTC_INT_STT_REG, APMG_RTC_INT_STT_RFKILL);
+		/* Clear the interrupt in APMG if the NIC is in RFKILL */
+		iwl_write_prph(trans, APMG_RTC_INT_STT_REG,
+			       APMG_RTC_INT_STT_RFKILL);
+	}
 
 	set_bit(STATUS_DEVICE_ENABLED, &trans->status);
 
@@ -273,7 +279,8 @@ static int iwl_pcie_nic_init(struct iwl_trans *trans)
 
 	spin_unlock(&trans_pcie->irq_lock);
 
-	iwl_pcie_set_pwr(trans, false);
+	if (trans->cfg->device_family != IWL_DEVICE_FAMILY_8000)
+		iwl_pcie_set_pwr(trans, false);
 
 	iwl_op_mode_nic_config(trans->op_mode);
 
@@ -435,78 +442,106 @@ static int iwl_pcie_load_section(struct iwl_trans *trans, u8 section_num,
 	return ret;
 }
 
-static int iwl_pcie_secure_set(struct iwl_trans *trans, int cpu)
+static int iwl_pcie_load_cpu_secured_sections(struct iwl_trans *trans,
+					      const struct fw_img *image,
+					      int cpu,
+					      int *first_ucode_section)
 {
 	int shift_param;
-	u32 address;
-	int ret = 0;
+	int i, ret = 0;
+	u32 last_read_idx = 0;
 
 	if (cpu == 1) {
 		shift_param = 0;
-		address = CSR_SECURE_BOOT_CPU1_STATUS_ADDR;
+		*first_ucode_section = 0;
 	} else {
 		shift_param = 16;
-		address = CSR_SECURE_BOOT_CPU2_STATUS_ADDR;
+		(*first_ucode_section)++;
 	}
 
-	/* set CPU to started */
-	iwl_trans_set_bits_mask(trans,
-				CSR_UCODE_LOAD_STATUS_ADDR,
-				CSR_CPU_STATUS_LOADING_STARTED << shift_param,
-				1);
-
-	/* set last complete descriptor number */
-	iwl_trans_set_bits_mask(trans,
-				CSR_UCODE_LOAD_STATUS_ADDR,
-				CSR_CPU_STATUS_NUM_OF_LAST_COMPLETED
-				<< shift_param,
-				1);
-
-	/* set last loaded block */
-	iwl_trans_set_bits_mask(trans,
-				CSR_UCODE_LOAD_STATUS_ADDR,
-				CSR_CPU_STATUS_NUM_OF_LAST_LOADED_BLOCK
-				<< shift_param,
-				1);
+	for (i = *first_ucode_section; i < IWL_UCODE_SECTION_MAX; i++) {
+		last_read_idx = i;
+
+		if (!image->sec[i].data ||
+		    image->sec[i].offset == CPU1_CPU2_SEPARATOR_SECTION) {
+			IWL_DEBUG_FW(trans,
+				     "Break since Data not valid or Empty section, sec = %d\n",
+				     i);
+			break;
+		}
+
+		if (i == (*first_ucode_section) + 1)
+			/* set CPU to started */
+			iwl_set_bits_prph(trans,
+					  CSR_UCODE_LOAD_STATUS_ADDR,
+					  LMPM_CPU_HDRS_LOADING_COMPLETED
+					  << shift_param);
 
+		ret = iwl_pcie_load_section(trans, i, &image->sec[i]);
+		if (ret)
+			return ret;
+	}
 	/* image loading complete */
-	iwl_trans_set_bits_mask(trans,
-				CSR_UCODE_LOAD_STATUS_ADDR,
-				CSR_CPU_STATUS_LOADING_COMPLETED
-				<< shift_param,
-				1);
-
-	/* set FH_TCSR_0_REG  */
-	iwl_trans_set_bits_mask(trans, FH_TCSR_0_REG0, 0x00400000, 1);
-
-	/* verify image verification started  */
-	ret = iwl_poll_bit(trans, address,
-			   CSR_SECURE_BOOT_CPU_STATUS_VERF_STATUS,
-			   CSR_SECURE_BOOT_CPU_STATUS_VERF_STATUS,
-			   CSR_SECURE_TIME_OUT);
-	if (ret < 0) {
-		IWL_ERR(trans, "secure boot process didn't start\n");
-		return ret;
+	iwl_set_bits_prph(trans,
+			  CSR_UCODE_LOAD_STATUS_ADDR,
+			  LMPM_CPU_UCODE_LOADING_COMPLETED << shift_param);
+
+	*first_ucode_section = last_read_idx;
+
+	return 0;
+}
+
+static int iwl_pcie_load_cpu_sections(struct iwl_trans *trans,
+				      const struct fw_img *image,
+				      int cpu,
+				      int *first_ucode_section)
+{
+	int shift_param;
+	int i, ret = 0;
+	u32 last_read_idx = 0;
+
+	if (cpu == 1) {
+		shift_param = 0;
+		*first_ucode_section = 0;
+	} else {
+		shift_param = 16;
+		(*first_ucode_section)++;
 	}
 
-	/* wait for image verification to complete  */
-	ret = iwl_poll_bit(trans, address,
-			   CSR_SECURE_BOOT_CPU_STATUS_VERF_COMPLETED,
-			   CSR_SECURE_BOOT_CPU_STATUS_VERF_COMPLETED,
-			   CSR_SECURE_TIME_OUT);
+	for (i = *first_ucode_section; i < IWL_UCODE_SECTION_MAX; i++) {
+		last_read_idx = i;
 
-	if (ret < 0) {
-		IWL_ERR(trans, "Time out on secure boot process\n");
-		return ret;
+		if (!image->sec[i].data ||
+		    image->sec[i].offset == CPU1_CPU2_SEPARATOR_SECTION) {
+			IWL_DEBUG_FW(trans,
+				     "Break since Data not valid or Empty section, sec = %d\n",
+				     i);
+			break;
+		}
+
+		ret = iwl_pcie_load_section(trans, i, &image->sec[i]);
+		if (ret)
+			return ret;
 	}
 
+	if (trans->cfg->device_family == IWL_DEVICE_FAMILY_8000)
+		iwl_set_bits_prph(trans,
+				  CSR_UCODE_LOAD_STATUS_ADDR,
+				  (LMPM_CPU_UCODE_LOADING_COMPLETED |
+				   LMPM_CPU_HDRS_LOADING_COMPLETED |
+				   LMPM_CPU_UCODE_LOADING_STARTED) <<
+					shift_param);
+
+	*first_ucode_section = last_read_idx;
+
 	return 0;
 }
 
 static int iwl_pcie_load_given_ucode(struct iwl_trans *trans,
 				const struct fw_img *image)
 {
-	int i, ret = 0;
+	int ret = 0;
+	int first_ucode_section;
 
 	IWL_DEBUG_FW(trans,
 		     "working with %s image\n",
@@ -518,53 +553,68 @@ static int iwl_pcie_load_given_ucode(struct iwl_trans *trans,
 	/* configure the ucode to be ready to get the secured image */
 	if (image->is_secure) {
 		/* set secure boot inspector addresses */
-		iwl_write32(trans, CSR_SECURE_INSPECTOR_CODE_ADDR, 0);
-		iwl_write32(trans, CSR_SECURE_INSPECTOR_DATA_ADDR, 0);
-
-		/* release CPU1 reset if secure inspector image burned in OTP */
-		iwl_write32(trans, CSR_RESET, 0);
-	}
-
-	/* load to FW the binary sections of CPU1 */
-	IWL_DEBUG_INFO(trans, "Loading CPU1\n");
-	for (i = 0;
-	     i < IWL_UCODE_FIRST_SECTION_OF_SECOND_CPU;
-	     i++) {
-		if (!image->sec[i].data)
-			break;
-		ret = iwl_pcie_load_section(trans, i, &image->sec[i]);
+		iwl_write_prph(trans,
+			       LMPM_SECURE_INSPECTOR_CODE_ADDR,
+			       LMPM_SECURE_INSPECTOR_CODE_MEM_SPACE);
+
+		iwl_write_prph(trans,
+			       LMPM_SECURE_INSPECTOR_DATA_ADDR,
+			       LMPM_SECURE_INSPECTOR_DATA_MEM_SPACE);
+
+		/* set CPU1 header address */
+		iwl_write_prph(trans,
+			       LMPM_SECURE_UCODE_LOAD_CPU1_HDR_ADDR,
+			       LMPM_SECURE_CPU1_HDR_MEM_SPACE);
+
+		/* load to FW the binary Secured sections of CPU1 */
+		ret = iwl_pcie_load_cpu_secured_sections(trans, image, 1,
+							 &first_ucode_section);
 		if (ret)
 			return ret;
-	}
 
-	/* configure the ucode to start secure process on CPU1 */
-	if (image->is_secure) {
-		/* config CPU1 to start secure protocol */
-		ret = iwl_pcie_secure_set(trans, 1);
+	} else {
+		/* load to FW the binary Non secured sections of CPU1 */
+		ret = iwl_pcie_load_cpu_sections(trans, image, 1,
+						 &first_ucode_section);
 		if (ret)
 			return ret;
-	} else {
-		/* Remove all resets to allow NIC to operate */
-		iwl_write32(trans, CSR_RESET, 0);
 	}
 
 	if (image->is_dual_cpus) {
+		/* set CPU2 header address */
+		iwl_write_prph(trans,
+			       LMPM_SECURE_UCODE_LOAD_CPU2_HDR_ADDR,
+			       LMPM_SECURE_CPU2_HDR_MEM_SPACE);
+
 		/* load to FW the binary sections of CPU2 */
-		IWL_DEBUG_INFO(trans, "working w/ DUAL CPUs - Loading CPU2\n");
-		for (i = IWL_UCODE_FIRST_SECTION_OF_SECOND_CPU;
-			i < IWL_UCODE_SECTION_MAX; i++) {
-			if (!image->sec[i].data)
-				break;
-			ret = iwl_pcie_load_section(trans, i, &image->sec[i]);
-			if (ret)
-				return ret;
-		}
+		if (image->is_secure)
+			ret = iwl_pcie_load_cpu_secured_sections(
+							trans, image, 2,
+							&first_ucode_section);
+		else
+			ret = iwl_pcie_load_cpu_sections(trans, image, 2,
+							 &first_ucode_section);
+		if (ret)
+			return ret;
+	}
 
-		if (image->is_secure) {
-			/* set CPU2 for secure protocol */
-			ret = iwl_pcie_secure_set(trans, 2);
-			if (ret)
-				return ret;
+	/* release CPU reset */
+	if (trans->cfg->device_family == IWL_DEVICE_FAMILY_8000)
+		iwl_write_prph(trans, RELEASE_CPU_RESET, RELEASE_CPU_RESET_BIT);
+	else
+		iwl_write32(trans, CSR_RESET, 0);
+
+	if (image->is_secure) {
+		/* wait for image verification to complete  */
+		ret = iwl_poll_prph_bit(trans,
+					LMPM_SECURE_BOOT_CPU1_STATUS_ADDR,
+					LMPM_SECURE_BOOT_STATUS_SUCCESS,
+					LMPM_SECURE_BOOT_STATUS_SUCCESS,
+					LMPM_SECURE_TIME_OUT);
+
+		if (ret < 0) {
+			IWL_ERR(trans, "Time out on secure boot process\n");
+			return ret;
 		}
 	}
 
@@ -1407,16 +1457,15 @@ static ssize_t iwl_dbgfs_fh_reg_read(struct file *file,
 {
 	struct iwl_trans *trans = file->private_data;
 	char *buf = NULL;
-	int pos = 0;
-	ssize_t ret = -EFAULT;
-
-	ret = pos = iwl_dump_fh(trans, &buf);
-	if (buf) {
-		ret = simple_read_from_buffer(user_buf,
-					      count, ppos, buf, pos);
-		kfree(buf);
-	}
+	ssize_t ret;
 
+	ret = iwl_dump_fh(trans, &buf);
+	if (ret < 0)
+		return ret;
+	if (!buf)
+		return -EINVAL;
+	ret = simple_read_from_buffer(user_buf, count, ppos, buf, ret);
+	kfree(buf);
 	return ret;
 }
 
diff --git a/drivers/net/wireless/iwlwifi/pcie/tx.c b/drivers/net/wireless/iwlwifi/pcie/tx.c
index 3d549008b3e2..3b0c72c10054 100644
--- a/drivers/net/wireless/iwlwifi/pcie/tx.c
+++ b/drivers/net/wireless/iwlwifi/pcie/tx.c
@@ -207,7 +207,7 @@ static void iwl_pcie_txq_stuck_timer(unsigned long data)
 		IWL_ERR(trans, "scratch %d = 0x%08x\n", i,
 			le32_to_cpu(txq->scratchbufs[i].scratch));
 
-	iwl_trans_fw_error(trans);
+	iwl_write_prph(trans, DEVICE_SET_NMI_REG, 1);
 }
 
 /*
@@ -296,43 +296,38 @@ void iwl_pcie_txq_inc_wr_ptr(struct iwl_trans *trans, struct iwl_txq *txq)
 	if (txq->need_update == 0)
 		return;
 
-	if (trans->cfg->base_params->shadow_reg_enable ||
-	    txq_id == trans_pcie->cmd_queue) {
-		/* shadow register enabled */
-		iwl_write32(trans, HBUS_TARG_WRPTR,
-			    txq->q.write_ptr | (txq_id << 8));
-	} else {
-		/* if we're trying to save power */
-		if (test_bit(STATUS_TPOWER_PMI, &trans->status)) {
-			/* wake up nic if it's powered down ...
-			 * uCode will wake up, and interrupt us again, so next
-			 * time we'll skip this part. */
-			reg = iwl_read32(trans, CSR_UCODE_DRV_GP1);
-
-			if (reg & CSR_UCODE_DRV_GP1_BIT_MAC_SLEEP) {
-				IWL_DEBUG_INFO(trans,
-					"Tx queue %d requesting wakeup,"
-					" GP1 = 0x%x\n", txq_id, reg);
-				iwl_set_bit(trans, CSR_GP_CNTRL,
-					CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
-				return;
-			}
-
-			IWL_DEBUG_TX(trans, "Q:%d WR: 0x%x\n", txq_id,
-				     txq->q.write_ptr);
-
-			iwl_write_direct32(trans, HBUS_TARG_WRPTR,
-				     txq->q.write_ptr | (txq_id << 8));
-
+	/*
+	 * explicitly wake up the NIC if:
+	 * 1. shadow registers aren't enabled
+	 * 2. NIC is woken up for CMD regardless of shadow outside this function
+	 * 3. there is a chance that the NIC is asleep
+	 */
+	if (!trans->cfg->base_params->shadow_reg_enable &&
+	    txq_id != trans_pcie->cmd_queue &&
+	    test_bit(STATUS_TPOWER_PMI, &trans->status)) {
 		/*
-		 * else not in power-save mode,
-		 * uCode will never sleep when we're
-		 * trying to tx (during RFKILL, we're not trying to tx).
+		 * wake up nic if it's powered down ...
+		 * uCode will wake up, and interrupt us again, so next
+		 * time we'll skip this part.
 		 */
-		} else
-			iwl_write32(trans, HBUS_TARG_WRPTR,
-				    txq->q.write_ptr | (txq_id << 8));
+		reg = iwl_read32(trans, CSR_UCODE_DRV_GP1);
+
+		if (reg & CSR_UCODE_DRV_GP1_BIT_MAC_SLEEP) {
+			IWL_DEBUG_INFO(trans, "Tx queue %d requesting wakeup, GP1 = 0x%x\n",
+				       txq_id, reg);
+			iwl_set_bit(trans, CSR_GP_CNTRL,
+				    CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
+			return;
+		}
 	}
+
+	/*
+	 * if not in power-save mode, uCode will never sleep when we're
+	 * trying to tx (during RFKILL, we're not trying to tx).
+	 */
+	IWL_DEBUG_TX(trans, "Q:%d WR: 0x%x\n", txq_id, txq->q.write_ptr);
+	iwl_write32(trans, HBUS_TARG_WRPTR, txq->q.write_ptr | (txq_id << 8));
+
 	txq->need_update = 0;
 }
 
@@ -705,8 +700,9 @@ void iwl_pcie_tx_start(struct iwl_trans *trans, u32 scd_base_addr)
 			   reg_val | FH_TX_CHICKEN_BITS_SCD_AUTO_RETRY_EN);
 
 	/* Enable L1-Active */
-	iwl_clear_bits_prph(trans, APMG_PCIDEV_STT_REG,
-			    APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
+	if (trans->cfg->device_family != IWL_DEVICE_FAMILY_8000)
+		iwl_clear_bits_prph(trans, APMG_PCIDEV_STT_REG,
+				    APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
 }
 
 void iwl_trans_pcie_tx_reset(struct iwl_trans *trans)
@@ -1028,7 +1024,7 @@ static void iwl_pcie_cmdq_reclaim(struct iwl_trans *trans, int txq_id, int idx)
 		if (nfreed++ > 0) {
 			IWL_ERR(trans, "HCMD skipped: index (%d) %d %d\n",
 				idx, q->write_ptr, q->read_ptr);
-			iwl_trans_fw_error(trans);
+			iwl_write_prph(trans, DEVICE_SET_NMI_REG, 1);
 		}
 	}
 
@@ -1587,6 +1583,7 @@ static int iwl_pcie_send_hcmd_sync(struct iwl_trans *trans,
 			       get_cmd_string(trans_pcie, cmd->id));
 		ret = -ETIMEDOUT;
 
+		iwl_write_prph(trans, DEVICE_SET_NMI_REG, 1);
 		iwl_trans_fw_error(trans);
 
 		goto cancel;
diff --git a/drivers/net/wireless/libertas/cfg.c b/drivers/net/wireless/libertas/cfg.c
index 32f75007a825..54e344aed6e0 100644
--- a/drivers/net/wireless/libertas/cfg.c
+++ b/drivers/net/wireless/libertas/cfg.c
@@ -621,7 +621,7 @@ static int lbs_ret_scan(struct lbs_private *priv, unsigned long dummy,
 			id = *pos++;
 			elen = *pos++;
 			left -= 2;
-			if (elen > left || elen == 0) {
+			if (elen > left) {
 				lbs_deb_scan("scan response: invalid IE fmt\n");
 				goto done;
 			}
@@ -1766,7 +1766,8 @@ static void lbs_join_post(struct lbs_private *priv,
 	memcpy(priv->wdev->ssid, params->ssid, params->ssid_len);
 	priv->wdev->ssid_len = params->ssid_len;
 
-	cfg80211_ibss_joined(priv->dev, bssid, GFP_KERNEL);
+	cfg80211_ibss_joined(priv->dev, bssid, params->chandef.chan,
+			     GFP_KERNEL);
 
 	/* TODO: consider doing this at MACREG_INT_CODE_LINK_SENSED time */
 	priv->connect_status = LBS_CONNECTED;
diff --git a/drivers/net/wireless/libertas/if_sdio.c b/drivers/net/wireless/libertas/if_sdio.c
index 58c6ee5de98f..33ceda296c9c 100644
--- a/drivers/net/wireless/libertas/if_sdio.c
+++ b/drivers/net/wireless/libertas/if_sdio.c
@@ -498,7 +498,7 @@ static int if_sdio_prog_helper(struct if_sdio_card *card,
 		 */
 		mdelay(2);
 
-		chunk_size = min(size, (size_t)60);
+		chunk_size = min_t(size_t, size, 60);
 
 		*((__le32*)chunk_buffer) = cpu_to_le32(chunk_size);
 		memcpy(chunk_buffer + 4, firmware, chunk_size);
@@ -639,7 +639,7 @@ static int if_sdio_prog_real(struct if_sdio_card *card,
 			req_size = size;
 
 		while (req_size) {
-			chunk_size = min(req_size, (size_t)512);
+			chunk_size = min_t(size_t, req_size, 512);
 
 			memcpy(chunk_buffer, firmware, chunk_size);
 /*
diff --git a/drivers/net/wireless/mac80211_hwsim.c b/drivers/net/wireless/mac80211_hwsim.c
index 69d4c3179d04..9d7a52f5a410 100644
--- a/drivers/net/wireless/mac80211_hwsim.c
+++ b/drivers/net/wireless/mac80211_hwsim.c
@@ -57,6 +57,10 @@ static bool rctbl = false;
 module_param(rctbl, bool, 0444);
 MODULE_PARM_DESC(rctbl, "Handle rate control table");
 
+static bool support_p2p_device = true;
+module_param(support_p2p_device, bool, 0444);
+MODULE_PARM_DESC(support_p2p_device, "Support P2P-Device interface type");
+
 /**
  * enum hwsim_regtest - the type of regulatory tests we offer
  *
@@ -335,7 +339,8 @@ static const struct ieee80211_iface_limit hwsim_if_limits[] = {
 #endif
 				 BIT(NL80211_IFTYPE_AP) |
 				 BIT(NL80211_IFTYPE_P2P_GO) },
-	{ .max = 1, .types = BIT(NL80211_IFTYPE_P2P_DEVICE) },
+	/* must be last, see hwsim_if_comb */
+	{ .max = 1, .types = BIT(NL80211_IFTYPE_P2P_DEVICE) }
 };
 
 static const struct ieee80211_iface_limit hwsim_if_dfs_limits[] = {
@@ -345,6 +350,27 @@ static const struct ieee80211_iface_limit hwsim_if_dfs_limits[] = {
 static const struct ieee80211_iface_combination hwsim_if_comb[] = {
 	{
 		.limits = hwsim_if_limits,
+		/* remove the last entry which is P2P_DEVICE */
+		.n_limits = ARRAY_SIZE(hwsim_if_limits) - 1,
+		.max_interfaces = 2048,
+		.num_different_channels = 1,
+	},
+	{
+		.limits = hwsim_if_dfs_limits,
+		.n_limits = ARRAY_SIZE(hwsim_if_dfs_limits),
+		.max_interfaces = 8,
+		.num_different_channels = 1,
+		.radar_detect_widths = BIT(NL80211_CHAN_WIDTH_20_NOHT) |
+				       BIT(NL80211_CHAN_WIDTH_20) |
+				       BIT(NL80211_CHAN_WIDTH_40) |
+				       BIT(NL80211_CHAN_WIDTH_80) |
+				       BIT(NL80211_CHAN_WIDTH_160),
+	}
+};
+
+static const struct ieee80211_iface_combination hwsim_if_comb_p2p_dev[] = {
+	{
+		.limits = hwsim_if_limits,
 		.n_limits = ARRAY_SIZE(hwsim_if_limits),
 		.max_interfaces = 2048,
 		.num_different_channels = 1,
@@ -385,6 +411,7 @@ struct mac80211_hwsim_data {
 
 	struct mac_address addresses[2];
 	int channels, idx;
+	bool use_chanctx;
 
 	struct ieee80211_channel *tmp_chan;
 	struct delayed_work roc_done;
@@ -451,7 +478,7 @@ static struct genl_family hwsim_genl_family = {
 
 /* MAC80211_HWSIM netlink policy */
 
-static struct nla_policy hwsim_genl_policy[HWSIM_ATTR_MAX + 1] = {
+static const struct nla_policy hwsim_genl_policy[HWSIM_ATTR_MAX + 1] = {
 	[HWSIM_ATTR_ADDR_RECEIVER] = { .type = NLA_UNSPEC, .len = ETH_ALEN },
 	[HWSIM_ATTR_ADDR_TRANSMITTER] = { .type = NLA_UNSPEC, .len = ETH_ALEN },
 	[HWSIM_ATTR_FRAME] = { .type = NLA_BINARY,
@@ -468,6 +495,7 @@ static struct nla_policy hwsim_genl_policy[HWSIM_ATTR_MAX + 1] = {
 	[HWSIM_ATTR_REG_HINT_ALPHA2] = { .type = NLA_STRING, .len = 2 },
 	[HWSIM_ATTR_REG_CUSTOM_REG] = { .type = NLA_U32 },
 	[HWSIM_ATTR_REG_STRICT_REG] = { .type = NLA_FLAG },
+	[HWSIM_ATTR_SUPPORT_P2P_DEVICE] = { .type = NLA_FLAG },
 };
 
 static void mac80211_hwsim_tx_frame(struct ieee80211_hw *hw,
@@ -1035,32 +1063,6 @@ static bool mac80211_hwsim_tx_frame_no_nl(struct ieee80211_hw *hw,
 			ack = true;
 
 		rx_status.mactime = now + data2->tsf_offset;
-#if 0
-		/*
-		 * Don't enable this code by default as the OUI 00:00:00
-		 * is registered to Xerox so we shouldn't use it here, it
-		 * might find its way into pcap files.
-		 * Note that this code requires the headroom in the SKB
-		 * that was allocated earlier.
-		 */
-		rx_status.vendor_radiotap_oui[0] = 0x00;
-		rx_status.vendor_radiotap_oui[1] = 0x00;
-		rx_status.vendor_radiotap_oui[2] = 0x00;
-		rx_status.vendor_radiotap_subns = 127;
-		/*
-		 * Radiotap vendor namespaces can (and should) also be
-		 * split into fields by using the standard radiotap
-		 * presence bitmap mechanism. Use just BIT(0) here for
-		 * the presence bitmap.
-		 */
-		rx_status.vendor_radiotap_bitmap = BIT(0);
-		/* We have 8 bytes of (dummy) data */
-		rx_status.vendor_radiotap_len = 8;
-		/* For testing, also require it to be aligned */
-		rx_status.vendor_radiotap_align = 8;
-		/* push the data */
-		memcpy(skb_push(nskb, 8), "ABCDEFGH", 8);
-#endif
 
 		memcpy(IEEE80211_SKB_RXCB(nskb), &rx_status, sizeof(rx_status));
 		ieee80211_rx_irqsafe(data2->hw, nskb);
@@ -1087,7 +1089,7 @@ static void mac80211_hwsim_tx(struct ieee80211_hw *hw,
 		return;
 	}
 
-	if (data->channels == 1) {
+	if (!data->use_chanctx) {
 		channel = data->channel;
 	} else if (txi->hw_queue == 4) {
 		channel = data->tmp_chan;
@@ -1275,6 +1277,9 @@ static void mac80211_hwsim_beacon_tx(void *arg, u8 *mac,
 
 	mac80211_hwsim_tx_frame(hw, skb,
 				rcu_dereference(vif->chanctx_conf)->def.chan);
+
+	if (vif->csa_active && ieee80211_csa_is_complete(vif))
+		ieee80211_csa_finish(vif);
 }
 
 static enum hrtimer_restart
@@ -1350,7 +1355,7 @@ static int mac80211_hwsim_config(struct ieee80211_hw *hw, u32 changed)
 
 	data->channel = conf->chandef.chan;
 
-	WARN_ON(data->channel && data->channels > 1);
+	WARN_ON(data->channel && data->use_chanctx);
 
 	data->power_level = conf->power_level;
 	if (!data->started || !data->beacon_int)
@@ -1936,7 +1941,8 @@ static struct ieee80211_ops mac80211_hwsim_mchan_ops;
 
 static int mac80211_hwsim_create_radio(int channels, const char *reg_alpha2,
 				       const struct ieee80211_regdomain *regd,
-				       bool reg_strict)
+				       bool reg_strict, bool p2p_device,
+				       bool use_chanctx)
 {
 	int err;
 	u8 addr[ETH_ALEN];
@@ -1946,11 +1952,14 @@ static int mac80211_hwsim_create_radio(int channels, const char *reg_alpha2,
 	const struct ieee80211_ops *ops = &mac80211_hwsim_ops;
 	int idx;
 
+	if (WARN_ON(channels > 1 && !use_chanctx))
+		return -EINVAL;
+
 	spin_lock_bh(&hwsim_radio_lock);
 	idx = hwsim_radio_idx++;
 	spin_unlock_bh(&hwsim_radio_lock);
 
-	if (channels > 1)
+	if (use_chanctx)
 		ops = &mac80211_hwsim_mchan_ops;
 	hw = ieee80211_alloc_hw(sizeof(*data), ops);
 	if (!hw) {
@@ -1991,17 +2000,25 @@ static int mac80211_hwsim_create_radio(int channels, const char *reg_alpha2,
 	hw->wiphy->addresses = data->addresses;
 
 	data->channels = channels;
+	data->use_chanctx = use_chanctx;
 	data->idx = idx;
 
-	if (data->channels > 1) {
+	if (data->use_chanctx) {
 		hw->wiphy->max_scan_ssids = 255;
 		hw->wiphy->max_scan_ie_len = IEEE80211_MAX_DATA_LEN;
 		hw->wiphy->max_remain_on_channel_duration = 1000;
 		/* For channels > 1 DFS is not allowed */
 		hw->wiphy->n_iface_combinations = 1;
 		hw->wiphy->iface_combinations = &data->if_combination;
-		data->if_combination = hwsim_if_comb[0];
+		if (p2p_device)
+			data->if_combination = hwsim_if_comb_p2p_dev[0];
+		else
+			data->if_combination = hwsim_if_comb[0];
 		data->if_combination.num_different_channels = data->channels;
+	} else if (p2p_device) {
+		hw->wiphy->iface_combinations = hwsim_if_comb_p2p_dev;
+		hw->wiphy->n_iface_combinations =
+			ARRAY_SIZE(hwsim_if_comb_p2p_dev);
 	} else {
 		hw->wiphy->iface_combinations = hwsim_if_comb;
 		hw->wiphy->n_iface_combinations = ARRAY_SIZE(hwsim_if_comb);
@@ -2017,8 +2034,10 @@ static int mac80211_hwsim_create_radio(int channels, const char *reg_alpha2,
 				     BIT(NL80211_IFTYPE_P2P_CLIENT) |
 				     BIT(NL80211_IFTYPE_P2P_GO) |
 				     BIT(NL80211_IFTYPE_ADHOC) |
-				     BIT(NL80211_IFTYPE_MESH_POINT) |
-				     BIT(NL80211_IFTYPE_P2P_DEVICE);
+				     BIT(NL80211_IFTYPE_MESH_POINT);
+
+	if (p2p_device)
+		hw->wiphy->interface_modes |= BIT(NL80211_IFTYPE_P2P_DEVICE);
 
 	hw->flags = IEEE80211_HW_MFP_CAPABLE |
 		    IEEE80211_HW_SIGNAL_DBM |
@@ -2027,13 +2046,15 @@ static int mac80211_hwsim_create_radio(int channels, const char *reg_alpha2,
 		    IEEE80211_HW_AMPDU_AGGREGATION |
 		    IEEE80211_HW_WANT_MONITOR_VIF |
 		    IEEE80211_HW_QUEUE_CONTROL |
-		    IEEE80211_HW_SUPPORTS_HT_CCK_RATES;
+		    IEEE80211_HW_SUPPORTS_HT_CCK_RATES |
+		    IEEE80211_HW_CHANCTX_STA_CSA;
 	if (rctbl)
 		hw->flags |= IEEE80211_HW_SUPPORTS_RC_TABLE;
 
 	hw->wiphy->flags |= WIPHY_FLAG_SUPPORTS_TDLS |
 			    WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL |
-			    WIPHY_FLAG_AP_UAPSD;
+			    WIPHY_FLAG_AP_UAPSD |
+			    WIPHY_FLAG_HAS_CHANNEL_SWITCH;
 	hw->wiphy->features |= NL80211_FEATURE_ACTIVE_MONITOR;
 
 	/* ask mac80211 to reserve space for magic */
@@ -2141,7 +2162,7 @@ static int mac80211_hwsim_create_radio(int channels, const char *reg_alpha2,
 	debugfs_create_file("ps", 0666, data->debugfs, data, &hwsim_fops_ps);
 	debugfs_create_file("group", 0666, data->debugfs, data,
 			    &hwsim_fops_group);
-	if (data->channels == 1)
+	if (!data->use_chanctx)
 		debugfs_create_file("dfs_simulate_radar", 0222,
 				    data->debugfs,
 				    data, &hwsim_simulate_radar);
@@ -2407,10 +2428,17 @@ static int hwsim_create_radio_nl(struct sk_buff *msg, struct genl_info *info)
 	const char *alpha2 = NULL;
 	const struct ieee80211_regdomain *regd = NULL;
 	bool reg_strict = info->attrs[HWSIM_ATTR_REG_STRICT_REG];
+	bool p2p_device = info->attrs[HWSIM_ATTR_SUPPORT_P2P_DEVICE];
+	bool use_chanctx;
 
 	if (info->attrs[HWSIM_ATTR_CHANNELS])
 		chans = nla_get_u32(info->attrs[HWSIM_ATTR_CHANNELS]);
 
+	if (info->attrs[HWSIM_ATTR_USE_CHANCTX])
+		use_chanctx = true;
+	else
+		use_chanctx = (chans > 1);
+
 	if (info->attrs[HWSIM_ATTR_REG_HINT_ALPHA2])
 		alpha2 = nla_data(info->attrs[HWSIM_ATTR_REG_HINT_ALPHA2]);
 
@@ -2422,7 +2450,8 @@ static int hwsim_create_radio_nl(struct sk_buff *msg, struct genl_info *info)
 		regd = hwsim_world_regdom_custom[idx];
 	}
 
-	return mac80211_hwsim_create_radio(chans, alpha2, regd, reg_strict);
+	return mac80211_hwsim_create_radio(chans, alpha2, regd, reg_strict,
+					   p2p_device, use_chanctx);
 }
 
 static int hwsim_destroy_radio_nl(struct sk_buff *msg, struct genl_info *info)
@@ -2640,7 +2669,9 @@ static int __init init_mac80211_hwsim(void)
 		}
 
 		err = mac80211_hwsim_create_radio(channels, reg_alpha2,
-						  regd, reg_strict);
+						  regd, reg_strict,
+						  support_p2p_device,
+						  channels > 1);
 		if (err < 0)
 			goto out_free_radios;
 	}
diff --git a/drivers/net/wireless/mac80211_hwsim.h b/drivers/net/wireless/mac80211_hwsim.h
index 2747cce5a269..c9d0315575ba 100644
--- a/drivers/net/wireless/mac80211_hwsim.h
+++ b/drivers/net/wireless/mac80211_hwsim.h
@@ -107,6 +107,10 @@ enum {
  *	(nla string, length 2)
  * @HWSIM_ATTR_REG_CUSTOM_REG: custom regulatory domain index (u32 attribute)
  * @HWSIM_ATTR_REG_STRICT_REG: request REGULATORY_STRICT_REG (flag attribute)
+ * @HWSIM_ATTR_SUPPORT_P2P_DEVICE: support P2P Device virtual interface (flag)
+ * @HWSIM_ATTR_USE_CHANCTX: used with the %HWSIM_CMD_CREATE_RADIO
+ *	command to force use of channel contexts even when only a
+ *	single channel is supported
  * @__HWSIM_ATTR_MAX: enum limit
  */
 
@@ -126,6 +130,8 @@ enum {
 	HWSIM_ATTR_REG_HINT_ALPHA2,
 	HWSIM_ATTR_REG_CUSTOM_REG,
 	HWSIM_ATTR_REG_STRICT_REG,
+	HWSIM_ATTR_SUPPORT_P2P_DEVICE,
+	HWSIM_ATTR_USE_CHANCTX,
 	__HWSIM_ATTR_MAX,
 };
 #define HWSIM_ATTR_MAX (__HWSIM_ATTR_MAX - 1)
diff --git a/drivers/net/wireless/mwifiex/11ac.c b/drivers/net/wireless/mwifiex/11ac.c
index 5e0eec4d71c7..bb43251c18f2 100644
--- a/drivers/net/wireless/mwifiex/11ac.c
+++ b/drivers/net/wireless/mwifiex/11ac.c
@@ -23,6 +23,31 @@
 #include "main.h"
 #include "11ac.h"
 
+/* Tables of the MCS map to the highest data rate (in Mbps) supported
+ * for long GI.
+ */
+static const u16 max_rate_lgi_80MHZ[8][3] = {
+	{0x124, 0x15F, 0x186},	/* NSS = 1 */
+	{0x249, 0x2BE, 0x30C},  /* NSS = 2 */
+	{0x36D, 0x41D, 0x492},  /* NSS = 3 */
+	{0x492, 0x57C, 0x618},  /* NSS = 4 */
+	{0x5B6, 0x6DB, 0x79E},  /* NSS = 5 */
+	{0x6DB, 0x83A, 0x0},    /* NSS = 6 */
+	{0x7FF, 0x999, 0xAAA},  /* NSS = 7 */
+	{0x924, 0xAF8, 0xC30}   /* NSS = 8 */
+};
+
+static const u16 max_rate_lgi_160MHZ[8][3] = {
+	{0x249, 0x2BE, 0x30C},   /* NSS = 1 */
+	{0x492, 0x57C, 0x618},   /* NSS = 2 */
+	{0x6DB, 0x83A, 0x0},     /* NSS = 3 */
+	{0x924, 0xAF8, 0xC30},   /* NSS = 4 */
+	{0xB6D, 0xDB6, 0xF3C},   /* NSS = 5 */
+	{0xDB6, 0x1074, 0x1248}, /* NSS = 6 */
+	{0xFFF, 0x1332, 0x1554}, /* NSS = 7 */
+	{0x1248, 0x15F0, 0x1860} /* NSS = 8 */
+};
+
 /* This function converts the 2-bit MCS map to the highest long GI
  * VHT data rate.
  */
@@ -30,33 +55,10 @@ static u16
 mwifiex_convert_mcsmap_to_maxrate(struct mwifiex_private *priv,
 				  u8 bands, u16 mcs_map)
 {
-	u8 i, nss, max_mcs;
+	u8 i, nss, mcs;
 	u16 max_rate = 0;
 	u32 usr_vht_cap_info = 0;
 	struct mwifiex_adapter *adapter = priv->adapter;
-	/* tables of the MCS map to the highest data rate (in Mbps)
-	 * supported for long GI
-	 */
-	u16 max_rate_lgi_80MHZ[8][3] = {
-		{0x124, 0x15F, 0x186},	/* NSS = 1 */
-		{0x249, 0x2BE, 0x30C},  /* NSS = 2 */
-		{0x36D, 0x41D, 0x492},  /* NSS = 3 */
-		{0x492, 0x57C, 0x618},  /* NSS = 4 */
-		{0x5B6, 0x6DB, 0x79E},  /* NSS = 5 */
-		{0x6DB, 0x83A, 0x0},    /* NSS = 6 */
-		{0x7FF, 0x999, 0xAAA},  /* NSS = 7 */
-		{0x924, 0xAF8, 0xC30}   /* NSS = 8 */
-	};
-	u16 max_rate_lgi_160MHZ[8][3] = {
-		{0x249, 0x2BE, 0x30C},   /* NSS = 1 */
-		{0x492, 0x57C, 0x618},   /* NSS = 2 */
-		{0x6DB, 0x83A, 0x0},     /* NSS = 3 */
-		{0x924, 0xAF8, 0xC30},   /* NSS = 4 */
-		{0xB6D, 0xDB6, 0xF3C},   /* NSS = 5 */
-		{0xDB6, 0x1074, 0x1248}, /* NSS = 6 */
-		{0xFFF, 0x1332, 0x1554}, /* NSS = 7 */
-		{0x1248, 0x15F0, 0x1860} /* NSS = 8 */
-	};
 
 	if (bands & BAND_AAC)
 		usr_vht_cap_info = adapter->usr_dot_11ac_dev_cap_a;
@@ -64,29 +66,29 @@ mwifiex_convert_mcsmap_to_maxrate(struct mwifiex_private *priv,
 		usr_vht_cap_info = adapter->usr_dot_11ac_dev_cap_bg;
 
 	/* find the max NSS supported */
-	nss = 0;
-	for (i = 0; i < 8; i++) {
-		max_mcs = (mcs_map >> (2 * i)) & 0x3;
-		if (max_mcs < 3)
+	nss = 1;
+	for (i = 1; i <= 8; i++) {
+		mcs = GET_VHTNSSMCS(mcs_map, i);
+		if (mcs < IEEE80211_VHT_MCS_NOT_SUPPORTED)
 			nss = i;
 	}
-	max_mcs = (mcs_map >> (2 * nss)) & 0x3;
+	mcs = GET_VHTNSSMCS(mcs_map, nss);
 
-	/* if max_mcs is 3, nss must be 0 (SS = 1). Thus, max mcs is MCS 9 */
-	if (max_mcs >= 3)
-		max_mcs = 2;
+	/* if mcs is 3, nss must be 1 (NSS = 1). Default mcs to MCS 0~9 */
+	if (mcs == IEEE80211_VHT_MCS_NOT_SUPPORTED)
+		mcs = IEEE80211_VHT_MCS_SUPPORT_0_9;
 
 	if (GET_VHTCAP_CHWDSET(usr_vht_cap_info)) {
 		/* support 160 MHz */
-		max_rate = max_rate_lgi_160MHZ[nss][max_mcs];
+		max_rate = max_rate_lgi_160MHZ[nss - 1][mcs];
 		if (!max_rate)
 			/* MCS9 is not supported in NSS6 */
-			max_rate = max_rate_lgi_160MHZ[nss][max_mcs - 1];
+			max_rate = max_rate_lgi_160MHZ[nss - 1][mcs - 1];
 	} else {
-		max_rate = max_rate_lgi_80MHZ[nss][max_mcs];
+		max_rate = max_rate_lgi_80MHZ[nss - 1][mcs];
 		if (!max_rate)
 			/* MCS9 is not supported in NSS3 */
-			max_rate = max_rate_lgi_80MHZ[nss][max_mcs - 1];
+			max_rate = max_rate_lgi_80MHZ[nss - 1][mcs - 1];
 	}
 
 	return max_rate;
@@ -94,21 +96,20 @@ mwifiex_convert_mcsmap_to_maxrate(struct mwifiex_private *priv,
 
 static void
 mwifiex_fill_vht_cap_info(struct mwifiex_private *priv,
-			  struct mwifiex_ie_types_vhtcap *vht_cap, u8 bands)
+			  struct ieee80211_vht_cap *vht_cap, u8 bands)
 {
 	struct mwifiex_adapter *adapter = priv->adapter;
 
 	if (bands & BAND_A)
-		vht_cap->vht_cap.vht_cap_info =
+		vht_cap->vht_cap_info =
 				cpu_to_le32(adapter->usr_dot_11ac_dev_cap_a);
 	else
-		vht_cap->vht_cap.vht_cap_info =
+		vht_cap->vht_cap_info =
 				cpu_to_le32(adapter->usr_dot_11ac_dev_cap_bg);
 }
 
-static void
-mwifiex_fill_vht_cap_tlv(struct mwifiex_private *priv,
-			 struct mwifiex_ie_types_vhtcap *vht_cap, u8 bands)
+void mwifiex_fill_vht_cap_tlv(struct mwifiex_private *priv,
+			      struct ieee80211_vht_cap *vht_cap, u8 bands)
 {
 	struct mwifiex_adapter *adapter = priv->adapter;
 	u16 mcs_map_user, mcs_map_resp, mcs_map_result;
@@ -119,46 +120,48 @@ mwifiex_fill_vht_cap_tlv(struct mwifiex_private *priv,
 
 	/* rx MCS Set: find the minimum of the user rx mcs and ap rx mcs */
 	mcs_map_user = GET_DEVRXMCSMAP(adapter->usr_dot_11ac_mcs_support);
-	mcs_map_resp = le16_to_cpu(vht_cap->vht_cap.supp_mcs.rx_mcs_map);
+	mcs_map_resp = le16_to_cpu(vht_cap->supp_mcs.rx_mcs_map);
 	mcs_map_result = 0;
 
 	for (nss = 1; nss <= 8; nss++) {
 		mcs_user = GET_VHTNSSMCS(mcs_map_user, nss);
 		mcs_resp = GET_VHTNSSMCS(mcs_map_resp, nss);
 
-		if ((mcs_user == NO_NSS_SUPPORT) ||
-		    (mcs_resp == NO_NSS_SUPPORT))
-			SET_VHTNSSMCS(mcs_map_result, nss, NO_NSS_SUPPORT);
+		if ((mcs_user == IEEE80211_VHT_MCS_NOT_SUPPORTED) ||
+		    (mcs_resp == IEEE80211_VHT_MCS_NOT_SUPPORTED))
+			SET_VHTNSSMCS(mcs_map_result, nss,
+				      IEEE80211_VHT_MCS_NOT_SUPPORTED);
 		else
 			SET_VHTNSSMCS(mcs_map_result, nss,
 				      min(mcs_user, mcs_resp));
 	}
 
-	vht_cap->vht_cap.supp_mcs.rx_mcs_map = cpu_to_le16(mcs_map_result);
+	vht_cap->supp_mcs.rx_mcs_map = cpu_to_le16(mcs_map_result);
 
 	tmp = mwifiex_convert_mcsmap_to_maxrate(priv, bands, mcs_map_result);
-	vht_cap->vht_cap.supp_mcs.rx_highest = cpu_to_le16(tmp);
+	vht_cap->supp_mcs.rx_highest = cpu_to_le16(tmp);
 
 	/* tx MCS Set: find the minimum of the user tx mcs and ap tx mcs */
 	mcs_map_user = GET_DEVTXMCSMAP(adapter->usr_dot_11ac_mcs_support);
-	mcs_map_resp = le16_to_cpu(vht_cap->vht_cap.supp_mcs.tx_mcs_map);
+	mcs_map_resp = le16_to_cpu(vht_cap->supp_mcs.tx_mcs_map);
 	mcs_map_result = 0;
 
 	for (nss = 1; nss <= 8; nss++) {
 		mcs_user = GET_VHTNSSMCS(mcs_map_user, nss);
 		mcs_resp = GET_VHTNSSMCS(mcs_map_resp, nss);
-		if ((mcs_user == NO_NSS_SUPPORT) ||
-		    (mcs_resp == NO_NSS_SUPPORT))
-			SET_VHTNSSMCS(mcs_map_result, nss, NO_NSS_SUPPORT);
+		if ((mcs_user == IEEE80211_VHT_MCS_NOT_SUPPORTED) ||
+		    (mcs_resp == IEEE80211_VHT_MCS_NOT_SUPPORTED))
+			SET_VHTNSSMCS(mcs_map_result, nss,
+				      IEEE80211_VHT_MCS_NOT_SUPPORTED);
 		else
 			SET_VHTNSSMCS(mcs_map_result, nss,
 				      min(mcs_user, mcs_resp));
 	}
 
-	vht_cap->vht_cap.supp_mcs.tx_mcs_map = cpu_to_le16(mcs_map_result);
+	vht_cap->supp_mcs.tx_mcs_map = cpu_to_le16(mcs_map_result);
 
 	tmp = mwifiex_convert_mcsmap_to_maxrate(priv, bands, mcs_map_result);
-	vht_cap->vht_cap.supp_mcs.tx_highest = cpu_to_le16(tmp);
+	vht_cap->supp_mcs.tx_highest = cpu_to_le16(tmp);
 
 	return;
 }
@@ -193,7 +196,8 @@ int mwifiex_cmd_append_11ac_tlv(struct mwifiex_private *priv,
 		       sizeof(struct ieee_types_header),
 		       le16_to_cpu(vht_cap->header.len));
 
-		mwifiex_fill_vht_cap_tlv(priv, vht_cap, bss_desc->bss_band);
+		mwifiex_fill_vht_cap_tlv(priv, &vht_cap->vht_cap,
+					 bss_desc->bss_band);
 		*buffer += sizeof(*vht_cap);
 		ret_len += sizeof(*vht_cap);
 	}
@@ -300,3 +304,81 @@ void mwifiex_set_11ac_ba_params(struct mwifiex_private *priv)
 
 	return;
 }
+
+bool mwifiex_is_bss_in_11ac_mode(struct mwifiex_private *priv)
+{
+	struct mwifiex_bssdescriptor *bss_desc;
+	struct ieee80211_vht_operation *vht_oper;
+
+	bss_desc = &priv->curr_bss_params.bss_descriptor;
+	vht_oper = bss_desc->bcn_vht_oper;
+
+	if (!bss_desc->bcn_vht_cap || !vht_oper)
+		return false;
+
+	if (vht_oper->chan_width == IEEE80211_VHT_CHANWIDTH_USE_HT)
+		return false;
+
+	return true;
+}
+
+u8 mwifiex_get_center_freq_index(struct mwifiex_private *priv, u8 band,
+				 u32 pri_chan, u8 chan_bw)
+{
+	u8 center_freq_idx = 0;
+
+	if (band & BAND_AAC) {
+		switch (pri_chan) {
+		case 36:
+		case 40:
+		case 44:
+		case 48:
+			if (chan_bw == IEEE80211_VHT_CHANWIDTH_80MHZ)
+				center_freq_idx = 42;
+			break;
+		case 52:
+		case 56:
+		case 60:
+		case 64:
+			if (chan_bw == IEEE80211_VHT_CHANWIDTH_80MHZ)
+				center_freq_idx = 58;
+			else if (chan_bw == IEEE80211_VHT_CHANWIDTH_160MHZ)
+				center_freq_idx = 50;
+			break;
+		case 100:
+		case 104:
+		case 108:
+		case 112:
+			if (chan_bw == IEEE80211_VHT_CHANWIDTH_80MHZ)
+				center_freq_idx = 106;
+			break;
+		case 116:
+		case 120:
+		case 124:
+		case 128:
+			if (chan_bw == IEEE80211_VHT_CHANWIDTH_80MHZ)
+				center_freq_idx = 122;
+			else if (chan_bw == IEEE80211_VHT_CHANWIDTH_160MHZ)
+				center_freq_idx = 114;
+			break;
+		case 132:
+		case 136:
+		case 140:
+		case 144:
+			if (chan_bw == IEEE80211_VHT_CHANWIDTH_80MHZ)
+				center_freq_idx = 138;
+			break;
+		case 149:
+		case 153:
+		case 157:
+		case 161:
+			if (chan_bw == IEEE80211_VHT_CHANWIDTH_80MHZ)
+				center_freq_idx = 155;
+			break;
+		default:
+			center_freq_idx = 42;
+		}
+	}
+
+	return center_freq_idx;
+}
diff --git a/drivers/net/wireless/mwifiex/11ac.h b/drivers/net/wireless/mwifiex/11ac.h
index 7c2c69b5b3eb..0b02cb6cfcb4 100644
--- a/drivers/net/wireless/mwifiex/11ac.h
+++ b/drivers/net/wireless/mwifiex/11ac.h
@@ -40,4 +40,6 @@ int mwifiex_cmd_append_11ac_tlv(struct mwifiex_private *priv,
 int mwifiex_cmd_11ac_cfg(struct mwifiex_private *priv,
 			 struct host_cmd_ds_command *cmd, u16 cmd_action,
 			 struct mwifiex_11ac_vht_cfg *cfg);
+void mwifiex_fill_vht_cap_tlv(struct mwifiex_private *priv,
+			      struct ieee80211_vht_cap *vht_cap, u8 bands);
 #endif /* _MWIFIEX_11AC_H_ */
diff --git a/drivers/net/wireless/mwifiex/11h.c b/drivers/net/wireless/mwifiex/11h.c
index 8d683070bdb3..e76b0db4e3e6 100644
--- a/drivers/net/wireless/mwifiex/11h.c
+++ b/drivers/net/wireless/mwifiex/11h.c
@@ -73,8 +73,8 @@ static int mwifiex_11h_activate(struct mwifiex_private *priv, bool flag)
 {
 	u32 enable = flag;
 
-	return mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_SNMP_MIB,
-				     HostCmd_ACT_GEN_SET, DOT11H_I, &enable);
+	return mwifiex_send_cmd(priv, HostCmd_CMD_802_11_SNMP_MIB,
+				HostCmd_ACT_GEN_SET, DOT11H_I, &enable, true);
 }
 
 /* This functions processes TLV buffer for a pending BSS Join command.
diff --git a/drivers/net/wireless/mwifiex/11n.c b/drivers/net/wireless/mwifiex/11n.c
index 6261f8c53d44..79ead928a64e 100644
--- a/drivers/net/wireless/mwifiex/11n.c
+++ b/drivers/net/wireless/mwifiex/11n.c
@@ -34,22 +34,26 @@
  *
  * RD responder bit to set to clear in the extended capability header.
  */
-void
-mwifiex_fill_cap_info(struct mwifiex_private *priv, u8 radio_type,
-		      struct mwifiex_ie_types_htcap *ht_cap)
+int mwifiex_fill_cap_info(struct mwifiex_private *priv, u8 radio_type,
+			  struct ieee80211_ht_cap *ht_cap)
 {
-	uint16_t ht_ext_cap = le16_to_cpu(ht_cap->ht_cap.extended_ht_cap_info);
+	uint16_t ht_ext_cap = le16_to_cpu(ht_cap->extended_ht_cap_info);
 	struct ieee80211_supported_band *sband =
 					priv->wdev->wiphy->bands[radio_type];
 
-	ht_cap->ht_cap.ampdu_params_info =
+	if (WARN_ON_ONCE(!sband)) {
+		dev_err(priv->adapter->dev, "Invalid radio type!\n");
+		return -EINVAL;
+	}
+
+	ht_cap->ampdu_params_info =
 		(sband->ht_cap.ampdu_factor &
 		 IEEE80211_HT_AMPDU_PARM_FACTOR) |
 		((sband->ht_cap.ampdu_density <<
 		 IEEE80211_HT_AMPDU_PARM_DENSITY_SHIFT) &
 		 IEEE80211_HT_AMPDU_PARM_DENSITY);
 
-	memcpy((u8 *) &ht_cap->ht_cap.mcs, &sband->ht_cap.mcs,
+	memcpy((u8 *)&ht_cap->mcs, &sband->ht_cap.mcs,
 	       sizeof(sband->ht_cap.mcs));
 
 	if (priv->bss_mode == NL80211_IFTYPE_STATION ||
@@ -57,13 +61,18 @@ mwifiex_fill_cap_info(struct mwifiex_private *priv, u8 radio_type,
 	     (priv->adapter->sec_chan_offset !=
 					IEEE80211_HT_PARAM_CHA_SEC_NONE)))
 		/* Set MCS32 for infra mode or ad-hoc mode with 40MHz support */
-		SETHT_MCS32(ht_cap->ht_cap.mcs.rx_mask);
+		SETHT_MCS32(ht_cap->mcs.rx_mask);
 
 	/* Clear RD responder bit */
 	ht_ext_cap &= ~IEEE80211_HT_EXT_CAP_RD_RESPONDER;
 
-	ht_cap->ht_cap.cap_info = cpu_to_le16(sband->ht_cap.cap);
-	ht_cap->ht_cap.extended_ht_cap_info = cpu_to_le16(ht_ext_cap);
+	ht_cap->cap_info = cpu_to_le16(sband->ht_cap.cap);
+	ht_cap->extended_ht_cap_info = cpu_to_le16(ht_ext_cap);
+
+	if (ISSUPP_BEAMFORMING(priv->adapter->hw_dot_11n_dev_cap))
+		ht_cap->tx_BF_cap_info = cpu_to_le32(MWIFIEX_DEF_11N_TX_BF_CAP);
+
+	return 0;
 }
 
 /*
@@ -312,7 +321,7 @@ mwifiex_cmd_append_11n_tlv(struct mwifiex_private *priv,
 		       sizeof(struct ieee_types_header),
 		       le16_to_cpu(ht_cap->header.len));
 
-		mwifiex_fill_cap_info(priv, radio_type, ht_cap);
+		mwifiex_fill_cap_info(priv, radio_type, &ht_cap->ht_cap);
 
 		*buffer += sizeof(struct mwifiex_ie_types_htcap);
 		ret_len += sizeof(struct mwifiex_ie_types_htcap);
@@ -528,16 +537,32 @@ void mwifiex_create_ba_tbl(struct mwifiex_private *priv, u8 *ra, int tid,
 int mwifiex_send_addba(struct mwifiex_private *priv, int tid, u8 *peer_mac)
 {
 	struct host_cmd_ds_11n_addba_req add_ba_req;
+	struct mwifiex_sta_node *sta_ptr;
+	u32 tx_win_size = priv->add_ba_param.tx_win_size;
 	static u8 dialog_tok;
 	int ret;
 
 	dev_dbg(priv->adapter->dev, "cmd: %s: tid %d\n", __func__, tid);
 
+	if ((GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_STA) &&
+	    ISSUPP_TDLS_ENABLED(priv->adapter->fw_cap_info) &&
+	    priv->adapter->is_hw_11ac_capable &&
+	    memcmp(priv->cfg_bssid, peer_mac, ETH_ALEN)) {
+		sta_ptr = mwifiex_get_sta_entry(priv, peer_mac);
+		if (!sta_ptr) {
+			dev_warn(priv->adapter->dev,
+				 "BA setup with unknown TDLS peer %pM!\n",
+				peer_mac);
+			return -1;
+		}
+		if (sta_ptr->is_11ac_enabled)
+			tx_win_size = MWIFIEX_11AC_STA_AMPDU_DEF_TXWINSIZE;
+	}
+
 	add_ba_req.block_ack_param_set = cpu_to_le16(
 		(u16) ((tid << BLOCKACKPARAM_TID_POS) |
-			 (priv->add_ba_param.
-			  tx_win_size << BLOCKACKPARAM_WINSIZE_POS) |
-			 IMMEDIATE_BLOCK_ACK));
+		       tx_win_size << BLOCKACKPARAM_WINSIZE_POS |
+		       IMMEDIATE_BLOCK_ACK));
 	add_ba_req.block_ack_tmo = cpu_to_le16((u16)priv->add_ba_param.timeout);
 
 	++dialog_tok;
@@ -549,8 +574,8 @@ int mwifiex_send_addba(struct mwifiex_private *priv, int tid, u8 *peer_mac)
 	memcpy(&add_ba_req.peer_mac_addr, peer_mac, ETH_ALEN);
 
 	/* We don't wait for the response of this command */
-	ret = mwifiex_send_cmd_async(priv, HostCmd_CMD_11N_ADDBA_REQ,
-				     0, 0, &add_ba_req);
+	ret = mwifiex_send_cmd(priv, HostCmd_CMD_11N_ADDBA_REQ,
+			       0, 0, &add_ba_req, false);
 
 	return ret;
 }
@@ -577,8 +602,8 @@ int mwifiex_send_delba(struct mwifiex_private *priv, int tid, u8 *peer_mac,
 	memcpy(&delba.peer_mac_addr, peer_mac, ETH_ALEN);
 
 	/* We don't wait for the response of this command */
-	ret = mwifiex_send_cmd_async(priv, HostCmd_CMD_11N_DELBA,
-				     HostCmd_ACT_GEN_SET, 0, &delba);
+	ret = mwifiex_send_cmd(priv, HostCmd_CMD_11N_DELBA,
+			       HostCmd_ACT_GEN_SET, 0, &delba, false);
 
 	return ret;
 }
diff --git a/drivers/net/wireless/mwifiex/11n.h b/drivers/net/wireless/mwifiex/11n.h
index 375db01442bf..12bb6acbdd58 100644
--- a/drivers/net/wireless/mwifiex/11n.h
+++ b/drivers/net/wireless/mwifiex/11n.h
@@ -34,8 +34,8 @@ int mwifiex_cmd_11n_cfg(struct mwifiex_private *priv,
 int mwifiex_cmd_append_11n_tlv(struct mwifiex_private *priv,
 			       struct mwifiex_bssdescriptor *bss_desc,
 			       u8 **buffer);
-void mwifiex_fill_cap_info(struct mwifiex_private *, u8 radio_type,
-			   struct mwifiex_ie_types_htcap *);
+int mwifiex_fill_cap_info(struct mwifiex_private *, u8 radio_type,
+			  struct ieee80211_ht_cap *);
 int mwifiex_set_get_11n_htcap_cfg(struct mwifiex_private *priv,
 				  u16 action, int *htcap_cfg);
 void mwifiex_11n_delete_tx_ba_stream_tbl_entry(struct mwifiex_private *priv,
@@ -64,14 +64,32 @@ int mwifiex_cmd_amsdu_aggr_ctrl(struct host_cmd_ds_command *cmd,
 				struct mwifiex_ds_11n_amsdu_aggr_ctrl *aa_ctrl);
 void mwifiex_del_tx_ba_stream_tbl_by_ra(struct mwifiex_private *priv, u8 *ra);
 
-/*
- * This function checks whether AMPDU is allowed or not for a particular TID.
- */
 static inline u8
-mwifiex_is_ampdu_allowed(struct mwifiex_private *priv, int tid)
+mwifiex_is_station_ampdu_allowed(struct mwifiex_private *priv,
+				 struct mwifiex_ra_list_tbl *ptr, int tid)
 {
-	return ((priv->aggr_prio_tbl[tid].ampdu_ap != BA_STREAM_NOT_ALLOWED)
-		? true : false);
+	struct mwifiex_sta_node *node = mwifiex_get_sta_entry(priv, ptr->ra);
+
+	if (unlikely(!node))
+		return false;
+
+	return (node->ampdu_sta[tid] != BA_STREAM_NOT_ALLOWED) ? true : false;
+}
+
+/* This function checks whether AMPDU is allowed or not for a particular TID. */
+static inline u8
+mwifiex_is_ampdu_allowed(struct mwifiex_private *priv,
+			 struct mwifiex_ra_list_tbl *ptr, int tid)
+{
+	if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_UAP) {
+		return mwifiex_is_station_ampdu_allowed(priv, ptr, tid);
+	} else {
+		if (ptr->tdls_link)
+			return mwifiex_is_station_ampdu_allowed(priv, ptr, tid);
+
+		return (priv->aggr_prio_tbl[tid].ampdu_ap !=
+			BA_STREAM_NOT_ALLOWED) ? true : false;
+	}
 }
 
 /*
@@ -165,4 +183,14 @@ static inline int mwifiex_is_sta_11n_enabled(struct mwifiex_private *priv,
 
 	return node->is_11n_enabled;
 }
+
+static inline u8
+mwifiex_tdls_peer_11n_enabled(struct mwifiex_private *priv, u8 *ra)
+{
+	struct mwifiex_sta_node *node = mwifiex_get_sta_entry(priv, ra);
+	if (node)
+		return node->is_11n_enabled;
+
+	return false;
+}
 #endif /* !_MWIFIEX_11N_H_ */
diff --git a/drivers/net/wireless/mwifiex/11n_rxreorder.c b/drivers/net/wireless/mwifiex/11n_rxreorder.c
index ada809f576fe..c3323c492614 100644
--- a/drivers/net/wireless/mwifiex/11n_rxreorder.c
+++ b/drivers/net/wireless/mwifiex/11n_rxreorder.c
@@ -142,7 +142,7 @@ mwifiex_del_rx_reorder_entry(struct mwifiex_private *priv,
 	mwifiex_11n_dispatch_pkt(priv, tbl, (tbl->start_win + tbl->win_size) &
 					    (MAX_TID_VALUE - 1));
 
-	del_timer(&tbl->timer_context.timer);
+	del_timer_sync(&tbl->timer_context.timer);
 
 	spin_lock_irqsave(&priv->rx_reorder_tbl_lock, flags);
 	list_del(&tbl->list);
@@ -279,6 +279,8 @@ mwifiex_11n_create_rx_reorder_tbl(struct mwifiex_private *priv, u8 *ta,
 	new_node->tid = tid;
 	memcpy(new_node->ta, ta, ETH_ALEN);
 	new_node->start_win = seq_num;
+	new_node->init_win = seq_num;
+	new_node->flags = 0;
 
 	if (mwifiex_queuing_ra_based(priv)) {
 		dev_dbg(priv->adapter->dev,
@@ -290,15 +292,20 @@ mwifiex_11n_create_rx_reorder_tbl(struct mwifiex_private *priv, u8 *ta,
 				last_seq = node->rx_seq[tid];
 		}
 	} else {
-		last_seq = priv->rx_seq[tid];
+		node = mwifiex_get_sta_entry(priv, ta);
+		if (node)
+			last_seq = node->rx_seq[tid];
+		else
+			last_seq = priv->rx_seq[tid];
 	}
 
 	if (last_seq != MWIFIEX_DEF_11N_RX_SEQ_NUM &&
-	    last_seq >= new_node->start_win)
+	    last_seq >= new_node->start_win) {
 		new_node->start_win = last_seq + 1;
+		new_node->flags |= RXREOR_INIT_WINDOW_SHIFT;
+	}
 
 	new_node->win_size = win_size;
-	new_node->flags = 0;
 
 	new_node->rx_reorder_ptr = kzalloc(sizeof(void *) * win_size,
 					GFP_KERNEL);
@@ -358,10 +365,28 @@ int mwifiex_cmd_11n_addba_rsp_gen(struct mwifiex_private *priv,
 				  *cmd_addba_req)
 {
 	struct host_cmd_ds_11n_addba_rsp *add_ba_rsp = &cmd->params.add_ba_rsp;
+	struct mwifiex_sta_node *sta_ptr;
+	u32 rx_win_size = priv->add_ba_param.rx_win_size;
 	u8 tid;
 	int win_size;
 	uint16_t block_ack_param_set;
 
+	if ((GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_STA) &&
+	    ISSUPP_TDLS_ENABLED(priv->adapter->fw_cap_info) &&
+	    priv->adapter->is_hw_11ac_capable &&
+	    memcmp(priv->cfg_bssid, cmd_addba_req->peer_mac_addr, ETH_ALEN)) {
+		sta_ptr = mwifiex_get_sta_entry(priv,
+						cmd_addba_req->peer_mac_addr);
+		if (!sta_ptr) {
+			dev_warn(priv->adapter->dev,
+				 "BA setup with unknown TDLS peer %pM!\n",
+				 cmd_addba_req->peer_mac_addr);
+			return -1;
+		}
+		if (sta_ptr->is_11ac_enabled)
+			rx_win_size = MWIFIEX_11AC_STA_AMPDU_DEF_RXWINSIZE;
+	}
+
 	cmd->command = cpu_to_le16(HostCmd_CMD_11N_ADDBA_RSP);
 	cmd->size = cpu_to_le16(sizeof(*add_ba_rsp) + S_DS_GEN);
 
@@ -378,8 +403,7 @@ int mwifiex_cmd_11n_addba_rsp_gen(struct mwifiex_private *priv,
 	block_ack_param_set &= ~IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
 	/* We donot support AMSDU inside AMPDU, hence reset the bit */
 	block_ack_param_set &= ~BLOCKACKPARAM_AMSDU_SUPP_MASK;
-	block_ack_param_set |= (priv->add_ba_param.rx_win_size <<
-					     BLOCKACKPARAM_WINSIZE_POS);
+	block_ack_param_set |= rx_win_size << BLOCKACKPARAM_WINSIZE_POS;
 	add_ba_rsp->block_ack_param_set = cpu_to_le16(block_ack_param_set);
 	win_size = (le16_to_cpu(add_ba_rsp->block_ack_param_set)
 					& IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK)
@@ -431,6 +455,7 @@ int mwifiex_11n_rx_reorder_pkt(struct mwifiex_private *priv,
 	struct mwifiex_rx_reorder_tbl *tbl;
 	int start_win, end_win, win_size;
 	u16 pkt_index;
+	bool init_window_shift = false;
 
 	tbl = mwifiex_11n_get_rx_reorder_tbl(priv, tid, ta);
 	if (!tbl) {
@@ -445,19 +470,29 @@ int mwifiex_11n_rx_reorder_pkt(struct mwifiex_private *priv,
 	start_win = tbl->start_win;
 	win_size = tbl->win_size;
 	end_win = ((start_win + win_size) - 1) & (MAX_TID_VALUE - 1);
-	del_timer(&tbl->timer_context.timer);
+	if (tbl->flags & RXREOR_INIT_WINDOW_SHIFT) {
+		init_window_shift = true;
+		tbl->flags &= ~RXREOR_INIT_WINDOW_SHIFT;
+	}
 	mod_timer(&tbl->timer_context.timer,
 		  jiffies + msecs_to_jiffies(MIN_FLUSH_TIMER_MS * win_size));
 
-	/*
-	 * If seq_num is less then starting win then ignore and drop the
-	 * packet
-	 */
 	if (tbl->flags & RXREOR_FORCE_NO_DROP) {
 		dev_dbg(priv->adapter->dev,
 			"RXREOR_FORCE_NO_DROP when HS is activated\n");
 		tbl->flags &= ~RXREOR_FORCE_NO_DROP;
+	} else if (init_window_shift && seq_num < start_win &&
+		   seq_num >= tbl->init_win) {
+		dev_dbg(priv->adapter->dev,
+			"Sender TID sequence number reset %d->%d for SSN %d\n",
+			start_win, seq_num, tbl->init_win);
+		tbl->start_win = start_win = seq_num;
+		end_win = ((start_win + win_size) - 1) & (MAX_TID_VALUE - 1);
 	} else {
+		/*
+		 * If seq_num is less then starting win then ignore and drop
+		 * the packet
+		 */
 		if ((start_win + TWOPOW11) > (MAX_TID_VALUE - 1)) {
 			if (seq_num >= ((start_win + TWOPOW11) &
 					(MAX_TID_VALUE - 1)) &&
@@ -615,7 +650,7 @@ void mwifiex_11n_ba_stream_timeout(struct mwifiex_private *priv,
 	delba.del_ba_param_set |= cpu_to_le16(
 		(u16) event->origninator << DELBA_INITIATOR_POS);
 	delba.reason_code = cpu_to_le16(WLAN_REASON_QSTA_TIMEOUT);
-	mwifiex_send_cmd_async(priv, HostCmd_CMD_11N_DELBA, 0, 0, &delba);
+	mwifiex_send_cmd(priv, HostCmd_CMD_11N_DELBA, 0, 0, &delba, false);
 }
 
 /*
diff --git a/drivers/net/wireless/mwifiex/11n_rxreorder.h b/drivers/net/wireless/mwifiex/11n_rxreorder.h
index 4064041ac852..0fc76e4a60f8 100644
--- a/drivers/net/wireless/mwifiex/11n_rxreorder.h
+++ b/drivers/net/wireless/mwifiex/11n_rxreorder.h
@@ -42,7 +42,8 @@
 #define BA_SETUP_PACKET_OFFSET		16
 
 enum mwifiex_rxreor_flags {
-	RXREOR_FORCE_NO_DROP	= 1<<0,
+	RXREOR_FORCE_NO_DROP		= 1<<0,
+	RXREOR_INIT_WINDOW_SHIFT	= 1<<1,
 };
 
 static inline void mwifiex_reset_11n_rx_seq_num(struct mwifiex_private *priv)
diff --git a/drivers/net/wireless/mwifiex/Makefile b/drivers/net/wireless/mwifiex/Makefile
index a42a506fd32b..2aa208ffbe23 100644
--- a/drivers/net/wireless/mwifiex/Makefile
+++ b/drivers/net/wireless/mwifiex/Makefile
@@ -41,6 +41,7 @@ mwifiex-y += uap_txrx.o
 mwifiex-y += cfg80211.o
 mwifiex-y += ethtool.o
 mwifiex-y += 11h.o
+mwifiex-y += tdls.o
 mwifiex-$(CONFIG_DEBUG_FS) += debugfs.o
 obj-$(CONFIG_MWIFIEX) += mwifiex.o
 
diff --git a/drivers/net/wireless/mwifiex/README b/drivers/net/wireless/mwifiex/README
index 3d64613ebb29..b9242c3dca43 100644
--- a/drivers/net/wireless/mwifiex/README
+++ b/drivers/net/wireless/mwifiex/README
@@ -131,7 +131,7 @@ info
 	hs_configured = <0/1, host sleep not configured/configured>
 	hs_activated = <0/1, extended host sleep not activated/activated>
 	num_tx_timeout = <number of Tx timeout>
-	num_cmd_timeout = <number of timeout commands>
+	is_cmd_timedout = <0/1 command timeout not occurred/occurred>
 	timeout_cmd_id = <command id of the last timeout command>
 	timeout_cmd_act = <command action of the last timeout command>
 	last_cmd_id = <command id of the last several commands sent to device>
diff --git a/drivers/net/wireless/mwifiex/cfg80211.c b/drivers/net/wireless/mwifiex/cfg80211.c
index 8bfc07cd330e..51ce99cfcfb9 100644
--- a/drivers/net/wireless/mwifiex/cfg80211.c
+++ b/drivers/net/wireless/mwifiex/cfg80211.c
@@ -252,9 +252,9 @@ mwifiex_cfg80211_mgmt_frame_register(struct wiphy *wiphy,
 
 	if (mask != priv->mgmt_frame_mask) {
 		priv->mgmt_frame_mask = mask;
-		mwifiex_send_cmd_async(priv, HostCmd_CMD_MGMT_FRAME_REG,
-				       HostCmd_ACT_GEN_SET, 0,
-				       &priv->mgmt_frame_mask);
+		mwifiex_send_cmd(priv, HostCmd_CMD_MGMT_FRAME_REG,
+				 HostCmd_ACT_GEN_SET, 0,
+				 &priv->mgmt_frame_mask, false);
 		wiphy_dbg(wiphy, "info: mgmt frame registered\n");
 	}
 }
@@ -515,8 +515,8 @@ static int mwifiex_send_domain_info_cmd_fw(struct wiphy *wiphy)
 
 	priv = mwifiex_get_priv(adapter, MWIFIEX_BSS_ROLE_ANY);
 
-	if (mwifiex_send_cmd_async(priv, HostCmd_CMD_802_11D_DOMAIN_INFO,
-				   HostCmd_ACT_GEN_SET, 0, NULL)) {
+	if (mwifiex_send_cmd(priv, HostCmd_CMD_802_11D_DOMAIN_INFO,
+			     HostCmd_ACT_GEN_SET, 0, NULL, false)) {
 		wiphy_err(wiphy, "11D: setting domain info in FW\n");
 		return -1;
 	}
@@ -580,9 +580,9 @@ mwifiex_set_frag(struct mwifiex_private *priv, u32 frag_thr)
 	    frag_thr > MWIFIEX_FRAG_MAX_VALUE)
 		frag_thr = MWIFIEX_FRAG_MAX_VALUE;
 
-	return mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_SNMP_MIB,
-				     HostCmd_ACT_GEN_SET, FRAG_THRESH_I,
-				     &frag_thr);
+	return mwifiex_send_cmd(priv, HostCmd_CMD_802_11_SNMP_MIB,
+				HostCmd_ACT_GEN_SET, FRAG_THRESH_I,
+				&frag_thr, true);
 }
 
 /*
@@ -597,9 +597,9 @@ mwifiex_set_rts(struct mwifiex_private *priv, u32 rts_thr)
 	if (rts_thr < MWIFIEX_RTS_MIN_VALUE || rts_thr > MWIFIEX_RTS_MAX_VALUE)
 		rts_thr = MWIFIEX_RTS_MAX_VALUE;
 
-	return mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_SNMP_MIB,
-				    HostCmd_ACT_GEN_SET, RTS_THRESH_I,
-				    &rts_thr);
+	return mwifiex_send_cmd(priv, HostCmd_CMD_802_11_SNMP_MIB,
+				HostCmd_ACT_GEN_SET, RTS_THRESH_I,
+				&rts_thr, true);
 }
 
 /*
@@ -637,20 +637,19 @@ mwifiex_cfg80211_set_wiphy_params(struct wiphy *wiphy, u32 changed)
 
 			bss_started = priv->bss_started;
 
-			ret = mwifiex_send_cmd_sync(priv,
-						    HostCmd_CMD_UAP_BSS_STOP,
-						    HostCmd_ACT_GEN_SET, 0,
-						    NULL);
+			ret = mwifiex_send_cmd(priv, HostCmd_CMD_UAP_BSS_STOP,
+					       HostCmd_ACT_GEN_SET, 0,
+					       NULL, true);
 			if (ret) {
 				wiphy_err(wiphy, "Failed to stop the BSS\n");
 				kfree(bss_cfg);
 				return ret;
 			}
 
-			ret = mwifiex_send_cmd_async(priv,
-						     HostCmd_CMD_UAP_SYS_CONFIG,
-						     HostCmd_ACT_GEN_SET,
-						     UAP_BSS_PARAMS_I, bss_cfg);
+			ret = mwifiex_send_cmd(priv, HostCmd_CMD_UAP_SYS_CONFIG,
+					       HostCmd_ACT_GEN_SET,
+					       UAP_BSS_PARAMS_I, bss_cfg,
+					       false);
 
 			kfree(bss_cfg);
 
@@ -662,10 +661,9 @@ mwifiex_cfg80211_set_wiphy_params(struct wiphy *wiphy, u32 changed)
 			if (!bss_started)
 				break;
 
-			ret = mwifiex_send_cmd_async(priv,
-						     HostCmd_CMD_UAP_BSS_START,
-						     HostCmd_ACT_GEN_SET, 0,
-						     NULL);
+			ret = mwifiex_send_cmd(priv, HostCmd_CMD_UAP_BSS_START,
+					       HostCmd_ACT_GEN_SET, 0,
+					       NULL, false);
 			if (ret) {
 				wiphy_err(wiphy, "Failed to start BSS\n");
 				return ret;
@@ -700,8 +698,8 @@ mwifiex_cfg80211_deinit_p2p(struct mwifiex_private *priv)
 	if (GET_BSS_ROLE(priv) != MWIFIEX_BSS_ROLE_STA)
 		mwifiex_set_bss_role(priv, MWIFIEX_BSS_ROLE_STA);
 
-	if (mwifiex_send_cmd_sync(priv, HostCmd_CMD_P2P_MODE_CFG,
-				  HostCmd_ACT_GEN_SET, 0, &mode))
+	if (mwifiex_send_cmd(priv, HostCmd_CMD_P2P_MODE_CFG,
+			     HostCmd_ACT_GEN_SET, 0, &mode, true))
 		return -1;
 
 	return 0;
@@ -721,13 +719,13 @@ mwifiex_cfg80211_init_p2p_client(struct mwifiex_private *priv)
 		return -1;
 
 	mode = P2P_MODE_DEVICE;
-	if (mwifiex_send_cmd_sync(priv, HostCmd_CMD_P2P_MODE_CFG,
-				  HostCmd_ACT_GEN_SET, 0, &mode))
+	if (mwifiex_send_cmd(priv, HostCmd_CMD_P2P_MODE_CFG,
+			     HostCmd_ACT_GEN_SET, 0, &mode, true))
 		return -1;
 
 	mode = P2P_MODE_CLIENT;
-	if (mwifiex_send_cmd_sync(priv, HostCmd_CMD_P2P_MODE_CFG,
-				  HostCmd_ACT_GEN_SET, 0, &mode))
+	if (mwifiex_send_cmd(priv, HostCmd_CMD_P2P_MODE_CFG,
+			     HostCmd_ACT_GEN_SET, 0, &mode, true))
 		return -1;
 
 	return 0;
@@ -747,13 +745,13 @@ mwifiex_cfg80211_init_p2p_go(struct mwifiex_private *priv)
 		return -1;
 
 	mode = P2P_MODE_DEVICE;
-	if (mwifiex_send_cmd_sync(priv, HostCmd_CMD_P2P_MODE_CFG,
-				  HostCmd_ACT_GEN_SET, 0, &mode))
+	if (mwifiex_send_cmd(priv, HostCmd_CMD_P2P_MODE_CFG,
+			     HostCmd_ACT_GEN_SET, 0, &mode, true))
 		return -1;
 
 	mode = P2P_MODE_GO;
-	if (mwifiex_send_cmd_sync(priv, HostCmd_CMD_P2P_MODE_CFG,
-				  HostCmd_ACT_GEN_SET, 0, &mode))
+	if (mwifiex_send_cmd(priv, HostCmd_CMD_P2P_MODE_CFG,
+			     HostCmd_ACT_GEN_SET, 0, &mode, true))
 		return -1;
 
 	if (GET_BSS_ROLE(priv) != MWIFIEX_BSS_ROLE_UAP)
@@ -853,8 +851,8 @@ mwifiex_cfg80211_change_virtual_intf(struct wiphy *wiphy,
 
 	priv->sec_info.authentication_mode = NL80211_AUTHTYPE_OPEN_SYSTEM;
 
-	ret = mwifiex_send_cmd_sync(priv, HostCmd_CMD_SET_BSS_MODE,
-				    HostCmd_ACT_GEN_SET, 0, NULL);
+	ret = mwifiex_send_cmd(priv, HostCmd_CMD_SET_BSS_MODE,
+			       HostCmd_ACT_GEN_SET, 0, NULL, true);
 
 	return ret;
 }
@@ -942,8 +940,8 @@ mwifiex_dump_station_info(struct mwifiex_private *priv,
 			STATION_INFO_SIGNAL | STATION_INFO_SIGNAL_AVG;
 
 	/* Get signal information from the firmware */
-	if (mwifiex_send_cmd_sync(priv, HostCmd_CMD_RSSI_INFO,
-				  HostCmd_ACT_GEN_GET, 0, NULL)) {
+	if (mwifiex_send_cmd(priv, HostCmd_CMD_RSSI_INFO,
+			     HostCmd_ACT_GEN_GET, 0, NULL, true)) {
 		dev_err(priv->adapter->dev, "failed to get signal information\n");
 		return -EFAULT;
 	}
@@ -954,9 +952,9 @@ mwifiex_dump_station_info(struct mwifiex_private *priv,
 	}
 
 	/* Get DTIM period information from firmware */
-	mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_SNMP_MIB,
-			      HostCmd_ACT_GEN_GET, DTIM_PERIOD_I,
-			      &priv->dtim_period);
+	mwifiex_send_cmd(priv, HostCmd_CMD_802_11_SNMP_MIB,
+			 HostCmd_ACT_GEN_GET, DTIM_PERIOD_I,
+			 &priv->dtim_period, true);
 
 	mwifiex_parse_htinfo(priv, priv->tx_htinfo, &sinfo->txrate);
 
@@ -1186,8 +1184,8 @@ static int mwifiex_cfg80211_set_bitrate_mask(struct wiphy *wiphy,
 	if (priv->adapter->hw_dev_mcs_support == HT_STREAM_2X2)
 		bitmap_rates[2] |= mask->control[band].ht_mcs[1] << 8;
 
-	return mwifiex_send_cmd_sync(priv, HostCmd_CMD_TX_RATE_CFG,
-				     HostCmd_ACT_GEN_SET, 0, bitmap_rates);
+	return mwifiex_send_cmd(priv, HostCmd_CMD_TX_RATE_CFG,
+				HostCmd_ACT_GEN_SET, 0, bitmap_rates, true);
 }
 
 /*
@@ -1216,14 +1214,14 @@ static int mwifiex_cfg80211_set_cqm_rssi_config(struct wiphy *wiphy,
 		subsc_evt.bcn_h_rssi_cfg.abs_value = abs(rssi_thold);
 		subsc_evt.bcn_l_rssi_cfg.evt_freq = 1;
 		subsc_evt.bcn_h_rssi_cfg.evt_freq = 1;
-		return mwifiex_send_cmd_sync(priv,
-					     HostCmd_CMD_802_11_SUBSCRIBE_EVENT,
-					     0, 0, &subsc_evt);
+		return mwifiex_send_cmd(priv,
+					HostCmd_CMD_802_11_SUBSCRIBE_EVENT,
+					0, 0, &subsc_evt, true);
 	} else {
 		subsc_evt.action = HostCmd_ACT_BITWISE_CLR;
-		return mwifiex_send_cmd_sync(priv,
-					     HostCmd_CMD_802_11_SUBSCRIBE_EVENT,
-					     0, 0, &subsc_evt);
+		return mwifiex_send_cmd(priv,
+					HostCmd_CMD_802_11_SUBSCRIBE_EVENT,
+					0, 0, &subsc_evt, true);
 	}
 
 	return 0;
@@ -1276,10 +1274,9 @@ mwifiex_cfg80211_del_station(struct wiphy *wiphy, struct net_device *dev,
 	if (!mac || is_broadcast_ether_addr(mac)) {
 		wiphy_dbg(wiphy, "%s: NULL/broadcast mac address\n", __func__);
 		list_for_each_entry(sta_node, &priv->sta_list, list) {
-			if (mwifiex_send_cmd_sync(priv,
-						  HostCmd_CMD_UAP_STA_DEAUTH,
-						  HostCmd_ACT_GEN_SET, 0,
-						  sta_node->mac_addr))
+			if (mwifiex_send_cmd(priv, HostCmd_CMD_UAP_STA_DEAUTH,
+					     HostCmd_ACT_GEN_SET, 0,
+					     sta_node->mac_addr, true))
 				return -1;
 			mwifiex_uap_del_sta_data(priv, sta_node);
 		}
@@ -1289,10 +1286,9 @@ mwifiex_cfg80211_del_station(struct wiphy *wiphy, struct net_device *dev,
 		sta_node = mwifiex_get_sta_entry(priv, mac);
 		spin_unlock_irqrestore(&priv->sta_list_spinlock, flags);
 		if (sta_node) {
-			if (mwifiex_send_cmd_sync(priv,
-						  HostCmd_CMD_UAP_STA_DEAUTH,
-						  HostCmd_ACT_GEN_SET, 0,
-						  sta_node->mac_addr))
+			if (mwifiex_send_cmd(priv, HostCmd_CMD_UAP_STA_DEAUTH,
+					     HostCmd_ACT_GEN_SET, 0,
+					     sta_node->mac_addr, true))
 				return -1;
 			mwifiex_uap_del_sta_data(priv, sta_node);
 		}
@@ -1328,13 +1324,40 @@ mwifiex_cfg80211_set_antenna(struct wiphy *wiphy, u32 tx_ant, u32 rx_ant)
 			tx_ant = RF_ANTENNA_AUTO;
 			rx_ant = RF_ANTENNA_AUTO;
 		}
+	} else {
+		struct ieee80211_sta_ht_cap *ht_info;
+		int rx_mcs_supp;
+		enum ieee80211_band band;
+
+		if ((tx_ant == 0x1 && rx_ant == 0x1)) {
+			adapter->user_dev_mcs_support = HT_STREAM_1X1;
+			if (adapter->is_hw_11ac_capable)
+				adapter->usr_dot_11ac_mcs_support =
+						MWIFIEX_11AC_MCS_MAP_1X1;
+		} else {
+			adapter->user_dev_mcs_support = HT_STREAM_2X2;
+			if (adapter->is_hw_11ac_capable)
+				adapter->usr_dot_11ac_mcs_support =
+						MWIFIEX_11AC_MCS_MAP_2X2;
+		}
+
+		for (band = 0; band < IEEE80211_NUM_BANDS; band++) {
+			if (!adapter->wiphy->bands[band])
+				continue;
+
+			ht_info = &adapter->wiphy->bands[band]->ht_cap;
+			rx_mcs_supp =
+				GET_RXMCSSUPP(adapter->user_dev_mcs_support);
+			memset(&ht_info->mcs, 0, adapter->number_of_antenna);
+			memset(&ht_info->mcs, 0xff, rx_mcs_supp);
+		}
 	}
 
 	ant_cfg.tx_ant = tx_ant;
 	ant_cfg.rx_ant = rx_ant;
 
-	return mwifiex_send_cmd_sync(priv, HostCmd_CMD_RF_ANTENNA,
-				     HostCmd_ACT_GEN_SET, 0, &ant_cfg);
+	return mwifiex_send_cmd(priv, HostCmd_CMD_RF_ANTENNA,
+				HostCmd_ACT_GEN_SET, 0, &ant_cfg, true);
 }
 
 /* cfg80211 operation handler for stop ap.
@@ -1349,8 +1372,8 @@ static int mwifiex_cfg80211_stop_ap(struct wiphy *wiphy, struct net_device *dev)
 
 	priv->ap_11n_enabled = 0;
 
-	if (mwifiex_send_cmd_sync(priv, HostCmd_CMD_UAP_BSS_STOP,
-				  HostCmd_ACT_GEN_SET, 0, NULL)) {
+	if (mwifiex_send_cmd(priv, HostCmd_CMD_UAP_BSS_STOP,
+			     HostCmd_ACT_GEN_SET, 0, NULL, true)) {
 		wiphy_err(wiphy, "Failed to stop the BSS\n");
 		return -1;
 	}
@@ -1416,9 +1439,6 @@ static int mwifiex_cfg80211_start_ap(struct wiphy *wiphy,
 
 		if (params->chandef.width > NL80211_CHAN_WIDTH_20_NOHT)
 			config_bands |= BAND_GN;
-
-		if (params->chandef.width > NL80211_CHAN_WIDTH_40)
-			config_bands |= BAND_GAC;
 	} else {
 		bss_cfg->band_cfg = BAND_CONFIG_A;
 		config_bands = BAND_A;
@@ -1464,16 +1484,16 @@ static int mwifiex_cfg80211_start_ap(struct wiphy *wiphy,
 		bss_cfg->ps_sta_ao_timer = 10 * params->inactivity_timeout;
 	}
 
-	if (mwifiex_send_cmd_sync(priv, HostCmd_CMD_UAP_BSS_STOP,
-				  HostCmd_ACT_GEN_SET, 0, NULL)) {
+	if (mwifiex_send_cmd(priv, HostCmd_CMD_UAP_BSS_STOP,
+			     HostCmd_ACT_GEN_SET, 0, NULL, true)) {
 		wiphy_err(wiphy, "Failed to stop the BSS\n");
 		kfree(bss_cfg);
 		return -1;
 	}
 
-	if (mwifiex_send_cmd_async(priv, HostCmd_CMD_UAP_SYS_CONFIG,
-				   HostCmd_ACT_GEN_SET,
-				   UAP_BSS_PARAMS_I, bss_cfg)) {
+	if (mwifiex_send_cmd(priv, HostCmd_CMD_UAP_SYS_CONFIG,
+			     HostCmd_ACT_GEN_SET,
+			     UAP_BSS_PARAMS_I, bss_cfg, false)) {
 		wiphy_err(wiphy, "Failed to set the SSID\n");
 		kfree(bss_cfg);
 		return -1;
@@ -1481,8 +1501,8 @@ static int mwifiex_cfg80211_start_ap(struct wiphy *wiphy,
 
 	kfree(bss_cfg);
 
-	if (mwifiex_send_cmd_async(priv, HostCmd_CMD_UAP_BSS_START,
-				   HostCmd_ACT_GEN_SET, 0, NULL)) {
+	if (mwifiex_send_cmd(priv, HostCmd_CMD_UAP_BSS_START,
+			     HostCmd_ACT_GEN_SET, 0, NULL, false)) {
 		wiphy_err(wiphy, "Failed to start the BSS\n");
 		return -1;
 	}
@@ -1492,9 +1512,9 @@ static int mwifiex_cfg80211_start_ap(struct wiphy *wiphy,
 	else
 		priv->curr_pkt_filter &= ~HostCmd_ACT_MAC_WEP_ENABLE;
 
-	if (mwifiex_send_cmd_sync(priv, HostCmd_CMD_MAC_CONTROL,
-				  HostCmd_ACT_GEN_SET, 0,
-				  &priv->curr_pkt_filter))
+	if (mwifiex_send_cmd(priv, HostCmd_CMD_MAC_CONTROL,
+			     HostCmd_ACT_GEN_SET, 0,
+			     &priv->curr_pkt_filter, true))
 		return -1;
 
 	return 0;
@@ -1583,8 +1603,9 @@ static int mwifiex_cfg80211_inform_ibss_bss(struct mwifiex_private *priv)
  * the function notifies the CFG802.11 subsystem of the new BSS connection.
  */
 static int
-mwifiex_cfg80211_assoc(struct mwifiex_private *priv, size_t ssid_len, u8 *ssid,
-		       u8 *bssid, int mode, struct ieee80211_channel *channel,
+mwifiex_cfg80211_assoc(struct mwifiex_private *priv, size_t ssid_len,
+		       const u8 *ssid, const u8 *bssid, int mode,
+		       struct ieee80211_channel *channel,
 		       struct cfg80211_connect_params *sme, bool privacy)
 {
 	struct cfg80211_ssid req_ssid;
@@ -1881,7 +1902,8 @@ mwifiex_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *dev,
 				     params->privacy);
 done:
 	if (!ret) {
-		cfg80211_ibss_joined(priv->netdev, priv->cfg_bssid, GFP_KERNEL);
+		cfg80211_ibss_joined(priv->netdev, priv->cfg_bssid,
+				     params->chandef.chan, GFP_KERNEL);
 		dev_dbg(priv->adapter->dev,
 			"info: joined/created adhoc network with bssid"
 			" %pM successfully\n", priv->cfg_bssid);
@@ -2098,8 +2120,8 @@ mwifiex_setup_ht_caps(struct ieee80211_sta_ht_cap *ht_info,
 	ht_info->cap &= ~IEEE80211_HT_CAP_MAX_AMSDU;
 	ht_info->cap |= IEEE80211_HT_CAP_SM_PS;
 
-	rx_mcs_supp = GET_RXMCSSUPP(adapter->hw_dev_mcs_support);
-	/* Set MCS for 1x1 */
+	rx_mcs_supp = GET_RXMCSSUPP(adapter->user_dev_mcs_support);
+	/* Set MCS for 1x1/2x2 */
 	memset(mcs, 0xff, rx_mcs_supp);
 	/* Clear all the other values */
 	memset(&mcs[rx_mcs_supp], 0,
@@ -2460,9 +2482,8 @@ static int mwifiex_cfg80211_suspend(struct wiphy *wiphy,
 				   MWIFIEX_CRITERIA_UNICAST |
 				   MWIFIEX_CRITERIA_MULTICAST;
 
-	ret =  mwifiex_send_cmd_sync(priv, HostCmd_CMD_MEF_CFG,
-				     HostCmd_ACT_GEN_SET, 0,
-				     &mef_cfg);
+	ret = mwifiex_send_cmd(priv, HostCmd_CMD_MEF_CFG,
+			       HostCmd_ACT_GEN_SET, 0, &mef_cfg, true);
 
 	kfree(mef_entry);
 	return ret;
@@ -2574,9 +2595,9 @@ static int mwifiex_cfg80211_set_coalesce(struct wiphy *wiphy,
 	if (!coalesce) {
 		dev_dbg(adapter->dev,
 			"Disable coalesce and reset all previous rules\n");
-		return mwifiex_send_cmd_sync(priv, HostCmd_CMD_COALESCE_CFG,
-					     HostCmd_ACT_GEN_SET, 0,
-					     &coalesce_cfg);
+		return mwifiex_send_cmd(priv, HostCmd_CMD_COALESCE_CFG,
+					HostCmd_ACT_GEN_SET, 0,
+					&coalesce_cfg, true);
 	}
 
 	coalesce_cfg.num_of_rules = coalesce->n_rules;
@@ -2591,8 +2612,172 @@ static int mwifiex_cfg80211_set_coalesce(struct wiphy *wiphy,
 		}
 	}
 
-	return mwifiex_send_cmd_sync(priv, HostCmd_CMD_COALESCE_CFG,
-				     HostCmd_ACT_GEN_SET, 0, &coalesce_cfg);
+	return mwifiex_send_cmd(priv, HostCmd_CMD_COALESCE_CFG,
+				HostCmd_ACT_GEN_SET, 0, &coalesce_cfg, true);
+}
+
+/* cfg80211 ops handler for tdls_mgmt.
+ * Function prepares TDLS action frame packets and forwards them to FW
+ */
+static int
+mwifiex_cfg80211_tdls_mgmt(struct wiphy *wiphy, struct net_device *dev,
+			   u8 *peer, u8 action_code, u8 dialog_token,
+			   u16 status_code, u32 peer_capability,
+			   const u8 *extra_ies, size_t extra_ies_len)
+{
+	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
+	int ret;
+
+	if (!(wiphy->flags & WIPHY_FLAG_SUPPORTS_TDLS))
+		return -ENOTSUPP;
+
+	/* make sure we are in station mode and connected */
+	if (!(priv->bss_type == MWIFIEX_BSS_TYPE_STA && priv->media_connected))
+		return -ENOTSUPP;
+
+	switch (action_code) {
+	case WLAN_TDLS_SETUP_REQUEST:
+		dev_dbg(priv->adapter->dev,
+			"Send TDLS Setup Request to %pM status_code=%d\n", peer,
+			 status_code);
+		ret = mwifiex_send_tdls_data_frame(priv, peer, action_code,
+						   dialog_token, status_code,
+						   extra_ies, extra_ies_len);
+		break;
+	case WLAN_TDLS_SETUP_RESPONSE:
+		dev_dbg(priv->adapter->dev,
+			"Send TDLS Setup Response to %pM status_code=%d\n",
+			peer, status_code);
+		ret = mwifiex_send_tdls_data_frame(priv, peer, action_code,
+						   dialog_token, status_code,
+						   extra_ies, extra_ies_len);
+		break;
+	case WLAN_TDLS_SETUP_CONFIRM:
+		dev_dbg(priv->adapter->dev,
+			"Send TDLS Confirm to %pM status_code=%d\n", peer,
+			status_code);
+		ret = mwifiex_send_tdls_data_frame(priv, peer, action_code,
+						   dialog_token, status_code,
+						   extra_ies, extra_ies_len);
+		break;
+	case WLAN_TDLS_TEARDOWN:
+		dev_dbg(priv->adapter->dev, "Send TDLS Tear down to %pM\n",
+			peer);
+		ret = mwifiex_send_tdls_data_frame(priv, peer, action_code,
+						   dialog_token, status_code,
+						   extra_ies, extra_ies_len);
+		break;
+	case WLAN_TDLS_DISCOVERY_REQUEST:
+		dev_dbg(priv->adapter->dev,
+			"Send TDLS Discovery Request to %pM\n", peer);
+		ret = mwifiex_send_tdls_data_frame(priv, peer, action_code,
+						   dialog_token, status_code,
+						   extra_ies, extra_ies_len);
+		break;
+	case WLAN_PUB_ACTION_TDLS_DISCOVER_RES:
+		dev_dbg(priv->adapter->dev,
+			"Send TDLS Discovery Response to %pM\n", peer);
+		ret = mwifiex_send_tdls_action_frame(priv, peer, action_code,
+						   dialog_token, status_code,
+						   extra_ies, extra_ies_len);
+		break;
+	default:
+		dev_warn(priv->adapter->dev,
+			 "Unknown TDLS mgmt/action frame %pM\n", peer);
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static int
+mwifiex_cfg80211_tdls_oper(struct wiphy *wiphy, struct net_device *dev,
+			   u8 *peer, enum nl80211_tdls_operation action)
+{
+	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
+
+	if (!(wiphy->flags & WIPHY_FLAG_SUPPORTS_TDLS) ||
+	    !(wiphy->flags & WIPHY_FLAG_TDLS_EXTERNAL_SETUP))
+		return -ENOTSUPP;
+
+	/* make sure we are in station mode and connected */
+	if (!(priv->bss_type == MWIFIEX_BSS_TYPE_STA && priv->media_connected))
+		return -ENOTSUPP;
+
+	dev_dbg(priv->adapter->dev,
+		"TDLS peer=%pM, oper=%d\n", peer, action);
+
+	switch (action) {
+	case NL80211_TDLS_ENABLE_LINK:
+		action = MWIFIEX_TDLS_ENABLE_LINK;
+		break;
+	case NL80211_TDLS_DISABLE_LINK:
+		action = MWIFIEX_TDLS_DISABLE_LINK;
+		break;
+	case NL80211_TDLS_TEARDOWN:
+		/* shouldn't happen!*/
+		dev_warn(priv->adapter->dev,
+			 "tdls_oper: teardown from driver not supported\n");
+		return -EINVAL;
+	case NL80211_TDLS_SETUP:
+		/* shouldn't happen!*/
+		dev_warn(priv->adapter->dev,
+			 "tdls_oper: setup from driver not supported\n");
+		return -EINVAL;
+	case NL80211_TDLS_DISCOVERY_REQ:
+		/* shouldn't happen!*/
+		dev_warn(priv->adapter->dev,
+			 "tdls_oper: discovery from driver not supported\n");
+		return -EINVAL;
+	default:
+		dev_err(priv->adapter->dev,
+			"tdls_oper: operation not supported\n");
+		return -ENOTSUPP;
+	}
+
+	return mwifiex_tdls_oper(priv, peer, action);
+}
+
+static int
+mwifiex_cfg80211_add_station(struct wiphy *wiphy,
+			     struct net_device *dev,
+			     u8 *mac, struct station_parameters *params)
+{
+	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
+
+	if (!(params->sta_flags_set & BIT(NL80211_STA_FLAG_TDLS_PEER)))
+		return -ENOTSUPP;
+
+	/* make sure we are in station mode and connected */
+	if ((priv->bss_type != MWIFIEX_BSS_TYPE_STA) || !priv->media_connected)
+		return -ENOTSUPP;
+
+	return mwifiex_tdls_oper(priv, mac, MWIFIEX_TDLS_CREATE_LINK);
+}
+
+static int
+mwifiex_cfg80211_change_station(struct wiphy *wiphy,
+				struct net_device *dev,
+				u8 *mac, struct station_parameters *params)
+{
+	int ret;
+	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
+
+	/* we support change_station handler only for TDLS peers*/
+	if (!(params->sta_flags_set & BIT(NL80211_STA_FLAG_TDLS_PEER)))
+		return -ENOTSUPP;
+
+	/* make sure we are in station mode and connected */
+	if ((priv->bss_type != MWIFIEX_BSS_TYPE_STA) || !priv->media_connected)
+		return -ENOTSUPP;
+
+	priv->sta_params = params;
+
+	ret = mwifiex_tdls_oper(priv, mac, MWIFIEX_TDLS_CONFIG_LINK);
+	priv->sta_params = NULL;
+
+	return ret;
 }
 
 /* station cfg80211 operations */
@@ -2630,6 +2815,10 @@ static struct cfg80211_ops mwifiex_cfg80211_ops = {
 	.set_wakeup = mwifiex_cfg80211_set_wakeup,
 #endif
 	.set_coalesce = mwifiex_cfg80211_set_coalesce,
+	.tdls_mgmt = mwifiex_cfg80211_tdls_mgmt,
+	.tdls_oper = mwifiex_cfg80211_tdls_oper,
+	.add_station = mwifiex_cfg80211_add_station,
+	.change_station = mwifiex_cfg80211_change_station,
 };
 
 #ifdef CONFIG_PM
@@ -2715,6 +2904,11 @@ int mwifiex_register_cfg80211(struct mwifiex_adapter *adapter)
 			WIPHY_FLAG_AP_PROBE_RESP_OFFLOAD |
 			WIPHY_FLAG_AP_UAPSD |
 			WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL;
+
+	if (ISSUPP_TDLS_ENABLED(adapter->fw_cap_info))
+		wiphy->flags |= WIPHY_FLAG_SUPPORTS_TDLS |
+				WIPHY_FLAG_TDLS_EXTERNAL_SETUP;
+
 	wiphy->regulatory_flags |=
 			REGULATORY_CUSTOM_REG |
 			REGULATORY_STRICT_REG;
@@ -2736,7 +2930,8 @@ int mwifiex_register_cfg80211(struct mwifiex_adapter *adapter)
 
 	wiphy->features |= NL80211_FEATURE_HT_IBSS |
 			   NL80211_FEATURE_INACTIVITY_TIMER |
-			   NL80211_FEATURE_LOW_PRIORITY_SCAN;
+			   NL80211_FEATURE_LOW_PRIORITY_SCAN |
+			   NL80211_FEATURE_NEED_OBSS_SCAN;
 
 	/* Reserve space for mwifiex specific private data for BSS */
 	wiphy->bss_priv_size = sizeof(struct mwifiex_bss_priv);
@@ -2767,17 +2962,17 @@ int mwifiex_register_cfg80211(struct mwifiex_adapter *adapter)
 				   country_code);
 	}
 
-	mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_SNMP_MIB,
-			      HostCmd_ACT_GEN_GET, FRAG_THRESH_I, &thr);
+	mwifiex_send_cmd(priv, HostCmd_CMD_802_11_SNMP_MIB,
+			 HostCmd_ACT_GEN_GET, FRAG_THRESH_I, &thr, true);
 	wiphy->frag_threshold = thr;
-	mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_SNMP_MIB,
-			      HostCmd_ACT_GEN_GET, RTS_THRESH_I, &thr);
+	mwifiex_send_cmd(priv, HostCmd_CMD_802_11_SNMP_MIB,
+			 HostCmd_ACT_GEN_GET, RTS_THRESH_I, &thr, true);
 	wiphy->rts_threshold = thr;
-	mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_SNMP_MIB,
-			      HostCmd_ACT_GEN_GET, SHORT_RETRY_LIM_I, &retry);
+	mwifiex_send_cmd(priv, HostCmd_CMD_802_11_SNMP_MIB,
+			 HostCmd_ACT_GEN_GET, SHORT_RETRY_LIM_I, &retry, true);
 	wiphy->retry_short = (u8) retry;
-	mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_SNMP_MIB,
-			      HostCmd_ACT_GEN_GET, LONG_RETRY_LIM_I, &retry);
+	mwifiex_send_cmd(priv, HostCmd_CMD_802_11_SNMP_MIB,
+			 HostCmd_ACT_GEN_GET, LONG_RETRY_LIM_I, &retry, true);
 	wiphy->retry_long = (u8) retry;
 
 	adapter->wiphy = wiphy;
diff --git a/drivers/net/wireless/mwifiex/cfp.c b/drivers/net/wireless/mwifiex/cfp.c
index 9eefacbc844b..0ddec3d4b059 100644
--- a/drivers/net/wireless/mwifiex/cfp.c
+++ b/drivers/net/wireless/mwifiex/cfp.c
@@ -71,6 +71,95 @@ u16 region_code_index[MWIFIEX_MAX_REGION_CODE] = { 0x10, 0x20, 0x30,
 
 static u8 supported_rates_n[N_SUPPORTED_RATES] = { 0x02, 0x04, 0 };
 
+/* For every mcs_rate line, the first 8 bytes are for stream 1x1,
+ * and all 16 bytes are for stream 2x2.
+ */
+static const u16 mcs_rate[4][16] = {
+	/* LGI 40M */
+	{ 0x1b, 0x36, 0x51, 0x6c, 0xa2, 0xd8, 0xf3, 0x10e,
+	  0x36, 0x6c, 0xa2, 0xd8, 0x144, 0x1b0, 0x1e6, 0x21c },
+
+	/* SGI 40M */
+	{ 0x1e, 0x3c, 0x5a, 0x78, 0xb4, 0xf0, 0x10e, 0x12c,
+	  0x3c, 0x78, 0xb4, 0xf0, 0x168, 0x1e0, 0x21c, 0x258 },
+
+	/* LGI 20M */
+	{ 0x0d, 0x1a, 0x27, 0x34, 0x4e, 0x68, 0x75, 0x82,
+	  0x1a, 0x34, 0x4e, 0x68, 0x9c, 0xd0, 0xea, 0x104 },
+
+	/* SGI 20M */
+	{ 0x0e, 0x1c, 0x2b, 0x39, 0x56, 0x73, 0x82, 0x90,
+	  0x1c, 0x39, 0x56, 0x73, 0xad, 0xe7, 0x104, 0x120 }
+};
+
+/* AC rates */
+static const u16 ac_mcs_rate_nss1[8][10] = {
+	/* LG 160M */
+	{ 0x75, 0xEA, 0x15F, 0x1D4, 0x2BE, 0x3A8, 0x41D,
+	  0x492, 0x57C, 0x618 },
+
+	/* SG 160M */
+	{ 0x82, 0x104, 0x186, 0x208, 0x30C, 0x410, 0x492,
+	  0x514, 0x618, 0x6C6 },
+
+	/* LG 80M */
+	{ 0x3B, 0x75, 0xB0, 0xEA, 0x15F, 0x1D4, 0x20F,
+	  0x249, 0x2BE, 0x30C },
+
+	/* SG 80M */
+	{ 0x41, 0x82, 0xC3, 0x104, 0x186, 0x208, 0x249,
+	  0x28A, 0x30C, 0x363 },
+
+	/* LG 40M */
+	{ 0x1B, 0x36, 0x51, 0x6C, 0xA2, 0xD8, 0xF3,
+	  0x10E, 0x144, 0x168 },
+
+	/* SG 40M */
+	{ 0x1E, 0x3C, 0x5A, 0x78, 0xB4, 0xF0, 0x10E,
+	  0x12C, 0x168, 0x190 },
+
+	/* LG 20M */
+	{ 0xD, 0x1A, 0x27, 0x34, 0x4E, 0x68, 0x75, 0x82, 0x9C, 0x00 },
+
+	/* SG 20M */
+	{ 0xF, 0x1D, 0x2C, 0x3A, 0x57, 0x74, 0x82, 0x91, 0xAE, 0x00 },
+};
+
+/* NSS2 note: the value in the table is 2 multiplier of the actual rate */
+static const u16 ac_mcs_rate_nss2[8][10] = {
+	/* LG 160M */
+	{ 0xEA, 0x1D4, 0x2BE, 0x3A8, 0x57C, 0x750, 0x83A,
+	  0x924, 0xAF8, 0xC30 },
+
+	/* SG 160M */
+	{ 0x104, 0x208, 0x30C, 0x410, 0x618, 0x820, 0x924,
+	  0xA28, 0xC30, 0xD8B },
+
+	/* LG 80M */
+	{ 0x75, 0xEA, 0x15F, 0x1D4, 0x2BE, 0x3A8, 0x41D,
+	  0x492, 0x57C, 0x618 },
+
+	/* SG 80M */
+	{ 0x82, 0x104, 0x186, 0x208, 0x30C, 0x410, 0x492,
+	  0x514, 0x618, 0x6C6 },
+
+	/* LG 40M */
+	{ 0x36, 0x6C, 0xA2, 0xD8, 0x144, 0x1B0, 0x1E6,
+	  0x21C, 0x288, 0x2D0 },
+
+	/* SG 40M */
+	{ 0x3C, 0x78, 0xB4, 0xF0, 0x168, 0x1E0, 0x21C,
+	  0x258, 0x2D0, 0x320 },
+
+	/* LG 20M */
+	{ 0x1A, 0x34, 0x4A, 0x68, 0x9C, 0xD0, 0xEA, 0x104,
+	  0x138, 0x00 },
+
+	/* SG 20M */
+	{ 0x1D, 0x3A, 0x57, 0x74, 0xAE, 0xE6, 0x104, 0x121,
+	  0x15B, 0x00 },
+};
+
 struct region_code_mapping {
 	u8 code;
 	u8 region[IEEE80211_COUNTRY_STRING_LEN];
@@ -109,95 +198,6 @@ u8 *mwifiex_11d_code_2_region(u8 code)
 u32 mwifiex_index_to_acs_data_rate(struct mwifiex_private *priv,
 				   u8 index, u8 ht_info)
 {
-	/*
-	 * For every mcs_rate line, the first 8 bytes are for stream 1x1,
-	 * and all 16 bytes are for stream 2x2.
-	 */
-	u16  mcs_rate[4][16] = {
-		/* LGI 40M */
-		{ 0x1b, 0x36, 0x51, 0x6c, 0xa2, 0xd8, 0xf3, 0x10e,
-		  0x36, 0x6c, 0xa2, 0xd8, 0x144, 0x1b0, 0x1e6, 0x21c },
-
-		/* SGI 40M */
-		{ 0x1e, 0x3c, 0x5a, 0x78, 0xb4, 0xf0, 0x10e, 0x12c,
-		  0x3c, 0x78, 0xb4, 0xf0, 0x168, 0x1e0, 0x21c, 0x258 },
-
-		/* LGI 20M */
-		{ 0x0d, 0x1a, 0x27, 0x34, 0x4e, 0x68, 0x75, 0x82,
-		  0x1a, 0x34, 0x4e, 0x68, 0x9c, 0xd0, 0xea, 0x104 },
-
-		/* SGI 20M */
-		{ 0x0e, 0x1c, 0x2b, 0x39, 0x56, 0x73, 0x82, 0x90,
-		  0x1c, 0x39, 0x56, 0x73, 0xad, 0xe7, 0x104, 0x120 }
-	};
-	/* AC rates */
-	u16 ac_mcs_rate_nss1[8][10] = {
-		/* LG 160M */
-		{ 0x75, 0xEA, 0x15F, 0x1D4, 0x2BE, 0x3A8, 0x41D,
-		  0x492, 0x57C, 0x618 },
-
-		/* SG 160M */
-		{ 0x82, 0x104, 0x186, 0x208, 0x30C, 0x410, 0x492,
-		  0x514, 0x618, 0x6C6 },
-
-		/* LG 80M */
-		{ 0x3B, 0x75, 0xB0, 0xEA, 0x15F, 0x1D4, 0x20F,
-		  0x249, 0x2BE, 0x30C },
-
-		/* SG 80M */
-		{ 0x41, 0x82, 0xC3, 0x104, 0x186, 0x208, 0x249,
-		  0x28A, 0x30C, 0x363 },
-
-		/* LG 40M */
-		{ 0x1B, 0x36, 0x51, 0x6C, 0xA2, 0xD8, 0xF3,
-		  0x10E, 0x144, 0x168 },
-
-		/* SG 40M */
-		{ 0x1E, 0x3C, 0x5A, 0x78, 0xB4, 0xF0, 0x10E,
-		  0x12C, 0x168, 0x190 },
-
-		/* LG 20M */
-		{ 0xD, 0x1A, 0x27, 0x34, 0x4E, 0x68, 0x75, 0x82, 0x9C, 0x00 },
-
-		/* SG 20M */
-		{ 0xF, 0x1D, 0x2C, 0x3A, 0x57, 0x74, 0x82, 0x91, 0xAE, 0x00 },
-	};
-	/* NSS2 note: the value in the table is 2 multiplier of the actual
-	 * rate
-	 */
-	u16 ac_mcs_rate_nss2[8][10] = {
-		/* LG 160M */
-		{ 0xEA, 0x1D4, 0x2BE, 0x3A8, 0x57C, 0x750, 0x83A,
-		  0x924, 0xAF8, 0xC30 },
-
-		/* SG 160M */
-		{ 0x104, 0x208, 0x30C, 0x410, 0x618, 0x820, 0x924,
-		  0xA28, 0xC30, 0xD8B },
-
-		/* LG 80M */
-		{ 0x75, 0xEA, 0x15F, 0x1D4, 0x2BE, 0x3A8, 0x41D,
-		  0x492, 0x57C, 0x618 },
-
-		/* SG 80M */
-		{ 0x82, 0x104, 0x186, 0x208, 0x30C, 0x410, 0x492,
-		  0x514, 0x618, 0x6C6 },
-
-		/* LG 40M */
-		{ 0x36, 0x6C, 0xA2, 0xD8, 0x144, 0x1B0, 0x1E6,
-		  0x21C, 0x288, 0x2D0 },
-
-		/* SG 40M */
-		{ 0x3C, 0x78, 0xB4, 0xF0, 0x168, 0x1E0, 0x21C,
-		  0x258, 0x2D0, 0x320 },
-
-		/* LG 20M */
-		{ 0x1A, 0x34, 0x4A, 0x68, 0x9C, 0xD0, 0xEA, 0x104,
-		  0x138, 0x00 },
-
-		/* SG 20M */
-		{ 0x1D, 0x3A, 0x57, 0x74, 0xAE, 0xE6, 0x104, 0x121,
-		  0x15B, 0x00 },
-	};
 	u32 rate = 0;
 	u8 mcs_index = 0;
 	u8 bw = 0;
@@ -252,28 +252,8 @@ u32 mwifiex_index_to_acs_data_rate(struct mwifiex_private *priv,
 u32 mwifiex_index_to_data_rate(struct mwifiex_private *priv,
 			       u8 index, u8 ht_info)
 {
-	/* For every mcs_rate line, the first 8 bytes are for stream 1x1,
-	 * and all 16 bytes are for stream 2x2.
-	 */
-	u16  mcs_rate[4][16] = {
-		/* LGI 40M */
-		{ 0x1b, 0x36, 0x51, 0x6c, 0xa2, 0xd8, 0xf3, 0x10e,
-		  0x36, 0x6c, 0xa2, 0xd8, 0x144, 0x1b0, 0x1e6, 0x21c },
-
-		/* SGI 40M */
-		{ 0x1e, 0x3c, 0x5a, 0x78, 0xb4, 0xf0, 0x10e, 0x12c,
-		  0x3c, 0x78, 0xb4, 0xf0, 0x168, 0x1e0, 0x21c, 0x258 },
-
-		/* LGI 20M */
-		{ 0x0d, 0x1a, 0x27, 0x34, 0x4e, 0x68, 0x75, 0x82,
-		  0x1a, 0x34, 0x4e, 0x68, 0x9c, 0xd0, 0xea, 0x104 },
-
-		/* SGI 20M */
-		{ 0x0e, 0x1c, 0x2b, 0x39, 0x56, 0x73, 0x82, 0x90,
-		  0x1c, 0x39, 0x56, 0x73, 0xad, 0xe7, 0x104, 0x120 }
-	};
 	u32 mcs_num_supp =
-		(priv->adapter->hw_dev_mcs_support == HT_STREAM_2X2) ? 16 : 8;
+		(priv->adapter->user_dev_mcs_support == HT_STREAM_2X2) ? 16 : 8;
 	u32 rate;
 
 	if (priv->adapter->is_hw_11ac_capable)
@@ -458,7 +438,6 @@ u32 mwifiex_get_supported_rates(struct mwifiex_private *priv, u8 *rates)
 			break;
 		case BAND_G:
 		case BAND_G | BAND_GN:
-		case BAND_G | BAND_GN | BAND_GAC:
 			dev_dbg(adapter->dev, "info: infra band=%d "
 				"supported_rates_g\n", adapter->config_bands);
 			k = mwifiex_copy_rates(rates, k, supported_rates_g,
@@ -469,10 +448,7 @@ u32 mwifiex_get_supported_rates(struct mwifiex_private *priv, u8 *rates)
 		case BAND_A | BAND_B:
 		case BAND_A | BAND_B | BAND_G | BAND_GN | BAND_AN:
 		case BAND_A | BAND_B | BAND_G | BAND_GN | BAND_AN | BAND_AAC:
-		case BAND_A | BAND_B | BAND_G | BAND_GN | BAND_AN |
-		     BAND_AAC | BAND_GAC:
 		case BAND_B | BAND_G | BAND_GN:
-		case BAND_B | BAND_G | BAND_GN | BAND_GAC:
 			dev_dbg(adapter->dev, "info: infra band=%d "
 				"supported_rates_bg\n", adapter->config_bands);
 			k = mwifiex_copy_rates(rates, k, supported_rates_bg,
@@ -496,7 +472,6 @@ u32 mwifiex_get_supported_rates(struct mwifiex_private *priv, u8 *rates)
 					       sizeof(supported_rates_a));
 			break;
 		case BAND_GN:
-		case BAND_GN | BAND_GAC:
 			dev_dbg(adapter->dev, "info: infra band=%d "
 				"supported_rates_n\n", adapter->config_bands);
 			k = mwifiex_copy_rates(rates, k, supported_rates_n,
diff --git a/drivers/net/wireless/mwifiex/cmdevt.c b/drivers/net/wireless/mwifiex/cmdevt.c
index 1ddc8b2e3722..14e05c9f4663 100644
--- a/drivers/net/wireless/mwifiex/cmdevt.c
+++ b/drivers/net/wireless/mwifiex/cmdevt.c
@@ -37,13 +37,12 @@
 static void
 mwifiex_init_cmd_node(struct mwifiex_private *priv,
 		      struct cmd_ctrl_node *cmd_node,
-		      u32 cmd_oid, void *data_buf)
+		      u32 cmd_oid, void *data_buf, bool sync)
 {
 	cmd_node->priv = priv;
 	cmd_node->cmd_oid = cmd_oid;
-	if (priv->adapter->cmd_wait_q_required) {
-		cmd_node->wait_q_enabled = priv->adapter->cmd_wait_q_required;
-		priv->adapter->cmd_wait_q_required = false;
+	if (sync) {
+		cmd_node->wait_q_enabled = true;
 		cmd_node->cmd_wait_q_woken = false;
 		cmd_node->condition = &cmd_node->cmd_wait_q_woken;
 	}
@@ -166,8 +165,10 @@ static int mwifiex_dnld_cmd_to_fw(struct mwifiex_private *priv,
 		dev_err(adapter->dev,
 			"DNLD_CMD: FW in reset state, ignore cmd %#x\n",
 			cmd_code);
-		mwifiex_complete_cmd(adapter, cmd_node);
+		if (cmd_node->wait_q_enabled)
+			mwifiex_complete_cmd(adapter, cmd_node);
 		mwifiex_recycle_cmd_node(adapter, cmd_node);
+		queue_work(adapter->workqueue, &adapter->main_work);
 		return -1;
 	}
 
@@ -480,28 +481,7 @@ int mwifiex_process_event(struct mwifiex_adapter *adapter)
 }
 
 /*
- * This function is used to send synchronous command to the firmware.
- *
- * it allocates a wait queue for the command and wait for the command
- * response.
- */
-int mwifiex_send_cmd_sync(struct mwifiex_private *priv, uint16_t cmd_no,
-			  u16 cmd_action, u32 cmd_oid, void *data_buf)
-{
-	int ret = 0;
-	struct mwifiex_adapter *adapter = priv->adapter;
-
-	adapter->cmd_wait_q_required = true;
-
-	ret = mwifiex_send_cmd_async(priv, cmd_no, cmd_action, cmd_oid,
-				     data_buf);
-
-	return ret;
-}
-
-
-/*
- * This function prepares a command and asynchronously send it to the firmware.
+ * This function prepares a command and send it to the firmware.
  *
  * Preparation includes -
  *      - Sanity tests to make sure the card is still present or the FW
@@ -511,8 +491,8 @@ int mwifiex_send_cmd_sync(struct mwifiex_private *priv, uint16_t cmd_no,
  *      - Fill up the non-default parameters and buffer pointers
  *      - Add the command to pending queue
  */
-int mwifiex_send_cmd_async(struct mwifiex_private *priv, uint16_t cmd_no,
-			   u16 cmd_action, u32 cmd_oid, void *data_buf)
+int mwifiex_send_cmd(struct mwifiex_private *priv, u16 cmd_no,
+		     u16 cmd_action, u32 cmd_oid, void *data_buf, bool sync)
 {
 	int ret;
 	struct mwifiex_adapter *adapter = priv->adapter;
@@ -534,6 +514,11 @@ int mwifiex_send_cmd_async(struct mwifiex_private *priv, uint16_t cmd_no,
 		return -1;
 	}
 
+	if (adapter->is_cmd_timedout) {
+		dev_err(adapter->dev, "PREP_CMD: FW is in bad state\n");
+		return -1;
+	}
+
 	if (adapter->hw_status == MWIFIEX_HW_STATUS_RESET) {
 		if (cmd_no != HostCmd_CMD_FUNC_INIT) {
 			dev_err(adapter->dev, "PREP_CMD: FW in reset state\n");
@@ -550,7 +535,7 @@ int mwifiex_send_cmd_async(struct mwifiex_private *priv, uint16_t cmd_no,
 	}
 
 	/* Initialize the command node */
-	mwifiex_init_cmd_node(priv, cmd_node, cmd_oid, data_buf);
+	mwifiex_init_cmd_node(priv, cmd_node, cmd_oid, data_buf, sync);
 
 	if (!cmd_node->cmd_skb) {
 		dev_err(adapter->dev, "PREP_CMD: no free cmd buf\n");
@@ -595,7 +580,8 @@ int mwifiex_send_cmd_async(struct mwifiex_private *priv, uint16_t cmd_no,
 	}
 
 	/* Send command */
-	if (cmd_no == HostCmd_CMD_802_11_SCAN) {
+	if (cmd_no == HostCmd_CMD_802_11_SCAN ||
+	    cmd_no == HostCmd_CMD_802_11_SCAN_EXT) {
 		mwifiex_queue_scan_cmd(priv, cmd_node);
 	} else {
 		mwifiex_insert_cmd_to_pending_q(adapter, cmd_node, true);
@@ -785,7 +771,7 @@ int mwifiex_process_cmdresp(struct mwifiex_adapter *adapter)
 	unsigned long flags;
 
 	/* Now we got response from FW, cancel the command timer */
-	del_timer(&adapter->cmd_timer);
+	del_timer_sync(&adapter->cmd_timer);
 
 	if (!adapter->curr_cmd || !adapter->curr_cmd->resp_skb) {
 		resp = (struct host_cmd_ds_command *) adapter->upld_buf;
@@ -794,7 +780,7 @@ int mwifiex_process_cmdresp(struct mwifiex_adapter *adapter)
 		return -1;
 	}
 
-	adapter->num_cmd_timeout = 0;
+	adapter->is_cmd_timedout = 0;
 
 	resp = (struct host_cmd_ds_command *) adapter->curr_cmd->resp_skb->data;
 	if (adapter->curr_cmd->cmd_flag & CMD_F_CANCELED) {
@@ -905,8 +891,7 @@ mwifiex_cmd_timeout_func(unsigned long function_context)
 	struct cmd_ctrl_node *cmd_node;
 	struct timeval tstamp;
 
-	adapter->num_cmd_timeout++;
-	adapter->dbg.num_cmd_timeout++;
+	adapter->is_cmd_timedout = 1;
 	if (!adapter->curr_cmd) {
 		dev_dbg(adapter->dev, "cmd: empty curr_cmd\n");
 		return;
@@ -929,8 +914,8 @@ mwifiex_cmd_timeout_func(unsigned long function_context)
 		dev_err(adapter->dev, "num_cmd_h2c_failure = %d\n",
 			adapter->dbg.num_cmd_host_to_card_failure);
 
-		dev_err(adapter->dev, "num_cmd_timeout = %d\n",
-			adapter->dbg.num_cmd_timeout);
+		dev_err(adapter->dev, "is_cmd_timedout = %d\n",
+			adapter->is_cmd_timedout);
 		dev_err(adapter->dev, "num_tx_timeout = %d\n",
 			adapter->dbg.num_tx_timeout);
 
@@ -987,7 +972,9 @@ void
 mwifiex_cancel_all_pending_cmd(struct mwifiex_adapter *adapter)
 {
 	struct cmd_ctrl_node *cmd_node = NULL, *tmp_node;
-	unsigned long flags;
+	unsigned long flags, cmd_flags;
+	struct mwifiex_private *priv;
+	int i;
 
 	/* Cancel current cmd */
 	if ((adapter->curr_cmd) && (adapter->curr_cmd->wait_q_enabled)) {
@@ -1027,9 +1014,21 @@ mwifiex_cancel_all_pending_cmd(struct mwifiex_adapter *adapter)
 	}
 	spin_unlock_irqrestore(&adapter->scan_pending_q_lock, flags);
 
-	spin_lock_irqsave(&adapter->mwifiex_cmd_lock, flags);
-	adapter->scan_processing = false;
-	spin_unlock_irqrestore(&adapter->mwifiex_cmd_lock, flags);
+	if (adapter->scan_processing) {
+		spin_lock_irqsave(&adapter->mwifiex_cmd_lock, cmd_flags);
+		adapter->scan_processing = false;
+		spin_unlock_irqrestore(&adapter->mwifiex_cmd_lock, cmd_flags);
+		for (i = 0; i < adapter->priv_num; i++) {
+			priv = adapter->priv[i];
+			if (!priv)
+				continue;
+			if (priv->scan_request) {
+				dev_dbg(adapter->dev, "info: aborting scan\n");
+				cfg80211_scan_done(priv->scan_request, 1);
+				priv->scan_request = NULL;
+			}
+		}
+	}
 }
 
 /*
@@ -1048,7 +1047,8 @@ mwifiex_cancel_pending_ioctl(struct mwifiex_adapter *adapter)
 	struct cmd_ctrl_node *cmd_node = NULL, *tmp_node = NULL;
 	unsigned long cmd_flags;
 	unsigned long scan_pending_q_flags;
-	bool cancel_scan_cmd = false;
+	struct mwifiex_private *priv;
+	int i;
 
 	if ((adapter->curr_cmd) &&
 	    (adapter->curr_cmd->wait_q_enabled)) {
@@ -1074,15 +1074,24 @@ mwifiex_cancel_pending_ioctl(struct mwifiex_adapter *adapter)
 		mwifiex_insert_cmd_to_free_q(adapter, cmd_node);
 		spin_lock_irqsave(&adapter->scan_pending_q_lock,
 				  scan_pending_q_flags);
-		cancel_scan_cmd = true;
 	}
 	spin_unlock_irqrestore(&adapter->scan_pending_q_lock,
 			       scan_pending_q_flags);
 
-	if (cancel_scan_cmd) {
+	if (adapter->scan_processing) {
 		spin_lock_irqsave(&adapter->mwifiex_cmd_lock, cmd_flags);
 		adapter->scan_processing = false;
 		spin_unlock_irqrestore(&adapter->mwifiex_cmd_lock, cmd_flags);
+		for (i = 0; i < adapter->priv_num; i++) {
+			priv = adapter->priv[i];
+			if (!priv)
+				continue;
+			if (priv->scan_request) {
+				dev_dbg(adapter->dev, "info: aborting scan\n");
+				cfg80211_scan_done(priv->scan_request, 1);
+				priv->scan_request = NULL;
+			}
+		}
 	}
 	adapter->cmd_wait_q.status = -1;
 }
@@ -1454,7 +1463,10 @@ int mwifiex_ret_get_hw_spec(struct mwifiex_private *priv,
 {
 	struct host_cmd_ds_get_hw_spec *hw_spec = &resp->params.hw_spec;
 	struct mwifiex_adapter *adapter = priv->adapter;
-	int i;
+	struct mwifiex_ie_types_header *tlv;
+	struct hw_spec_fw_api_rev *api_rev;
+	u16 resp_size, api_id;
+	int i, left_len, parsed_len = 0;
 
 	adapter->fw_cap_info = le32_to_cpu(hw_spec->fw_cap_info);
 
@@ -1498,8 +1510,10 @@ int mwifiex_ret_get_hw_spec(struct mwifiex_private *priv,
 		/* Copy 11AC cap */
 		adapter->hw_dot_11ac_dev_cap =
 					le32_to_cpu(hw_spec->dot_11ac_dev_cap);
-		adapter->usr_dot_11ac_dev_cap_bg = adapter->hw_dot_11ac_dev_cap;
-		adapter->usr_dot_11ac_dev_cap_a = adapter->hw_dot_11ac_dev_cap;
+		adapter->usr_dot_11ac_dev_cap_bg = adapter->hw_dot_11ac_dev_cap
+					& ~MWIFIEX_DEF_11AC_CAP_BF_RESET_MASK;
+		adapter->usr_dot_11ac_dev_cap_a = adapter->hw_dot_11ac_dev_cap
+					& ~MWIFIEX_DEF_11AC_CAP_BF_RESET_MASK;
 
 		/* Copy 11AC mcs */
 		adapter->hw_dot_11ac_mcs_support =
@@ -1510,6 +1524,46 @@ int mwifiex_ret_get_hw_spec(struct mwifiex_private *priv,
 		adapter->is_hw_11ac_capable = false;
 	}
 
+	resp_size = le16_to_cpu(resp->size) - S_DS_GEN;
+	if (resp_size > sizeof(struct host_cmd_ds_get_hw_spec)) {
+		/* we have variable HW SPEC information */
+		left_len = resp_size - sizeof(struct host_cmd_ds_get_hw_spec);
+		while (left_len > sizeof(struct mwifiex_ie_types_header)) {
+			tlv = (void *)&hw_spec->tlvs + parsed_len;
+			switch (le16_to_cpu(tlv->type)) {
+			case TLV_TYPE_FW_API_REV:
+				api_rev = (struct hw_spec_fw_api_rev *)tlv;
+				api_id = le16_to_cpu(api_rev->api_id);
+				switch (api_id) {
+				case KEY_API_VER_ID:
+					adapter->fw_key_api_major_ver =
+							api_rev->major_ver;
+					adapter->fw_key_api_minor_ver =
+							api_rev->minor_ver;
+					dev_dbg(adapter->dev,
+						"fw_key_api v%d.%d\n",
+						adapter->fw_key_api_major_ver,
+						adapter->fw_key_api_minor_ver);
+					break;
+				default:
+					dev_warn(adapter->dev,
+						 "Unknown FW api_id: %d\n",
+						 api_id);
+					break;
+				}
+				break;
+			default:
+				dev_warn(adapter->dev,
+					 "Unknown GET_HW_SPEC TLV type: %#x\n",
+					 le16_to_cpu(tlv->type));
+				break;
+			}
+			parsed_len += le16_to_cpu(tlv->len) +
+				      sizeof(struct mwifiex_ie_types_header);
+			left_len -= parsed_len;
+		}
+	}
+
 	dev_dbg(adapter->dev, "info: GET_HW_SPEC: fw_release_number- %#x\n",
 		adapter->fw_release_number);
 	dev_dbg(adapter->dev, "info: GET_HW_SPEC: permanent addr: %pM\n",
@@ -1538,6 +1592,7 @@ int mwifiex_ret_get_hw_spec(struct mwifiex_private *priv,
 
 	adapter->hw_dot_11n_dev_cap = le32_to_cpu(hw_spec->dot_11n_dev_cap);
 	adapter->hw_dev_mcs_support = hw_spec->dev_mcs_support;
+	adapter->user_dev_mcs_support = adapter->hw_dev_mcs_support;
 
 	if (adapter->if_ops.update_mp_end_port)
 		adapter->if_ops.update_mp_end_port(adapter,
diff --git a/drivers/net/wireless/mwifiex/debugfs.c b/drivers/net/wireless/mwifiex/debugfs.c
index a5f9875cfd6e..b8a49aad12fd 100644
--- a/drivers/net/wireless/mwifiex/debugfs.c
+++ b/drivers/net/wireless/mwifiex/debugfs.c
@@ -85,8 +85,8 @@ static struct mwifiex_debug_data items[] = {
 	 item_addr(hs_activated), 1},
 	{"num_tx_timeout", item_size(num_tx_timeout),
 	 item_addr(num_tx_timeout), 1},
-	{"num_cmd_timeout", item_size(num_cmd_timeout),
-	 item_addr(num_cmd_timeout), 1},
+	{"is_cmd_timedout", item_size(is_cmd_timedout),
+	 item_addr(is_cmd_timedout), 1},
 	{"timeout_cmd_id", item_size(timeout_cmd_id),
 	 item_addr(timeout_cmd_id), 1},
 	{"timeout_cmd_act", item_size(timeout_cmd_act),
@@ -493,7 +493,7 @@ mwifiex_regrdwr_write(struct file *file,
 {
 	unsigned long addr = get_zeroed_page(GFP_KERNEL);
 	char *buf = (char *) addr;
-	size_t buf_size = min(count, (size_t) (PAGE_SIZE - 1));
+	size_t buf_size = min_t(size_t, count, PAGE_SIZE - 1);
 	int ret;
 	u32 reg_type = 0, reg_offset = 0, reg_value = UINT_MAX;
 
@@ -594,7 +594,7 @@ mwifiex_rdeeprom_write(struct file *file,
 {
 	unsigned long addr = get_zeroed_page(GFP_KERNEL);
 	char *buf = (char *) addr;
-	size_t buf_size = min(count, (size_t) (PAGE_SIZE - 1));
+	size_t buf_size = min_t(size_t, count, PAGE_SIZE - 1);
 	int ret = 0;
 	int offset = -1, bytes = -1;
 
diff --git a/drivers/net/wireless/mwifiex/decl.h b/drivers/net/wireless/mwifiex/decl.h
index 3a21bd03d6db..e7b3e16e5d34 100644
--- a/drivers/net/wireless/mwifiex/decl.h
+++ b/drivers/net/wireless/mwifiex/decl.h
@@ -75,10 +75,16 @@
 
 #define MWIFIEX_BUF_FLAG_REQUEUED_PKT      BIT(0)
 #define MWIFIEX_BUF_FLAG_BRIDGED_PKT	   BIT(1)
+#define MWIFIEX_BUF_FLAG_TDLS_PKT	   BIT(2)
 
 #define MWIFIEX_BRIDGED_PKTS_THR_HIGH      1024
 #define MWIFIEX_BRIDGED_PKTS_THR_LOW        128
 
+#define MWIFIEX_TDLS_DISABLE_LINK             0x00
+#define MWIFIEX_TDLS_ENABLE_LINK              0x01
+#define MWIFIEX_TDLS_CREATE_LINK              0x02
+#define MWIFIEX_TDLS_CONFIG_LINK              0x03
+
 enum mwifiex_bss_type {
 	MWIFIEX_BSS_TYPE_STA = 0,
 	MWIFIEX_BSS_TYPE_UAP = 1,
@@ -92,6 +98,23 @@ enum mwifiex_bss_role {
 	MWIFIEX_BSS_ROLE_ANY = 0xff,
 };
 
+enum mwifiex_tdls_status {
+	TDLS_NOT_SETUP = 0,
+	TDLS_SETUP_INPROGRESS,
+	TDLS_SETUP_COMPLETE,
+	TDLS_SETUP_FAILURE,
+	TDLS_LINK_TEARDOWN,
+};
+
+enum mwifiex_tdls_error_code {
+	TDLS_ERR_NO_ERROR = 0,
+	TDLS_ERR_INTERNAL_ERROR,
+	TDLS_ERR_MAX_LINKS_EST,
+	TDLS_ERR_LINK_EXISTS,
+	TDLS_ERR_LINK_NONEXISTENT,
+	TDLS_ERR_PEER_STA_UNREACHABLE = 25,
+};
+
 #define BSS_ROLE_BIT_MASK    BIT(0)
 
 #define GET_BSS_ROLE(priv)   ((priv)->bss_role & BSS_ROLE_BIT_MASK)
diff --git a/drivers/net/wireless/mwifiex/fw.h b/drivers/net/wireless/mwifiex/fw.h
index 5fa932d5f905..39cb3542f79c 100644
--- a/drivers/net/wireless/mwifiex/fw.h
+++ b/drivers/net/wireless/mwifiex/fw.h
@@ -50,21 +50,23 @@ struct tx_packet_hdr {
 #define HOSTCMD_SUPPORTED_RATES         14
 #define N_SUPPORTED_RATES               3
 #define ALL_802_11_BANDS           (BAND_A | BAND_B | BAND_G | BAND_GN | \
-				    BAND_AN | BAND_GAC | BAND_AAC)
+				    BAND_AN | BAND_AAC)
 
 #define FW_MULTI_BANDS_SUPPORT  (BIT(8) | BIT(9) | BIT(10) | BIT(11) | \
-				 BIT(12) | BIT(13))
+				 BIT(13))
 #define IS_SUPPORT_MULTI_BANDS(adapter)        \
 	(adapter->fw_cap_info & FW_MULTI_BANDS_SUPPORT)
 
-/* shift bit 12 and bit 13 in fw_cap_info from the firmware to bit 13 and 14
- * for 11ac so that bit 11 is for GN, bit 12 for AN, bit 13 for GAC, and bit
- * bit 14 for AAC, in order to be compatible with the band capability
- * defined in the driver after right shift of 8 bits.
+/* bit 13: 11ac BAND_AAC
+ * bit 12: reserved for lab testing, will be reused for BAND_AN
+ * bit 11: 11n  BAND_GN
+ * bit 10: 11a  BAND_A
+ * bit 9: 11g   BAND_G
+ * bit 8: 11b   BAND_B
+ * Map these bits to band capability by right shifting 8 bits.
  */
 #define GET_FW_DEFAULT_BANDS(adapter)  \
-	    (((((adapter->fw_cap_info & 0x3000) << 1) | \
-	       (adapter->fw_cap_info & ~0xF000)) >> 8) & \
+	    (((adapter->fw_cap_info & 0x2f00) >> 8) & \
 	     ALL_802_11_BANDS)
 
 #define HostCmd_WEP_KEY_INDEX_MASK              0x3fff
@@ -77,12 +79,21 @@ enum KEY_TYPE_ID {
 	KEY_TYPE_ID_WAPI,
 	KEY_TYPE_ID_AES_CMAC,
 };
+
+#define WPA_PN_SIZE		8
+#define KEY_PARAMS_FIXED_LEN	10
+#define KEY_INDEX_MASK		0xf
+#define FW_KEY_API_VER_MAJOR_V2	2
+
 #define KEY_MCAST	BIT(0)
 #define KEY_UNICAST	BIT(1)
 #define KEY_ENABLED	BIT(2)
+#define KEY_DEFAULT	BIT(3)
+#define KEY_TX_KEY	BIT(4)
+#define KEY_RX_KEY	BIT(5)
 #define KEY_IGTK	BIT(10)
 
-#define WAPI_KEY_LEN			50
+#define WAPI_KEY_LEN			(WLAN_KEY_LEN_SMS4 + PN_LEN + 2)
 
 #define MAX_POLL_TRIES			100
 #define MAX_FIRMWARE_POLL_TRIES			100
@@ -130,6 +141,7 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 #define TLV_TYPE_RSSI_HIGH          (PROPRIETARY_TLV_BASE_ID + 22)
 #define TLV_TYPE_AUTH_TYPE          (PROPRIETARY_TLV_BASE_ID + 31)
 #define TLV_TYPE_STA_MAC_ADDR       (PROPRIETARY_TLV_BASE_ID + 32)
+#define TLV_TYPE_BSSID              (PROPRIETARY_TLV_BASE_ID + 35)
 #define TLV_TYPE_CHANNELBANDLIST    (PROPRIETARY_TLV_BASE_ID + 42)
 #define TLV_TYPE_UAP_BEACON_PERIOD  (PROPRIETARY_TLV_BASE_ID + 44)
 #define TLV_TYPE_UAP_DTIM_PERIOD    (PROPRIETARY_TLV_BASE_ID + 45)
@@ -144,6 +156,8 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 #define TLV_TYPE_RATE_DROP_CONTROL  (PROPRIETARY_TLV_BASE_ID + 82)
 #define TLV_TYPE_RATE_SCOPE         (PROPRIETARY_TLV_BASE_ID + 83)
 #define TLV_TYPE_POWER_GROUP        (PROPRIETARY_TLV_BASE_ID + 84)
+#define TLV_TYPE_BSS_SCAN_RSP       (PROPRIETARY_TLV_BASE_ID + 86)
+#define TLV_TYPE_BSS_SCAN_INFO      (PROPRIETARY_TLV_BASE_ID + 87)
 #define TLV_TYPE_UAP_RETRY_LIMIT    (PROPRIETARY_TLV_BASE_ID + 93)
 #define TLV_TYPE_WAPI_IE            (PROPRIETARY_TLV_BASE_ID + 94)
 #define TLV_TYPE_UAP_MGMT_FRAME     (PROPRIETARY_TLV_BASE_ID + 104)
@@ -154,6 +168,8 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 #define TLV_TYPE_PWK_CIPHER         (PROPRIETARY_TLV_BASE_ID + 145)
 #define TLV_TYPE_GWK_CIPHER         (PROPRIETARY_TLV_BASE_ID + 146)
 #define TLV_TYPE_COALESCE_RULE      (PROPRIETARY_TLV_BASE_ID + 154)
+#define TLV_TYPE_KEY_PARAM_V2       (PROPRIETARY_TLV_BASE_ID + 156)
+#define TLV_TYPE_FW_API_REV         (PROPRIETARY_TLV_BASE_ID + 199)
 
 #define MWIFIEX_TX_DATA_BUF_SIZE_2K        2048
 
@@ -176,11 +192,14 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 #define MWIFIEX_TX_DATA_BUF_SIZE_8K        8192
 
 #define ISSUPP_11NENABLED(FwCapInfo) (FwCapInfo & BIT(11))
+#define ISSUPP_TDLS_ENABLED(FwCapInfo) (FwCapInfo & BIT(14))
 
 #define MWIFIEX_DEF_HT_CAP	(IEEE80211_HT_CAP_DSSSCCK40 | \
 				 (1 << IEEE80211_HT_CAP_RX_STBC_SHIFT) | \
 				 IEEE80211_HT_CAP_SM_PS)
 
+#define MWIFIEX_DEF_11N_TX_BF_CAP	0x09E1E008
+
 #define MWIFIEX_DEF_AMPDU	IEEE80211_HT_AMPDU_PARM_FACTOR
 
 /* dev_cap bitmap
@@ -204,6 +223,7 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 #define ISSUPP_GREENFIELD(Dot11nDevCap) (Dot11nDevCap & BIT(29))
 #define ISENABLED_40MHZ_INTOLERANT(Dot11nDevCap) (Dot11nDevCap & BIT(8))
 #define ISSUPP_RXLDPC(Dot11nDevCap) (Dot11nDevCap & BIT(22))
+#define ISSUPP_BEAMFORMING(Dot11nDevCap) (Dot11nDevCap & BIT(30))
 
 /* httxcfg bitmap
  * 0		reserved
@@ -216,8 +236,21 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
  */
 #define MWIFIEX_FW_DEF_HTTXCFG (BIT(1) | BIT(4) | BIT(5) | BIT(6))
 
+/* 11AC Tx and Rx MCS map for 1x1 mode:
+ * IEEE80211_VHT_MCS_SUPPORT_0_9 for stream 1
+ * IEEE80211_VHT_MCS_NOT_SUPPORTED for remaining 7 streams
+ */
+#define MWIFIEX_11AC_MCS_MAP_1X1	0xfffefffe
+
+/* 11AC Tx and Rx MCS map for 2x2 mode:
+ * IEEE80211_VHT_MCS_SUPPORT_0_9 for stream 1 and 2
+ * IEEE80211_VHT_MCS_NOT_SUPPORTED for remaining 6 streams
+ */
+#define MWIFIEX_11AC_MCS_MAP_2X2	0xfffafffa
+
 #define GET_RXMCSSUPP(DevMCSSupported) (DevMCSSupported & 0x0f)
 #define SETHT_MCS32(x) (x[4] |= 1)
+#define HT_STREAM_1X1	0x11
 #define HT_STREAM_2X2	0x22
 
 #define SET_SECONDARYCHAN(RadioType, SECCHAN) (RadioType |= (SECCHAN << 4))
@@ -226,17 +259,24 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 
 /* HW_SPEC fw_cap_info */
 
-#define ISSUPP_11ACENABLED(fw_cap_info) (fw_cap_info & (BIT(12)|BIT(13)))
+#define ISSUPP_11ACENABLED(fw_cap_info) (fw_cap_info & BIT(13))
 
 #define GET_VHTCAP_CHWDSET(vht_cap_info)    ((vht_cap_info >> 2) & 0x3)
 #define GET_VHTNSSMCS(mcs_mapset, nss) ((mcs_mapset >> (2 * (nss - 1))) & 0x3)
 #define SET_VHTNSSMCS(mcs_mapset, nss, value) (mcs_mapset |= (value & 0x3) << \
 					      (2 * (nss - 1)))
-#define NO_NSS_SUPPORT		0x3
-
 #define GET_DEVTXMCSMAP(dev_mcs_map)      (dev_mcs_map >> 16)
 #define GET_DEVRXMCSMAP(dev_mcs_map)      (dev_mcs_map & 0xFFFF)
 
+/* Clear SU Beanformer, MU beanformer, MU beanformee and
+ * sounding dimensions bits
+ */
+#define MWIFIEX_DEF_11AC_CAP_BF_RESET_MASK \
+			(IEEE80211_VHT_CAP_SU_BEAMFORMER_CAPABLE | \
+			 IEEE80211_VHT_CAP_MU_BEAMFORMER_CAPABLE | \
+			 IEEE80211_VHT_CAP_MU_BEAMFORMEE_CAPABLE | \
+			 IEEE80211_VHT_CAP_SOUNDING_DIMENSIONS_MASK)
+
 #define MOD_CLASS_HR_DSSS       0x03
 #define MOD_CLASS_OFDM          0x07
 #define MOD_CLASS_HT            0x08
@@ -295,10 +335,12 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 #define HostCmd_CMD_CAU_REG_ACCESS                    0x00ed
 #define HostCmd_CMD_SET_BSS_MODE                      0x00f7
 #define HostCmd_CMD_PCIE_DESC_DETAILS                 0x00fa
+#define HostCmd_CMD_802_11_SCAN_EXT                   0x0107
 #define HostCmd_CMD_COALESCE_CFG                      0x010a
 #define HostCmd_CMD_MGMT_FRAME_REG                    0x010c
 #define HostCmd_CMD_REMAIN_ON_CHAN                    0x010d
 #define HostCmd_CMD_11AC_CFG			      0x0112
+#define HostCmd_CMD_TDLS_OPER                         0x0122
 
 #define PROTOCOL_NO_SECURITY        0x01
 #define PROTOCOL_STATIC_WEP         0x02
@@ -440,6 +482,7 @@ enum P2P_MODES {
 #define EVENT_UAP_MIC_COUNTERMEASURES   0x0000004c
 #define EVENT_HOSTWAKE_STAIE		0x0000004d
 #define EVENT_CHANNEL_SWITCH_ANN        0x00000050
+#define EVENT_EXT_SCAN_REPORT           0x00000058
 #define EVENT_REMAIN_ON_CHAN_EXPIRED    0x0000005f
 
 #define EVENT_ID_MASK                   0xffff
@@ -468,6 +511,10 @@ enum P2P_MODES {
 #define MWIFIEX_CRITERIA_UNICAST	BIT(1)
 #define MWIFIEX_CRITERIA_MULTICAST	BIT(3)
 
+#define ACT_TDLS_DELETE            0x00
+#define ACT_TDLS_CREATE            0x01
+#define ACT_TDLS_CONFIG            0x02
+
 struct mwifiex_ie_types_header {
 	__le16 type;
 	__le16 len;
@@ -480,6 +527,7 @@ struct mwifiex_ie_types_data {
 
 #define MWIFIEX_TxPD_POWER_MGMT_NULL_PACKET 0x01
 #define MWIFIEX_TxPD_POWER_MGMT_LAST_PACKET 0x08
+#define MWIFIEX_TXPD_FLAGS_TDLS_PACKET      0x10
 
 struct txpd {
 	u8 bss_type;
@@ -676,6 +724,56 @@ struct mwifiex_cmac_param {
 	u8 key[WLAN_KEY_LEN_AES_CMAC];
 } __packed;
 
+struct mwifiex_wep_param {
+	__le16 key_len;
+	u8 key[WLAN_KEY_LEN_WEP104];
+} __packed;
+
+struct mwifiex_tkip_param {
+	u8 pn[WPA_PN_SIZE];
+	__le16 key_len;
+	u8 key[WLAN_KEY_LEN_TKIP];
+} __packed;
+
+struct mwifiex_aes_param {
+	u8 pn[WPA_PN_SIZE];
+	__le16 key_len;
+	u8 key[WLAN_KEY_LEN_CCMP];
+} __packed;
+
+struct mwifiex_wapi_param {
+	u8 pn[PN_LEN];
+	__le16 key_len;
+	u8 key[WLAN_KEY_LEN_SMS4];
+} __packed;
+
+struct mwifiex_cmac_aes_param {
+	u8 ipn[IGTK_PN_LEN];
+	__le16 key_len;
+	u8 key[WLAN_KEY_LEN_AES_CMAC];
+} __packed;
+
+struct mwifiex_ie_type_key_param_set_v2 {
+	__le16 type;
+	__le16 len;
+	u8 mac_addr[ETH_ALEN];
+	u8 key_idx;
+	u8 key_type;
+	__le16 key_info;
+	union {
+		struct mwifiex_wep_param wep;
+		struct mwifiex_tkip_param tkip;
+		struct mwifiex_aes_param aes;
+		struct mwifiex_wapi_param wapi;
+		struct mwifiex_cmac_aes_param cmac_aes;
+	} key_params;
+} __packed;
+
+struct host_cmd_ds_802_11_key_material_v2 {
+	__le16 action;
+	struct mwifiex_ie_type_key_param_set_v2 key_param_set;
+} __packed;
+
 struct host_cmd_ds_802_11_key_material {
 	__le16 action;
 	struct mwifiex_ie_type_key_param_set key_param_set;
@@ -727,6 +825,17 @@ struct host_cmd_ds_802_11_ps_mode_enh {
 	} params;
 } __packed;
 
+enum FW_API_VER_ID {
+	KEY_API_VER_ID = 1,
+};
+
+struct hw_spec_fw_api_rev {
+	struct mwifiex_ie_types_header header;
+	__le16 api_id;
+	u8 major_ver;
+	u8 minor_ver;
+} __packed;
+
 struct host_cmd_ds_get_hw_spec {
 	__le16 hw_if_version;
 	__le16 version;
@@ -748,6 +857,7 @@ struct host_cmd_ds_get_hw_spec {
 	__le32 reserved_6;
 	__le32 dot_11ac_dev_cap;
 	__le32 dot_11ac_mcs_support;
+	u8 tlvs[0];
 } __packed;
 
 struct host_cmd_ds_802_11_rssi_info {
@@ -1047,14 +1157,28 @@ struct host_cmd_ds_rf_ant_siso {
 	__le16 ant_mode;
 };
 
-struct mwifiex_bcn_param {
-	u8 bssid[ETH_ALEN];
-	u8 rssi;
+struct host_cmd_ds_tdls_oper {
+	__le16 tdls_action;
+	__le16 reason;
+	u8 peer_mac[ETH_ALEN];
+} __packed;
+
+struct mwifiex_fixed_bcn_param {
 	__le64 timestamp;
 	__le16 beacon_period;
 	__le16 cap_info_bitmap;
 } __packed;
 
+struct mwifiex_event_scan_result {
+	__le16 event_id;
+	u8 bss_index;
+	u8 bss_type;
+	u8 more_event;
+	u8 reserved[3];
+	__le16 buf_size;
+	u8 num_of_set;
+} __packed;
+
 #define MWIFIEX_USER_SCAN_CHAN_MAX             50
 
 #define MWIFIEX_MAX_SSID_LIST_LENGTH         10
@@ -1124,6 +1248,28 @@ struct host_cmd_ds_802_11_scan_rsp {
 	u8 bss_desc_and_tlv_buffer[1];
 } __packed;
 
+struct host_cmd_ds_802_11_scan_ext {
+	u32   reserved;
+	u8    tlv_buffer[1];
+} __packed;
+
+struct mwifiex_ie_types_bss_scan_rsp {
+	struct mwifiex_ie_types_header header;
+	u8 bssid[ETH_ALEN];
+	u8 frame_body[1];
+} __packed;
+
+struct mwifiex_ie_types_bss_scan_info {
+	struct mwifiex_ie_types_header header;
+	__le16 rssi;
+	__le16 anpi;
+	u8 cca_busy_fraction;
+	u8 radio_type;
+	u8 channel;
+	u8 reserved;
+	__le64 tsf;
+} __packed;
+
 struct host_cmd_ds_802_11_bg_scan_query {
 	u8 flush;
 } __packed;
@@ -1296,6 +1442,11 @@ struct mwifiex_ie_types_vhtcap {
 	struct ieee80211_vht_cap vht_cap;
 } __packed;
 
+struct mwifiex_ie_types_aid {
+	struct mwifiex_ie_types_header header;
+	__le16 aid;
+} __packed;
+
 struct mwifiex_ie_types_oper_mode_ntf {
 	struct mwifiex_ie_types_header header;
 	u8 oper_mode;
@@ -1331,6 +1482,11 @@ struct mwifiex_ie_types_extcap {
 	u8 ext_capab[0];
 } __packed;
 
+struct mwifiex_ie_types_qos_info {
+	struct mwifiex_ie_types_header header;
+	u8 qos_info;
+} __packed;
+
 struct host_cmd_ds_mac_reg_access {
 	__le16 action;
 	__le16 offset;
@@ -1441,6 +1597,11 @@ struct host_cmd_tlv_rates {
 	u8 rates[0];
 } __packed;
 
+struct mwifiex_ie_types_bssid_list {
+	struct mwifiex_ie_types_header header;
+	u8 bssid[ETH_ALEN];
+} __packed;
+
 struct host_cmd_tlv_bcast_ssid {
 	struct mwifiex_ie_types_header header;
 	u8 bcast_ctl;
@@ -1634,6 +1795,7 @@ struct host_cmd_ds_command {
 		struct host_cmd_ds_802_11_ps_mode_enh psmode_enh;
 		struct host_cmd_ds_802_11_hs_cfg_enh opt_hs_cfg;
 		struct host_cmd_ds_802_11_scan scan;
+		struct host_cmd_ds_802_11_scan_ext ext_scan;
 		struct host_cmd_ds_802_11_scan_rsp scan_resp;
 		struct host_cmd_ds_802_11_bg_scan_query bg_scan_query;
 		struct host_cmd_ds_802_11_bg_scan_query_rsp bg_scan_query_resp;
@@ -1653,6 +1815,7 @@ struct host_cmd_ds_command {
 		struct host_cmd_ds_11n_cfg htcfg;
 		struct host_cmd_ds_wmm_get_status get_wmm_status;
 		struct host_cmd_ds_802_11_key_material key_material;
+		struct host_cmd_ds_802_11_key_material_v2 key_material_v2;
 		struct host_cmd_ds_version_ext verext;
 		struct host_cmd_ds_mgmt_frame_reg reg_mask;
 		struct host_cmd_ds_remain_on_chan roc_cfg;
@@ -1671,6 +1834,7 @@ struct host_cmd_ds_command {
 		struct host_cmd_ds_sta_deauth sta_deauth;
 		struct host_cmd_11ac_vht_cfg vht_cfg;
 		struct host_cmd_ds_coalesce_cfg coalesce_cfg;
+		struct host_cmd_ds_tdls_oper tdls_oper;
 	} params;
 } __packed;
 
diff --git a/drivers/net/wireless/mwifiex/ie.c b/drivers/net/wireless/mwifiex/ie.c
index 81ac001ee741..3bf3d58bbc02 100644
--- a/drivers/net/wireless/mwifiex/ie.c
+++ b/drivers/net/wireless/mwifiex/ie.c
@@ -138,9 +138,9 @@ mwifiex_update_autoindex_ies(struct mwifiex_private *priv,
 	}
 
 	if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_UAP)
-		return mwifiex_send_cmd_async(priv, HostCmd_CMD_UAP_SYS_CONFIG,
-					      HostCmd_ACT_GEN_SET,
-					      UAP_CUSTOM_IE_I, ie_list);
+		return mwifiex_send_cmd(priv, HostCmd_CMD_UAP_SYS_CONFIG,
+					HostCmd_ACT_GEN_SET,
+					UAP_CUSTOM_IE_I, ie_list, false);
 
 	return 0;
 }
diff --git a/drivers/net/wireless/mwifiex/init.c b/drivers/net/wireless/mwifiex/init.c
index 1d0a817f2bf0..4ecd0b208ac6 100644
--- a/drivers/net/wireless/mwifiex/init.c
+++ b/drivers/net/wireless/mwifiex/init.c
@@ -137,6 +137,7 @@ int mwifiex_init_priv(struct mwifiex_private *priv)
 	priv->csa_expire_time = 0;
 	priv->del_list_idx = 0;
 	priv->hs2_enabled = false;
+	memcpy(priv->tos_to_tid_inv, tos_to_tid_inv, MAX_NUM_TID);
 
 	return mwifiex_add_bss_prio_tbl(priv);
 }
@@ -233,7 +234,6 @@ static void mwifiex_init_adapter(struct mwifiex_adapter *adapter)
 
 	adapter->pm_wakeup_fw_try = false;
 
-	adapter->tx_buf_size = MWIFIEX_TX_DATA_BUF_SIZE_2K;
 	adapter->curr_tx_buf_size = MWIFIEX_TX_DATA_BUF_SIZE_2K;
 
 	adapter->is_hs_configured = false;
@@ -281,6 +281,9 @@ static void mwifiex_init_adapter(struct mwifiex_adapter *adapter)
 	adapter->arp_filter_size = 0;
 	adapter->max_mgmt_ie_index = MAX_MGMT_IE_INDEX;
 	adapter->empty_tx_q_cnt = 0;
+	adapter->ext_scan = true;
+	adapter->fw_key_api_major_ver = 0;
+	adapter->fw_key_api_minor_ver = 0;
 }
 
 /*
@@ -450,6 +453,7 @@ int mwifiex_init_lock_list(struct mwifiex_adapter *adapter)
 		INIT_LIST_HEAD(&priv->tx_ba_stream_tbl_ptr);
 		INIT_LIST_HEAD(&priv->rx_reorder_tbl_ptr);
 		INIT_LIST_HEAD(&priv->sta_list);
+		skb_queue_head_init(&priv->tdls_txq);
 
 		spin_lock_init(&priv->tx_ba_stream_tbl_lock);
 		spin_lock_init(&priv->rx_reorder_tbl_lock);
@@ -615,7 +619,7 @@ mwifiex_shutdown_drv(struct mwifiex_adapter *adapter)
 	/* cancel current command */
 	if (adapter->curr_cmd) {
 		dev_warn(adapter->dev, "curr_cmd is still in processing\n");
-		del_timer(&adapter->cmd_timer);
+		del_timer_sync(&adapter->cmd_timer);
 		mwifiex_recycle_cmd_node(adapter, adapter->curr_cmd);
 		adapter->curr_cmd = NULL;
 	}
diff --git a/drivers/net/wireless/mwifiex/ioctl.h b/drivers/net/wireless/mwifiex/ioctl.h
index 00a95f4c6a6c..1fb2212079ae 100644
--- a/drivers/net/wireless/mwifiex/ioctl.h
+++ b/drivers/net/wireless/mwifiex/ioctl.h
@@ -60,8 +60,7 @@ enum {
 	BAND_A = 4,
 	BAND_GN = 8,
 	BAND_AN = 16,
-	BAND_GAC = 32,
-	BAND_AAC = 64,
+	BAND_AAC = 32,
 };
 
 #define MWIFIEX_WPA_PASSHPHRASE_LEN 64
@@ -86,6 +85,10 @@ struct wep_key {
 #define BAND_CONFIG_A           0x01
 #define MWIFIEX_SUPPORTED_RATES                 14
 #define MWIFIEX_SUPPORTED_RATES_EXT             32
+#define MWIFIEX_TDLS_SUPPORTED_RATES		8
+#define MWIFIEX_TDLS_DEF_QOS_CAPAB		0xf
+#define MWIFIEX_PRIO_BK				2
+#define MWIFIEX_PRIO_VI				5
 
 struct mwifiex_uap_bss_param {
 	u8 channel;
@@ -206,7 +209,7 @@ struct mwifiex_debug_info {
 	u32 num_cmd_assoc_success;
 	u32 num_cmd_assoc_failure;
 	u32 num_tx_timeout;
-	u32 num_cmd_timeout;
+	u8 is_cmd_timedout;
 	u16 timeout_cmd_id;
 	u16 timeout_cmd_act;
 	u16 last_cmd_id[DBG_CMD_NUM];
@@ -233,7 +236,10 @@ struct mwifiex_ds_encrypt_key {
 	u8 mac_addr[ETH_ALEN];
 	u32 is_wapi_key;
 	u8 pn[PN_LEN];		/* packet number */
+	u8 pn_len;
 	u8 is_igtk_key;
+	u8 is_current_wep_key;
+	u8 is_rx_seq_valid;
 };
 
 struct mwifiex_power_cfg {
@@ -432,4 +438,16 @@ struct mwifiex_ds_coalesce_cfg {
 	struct mwifiex_coalesce_rule rule[MWIFIEX_COALESCE_MAX_RULES];
 };
 
+struct mwifiex_ds_tdls_oper {
+	u16 tdls_action;
+	u8 peer_mac[ETH_ALEN];
+	u16 capability;
+	u8 qos_info;
+	u8 *ext_capab;
+	u8 ext_capab_len;
+	u8 *supp_rates;
+	u8 supp_rates_len;
+	u8 *ht_capab;
+};
+
 #endif /* !_MWIFIEX_IOCTL_H_ */
diff --git a/drivers/net/wireless/mwifiex/join.c b/drivers/net/wireless/mwifiex/join.c
index 4e4686e6ac09..89dc62a467f4 100644
--- a/drivers/net/wireless/mwifiex/join.c
+++ b/drivers/net/wireless/mwifiex/join.c
@@ -515,8 +515,7 @@ int mwifiex_cmd_802_11_associate(struct mwifiex_private *priv,
 
 	if (ISSUPP_11ACENABLED(priv->adapter->fw_cap_info) &&
 	    !bss_desc->disable_11n && !bss_desc->disable_11ac &&
-	    (priv->adapter->config_bands & BAND_GAC ||
-	     priv->adapter->config_bands & BAND_AAC))
+	    priv->adapter->config_bands & BAND_AAC)
 		mwifiex_cmd_append_11ac_tlv(priv, bss_desc, &pos);
 
 	/* Append vendor specific IE TLV */
@@ -902,9 +901,9 @@ mwifiex_cmd_802_11_ad_hoc_start(struct mwifiex_private *priv,
 	mwifiex_get_active_data_rates(priv, adhoc_start->data_rate);
 	if ((adapter->adhoc_start_band & BAND_G) &&
 	    (priv->curr_pkt_filter & HostCmd_ACT_MAC_ADHOC_G_PROTECTION_ON)) {
-		if (mwifiex_send_cmd_async(priv, HostCmd_CMD_MAC_CONTROL,
-					   HostCmd_ACT_GEN_SET, 0,
-					   &priv->curr_pkt_filter)) {
+		if (mwifiex_send_cmd(priv, HostCmd_CMD_MAC_CONTROL,
+				     HostCmd_ACT_GEN_SET, 0,
+				     &priv->curr_pkt_filter, false)) {
 			dev_err(adapter->dev,
 				"ADHOC_S_CMD: G Protection config failed\n");
 			return -1;
@@ -983,7 +982,7 @@ mwifiex_cmd_802_11_ad_hoc_start(struct mwifiex_private *priv,
 		       cpu_to_le16(sizeof(struct ieee80211_ht_cap));
 		radio_type = mwifiex_band_to_radio_type(
 					priv->adapter->config_bands);
-		mwifiex_fill_cap_info(priv, radio_type, ht_cap);
+		mwifiex_fill_cap_info(priv, radio_type, &ht_cap->ht_cap);
 
 		if (adapter->sec_chan_offset ==
 					IEEE80211_HT_PARAM_CHA_SEC_NONE) {
@@ -1074,9 +1073,9 @@ mwifiex_cmd_802_11_ad_hoc_join(struct mwifiex_private *priv,
 			priv->
 			curr_pkt_filter | HostCmd_ACT_MAC_ADHOC_G_PROTECTION_ON;
 
-		if (mwifiex_send_cmd_async(priv, HostCmd_CMD_MAC_CONTROL,
-					   HostCmd_ACT_GEN_SET, 0,
-					   &curr_pkt_filter)) {
+		if (mwifiex_send_cmd(priv, HostCmd_CMD_MAC_CONTROL,
+				     HostCmd_ACT_GEN_SET, 0,
+				     &curr_pkt_filter, false)) {
 			dev_err(priv->adapter->dev,
 				"ADHOC_J_CMD: G Protection config failed\n");
 			return -1;
@@ -1300,8 +1299,7 @@ int mwifiex_associate(struct mwifiex_private *priv,
 
 	if (ISSUPP_11ACENABLED(priv->adapter->fw_cap_info) &&
 	    !bss_desc->disable_11n && !bss_desc->disable_11ac &&
-	    (priv->adapter->config_bands & BAND_GAC ||
-	     priv->adapter->config_bands & BAND_AAC))
+	    priv->adapter->config_bands & BAND_AAC)
 		mwifiex_set_11ac_ba_params(priv);
 	else
 		mwifiex_set_ba_params(priv);
@@ -1314,8 +1312,8 @@ int mwifiex_associate(struct mwifiex_private *priv,
 	   retrieval */
 	priv->assoc_rsp_size = 0;
 
-	return mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_ASSOCIATE,
-				    HostCmd_ACT_GEN_SET, 0, bss_desc);
+	return mwifiex_send_cmd(priv, HostCmd_CMD_802_11_ASSOCIATE,
+				HostCmd_ACT_GEN_SET, 0, bss_desc, true);
 }
 
 /*
@@ -1335,14 +1333,13 @@ mwifiex_adhoc_start(struct mwifiex_private *priv,
 		priv->curr_bss_params.band);
 
 	if (ISSUPP_11ACENABLED(priv->adapter->fw_cap_info) &&
-	    (priv->adapter->config_bands & BAND_GAC ||
-	     priv->adapter->config_bands & BAND_AAC))
+	    priv->adapter->config_bands & BAND_AAC)
 		mwifiex_set_11ac_ba_params(priv);
 	else
 		mwifiex_set_ba_params(priv);
 
-	return mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_AD_HOC_START,
-				    HostCmd_ACT_GEN_SET, 0, adhoc_ssid);
+	return mwifiex_send_cmd(priv, HostCmd_CMD_802_11_AD_HOC_START,
+				HostCmd_ACT_GEN_SET, 0, adhoc_ssid, true);
 }
 
 /*
@@ -1376,8 +1373,7 @@ int mwifiex_adhoc_join(struct mwifiex_private *priv,
 
 	if (ISSUPP_11ACENABLED(priv->adapter->fw_cap_info) &&
 	    !bss_desc->disable_11n && !bss_desc->disable_11ac &&
-	    (priv->adapter->config_bands & BAND_GAC ||
-	     priv->adapter->config_bands & BAND_AAC))
+	    priv->adapter->config_bands & BAND_AAC)
 		mwifiex_set_11ac_ba_params(priv);
 	else
 		mwifiex_set_ba_params(priv);
@@ -1387,8 +1383,8 @@ int mwifiex_adhoc_join(struct mwifiex_private *priv,
 	dev_dbg(priv->adapter->dev, "info: curr_bss_params.band = %c\n",
 		priv->curr_bss_params.band);
 
-	return mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_AD_HOC_JOIN,
-				    HostCmd_ACT_GEN_SET, 0, bss_desc);
+	return mwifiex_send_cmd(priv, HostCmd_CMD_802_11_AD_HOC_JOIN,
+				HostCmd_ACT_GEN_SET, 0, bss_desc, true);
 }
 
 /*
@@ -1407,8 +1403,8 @@ static int mwifiex_deauthenticate_infra(struct mwifiex_private *priv, u8 *mac)
 	else
 		memcpy(mac_address, mac, ETH_ALEN);
 
-	ret = mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_DEAUTHENTICATE,
-				    HostCmd_ACT_GEN_SET, 0, mac_address);
+	ret = mwifiex_send_cmd(priv, HostCmd_CMD_802_11_DEAUTHENTICATE,
+			       HostCmd_ACT_GEN_SET, 0, mac_address, true);
 
 	return ret;
 }
@@ -1436,19 +1432,31 @@ int mwifiex_deauthenticate(struct mwifiex_private *priv, u8 *mac)
 					      GFP_KERNEL);
 		break;
 	case NL80211_IFTYPE_ADHOC:
-		return mwifiex_send_cmd_sync(priv,
-					     HostCmd_CMD_802_11_AD_HOC_STOP,
-					     HostCmd_ACT_GEN_SET, 0, NULL);
+		return mwifiex_send_cmd(priv, HostCmd_CMD_802_11_AD_HOC_STOP,
+					HostCmd_ACT_GEN_SET, 0, NULL, true);
 	case NL80211_IFTYPE_AP:
-		return mwifiex_send_cmd_sync(priv, HostCmd_CMD_UAP_BSS_STOP,
-					     HostCmd_ACT_GEN_SET, 0, NULL);
+		return mwifiex_send_cmd(priv, HostCmd_CMD_UAP_BSS_STOP,
+					HostCmd_ACT_GEN_SET, 0, NULL, true);
 	default:
 		break;
 	}
 
 	return ret;
 }
-EXPORT_SYMBOL_GPL(mwifiex_deauthenticate);
+
+/* This function deauthenticates/disconnects from all BSS. */
+void mwifiex_deauthenticate_all(struct mwifiex_adapter *adapter)
+{
+	struct mwifiex_private *priv;
+	int i;
+
+	for (i = 0; i < adapter->priv_num; i++) {
+		priv = adapter->priv[i];
+		if (priv)
+			mwifiex_deauthenticate(priv, NULL);
+	}
+}
+EXPORT_SYMBOL_GPL(mwifiex_deauthenticate_all);
 
 /*
  * This function converts band to radio type used in channel TLV.
diff --git a/drivers/net/wireless/mwifiex/main.c b/drivers/net/wireless/mwifiex/main.c
index 4d79761b9c87..5397ee0ad652 100644
--- a/drivers/net/wireless/mwifiex/main.c
+++ b/drivers/net/wireless/mwifiex/main.c
@@ -194,7 +194,7 @@ static int mwifiex_unregister(struct mwifiex_adapter *adapter)
 	if (adapter->if_ops.cleanup_if)
 		adapter->if_ops.cleanup_if(adapter);
 
-	del_timer(&adapter->cmd_timer);
+	del_timer_sync(&adapter->cmd_timer);
 
 	/* Free private structures */
 	for (i = 0; i < adapter->priv_num; i++) {
@@ -678,8 +678,8 @@ mwifiex_set_mac_address(struct net_device *dev, void *addr)
 	memcpy(priv->curr_addr, hw_addr->sa_data, ETH_ALEN);
 
 	/* Send request to firmware */
-	ret = mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_MAC_ADDRESS,
-				    HostCmd_ACT_GEN_SET, 0, NULL);
+	ret = mwifiex_send_cmd(priv, HostCmd_CMD_802_11_MAC_ADDRESS,
+			       HostCmd_ACT_GEN_SET, 0, NULL, true);
 
 	if (!ret)
 		memcpy(priv->netdev->dev_addr, priv->curr_addr, ETH_ALEN);
@@ -871,7 +871,6 @@ mwifiex_add_card(void *card, struct semaphore *sem,
 	adapter->is_suspended = false;
 	adapter->hs_activated = false;
 	init_waitqueue_head(&adapter->hs_activate_wait_q);
-	adapter->cmd_wait_q_required = false;
 	init_waitqueue_head(&adapter->cmd_wait_q.wait);
 	adapter->cmd_wait_q.status = 0;
 	adapter->scan_wait_q_woken = false;
diff --git a/drivers/net/wireless/mwifiex/main.h b/drivers/net/wireless/mwifiex/main.h
index d8ad554ce39f..f0289c12e041 100644
--- a/drivers/net/wireless/mwifiex/main.h
+++ b/drivers/net/wireless/mwifiex/main.h
@@ -59,7 +59,7 @@ enum {
 
 #define MWIFIEX_UPLD_SIZE               (2312)
 
-#define MAX_EVENT_SIZE                  1024
+#define MAX_EVENT_SIZE                  2048
 
 #define ARP_FILTER_MAX_BUF_SIZE         68
 
@@ -145,7 +145,6 @@ struct mwifiex_dbg {
 	u32 num_cmd_assoc_success;
 	u32 num_cmd_assoc_failure;
 	u32 num_tx_timeout;
-	u32 num_cmd_timeout;
 	u16 timeout_cmd_id;
 	u16 timeout_cmd_act;
 	u16 last_cmd_id[DBG_CMD_NUM];
@@ -210,6 +209,7 @@ struct mwifiex_ra_list_tbl {
 	u16 ba_pkt_count;
 	u8 ba_packet_thr;
 	u16 total_pkt_count;
+	bool tdls_link;
 };
 
 struct mwifiex_tid_tbl {
@@ -262,6 +262,31 @@ struct ieee_types_generic {
 	u8 data[IEEE_MAX_IE_SIZE - sizeof(struct ieee_types_header)];
 } __packed;
 
+struct ieee_types_bss_co_2040 {
+	struct ieee_types_header ieee_hdr;
+	u8 bss_2040co;
+} __packed;
+
+struct ieee_types_extcap {
+	struct ieee_types_header ieee_hdr;
+	u8 ext_capab[8];
+} __packed;
+
+struct ieee_types_vht_cap {
+	struct ieee_types_header ieee_hdr;
+	struct ieee80211_vht_cap vhtcap;
+} __packed;
+
+struct ieee_types_vht_oper {
+	struct ieee_types_header ieee_hdr;
+	struct ieee80211_vht_operation vhtoper;
+} __packed;
+
+struct ieee_types_aid {
+	struct ieee_types_header ieee_hdr;
+	u16 aid;
+} __packed;
+
 struct mwifiex_bssdescriptor {
 	u8 mac_address[ETH_ALEN];
 	struct cfg80211_ssid ssid;
@@ -443,6 +468,7 @@ struct mwifiex_private {
 	u8 wpa_ie_len;
 	u8 wpa_is_gtk_set;
 	struct host_cmd_ds_802_11_key_material aes_key;
+	struct host_cmd_ds_802_11_key_material_v2 aes_key_v2;
 	u8 wapi_ie[256];
 	u8 wapi_ie_len;
 	u8 *wps_ie;
@@ -461,6 +487,7 @@ struct mwifiex_private {
 	struct mwifiex_tx_aggr aggr_prio_tbl[MAX_NUM_TID];
 	struct mwifiex_add_ba_param add_ba_param;
 	u16 rx_seq[MAX_NUM_TID];
+	u8 tos_to_tid_inv[MAX_NUM_TID];
 	struct list_head rx_reorder_tbl_ptr;
 	/* spin lock for rx_reorder_tbl_ptr queue */
 	spinlock_t rx_reorder_tbl_lock;
@@ -518,6 +545,8 @@ struct mwifiex_private {
 	unsigned long csa_expire_time;
 	u8 del_list_idx;
 	bool hs2_enabled;
+	struct station_parameters *sta_params;
+	struct sk_buff_head tdls_txq;
 };
 
 enum mwifiex_ba_status {
@@ -545,6 +574,7 @@ struct mwifiex_rx_reorder_tbl {
 	struct list_head list;
 	int tid;
 	u8 ta[ETH_ALEN];
+	int init_win;
 	int start_win;
 	int win_size;
 	void **rx_reorder_ptr;
@@ -583,17 +613,35 @@ struct mwifiex_bss_priv {
 	u64 fw_tsf;
 };
 
-/* This is AP specific structure which stores information
- * about associated STA
+struct mwifiex_tdls_capab {
+	__le16 capab;
+	u8 rates[32];
+	u8 rates_len;
+	u8 qos_info;
+	u8 coex_2040;
+	u16 aid;
+	struct ieee80211_ht_cap ht_capb;
+	struct ieee80211_ht_operation ht_oper;
+	struct ieee_types_extcap extcap;
+	struct ieee_types_generic rsn_ie;
+	struct ieee80211_vht_cap vhtcap;
+	struct ieee80211_vht_operation vhtoper;
+};
+
+/* This is AP/TDLS specific structure which stores information
+ * about associated/peer STA
  */
 struct mwifiex_sta_node {
 	struct list_head list;
 	u8 mac_addr[ETH_ALEN];
 	u8 is_wmm_enabled;
 	u8 is_11n_enabled;
+	u8 is_11ac_enabled;
 	u8 ampdu_sta[MAX_NUM_TID];
 	u16 rx_seq[MAX_NUM_TID];
 	u16 max_amsdu;
+	u8 tdls_status;
+	struct mwifiex_tdls_capab tdls_cap;
 };
 
 struct mwifiex_if_ops {
@@ -671,7 +719,7 @@ struct mwifiex_adapter {
 	struct cmd_ctrl_node *curr_cmd;
 	/* spin lock for command */
 	spinlock_t mwifiex_cmd_lock;
-	u32 num_cmd_timeout;
+	u8 is_cmd_timedout;
 	u16 last_init_cmd;
 	struct timer_list cmd_timer;
 	struct list_head cmd_free_q;
@@ -725,12 +773,12 @@ struct mwifiex_adapter {
 	u8 event_body[MAX_EVENT_SIZE];
 	u32 hw_dot_11n_dev_cap;
 	u8 hw_dev_mcs_support;
+	u8 user_dev_mcs_support;
 	u8 adhoc_11n_enabled;
 	u8 sec_chan_offset;
 	struct mwifiex_dbg dbg;
 	u8 arp_filter[ARP_FILTER_MAX_BUF_SIZE];
 	u32 arp_filter_size;
-	u16 cmd_wait_q_required;
 	struct mwifiex_wait_queue cmd_wait_q;
 	u8 scan_wait_q_woken;
 	spinlock_t queue_lock;		/* lock for tx queues */
@@ -753,6 +801,8 @@ struct mwifiex_adapter {
 	atomic_t is_tx_received;
 	atomic_t pending_bridged_pkts;
 	struct semaphore *card_sem;
+	bool ext_scan;
+	u8 fw_key_api_major_ver, fw_key_api_minor_ver;
 };
 
 int mwifiex_init_lock_list(struct mwifiex_adapter *adapter);
@@ -788,11 +838,8 @@ int mwifiex_process_event(struct mwifiex_adapter *adapter);
 int mwifiex_complete_cmd(struct mwifiex_adapter *adapter,
 			 struct cmd_ctrl_node *cmd_node);
 
-int mwifiex_send_cmd_async(struct mwifiex_private *priv, uint16_t cmd_no,
-			   u16 cmd_action, u32 cmd_oid, void *data_buf);
-
-int mwifiex_send_cmd_sync(struct mwifiex_private *priv, uint16_t cmd_no,
-			  u16 cmd_action, u32 cmd_oid, void *data_buf);
+int mwifiex_send_cmd(struct mwifiex_private *priv, u16 cmd_no,
+		     u16 cmd_action, u32 cmd_oid, void *data_buf, bool sync);
 
 void mwifiex_cmd_timeout_func(unsigned long function_context);
 
@@ -880,6 +927,7 @@ int mwifiex_ret_802_11_associate(struct mwifiex_private *priv,
 void mwifiex_reset_connect_state(struct mwifiex_private *priv, u16 reason);
 u8 mwifiex_band_to_radio_type(u8 band);
 int mwifiex_deauthenticate(struct mwifiex_private *priv, u8 *mac);
+void mwifiex_deauthenticate_all(struct mwifiex_adapter *adapter);
 int mwifiex_adhoc_start(struct mwifiex_private *priv,
 			struct cfg80211_ssid *adhoc_ssid);
 int mwifiex_adhoc_join(struct mwifiex_private *priv,
@@ -938,6 +986,12 @@ mwifiex_set_wmm_params(struct mwifiex_private *priv,
 		       struct cfg80211_ap_settings *params);
 void mwifiex_set_ba_params(struct mwifiex_private *priv);
 void mwifiex_set_11ac_ba_params(struct mwifiex_private *priv);
+int mwifiex_cmd_802_11_scan_ext(struct mwifiex_private *priv,
+				struct host_cmd_ds_command *cmd,
+				void *data_buf);
+int mwifiex_ret_802_11_scan_ext(struct mwifiex_private *priv);
+int mwifiex_handle_event_ext_scan_report(struct mwifiex_private *priv,
+					 void *buf);
 
 /*
  * This function checks if the queuing is RA based or not.
@@ -1078,7 +1132,7 @@ int mwifiex_set_encode(struct mwifiex_private *priv, struct key_params *kp,
 		       const u8 *key, int key_len, u8 key_index,
 		       const u8 *mac_addr, int disable);
 
-int mwifiex_set_gen_ie(struct mwifiex_private *priv, u8 *ie, int ie_len);
+int mwifiex_set_gen_ie(struct mwifiex_private *priv, const u8 *ie, int ie_len);
 
 int mwifiex_get_ver_ext(struct mwifiex_private *priv);
 
@@ -1159,6 +1213,32 @@ void mwifiex_dnld_txpwr_table(struct mwifiex_private *priv);
 
 extern const struct ethtool_ops mwifiex_ethtool_ops;
 
+void mwifiex_del_all_sta_list(struct mwifiex_private *priv);
+void mwifiex_del_sta_entry(struct mwifiex_private *priv, u8 *mac);
+void
+mwifiex_set_sta_ht_cap(struct mwifiex_private *priv, const u8 *ies,
+		       int ies_len, struct mwifiex_sta_node *node);
+struct mwifiex_sta_node *
+mwifiex_add_sta_entry(struct mwifiex_private *priv, u8 *mac);
+struct mwifiex_sta_node *
+mwifiex_get_sta_entry(struct mwifiex_private *priv, u8 *mac);
+int mwifiex_send_tdls_data_frame(struct mwifiex_private *priv, u8 *peer,
+				 u8 action_code, u8 dialog_token,
+				 u16 status_code, const u8 *extra_ies,
+				 size_t extra_ies_len);
+int mwifiex_send_tdls_action_frame(struct mwifiex_private *priv,
+				 u8 *peer, u8 action_code, u8 dialog_token,
+				 u16 status_code, const u8 *extra_ies,
+				 size_t extra_ies_len);
+void mwifiex_process_tdls_action_frame(struct mwifiex_private *priv,
+				       u8 *buf, int len);
+int mwifiex_tdls_oper(struct mwifiex_private *priv, u8 *peer, u8 action);
+int mwifiex_get_tdls_link_status(struct mwifiex_private *priv, u8 *mac);
+void mwifiex_disable_all_tdls_links(struct mwifiex_private *priv);
+bool mwifiex_is_bss_in_11ac_mode(struct mwifiex_private *priv);
+u8 mwifiex_get_center_freq_index(struct mwifiex_private *priv, u8 band,
+				 u32 pri_chan, u8 chan_bw);
+
 #ifdef CONFIG_DEBUG_FS
 void mwifiex_debugfs_init(void);
 void mwifiex_debugfs_remove(void);
diff --git a/drivers/net/wireless/mwifiex/pcie.c b/drivers/net/wireless/mwifiex/pcie.c
index 03688aa14e8a..9f1683b5f28f 100644
--- a/drivers/net/wireless/mwifiex/pcie.c
+++ b/drivers/net/wireless/mwifiex/pcie.c
@@ -39,20 +39,31 @@ static struct semaphore add_remove_card_sem;
 
 static int
 mwifiex_map_pci_memory(struct mwifiex_adapter *adapter, struct sk_buff *skb,
-		       int size, int flags)
+		       size_t size, int flags)
 {
 	struct pcie_service_card *card = adapter->card;
-	dma_addr_t buf_pa;
+	struct mwifiex_dma_mapping mapping;
 
-	buf_pa = pci_map_single(card->dev, skb->data, size, flags);
-	if (pci_dma_mapping_error(card->dev, buf_pa)) {
+	mapping.addr = pci_map_single(card->dev, skb->data, size, flags);
+	if (pci_dma_mapping_error(card->dev, mapping.addr)) {
 		dev_err(adapter->dev, "failed to map pci memory!\n");
 		return -1;
 	}
-	memcpy(skb->cb, &buf_pa, sizeof(dma_addr_t));
+	mapping.len = size;
+	memcpy(skb->cb, &mapping, sizeof(mapping));
 	return 0;
 }
 
+static void mwifiex_unmap_pci_memory(struct mwifiex_adapter *adapter,
+				     struct sk_buff *skb, int flags)
+{
+	struct pcie_service_card *card = adapter->card;
+	struct mwifiex_dma_mapping mapping;
+
+	MWIFIEX_SKB_PACB(skb, &mapping);
+	pci_unmap_single(card->dev, mapping.addr, mapping.len, flags);
+}
+
 /*
  * This function reads sleep cookie and checks if FW is ready
  */
@@ -179,6 +190,7 @@ static int mwifiex_pcie_probe(struct pci_dev *pdev,
 		card->pcie.firmware = data->firmware;
 		card->pcie.reg = data->reg;
 		card->pcie.blksz_fw_dl = data->blksz_fw_dl;
+		card->pcie.tx_buf_size = data->tx_buf_size;
 	}
 
 	if (mwifiex_add_card(card, &add_remove_card_sem, &pcie_ops,
@@ -199,7 +211,6 @@ static void mwifiex_pcie_remove(struct pci_dev *pdev)
 	struct pcie_service_card *card;
 	struct mwifiex_adapter *adapter;
 	struct mwifiex_private *priv;
-	int i;
 
 	card = pci_get_drvdata(pdev);
 	if (!card)
@@ -218,11 +229,7 @@ static void mwifiex_pcie_remove(struct pci_dev *pdev)
 			mwifiex_pcie_resume(&pdev->dev);
 #endif
 
-		for (i = 0; i < adapter->priv_num; i++)
-			if ((GET_BSS_ROLE(adapter->priv[i]) ==
-			     MWIFIEX_BSS_ROLE_STA) &&
-			    adapter->priv[i]->media_connected)
-				mwifiex_deauthenticate(adapter->priv[i], NULL);
+		mwifiex_deauthenticate_all(adapter);
 
 		priv = mwifiex_get_priv(adapter, MWIFIEX_BSS_ROLE_ANY);
 
@@ -456,7 +463,7 @@ static int mwifiex_init_rxq_ring(struct mwifiex_adapter *adapter)
 					   PCI_DMA_FROMDEVICE))
 			return -1;
 
-		MWIFIEX_SKB_PACB(skb, &buf_pa);
+		buf_pa = MWIFIEX_SKB_DMA_ADDR(skb);
 
 		dev_dbg(adapter->dev,
 			"info: RX ring: skb=%p len=%d data=%p buf_pa=%#x:%x\n",
@@ -513,7 +520,7 @@ static int mwifiex_pcie_init_evt_ring(struct mwifiex_adapter *adapter)
 					   PCI_DMA_FROMDEVICE))
 			return -1;
 
-		MWIFIEX_SKB_PACB(skb, &buf_pa);
+		buf_pa = MWIFIEX_SKB_DMA_ADDR(skb);
 
 		dev_dbg(adapter->dev,
 			"info: EVT ring: skb=%p len=%d data=%p buf_pa=%#x:%x\n",
@@ -549,8 +556,8 @@ static void mwifiex_cleanup_txq_ring(struct mwifiex_adapter *adapter)
 			desc2 = card->txbd_ring[i];
 			if (card->tx_buf_list[i]) {
 				skb = card->tx_buf_list[i];
-				pci_unmap_single(card->dev, desc2->paddr,
-						 skb->len, PCI_DMA_TODEVICE);
+				mwifiex_unmap_pci_memory(adapter, skb,
+							 PCI_DMA_TODEVICE);
 				dev_kfree_skb_any(skb);
 			}
 			memset(desc2, 0, sizeof(*desc2));
@@ -558,8 +565,8 @@ static void mwifiex_cleanup_txq_ring(struct mwifiex_adapter *adapter)
 			desc = card->txbd_ring[i];
 			if (card->tx_buf_list[i]) {
 				skb = card->tx_buf_list[i];
-				pci_unmap_single(card->dev, desc->paddr,
-						 skb->len, PCI_DMA_TODEVICE);
+				mwifiex_unmap_pci_memory(adapter, skb,
+							 PCI_DMA_TODEVICE);
 				dev_kfree_skb_any(skb);
 			}
 			memset(desc, 0, sizeof(*desc));
@@ -587,8 +594,8 @@ static void mwifiex_cleanup_rxq_ring(struct mwifiex_adapter *adapter)
 			desc2 = card->rxbd_ring[i];
 			if (card->rx_buf_list[i]) {
 				skb = card->rx_buf_list[i];
-				pci_unmap_single(card->dev, desc2->paddr,
-						 skb->len, PCI_DMA_FROMDEVICE);
+				mwifiex_unmap_pci_memory(adapter, skb,
+							 PCI_DMA_FROMDEVICE);
 				dev_kfree_skb_any(skb);
 			}
 			memset(desc2, 0, sizeof(*desc2));
@@ -596,8 +603,8 @@ static void mwifiex_cleanup_rxq_ring(struct mwifiex_adapter *adapter)
 			desc = card->rxbd_ring[i];
 			if (card->rx_buf_list[i]) {
 				skb = card->rx_buf_list[i];
-				pci_unmap_single(card->dev, desc->paddr,
-						 skb->len, PCI_DMA_FROMDEVICE);
+				mwifiex_unmap_pci_memory(adapter, skb,
+							 PCI_DMA_FROMDEVICE);
 				dev_kfree_skb_any(skb);
 			}
 			memset(desc, 0, sizeof(*desc));
@@ -622,8 +629,8 @@ static void mwifiex_cleanup_evt_ring(struct mwifiex_adapter *adapter)
 		desc = card->evtbd_ring[i];
 		if (card->evt_buf_list[i]) {
 			skb = card->evt_buf_list[i];
-			pci_unmap_single(card->dev, desc->paddr, MAX_EVENT_SIZE,
-					 PCI_DMA_FROMDEVICE);
+			mwifiex_unmap_pci_memory(adapter, skb,
+						 PCI_DMA_FROMDEVICE);
 			dev_kfree_skb_any(skb);
 		}
 		card->evt_buf_list[i] = NULL;
@@ -861,7 +868,6 @@ static int mwifiex_pcie_alloc_cmdrsp_buf(struct mwifiex_adapter *adapter)
 static int mwifiex_pcie_delete_cmdrsp_buf(struct mwifiex_adapter *adapter)
 {
 	struct pcie_service_card *card;
-	dma_addr_t buf_pa;
 
 	if (!adapter)
 		return 0;
@@ -869,16 +875,14 @@ static int mwifiex_pcie_delete_cmdrsp_buf(struct mwifiex_adapter *adapter)
 	card = adapter->card;
 
 	if (card && card->cmdrsp_buf) {
-		MWIFIEX_SKB_PACB(card->cmdrsp_buf, &buf_pa);
-		pci_unmap_single(card->dev, buf_pa, MWIFIEX_UPLD_SIZE,
-				 PCI_DMA_FROMDEVICE);
+		mwifiex_unmap_pci_memory(adapter, card->cmdrsp_buf,
+					 PCI_DMA_FROMDEVICE);
 		dev_kfree_skb_any(card->cmdrsp_buf);
 	}
 
 	if (card && card->cmd_buf) {
-		MWIFIEX_SKB_PACB(card->cmd_buf, &buf_pa);
-		pci_unmap_single(card->dev, buf_pa, card->cmd_buf->len,
-				 PCI_DMA_TODEVICE);
+		mwifiex_unmap_pci_memory(adapter, card->cmd_buf,
+					 PCI_DMA_TODEVICE);
 	}
 	return 0;
 }
@@ -956,7 +960,6 @@ static int mwifiex_clean_pcie_ring_buf(struct mwifiex_adapter *adapter)
 static int mwifiex_pcie_send_data_complete(struct mwifiex_adapter *adapter)
 {
 	struct sk_buff *skb;
-	dma_addr_t buf_pa;
 	u32 wrdoneidx, rdptr, num_tx_buffs, unmap_count = 0;
 	struct mwifiex_pcie_buf_desc *desc;
 	struct mwifiex_pfu_buf_desc *desc2;
@@ -986,13 +989,13 @@ static int mwifiex_pcie_send_data_complete(struct mwifiex_adapter *adapter)
 			    reg->tx_start_ptr;
 
 		skb = card->tx_buf_list[wrdoneidx];
+
 		if (skb) {
 			dev_dbg(adapter->dev,
 				"SEND COMP: Detach skb %p at txbd_rdidx=%d\n",
 				skb, wrdoneidx);
-			MWIFIEX_SKB_PACB(skb, &buf_pa);
-			pci_unmap_single(card->dev, buf_pa, skb->len,
-					 PCI_DMA_TODEVICE);
+			mwifiex_unmap_pci_memory(adapter, skb,
+						 PCI_DMA_TODEVICE);
 
 			unmap_count++;
 
@@ -1082,12 +1085,12 @@ mwifiex_pcie_send_data(struct mwifiex_adapter *adapter, struct sk_buff *skb,
 		tmp = (__le16 *)&payload[2];
 		*tmp = cpu_to_le16(MWIFIEX_TYPE_DATA);
 
-		if (mwifiex_map_pci_memory(adapter, skb, skb->len ,
+		if (mwifiex_map_pci_memory(adapter, skb, skb->len,
 					   PCI_DMA_TODEVICE))
 			return -1;
 
 		wrindx = (card->txbd_wrptr & reg->tx_mask) >> reg->tx_start_ptr;
-		MWIFIEX_SKB_PACB(skb, &buf_pa);
+		buf_pa = MWIFIEX_SKB_DMA_ADDR(skb);
 		card->tx_buf_list[wrindx] = skb;
 
 		if (reg->pfu_enabled) {
@@ -1162,8 +1165,7 @@ mwifiex_pcie_send_data(struct mwifiex_adapter *adapter, struct sk_buff *skb,
 
 	return -EINPROGRESS;
 done_unmap:
-	MWIFIEX_SKB_PACB(skb, &buf_pa);
-	pci_unmap_single(card->dev, buf_pa, skb->len, PCI_DMA_TODEVICE);
+	mwifiex_unmap_pci_memory(adapter, skb, PCI_DMA_TODEVICE);
 	card->tx_buf_list[wrindx] = NULL;
 	if (reg->pfu_enabled)
 		memset(desc2, 0, sizeof(*desc2));
@@ -1211,9 +1213,13 @@ static int mwifiex_pcie_process_recv_data(struct mwifiex_adapter *adapter)
 		rd_index = card->rxbd_rdptr & reg->rx_mask;
 		skb_data = card->rx_buf_list[rd_index];
 
-		MWIFIEX_SKB_PACB(skb_data, &buf_pa);
-		pci_unmap_single(card->dev, buf_pa, MWIFIEX_RX_DATA_BUF_SIZE,
-				 PCI_DMA_FROMDEVICE);
+		/* If skb allocation was failed earlier for Rx packet,
+		 * rx_buf_list[rd_index] would have been left with a NULL.
+		 */
+		if (!skb_data)
+			return -ENOMEM;
+
+		mwifiex_unmap_pci_memory(adapter, skb_data, PCI_DMA_FROMDEVICE);
 		card->rx_buf_list[rd_index] = NULL;
 
 		/* Get data length from interface header -
@@ -1240,7 +1246,7 @@ static int mwifiex_pcie_process_recv_data(struct mwifiex_adapter *adapter)
 					   PCI_DMA_FROMDEVICE))
 			return -1;
 
-		MWIFIEX_SKB_PACB(skb_tmp, &buf_pa);
+		buf_pa = MWIFIEX_SKB_DMA_ADDR(skb_tmp);
 
 		dev_dbg(adapter->dev,
 			"RECV DATA: Attach new sk_buff %p at rxbd_rdidx=%d\n",
@@ -1316,7 +1322,7 @@ mwifiex_pcie_send_boot_cmd(struct mwifiex_adapter *adapter, struct sk_buff *skb)
 	if (mwifiex_map_pci_memory(adapter, skb, skb->len , PCI_DMA_TODEVICE))
 		return -1;
 
-	MWIFIEX_SKB_PACB(skb, &buf_pa);
+	buf_pa = MWIFIEX_SKB_DMA_ADDR(skb);
 
 	/* Write the lower 32bits of the physical address to low command
 	 * address scratch register
@@ -1325,8 +1331,7 @@ mwifiex_pcie_send_boot_cmd(struct mwifiex_adapter *adapter, struct sk_buff *skb)
 		dev_err(adapter->dev,
 			"%s: failed to write download command to boot code.\n",
 			__func__);
-		pci_unmap_single(card->dev, buf_pa, MWIFIEX_UPLD_SIZE,
-				 PCI_DMA_TODEVICE);
+		mwifiex_unmap_pci_memory(adapter, skb, PCI_DMA_TODEVICE);
 		return -1;
 	}
 
@@ -1338,8 +1343,7 @@ mwifiex_pcie_send_boot_cmd(struct mwifiex_adapter *adapter, struct sk_buff *skb)
 		dev_err(adapter->dev,
 			"%s: failed to write download command to boot code.\n",
 			__func__);
-		pci_unmap_single(card->dev, buf_pa, MWIFIEX_UPLD_SIZE,
-				 PCI_DMA_TODEVICE);
+		mwifiex_unmap_pci_memory(adapter, skb, PCI_DMA_TODEVICE);
 		return -1;
 	}
 
@@ -1348,8 +1352,7 @@ mwifiex_pcie_send_boot_cmd(struct mwifiex_adapter *adapter, struct sk_buff *skb)
 		dev_err(adapter->dev,
 			"%s: failed to write command len to cmd_size scratch reg\n",
 			__func__);
-		pci_unmap_single(card->dev, buf_pa, MWIFIEX_UPLD_SIZE,
-				 PCI_DMA_TODEVICE);
+		mwifiex_unmap_pci_memory(adapter, skb, PCI_DMA_TODEVICE);
 		return -1;
 	}
 
@@ -1358,8 +1361,7 @@ mwifiex_pcie_send_boot_cmd(struct mwifiex_adapter *adapter, struct sk_buff *skb)
 			      CPU_INTR_DOOR_BELL)) {
 		dev_err(adapter->dev,
 			"%s: failed to assert door-bell intr\n", __func__);
-		pci_unmap_single(card->dev, buf_pa,
-				 MWIFIEX_UPLD_SIZE, PCI_DMA_TODEVICE);
+		mwifiex_unmap_pci_memory(adapter, skb, PCI_DMA_TODEVICE);
 		return -1;
 	}
 
@@ -1433,7 +1435,7 @@ mwifiex_pcie_send_cmd(struct mwifiex_adapter *adapter, struct sk_buff *skb)
 	*/
 
 	if (card->cmdrsp_buf) {
-		MWIFIEX_SKB_PACB(card->cmdrsp_buf, &cmdrsp_buf_pa);
+		cmdrsp_buf_pa = MWIFIEX_SKB_DMA_ADDR(card->cmdrsp_buf);
 		/* Write the lower 32bits of the cmdrsp buffer physical
 		   address */
 		if (mwifiex_write_reg(adapter, reg->cmdrsp_addr_lo,
@@ -1454,7 +1456,7 @@ mwifiex_pcie_send_cmd(struct mwifiex_adapter *adapter, struct sk_buff *skb)
 		}
 	}
 
-	MWIFIEX_SKB_PACB(card->cmd_buf, &cmd_buf_pa);
+	cmd_buf_pa = MWIFIEX_SKB_DMA_ADDR(card->cmd_buf);
 	/* Write the lower 32bits of the physical address to reg->cmd_addr_lo */
 	if (mwifiex_write_reg(adapter, reg->cmd_addr_lo,
 			      (u32)cmd_buf_pa)) {
@@ -1508,13 +1510,17 @@ static int mwifiex_pcie_process_cmd_complete(struct mwifiex_adapter *adapter)
 	int count = 0;
 	u16 rx_len;
 	__le16 pkt_len;
-	dma_addr_t buf_pa;
 
 	dev_dbg(adapter->dev, "info: Rx CMD Response\n");
 
-	MWIFIEX_SKB_PACB(skb, &buf_pa);
-	pci_unmap_single(card->dev, buf_pa, MWIFIEX_UPLD_SIZE,
-			 PCI_DMA_FROMDEVICE);
+	mwifiex_unmap_pci_memory(adapter, skb, PCI_DMA_FROMDEVICE);
+
+	/* Unmap the command as a response has been received. */
+	if (card->cmd_buf) {
+		mwifiex_unmap_pci_memory(adapter, card->cmd_buf,
+					 PCI_DMA_TODEVICE);
+		card->cmd_buf = NULL;
+	}
 
 	pkt_len = *((__le16 *)skb->data);
 	rx_len = le16_to_cpu(pkt_len);
@@ -1525,6 +1531,14 @@ static int mwifiex_pcie_process_cmd_complete(struct mwifiex_adapter *adapter)
 		if (adapter->ps_state == PS_STATE_SLEEP_CFM) {
 			mwifiex_process_sleep_confirm_resp(adapter, skb->data,
 							   skb->len);
+			mwifiex_pcie_enable_host_int(adapter);
+			if (mwifiex_write_reg(adapter,
+					      PCIE_CPU_INT_EVENT,
+					      CPU_INTR_SLEEP_CFM_DONE)) {
+				dev_warn(adapter->dev,
+					 "Write register failed\n");
+				return -1;
+			}
 			while (reg->sleep_cookie && (count++ < 10) &&
 			       mwifiex_pcie_ok_to_access_hw(adapter))
 				usleep_range(50, 60);
@@ -1538,8 +1552,6 @@ static int mwifiex_pcie_process_cmd_complete(struct mwifiex_adapter *adapter)
 		if (mwifiex_map_pci_memory(adapter, skb, MWIFIEX_UPLD_SIZE,
 					   PCI_DMA_FROMDEVICE))
 			return -1;
-
-		MWIFIEX_SKB_PACB(skb, &buf_pa);
 	} else if (mwifiex_pcie_ok_to_access_hw(adapter)) {
 		adapter->curr_cmd->resp_skb = skb;
 		adapter->cmd_resp_received = true;
@@ -1574,8 +1586,6 @@ static int mwifiex_pcie_cmdrsp_complete(struct mwifiex_adapter *adapter,
 					struct sk_buff *skb)
 {
 	struct pcie_service_card *card = adapter->card;
-	dma_addr_t buf_pa;
-	struct sk_buff *skb_tmp;
 
 	if (skb) {
 		card->cmdrsp_buf = skb;
@@ -1585,14 +1595,6 @@ static int mwifiex_pcie_cmdrsp_complete(struct mwifiex_adapter *adapter,
 			return -1;
 	}
 
-	skb_tmp = card->cmd_buf;
-	if (skb_tmp) {
-		MWIFIEX_SKB_PACB(skb_tmp, &buf_pa);
-		pci_unmap_single(card->dev, buf_pa, skb_tmp->len,
-				 PCI_DMA_FROMDEVICE);
-		card->cmd_buf = NULL;
-	}
-
 	return 0;
 }
 
@@ -1605,7 +1607,6 @@ static int mwifiex_pcie_process_event_ready(struct mwifiex_adapter *adapter)
 	const struct mwifiex_pcie_card_reg *reg = card->pcie.reg;
 	u32 rdptr = card->evtbd_rdptr & MWIFIEX_EVTBD_MASK;
 	u32 wrptr, event;
-	dma_addr_t buf_pa;
 	struct mwifiex_evt_buf_desc *desc;
 
 	if (!mwifiex_pcie_ok_to_access_hw(adapter))
@@ -1641,9 +1642,7 @@ static int mwifiex_pcie_process_event_ready(struct mwifiex_adapter *adapter)
 
 		dev_dbg(adapter->dev, "info: Read Index: %d\n", rdptr);
 		skb_cmd = card->evt_buf_list[rdptr];
-		MWIFIEX_SKB_PACB(skb_cmd, &buf_pa);
-		pci_unmap_single(card->dev, buf_pa, MAX_EVENT_SIZE,
-				 PCI_DMA_FROMDEVICE);
+		mwifiex_unmap_pci_memory(adapter, skb_cmd, PCI_DMA_FROMDEVICE);
 
 		/* Take the pointer and set it to event pointer in adapter
 		   and will return back after event handling callback */
@@ -1689,7 +1688,6 @@ static int mwifiex_pcie_event_complete(struct mwifiex_adapter *adapter,
 	int ret = 0;
 	u32 rdptr = card->evtbd_rdptr & MWIFIEX_EVTBD_MASK;
 	u32 wrptr;
-	dma_addr_t buf_pa;
 	struct mwifiex_evt_buf_desc *desc;
 
 	if (!skb)
@@ -1714,11 +1712,9 @@ static int mwifiex_pcie_event_complete(struct mwifiex_adapter *adapter,
 					   MAX_EVENT_SIZE,
 					   PCI_DMA_FROMDEVICE))
 			return -1;
-		MWIFIEX_SKB_PACB(skb, &buf_pa);
 		card->evt_buf_list[rdptr] = skb;
-		MWIFIEX_SKB_PACB(skb, &buf_pa);
 		desc = card->evtbd_ring[rdptr];
-		desc->paddr = buf_pa;
+		desc->paddr = MWIFIEX_SKB_DMA_ADDR(skb);
 		desc->len = (u16)skb->len;
 		desc->flags = 0;
 		skb = NULL;
@@ -1768,7 +1764,6 @@ static int mwifiex_prog_fw_w_helper(struct mwifiex_adapter *adapter,
 	struct sk_buff *skb;
 	u32 txlen, tx_blocks = 0, tries, len;
 	u32 block_retry_cnt = 0;
-	dma_addr_t buf_pa;
 	struct pcie_service_card *card = adapter->card;
 	const struct mwifiex_pcie_card_reg *reg = card->pcie.reg;
 
@@ -1866,8 +1861,6 @@ static int mwifiex_prog_fw_w_helper(struct mwifiex_adapter *adapter,
 			goto done;
 		}
 
-		MWIFIEX_SKB_PACB(skb, &buf_pa);
-
 		/* Wait for the command done interrupt */
 		do {
 			if (mwifiex_read_reg(adapter, PCIE_CPU_INT_STATUS,
@@ -1875,16 +1868,15 @@ static int mwifiex_prog_fw_w_helper(struct mwifiex_adapter *adapter,
 				dev_err(adapter->dev, "%s: Failed to read "
 					"interrupt status during fw dnld.\n",
 					__func__);
-				pci_unmap_single(card->dev, buf_pa, skb->len,
-						 PCI_DMA_TODEVICE);
+				mwifiex_unmap_pci_memory(adapter, skb,
+							 PCI_DMA_TODEVICE);
 				ret = -1;
 				goto done;
 			}
 		} while ((ireg_intr & CPU_INTR_DOOR_BELL) ==
 			 CPU_INTR_DOOR_BELL);
 
-		pci_unmap_single(card->dev, buf_pa, skb->len,
-				 PCI_DMA_TODEVICE);
+		mwifiex_unmap_pci_memory(adapter, skb, PCI_DMA_TODEVICE);
 
 		offset += txlen;
 	} while (true);
@@ -1993,23 +1985,9 @@ static void mwifiex_interrupt_status(struct mwifiex_adapter *adapter)
 		adapter->int_status |= pcie_ireg;
 		spin_unlock_irqrestore(&adapter->int_lock, flags);
 
-		if (pcie_ireg & HOST_INTR_CMD_DONE) {
-			if ((adapter->ps_state == PS_STATE_SLEEP_CFM) ||
-			    (adapter->ps_state == PS_STATE_SLEEP)) {
-				mwifiex_pcie_enable_host_int(adapter);
-				if (mwifiex_write_reg(adapter,
-						      PCIE_CPU_INT_EVENT,
-						      CPU_INTR_SLEEP_CFM_DONE)
-						      ) {
-					dev_warn(adapter->dev,
-						 "Write register failed\n");
-					return;
-
-				}
-			}
-		} else if (!adapter->pps_uapsd_mode &&
-			   adapter->ps_state == PS_STATE_SLEEP &&
-			   mwifiex_pcie_ok_to_access_hw(adapter)) {
+		if (!adapter->pps_uapsd_mode &&
+		    adapter->ps_state == PS_STATE_SLEEP &&
+		    mwifiex_pcie_ok_to_access_hw(adapter)) {
 				/* Potentially for PCIe we could get other
 				 * interrupts like shared. Don't change power
 				 * state until cookie is set */
@@ -2338,6 +2316,7 @@ static int mwifiex_register_dev(struct mwifiex_adapter *adapter)
 	}
 
 	adapter->dev = &pdev->dev;
+	adapter->tx_buf_size = card->pcie.tx_buf_size;
 	strcpy(adapter->fw_name, card->pcie.firmware);
 
 	return 0;
diff --git a/drivers/net/wireless/mwifiex/pcie.h b/drivers/net/wireless/mwifiex/pcie.h
index d322ab8604ea..193af75bf582 100644
--- a/drivers/net/wireless/mwifiex/pcie.h
+++ b/drivers/net/wireless/mwifiex/pcie.h
@@ -195,18 +195,21 @@ struct mwifiex_pcie_device {
 	const char *firmware;
 	const struct mwifiex_pcie_card_reg *reg;
 	u16 blksz_fw_dl;
+	u16 tx_buf_size;
 };
 
 static const struct mwifiex_pcie_device mwifiex_pcie8766 = {
 	.firmware       = PCIE8766_DEFAULT_FW_NAME,
 	.reg            = &mwifiex_reg_8766,
 	.blksz_fw_dl = MWIFIEX_PCIE_BLOCK_SIZE_FW_DNLD,
+	.tx_buf_size = MWIFIEX_TX_DATA_BUF_SIZE_2K,
 };
 
 static const struct mwifiex_pcie_device mwifiex_pcie8897 = {
 	.firmware       = PCIE8897_DEFAULT_FW_NAME,
 	.reg            = &mwifiex_reg_8897,
 	.blksz_fw_dl = MWIFIEX_PCIE_BLOCK_SIZE_FW_DNLD,
+	.tx_buf_size = MWIFIEX_TX_DATA_BUF_SIZE_4K,
 };
 
 struct mwifiex_evt_buf_desc {
diff --git a/drivers/net/wireless/mwifiex/scan.c b/drivers/net/wireless/mwifiex/scan.c
index 0a8a26e10f01..0e8ca7bab3e7 100644
--- a/drivers/net/wireless/mwifiex/scan.c
+++ b/drivers/net/wireless/mwifiex/scan.c
@@ -595,7 +595,7 @@ mwifiex_scan_channel_list(struct mwifiex_private *priv,
 	struct mwifiex_chan_scan_param_set *tmp_chan_list;
 	struct mwifiex_chan_scan_param_set *start_chan;
 
-	u32 tlv_idx, rates_size;
+	u32 tlv_idx, rates_size, cmd_no;
 	u32 total_scan_time;
 	u32 done_early;
 	u8 radio_type;
@@ -733,9 +733,13 @@ mwifiex_scan_channel_list(struct mwifiex_private *priv,
 
 		/* Send the scan command to the firmware with the specified
 		   cfg */
-		ret = mwifiex_send_cmd_async(priv, HostCmd_CMD_802_11_SCAN,
-					     HostCmd_ACT_GEN_SET, 0,
-					     scan_cfg_out);
+		if (priv->adapter->ext_scan)
+			cmd_no = HostCmd_CMD_802_11_SCAN_EXT;
+		else
+			cmd_no = HostCmd_CMD_802_11_SCAN;
+
+		ret = mwifiex_send_cmd(priv, cmd_no, HostCmd_ACT_GEN_SET,
+				       0, scan_cfg_out, false);
 
 		/* rate IE is updated per scan command but same starting
 		 * pointer is used each time so that rate IE from earlier
@@ -786,6 +790,7 @@ mwifiex_config_scan(struct mwifiex_private *priv,
 	struct mwifiex_adapter *adapter = priv->adapter;
 	struct mwifiex_ie_types_num_probes *num_probes_tlv;
 	struct mwifiex_ie_types_wildcard_ssid_params *wildcard_ssid_tlv;
+	struct mwifiex_ie_types_bssid_list *bssid_tlv;
 	u8 *tlv_pos;
 	u32 num_probes;
 	u32 ssid_len;
@@ -848,6 +853,17 @@ mwifiex_config_scan(struct mwifiex_private *priv,
 		       user_scan_in->specific_bssid,
 		       sizeof(scan_cfg_out->specific_bssid));
 
+		if (adapter->ext_scan &&
+		    !is_zero_ether_addr(scan_cfg_out->specific_bssid)) {
+			bssid_tlv =
+				(struct mwifiex_ie_types_bssid_list *)tlv_pos;
+			bssid_tlv->header.type = cpu_to_le16(TLV_TYPE_BSSID);
+			bssid_tlv->header.len = cpu_to_le16(ETH_ALEN);
+			memcpy(bssid_tlv->bssid, user_scan_in->specific_bssid,
+			       ETH_ALEN);
+			tlv_pos += sizeof(struct mwifiex_ie_types_bssid_list);
+		}
+
 		for (i = 0; i < user_scan_in->num_ssids; i++) {
 			ssid_len = user_scan_in->ssid_list[i].ssid_len;
 
@@ -941,7 +957,7 @@ mwifiex_config_scan(struct mwifiex_private *priv,
 				cpu_to_le16(sizeof(struct ieee80211_ht_cap));
 		radio_type =
 			mwifiex_band_to_radio_type(priv->adapter->config_bands);
-		mwifiex_fill_cap_info(priv, radio_type, ht_cap);
+		mwifiex_fill_cap_info(priv, radio_type, &ht_cap->ht_cap);
 		tlv_pos += sizeof(struct mwifiex_ie_types_htcap);
 	}
 
@@ -1576,6 +1592,228 @@ done:
 	return 0;
 }
 
+static int
+mwifiex_parse_single_response_buf(struct mwifiex_private *priv, u8 **bss_info,
+				  u32 *bytes_left, u64 fw_tsf, u8 *radio_type,
+				  bool ext_scan, s32 rssi_val)
+{
+	struct mwifiex_adapter *adapter = priv->adapter;
+	struct mwifiex_chan_freq_power *cfp;
+	struct cfg80211_bss *bss;
+	u8 bssid[ETH_ALEN];
+	s32 rssi;
+	const u8 *ie_buf;
+	size_t ie_len;
+	u16 channel = 0;
+	u16 beacon_size = 0;
+	u32 curr_bcn_bytes;
+	u32 freq;
+	u16 beacon_period;
+	u16 cap_info_bitmap;
+	u8 *current_ptr;
+	u64 timestamp;
+	struct mwifiex_fixed_bcn_param *bcn_param;
+	struct mwifiex_bss_priv *bss_priv;
+
+	if (*bytes_left >= sizeof(beacon_size)) {
+		/* Extract & convert beacon size from command buffer */
+		memcpy(&beacon_size, *bss_info, sizeof(beacon_size));
+		*bytes_left -= sizeof(beacon_size);
+		*bss_info += sizeof(beacon_size);
+	}
+
+	if (!beacon_size || beacon_size > *bytes_left) {
+		*bss_info += *bytes_left;
+		*bytes_left = 0;
+		return -EFAULT;
+	}
+
+	/* Initialize the current working beacon pointer for this BSS
+	 * iteration
+	 */
+	current_ptr = *bss_info;
+
+	/* Advance the return beacon pointer past the current beacon */
+	*bss_info += beacon_size;
+	*bytes_left -= beacon_size;
+
+	curr_bcn_bytes = beacon_size;
+
+	/* First 5 fields are bssid, RSSI(for legacy scan only),
+	 * time stamp, beacon interval, and capability information
+	 */
+	if (curr_bcn_bytes < ETH_ALEN + sizeof(u8) +
+	    sizeof(struct mwifiex_fixed_bcn_param)) {
+		dev_err(adapter->dev, "InterpretIE: not enough bytes left\n");
+		return -EFAULT;
+	}
+
+	memcpy(bssid, current_ptr, ETH_ALEN);
+	current_ptr += ETH_ALEN;
+	curr_bcn_bytes -= ETH_ALEN;
+
+	if (!ext_scan) {
+		rssi = (s32) *(u8 *)current_ptr;
+		rssi = (-rssi) * 100;		/* Convert dBm to mBm */
+		current_ptr += sizeof(u8);
+		curr_bcn_bytes -= sizeof(u8);
+		dev_dbg(adapter->dev, "info: InterpretIE: RSSI=%d\n", rssi);
+	} else {
+		rssi = rssi_val;
+	}
+
+	bcn_param = (struct mwifiex_fixed_bcn_param *)current_ptr;
+	current_ptr += sizeof(*bcn_param);
+	curr_bcn_bytes -= sizeof(*bcn_param);
+
+	timestamp = le64_to_cpu(bcn_param->timestamp);
+	beacon_period = le16_to_cpu(bcn_param->beacon_period);
+
+	cap_info_bitmap = le16_to_cpu(bcn_param->cap_info_bitmap);
+	dev_dbg(adapter->dev, "info: InterpretIE: capabilities=0x%X\n",
+		cap_info_bitmap);
+
+	/* Rest of the current buffer are IE's */
+	ie_buf = current_ptr;
+	ie_len = curr_bcn_bytes;
+	dev_dbg(adapter->dev, "info: InterpretIE: IELength for this AP = %d\n",
+		curr_bcn_bytes);
+
+	while (curr_bcn_bytes >= sizeof(struct ieee_types_header)) {
+		u8 element_id, element_len;
+
+		element_id = *current_ptr;
+		element_len = *(current_ptr + 1);
+		if (curr_bcn_bytes < element_len +
+				sizeof(struct ieee_types_header)) {
+			dev_err(adapter->dev,
+				"%s: bytes left < IE length\n", __func__);
+			return -EFAULT;
+		}
+		if (element_id == WLAN_EID_DS_PARAMS) {
+			channel = *(current_ptr +
+				    sizeof(struct ieee_types_header));
+			break;
+		}
+
+		current_ptr += element_len + sizeof(struct ieee_types_header);
+		curr_bcn_bytes -= element_len +
+					sizeof(struct ieee_types_header);
+	}
+
+	if (channel) {
+		struct ieee80211_channel *chan;
+		u8 band;
+
+		/* Skip entry if on csa closed channel */
+		if (channel == priv->csa_chan) {
+			dev_dbg(adapter->dev,
+				"Dropping entry on csa closed channel\n");
+			return 0;
+		}
+
+		band = BAND_G;
+		if (radio_type)
+			band = mwifiex_radio_type_to_band(*radio_type &
+							  (BIT(0) | BIT(1)));
+
+		cfp = mwifiex_get_cfp(priv, band, channel, 0);
+
+		freq = cfp ? cfp->freq : 0;
+
+		chan = ieee80211_get_channel(priv->wdev->wiphy, freq);
+
+		if (chan && !(chan->flags & IEEE80211_CHAN_DISABLED)) {
+			bss = cfg80211_inform_bss(priv->wdev->wiphy,
+					    chan, bssid, timestamp,
+					    cap_info_bitmap, beacon_period,
+					    ie_buf, ie_len, rssi, GFP_KERNEL);
+			bss_priv = (struct mwifiex_bss_priv *)bss->priv;
+			bss_priv->band = band;
+			bss_priv->fw_tsf = fw_tsf;
+			if (priv->media_connected &&
+			    !memcmp(bssid, priv->curr_bss_params.bss_descriptor
+				    .mac_address, ETH_ALEN))
+				mwifiex_update_curr_bss_params(priv, bss);
+			cfg80211_put_bss(priv->wdev->wiphy, bss);
+		}
+	} else {
+		dev_dbg(adapter->dev, "missing BSS channel IE\n");
+	}
+
+	return 0;
+}
+
+static void mwifiex_check_next_scan_command(struct mwifiex_private *priv)
+{
+	struct mwifiex_adapter *adapter = priv->adapter;
+	struct cmd_ctrl_node *cmd_node;
+	unsigned long flags;
+
+	spin_lock_irqsave(&adapter->scan_pending_q_lock, flags);
+	if (list_empty(&adapter->scan_pending_q)) {
+		spin_unlock_irqrestore(&adapter->scan_pending_q_lock, flags);
+		spin_lock_irqsave(&adapter->mwifiex_cmd_lock, flags);
+		adapter->scan_processing = false;
+		spin_unlock_irqrestore(&adapter->mwifiex_cmd_lock, flags);
+
+		/* Need to indicate IOCTL complete */
+		if (adapter->curr_cmd->wait_q_enabled) {
+			adapter->cmd_wait_q.status = 0;
+			if (!priv->scan_request) {
+				dev_dbg(adapter->dev,
+					"complete internal scan\n");
+				mwifiex_complete_cmd(adapter,
+						     adapter->curr_cmd);
+			}
+		}
+		if (priv->report_scan_result)
+			priv->report_scan_result = false;
+
+		if (priv->scan_request) {
+			dev_dbg(adapter->dev, "info: notifying scan done\n");
+			cfg80211_scan_done(priv->scan_request, 0);
+			priv->scan_request = NULL;
+		} else {
+			priv->scan_aborting = false;
+			dev_dbg(adapter->dev, "info: scan already aborted\n");
+		}
+	} else {
+		if ((priv->scan_aborting && !priv->scan_request) ||
+		    priv->scan_block) {
+			spin_unlock_irqrestore(&adapter->scan_pending_q_lock,
+					       flags);
+			adapter->scan_delay_cnt = MWIFIEX_MAX_SCAN_DELAY_CNT;
+			mod_timer(&priv->scan_delay_timer, jiffies);
+			dev_dbg(priv->adapter->dev,
+				"info: %s: triggerring scan abort\n", __func__);
+		} else if (!mwifiex_wmm_lists_empty(adapter) &&
+			   (priv->scan_request && (priv->scan_request->flags &
+					    NL80211_SCAN_FLAG_LOW_PRIORITY))) {
+			spin_unlock_irqrestore(&adapter->scan_pending_q_lock,
+					       flags);
+			adapter->scan_delay_cnt = 1;
+			mod_timer(&priv->scan_delay_timer, jiffies +
+				  msecs_to_jiffies(MWIFIEX_SCAN_DELAY_MSEC));
+			dev_dbg(priv->adapter->dev,
+				"info: %s: deferring scan\n", __func__);
+		} else {
+			/* Get scan command from scan_pending_q and put to
+			 * cmd_pending_q
+			 */
+			cmd_node = list_first_entry(&adapter->scan_pending_q,
+						    struct cmd_ctrl_node, list);
+			list_del(&cmd_node->list);
+			spin_unlock_irqrestore(&adapter->scan_pending_q_lock,
+					       flags);
+			mwifiex_insert_cmd_to_pending_q(adapter, cmd_node,
+							true);
+		}
+	}
+
+	return;
+}
+
 /*
  * This function handles the command response of scan.
  *
@@ -1600,7 +1838,6 @@ int mwifiex_ret_802_11_scan(struct mwifiex_private *priv,
 {
 	int ret = 0;
 	struct mwifiex_adapter *adapter = priv->adapter;
-	struct cmd_ctrl_node *cmd_node;
 	struct host_cmd_ds_802_11_scan_rsp *scan_rsp;
 	struct mwifiex_ie_types_data *tlv_data;
 	struct mwifiex_ie_types_tsf_timestamp *tsf_tlv;
@@ -1609,12 +1846,11 @@ int mwifiex_ret_802_11_scan(struct mwifiex_private *priv,
 	u32 bytes_left;
 	u32 idx;
 	u32 tlv_buf_size;
-	struct mwifiex_chan_freq_power *cfp;
 	struct mwifiex_ie_types_chan_band_list_param_set *chan_band_tlv;
 	struct chan_band_param_set *chan_band;
 	u8 is_bgscan_resp;
-	unsigned long flags;
-	struct cfg80211_bss *bss;
+	__le64 fw_tsf = 0;
+	u8 *radio_type;
 
 	is_bgscan_resp = (le16_to_cpu(resp->command)
 			  == HostCmd_CMD_802_11_BG_SCAN_QUERY);
@@ -1676,220 +1912,194 @@ int mwifiex_ret_802_11_scan(struct mwifiex_private *priv,
 					     &chan_band_tlv);
 
 	for (idx = 0; idx < scan_rsp->number_of_sets && bytes_left; idx++) {
-		u8 bssid[ETH_ALEN];
-		s32 rssi;
-		const u8 *ie_buf;
-		size_t ie_len;
-		u16 channel = 0;
-		__le64 fw_tsf = 0;
-		u16 beacon_size = 0;
-		u32 curr_bcn_bytes;
-		u32 freq;
-		u16 beacon_period;
-		u16 cap_info_bitmap;
-		u8 *current_ptr;
-		u64 timestamp;
-		struct mwifiex_bcn_param *bcn_param;
-		struct mwifiex_bss_priv *bss_priv;
-
-		if (bytes_left >= sizeof(beacon_size)) {
-			/* Extract & convert beacon size from command buffer */
-			memcpy(&beacon_size, bss_info, sizeof(beacon_size));
-			bytes_left -= sizeof(beacon_size);
-			bss_info += sizeof(beacon_size);
-		}
+		/*
+		 * If the TSF TLV was appended to the scan results, save this
+		 * entry's TSF value in the fw_tsf field. It is the firmware's
+		 * TSF value at the time the beacon or probe response was
+		 * received.
+		 */
+		if (tsf_tlv)
+			memcpy(&fw_tsf, &tsf_tlv->tsf_data[idx * TSF_DATA_SIZE],
+			       sizeof(fw_tsf));
 
-		if (!beacon_size || beacon_size > bytes_left) {
-			bss_info += bytes_left;
-			bytes_left = 0;
-			ret = -1;
-			goto check_next_scan;
+		if (chan_band_tlv) {
+			chan_band = &chan_band_tlv->chan_band_param[idx];
+			radio_type = &chan_band->radio_type;
+		} else {
+			radio_type = NULL;
 		}
 
-		/* Initialize the current working beacon pointer for this BSS
-		 * iteration */
-		current_ptr = bss_info;
+		ret = mwifiex_parse_single_response_buf(priv, &bss_info,
+							&bytes_left,
+							le64_to_cpu(fw_tsf),
+							radio_type, false, 0);
+		if (ret)
+			goto check_next_scan;
+	}
 
-		/* Advance the return beacon pointer past the current beacon */
-		bss_info += beacon_size;
-		bytes_left -= beacon_size;
+check_next_scan:
+	mwifiex_check_next_scan_command(priv);
+	return ret;
+}
 
-		curr_bcn_bytes = beacon_size;
+/*
+ * This function prepares an extended scan command to be sent to the firmware
+ *
+ * This uses the scan command configuration sent to the command processing
+ * module in command preparation stage to configure a extended scan command
+ * structure to send to firmware.
+ */
+int mwifiex_cmd_802_11_scan_ext(struct mwifiex_private *priv,
+				struct host_cmd_ds_command *cmd,
+				void *data_buf)
+{
+	struct host_cmd_ds_802_11_scan_ext *ext_scan = &cmd->params.ext_scan;
+	struct mwifiex_scan_cmd_config *scan_cfg = data_buf;
 
-		/*
-		 * First 5 fields are bssid, RSSI, time stamp, beacon interval,
-		 *   and capability information
-		 */
-		if (curr_bcn_bytes < sizeof(struct mwifiex_bcn_param)) {
-			dev_err(adapter->dev,
-				"InterpretIE: not enough bytes left\n");
-			continue;
-		}
-		bcn_param = (struct mwifiex_bcn_param *)current_ptr;
-		current_ptr += sizeof(*bcn_param);
-		curr_bcn_bytes -= sizeof(*bcn_param);
+	memcpy(ext_scan->tlv_buffer, scan_cfg->tlv_buf, scan_cfg->tlv_buf_len);
 
-		memcpy(bssid, bcn_param->bssid, ETH_ALEN);
+	cmd->command = cpu_to_le16(HostCmd_CMD_802_11_SCAN_EXT);
 
-		rssi = (s32) bcn_param->rssi;
-		rssi = (-rssi) * 100;		/* Convert dBm to mBm */
-		dev_dbg(adapter->dev, "info: InterpretIE: RSSI=%d\n", rssi);
+	/* Size is equal to the sizeof(fixed portions) + the TLV len + header */
+	cmd->size = cpu_to_le16((u16)(sizeof(ext_scan->reserved)
+				      + scan_cfg->tlv_buf_len + S_DS_GEN));
 
-		timestamp = le64_to_cpu(bcn_param->timestamp);
-		beacon_period = le16_to_cpu(bcn_param->beacon_period);
+	return 0;
+}
 
-		cap_info_bitmap = le16_to_cpu(bcn_param->cap_info_bitmap);
-		dev_dbg(adapter->dev, "info: InterpretIE: capabilities=0x%X\n",
-			cap_info_bitmap);
+/* This function handles the command response of extended scan */
+int mwifiex_ret_802_11_scan_ext(struct mwifiex_private *priv)
+{
+	dev_dbg(priv->adapter->dev, "info: EXT scan returns successfully\n");
+	return 0;
+}
 
-		/* Rest of the current buffer are IE's */
-		ie_buf = current_ptr;
-		ie_len = curr_bcn_bytes;
-		dev_dbg(adapter->dev,
-			"info: InterpretIE: IELength for this AP = %d\n",
-			curr_bcn_bytes);
+/* This function This function handles the event extended scan report. It
+ * parses extended scan results and informs to cfg80211 stack.
+ */
+int mwifiex_handle_event_ext_scan_report(struct mwifiex_private *priv,
+					 void *buf)
+{
+	int ret = 0;
+	struct mwifiex_adapter *adapter = priv->adapter;
+	u8 *bss_info;
+	u32 bytes_left, bytes_left_for_tlv, idx;
+	u16 type, len;
+	struct mwifiex_ie_types_data *tlv;
+	struct mwifiex_ie_types_bss_scan_rsp *scan_rsp_tlv;
+	struct mwifiex_ie_types_bss_scan_info *scan_info_tlv;
+	u8 *radio_type;
+	u64 fw_tsf = 0;
+	s32 rssi = 0;
+	struct mwifiex_event_scan_result *event_scan = buf;
+	u8 num_of_set = event_scan->num_of_set;
+	u8 *scan_resp = buf + sizeof(struct mwifiex_event_scan_result);
+	u16 scan_resp_size = le16_to_cpu(event_scan->buf_size);
+
+	if (num_of_set > MWIFIEX_MAX_AP) {
+		dev_err(adapter->dev,
+			"EXT_SCAN: Invalid number of AP returned (%d)!!\n",
+			num_of_set);
+		ret = -1;
+		goto check_next_scan;
+	}
 
-		while (curr_bcn_bytes >= sizeof(struct ieee_types_header)) {
-			u8 element_id, element_len;
+	bytes_left = scan_resp_size;
+	dev_dbg(adapter->dev,
+		"EXT_SCAN: size %d, returned %d APs...",
+		scan_resp_size, num_of_set);
 
-			element_id = *current_ptr;
-			element_len = *(current_ptr + 1);
-			if (curr_bcn_bytes < element_len +
-					sizeof(struct ieee_types_header)) {
-				dev_err(priv->adapter->dev,
-					"%s: bytes left < IE length\n",
-					__func__);
-				goto check_next_scan;
-			}
-			if (element_id == WLAN_EID_DS_PARAMS) {
-				channel = *(current_ptr + sizeof(struct ieee_types_header));
-				break;
-			}
+	tlv = (struct mwifiex_ie_types_data *)scan_resp;
 
-			current_ptr += element_len +
-					sizeof(struct ieee_types_header);
-			curr_bcn_bytes -= element_len +
-					sizeof(struct ieee_types_header);
+	for (idx = 0; idx < num_of_set && bytes_left; idx++) {
+		type = le16_to_cpu(tlv->header.type);
+		len = le16_to_cpu(tlv->header.len);
+		if (bytes_left < sizeof(struct mwifiex_ie_types_header) + len) {
+			dev_err(adapter->dev, "EXT_SCAN: Error bytes left < TLV length\n");
+			break;
 		}
+		scan_rsp_tlv = NULL;
+		scan_info_tlv = NULL;
+		bytes_left_for_tlv = bytes_left;
 
-		/*
-		 * If the TSF TLV was appended to the scan results, save this
-		 * entry's TSF value in the fw_tsf field. It is the firmware's
-		 * TSF value at the time the beacon or probe response was
-		 * received.
+		/* BSS response TLV with beacon or probe response buffer
+		 * at the initial position of each descriptor
 		 */
-		if (tsf_tlv)
-			memcpy(&fw_tsf, &tsf_tlv->tsf_data[idx * TSF_DATA_SIZE],
-			       sizeof(fw_tsf));
-
-		if (channel) {
-			struct ieee80211_channel *chan;
-			u8 band;
+		if (type != TLV_TYPE_BSS_SCAN_RSP)
+			break;
 
-			/* Skip entry if on csa closed channel */
-			if (channel == priv->csa_chan) {
-				dev_dbg(adapter->dev,
-					"Dropping entry on csa closed channel\n");
+		bss_info = (u8 *)tlv;
+		scan_rsp_tlv = (struct mwifiex_ie_types_bss_scan_rsp *)tlv;
+		tlv = (struct mwifiex_ie_types_data *)(tlv->data + len);
+		bytes_left_for_tlv -=
+				(len + sizeof(struct mwifiex_ie_types_header));
+
+		while (bytes_left_for_tlv >=
+		       sizeof(struct mwifiex_ie_types_header) &&
+		       le16_to_cpu(tlv->header.type) != TLV_TYPE_BSS_SCAN_RSP) {
+			type = le16_to_cpu(tlv->header.type);
+			len = le16_to_cpu(tlv->header.len);
+			if (bytes_left_for_tlv <
+			    sizeof(struct mwifiex_ie_types_header) + len) {
+				dev_err(adapter->dev,
+					"EXT_SCAN: Error in processing TLV, bytes left < TLV length\n");
+				scan_rsp_tlv = NULL;
+				bytes_left_for_tlv = 0;
 				continue;
 			}
-
-			band = BAND_G;
-			if (chan_band_tlv) {
-				chan_band =
-					&chan_band_tlv->chan_band_param[idx];
-				band = mwifiex_radio_type_to_band(
-						chan_band->radio_type
-						& (BIT(0) | BIT(1)));
-			}
-
-			cfp = mwifiex_get_cfp(priv, band, channel, 0);
-
-			freq = cfp ? cfp->freq : 0;
-
-			chan = ieee80211_get_channel(priv->wdev->wiphy, freq);
-
-			if (chan && !(chan->flags & IEEE80211_CHAN_DISABLED)) {
-				bss = cfg80211_inform_bss(priv->wdev->wiphy,
-					      chan, bssid, timestamp,
-					      cap_info_bitmap, beacon_period,
-					      ie_buf, ie_len, rssi, GFP_KERNEL);
-				bss_priv = (struct mwifiex_bss_priv *)bss->priv;
-				bss_priv->band = band;
-				bss_priv->fw_tsf = le64_to_cpu(fw_tsf);
-				if (priv->media_connected &&
-				    !memcmp(bssid,
-					    priv->curr_bss_params.bss_descriptor
-					    .mac_address, ETH_ALEN))
-					mwifiex_update_curr_bss_params(priv,
-								       bss);
-				cfg80211_put_bss(priv->wdev->wiphy, bss);
+			switch (type) {
+			case TLV_TYPE_BSS_SCAN_INFO:
+				scan_info_tlv =
+				  (struct mwifiex_ie_types_bss_scan_info *)tlv;
+				if (len !=
+				 sizeof(struct mwifiex_ie_types_bss_scan_info) -
+				 sizeof(struct mwifiex_ie_types_header)) {
+					bytes_left_for_tlv = 0;
+					continue;
+				}
+				break;
+			default:
+				break;
 			}
-		} else {
-			dev_dbg(adapter->dev, "missing BSS channel IE\n");
+			tlv = (struct mwifiex_ie_types_data *)(tlv->data + len);
+			bytes_left -=
+				(len + sizeof(struct mwifiex_ie_types_header));
+			bytes_left_for_tlv -=
+				(len + sizeof(struct mwifiex_ie_types_header));
 		}
-	}
 
-check_next_scan:
-	spin_lock_irqsave(&adapter->scan_pending_q_lock, flags);
-	if (list_empty(&adapter->scan_pending_q)) {
-		spin_unlock_irqrestore(&adapter->scan_pending_q_lock, flags);
-		spin_lock_irqsave(&adapter->mwifiex_cmd_lock, flags);
-		adapter->scan_processing = false;
-		spin_unlock_irqrestore(&adapter->mwifiex_cmd_lock, flags);
+		if (!scan_rsp_tlv)
+			break;
 
-		/* Need to indicate IOCTL complete */
-		if (adapter->curr_cmd->wait_q_enabled) {
-			adapter->cmd_wait_q.status = 0;
-			if (!priv->scan_request) {
-				dev_dbg(adapter->dev,
-					"complete internal scan\n");
-				mwifiex_complete_cmd(adapter,
-						     adapter->curr_cmd);
-			}
-		}
-		if (priv->report_scan_result)
-			priv->report_scan_result = false;
+		/* Advance pointer to the beacon buffer length and
+		 * update the bytes count so that the function
+		 * wlan_interpret_bss_desc_with_ie() can handle the
+		 * scan buffer withut any change
+		 */
+		bss_info += sizeof(u16);
+		bytes_left -= sizeof(u16);
 
-		if (priv->scan_request) {
-			dev_dbg(adapter->dev, "info: notifying scan done\n");
-			cfg80211_scan_done(priv->scan_request, 0);
-			priv->scan_request = NULL;
-		} else {
-			priv->scan_aborting = false;
-			dev_dbg(adapter->dev, "info: scan already aborted\n");
-		}
-	} else {
-		if ((priv->scan_aborting && !priv->scan_request) ||
-		    priv->scan_block) {
-			spin_unlock_irqrestore(&adapter->scan_pending_q_lock,
-					       flags);
-			adapter->scan_delay_cnt = MWIFIEX_MAX_SCAN_DELAY_CNT;
-			mod_timer(&priv->scan_delay_timer, jiffies);
-			dev_dbg(priv->adapter->dev,
-				"info: %s: triggerring scan abort\n", __func__);
-		} else if (!mwifiex_wmm_lists_empty(adapter) &&
-			   (priv->scan_request && (priv->scan_request->flags &
-					    NL80211_SCAN_FLAG_LOW_PRIORITY))) {
-			spin_unlock_irqrestore(&adapter->scan_pending_q_lock,
-					       flags);
-			adapter->scan_delay_cnt = 1;
-			mod_timer(&priv->scan_delay_timer, jiffies +
-				  msecs_to_jiffies(MWIFIEX_SCAN_DELAY_MSEC));
-			dev_dbg(priv->adapter->dev,
-				"info: %s: deferring scan\n", __func__);
+		if (scan_info_tlv) {
+			rssi = (s32)(s16)(le16_to_cpu(scan_info_tlv->rssi));
+			rssi *= 100;           /* Convert dBm to mBm */
+			dev_dbg(adapter->dev,
+				"info: InterpretIE: RSSI=%d\n", rssi);
+			fw_tsf = le64_to_cpu(scan_info_tlv->tsf);
+			radio_type = &scan_info_tlv->radio_type;
 		} else {
-			/* Get scan command from scan_pending_q and put to
-			   cmd_pending_q */
-			cmd_node = list_first_entry(&adapter->scan_pending_q,
-						    struct cmd_ctrl_node, list);
-			list_del(&cmd_node->list);
-			spin_unlock_irqrestore(&adapter->scan_pending_q_lock,
-					       flags);
-			mwifiex_insert_cmd_to_pending_q(adapter, cmd_node,
-							true);
+			radio_type = NULL;
 		}
+		ret = mwifiex_parse_single_response_buf(priv, &bss_info,
+							&bytes_left, fw_tsf,
+							radio_type, true, rssi);
+		if (ret)
+			goto check_next_scan;
 	}
 
+check_next_scan:
+	if (!event_scan->more_event)
+		mwifiex_check_next_scan_command(priv);
+
 	return ret;
 }
 
diff --git a/drivers/net/wireless/mwifiex/sdio.c b/drivers/net/wireless/mwifiex/sdio.c
index b44a31523461..e0dcd3ed7a69 100644
--- a/drivers/net/wireless/mwifiex/sdio.c
+++ b/drivers/net/wireless/mwifiex/sdio.c
@@ -84,6 +84,7 @@ mwifiex_sdio_probe(struct sdio_func *func, const struct sdio_device_id *id)
 		card->mp_agg_pkt_limit = data->mp_agg_pkt_limit;
 		card->supports_sdio_new_mode = data->supports_sdio_new_mode;
 		card->has_control_mask = data->has_control_mask;
+		card->tx_buf_size = data->tx_buf_size;
 	}
 
 	sdio_claim_host(func);
@@ -165,7 +166,6 @@ mwifiex_sdio_remove(struct sdio_func *func)
 	struct sdio_mmc_card *card;
 	struct mwifiex_adapter *adapter;
 	struct mwifiex_private *priv;
-	int i;
 
 	pr_debug("info: SDIO func num=%d\n", func->num);
 
@@ -184,11 +184,7 @@ mwifiex_sdio_remove(struct sdio_func *func)
 		if (adapter->is_suspended)
 			mwifiex_sdio_resume(adapter->dev);
 
-		for (i = 0; i < adapter->priv_num; i++)
-			if ((GET_BSS_ROLE(adapter->priv[i]) ==
-						MWIFIEX_BSS_ROLE_STA) &&
-			    adapter->priv[i]->media_connected)
-				mwifiex_deauthenticate(adapter->priv[i], NULL);
+		mwifiex_deauthenticate_all(adapter);
 
 		priv = mwifiex_get_priv(adapter, MWIFIEX_BSS_ROLE_ANY);
 		mwifiex_disable_auto_ds(priv);
@@ -1760,6 +1756,7 @@ static int mwifiex_register_dev(struct mwifiex_adapter *adapter)
 
 	/* save adapter pointer in card */
 	card->adapter = adapter;
+	adapter->tx_buf_size = card->tx_buf_size;
 
 	sdio_claim_host(func);
 
diff --git a/drivers/net/wireless/mwifiex/sdio.h b/drivers/net/wireless/mwifiex/sdio.h
index 532ae0ac4dfb..c71201b2e2a3 100644
--- a/drivers/net/wireless/mwifiex/sdio.h
+++ b/drivers/net/wireless/mwifiex/sdio.h
@@ -233,6 +233,7 @@ struct sdio_mmc_card {
 	u8 mp_agg_pkt_limit;
 	bool supports_sdio_new_mode;
 	bool has_control_mask;
+	u16 tx_buf_size;
 
 	u32 mp_rd_bitmap;
 	u32 mp_wr_bitmap;
@@ -256,6 +257,7 @@ struct mwifiex_sdio_device {
 	u8 mp_agg_pkt_limit;
 	bool supports_sdio_new_mode;
 	bool has_control_mask;
+	u16 tx_buf_size;
 };
 
 static const struct mwifiex_sdio_card_reg mwifiex_reg_sd87xx = {
@@ -312,6 +314,7 @@ static const struct mwifiex_sdio_device mwifiex_sdio_sd8786 = {
 	.mp_agg_pkt_limit = 8,
 	.supports_sdio_new_mode = false,
 	.has_control_mask = true,
+	.tx_buf_size = MWIFIEX_TX_DATA_BUF_SIZE_2K,
 };
 
 static const struct mwifiex_sdio_device mwifiex_sdio_sd8787 = {
@@ -321,6 +324,7 @@ static const struct mwifiex_sdio_device mwifiex_sdio_sd8787 = {
 	.mp_agg_pkt_limit = 8,
 	.supports_sdio_new_mode = false,
 	.has_control_mask = true,
+	.tx_buf_size = MWIFIEX_TX_DATA_BUF_SIZE_2K,
 };
 
 static const struct mwifiex_sdio_device mwifiex_sdio_sd8797 = {
@@ -330,6 +334,7 @@ static const struct mwifiex_sdio_device mwifiex_sdio_sd8797 = {
 	.mp_agg_pkt_limit = 8,
 	.supports_sdio_new_mode = false,
 	.has_control_mask = true,
+	.tx_buf_size = MWIFIEX_TX_DATA_BUF_SIZE_2K,
 };
 
 static const struct mwifiex_sdio_device mwifiex_sdio_sd8897 = {
@@ -339,6 +344,7 @@ static const struct mwifiex_sdio_device mwifiex_sdio_sd8897 = {
 	.mp_agg_pkt_limit = 16,
 	.supports_sdio_new_mode = true,
 	.has_control_mask = false,
+	.tx_buf_size = MWIFIEX_TX_DATA_BUF_SIZE_4K,
 };
 
 /*
diff --git a/drivers/net/wireless/mwifiex/sta_cmd.c b/drivers/net/wireless/mwifiex/sta_cmd.c
index 9208a8816b80..4315a3ba3b92 100644
--- a/drivers/net/wireless/mwifiex/sta_cmd.c
+++ b/drivers/net/wireless/mwifiex/sta_cmd.c
@@ -532,8 +532,228 @@ mwifiex_set_keyparamset_wep(struct mwifiex_private *priv,
 	return 0;
 }
 
+/* This function populates key material v2 command
+ * to set network key for AES & CMAC AES.
+ */
+static int mwifiex_set_aes_key_v2(struct mwifiex_private *priv,
+				  struct host_cmd_ds_command *cmd,
+				  struct mwifiex_ds_encrypt_key *enc_key,
+				  struct host_cmd_ds_802_11_key_material_v2 *km)
+{
+	struct mwifiex_adapter *adapter = priv->adapter;
+	u16 size, len = KEY_PARAMS_FIXED_LEN;
+
+	if (enc_key->is_igtk_key) {
+		dev_dbg(adapter->dev, "%s: Set CMAC AES Key\n", __func__);
+		if (enc_key->is_rx_seq_valid)
+			memcpy(km->key_param_set.key_params.cmac_aes.ipn,
+			       enc_key->pn, enc_key->pn_len);
+		km->key_param_set.key_info &= cpu_to_le16(~KEY_MCAST);
+		km->key_param_set.key_info |= cpu_to_le16(KEY_IGTK);
+		km->key_param_set.key_type = KEY_TYPE_ID_AES_CMAC;
+		km->key_param_set.key_params.cmac_aes.key_len =
+					  cpu_to_le16(enc_key->key_len);
+		memcpy(km->key_param_set.key_params.cmac_aes.key,
+		       enc_key->key_material, enc_key->key_len);
+		len += sizeof(struct mwifiex_cmac_aes_param);
+	} else {
+		dev_dbg(adapter->dev, "%s: Set AES Key\n", __func__);
+		if (enc_key->is_rx_seq_valid)
+			memcpy(km->key_param_set.key_params.aes.pn,
+			       enc_key->pn, enc_key->pn_len);
+		km->key_param_set.key_type = KEY_TYPE_ID_AES;
+		km->key_param_set.key_params.aes.key_len =
+					  cpu_to_le16(enc_key->key_len);
+		memcpy(km->key_param_set.key_params.aes.key,
+		       enc_key->key_material, enc_key->key_len);
+		len += sizeof(struct mwifiex_aes_param);
+	}
+
+	km->key_param_set.len = cpu_to_le16(len);
+	size = len + sizeof(struct mwifiex_ie_types_header) +
+	       sizeof(km->action) + S_DS_GEN;
+	cmd->size = cpu_to_le16(size);
+
+	return 0;
+}
+
+/* This function prepares command to set/get/reset network key(s).
+ * This function prepares key material command for V2 format.
+ * Preparation includes -
+ *      - Setting command ID, action and proper size
+ *      - Setting WEP keys, WAPI keys or WPA keys along with required
+ *        encryption (TKIP, AES) (as required)
+ *      - Ensuring correct endian-ness
+ */
+static int
+mwifiex_cmd_802_11_key_material_v2(struct mwifiex_private *priv,
+				   struct host_cmd_ds_command *cmd,
+				   u16 cmd_action, u32 cmd_oid,
+				   struct mwifiex_ds_encrypt_key *enc_key)
+{
+	struct mwifiex_adapter *adapter = priv->adapter;
+	u8 *mac = enc_key->mac_addr;
+	u16 key_info, len = KEY_PARAMS_FIXED_LEN;
+	struct host_cmd_ds_802_11_key_material_v2 *km =
+						&cmd->params.key_material_v2;
+
+	cmd->command = cpu_to_le16(HostCmd_CMD_802_11_KEY_MATERIAL);
+	km->action = cpu_to_le16(cmd_action);
+
+	if (cmd_action == HostCmd_ACT_GEN_GET) {
+		dev_dbg(adapter->dev, "%s: Get key\n", __func__);
+		km->key_param_set.key_idx =
+					enc_key->key_index & KEY_INDEX_MASK;
+		km->key_param_set.type = cpu_to_le16(TLV_TYPE_KEY_PARAM_V2);
+		km->key_param_set.len = cpu_to_le16(KEY_PARAMS_FIXED_LEN);
+		memcpy(km->key_param_set.mac_addr, mac, ETH_ALEN);
+
+		if (enc_key->key_index & MWIFIEX_KEY_INDEX_UNICAST)
+			key_info = KEY_UNICAST;
+		else
+			key_info = KEY_MCAST;
+
+		if (enc_key->is_igtk_key)
+			key_info |= KEY_IGTK;
+
+		km->key_param_set.key_info = cpu_to_le16(key_info);
+
+		cmd->size = cpu_to_le16(sizeof(struct mwifiex_ie_types_header) +
+					S_DS_GEN + KEY_PARAMS_FIXED_LEN +
+					sizeof(km->action));
+		return 0;
+	}
+
+	memset(&km->key_param_set, 0,
+	       sizeof(struct mwifiex_ie_type_key_param_set_v2));
+
+	if (enc_key->key_disable) {
+		dev_dbg(adapter->dev, "%s: Remove key\n", __func__);
+		km->action = cpu_to_le16(HostCmd_ACT_GEN_REMOVE);
+		km->key_param_set.type = cpu_to_le16(TLV_TYPE_KEY_PARAM_V2);
+		km->key_param_set.len = cpu_to_le16(KEY_PARAMS_FIXED_LEN);
+		km->key_param_set.key_idx = enc_key->key_index & KEY_INDEX_MASK;
+		key_info = KEY_MCAST | KEY_UNICAST;
+		km->key_param_set.key_info = cpu_to_le16(key_info);
+		memcpy(km->key_param_set.mac_addr, mac, ETH_ALEN);
+		cmd->size = cpu_to_le16(sizeof(struct mwifiex_ie_types_header) +
+					S_DS_GEN + KEY_PARAMS_FIXED_LEN +
+					sizeof(km->action));
+		return 0;
+	}
+
+	km->action = cpu_to_le16(HostCmd_ACT_GEN_SET);
+	km->key_param_set.key_idx = enc_key->key_index & KEY_INDEX_MASK;
+	km->key_param_set.type = cpu_to_le16(TLV_TYPE_KEY_PARAM_V2);
+	key_info = KEY_ENABLED;
+	memcpy(km->key_param_set.mac_addr, mac, ETH_ALEN);
+
+	if (enc_key->key_len <= WLAN_KEY_LEN_WEP104) {
+		dev_dbg(adapter->dev, "%s: Set WEP Key\n", __func__);
+		len += sizeof(struct mwifiex_wep_param);
+		km->key_param_set.len = cpu_to_le16(len);
+		km->key_param_set.key_type = KEY_TYPE_ID_WEP;
+
+		if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_UAP) {
+				key_info |= KEY_MCAST | KEY_UNICAST;
+		} else {
+			if (enc_key->is_current_wep_key) {
+				key_info |= KEY_MCAST | KEY_UNICAST;
+				if (km->key_param_set.key_idx ==
+				    (priv->wep_key_curr_index & KEY_INDEX_MASK))
+					key_info |= KEY_DEFAULT;
+			} else {
+				if (mac) {
+					if (is_broadcast_ether_addr(mac))
+						key_info |= KEY_MCAST;
+					else
+						key_info |= KEY_UNICAST |
+							    KEY_DEFAULT;
+				} else {
+					key_info |= KEY_MCAST;
+				}
+			}
+		}
+		km->key_param_set.key_info = cpu_to_le16(key_info);
+
+		km->key_param_set.key_params.wep.key_len =
+						  cpu_to_le16(enc_key->key_len);
+		memcpy(km->key_param_set.key_params.wep.key,
+		       enc_key->key_material, enc_key->key_len);
+
+		cmd->size = cpu_to_le16(sizeof(struct mwifiex_ie_types_header) +
+					len + sizeof(km->action) + S_DS_GEN);
+		return 0;
+	}
+
+	if (is_broadcast_ether_addr(mac))
+		key_info |= KEY_MCAST | KEY_RX_KEY;
+	else
+		key_info |= KEY_UNICAST | KEY_TX_KEY | KEY_RX_KEY;
+
+	if (enc_key->is_wapi_key) {
+		dev_dbg(adapter->dev, "%s: Set WAPI Key\n", __func__);
+		km->key_param_set.key_type = KEY_TYPE_ID_WAPI;
+		memcpy(km->key_param_set.key_params.wapi.pn, enc_key->pn,
+		       PN_LEN);
+		km->key_param_set.key_params.wapi.key_len =
+						cpu_to_le16(enc_key->key_len);
+		memcpy(km->key_param_set.key_params.wapi.key,
+		       enc_key->key_material, enc_key->key_len);
+		if (is_broadcast_ether_addr(mac))
+			priv->sec_info.wapi_key_on = true;
+
+		if (!priv->sec_info.wapi_key_on)
+			key_info |= KEY_DEFAULT;
+		km->key_param_set.key_info = cpu_to_le16(key_info);
+
+		len += sizeof(struct mwifiex_wapi_param);
+		km->key_param_set.len = cpu_to_le16(len);
+		cmd->size = cpu_to_le16(sizeof(struct mwifiex_ie_types_header) +
+					len + sizeof(km->action) + S_DS_GEN);
+		return 0;
+	}
+
+	if (priv->bss_mode == NL80211_IFTYPE_ADHOC) {
+		key_info |= KEY_DEFAULT;
+		/* Enable unicast bit for WPA-NONE/ADHOC_AES */
+		if (!priv->sec_info.wpa2_enabled &&
+		    !is_broadcast_ether_addr(mac))
+			key_info |= KEY_UNICAST;
+	} else {
+		/* Enable default key for WPA/WPA2 */
+		if (!priv->wpa_is_gtk_set)
+			key_info |= KEY_DEFAULT;
+	}
+
+	km->key_param_set.key_info = cpu_to_le16(key_info);
+
+	if (enc_key->key_len == WLAN_KEY_LEN_CCMP)
+		return mwifiex_set_aes_key_v2(priv, cmd, enc_key, km);
+
+	if (enc_key->key_len == WLAN_KEY_LEN_TKIP) {
+		dev_dbg(adapter->dev, "%s: Set TKIP Key\n", __func__);
+		if (enc_key->is_rx_seq_valid)
+			memcpy(km->key_param_set.key_params.tkip.pn,
+			       enc_key->pn, enc_key->pn_len);
+		km->key_param_set.key_type = KEY_TYPE_ID_TKIP;
+		km->key_param_set.key_params.tkip.key_len =
+						cpu_to_le16(enc_key->key_len);
+		memcpy(km->key_param_set.key_params.tkip.key,
+		       enc_key->key_material, enc_key->key_len);
+
+		len += sizeof(struct mwifiex_tkip_param);
+		km->key_param_set.len = cpu_to_le16(len);
+		cmd->size = cpu_to_le16(sizeof(struct mwifiex_ie_types_header) +
+					len + sizeof(km->action) + S_DS_GEN);
+	}
+
+	return 0;
+}
+
 /*
  * This function prepares command to set/get/reset network key(s).
+ * This function prepares key material command for V1 format.
  *
  * Preparation includes -
  *      - Setting command ID, action and proper size
@@ -542,10 +762,10 @@ mwifiex_set_keyparamset_wep(struct mwifiex_private *priv,
  *      - Ensuring correct endian-ness
  */
 static int
-mwifiex_cmd_802_11_key_material(struct mwifiex_private *priv,
-				struct host_cmd_ds_command *cmd,
-				u16 cmd_action, u32 cmd_oid,
-				struct mwifiex_ds_encrypt_key *enc_key)
+mwifiex_cmd_802_11_key_material_v1(struct mwifiex_private *priv,
+				   struct host_cmd_ds_command *cmd,
+				   u16 cmd_action, u32 cmd_oid,
+				   struct mwifiex_ds_encrypt_key *enc_key)
 {
 	struct host_cmd_ds_802_11_key_material *key_material =
 		&cmd->params.key_material;
@@ -724,6 +944,24 @@ mwifiex_cmd_802_11_key_material(struct mwifiex_private *priv,
 	return ret;
 }
 
+/* Wrapper function for setting network key depending upon FW KEY API version */
+static int
+mwifiex_cmd_802_11_key_material(struct mwifiex_private *priv,
+				struct host_cmd_ds_command *cmd,
+				u16 cmd_action, u32 cmd_oid,
+				struct mwifiex_ds_encrypt_key *enc_key)
+{
+	if (priv->adapter->fw_key_api_major_ver == FW_KEY_API_VER_MAJOR_V2)
+		return mwifiex_cmd_802_11_key_material_v2(priv, cmd,
+							  cmd_action, cmd_oid,
+							  enc_key);
+
+	else
+		return mwifiex_cmd_802_11_key_material_v1(priv, cmd,
+							  cmd_action, cmd_oid,
+							  enc_key);
+}
+
 /*
  * This function prepares command to set/get 11d domain information.
  *
@@ -1173,9 +1411,9 @@ int mwifiex_dnld_dt_cfgdata(struct mwifiex_private *priv,
 		/* property header is 6 bytes, data must fit in cmd buffer */
 		if (prop && prop->value && prop->length > 6 &&
 		    prop->length <= MWIFIEX_SIZE_OF_CMD_BUFFER - S_DS_GEN) {
-			ret = mwifiex_send_cmd_sync(priv, HostCmd_CMD_CFG_DATA,
-						    HostCmd_ACT_GEN_SET, 0,
-						    prop);
+			ret = mwifiex_send_cmd(priv, HostCmd_CMD_CFG_DATA,
+					       HostCmd_ACT_GEN_SET, 0,
+					       prop, true);
 			if (ret)
 				return ret;
 		}
@@ -1280,6 +1518,127 @@ mwifiex_cmd_coalesce_cfg(struct mwifiex_private *priv,
 	return 0;
 }
 
+static int
+mwifiex_cmd_tdls_oper(struct mwifiex_private *priv,
+		      struct host_cmd_ds_command *cmd,
+		      void *data_buf)
+{
+	struct host_cmd_ds_tdls_oper *tdls_oper = &cmd->params.tdls_oper;
+	struct mwifiex_ds_tdls_oper *oper = data_buf;
+	struct mwifiex_sta_node *sta_ptr;
+	struct host_cmd_tlv_rates *tlv_rates;
+	struct mwifiex_ie_types_htcap *ht_capab;
+	struct mwifiex_ie_types_qos_info *wmm_qos_info;
+	struct mwifiex_ie_types_extcap *extcap;
+	struct mwifiex_ie_types_vhtcap *vht_capab;
+	struct mwifiex_ie_types_aid *aid;
+	u8 *pos, qos_info;
+	u16 config_len = 0;
+	struct station_parameters *params = priv->sta_params;
+
+	cmd->command = cpu_to_le16(HostCmd_CMD_TDLS_OPER);
+	cmd->size = cpu_to_le16(S_DS_GEN);
+	le16_add_cpu(&cmd->size, sizeof(struct host_cmd_ds_tdls_oper));
+
+	tdls_oper->reason = 0;
+	memcpy(tdls_oper->peer_mac, oper->peer_mac, ETH_ALEN);
+	sta_ptr = mwifiex_get_sta_entry(priv, oper->peer_mac);
+
+	pos = (u8 *)tdls_oper + sizeof(struct host_cmd_ds_tdls_oper);
+
+	switch (oper->tdls_action) {
+	case MWIFIEX_TDLS_DISABLE_LINK:
+		tdls_oper->tdls_action = cpu_to_le16(ACT_TDLS_DELETE);
+		break;
+	case MWIFIEX_TDLS_CREATE_LINK:
+		tdls_oper->tdls_action = cpu_to_le16(ACT_TDLS_CREATE);
+		break;
+	case MWIFIEX_TDLS_CONFIG_LINK:
+		tdls_oper->tdls_action = cpu_to_le16(ACT_TDLS_CONFIG);
+
+		if (!params) {
+			dev_err(priv->adapter->dev,
+				"TDLS config params not available for %pM\n",
+				oper->peer_mac);
+			return -ENODATA;
+		}
+
+		*(__le16 *)pos = cpu_to_le16(params->capability);
+		config_len += sizeof(params->capability);
+
+		qos_info = params->uapsd_queues | (params->max_sp << 5);
+		wmm_qos_info = (struct mwifiex_ie_types_qos_info *)(pos +
+								    config_len);
+		wmm_qos_info->header.type = cpu_to_le16(WLAN_EID_QOS_CAPA);
+		wmm_qos_info->header.len = cpu_to_le16(sizeof(qos_info));
+		wmm_qos_info->qos_info = qos_info;
+		config_len += sizeof(struct mwifiex_ie_types_qos_info);
+
+		if (params->ht_capa) {
+			ht_capab = (struct mwifiex_ie_types_htcap *)(pos +
+								    config_len);
+			ht_capab->header.type =
+					    cpu_to_le16(WLAN_EID_HT_CAPABILITY);
+			ht_capab->header.len =
+				   cpu_to_le16(sizeof(struct ieee80211_ht_cap));
+			memcpy(&ht_capab->ht_cap, params->ht_capa,
+			       sizeof(struct ieee80211_ht_cap));
+			config_len += sizeof(struct mwifiex_ie_types_htcap);
+		}
+
+		if (params->supported_rates && params->supported_rates_len) {
+			tlv_rates = (struct host_cmd_tlv_rates *)(pos +
+								  config_len);
+			tlv_rates->header.type =
+					       cpu_to_le16(WLAN_EID_SUPP_RATES);
+			tlv_rates->header.len =
+				       cpu_to_le16(params->supported_rates_len);
+			memcpy(tlv_rates->rates, params->supported_rates,
+			       params->supported_rates_len);
+			config_len += sizeof(struct host_cmd_tlv_rates) +
+				      params->supported_rates_len;
+		}
+
+		if (params->ext_capab && params->ext_capab_len) {
+			extcap = (struct mwifiex_ie_types_extcap *)(pos +
+								    config_len);
+			extcap->header.type =
+					   cpu_to_le16(WLAN_EID_EXT_CAPABILITY);
+			extcap->header.len = cpu_to_le16(params->ext_capab_len);
+			memcpy(extcap->ext_capab, params->ext_capab,
+			       params->ext_capab_len);
+			config_len += sizeof(struct mwifiex_ie_types_extcap) +
+				      params->ext_capab_len;
+		}
+		if (params->vht_capa) {
+			vht_capab = (struct mwifiex_ie_types_vhtcap *)(pos +
+								    config_len);
+			vht_capab->header.type =
+					   cpu_to_le16(WLAN_EID_VHT_CAPABILITY);
+			vht_capab->header.len =
+				  cpu_to_le16(sizeof(struct ieee80211_vht_cap));
+			memcpy(&vht_capab->vht_cap, params->vht_capa,
+			       sizeof(struct ieee80211_vht_cap));
+			config_len += sizeof(struct mwifiex_ie_types_vhtcap);
+		}
+		if (params->aid) {
+			aid = (struct mwifiex_ie_types_aid *)(pos + config_len);
+			aid->header.type = cpu_to_le16(WLAN_EID_AID);
+			aid->header.len = cpu_to_le16(sizeof(params->aid));
+			aid->aid = cpu_to_le16(params->aid);
+			config_len += sizeof(struct mwifiex_ie_types_aid);
+		}
+
+		break;
+	default:
+		dev_err(priv->adapter->dev, "Unknown TDLS operation\n");
+		return -ENOTSUPP;
+	}
+
+	le16_add_cpu(&cmd->size, config_len);
+
+	return 0;
+}
 /*
  * This function prepares the commands before sending them to the firmware.
  *
@@ -1472,6 +1831,9 @@ int mwifiex_sta_prepare_cmd(struct mwifiex_private *priv, uint16_t cmd_no,
 		ret = mwifiex_cmd_ibss_coalescing_status(cmd_ptr, cmd_action,
 							 data_buf);
 		break;
+	case HostCmd_CMD_802_11_SCAN_EXT:
+		ret = mwifiex_cmd_802_11_scan_ext(priv, cmd_ptr, data_buf);
+		break;
 	case HostCmd_CMD_MAC_REG_ACCESS:
 	case HostCmd_CMD_BBP_REG_ACCESS:
 	case HostCmd_CMD_RF_REG_ACCESS:
@@ -1507,6 +1869,9 @@ int mwifiex_sta_prepare_cmd(struct mwifiex_private *priv, uint16_t cmd_no,
 		ret = mwifiex_cmd_coalesce_cfg(priv, cmd_ptr, cmd_action,
 					       data_buf);
 		break;
+	case HostCmd_CMD_TDLS_OPER:
+		ret = mwifiex_cmd_tdls_oper(priv, cmd_ptr, data_buf);
+		break;
 	default:
 		dev_err(priv->adapter->dev,
 			"PREP_CMD: unknown cmd- %#x\n", cmd_no);
@@ -1547,15 +1912,16 @@ int mwifiex_sta_init_cmd(struct mwifiex_private *priv, u8 first_sta)
 
 	if (first_sta) {
 		if (priv->adapter->iface_type == MWIFIEX_PCIE) {
-			ret = mwifiex_send_cmd_sync(priv,
-						HostCmd_CMD_PCIE_DESC_DETAILS,
-						HostCmd_ACT_GEN_SET, 0, NULL);
+			ret = mwifiex_send_cmd(priv,
+					       HostCmd_CMD_PCIE_DESC_DETAILS,
+					       HostCmd_ACT_GEN_SET, 0, NULL,
+					       true);
 			if (ret)
 				return -1;
 		}
 
-		ret = mwifiex_send_cmd_sync(priv, HostCmd_CMD_FUNC_INIT,
-					    HostCmd_ACT_GEN_SET, 0, NULL);
+		ret = mwifiex_send_cmd(priv, HostCmd_CMD_FUNC_INIT,
+				       HostCmd_ACT_GEN_SET, 0, NULL, true);
 		if (ret)
 			return -1;
 
@@ -1573,55 +1939,57 @@ int mwifiex_sta_init_cmd(struct mwifiex_private *priv, u8 first_sta)
 		}
 
 		if (adapter->cal_data) {
-			ret = mwifiex_send_cmd_sync(priv, HostCmd_CMD_CFG_DATA,
-						HostCmd_ACT_GEN_SET, 0, NULL);
+			ret = mwifiex_send_cmd(priv, HostCmd_CMD_CFG_DATA,
+					       HostCmd_ACT_GEN_SET, 0, NULL,
+					       true);
 			if (ret)
 				return -1;
 		}
 
 		/* Read MAC address from HW */
-		ret = mwifiex_send_cmd_sync(priv, HostCmd_CMD_GET_HW_SPEC,
-					    HostCmd_ACT_GEN_GET, 0, NULL);
+		ret = mwifiex_send_cmd(priv, HostCmd_CMD_GET_HW_SPEC,
+				       HostCmd_ACT_GEN_GET, 0, NULL, true);
 		if (ret)
 			return -1;
 
 		/* Reconfigure tx buf size */
-		ret = mwifiex_send_cmd_sync(priv,
-					    HostCmd_CMD_RECONFIGURE_TX_BUFF,
-					    HostCmd_ACT_GEN_SET, 0,
-					    &priv->adapter->tx_buf_size);
+		ret = mwifiex_send_cmd(priv, HostCmd_CMD_RECONFIGURE_TX_BUFF,
+				       HostCmd_ACT_GEN_SET, 0,
+				       &priv->adapter->tx_buf_size, true);
 		if (ret)
 			return -1;
 
 		if (priv->bss_type != MWIFIEX_BSS_TYPE_UAP) {
 			/* Enable IEEE PS by default */
 			priv->adapter->ps_mode = MWIFIEX_802_11_POWER_MODE_PSP;
-			ret = mwifiex_send_cmd_sync(
-					priv, HostCmd_CMD_802_11_PS_MODE_ENH,
-					EN_AUTO_PS, BITMAP_STA_PS, NULL);
+			ret = mwifiex_send_cmd(priv,
+					       HostCmd_CMD_802_11_PS_MODE_ENH,
+					       EN_AUTO_PS, BITMAP_STA_PS, NULL,
+					       true);
 			if (ret)
 				return -1;
 		}
 	}
 
 	/* get tx rate */
-	ret = mwifiex_send_cmd_sync(priv, HostCmd_CMD_TX_RATE_CFG,
-				    HostCmd_ACT_GEN_GET, 0, NULL);
+	ret = mwifiex_send_cmd(priv, HostCmd_CMD_TX_RATE_CFG,
+			       HostCmd_ACT_GEN_GET, 0, NULL, true);
 	if (ret)
 		return -1;
 	priv->data_rate = 0;
 
 	/* get tx power */
-	ret = mwifiex_send_cmd_sync(priv, HostCmd_CMD_RF_TX_PWR,
-				    HostCmd_ACT_GEN_GET, 0, NULL);
+	ret = mwifiex_send_cmd(priv, HostCmd_CMD_RF_TX_PWR,
+			       HostCmd_ACT_GEN_GET, 0, NULL, true);
 	if (ret)
 		return -1;
 
 	if (priv->bss_type == MWIFIEX_BSS_TYPE_STA) {
 		/* set ibss coalescing_status */
-		ret = mwifiex_send_cmd_sync(
-				priv, HostCmd_CMD_802_11_IBSS_COALESCING_STATUS,
-				HostCmd_ACT_GEN_SET, 0, &enable);
+		ret = mwifiex_send_cmd(
+				priv,
+				HostCmd_CMD_802_11_IBSS_COALESCING_STATUS,
+				HostCmd_ACT_GEN_SET, 0, &enable, true);
 		if (ret)
 			return -1;
 	}
@@ -1629,16 +1997,16 @@ int mwifiex_sta_init_cmd(struct mwifiex_private *priv, u8 first_sta)
 	memset(&amsdu_aggr_ctrl, 0, sizeof(amsdu_aggr_ctrl));
 	amsdu_aggr_ctrl.enable = true;
 	/* Send request to firmware */
-	ret = mwifiex_send_cmd_sync(priv, HostCmd_CMD_AMSDU_AGGR_CTRL,
-				    HostCmd_ACT_GEN_SET, 0,
-				    &amsdu_aggr_ctrl);
+	ret = mwifiex_send_cmd(priv, HostCmd_CMD_AMSDU_AGGR_CTRL,
+			       HostCmd_ACT_GEN_SET, 0,
+			       &amsdu_aggr_ctrl, true);
 	if (ret)
 		return -1;
 	/* MAC Control must be the last command in init_fw */
 	/* set MAC Control */
-	ret = mwifiex_send_cmd_sync(priv, HostCmd_CMD_MAC_CONTROL,
-				    HostCmd_ACT_GEN_SET, 0,
-				    &priv->curr_pkt_filter);
+	ret = mwifiex_send_cmd(priv, HostCmd_CMD_MAC_CONTROL,
+			       HostCmd_ACT_GEN_SET, 0,
+			       &priv->curr_pkt_filter, true);
 	if (ret)
 		return -1;
 
@@ -1647,10 +2015,9 @@ int mwifiex_sta_init_cmd(struct mwifiex_private *priv, u8 first_sta)
 		/* Enable auto deep sleep */
 		auto_ds.auto_ds = DEEP_SLEEP_ON;
 		auto_ds.idle_time = DEEP_SLEEP_IDLE_TIME;
-		ret = mwifiex_send_cmd_sync(priv,
-					    HostCmd_CMD_802_11_PS_MODE_ENH,
-					    EN_AUTO_PS, BITMAP_AUTO_DS,
-					    &auto_ds);
+		ret = mwifiex_send_cmd(priv, HostCmd_CMD_802_11_PS_MODE_ENH,
+				       EN_AUTO_PS, BITMAP_AUTO_DS,
+				       &auto_ds, true);
 		if (ret)
 			return -1;
 	}
@@ -1658,9 +2025,9 @@ int mwifiex_sta_init_cmd(struct mwifiex_private *priv, u8 first_sta)
 	if (priv->bss_type != MWIFIEX_BSS_TYPE_UAP) {
 		/* Send cmd to FW to enable/disable 11D function */
 		state_11d = ENABLE_11D;
-		ret = mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_SNMP_MIB,
-					    HostCmd_ACT_GEN_SET, DOT11D_I,
-					    &state_11d);
+		ret = mwifiex_send_cmd(priv, HostCmd_CMD_802_11_SNMP_MIB,
+				       HostCmd_ACT_GEN_SET, DOT11D_I,
+				       &state_11d, true);
 		if (ret)
 			dev_err(priv->adapter->dev,
 				"11D: failed to enable 11D\n");
@@ -1673,8 +2040,8 @@ int mwifiex_sta_init_cmd(struct mwifiex_private *priv, u8 first_sta)
 	 * (Short GI, Channel BW, Green field support etc.) for transmit
 	 */
 	tx_cfg.tx_htcap = MWIFIEX_FW_DEF_HTTXCFG;
-	ret = mwifiex_send_cmd_sync(priv, HostCmd_CMD_11N_CFG,
-				    HostCmd_ACT_GEN_SET, 0, &tx_cfg);
+	ret = mwifiex_send_cmd(priv, HostCmd_CMD_11N_CFG,
+			       HostCmd_ACT_GEN_SET, 0, &tx_cfg, true);
 
 	ret = -EINPROGRESS;
 
diff --git a/drivers/net/wireless/mwifiex/sta_cmdresp.c b/drivers/net/wireless/mwifiex/sta_cmdresp.c
index 24523e4015cb..a8f7d545e22a 100644
--- a/drivers/net/wireless/mwifiex/sta_cmdresp.c
+++ b/drivers/net/wireless/mwifiex/sta_cmdresp.c
@@ -69,6 +69,7 @@ mwifiex_process_cmdresp_error(struct mwifiex_private *priv,
 
 		break;
 	case HostCmd_CMD_802_11_SCAN:
+	case HostCmd_CMD_802_11_SCAN_EXT:
 		/* Cancel all pending scan command */
 		spin_lock_irqsave(&adapter->scan_pending_q_lock, flags);
 		list_for_each_entry_safe(cmd_node, tmp_node,
@@ -157,8 +158,8 @@ static int mwifiex_ret_802_11_rssi_info(struct mwifiex_private *priv,
 
 	priv->subsc_evt_rssi_state = EVENT_HANDLED;
 
-	mwifiex_send_cmd_async(priv, HostCmd_CMD_802_11_SUBSCRIBE_EVENT,
-			       0, 0, subsc_evt);
+	mwifiex_send_cmd(priv, HostCmd_CMD_802_11_SUBSCRIBE_EVENT,
+			 0, 0, subsc_evt, false);
 
 	return 0;
 }
@@ -316,9 +317,8 @@ static int mwifiex_ret_tx_rate_cfg(struct mwifiex_private *priv,
 	if (priv->is_data_rate_auto)
 		priv->data_rate = 0;
 	else
-		return mwifiex_send_cmd_async(priv,
-					      HostCmd_CMD_802_11_TX_RATE_QUERY,
-					      HostCmd_ACT_GEN_GET, 0, NULL);
+		return mwifiex_send_cmd(priv, HostCmd_CMD_802_11_TX_RATE_QUERY,
+					HostCmd_ACT_GEN_GET, 0, NULL, false);
 
 	return 0;
 }
@@ -561,13 +561,13 @@ static int mwifiex_ret_802_11_ad_hoc_stop(struct mwifiex_private *priv,
 }
 
 /*
- * This function handles the command response of set/get key material.
+ * This function handles the command response of set/get v1 key material.
  *
  * Handling includes updating the driver parameters to reflect the
  * changes.
  */
-static int mwifiex_ret_802_11_key_material(struct mwifiex_private *priv,
-					   struct host_cmd_ds_command *resp)
+static int mwifiex_ret_802_11_key_material_v1(struct mwifiex_private *priv,
+					      struct host_cmd_ds_command *resp)
 {
 	struct host_cmd_ds_802_11_key_material *key =
 						&resp->params.key_material;
@@ -590,6 +590,51 @@ static int mwifiex_ret_802_11_key_material(struct mwifiex_private *priv,
 }
 
 /*
+ * This function handles the command response of set/get v2 key material.
+ *
+ * Handling includes updating the driver parameters to reflect the
+ * changes.
+ */
+static int mwifiex_ret_802_11_key_material_v2(struct mwifiex_private *priv,
+					      struct host_cmd_ds_command *resp)
+{
+	struct host_cmd_ds_802_11_key_material_v2 *key_v2;
+	__le16 len;
+
+	key_v2 = &resp->params.key_material_v2;
+	if (le16_to_cpu(key_v2->action) == HostCmd_ACT_GEN_SET) {
+		if ((le16_to_cpu(key_v2->key_param_set.key_info) & KEY_MCAST)) {
+			dev_dbg(priv->adapter->dev, "info: key: GTK is set\n");
+			priv->wpa_is_gtk_set = true;
+			priv->scan_block = false;
+		}
+	}
+
+	if (key_v2->key_param_set.key_type != KEY_TYPE_ID_AES)
+		return 0;
+
+	memset(priv->aes_key_v2.key_param_set.key_params.aes.key, 0,
+	       WLAN_KEY_LEN_CCMP);
+	priv->aes_key_v2.key_param_set.key_params.aes.key_len =
+				key_v2->key_param_set.key_params.aes.key_len;
+	len = priv->aes_key_v2.key_param_set.key_params.aes.key_len;
+	memcpy(priv->aes_key_v2.key_param_set.key_params.aes.key,
+	       key_v2->key_param_set.key_params.aes.key, le16_to_cpu(len));
+
+	return 0;
+}
+
+/* Wrapper function for processing response of key material command */
+static int mwifiex_ret_802_11_key_material(struct mwifiex_private *priv,
+					   struct host_cmd_ds_command *resp)
+{
+	if (priv->adapter->fw_key_api_major_ver == FW_KEY_API_VER_MAJOR_V2)
+		return mwifiex_ret_802_11_key_material_v2(priv, resp);
+	else
+		return mwifiex_ret_802_11_key_material_v1(priv, resp);
+}
+
+/*
  * This function handles the command response of get 11d domain information.
  */
 static int mwifiex_ret_802_11d_domain_info(struct mwifiex_private *priv,
@@ -800,7 +845,60 @@ static int mwifiex_ret_ibss_coalescing_status(struct mwifiex_private *priv,
 
 	return 0;
 }
+static int mwifiex_ret_tdls_oper(struct mwifiex_private *priv,
+				 struct host_cmd_ds_command *resp)
+{
+	struct host_cmd_ds_tdls_oper *cmd_tdls_oper = &resp->params.tdls_oper;
+	u16 reason = le16_to_cpu(cmd_tdls_oper->reason);
+	u16 action = le16_to_cpu(cmd_tdls_oper->tdls_action);
+	struct mwifiex_sta_node *node =
+			   mwifiex_get_sta_entry(priv, cmd_tdls_oper->peer_mac);
 
+	switch (action) {
+	case ACT_TDLS_DELETE:
+		if (reason)
+			dev_err(priv->adapter->dev,
+				"TDLS link delete for %pM failed: reason %d\n",
+				cmd_tdls_oper->peer_mac, reason);
+		else
+			dev_dbg(priv->adapter->dev,
+				"TDLS link config for %pM successful\n",
+				cmd_tdls_oper->peer_mac);
+		break;
+	case ACT_TDLS_CREATE:
+		if (reason) {
+			dev_err(priv->adapter->dev,
+				"TDLS link creation for %pM failed: reason %d",
+				cmd_tdls_oper->peer_mac, reason);
+			if (node && reason != TDLS_ERR_LINK_EXISTS)
+				node->tdls_status = TDLS_SETUP_FAILURE;
+		} else {
+			dev_dbg(priv->adapter->dev,
+				"TDLS link creation for %pM successful",
+				cmd_tdls_oper->peer_mac);
+		}
+		break;
+	case ACT_TDLS_CONFIG:
+		if (reason) {
+			dev_err(priv->adapter->dev,
+				"TDLS link config for %pM failed, reason %d\n",
+				cmd_tdls_oper->peer_mac, reason);
+			if (node)
+				node->tdls_status = TDLS_SETUP_FAILURE;
+		} else {
+			dev_dbg(priv->adapter->dev,
+				"TDLS link config for %pM successful\n",
+				cmd_tdls_oper->peer_mac);
+		}
+		break;
+	default:
+		dev_err(priv->adapter->dev,
+			"Unknown TDLS command action respnse %d", action);
+		return -1;
+	}
+
+	return 0;
+}
 /*
  * This function handles the command response for subscribe event command.
  */
@@ -871,6 +969,10 @@ int mwifiex_process_sta_cmdresp(struct mwifiex_private *priv, u16 cmdresp_no,
 		ret = mwifiex_ret_802_11_scan(priv, resp);
 		adapter->curr_cmd->wait_q_enabled = false;
 		break;
+	case HostCmd_CMD_802_11_SCAN_EXT:
+		ret = mwifiex_ret_802_11_scan_ext(priv);
+		adapter->curr_cmd->wait_q_enabled = false;
+		break;
 	case HostCmd_CMD_802_11_BG_SCAN_QUERY:
 		ret = mwifiex_ret_802_11_scan(priv, resp);
 		dev_dbg(adapter->dev,
@@ -999,6 +1101,9 @@ int mwifiex_process_sta_cmdresp(struct mwifiex_private *priv, u16 cmdresp_no,
 		break;
 	case HostCmd_CMD_COALESCE_CFG:
 		break;
+	case HostCmd_CMD_TDLS_OPER:
+		ret = mwifiex_ret_tdls_oper(priv, resp);
+		break;
 	default:
 		dev_err(adapter->dev, "CMD_RESP: unknown cmd response %#x\n",
 			resp->command);
diff --git a/drivers/net/wireless/mwifiex/sta_event.c b/drivers/net/wireless/mwifiex/sta_event.c
index 8c351f71f72f..368450cc56c7 100644
--- a/drivers/net/wireless/mwifiex/sta_event.c
+++ b/drivers/net/wireless/mwifiex/sta_event.c
@@ -54,6 +54,10 @@ mwifiex_reset_connect_state(struct mwifiex_private *priv, u16 reason_code)
 
 	priv->scan_block = false;
 
+	if ((GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_STA) &&
+	    ISSUPP_TDLS_ENABLED(priv->adapter->fw_cap_info))
+		mwifiex_disable_all_tdls_links(priv);
+
 	/* Free Tx and Rx packets, report disconnect to upper layer */
 	mwifiex_clean_txrx(priv);
 
@@ -112,7 +116,7 @@ mwifiex_reset_connect_state(struct mwifiex_private *priv, u16 reason_code)
 	adapter->tx_lock_flag = false;
 	adapter->pps_uapsd_mode = false;
 
-	if (adapter->num_cmd_timeout && adapter->curr_cmd)
+	if (adapter->is_cmd_timedout && adapter->curr_cmd)
 		return;
 	priv->media_connected = false;
 	dev_dbg(adapter->dev,
@@ -289,9 +293,8 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 
 	case EVENT_HS_ACT_REQ:
 		dev_dbg(adapter->dev, "event: HS_ACT_REQ\n");
-		ret = mwifiex_send_cmd_async(priv,
-					     HostCmd_CMD_802_11_HS_CFG_ENH,
-					     0, 0, NULL);
+		ret = mwifiex_send_cmd(priv, HostCmd_CMD_802_11_HS_CFG_ENH,
+				       0, 0, NULL, false);
 		break;
 
 	case EVENT_MIC_ERR_UNICAST:
@@ -322,27 +325,34 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 
 	case EVENT_BG_SCAN_REPORT:
 		dev_dbg(adapter->dev, "event: BGS_REPORT\n");
-		ret = mwifiex_send_cmd_async(priv,
-					     HostCmd_CMD_802_11_BG_SCAN_QUERY,
-					     HostCmd_ACT_GEN_GET, 0, NULL);
+		ret = mwifiex_send_cmd(priv, HostCmd_CMD_802_11_BG_SCAN_QUERY,
+				       HostCmd_ACT_GEN_GET, 0, NULL, false);
 		break;
 
 	case EVENT_PORT_RELEASE:
 		dev_dbg(adapter->dev, "event: PORT RELEASE\n");
 		break;
 
+	case EVENT_EXT_SCAN_REPORT:
+		dev_dbg(adapter->dev, "event: EXT_SCAN Report\n");
+		if (adapter->ext_scan)
+			ret = mwifiex_handle_event_ext_scan_report(priv,
+						adapter->event_skb->data);
+
+		break;
+
 	case EVENT_WMM_STATUS_CHANGE:
 		dev_dbg(adapter->dev, "event: WMM status changed\n");
-		ret = mwifiex_send_cmd_async(priv, HostCmd_CMD_WMM_GET_STATUS,
-					     0, 0, NULL);
+		ret = mwifiex_send_cmd(priv, HostCmd_CMD_WMM_GET_STATUS,
+				       0, 0, NULL, false);
 		break;
 
 	case EVENT_RSSI_LOW:
 		cfg80211_cqm_rssi_notify(priv->netdev,
 					 NL80211_CQM_RSSI_THRESHOLD_EVENT_LOW,
 					 GFP_KERNEL);
-		mwifiex_send_cmd_async(priv, HostCmd_CMD_RSSI_INFO,
-				       HostCmd_ACT_GEN_GET, 0, NULL);
+		mwifiex_send_cmd(priv, HostCmd_CMD_RSSI_INFO,
+				 HostCmd_ACT_GEN_GET, 0, NULL, false);
 		priv->subsc_evt_rssi_state = RSSI_LOW_RECVD;
 		dev_dbg(adapter->dev, "event: Beacon RSSI_LOW\n");
 		break;
@@ -356,8 +366,8 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 		cfg80211_cqm_rssi_notify(priv->netdev,
 					 NL80211_CQM_RSSI_THRESHOLD_EVENT_HIGH,
 					 GFP_KERNEL);
-		mwifiex_send_cmd_async(priv, HostCmd_CMD_RSSI_INFO,
-				       HostCmd_ACT_GEN_GET, 0, NULL);
+		mwifiex_send_cmd(priv, HostCmd_CMD_RSSI_INFO,
+				 HostCmd_ACT_GEN_GET, 0, NULL, false);
 		priv->subsc_evt_rssi_state = RSSI_HIGH_RECVD;
 		dev_dbg(adapter->dev, "event: Beacon RSSI_HIGH\n");
 		break;
@@ -384,15 +394,15 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 		break;
 	case EVENT_IBSS_COALESCED:
 		dev_dbg(adapter->dev, "event: IBSS_COALESCED\n");
-		ret = mwifiex_send_cmd_async(priv,
+		ret = mwifiex_send_cmd(priv,
 				HostCmd_CMD_802_11_IBSS_COALESCING_STATUS,
-				HostCmd_ACT_GEN_GET, 0, NULL);
+				HostCmd_ACT_GEN_GET, 0, NULL, false);
 		break;
 	case EVENT_ADDBA:
 		dev_dbg(adapter->dev, "event: ADDBA Request\n");
-		mwifiex_send_cmd_async(priv, HostCmd_CMD_11N_ADDBA_RSP,
-				       HostCmd_ACT_GEN_SET, 0,
-				       adapter->event_body);
+		mwifiex_send_cmd(priv, HostCmd_CMD_11N_ADDBA_RSP,
+				 HostCmd_ACT_GEN_SET, 0,
+				 adapter->event_body, false);
 		break;
 	case EVENT_DELBA:
 		dev_dbg(adapter->dev, "event: DELBA Request\n");
@@ -443,10 +453,10 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 		priv->csa_expire_time =
 				jiffies + msecs_to_jiffies(DFS_CHAN_MOVE_TIME);
 		priv->csa_chan = priv->curr_bss_params.bss_descriptor.channel;
-		ret = mwifiex_send_cmd_async(priv,
-			HostCmd_CMD_802_11_DEAUTHENTICATE,
+		ret = mwifiex_send_cmd(priv, HostCmd_CMD_802_11_DEAUTHENTICATE,
 			HostCmd_ACT_GEN_SET, 0,
-			priv->curr_bss_params.bss_descriptor.mac_address);
+			priv->curr_bss_params.bss_descriptor.mac_address,
+			false);
 		break;
 
 	default:
diff --git a/drivers/net/wireless/mwifiex/sta_ioctl.c b/drivers/net/wireless/mwifiex/sta_ioctl.c
index c5cb2ed19ec2..33170af150f6 100644
--- a/drivers/net/wireless/mwifiex/sta_ioctl.c
+++ b/drivers/net/wireless/mwifiex/sta_ioctl.c
@@ -108,19 +108,19 @@ int mwifiex_request_set_multicast_list(struct mwifiex_private *priv,
 				"info: Set multicast list=%d\n",
 				mcast_list->num_multicast_addr);
 			/* Send multicast addresses to firmware */
-			ret = mwifiex_send_cmd_async(priv,
-				HostCmd_CMD_MAC_MULTICAST_ADR,
-				HostCmd_ACT_GEN_SET, 0,
-				mcast_list);
+			ret = mwifiex_send_cmd(priv,
+					       HostCmd_CMD_MAC_MULTICAST_ADR,
+					       HostCmd_ACT_GEN_SET, 0,
+					       mcast_list, false);
 		}
 	}
 	dev_dbg(priv->adapter->dev,
 		"info: old_pkt_filter=%#x, curr_pkt_filter=%#x\n",
 	       old_pkt_filter, priv->curr_pkt_filter);
 	if (old_pkt_filter != priv->curr_pkt_filter) {
-		ret = mwifiex_send_cmd_async(priv, HostCmd_CMD_MAC_CONTROL,
-					     HostCmd_ACT_GEN_SET,
-					     0, &priv->curr_pkt_filter);
+		ret = mwifiex_send_cmd(priv, HostCmd_CMD_MAC_CONTROL,
+				       HostCmd_ACT_GEN_SET,
+				       0, &priv->curr_pkt_filter, false);
 	}
 
 	return ret;
@@ -237,8 +237,8 @@ static int mwifiex_process_country_ie(struct mwifiex_private *priv,
 
 	rcu_read_unlock();
 
-	if (mwifiex_send_cmd_async(priv, HostCmd_CMD_802_11D_DOMAIN_INFO,
-				   HostCmd_ACT_GEN_SET, 0, NULL)) {
+	if (mwifiex_send_cmd(priv, HostCmd_CMD_802_11D_DOMAIN_INFO,
+			     HostCmd_ACT_GEN_SET, 0, NULL, false)) {
 		wiphy_err(priv->adapter->wiphy,
 			  "11D: setting domain info in FW\n");
 		return -1;
@@ -290,7 +290,7 @@ int mwifiex_bss_start(struct mwifiex_private *priv, struct cfg80211_bss *bss,
 
 		if (mwifiex_band_to_radio_type(bss_desc->bss_band) ==
 						HostCmd_SCAN_RADIO_TYPE_BG)
-			config_bands = BAND_B | BAND_G | BAND_GN | BAND_GAC;
+			config_bands = BAND_B | BAND_G | BAND_GN;
 		else
 			config_bands = BAND_A | BAND_AN | BAND_AAC;
 
@@ -429,16 +429,13 @@ static int mwifiex_set_hs_params(struct mwifiex_private *priv, u16 action,
 				status = -1;
 				break;
 			}
-			if (cmd_type == MWIFIEX_SYNC_CMD)
-				status = mwifiex_send_cmd_sync(priv,
-						HostCmd_CMD_802_11_HS_CFG_ENH,
-						HostCmd_ACT_GEN_SET, 0,
-						&adapter->hs_cfg);
-			else
-				status = mwifiex_send_cmd_async(priv,
-						HostCmd_CMD_802_11_HS_CFG_ENH,
-						HostCmd_ACT_GEN_SET, 0,
-						&adapter->hs_cfg);
+
+			status = mwifiex_send_cmd(priv,
+						  HostCmd_CMD_802_11_HS_CFG_ENH,
+						  HostCmd_ACT_GEN_SET, 0,
+						  &adapter->hs_cfg,
+						  cmd_type == MWIFIEX_SYNC_CMD);
+
 			if (hs_cfg->conditions == HS_CFG_CANCEL)
 				/* Restore previous condition */
 				adapter->hs_cfg.conditions =
@@ -586,8 +583,8 @@ int mwifiex_disable_auto_ds(struct mwifiex_private *priv)
 
 	auto_ds.auto_ds = DEEP_SLEEP_OFF;
 
-	return mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_PS_MODE_ENH,
-				     DIS_AUTO_PS, BITMAP_AUTO_DS, &auto_ds);
+	return mwifiex_send_cmd(priv, HostCmd_CMD_802_11_PS_MODE_ENH,
+				DIS_AUTO_PS, BITMAP_AUTO_DS, &auto_ds, true);
 }
 EXPORT_SYMBOL_GPL(mwifiex_disable_auto_ds);
 
@@ -601,8 +598,8 @@ int mwifiex_drv_get_data_rate(struct mwifiex_private *priv, u32 *rate)
 {
 	int ret;
 
-	ret = mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_TX_RATE_QUERY,
-				    HostCmd_ACT_GEN_GET, 0, NULL);
+	ret = mwifiex_send_cmd(priv, HostCmd_CMD_802_11_TX_RATE_QUERY,
+			       HostCmd_ACT_GEN_GET, 0, NULL, true);
 
 	if (!ret) {
 		if (priv->is_data_rate_auto)
@@ -698,8 +695,8 @@ int mwifiex_set_tx_power(struct mwifiex_private *priv,
 		pg->power_max = (s8) dbm;
 		pg->ht_bandwidth = HT_BW_40;
 	}
-	ret = mwifiex_send_cmd_sync(priv, HostCmd_CMD_TXPWR_CFG,
-				    HostCmd_ACT_GEN_SET, 0, buf);
+	ret = mwifiex_send_cmd(priv, HostCmd_CMD_TXPWR_CFG,
+			       HostCmd_ACT_GEN_SET, 0, buf, true);
 
 	kfree(buf);
 	return ret;
@@ -722,12 +719,11 @@ int mwifiex_drv_set_power(struct mwifiex_private *priv, u32 *ps_mode)
 	else
 		adapter->ps_mode = MWIFIEX_802_11_POWER_MODE_CAM;
 	sub_cmd = (*ps_mode) ? EN_AUTO_PS : DIS_AUTO_PS;
-	ret = mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_PS_MODE_ENH,
-				    sub_cmd, BITMAP_STA_PS, NULL);
+	ret = mwifiex_send_cmd(priv, HostCmd_CMD_802_11_PS_MODE_ENH,
+			       sub_cmd, BITMAP_STA_PS, NULL, true);
 	if ((!ret) && (sub_cmd == DIS_AUTO_PS))
-		ret = mwifiex_send_cmd_async(priv,
-					     HostCmd_CMD_802_11_PS_MODE_ENH,
-					     GET_PS, 0, NULL);
+		ret = mwifiex_send_cmd(priv, HostCmd_CMD_802_11_PS_MODE_ENH,
+				       GET_PS, 0, NULL, false);
 
 	return ret;
 }
@@ -851,9 +847,9 @@ static int mwifiex_sec_ioctl_set_wapi_key(struct mwifiex_private *priv,
 			       struct mwifiex_ds_encrypt_key *encrypt_key)
 {
 
-	return mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_KEY_MATERIAL,
-				     HostCmd_ACT_GEN_SET, KEY_INFO_ENABLED,
-				     encrypt_key);
+	return mwifiex_send_cmd(priv, HostCmd_CMD_802_11_KEY_MATERIAL,
+				HostCmd_ACT_GEN_SET, KEY_INFO_ENABLED,
+				encrypt_key, true);
 }
 
 /*
@@ -865,6 +861,7 @@ static int mwifiex_sec_ioctl_set_wapi_key(struct mwifiex_private *priv,
 static int mwifiex_sec_ioctl_set_wep_key(struct mwifiex_private *priv,
 			      struct mwifiex_ds_encrypt_key *encrypt_key)
 {
+	struct mwifiex_adapter *adapter = priv->adapter;
 	int ret;
 	struct mwifiex_wep_key *wep_key;
 	int index;
@@ -879,10 +876,17 @@ static int mwifiex_sec_ioctl_set_wep_key(struct mwifiex_private *priv,
 		/* Copy the required key as the current key */
 		wep_key = &priv->wep_key[index];
 		if (!wep_key->key_length) {
-			dev_err(priv->adapter->dev,
+			dev_err(adapter->dev,
 				"key not set, so cannot enable it\n");
 			return -1;
 		}
+
+		if (adapter->fw_key_api_major_ver == FW_KEY_API_VER_MAJOR_V2) {
+			memcpy(encrypt_key->key_material,
+			       wep_key->key_material, wep_key->key_length);
+			encrypt_key->key_len = wep_key->key_length;
+		}
+
 		priv->wep_key_curr_index = (u16) index;
 		priv->sec_info.wep_enabled = 1;
 	} else {
@@ -897,21 +901,32 @@ static int mwifiex_sec_ioctl_set_wep_key(struct mwifiex_private *priv,
 		priv->sec_info.wep_enabled = 1;
 	}
 	if (wep_key->key_length) {
+		void *enc_key;
+
+		if (encrypt_key->key_disable)
+			memset(&priv->wep_key[index], 0,
+			       sizeof(struct mwifiex_wep_key));
+
+		if (adapter->fw_key_api_major_ver == FW_KEY_API_VER_MAJOR_V2)
+			enc_key = encrypt_key;
+		else
+			enc_key = NULL;
+
 		/* Send request to firmware */
-		ret = mwifiex_send_cmd_async(priv,
-					     HostCmd_CMD_802_11_KEY_MATERIAL,
-					     HostCmd_ACT_GEN_SET, 0, NULL);
+		ret = mwifiex_send_cmd(priv, HostCmd_CMD_802_11_KEY_MATERIAL,
+				       HostCmd_ACT_GEN_SET, 0, enc_key, false);
 		if (ret)
 			return ret;
 	}
+
 	if (priv->sec_info.wep_enabled)
 		priv->curr_pkt_filter |= HostCmd_ACT_MAC_WEP_ENABLE;
 	else
 		priv->curr_pkt_filter &= ~HostCmd_ACT_MAC_WEP_ENABLE;
 
-	ret = mwifiex_send_cmd_sync(priv, HostCmd_CMD_MAC_CONTROL,
-				    HostCmd_ACT_GEN_SET, 0,
-				    &priv->curr_pkt_filter);
+	ret = mwifiex_send_cmd(priv, HostCmd_CMD_MAC_CONTROL,
+			       HostCmd_ACT_GEN_SET, 0,
+			       &priv->curr_pkt_filter, true);
 
 	return ret;
 }
@@ -946,10 +961,9 @@ static int mwifiex_sec_ioctl_set_wpa_key(struct mwifiex_private *priv,
 		 */
 		/* Send the key as PTK to firmware */
 		encrypt_key->key_index = MWIFIEX_KEY_INDEX_UNICAST;
-		ret = mwifiex_send_cmd_async(priv,
-					     HostCmd_CMD_802_11_KEY_MATERIAL,
-					     HostCmd_ACT_GEN_SET,
-					     KEY_INFO_ENABLED, encrypt_key);
+		ret = mwifiex_send_cmd(priv, HostCmd_CMD_802_11_KEY_MATERIAL,
+				       HostCmd_ACT_GEN_SET,
+				       KEY_INFO_ENABLED, encrypt_key, false);
 		if (ret)
 			return ret;
 
@@ -973,15 +987,13 @@ static int mwifiex_sec_ioctl_set_wpa_key(struct mwifiex_private *priv,
 		encrypt_key->key_index = MWIFIEX_KEY_INDEX_UNICAST;
 
 	if (remove_key)
-		ret = mwifiex_send_cmd_sync(priv,
-					    HostCmd_CMD_802_11_KEY_MATERIAL,
-					    HostCmd_ACT_GEN_SET,
-					    !KEY_INFO_ENABLED, encrypt_key);
+		ret = mwifiex_send_cmd(priv, HostCmd_CMD_802_11_KEY_MATERIAL,
+				       HostCmd_ACT_GEN_SET,
+				       !KEY_INFO_ENABLED, encrypt_key, true);
 	else
-		ret = mwifiex_send_cmd_sync(priv,
-					    HostCmd_CMD_802_11_KEY_MATERIAL,
-					    HostCmd_ACT_GEN_SET,
-					    KEY_INFO_ENABLED, encrypt_key);
+		ret = mwifiex_send_cmd(priv, HostCmd_CMD_802_11_KEY_MATERIAL,
+				       HostCmd_ACT_GEN_SET,
+				       KEY_INFO_ENABLED, encrypt_key, true);
 
 	return ret;
 }
@@ -1044,19 +1056,27 @@ int mwifiex_set_encode(struct mwifiex_private *priv, struct key_params *kp,
 
 	memset(&encrypt_key, 0, sizeof(struct mwifiex_ds_encrypt_key));
 	encrypt_key.key_len = key_len;
+	encrypt_key.key_index = key_index;
 
 	if (kp && kp->cipher == WLAN_CIPHER_SUITE_AES_CMAC)
 		encrypt_key.is_igtk_key = true;
 
 	if (!disable) {
-		encrypt_key.key_index = key_index;
 		if (key_len)
 			memcpy(encrypt_key.key_material, key, key_len);
+		else
+			encrypt_key.is_current_wep_key = true;
+
 		if (mac_addr)
 			memcpy(encrypt_key.mac_addr, mac_addr, ETH_ALEN);
-		if (kp && kp->seq && kp->seq_len)
+		if (kp && kp->seq && kp->seq_len) {
 			memcpy(encrypt_key.pn, kp->seq, kp->seq_len);
+			encrypt_key.pn_len = kp->seq_len;
+			encrypt_key.is_rx_seq_valid = true;
+		}
 	} else {
+		if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_UAP)
+			return 0;
 		encrypt_key.key_disable = true;
 		if (mac_addr)
 			memcpy(encrypt_key.mac_addr, mac_addr, ETH_ALEN);
@@ -1077,8 +1097,8 @@ mwifiex_get_ver_ext(struct mwifiex_private *priv)
 	struct mwifiex_ver_ext ver_ext;
 
 	memset(&ver_ext, 0, sizeof(struct host_cmd_ds_version_ext));
-	if (mwifiex_send_cmd_sync(priv, HostCmd_CMD_VERSION_EXT,
-				  HostCmd_ACT_GEN_GET, 0, &ver_ext))
+	if (mwifiex_send_cmd(priv, HostCmd_CMD_VERSION_EXT,
+			     HostCmd_ACT_GEN_GET, 0, &ver_ext, true))
 		return -1;
 
 	return 0;
@@ -1103,8 +1123,8 @@ mwifiex_remain_on_chan_cfg(struct mwifiex_private *priv, u16 action,
 			ieee80211_frequency_to_channel(chan->center_freq);
 		roc_cfg.duration = cpu_to_le32(duration);
 	}
-	if (mwifiex_send_cmd_sync(priv, HostCmd_CMD_REMAIN_ON_CHAN,
-				  action, 0, &roc_cfg)) {
+	if (mwifiex_send_cmd(priv, HostCmd_CMD_REMAIN_ON_CHAN,
+			     action, 0, &roc_cfg, true)) {
 		dev_err(priv->adapter->dev, "failed to remain on channel\n");
 		return -1;
 	}
@@ -1136,8 +1156,8 @@ mwifiex_set_bss_role(struct mwifiex_private *priv, u8 bss_role)
 		break;
 	}
 
-	mwifiex_send_cmd_sync(priv, HostCmd_CMD_SET_BSS_MODE,
-			      HostCmd_ACT_GEN_SET, 0, NULL);
+	mwifiex_send_cmd(priv, HostCmd_CMD_SET_BSS_MODE,
+			 HostCmd_ACT_GEN_SET, 0, NULL, true);
 
 	return mwifiex_sta_init_cmd(priv, false);
 }
@@ -1152,8 +1172,8 @@ int
 mwifiex_get_stats_info(struct mwifiex_private *priv,
 		       struct mwifiex_ds_get_stats *log)
 {
-	return mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_GET_LOG,
-				     HostCmd_ACT_GEN_GET, 0, log);
+	return mwifiex_send_cmd(priv, HostCmd_CMD_802_11_GET_LOG,
+				HostCmd_ACT_GEN_GET, 0, log, true);
 }
 
 /*
@@ -1195,8 +1215,7 @@ static int mwifiex_reg_mem_ioctl_reg_rw(struct mwifiex_private *priv,
 		return -1;
 	}
 
-	return mwifiex_send_cmd_sync(priv, cmd_no, action, 0, reg_rw);
-
+	return mwifiex_send_cmd(priv, cmd_no, action, 0, reg_rw, true);
 }
 
 /*
@@ -1261,8 +1280,8 @@ mwifiex_eeprom_read(struct mwifiex_private *priv, u16 offset, u16 bytes,
 	rd_eeprom.byte_count = cpu_to_le16((u16) bytes);
 
 	/* Send request to firmware */
-	ret = mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_EEPROM_ACCESS,
-				    HostCmd_ACT_GEN_GET, 0, &rd_eeprom);
+	ret = mwifiex_send_cmd(priv, HostCmd_CMD_802_11_EEPROM_ACCESS,
+			       HostCmd_ACT_GEN_GET, 0, &rd_eeprom, true);
 
 	if (!ret)
 		memcpy(value, rd_eeprom.value, MAX_EEPROM_DATA);
@@ -1391,7 +1410,7 @@ static int mwifiex_misc_ioctl_gen_ie(struct mwifiex_private *priv,
  * with requisite parameters and calls the IOCTL handler.
  */
 int
-mwifiex_set_gen_ie(struct mwifiex_private *priv, u8 *ie, int ie_len)
+mwifiex_set_gen_ie(struct mwifiex_private *priv, const u8 *ie, int ie_len)
 {
 	struct mwifiex_ds_misc_gen_ie gen_ie;
 
diff --git a/drivers/net/wireless/mwifiex/sta_rx.c b/drivers/net/wireless/mwifiex/sta_rx.c
index 4651d676df38..b6aa958bd6e4 100644
--- a/drivers/net/wireless/mwifiex/sta_rx.c
+++ b/drivers/net/wireless/mwifiex/sta_rx.c
@@ -88,11 +88,14 @@ int mwifiex_process_rx_packet(struct mwifiex_private *priv,
 	struct rxpd *local_rx_pd;
 	int hdr_chop;
 	struct ethhdr *eth;
+	u16 rx_pkt_off, rx_pkt_len;
+	u8 *offset;
 
 	local_rx_pd = (struct rxpd *) (skb->data);
 
-	rx_pkt_hdr = (void *)local_rx_pd +
-		     le16_to_cpu(local_rx_pd->rx_pkt_offset);
+	rx_pkt_off = le16_to_cpu(local_rx_pd->rx_pkt_offset);
+	rx_pkt_len = le16_to_cpu(local_rx_pd->rx_pkt_length);
+	rx_pkt_hdr = (void *)local_rx_pd + rx_pkt_off;
 
 	if ((!memcmp(&rx_pkt_hdr->rfc1042_hdr, bridge_tunnel_header,
 		     sizeof(bridge_tunnel_header))) ||
@@ -142,6 +145,12 @@ int mwifiex_process_rx_packet(struct mwifiex_private *priv,
 		return 0;
 	}
 
+	if (ISSUPP_TDLS_ENABLED(priv->adapter->fw_cap_info) &&
+	    ntohs(rx_pkt_hdr->eth803_hdr.h_proto) == ETH_P_TDLS) {
+		offset = (u8 *)local_rx_pd + rx_pkt_off;
+		mwifiex_process_tdls_action_frame(priv, offset, rx_pkt_len);
+	}
+
 	priv->rxpd_rate = local_rx_pd->rx_rate;
 
 	priv->rxpd_htinfo = local_rx_pd->ht_info;
diff --git a/drivers/net/wireless/mwifiex/sta_tx.c b/drivers/net/wireless/mwifiex/sta_tx.c
index 354d64c9606f..1236a5de7bca 100644
--- a/drivers/net/wireless/mwifiex/sta_tx.c
+++ b/drivers/net/wireless/mwifiex/sta_tx.c
@@ -95,6 +95,9 @@ void *mwifiex_process_sta_txpd(struct mwifiex_private *priv,
 		}
 	}
 
+	if (tx_info->flags & MWIFIEX_BUF_FLAG_TDLS_PKT)
+		local_tx_pd->flags |= MWIFIEX_TXPD_FLAGS_TDLS_PACKET;
+
 	/* Offset of actual data */
 	pkt_offset = sizeof(struct txpd) + pad;
 	if (pkt_type == PKT_TYPE_MGMT) {
diff --git a/drivers/net/wireless/mwifiex/tdls.c b/drivers/net/wireless/mwifiex/tdls.c
new file mode 100644
index 000000000000..8cec6e4ba8c4
--- /dev/null
+++ b/drivers/net/wireless/mwifiex/tdls.c
@@ -0,0 +1,1044 @@
+/* Marvell Wireless LAN device driver: TDLS handling
+ *
+ * Copyright (C) 2014, Marvell International Ltd.
+ *
+ * This software file (the "File") is distributed by Marvell International
+ * Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ * (the "License").  You may use, redistribute and/or modify this File in
+ * accordance with the terms and conditions of the License, a copy of which
+ * is available on the worldwide web at
+ * http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ * this warranty disclaimer.
+ */
+
+#include "main.h"
+#include "wmm.h"
+#include "11n.h"
+#include "11n_rxreorder.h"
+#include "11ac.h"
+
+#define TDLS_REQ_FIX_LEN      6
+#define TDLS_RESP_FIX_LEN     8
+#define TDLS_CONFIRM_FIX_LEN  6
+
+static void
+mwifiex_restore_tdls_packets(struct mwifiex_private *priv, u8 *mac, u8 status)
+{
+	struct mwifiex_ra_list_tbl *ra_list;
+	struct list_head *tid_list;
+	struct sk_buff *skb, *tmp;
+	struct mwifiex_txinfo *tx_info;
+	unsigned long flags;
+	u32 tid;
+	u8 tid_down;
+
+	dev_dbg(priv->adapter->dev, "%s: %pM\n", __func__, mac);
+	spin_lock_irqsave(&priv->wmm.ra_list_spinlock, flags);
+
+	skb_queue_walk_safe(&priv->tdls_txq, skb, tmp) {
+		if (!ether_addr_equal(mac, skb->data))
+			continue;
+
+		__skb_unlink(skb, &priv->tdls_txq);
+		tx_info = MWIFIEX_SKB_TXCB(skb);
+		tid = skb->priority;
+		tid_down = mwifiex_wmm_downgrade_tid(priv, tid);
+
+		if (status == TDLS_SETUP_COMPLETE) {
+			ra_list = mwifiex_wmm_get_queue_raptr(priv, tid, mac);
+			ra_list->tdls_link = true;
+			tx_info->flags |= MWIFIEX_BUF_FLAG_TDLS_PKT;
+		} else {
+			tid_list = &priv->wmm.tid_tbl_ptr[tid_down].ra_list;
+			if (!list_empty(tid_list))
+				ra_list = list_first_entry(tid_list,
+					      struct mwifiex_ra_list_tbl, list);
+			else
+				ra_list = NULL;
+			tx_info->flags &= ~MWIFIEX_BUF_FLAG_TDLS_PKT;
+		}
+
+		if (!ra_list) {
+			mwifiex_write_data_complete(priv->adapter, skb, 0, -1);
+			continue;
+		}
+
+		skb_queue_tail(&ra_list->skb_head, skb);
+
+		ra_list->ba_pkt_count++;
+		ra_list->total_pkt_count++;
+
+		if (atomic_read(&priv->wmm.highest_queued_prio) <
+						       tos_to_tid_inv[tid_down])
+			atomic_set(&priv->wmm.highest_queued_prio,
+				   tos_to_tid_inv[tid_down]);
+
+		atomic_inc(&priv->wmm.tx_pkts_queued);
+	}
+
+	spin_unlock_irqrestore(&priv->wmm.ra_list_spinlock, flags);
+	return;
+}
+
+static void mwifiex_hold_tdls_packets(struct mwifiex_private *priv, u8 *mac)
+{
+	struct mwifiex_ra_list_tbl *ra_list;
+	struct list_head *ra_list_head;
+	struct sk_buff *skb, *tmp;
+	unsigned long flags;
+	int i;
+
+	dev_dbg(priv->adapter->dev, "%s: %pM\n", __func__, mac);
+	spin_lock_irqsave(&priv->wmm.ra_list_spinlock, flags);
+
+	for (i = 0; i < MAX_NUM_TID; i++) {
+		if (!list_empty(&priv->wmm.tid_tbl_ptr[i].ra_list)) {
+			ra_list_head = &priv->wmm.tid_tbl_ptr[i].ra_list;
+			list_for_each_entry(ra_list, ra_list_head, list) {
+				skb_queue_walk_safe(&ra_list->skb_head, skb,
+						    tmp) {
+					if (!ether_addr_equal(mac, skb->data))
+						continue;
+					__skb_unlink(skb, &ra_list->skb_head);
+					atomic_dec(&priv->wmm.tx_pkts_queued);
+					ra_list->total_pkt_count--;
+					skb_queue_tail(&priv->tdls_txq, skb);
+				}
+			}
+		}
+	}
+
+	spin_unlock_irqrestore(&priv->wmm.ra_list_spinlock, flags);
+	return;
+}
+
+/* This function appends rate TLV to scan config command. */
+static int
+mwifiex_tdls_append_rates_ie(struct mwifiex_private *priv,
+			     struct sk_buff *skb)
+{
+	u8 rates[MWIFIEX_SUPPORTED_RATES], *pos;
+	u16 rates_size, supp_rates_size, ext_rates_size;
+
+	memset(rates, 0, sizeof(rates));
+	rates_size = mwifiex_get_supported_rates(priv, rates);
+
+	supp_rates_size = min_t(u16, rates_size, MWIFIEX_TDLS_SUPPORTED_RATES);
+
+	if (skb_tailroom(skb) < rates_size + 4) {
+		dev_err(priv->adapter->dev,
+			"Insuffient space while adding rates\n");
+		return -ENOMEM;
+	}
+
+	pos = skb_put(skb, supp_rates_size + 2);
+	*pos++ = WLAN_EID_SUPP_RATES;
+	*pos++ = supp_rates_size;
+	memcpy(pos, rates, supp_rates_size);
+
+	if (rates_size > MWIFIEX_TDLS_SUPPORTED_RATES) {
+		ext_rates_size = rates_size - MWIFIEX_TDLS_SUPPORTED_RATES;
+		pos = skb_put(skb, ext_rates_size + 2);
+		*pos++ = WLAN_EID_EXT_SUPP_RATES;
+		*pos++ = ext_rates_size;
+		memcpy(pos, rates + MWIFIEX_TDLS_SUPPORTED_RATES,
+		       ext_rates_size);
+	}
+
+	return 0;
+}
+
+static void mwifiex_tdls_add_aid(struct mwifiex_private *priv,
+				struct sk_buff *skb)
+{
+	struct ieee_types_assoc_rsp *assoc_rsp;
+	u8 *pos;
+
+	assoc_rsp = (struct ieee_types_assoc_rsp *)&priv->assoc_rsp_buf;
+	pos = (void *)skb_put(skb, 4);
+	*pos++ = WLAN_EID_AID;
+	*pos++ = 2;
+	*pos++ = le16_to_cpu(assoc_rsp->a_id);
+
+	return;
+}
+
+static int mwifiex_tdls_add_vht_capab(struct mwifiex_private *priv,
+				      struct sk_buff *skb)
+{
+	struct ieee80211_vht_cap vht_cap;
+	u8 *pos;
+
+	pos = (void *)skb_put(skb, sizeof(struct ieee80211_vht_cap) + 2);
+	*pos++ = WLAN_EID_VHT_CAPABILITY;
+	*pos++ = sizeof(struct ieee80211_vht_cap);
+
+	memset(&vht_cap, 0, sizeof(struct ieee80211_vht_cap));
+
+	mwifiex_fill_vht_cap_tlv(priv, &vht_cap, priv->curr_bss_params.band);
+	memcpy(pos, &vht_cap, sizeof(vht_cap));
+
+	return 0;
+}
+
+static int mwifiex_tdls_add_vht_oper(struct mwifiex_private *priv,
+				     u8 *mac, struct sk_buff *skb)
+{
+	struct mwifiex_bssdescriptor *bss_desc;
+	struct ieee80211_vht_operation *vht_oper;
+	struct ieee80211_vht_cap *vht_cap, *ap_vht_cap = NULL;
+	struct mwifiex_sta_node *sta_ptr;
+	struct mwifiex_adapter *adapter = priv->adapter;
+	u8 supp_chwd_set, peer_supp_chwd_set;
+	u8 *pos, ap_supp_chwd_set, chan_bw;
+	u16 mcs_map_user, mcs_map_resp, mcs_map_result;
+	u16 mcs_user, mcs_resp, nss;
+	u32 usr_vht_cap_info;
+
+	bss_desc = &priv->curr_bss_params.bss_descriptor;
+
+	sta_ptr = mwifiex_get_sta_entry(priv, mac);
+	if (unlikely(!sta_ptr)) {
+		dev_warn(adapter->dev, "TDLS peer station not found in list\n");
+		return -1;
+	}
+
+	if (!mwifiex_is_bss_in_11ac_mode(priv)) {
+		if (sta_ptr->tdls_cap.extcap.ext_capab[7] &
+		   WLAN_EXT_CAPA8_TDLS_WIDE_BW_ENABLED) {
+			dev_dbg(adapter->dev,
+				"TDLS peer doesn't support wider bandwitdh\n");
+			return 0;
+		}
+	} else {
+		ap_vht_cap = bss_desc->bcn_vht_cap;
+	}
+
+	pos = (void *)skb_put(skb, sizeof(struct ieee80211_vht_operation) + 2);
+	*pos++ = WLAN_EID_VHT_OPERATION;
+	*pos++ = sizeof(struct ieee80211_vht_operation);
+	vht_oper = (struct ieee80211_vht_operation *)pos;
+
+	if (bss_desc->bss_band & BAND_A)
+		usr_vht_cap_info = adapter->usr_dot_11ac_dev_cap_a;
+	else
+		usr_vht_cap_info = adapter->usr_dot_11ac_dev_cap_bg;
+
+	/* find the minmum bandwith between AP/TDLS peers */
+	vht_cap = &sta_ptr->tdls_cap.vhtcap;
+	supp_chwd_set = GET_VHTCAP_CHWDSET(usr_vht_cap_info);
+	peer_supp_chwd_set =
+			 GET_VHTCAP_CHWDSET(le32_to_cpu(vht_cap->vht_cap_info));
+	supp_chwd_set = min_t(u8, supp_chwd_set, peer_supp_chwd_set);
+
+	/* We need check AP's bandwidth when TDLS_WIDER_BANDWIDTH is off */
+
+	if (ap_vht_cap && sta_ptr->tdls_cap.extcap.ext_capab[7] &
+	    WLAN_EXT_CAPA8_TDLS_WIDE_BW_ENABLED) {
+		ap_supp_chwd_set =
+		      GET_VHTCAP_CHWDSET(le32_to_cpu(ap_vht_cap->vht_cap_info));
+		supp_chwd_set = min_t(u8, supp_chwd_set, ap_supp_chwd_set);
+	}
+
+	switch (supp_chwd_set) {
+	case IEEE80211_VHT_CHANWIDTH_80MHZ:
+		vht_oper->chan_width = IEEE80211_VHT_CHANWIDTH_80MHZ;
+		break;
+	case IEEE80211_VHT_CHANWIDTH_160MHZ:
+		vht_oper->chan_width = IEEE80211_VHT_CHANWIDTH_160MHZ;
+		break;
+	case IEEE80211_VHT_CHANWIDTH_80P80MHZ:
+		vht_oper->chan_width = IEEE80211_VHT_CHANWIDTH_80P80MHZ;
+		break;
+	default:
+		vht_oper->chan_width = IEEE80211_VHT_CHANWIDTH_USE_HT;
+		break;
+	}
+
+	mcs_map_user = GET_DEVRXMCSMAP(adapter->usr_dot_11ac_mcs_support);
+	mcs_map_resp = le16_to_cpu(vht_cap->supp_mcs.rx_mcs_map);
+	mcs_map_result = 0;
+
+	for (nss = 1; nss <= 8; nss++) {
+		mcs_user = GET_VHTNSSMCS(mcs_map_user, nss);
+		mcs_resp = GET_VHTNSSMCS(mcs_map_resp, nss);
+
+		if ((mcs_user == IEEE80211_VHT_MCS_NOT_SUPPORTED) ||
+		    (mcs_resp == IEEE80211_VHT_MCS_NOT_SUPPORTED))
+			SET_VHTNSSMCS(mcs_map_result, nss,
+				      IEEE80211_VHT_MCS_NOT_SUPPORTED);
+		else
+			SET_VHTNSSMCS(mcs_map_result, nss,
+				      min_t(u16, mcs_user, mcs_resp));
+	}
+
+	vht_oper->basic_mcs_set = cpu_to_le16(mcs_map_result);
+
+	switch (vht_oper->chan_width) {
+	case IEEE80211_VHT_CHANWIDTH_80MHZ:
+		chan_bw = IEEE80211_VHT_CHANWIDTH_80MHZ;
+		break;
+	case IEEE80211_VHT_CHANWIDTH_160MHZ:
+		chan_bw = IEEE80211_VHT_CHANWIDTH_160MHZ;
+		break;
+	case IEEE80211_VHT_CHANWIDTH_80P80MHZ:
+		chan_bw = IEEE80211_VHT_CHANWIDTH_80MHZ;
+		break;
+	default:
+		chan_bw = IEEE80211_VHT_CHANWIDTH_USE_HT;
+		break;
+	}
+	vht_oper->center_freq_seg1_idx =
+			mwifiex_get_center_freq_index(priv, BAND_AAC,
+						      bss_desc->channel,
+						      chan_bw);
+
+	return 0;
+}
+
+static void mwifiex_tdls_add_ext_capab(struct mwifiex_private *priv,
+				       struct sk_buff *skb)
+{
+	struct ieee_types_extcap *extcap;
+
+	extcap = (void *)skb_put(skb, sizeof(struct ieee_types_extcap));
+	extcap->ieee_hdr.element_id = WLAN_EID_EXT_CAPABILITY;
+	extcap->ieee_hdr.len = 8;
+	memset(extcap->ext_capab, 0, 8);
+	extcap->ext_capab[4] |= WLAN_EXT_CAPA5_TDLS_ENABLED;
+
+	if (priv->adapter->is_hw_11ac_capable)
+		extcap->ext_capab[7] |= WLAN_EXT_CAPA8_TDLS_WIDE_BW_ENABLED;
+}
+
+static void mwifiex_tdls_add_qos_capab(struct sk_buff *skb)
+{
+	u8 *pos = (void *)skb_put(skb, 3);
+
+	*pos++ = WLAN_EID_QOS_CAPA;
+	*pos++ = 1;
+	*pos++ = MWIFIEX_TDLS_DEF_QOS_CAPAB;
+}
+
+static int mwifiex_prep_tdls_encap_data(struct mwifiex_private *priv,
+			     u8 *peer, u8 action_code, u8 dialog_token,
+			     u16 status_code, struct sk_buff *skb)
+{
+	struct ieee80211_tdls_data *tf;
+	int ret;
+	u16 capab;
+	struct ieee80211_ht_cap *ht_cap;
+	u8 radio, *pos;
+
+	capab = priv->curr_bss_params.bss_descriptor.cap_info_bitmap;
+
+	tf = (void *)skb_put(skb, offsetof(struct ieee80211_tdls_data, u));
+	memcpy(tf->da, peer, ETH_ALEN);
+	memcpy(tf->sa, priv->curr_addr, ETH_ALEN);
+	tf->ether_type = cpu_to_be16(ETH_P_TDLS);
+	tf->payload_type = WLAN_TDLS_SNAP_RFTYPE;
+
+	switch (action_code) {
+	case WLAN_TDLS_SETUP_REQUEST:
+		tf->category = WLAN_CATEGORY_TDLS;
+		tf->action_code = WLAN_TDLS_SETUP_REQUEST;
+		skb_put(skb, sizeof(tf->u.setup_req));
+		tf->u.setup_req.dialog_token = dialog_token;
+		tf->u.setup_req.capability = cpu_to_le16(capab);
+		ret = mwifiex_tdls_append_rates_ie(priv, skb);
+		if (ret) {
+			dev_kfree_skb_any(skb);
+			return ret;
+		}
+
+		pos = (void *)skb_put(skb, sizeof(struct ieee80211_ht_cap) + 2);
+		*pos++ = WLAN_EID_HT_CAPABILITY;
+		*pos++ = sizeof(struct ieee80211_ht_cap);
+		ht_cap = (void *)pos;
+		radio = mwifiex_band_to_radio_type(priv->curr_bss_params.band);
+		ret = mwifiex_fill_cap_info(priv, radio, ht_cap);
+		if (ret) {
+			dev_kfree_skb_any(skb);
+			return ret;
+		}
+
+		if (priv->adapter->is_hw_11ac_capable) {
+			ret = mwifiex_tdls_add_vht_capab(priv, skb);
+			if (ret) {
+				dev_kfree_skb_any(skb);
+				return ret;
+			}
+			mwifiex_tdls_add_aid(priv, skb);
+		}
+
+		mwifiex_tdls_add_ext_capab(priv, skb);
+		mwifiex_tdls_add_qos_capab(skb);
+		break;
+
+	case WLAN_TDLS_SETUP_RESPONSE:
+		tf->category = WLAN_CATEGORY_TDLS;
+		tf->action_code = WLAN_TDLS_SETUP_RESPONSE;
+		skb_put(skb, sizeof(tf->u.setup_resp));
+		tf->u.setup_resp.status_code = cpu_to_le16(status_code);
+		tf->u.setup_resp.dialog_token = dialog_token;
+		tf->u.setup_resp.capability = cpu_to_le16(capab);
+		ret = mwifiex_tdls_append_rates_ie(priv, skb);
+		if (ret) {
+			dev_kfree_skb_any(skb);
+			return ret;
+		}
+
+		pos = (void *)skb_put(skb, sizeof(struct ieee80211_ht_cap) + 2);
+		*pos++ = WLAN_EID_HT_CAPABILITY;
+		*pos++ = sizeof(struct ieee80211_ht_cap);
+		ht_cap = (void *)pos;
+		radio = mwifiex_band_to_radio_type(priv->curr_bss_params.band);
+		ret = mwifiex_fill_cap_info(priv, radio, ht_cap);
+		if (ret) {
+			dev_kfree_skb_any(skb);
+			return ret;
+		}
+
+		if (priv->adapter->is_hw_11ac_capable) {
+			ret = mwifiex_tdls_add_vht_capab(priv, skb);
+			if (ret) {
+				dev_kfree_skb_any(skb);
+				return ret;
+			}
+			mwifiex_tdls_add_aid(priv, skb);
+		}
+
+		mwifiex_tdls_add_ext_capab(priv, skb);
+		mwifiex_tdls_add_qos_capab(skb);
+		break;
+
+	case WLAN_TDLS_SETUP_CONFIRM:
+		tf->category = WLAN_CATEGORY_TDLS;
+		tf->action_code = WLAN_TDLS_SETUP_CONFIRM;
+		skb_put(skb, sizeof(tf->u.setup_cfm));
+		tf->u.setup_cfm.status_code = cpu_to_le16(status_code);
+		tf->u.setup_cfm.dialog_token = dialog_token;
+		if (priv->adapter->is_hw_11ac_capable) {
+			ret = mwifiex_tdls_add_vht_oper(priv, peer, skb);
+			if (ret) {
+				dev_kfree_skb_any(skb);
+				return ret;
+			}
+		}
+		break;
+
+	case WLAN_TDLS_TEARDOWN:
+		tf->category = WLAN_CATEGORY_TDLS;
+		tf->action_code = WLAN_TDLS_TEARDOWN;
+		skb_put(skb, sizeof(tf->u.teardown));
+		tf->u.teardown.reason_code = cpu_to_le16(status_code);
+		break;
+
+	case WLAN_TDLS_DISCOVERY_REQUEST:
+		tf->category = WLAN_CATEGORY_TDLS;
+		tf->action_code = WLAN_TDLS_DISCOVERY_REQUEST;
+		skb_put(skb, sizeof(tf->u.discover_req));
+		tf->u.discover_req.dialog_token = dialog_token;
+		break;
+	default:
+		dev_err(priv->adapter->dev, "Unknown TDLS frame type.\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void
+mwifiex_tdls_add_link_ie(struct sk_buff *skb, u8 *src_addr, u8 *peer, u8 *bssid)
+{
+	struct ieee80211_tdls_lnkie *lnkid;
+
+	lnkid = (void *)skb_put(skb, sizeof(struct ieee80211_tdls_lnkie));
+	lnkid->ie_type = WLAN_EID_LINK_ID;
+	lnkid->ie_len = sizeof(struct ieee80211_tdls_lnkie) -
+			sizeof(struct ieee_types_header);
+
+	memcpy(lnkid->bssid, bssid, ETH_ALEN);
+	memcpy(lnkid->init_sta, src_addr, ETH_ALEN);
+	memcpy(lnkid->resp_sta, peer, ETH_ALEN);
+}
+
+int mwifiex_send_tdls_data_frame(struct mwifiex_private *priv,
+				 u8 *peer, u8 action_code, u8 dialog_token,
+				 u16 status_code, const u8 *extra_ies,
+				 size_t extra_ies_len)
+{
+	struct sk_buff *skb;
+	struct mwifiex_txinfo *tx_info;
+	struct timeval tv;
+	int ret;
+	u16 skb_len;
+
+	skb_len = MWIFIEX_MIN_DATA_HEADER_LEN +
+		  max(sizeof(struct ieee80211_mgmt),
+		      sizeof(struct ieee80211_tdls_data)) +
+		  MWIFIEX_MGMT_FRAME_HEADER_SIZE +
+		  MWIFIEX_SUPPORTED_RATES +
+		  3 + /* Qos Info */
+		  sizeof(struct ieee_types_extcap) +
+		  sizeof(struct ieee80211_ht_cap) +
+		  sizeof(struct ieee_types_bss_co_2040) +
+		  sizeof(struct ieee80211_ht_operation) +
+		  sizeof(struct ieee80211_tdls_lnkie) +
+		  extra_ies_len;
+
+	if (priv->adapter->is_hw_11ac_capable)
+		skb_len += sizeof(struct ieee_types_vht_cap) +
+			   sizeof(struct ieee_types_vht_oper) +
+			   sizeof(struct ieee_types_aid);
+
+	skb = dev_alloc_skb(skb_len);
+	if (!skb) {
+		dev_err(priv->adapter->dev,
+			"allocate skb failed for management frame\n");
+		return -ENOMEM;
+	}
+	skb_reserve(skb, MWIFIEX_MIN_DATA_HEADER_LEN);
+
+	switch (action_code) {
+	case WLAN_TDLS_SETUP_REQUEST:
+	case WLAN_TDLS_SETUP_CONFIRM:
+	case WLAN_TDLS_TEARDOWN:
+	case WLAN_TDLS_DISCOVERY_REQUEST:
+		ret = mwifiex_prep_tdls_encap_data(priv, peer, action_code,
+						   dialog_token, status_code,
+						   skb);
+		if (ret) {
+			dev_kfree_skb_any(skb);
+			return ret;
+		}
+		if (extra_ies_len)
+			memcpy(skb_put(skb, extra_ies_len), extra_ies,
+			       extra_ies_len);
+		mwifiex_tdls_add_link_ie(skb, priv->curr_addr, peer,
+					 priv->cfg_bssid);
+		break;
+	case WLAN_TDLS_SETUP_RESPONSE:
+		ret = mwifiex_prep_tdls_encap_data(priv, peer, action_code,
+						   dialog_token, status_code,
+						   skb);
+		if (ret) {
+			dev_kfree_skb_any(skb);
+			return ret;
+		}
+		if (extra_ies_len)
+			memcpy(skb_put(skb, extra_ies_len), extra_ies,
+			       extra_ies_len);
+		mwifiex_tdls_add_link_ie(skb, peer, priv->curr_addr,
+					 priv->cfg_bssid);
+		break;
+	}
+
+	switch (action_code) {
+	case WLAN_TDLS_SETUP_REQUEST:
+	case WLAN_TDLS_SETUP_RESPONSE:
+		skb->priority = MWIFIEX_PRIO_BK;
+		break;
+	default:
+		skb->priority = MWIFIEX_PRIO_VI;
+		break;
+	}
+
+	tx_info = MWIFIEX_SKB_TXCB(skb);
+	tx_info->bss_num = priv->bss_num;
+	tx_info->bss_type = priv->bss_type;
+
+	do_gettimeofday(&tv);
+	skb->tstamp = timeval_to_ktime(tv);
+	mwifiex_queue_tx_pkt(priv, skb);
+
+	return 0;
+}
+
+static int
+mwifiex_construct_tdls_action_frame(struct mwifiex_private *priv, u8 *peer,
+				    u8 action_code, u8 dialog_token,
+				    u16 status_code, struct sk_buff *skb)
+{
+	struct ieee80211_mgmt *mgmt;
+	u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	int ret;
+	u16 capab;
+	struct ieee80211_ht_cap *ht_cap;
+	u8 radio, *pos;
+
+	capab = priv->curr_bss_params.bss_descriptor.cap_info_bitmap;
+
+	mgmt = (void *)skb_put(skb, offsetof(struct ieee80211_mgmt, u));
+
+	memset(mgmt, 0, 24);
+	memcpy(mgmt->da, peer, ETH_ALEN);
+	memcpy(mgmt->sa, priv->curr_addr, ETH_ALEN);
+	memcpy(mgmt->bssid, priv->cfg_bssid, ETH_ALEN);
+	mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+					  IEEE80211_STYPE_ACTION);
+
+	/* add address 4 */
+	pos = skb_put(skb, ETH_ALEN);
+
+	switch (action_code) {
+	case WLAN_PUB_ACTION_TDLS_DISCOVER_RES:
+		skb_put(skb, sizeof(mgmt->u.action.u.tdls_discover_resp) + 1);
+		mgmt->u.action.category = WLAN_CATEGORY_PUBLIC;
+		mgmt->u.action.u.tdls_discover_resp.action_code =
+					      WLAN_PUB_ACTION_TDLS_DISCOVER_RES;
+		mgmt->u.action.u.tdls_discover_resp.dialog_token =
+								   dialog_token;
+		mgmt->u.action.u.tdls_discover_resp.capability =
+							     cpu_to_le16(capab);
+		/* move back for addr4 */
+		memmove(pos + ETH_ALEN, &mgmt->u.action.category,
+			sizeof(mgmt->u.action.u.tdls_discover_resp));
+		/* init address 4 */
+		memcpy(pos, bc_addr, ETH_ALEN);
+
+		ret = mwifiex_tdls_append_rates_ie(priv, skb);
+		if (ret) {
+			dev_kfree_skb_any(skb);
+			return ret;
+		}
+
+		pos = (void *)skb_put(skb, sizeof(struct ieee80211_ht_cap) + 2);
+		*pos++ = WLAN_EID_HT_CAPABILITY;
+		*pos++ = sizeof(struct ieee80211_ht_cap);
+		ht_cap = (void *)pos;
+		radio = mwifiex_band_to_radio_type(priv->curr_bss_params.band);
+		ret = mwifiex_fill_cap_info(priv, radio, ht_cap);
+		if (ret) {
+			dev_kfree_skb_any(skb);
+			return ret;
+		}
+
+		if (priv->adapter->is_hw_11ac_capable) {
+			ret = mwifiex_tdls_add_vht_capab(priv, skb);
+			if (ret) {
+				dev_kfree_skb_any(skb);
+				return ret;
+			}
+			mwifiex_tdls_add_aid(priv, skb);
+		}
+
+		mwifiex_tdls_add_ext_capab(priv, skb);
+		mwifiex_tdls_add_qos_capab(skb);
+		break;
+	default:
+		dev_err(priv->adapter->dev, "Unknown TDLS action frame type\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int mwifiex_send_tdls_action_frame(struct mwifiex_private *priv,
+				 u8 *peer, u8 action_code, u8 dialog_token,
+				 u16 status_code, const u8 *extra_ies,
+				 size_t extra_ies_len)
+{
+	struct sk_buff *skb;
+	struct mwifiex_txinfo *tx_info;
+	struct timeval tv;
+	u8 *pos;
+	u32 pkt_type, tx_control;
+	u16 pkt_len, skb_len;
+
+	skb_len = MWIFIEX_MIN_DATA_HEADER_LEN +
+		  max(sizeof(struct ieee80211_mgmt),
+		      sizeof(struct ieee80211_tdls_data)) +
+		  MWIFIEX_MGMT_FRAME_HEADER_SIZE +
+		  MWIFIEX_SUPPORTED_RATES +
+		  sizeof(struct ieee_types_extcap) +
+		  sizeof(struct ieee80211_ht_cap) +
+		  sizeof(struct ieee_types_bss_co_2040) +
+		  sizeof(struct ieee80211_ht_operation) +
+		  sizeof(struct ieee80211_tdls_lnkie) +
+		  extra_ies_len +
+		  3 + /* Qos Info */
+		  ETH_ALEN; /* Address4 */
+
+	if (priv->adapter->is_hw_11ac_capable)
+		skb_len += sizeof(struct ieee_types_vht_cap) +
+			   sizeof(struct ieee_types_vht_oper) +
+			   sizeof(struct ieee_types_aid);
+
+	skb = dev_alloc_skb(skb_len);
+	if (!skb) {
+		dev_err(priv->adapter->dev,
+			"allocate skb failed for management frame\n");
+		return -ENOMEM;
+	}
+
+	skb_reserve(skb, MWIFIEX_MIN_DATA_HEADER_LEN);
+
+	pkt_type = PKT_TYPE_MGMT;
+	tx_control = 0;
+	pos = skb_put(skb, MWIFIEX_MGMT_FRAME_HEADER_SIZE + sizeof(pkt_len));
+	memset(pos, 0, MWIFIEX_MGMT_FRAME_HEADER_SIZE + sizeof(pkt_len));
+	memcpy(pos, &pkt_type, sizeof(pkt_type));
+	memcpy(pos + sizeof(pkt_type), &tx_control, sizeof(tx_control));
+
+	if (mwifiex_construct_tdls_action_frame(priv, peer, action_code,
+						dialog_token, status_code,
+						skb)) {
+		dev_kfree_skb_any(skb);
+		return -EINVAL;
+	}
+
+	if (extra_ies_len)
+		memcpy(skb_put(skb, extra_ies_len), extra_ies, extra_ies_len);
+
+	/* the TDLS link IE is always added last we are the responder */
+
+	mwifiex_tdls_add_link_ie(skb, peer, priv->curr_addr,
+				 priv->cfg_bssid);
+
+	skb->priority = MWIFIEX_PRIO_VI;
+
+	tx_info = MWIFIEX_SKB_TXCB(skb);
+	tx_info->bss_num = priv->bss_num;
+	tx_info->bss_type = priv->bss_type;
+	tx_info->flags |= MWIFIEX_BUF_FLAG_TDLS_PKT;
+
+	pkt_len = skb->len - MWIFIEX_MGMT_FRAME_HEADER_SIZE - sizeof(pkt_len);
+	memcpy(skb->data + MWIFIEX_MGMT_FRAME_HEADER_SIZE, &pkt_len,
+	       sizeof(pkt_len));
+	do_gettimeofday(&tv);
+	skb->tstamp = timeval_to_ktime(tv);
+	mwifiex_queue_tx_pkt(priv, skb);
+
+	return 0;
+}
+
+/* This function process tdls action frame from peer.
+ * Peer capabilities are stored into station node structure.
+ */
+void mwifiex_process_tdls_action_frame(struct mwifiex_private *priv,
+				       u8 *buf, int len)
+{
+	struct mwifiex_sta_node *sta_ptr;
+	u8 *peer, *pos, *end;
+	u8 i, action, basic;
+	int ie_len = 0;
+
+	if (len < (sizeof(struct ethhdr) + 3))
+		return;
+	if (*(u8 *)(buf + sizeof(struct ethhdr)) != WLAN_TDLS_SNAP_RFTYPE)
+		return;
+	if (*(u8 *)(buf + sizeof(struct ethhdr) + 1) != WLAN_CATEGORY_TDLS)
+		return;
+
+	peer = buf + ETH_ALEN;
+	action = *(u8 *)(buf + sizeof(struct ethhdr) + 2);
+
+	/* just handle TDLS setup request/response/confirm */
+	if (action > WLAN_TDLS_SETUP_CONFIRM)
+		return;
+
+	dev_dbg(priv->adapter->dev,
+		"rx:tdls action: peer=%pM, action=%d\n", peer, action);
+
+	sta_ptr = mwifiex_add_sta_entry(priv, peer);
+	if (!sta_ptr)
+		return;
+
+	switch (action) {
+	case WLAN_TDLS_SETUP_REQUEST:
+		if (len < (sizeof(struct ethhdr) + TDLS_REQ_FIX_LEN))
+			return;
+
+		pos = buf + sizeof(struct ethhdr) + 4;
+		/* payload 1+ category 1 + action 1 + dialog 1 */
+		sta_ptr->tdls_cap.capab = cpu_to_le16(*(u16 *)pos);
+		ie_len = len - sizeof(struct ethhdr) - TDLS_REQ_FIX_LEN;
+		pos += 2;
+		break;
+
+	case WLAN_TDLS_SETUP_RESPONSE:
+		if (len < (sizeof(struct ethhdr) + TDLS_RESP_FIX_LEN))
+			return;
+		/* payload 1+ category 1 + action 1 + dialog 1 + status code 2*/
+		pos = buf + sizeof(struct ethhdr) + 6;
+		sta_ptr->tdls_cap.capab = cpu_to_le16(*(u16 *)pos);
+		ie_len = len - sizeof(struct ethhdr) - TDLS_RESP_FIX_LEN;
+		pos += 2;
+		break;
+
+	case WLAN_TDLS_SETUP_CONFIRM:
+		if (len < (sizeof(struct ethhdr) + TDLS_CONFIRM_FIX_LEN))
+			return;
+		pos = buf + sizeof(struct ethhdr) + TDLS_CONFIRM_FIX_LEN;
+		ie_len = len - sizeof(struct ethhdr) - TDLS_CONFIRM_FIX_LEN;
+		break;
+	default:
+		dev_warn(priv->adapter->dev, "Unknown TDLS frame type.\n");
+		return;
+	}
+
+	for (end = pos + ie_len; pos + 1 < end; pos += 2 + pos[1]) {
+		if (pos + 2 + pos[1] > end)
+			break;
+
+		switch (*pos) {
+		case WLAN_EID_SUPP_RATES:
+			sta_ptr->tdls_cap.rates_len = pos[1];
+			for (i = 0; i < pos[1]; i++)
+				sta_ptr->tdls_cap.rates[i] = pos[i + 2];
+			break;
+
+		case WLAN_EID_EXT_SUPP_RATES:
+			basic = sta_ptr->tdls_cap.rates_len;
+			for (i = 0; i < pos[1]; i++)
+				sta_ptr->tdls_cap.rates[basic + i] = pos[i + 2];
+			sta_ptr->tdls_cap.rates_len += pos[1];
+			break;
+		case WLAN_EID_HT_CAPABILITY:
+			memcpy((u8 *)&sta_ptr->tdls_cap.ht_capb, pos,
+			       sizeof(struct ieee80211_ht_cap));
+			sta_ptr->is_11n_enabled = 1;
+			break;
+		case WLAN_EID_HT_OPERATION:
+			memcpy(&sta_ptr->tdls_cap.ht_oper, pos,
+			       sizeof(struct ieee80211_ht_operation));
+			break;
+		case WLAN_EID_BSS_COEX_2040:
+			sta_ptr->tdls_cap.coex_2040 = pos[2];
+			break;
+		case WLAN_EID_EXT_CAPABILITY:
+			memcpy((u8 *)&sta_ptr->tdls_cap.extcap, pos,
+			       sizeof(struct ieee_types_header) +
+			       min_t(u8, pos[1], 8));
+			break;
+		case WLAN_EID_RSN:
+			memcpy((u8 *)&sta_ptr->tdls_cap.rsn_ie, pos,
+			       sizeof(struct ieee_types_header) + pos[1]);
+			break;
+		case WLAN_EID_QOS_CAPA:
+			sta_ptr->tdls_cap.qos_info = pos[2];
+			break;
+		case WLAN_EID_VHT_OPERATION:
+			if (priv->adapter->is_hw_11ac_capable)
+				memcpy(&sta_ptr->tdls_cap.vhtoper, pos,
+				       sizeof(struct ieee80211_vht_operation));
+			break;
+		case WLAN_EID_VHT_CAPABILITY:
+			if (priv->adapter->is_hw_11ac_capable) {
+				memcpy((u8 *)&sta_ptr->tdls_cap.vhtcap, pos,
+				       sizeof(struct ieee80211_vht_cap));
+				sta_ptr->is_11ac_enabled = 1;
+			}
+			break;
+		case WLAN_EID_AID:
+			if (priv->adapter->is_hw_11ac_capable)
+				sta_ptr->tdls_cap.aid =
+					      le16_to_cpu(*(__le16 *)(pos + 2));
+		default:
+			break;
+		}
+	}
+
+	return;
+}
+
+static int
+mwifiex_tdls_process_config_link(struct mwifiex_private *priv, u8 *peer)
+{
+	struct mwifiex_sta_node *sta_ptr;
+	struct mwifiex_ds_tdls_oper tdls_oper;
+
+	memset(&tdls_oper, 0, sizeof(struct mwifiex_ds_tdls_oper));
+	sta_ptr = mwifiex_get_sta_entry(priv, peer);
+
+	if (!sta_ptr || sta_ptr->tdls_status == TDLS_SETUP_FAILURE) {
+		dev_err(priv->adapter->dev,
+			"link absent for peer %pM; cannot config\n", peer);
+		return -EINVAL;
+	}
+
+	memcpy(&tdls_oper.peer_mac, peer, ETH_ALEN);
+	tdls_oper.tdls_action = MWIFIEX_TDLS_CONFIG_LINK;
+	return mwifiex_send_cmd(priv, HostCmd_CMD_TDLS_OPER,
+				HostCmd_ACT_GEN_SET, 0, &tdls_oper, true);
+}
+
+static int
+mwifiex_tdls_process_create_link(struct mwifiex_private *priv, u8 *peer)
+{
+	struct mwifiex_sta_node *sta_ptr;
+	struct mwifiex_ds_tdls_oper tdls_oper;
+
+	memset(&tdls_oper, 0, sizeof(struct mwifiex_ds_tdls_oper));
+	sta_ptr = mwifiex_get_sta_entry(priv, peer);
+
+	if (sta_ptr && sta_ptr->tdls_status == TDLS_SETUP_INPROGRESS) {
+		dev_dbg(priv->adapter->dev,
+			"Setup already in progress for peer %pM\n", peer);
+		return 0;
+	}
+
+	sta_ptr = mwifiex_add_sta_entry(priv, peer);
+	if (!sta_ptr)
+		return -ENOMEM;
+
+	sta_ptr->tdls_status = TDLS_SETUP_INPROGRESS;
+	mwifiex_hold_tdls_packets(priv, peer);
+	memcpy(&tdls_oper.peer_mac, peer, ETH_ALEN);
+	tdls_oper.tdls_action = MWIFIEX_TDLS_CREATE_LINK;
+	return mwifiex_send_cmd(priv, HostCmd_CMD_TDLS_OPER,
+				HostCmd_ACT_GEN_SET, 0, &tdls_oper, true);
+}
+
+static int
+mwifiex_tdls_process_disable_link(struct mwifiex_private *priv, u8 *peer)
+{
+	struct mwifiex_sta_node *sta_ptr;
+	struct mwifiex_ds_tdls_oper tdls_oper;
+	unsigned long flags;
+
+	memset(&tdls_oper, 0, sizeof(struct mwifiex_ds_tdls_oper));
+	sta_ptr = mwifiex_get_sta_entry(priv, peer);
+
+	if (sta_ptr) {
+		if (sta_ptr->is_11n_enabled) {
+			mwifiex_11n_cleanup_reorder_tbl(priv);
+			spin_lock_irqsave(&priv->wmm.ra_list_spinlock,
+					  flags);
+			mwifiex_11n_delete_all_tx_ba_stream_tbl(priv);
+			spin_unlock_irqrestore(&priv->wmm.ra_list_spinlock,
+					       flags);
+		}
+		mwifiex_del_sta_entry(priv, peer);
+	}
+
+	mwifiex_restore_tdls_packets(priv, peer, TDLS_LINK_TEARDOWN);
+	memcpy(&tdls_oper.peer_mac, peer, ETH_ALEN);
+	tdls_oper.tdls_action = MWIFIEX_TDLS_DISABLE_LINK;
+	return mwifiex_send_cmd(priv, HostCmd_CMD_TDLS_OPER,
+				HostCmd_ACT_GEN_SET, 0, &tdls_oper, true);
+}
+
+static int
+mwifiex_tdls_process_enable_link(struct mwifiex_private *priv, u8 *peer)
+{
+	struct mwifiex_sta_node *sta_ptr;
+	struct ieee80211_mcs_info mcs;
+	unsigned long flags;
+	int i;
+
+	sta_ptr = mwifiex_get_sta_entry(priv, peer);
+
+	if (sta_ptr && (sta_ptr->tdls_status != TDLS_SETUP_FAILURE)) {
+		dev_dbg(priv->adapter->dev,
+			"tdls: enable link %pM success\n", peer);
+
+		sta_ptr->tdls_status = TDLS_SETUP_COMPLETE;
+
+		mcs = sta_ptr->tdls_cap.ht_capb.mcs;
+		if (mcs.rx_mask[0] != 0xff)
+			sta_ptr->is_11n_enabled = true;
+		if (sta_ptr->is_11n_enabled) {
+			if (le16_to_cpu(sta_ptr->tdls_cap.ht_capb.cap_info) &
+			    IEEE80211_HT_CAP_MAX_AMSDU)
+				sta_ptr->max_amsdu =
+					MWIFIEX_TX_DATA_BUF_SIZE_8K;
+			else
+				sta_ptr->max_amsdu =
+					MWIFIEX_TX_DATA_BUF_SIZE_4K;
+
+			for (i = 0; i < MAX_NUM_TID; i++)
+				sta_ptr->ampdu_sta[i] =
+					      priv->aggr_prio_tbl[i].ampdu_user;
+		} else {
+			for (i = 0; i < MAX_NUM_TID; i++)
+				sta_ptr->ampdu_sta[i] = BA_STREAM_NOT_ALLOWED;
+		}
+
+		memset(sta_ptr->rx_seq, 0xff, sizeof(sta_ptr->rx_seq));
+		mwifiex_restore_tdls_packets(priv, peer, TDLS_SETUP_COMPLETE);
+	} else {
+		dev_dbg(priv->adapter->dev,
+			"tdls: enable link %pM failed\n", peer);
+		if (sta_ptr) {
+			mwifiex_11n_cleanup_reorder_tbl(priv);
+			spin_lock_irqsave(&priv->wmm.ra_list_spinlock,
+					  flags);
+			mwifiex_11n_delete_all_tx_ba_stream_tbl(priv);
+			spin_unlock_irqrestore(&priv->wmm.ra_list_spinlock,
+					       flags);
+			mwifiex_del_sta_entry(priv, peer);
+		}
+		mwifiex_restore_tdls_packets(priv, peer, TDLS_LINK_TEARDOWN);
+
+		return -1;
+	}
+
+	return 0;
+}
+
+int mwifiex_tdls_oper(struct mwifiex_private *priv, u8 *peer, u8 action)
+{
+	switch (action) {
+	case MWIFIEX_TDLS_ENABLE_LINK:
+		return mwifiex_tdls_process_enable_link(priv, peer);
+	case MWIFIEX_TDLS_DISABLE_LINK:
+		return mwifiex_tdls_process_disable_link(priv, peer);
+	case MWIFIEX_TDLS_CREATE_LINK:
+		return mwifiex_tdls_process_create_link(priv, peer);
+	case MWIFIEX_TDLS_CONFIG_LINK:
+		return mwifiex_tdls_process_config_link(priv, peer);
+	}
+	return 0;
+}
+
+int mwifiex_get_tdls_link_status(struct mwifiex_private *priv, u8 *mac)
+{
+	struct mwifiex_sta_node *sta_ptr;
+
+	sta_ptr = mwifiex_get_sta_entry(priv, mac);
+	if (sta_ptr)
+		return sta_ptr->tdls_status;
+
+	return TDLS_NOT_SETUP;
+}
+
+void mwifiex_disable_all_tdls_links(struct mwifiex_private *priv)
+{
+	struct mwifiex_sta_node *sta_ptr;
+	struct mwifiex_ds_tdls_oper tdls_oper;
+	unsigned long flags;
+
+	if (list_empty(&priv->sta_list))
+		return;
+
+	list_for_each_entry(sta_ptr, &priv->sta_list, list) {
+		memset(&tdls_oper, 0, sizeof(struct mwifiex_ds_tdls_oper));
+
+		if (sta_ptr->is_11n_enabled) {
+			mwifiex_11n_cleanup_reorder_tbl(priv);
+			spin_lock_irqsave(&priv->wmm.ra_list_spinlock,
+					  flags);
+			mwifiex_11n_delete_all_tx_ba_stream_tbl(priv);
+			spin_unlock_irqrestore(&priv->wmm.ra_list_spinlock,
+					       flags);
+		}
+
+		mwifiex_restore_tdls_packets(priv, sta_ptr->mac_addr,
+					     TDLS_LINK_TEARDOWN);
+		memcpy(&tdls_oper.peer_mac, sta_ptr->mac_addr, ETH_ALEN);
+		tdls_oper.tdls_action = MWIFIEX_TDLS_DISABLE_LINK;
+		if (mwifiex_send_cmd(priv, HostCmd_CMD_TDLS_OPER,
+				     HostCmd_ACT_GEN_SET, 0, &tdls_oper, false))
+			dev_warn(priv->adapter->dev,
+				 "Disable link failed for TDLS peer %pM",
+				 sta_ptr->mac_addr);
+	}
+
+	mwifiex_del_all_sta_list(priv);
+}
diff --git a/drivers/net/wireless/mwifiex/uap_cmd.c b/drivers/net/wireless/mwifiex/uap_cmd.c
index 64424c81b44f..a6a6a53cda40 100644
--- a/drivers/net/wireless/mwifiex/uap_cmd.c
+++ b/drivers/net/wireless/mwifiex/uap_cmd.c
@@ -226,8 +226,8 @@ void mwifiex_set_vht_width(struct mwifiex_private *priv,
 	if (ap_11ac_enable && width >= NL80211_CHAN_WIDTH_80)
 		vht_cfg.misc_config |= VHT_BW_80_160_80P80;
 
-	mwifiex_send_cmd_sync(priv, HostCmd_CMD_11AC_CFG,
-			      HostCmd_ACT_GEN_SET, 0, &vht_cfg);
+	mwifiex_send_cmd(priv, HostCmd_CMD_11AC_CFG,
+			 HostCmd_ACT_GEN_SET, 0, &vht_cfg, true);
 
 	return;
 }
diff --git a/drivers/net/wireless/mwifiex/uap_event.c b/drivers/net/wireless/mwifiex/uap_event.c
index 718066577c6c..ae50e916d8f2 100644
--- a/drivers/net/wireless/mwifiex/uap_event.c
+++ b/drivers/net/wireless/mwifiex/uap_event.c
@@ -21,126 +21,8 @@
 #include "main.h"
 #include "11n.h"
 
-/*
- * This function will return the pointer to station entry in station list
- * table which matches specified mac address.
- * This function should be called after acquiring RA list spinlock.
- * NULL is returned if station entry is not found in associated STA list.
- */
-struct mwifiex_sta_node *
-mwifiex_get_sta_entry(struct mwifiex_private *priv, u8 *mac)
-{
-	struct mwifiex_sta_node *node;
-
-	if (!mac)
-		return NULL;
-
-	list_for_each_entry(node, &priv->sta_list, list) {
-		if (!memcmp(node->mac_addr, mac, ETH_ALEN))
-			return node;
-	}
 
-	return NULL;
-}
-
-/*
- * This function will add a sta_node entry to associated station list
- * table with the given mac address.
- * If entry exist already, existing entry is returned.
- * If received mac address is NULL, NULL is returned.
- */
-static struct mwifiex_sta_node *
-mwifiex_add_sta_entry(struct mwifiex_private *priv, u8 *mac)
-{
-	struct mwifiex_sta_node *node;
-	unsigned long flags;
 
-	if (!mac)
-		return NULL;
-
-	spin_lock_irqsave(&priv->sta_list_spinlock, flags);
-	node = mwifiex_get_sta_entry(priv, mac);
-	if (node)
-		goto done;
-
-	node = kzalloc(sizeof(struct mwifiex_sta_node), GFP_ATOMIC);
-	if (!node)
-		goto done;
-
-	memcpy(node->mac_addr, mac, ETH_ALEN);
-	list_add_tail(&node->list, &priv->sta_list);
-
-done:
-	spin_unlock_irqrestore(&priv->sta_list_spinlock, flags);
-	return node;
-}
-
-/*
- * This function will search for HT IE in association request IEs
- * and set station HT parameters accordingly.
- */
-static void
-mwifiex_set_sta_ht_cap(struct mwifiex_private *priv, const u8 *ies,
-		       int ies_len, struct mwifiex_sta_node *node)
-{
-	const struct ieee80211_ht_cap *ht_cap;
-
-	if (!ies)
-		return;
-
-	ht_cap = (void *)cfg80211_find_ie(WLAN_EID_HT_CAPABILITY, ies, ies_len);
-	if (ht_cap) {
-		node->is_11n_enabled = 1;
-		node->max_amsdu = le16_to_cpu(ht_cap->cap_info) &
-				  IEEE80211_HT_CAP_MAX_AMSDU ?
-				  MWIFIEX_TX_DATA_BUF_SIZE_8K :
-				  MWIFIEX_TX_DATA_BUF_SIZE_4K;
-	} else {
-		node->is_11n_enabled = 0;
-	}
-
-	return;
-}
-
-/*
- * This function will delete a station entry from station list
- */
-static void mwifiex_del_sta_entry(struct mwifiex_private *priv, u8 *mac)
-{
-	struct mwifiex_sta_node *node;
-	unsigned long flags;
-
-	spin_lock_irqsave(&priv->sta_list_spinlock, flags);
-
-	node = mwifiex_get_sta_entry(priv, mac);
-	if (node) {
-		list_del(&node->list);
-		kfree(node);
-	}
-
-	spin_unlock_irqrestore(&priv->sta_list_spinlock, flags);
-	return;
-}
-
-/*
- * This function will delete all stations from associated station list.
- */
-static void mwifiex_del_all_sta_list(struct mwifiex_private *priv)
-{
-	struct mwifiex_sta_node *node, *tmp;
-	unsigned long flags;
-
-	spin_lock_irqsave(&priv->sta_list_spinlock, flags);
-
-	list_for_each_entry_safe(node, tmp, &priv->sta_list, list) {
-		list_del(&node->list);
-		kfree(node);
-	}
-
-	INIT_LIST_HEAD(&priv->sta_list);
-	spin_unlock_irqrestore(&priv->sta_list_spinlock, flags);
-	return;
-}
 
 /*
  * This function handles AP interface specific events generated by firmware.
@@ -268,9 +150,9 @@ int mwifiex_process_uap_event(struct mwifiex_private *priv)
 	case EVENT_ADDBA:
 		dev_dbg(adapter->dev, "event: ADDBA Request\n");
 		if (priv->media_connected)
-			mwifiex_send_cmd_async(priv, HostCmd_CMD_11N_ADDBA_RSP,
-					       HostCmd_ACT_GEN_SET, 0,
-					       adapter->event_body);
+			mwifiex_send_cmd(priv, HostCmd_CMD_11N_ADDBA_RSP,
+					 HostCmd_ACT_GEN_SET, 0,
+					 adapter->event_body, false);
 		break;
 	case EVENT_DELBA:
 		dev_dbg(adapter->dev, "event: DELBA Request\n");
diff --git a/drivers/net/wireless/mwifiex/usb.c b/drivers/net/wireless/mwifiex/usb.c
index e8ebbd4bc3cd..ae30c390ebd3 100644
--- a/drivers/net/wireless/mwifiex/usb.c
+++ b/drivers/net/wireless/mwifiex/usb.c
@@ -22,8 +22,6 @@
 
 #define USB_VERSION	"1.0"
 
-static const char usbdriver_name[] = "usb8xxx";
-
 static struct mwifiex_if_ops usb_ops;
 static struct semaphore add_remove_card_sem;
 static struct usb_card_rec *usb_card;
@@ -527,13 +525,6 @@ static int mwifiex_usb_resume(struct usb_interface *intf)
 						   MWIFIEX_BSS_ROLE_ANY),
 				  MWIFIEX_ASYNC_CMD);
 
-#ifdef CONFIG_PM
-	/* Resume handler may be called due to remote wakeup,
-	 * force to exit suspend anyway
-	 */
-	usb_disable_autosuspend(card->udev);
-#endif /* CONFIG_PM */
-
 	return 0;
 }
 
@@ -567,13 +558,12 @@ static void mwifiex_usb_disconnect(struct usb_interface *intf)
 }
 
 static struct usb_driver mwifiex_usb_driver = {
-	.name = usbdriver_name,
+	.name = "mwifiex_usb",
 	.probe = mwifiex_usb_probe,
 	.disconnect = mwifiex_usb_disconnect,
 	.id_table = mwifiex_usb_table,
 	.suspend = mwifiex_usb_suspend,
 	.resume = mwifiex_usb_resume,
-	.supports_autosuspend = 1,
 };
 
 static int mwifiex_usb_tx_init(struct mwifiex_adapter *adapter)
@@ -776,11 +766,13 @@ static int mwifiex_register_dev(struct mwifiex_adapter *adapter)
 	switch (le16_to_cpu(card->udev->descriptor.idProduct)) {
 	case USB8897_PID_1:
 	case USB8897_PID_2:
+		adapter->tx_buf_size = MWIFIEX_TX_DATA_BUF_SIZE_4K;
 		strcpy(adapter->fw_name, USB8897_DEFAULT_FW_NAME);
 		break;
 	case USB8797_PID_1:
 	case USB8797_PID_2:
 	default:
+		adapter->tx_buf_size = MWIFIEX_TX_DATA_BUF_SIZE_2K;
 		strcpy(adapter->fw_name, USB8797_DEFAULT_FW_NAME);
 		break;
 	}
@@ -1034,7 +1026,6 @@ static void mwifiex_usb_cleanup_module(void)
 
 	if (usb_card && usb_card->adapter) {
 		struct mwifiex_adapter *adapter = usb_card->adapter;
-		int i;
 
 		/* In case driver is removed when asynchronous FW downloading is
 		 * in progress
@@ -1045,11 +1036,8 @@ static void mwifiex_usb_cleanup_module(void)
 		if (adapter->is_suspended)
 			mwifiex_usb_resume(usb_card->intf);
 #endif
-		for (i = 0; i < adapter->priv_num; i++)
-			if ((GET_BSS_ROLE(adapter->priv[i]) ==
-			     MWIFIEX_BSS_ROLE_STA) &&
-			    adapter->priv[i]->media_connected)
-				mwifiex_deauthenticate(adapter->priv[i], NULL);
+
+		mwifiex_deauthenticate_all(adapter);
 
 		mwifiex_init_shutdown_fw(mwifiex_get_priv(adapter,
 							  MWIFIEX_BSS_ROLE_ANY),
diff --git a/drivers/net/wireless/mwifiex/util.c b/drivers/net/wireless/mwifiex/util.c
index 9b82e225880c..c3824e37f3f2 100644
--- a/drivers/net/wireless/mwifiex/util.c
+++ b/drivers/net/wireless/mwifiex/util.c
@@ -72,7 +72,7 @@ int mwifiex_init_shutdown_fw(struct mwifiex_private *priv,
 		return -1;
 	}
 
-	return mwifiex_send_cmd_sync(priv, cmd, HostCmd_ACT_GEN_SET, 0, NULL);
+	return mwifiex_send_cmd(priv, cmd, HostCmd_ACT_GEN_SET, 0, NULL, true);
 }
 EXPORT_SYMBOL_GPL(mwifiex_init_shutdown_fw);
 
@@ -104,6 +104,7 @@ int mwifiex_get_debug_info(struct mwifiex_private *priv,
 		info->pm_wakeup_fw_try = adapter->pm_wakeup_fw_try;
 		info->is_hs_configured = adapter->is_hs_configured;
 		info->hs_activated = adapter->hs_activated;
+		info->is_cmd_timedout = adapter->is_cmd_timedout;
 		info->num_cmd_host_to_card_failure
 				= adapter->dbg.num_cmd_host_to_card_failure;
 		info->num_cmd_sleep_cfm_host_to_card_failure
@@ -119,7 +120,6 @@ int mwifiex_get_debug_info(struct mwifiex_private *priv,
 		info->num_cmd_assoc_failure =
 					adapter->dbg.num_cmd_assoc_failure;
 		info->num_tx_timeout = adapter->dbg.num_tx_timeout;
-		info->num_cmd_timeout = adapter->dbg.num_cmd_timeout;
 		info->timeout_cmd_id = adapter->dbg.timeout_cmd_id;
 		info->timeout_cmd_act = adapter->dbg.timeout_cmd_act;
 		memcpy(info->last_cmd_id, adapter->dbg.last_cmd_id,
@@ -252,3 +252,117 @@ int mwifiex_complete_cmd(struct mwifiex_adapter *adapter,
 
 	return 0;
 }
+
+/* This function will return the pointer to station entry in station list
+ * table which matches specified mac address.
+ * This function should be called after acquiring RA list spinlock.
+ * NULL is returned if station entry is not found in associated STA list.
+ */
+struct mwifiex_sta_node *
+mwifiex_get_sta_entry(struct mwifiex_private *priv, u8 *mac)
+{
+	struct mwifiex_sta_node *node;
+
+	if (!mac)
+		return NULL;
+
+	list_for_each_entry(node, &priv->sta_list, list) {
+		if (!memcmp(node->mac_addr, mac, ETH_ALEN))
+			return node;
+	}
+
+	return NULL;
+}
+
+/* This function will add a sta_node entry to associated station list
+ * table with the given mac address.
+ * If entry exist already, existing entry is returned.
+ * If received mac address is NULL, NULL is returned.
+ */
+struct mwifiex_sta_node *
+mwifiex_add_sta_entry(struct mwifiex_private *priv, u8 *mac)
+{
+	struct mwifiex_sta_node *node;
+	unsigned long flags;
+
+	if (!mac)
+		return NULL;
+
+	spin_lock_irqsave(&priv->sta_list_spinlock, flags);
+	node = mwifiex_get_sta_entry(priv, mac);
+	if (node)
+		goto done;
+
+	node = kzalloc(sizeof(*node), GFP_ATOMIC);
+	if (!node)
+		goto done;
+
+	memcpy(node->mac_addr, mac, ETH_ALEN);
+	list_add_tail(&node->list, &priv->sta_list);
+
+done:
+	spin_unlock_irqrestore(&priv->sta_list_spinlock, flags);
+	return node;
+}
+
+/* This function will search for HT IE in association request IEs
+ * and set station HT parameters accordingly.
+ */
+void
+mwifiex_set_sta_ht_cap(struct mwifiex_private *priv, const u8 *ies,
+		       int ies_len, struct mwifiex_sta_node *node)
+{
+	const struct ieee80211_ht_cap *ht_cap;
+
+	if (!ies)
+		return;
+
+	ht_cap = (void *)cfg80211_find_ie(WLAN_EID_HT_CAPABILITY, ies, ies_len);
+	if (ht_cap) {
+		node->is_11n_enabled = 1;
+		node->max_amsdu = le16_to_cpu(ht_cap->cap_info) &
+				  IEEE80211_HT_CAP_MAX_AMSDU ?
+				  MWIFIEX_TX_DATA_BUF_SIZE_8K :
+				  MWIFIEX_TX_DATA_BUF_SIZE_4K;
+	} else {
+		node->is_11n_enabled = 0;
+	}
+
+	return;
+}
+
+/* This function will delete a station entry from station list */
+void mwifiex_del_sta_entry(struct mwifiex_private *priv, u8 *mac)
+{
+	struct mwifiex_sta_node *node;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->sta_list_spinlock, flags);
+
+	node = mwifiex_get_sta_entry(priv, mac);
+	if (node) {
+		list_del(&node->list);
+		kfree(node);
+	}
+
+	spin_unlock_irqrestore(&priv->sta_list_spinlock, flags);
+	return;
+}
+
+/* This function will delete all stations from associated station list. */
+void mwifiex_del_all_sta_list(struct mwifiex_private *priv)
+{
+	struct mwifiex_sta_node *node, *tmp;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->sta_list_spinlock, flags);
+
+	list_for_each_entry_safe(node, tmp, &priv->sta_list, list) {
+		list_del(&node->list);
+		kfree(node);
+	}
+
+	INIT_LIST_HEAD(&priv->sta_list);
+	spin_unlock_irqrestore(&priv->sta_list_spinlock, flags);
+	return;
+}
diff --git a/drivers/net/wireless/mwifiex/util.h b/drivers/net/wireless/mwifiex/util.h
index cb2d0582bd36..ddae57021397 100644
--- a/drivers/net/wireless/mwifiex/util.h
+++ b/drivers/net/wireless/mwifiex/util.h
@@ -30,8 +30,24 @@ static inline struct mwifiex_txinfo *MWIFIEX_SKB_TXCB(struct sk_buff *skb)
 	return (struct mwifiex_txinfo *)(skb->cb + sizeof(dma_addr_t));
 }
 
-static inline void MWIFIEX_SKB_PACB(struct sk_buff *skb, dma_addr_t *buf_pa)
+struct mwifiex_dma_mapping {
+	dma_addr_t addr;
+	size_t len;
+};
+
+static inline void MWIFIEX_SKB_PACB(struct sk_buff *skb,
+					struct mwifiex_dma_mapping *mapping)
 {
-	memcpy(buf_pa, skb->cb, sizeof(dma_addr_t));
+	memcpy(mapping, skb->cb, sizeof(*mapping));
 }
+
+static inline dma_addr_t MWIFIEX_SKB_DMA_ADDR(struct sk_buff *skb)
+{
+	struct mwifiex_dma_mapping mapping;
+
+	MWIFIEX_SKB_PACB(skb, &mapping);
+
+	return mapping.addr;
+}
+
 #endif /* !_MWIFIEX_UTIL_H_ */
diff --git a/drivers/net/wireless/mwifiex/wmm.c b/drivers/net/wireless/mwifiex/wmm.c
index 13eaeed03898..1c5f2b66f057 100644
--- a/drivers/net/wireless/mwifiex/wmm.c
+++ b/drivers/net/wireless/mwifiex/wmm.c
@@ -64,21 +64,6 @@ static u8 tos_to_tid[] = {
 	0x07			/* 1 1 1 AC_VO */
 };
 
-/*
- * This table inverses the tos_to_tid operation to get a priority
- * which is in sequential order, and can be compared.
- * Use this to compare the priority of two different TIDs.
- */
-static u8 tos_to_tid_inv[] = {
-	0x02,  /* from tos_to_tid[2] = 0 */
-	0x00,  /* from tos_to_tid[0] = 1 */
-	0x01,  /* from tos_to_tid[1] = 2 */
-	0x03,
-	0x04,
-	0x05,
-	0x06,
-	0x07};
-
 static u8 ac_to_tid[4][2] = { {1, 2}, {0, 3}, {4, 5}, {6, 7} };
 
 /*
@@ -175,8 +160,15 @@ mwifiex_ralist_add(struct mwifiex_private *priv, u8 *ra)
 			break;
 
 		ra_list->is_11n_enabled = 0;
+		ra_list->tdls_link = false;
 		if (!mwifiex_queuing_ra_based(priv)) {
-			ra_list->is_11n_enabled = IS_11N_ENABLED(priv);
+			if (mwifiex_get_tdls_link_status(priv, ra) ==
+			    TDLS_SETUP_COMPLETE) {
+				ra_list->is_11n_enabled =
+					mwifiex_tdls_peer_11n_enabled(priv, ra);
+			} else {
+				ra_list->is_11n_enabled = IS_11N_ENABLED(priv);
+			}
 		} else {
 			ra_list->is_11n_enabled =
 				      mwifiex_is_sta_11n_enabled(priv, node);
@@ -213,8 +205,9 @@ static void mwifiex_wmm_default_queue_priorities(struct mwifiex_private *priv)
  * This function map ACs to TIDs.
  */
 static void
-mwifiex_wmm_queue_priorities_tid(struct mwifiex_wmm_desc *wmm)
+mwifiex_wmm_queue_priorities_tid(struct mwifiex_private *priv)
 {
+	struct mwifiex_wmm_desc *wmm = &priv->wmm;
 	u8 *queue_priority = wmm->queue_priority;
 	int i;
 
@@ -224,7 +217,7 @@ mwifiex_wmm_queue_priorities_tid(struct mwifiex_wmm_desc *wmm)
 	}
 
 	for (i = 0; i < MAX_NUM_TID; ++i)
-		tos_to_tid_inv[tos_to_tid[i]] = (u8)i;
+		priv->tos_to_tid_inv[tos_to_tid[i]] = (u8)i;
 
 	atomic_set(&wmm->highest_queued_prio, HIGH_PRIO_TID);
 }
@@ -285,7 +278,7 @@ mwifiex_wmm_setup_queue_priorities(struct mwifiex_private *priv,
 		}
 	}
 
-	mwifiex_wmm_queue_priorities_tid(&priv->wmm);
+	mwifiex_wmm_queue_priorities_tid(priv);
 }
 
 /*
@@ -388,8 +381,7 @@ mwifiex_wmm_convert_tos_to_ac(struct mwifiex_adapter *adapter, u32 tos)
  * AP is disabled (due to call admission control (ACM bit). Mapping
  * of TID to AC is taken care of internally.
  */
-static u8
-mwifiex_wmm_downgrade_tid(struct mwifiex_private *priv, u32 tid)
+u8 mwifiex_wmm_downgrade_tid(struct mwifiex_private *priv, u32 tid)
 {
 	enum mwifiex_wmm_ac_e ac, ac_down;
 	u8 new_tid;
@@ -421,9 +413,11 @@ mwifiex_wmm_init(struct mwifiex_adapter *adapter)
 			continue;
 
 		for (i = 0; i < MAX_NUM_TID; ++i) {
-			priv->aggr_prio_tbl[i].amsdu = tos_to_tid_inv[i];
-			priv->aggr_prio_tbl[i].ampdu_ap = tos_to_tid_inv[i];
-			priv->aggr_prio_tbl[i].ampdu_user = tos_to_tid_inv[i];
+			priv->aggr_prio_tbl[i].amsdu = priv->tos_to_tid_inv[i];
+			priv->aggr_prio_tbl[i].ampdu_ap =
+							priv->tos_to_tid_inv[i];
+			priv->aggr_prio_tbl[i].ampdu_user =
+							priv->tos_to_tid_inv[i];
 		}
 
 		priv->aggr_prio_tbl[6].amsdu
@@ -546,6 +540,7 @@ void
 mwifiex_clean_txrx(struct mwifiex_private *priv)
 {
 	unsigned long flags;
+	struct sk_buff *skb, *tmp;
 
 	mwifiex_11n_cleanup_reorder_tbl(priv);
 	spin_lock_irqsave(&priv->wmm.ra_list_spinlock, flags);
@@ -559,9 +554,13 @@ mwifiex_clean_txrx(struct mwifiex_private *priv)
 	mwifiex_wmm_delete_all_ralist(priv);
 	memcpy(tos_to_tid, ac_to_tid, sizeof(tos_to_tid));
 
-	if (priv->adapter->if_ops.clean_pcie_ring)
+	if (priv->adapter->if_ops.clean_pcie_ring &&
+	    !priv->adapter->surprise_removed)
 		priv->adapter->if_ops.clean_pcie_ring(priv->adapter);
 	spin_unlock_irqrestore(&priv->wmm.ra_list_spinlock, flags);
+
+	skb_queue_walk_safe(&priv->tdls_txq, skb, tmp)
+		mwifiex_write_data_complete(priv->adapter, skb, 0, -1);
 }
 
 /*
@@ -590,7 +589,7 @@ mwifiex_wmm_get_ralist_node(struct mwifiex_private *priv, u8 tid,
  * If no such node is found, a new node is added first and then
  * retrieved.
  */
-static struct mwifiex_ra_list_tbl *
+struct mwifiex_ra_list_tbl *
 mwifiex_wmm_get_queue_raptr(struct mwifiex_private *priv, u8 tid, u8 *ra_addr)
 {
 	struct mwifiex_ra_list_tbl *ra_list;
@@ -640,6 +639,21 @@ mwifiex_wmm_add_buf_txqueue(struct mwifiex_private *priv,
 	struct mwifiex_ra_list_tbl *ra_list;
 	u8 ra[ETH_ALEN], tid_down;
 	unsigned long flags;
+	struct list_head list_head;
+	int tdls_status = TDLS_NOT_SETUP;
+	struct ethhdr *eth_hdr = (struct ethhdr *)skb->data;
+	struct mwifiex_txinfo *tx_info = MWIFIEX_SKB_TXCB(skb);
+
+	memcpy(ra, eth_hdr->h_dest, ETH_ALEN);
+
+	if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_STA &&
+	    ISSUPP_TDLS_ENABLED(adapter->fw_cap_info)) {
+		if (ntohs(eth_hdr->h_proto) == ETH_P_TDLS)
+			dev_dbg(adapter->dev,
+				"TDLS setup packet for %pM. Don't block\n", ra);
+		else
+			tdls_status = mwifiex_get_tdls_link_status(priv, ra);
+	}
 
 	if (!priv->media_connected && !mwifiex_is_skb_mgmt_frame(skb)) {
 		dev_dbg(adapter->dev, "data: drop packet in disconnect\n");
@@ -658,12 +672,27 @@ mwifiex_wmm_add_buf_txqueue(struct mwifiex_private *priv,
 	   have only 1 raptr for a tid in case of infra */
 	if (!mwifiex_queuing_ra_based(priv) &&
 	    !mwifiex_is_skb_mgmt_frame(skb)) {
-		if (!list_empty(&priv->wmm.tid_tbl_ptr[tid_down].ra_list))
-			ra_list = list_first_entry(
-				&priv->wmm.tid_tbl_ptr[tid_down].ra_list,
-				struct mwifiex_ra_list_tbl, list);
-		else
-			ra_list = NULL;
+		switch (tdls_status) {
+		case TDLS_SETUP_COMPLETE:
+			ra_list = mwifiex_wmm_get_queue_raptr(priv, tid_down,
+							      ra);
+			tx_info->flags |= MWIFIEX_BUF_FLAG_TDLS_PKT;
+			break;
+		case TDLS_SETUP_INPROGRESS:
+			skb_queue_tail(&priv->tdls_txq, skb);
+			spin_unlock_irqrestore(&priv->wmm.ra_list_spinlock,
+					       flags);
+			return;
+		default:
+			list_head = priv->wmm.tid_tbl_ptr[tid_down].ra_list;
+			if (!list_empty(&list_head))
+				ra_list = list_first_entry(
+					&list_head, struct mwifiex_ra_list_tbl,
+					list);
+			else
+				ra_list = NULL;
+			break;
+		}
 	} else {
 		memcpy(ra, skb->data, ETH_ALEN);
 		if (ra[0] & 0x01 || mwifiex_is_skb_mgmt_frame(skb))
@@ -683,9 +712,9 @@ mwifiex_wmm_add_buf_txqueue(struct mwifiex_private *priv,
 	ra_list->total_pkt_count++;
 
 	if (atomic_read(&priv->wmm.highest_queued_prio) <
-						tos_to_tid_inv[tid_down])
+						priv->tos_to_tid_inv[tid_down])
 		atomic_set(&priv->wmm.highest_queued_prio,
-			   tos_to_tid_inv[tid_down]);
+			   priv->tos_to_tid_inv[tid_down]);
 
 	atomic_inc(&priv->wmm.tx_pkts_queued);
 
@@ -1226,7 +1255,7 @@ mwifiex_dequeue_tx_packet(struct mwifiex_adapter *adapter)
 		/* ra_list_spinlock has been freed in
 		   mwifiex_send_single_packet() */
 	} else {
-		if (mwifiex_is_ampdu_allowed(priv, tid) &&
+		if (mwifiex_is_ampdu_allowed(priv, ptr, tid) &&
 		    ptr->ba_pkt_count > ptr->ba_packet_thr) {
 			if (mwifiex_space_avail_for_new_ba_stream(adapter)) {
 				mwifiex_create_ba_tbl(priv, ptr->ra, tid,
diff --git a/drivers/net/wireless/mwifiex/wmm.h b/drivers/net/wireless/mwifiex/wmm.h
index 0f129d498fb1..83e42083ebff 100644
--- a/drivers/net/wireless/mwifiex/wmm.h
+++ b/drivers/net/wireless/mwifiex/wmm.h
@@ -34,6 +34,21 @@ enum ieee_types_wmm_ecw_bitmasks {
 static const u16 mwifiex_1d_to_wmm_queue[8] = { 1, 0, 0, 1, 2, 2, 3, 3 };
 
 /*
+ * This table inverses the tos_to_tid operation to get a priority
+ * which is in sequential order, and can be compared.
+ * Use this to compare the priority of two different TIDs.
+ */
+static const u8 tos_to_tid_inv[] = {
+	0x02,  /* from tos_to_tid[2] = 0 */
+	0x00,  /* from tos_to_tid[0] = 1 */
+	0x01,  /* from tos_to_tid[1] = 2 */
+	0x03,
+	0x04,
+	0x05,
+	0x06,
+	0x07};
+
+/*
  * This function retrieves the TID of the given RA list.
  */
 static inline int
@@ -107,5 +122,8 @@ void mwifiex_wmm_setup_queue_priorities(struct mwifiex_private *priv,
 void mwifiex_wmm_setup_ac_downgrade(struct mwifiex_private *priv);
 int mwifiex_ret_wmm_get_status(struct mwifiex_private *priv,
 			       const struct host_cmd_ds_command *resp);
+struct mwifiex_ra_list_tbl *
+mwifiex_wmm_get_queue_raptr(struct mwifiex_private *priv, u8 tid, u8 *ra_addr);
+u8 mwifiex_wmm_downgrade_tid(struct mwifiex_private *priv, u32 tid);
 
 #endif /* !_MWIFIEX_WMM_H_ */
diff --git a/drivers/net/wireless/mwl8k.c b/drivers/net/wireless/mwl8k.c
index 4987c3f942ce..3c0a0a86ba12 100644
--- a/drivers/net/wireless/mwl8k.c
+++ b/drivers/net/wireless/mwl8k.c
@@ -81,6 +81,9 @@ MODULE_PARM_DESC(ap_mode_default,
  */
 
 #define	MWL8K_HW_TIMER_REGISTER			0x0000a600
+#define BBU_RXRDY_CNT_REG			0x0000a860
+#define NOK_CCA_CNT_REG				0x0000a6a0
+#define BBU_AVG_NOISE_VAL			0x67
 
 #define MWL8K_A2H_EVENTS	(MWL8K_A2H_INT_DUMMY | \
 				 MWL8K_A2H_INT_CHNL_SWITCHED | \
@@ -112,6 +115,8 @@ MODULE_PARM_DESC(ap_mode_default,
  */
 #define MWL8K_NUM_AMPDU_STREAMS	(TOTAL_HW_TX_QUEUES - 1)
 
+#define MWL8K_NUM_CHANS 18
+
 struct rxd_ops {
 	int rxd_size;
 	void (*rxd_init)(void *rxd, dma_addr_t next_dma_addr);
@@ -289,6 +294,12 @@ struct mwl8k_priv {
 
 	/* bitmap of running BSSes */
 	u32 running_bsses;
+
+	/* ACS related */
+	bool sw_scan_start;
+	struct ieee80211_channel *acs_chan;
+	unsigned long channel_time;
+	struct survey_info survey[MWL8K_NUM_CHANS];
 };
 
 #define MAX_WEP_KEY_LEN         13
@@ -396,6 +407,7 @@ static const struct ieee80211_rate mwl8k_rates_50[] = {
 #define MWL8K_CMD_SET_HW_SPEC		0x0004
 #define MWL8K_CMD_MAC_MULTICAST_ADR	0x0010
 #define MWL8K_CMD_GET_STAT		0x0014
+#define MWL8K_CMD_BBP_REG_ACCESS	0x001a
 #define MWL8K_CMD_RADIO_CONTROL		0x001c
 #define MWL8K_CMD_RF_TX_POWER		0x001e
 #define MWL8K_CMD_TX_POWER		0x001f
@@ -2987,6 +2999,47 @@ static int mwl8k_cmd_set_pre_scan(struct ieee80211_hw *hw)
 }
 
 /*
+ * CMD_BBP_REG_ACCESS.
+ */
+struct mwl8k_cmd_bbp_reg_access {
+	struct mwl8k_cmd_pkt header;
+	__le16 action;
+	__le16 offset;
+	u8 value;
+	u8 rsrv[3];
+} __packed;
+
+static int
+mwl8k_cmd_bbp_reg_access(struct ieee80211_hw *hw,
+			 u16 action,
+			 u16 offset,
+			 u8 *value)
+{
+	struct mwl8k_cmd_bbp_reg_access *cmd;
+	int rc;
+
+	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
+	if (cmd == NULL)
+		return -ENOMEM;
+
+	cmd->header.code = cpu_to_le16(MWL8K_CMD_BBP_REG_ACCESS);
+	cmd->header.length = cpu_to_le16(sizeof(*cmd));
+	cmd->action = cpu_to_le16(action);
+	cmd->offset = cpu_to_le16(offset);
+
+	rc = mwl8k_post_cmd(hw, &cmd->header);
+
+	if (!rc)
+		*value = cmd->value;
+	else
+		*value = 0;
+
+	kfree(cmd);
+
+	return rc;
+}
+
+/*
  * CMD_SET_POST_SCAN.
  */
 struct mwl8k_cmd_set_post_scan {
@@ -3016,6 +3069,64 @@ mwl8k_cmd_set_post_scan(struct ieee80211_hw *hw, const __u8 *mac)
 	return rc;
 }
 
+static int freq_to_idx(struct mwl8k_priv *priv, int freq)
+{
+	struct ieee80211_supported_band *sband;
+	int band, ch, idx = 0;
+
+	for (band = IEEE80211_BAND_2GHZ; band < IEEE80211_NUM_BANDS; band++) {
+		sband = priv->hw->wiphy->bands[band];
+		if (!sband)
+			continue;
+
+		for (ch = 0; ch < sband->n_channels; ch++, idx++)
+			if (sband->channels[ch].center_freq == freq)
+				goto exit;
+	}
+
+exit:
+	return idx;
+}
+
+static void mwl8k_update_survey(struct mwl8k_priv *priv,
+				struct ieee80211_channel *channel)
+{
+	u32 cca_cnt, rx_rdy;
+	s8 nf = 0, idx;
+	struct survey_info *survey;
+
+	idx = freq_to_idx(priv, priv->acs_chan->center_freq);
+	if (idx >= MWL8K_NUM_CHANS) {
+		wiphy_err(priv->hw->wiphy, "Failed to update survey\n");
+		return;
+	}
+
+	survey = &priv->survey[idx];
+
+	cca_cnt = ioread32(priv->regs + NOK_CCA_CNT_REG);
+	cca_cnt /= 1000; /* uSecs to mSecs */
+	survey->channel_time_busy = (u64) cca_cnt;
+
+	rx_rdy = ioread32(priv->regs + BBU_RXRDY_CNT_REG);
+	rx_rdy /= 1000; /* uSecs to mSecs */
+	survey->channel_time_rx = (u64) rx_rdy;
+
+	priv->channel_time = jiffies - priv->channel_time;
+	survey->channel_time = jiffies_to_msecs(priv->channel_time);
+
+	survey->channel = channel;
+
+	mwl8k_cmd_bbp_reg_access(priv->hw, 0, BBU_AVG_NOISE_VAL, &nf);
+
+	/* Make sure sign is negative else ACS  at hostapd fails */
+	survey->noise = nf * -1;
+
+	survey->filled = SURVEY_INFO_NOISE_DBM |
+			 SURVEY_INFO_CHANNEL_TIME |
+			 SURVEY_INFO_CHANNEL_TIME_BUSY |
+			 SURVEY_INFO_CHANNEL_TIME_RX;
+}
+
 /*
  * CMD_SET_RF_CHANNEL.
  */
@@ -3033,6 +3144,7 @@ static int mwl8k_cmd_set_rf_channel(struct ieee80211_hw *hw,
 	enum nl80211_channel_type channel_type =
 		cfg80211_get_chandef_type(&conf->chandef);
 	struct mwl8k_cmd_set_rf_channel *cmd;
+	struct mwl8k_priv *priv = hw->priv;
 	int rc;
 
 	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
@@ -3049,13 +3161,29 @@ static int mwl8k_cmd_set_rf_channel(struct ieee80211_hw *hw,
 	else if (channel->band == IEEE80211_BAND_5GHZ)
 		cmd->channel_flags |= cpu_to_le32(0x00000004);
 
-	if (channel_type == NL80211_CHAN_NO_HT ||
-	    channel_type == NL80211_CHAN_HT20)
+	if (!priv->sw_scan_start) {
+		if (channel_type == NL80211_CHAN_NO_HT ||
+		    channel_type == NL80211_CHAN_HT20)
+			cmd->channel_flags |= cpu_to_le32(0x00000080);
+		else if (channel_type == NL80211_CHAN_HT40MINUS)
+			cmd->channel_flags |= cpu_to_le32(0x000001900);
+		else if (channel_type == NL80211_CHAN_HT40PLUS)
+			cmd->channel_flags |= cpu_to_le32(0x000000900);
+	} else {
 		cmd->channel_flags |= cpu_to_le32(0x00000080);
-	else if (channel_type == NL80211_CHAN_HT40MINUS)
-		cmd->channel_flags |= cpu_to_le32(0x000001900);
-	else if (channel_type == NL80211_CHAN_HT40PLUS)
-		cmd->channel_flags |= cpu_to_le32(0x000000900);
+	}
+
+	if (priv->sw_scan_start) {
+		/* Store current channel stats
+		 * before switching to newer one.
+		 * This will be processed only for AP fw.
+		 */
+		if (priv->channel_time != 0)
+			mwl8k_update_survey(priv, priv->acs_chan);
+
+		priv->channel_time = jiffies;
+		priv->acs_chan =  channel;
+	}
 
 	rc = mwl8k_post_cmd(hw, &cmd->header);
 	kfree(cmd);
@@ -5263,6 +5391,27 @@ static int mwl8k_get_survey(struct ieee80211_hw *hw, int idx,
 {
 	struct mwl8k_priv *priv = hw->priv;
 	struct ieee80211_conf *conf = &hw->conf;
+	struct ieee80211_supported_band *sband;
+
+	if (priv->ap_fw) {
+		sband = hw->wiphy->bands[IEEE80211_BAND_2GHZ];
+
+		if (sband && idx >= sband->n_channels) {
+			idx -= sband->n_channels;
+			sband = NULL;
+		}
+
+		if (!sband)
+			sband = hw->wiphy->bands[IEEE80211_BAND_5GHZ];
+
+		if (!sband || idx >= sband->n_channels)
+			return -ENOENT;
+
+		memcpy(survey, &priv->survey[idx], sizeof(*survey));
+		survey->channel = &sband->channels[idx];
+
+		return 0;
+	}
 
 	if (idx != 0)
 		return -ENOENT;
@@ -5406,6 +5555,40 @@ mwl8k_ampdu_action(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	return rc;
 }
 
+static void mwl8k_sw_scan_start(struct ieee80211_hw *hw)
+{
+	struct mwl8k_priv *priv = hw->priv;
+	u8 tmp;
+
+	if (!priv->ap_fw)
+		return;
+
+	/* clear all stats */
+	priv->channel_time = 0;
+	ioread32(priv->regs + BBU_RXRDY_CNT_REG);
+	ioread32(priv->regs + NOK_CCA_CNT_REG);
+	mwl8k_cmd_bbp_reg_access(priv->hw, 0, BBU_AVG_NOISE_VAL, &tmp);
+
+	priv->sw_scan_start = true;
+}
+
+static void mwl8k_sw_scan_complete(struct ieee80211_hw *hw)
+{
+	struct mwl8k_priv *priv = hw->priv;
+	u8 tmp;
+
+	if (!priv->ap_fw)
+		return;
+
+	priv->sw_scan_start = false;
+
+	/* clear all stats */
+	priv->channel_time = 0;
+	ioread32(priv->regs + BBU_RXRDY_CNT_REG);
+	ioread32(priv->regs + NOK_CCA_CNT_REG);
+	mwl8k_cmd_bbp_reg_access(priv->hw, 0, BBU_AVG_NOISE_VAL, &tmp);
+}
+
 static const struct ieee80211_ops mwl8k_ops = {
 	.tx			= mwl8k_tx,
 	.start			= mwl8k_start,
@@ -5424,6 +5607,8 @@ static const struct ieee80211_ops mwl8k_ops = {
 	.get_stats		= mwl8k_get_stats,
 	.get_survey		= mwl8k_get_survey,
 	.ampdu_action		= mwl8k_ampdu_action,
+	.sw_scan_start		= mwl8k_sw_scan_start,
+	.sw_scan_complete	= mwl8k_sw_scan_complete,
 };
 
 static void mwl8k_finalize_join_worker(struct work_struct *work)
diff --git a/drivers/net/wireless/orinoco/cfg.c b/drivers/net/wireless/orinoco/cfg.c
index d01edd2c50c5..a9e94b6db5b7 100644
--- a/drivers/net/wireless/orinoco/cfg.c
+++ b/drivers/net/wireless/orinoco/cfg.c
@@ -59,7 +59,8 @@ int orinoco_wiphy_register(struct wiphy *wiphy)
 	for (i = 0; i < NUM_CHANNELS; i++) {
 		if (priv->channel_mask & (1 << i)) {
 			priv->channels[i].center_freq =
-				ieee80211_dsss_chan_to_freq(i + 1);
+				ieee80211_channel_to_frequency(i + 1,
+							   IEEE80211_BAND_2GHZ);
 			channels++;
 		}
 	}
@@ -177,7 +178,7 @@ static int orinoco_set_monitor_channel(struct wiphy *wiphy,
 	if (chandef->chan->band != IEEE80211_BAND_2GHZ)
 		return -EINVAL;
 
-	channel = ieee80211_freq_to_dsss_chan(chandef->chan->center_freq);
+	channel = ieee80211_frequency_to_channel(chandef->chan->center_freq);
 
 	if ((channel < 1) || (channel > NUM_CHANNELS) ||
 	     !(priv->channel_mask & (1 << (channel - 1))))
diff --git a/drivers/net/wireless/orinoco/hw.c b/drivers/net/wireless/orinoco/hw.c
index c09c8437c0b8..49300d04efdf 100644
--- a/drivers/net/wireless/orinoco/hw.c
+++ b/drivers/net/wireless/orinoco/hw.c
@@ -1193,7 +1193,7 @@ int orinoco_hw_get_freq(struct orinoco_private *priv)
 		goto out;
 
 	}
-	freq = ieee80211_dsss_chan_to_freq(channel);
+	freq = ieee80211_channel_to_frequency(channel, IEEE80211_BAND_2GHZ);
 
  out:
 	orinoco_unlock(priv, &flags);
diff --git a/drivers/net/wireless/orinoco/scan.c b/drivers/net/wireless/orinoco/scan.c
index e8c5714bfd11..e175b9b8561b 100644
--- a/drivers/net/wireless/orinoco/scan.c
+++ b/drivers/net/wireless/orinoco/scan.c
@@ -110,7 +110,8 @@ static void orinoco_add_hostscan_result(struct orinoco_private *priv,
 		break;
 	}
 
-	freq = ieee80211_dsss_chan_to_freq(le16_to_cpu(bss->a.channel));
+	freq = ieee80211_channel_to_frequency(
+		le16_to_cpu(bss->a.channel), IEEE80211_BAND_2GHZ);
 	channel = ieee80211_get_channel(wiphy, freq);
 	if (!channel) {
 		printk(KERN_DEBUG "Invalid channel designation %04X(%04X)",
@@ -146,7 +147,7 @@ void orinoco_add_extscan_result(struct orinoco_private *priv,
 	ie_len = len - sizeof(*bss);
 	ie = cfg80211_find_ie(WLAN_EID_DS_PARAMS, bss->data, ie_len);
 	chan = ie ? ie[2] : 0;
-	freq = ieee80211_dsss_chan_to_freq(chan);
+	freq = ieee80211_channel_to_frequency(chan, IEEE80211_BAND_2GHZ);
 	channel = ieee80211_get_channel(wiphy, freq);
 
 	timestamp = le64_to_cpu(bss->timestamp);
diff --git a/drivers/net/wireless/orinoco/wext.c b/drivers/net/wireless/orinoco/wext.c
index 3b5508f982e8..b7a867b50b94 100644
--- a/drivers/net/wireless/orinoco/wext.c
+++ b/drivers/net/wireless/orinoco/wext.c
@@ -444,7 +444,7 @@ static int orinoco_ioctl_setfreq(struct net_device *dev,
 		for (i = 0; i < (6 - frq->e); i++)
 			denom *= 10;
 
-		chan = ieee80211_freq_to_dsss_chan(frq->m / denom);
+		chan = ieee80211_frequency_to_channel(frq->m / denom);
 	}
 
 	if ((chan < 1) || (chan > NUM_CHANNELS) ||
diff --git a/drivers/net/wireless/p54/p54usb.c b/drivers/net/wireless/p54/p54usb.c
index 6e635cfa24c8..b7ab3dfb3de8 100644
--- a/drivers/net/wireless/p54/p54usb.c
+++ b/drivers/net/wireless/p54/p54usb.c
@@ -513,7 +513,7 @@ static int p54u_upload_firmware_3887(struct ieee80211_hw *dev)
 	if (!buf)
 		return -ENOMEM;
 
-	left = block_size = min((size_t)P54U_FW_BLOCK, priv->fw->size);
+	left = block_size = min_t(size_t, P54U_FW_BLOCK, priv->fw->size);
 	strcpy(buf, p54u_firmware_upload_3887);
 	left -= strlen(p54u_firmware_upload_3887);
 	tmp += strlen(p54u_firmware_upload_3887);
diff --git a/drivers/net/wireless/prism54/isl_ioctl.c b/drivers/net/wireless/prism54/isl_ioctl.c
index 78fa64d3f223..ecbb0546cf3e 100644
--- a/drivers/net/wireless/prism54/isl_ioctl.c
+++ b/drivers/net/wireless/prism54/isl_ioctl.c
@@ -644,7 +644,7 @@ prism54_translate_bss(struct net_device *ndev, struct iw_request_info *info,
 	wpa_ie_len = prism54_wpa_bss_ie_get(priv, bss->address, wpa_ie);
 	if (wpa_ie_len > 0) {
 		iwe.cmd = IWEVGENIE;
-		iwe.u.data.length = min(wpa_ie_len, (size_t)MAX_WPA_IE_LEN);
+		iwe.u.data.length = min_t(size_t, wpa_ie_len, MAX_WPA_IE_LEN);
 		current_ev = iwe_stream_add_point(info, current_ev, end_buf,
 						  &iwe, wpa_ie);
 	}
diff --git a/drivers/net/wireless/rndis_wlan.c b/drivers/net/wireless/rndis_wlan.c
index 5028557aa18a..39d22a154341 100644
--- a/drivers/net/wireless/rndis_wlan.c
+++ b/drivers/net/wireless/rndis_wlan.c
@@ -1290,7 +1290,8 @@ static int set_channel(struct usbnet *usbdev, int channel)
 	if (is_associated(usbdev))
 		return 0;
 
-	dsconfig = ieee80211_dsss_chan_to_freq(channel) * 1000;
+	dsconfig = 1000 *
+		ieee80211_channel_to_frequency(channel, IEEE80211_BAND_2GHZ);
 
 	len = sizeof(config);
 	ret = rndis_query_oid(usbdev,
@@ -2835,7 +2836,9 @@ static void rndis_wlan_do_link_up_work(struct usbnet *usbdev)
 					bssid, req_ie, req_ie_len,
 					resp_ie, resp_ie_len, GFP_KERNEL);
 	} else if (priv->infra_mode == NDIS_80211_INFRA_ADHOC)
-		cfg80211_ibss_joined(usbdev->net, bssid, GFP_KERNEL);
+		cfg80211_ibss_joined(usbdev->net, bssid,
+				     get_current_channel(usbdev, NULL),
+				     GFP_KERNEL);
 
 	kfree(info);
 
diff --git a/drivers/net/wireless/rt2x00/rt2500pci.c b/drivers/net/wireless/rt2x00/rt2500pci.c
index abc5f56f29fe..2f1cd929c6f6 100644
--- a/drivers/net/wireless/rt2x00/rt2500pci.c
+++ b/drivers/net/wireless/rt2x00/rt2500pci.c
@@ -1877,6 +1877,11 @@ static int rt2500pci_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 						   EEPROM_MAC_ADDR_0));
 
 	/*
+	 * Disable powersaving as default.
+	 */
+	rt2x00dev->hw->wiphy->flags &= ~WIPHY_FLAG_PS_ON_BY_DEFAULT;
+
+	/*
 	 * Initialize hw_mode information.
 	 */
 	spec->supported_bands = SUPPORT_BAND_2GHZ;
diff --git a/drivers/net/wireless/rt2x00/rt2500usb.c b/drivers/net/wireless/rt2x00/rt2500usb.c
index 9f16824cd1bc..d849d590de25 100644
--- a/drivers/net/wireless/rt2x00/rt2500usb.c
+++ b/drivers/net/wireless/rt2x00/rt2500usb.c
@@ -1706,6 +1706,11 @@ static int rt2500usb_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 	    IEEE80211_HW_SUPPORTS_PS |
 	    IEEE80211_HW_PS_NULLFUNC_STACK;
 
+	/*
+	 * Disable powersaving as default.
+	 */
+	rt2x00dev->hw->wiphy->flags &= ~WIPHY_FLAG_PS_ON_BY_DEFAULT;
+
 	SET_IEEE80211_DEV(rt2x00dev->hw, rt2x00dev->dev);
 	SET_IEEE80211_PERM_ADDR(rt2x00dev->hw,
 				rt2x00_eeprom_addr(rt2x00dev,
diff --git a/drivers/net/wireless/rt2x00/rt2800lib.c b/drivers/net/wireless/rt2x00/rt2800lib.c
index b8f5b06006c4..7f8b5d156c8c 100644
--- a/drivers/net/wireless/rt2x00/rt2800lib.c
+++ b/drivers/net/wireless/rt2x00/rt2800lib.c
@@ -7458,10 +7458,9 @@ static int rt2800_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 	u32 reg;
 
 	/*
-	 * Disable powersaving as default on PCI devices.
+	 * Disable powersaving as default.
 	 */
-	if (rt2x00_is_pci(rt2x00dev) || rt2x00_is_soc(rt2x00dev))
-		rt2x00dev->hw->wiphy->flags &= ~WIPHY_FLAG_PS_ON_BY_DEFAULT;
+	rt2x00dev->hw->wiphy->flags &= ~WIPHY_FLAG_PS_ON_BY_DEFAULT;
 
 	/*
 	 * Initialize all hw fields.
diff --git a/drivers/net/wireless/rt2x00/rt2800usb.c b/drivers/net/wireless/rt2x00/rt2800usb.c
index caddc1b427a9..a49c3d73ea2c 100644
--- a/drivers/net/wireless/rt2x00/rt2800usb.c
+++ b/drivers/net/wireless/rt2x00/rt2800usb.c
@@ -125,9 +125,9 @@ static inline bool rt2800usb_entry_txstatus_timeout(struct queue_entry *entry)
 
 	tout = time_after(jiffies, entry->last_action + msecs_to_jiffies(100));
 	if (unlikely(tout))
-		rt2x00_warn(entry->queue->rt2x00dev,
-			    "TX status timeout for entry %d in queue %d\n",
-			    entry->entry_idx, entry->queue->qid);
+		rt2x00_dbg(entry->queue->rt2x00dev,
+			   "TX status timeout for entry %d in queue %d\n",
+			   entry->entry_idx, entry->queue->qid);
 	return tout;
 
 }
@@ -566,8 +566,8 @@ static void rt2800usb_txdone(struct rt2x00_dev *rt2x00dev)
 		queue = rt2x00queue_get_tx_queue(rt2x00dev, qid);
 
 		if (unlikely(rt2x00queue_empty(queue))) {
-			rt2x00_warn(rt2x00dev, "Got TX status for an empty queue %u, dropping\n",
-				    qid);
+			rt2x00_dbg(rt2x00dev, "Got TX status for an empty queue %u, dropping\n",
+				   qid);
 			break;
 		}
 
@@ -764,7 +764,7 @@ static int rt2800usb_probe_hw(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Overwrite TX done handler
 	 */
-	PREPARE_WORK(&rt2x00dev->txdone_work, rt2800usb_work_txdone);
+	INIT_WORK(&rt2x00dev->txdone_work, rt2800usb_work_txdone);
 
 	return 0;
 }
diff --git a/drivers/net/wireless/rt2x00/rt2x00debug.c b/drivers/net/wireless/rt2x00/rt2x00debug.c
index 2e3d1645e68b..90fdb02b55e7 100644
--- a/drivers/net/wireless/rt2x00/rt2x00debug.c
+++ b/drivers/net/wireless/rt2x00/rt2x00debug.c
@@ -286,7 +286,7 @@ static ssize_t rt2x00debug_read_queue_dump(struct file *file,
 	if (retval)
 		return retval;
 
-	status = min((size_t)skb->len, length);
+	status = min_t(size_t, skb->len, length);
 	if (copy_to_user(buf, skb->data, status)) {
 		status = -EFAULT;
 		goto exit;
diff --git a/drivers/net/wireless/rtl818x/rtl8180/dev.c b/drivers/net/wireless/rtl818x/rtl8180/dev.c
index 8ec17aad0e52..959e699702e8 100644
--- a/drivers/net/wireless/rtl818x/rtl8180/dev.c
+++ b/drivers/net/wireless/rtl818x/rtl8180/dev.c
@@ -107,6 +107,7 @@ static void rtl8180_handle_rx(struct ieee80211_hw *dev)
 	struct rtl8180_priv *priv = dev->priv;
 	unsigned int count = 32;
 	u8 signal, agc, sq;
+	dma_addr_t mapping;
 
 	while (count--) {
 		struct rtl8180_rx_desc *entry = &priv->rx_ring[priv->rx_idx];
@@ -128,6 +129,17 @@ static void rtl8180_handle_rx(struct ieee80211_hw *dev)
 			if (unlikely(!new_skb))
 				goto done;
 
+			mapping = pci_map_single(priv->pdev,
+					       skb_tail_pointer(new_skb),
+					       MAX_RX_SIZE, PCI_DMA_FROMDEVICE);
+
+			if (pci_dma_mapping_error(priv->pdev, mapping)) {
+				kfree_skb(new_skb);
+				dev_err(&priv->pdev->dev, "RX DMA map error\n");
+
+				goto done;
+			}
+
 			pci_unmap_single(priv->pdev,
 					 *((dma_addr_t *)skb->cb),
 					 MAX_RX_SIZE, PCI_DMA_FROMDEVICE);
@@ -158,9 +170,7 @@ static void rtl8180_handle_rx(struct ieee80211_hw *dev)
 
 			skb = new_skb;
 			priv->rx_buf[priv->rx_idx] = skb;
-			*((dma_addr_t *) skb->cb) =
-				pci_map_single(priv->pdev, skb_tail_pointer(skb),
-					       MAX_RX_SIZE, PCI_DMA_FROMDEVICE);
+			*((dma_addr_t *) skb->cb) = mapping;
 		}
 
 	done:
@@ -266,6 +276,13 @@ static void rtl8180_tx(struct ieee80211_hw *dev,
 	mapping = pci_map_single(priv->pdev, skb->data,
 				 skb->len, PCI_DMA_TODEVICE);
 
+	if (pci_dma_mapping_error(priv->pdev, mapping)) {
+		kfree_skb(skb);
+		dev_err(&priv->pdev->dev, "TX DMA mapping error\n");
+		return;
+
+	}
+
 	tx_flags = RTL818X_TX_DESC_FLAG_OWN | RTL818X_TX_DESC_FLAG_FS |
 		   RTL818X_TX_DESC_FLAG_LS |
 		   (ieee80211_get_tx_rate(dev, info)->hw_value << 24) |
@@ -318,7 +335,18 @@ static void rtl8180_tx(struct ieee80211_hw *dev,
 	entry->flags2 = info->control.rates[1].idx >= 0 ?
 		ieee80211_get_alt_retry_rate(dev, info, 0)->bitrate << 4 : 0;
 	entry->retry_limit = info->control.rates[0].count;
+
+	/* We must be sure that tx_flags is written last because the HW
+	 * looks at it to check if the rest of data is valid or not
+	 */
+	wmb();
 	entry->flags = cpu_to_le32(tx_flags);
+	/* We must be sure this has been written before followings HW
+	 * register write, because this write will made the HW attempts
+	 * to DMA the just-written data
+	 */
+	wmb();
+
 	__skb_queue_tail(&ring->queue, skb);
 	if (ring->entries - skb_queue_len(&ring->queue) < 2)
 		ieee80211_stop_queue(dev, prio);
@@ -459,13 +487,21 @@ static int rtl8180_init_rx_ring(struct ieee80211_hw *dev)
 		struct sk_buff *skb = dev_alloc_skb(MAX_RX_SIZE);
 		dma_addr_t *mapping;
 		entry = &priv->rx_ring[i];
-		if (!skb)
-			return 0;
-
+		if (!skb) {
+			wiphy_err(dev->wiphy, "Cannot allocate RX skb\n");
+			return -ENOMEM;
+		}
 		priv->rx_buf[i] = skb;
 		mapping = (dma_addr_t *)skb->cb;
 		*mapping = pci_map_single(priv->pdev, skb_tail_pointer(skb),
 					  MAX_RX_SIZE, PCI_DMA_FROMDEVICE);
+
+		if (pci_dma_mapping_error(priv->pdev, *mapping)) {
+			kfree_skb(skb);
+			wiphy_err(dev->wiphy, "Cannot map DMA for RX skb\n");
+			return -ENOMEM;
+		}
+
 		entry->rx_buf = cpu_to_le32(*mapping);
 		entry->flags = cpu_to_le32(RTL818X_RX_DESC_FLAG_OWN |
 					   MAX_RX_SIZE);
@@ -602,13 +638,25 @@ static int rtl8180_start(struct ieee80211_hw *dev)
 
 	if (priv->r8185) {
 		reg = rtl818x_ioread8(priv, &priv->map->CW_CONF);
-		reg &= ~RTL818X_CW_CONF_PERPACKET_CW_SHIFT;
-		reg |= RTL818X_CW_CONF_PERPACKET_RETRY_SHIFT;
+
+		/* CW is not on per-packet basis.
+		 * in rtl8185 the CW_VALUE reg is used.
+		 */
+		reg &= ~RTL818X_CW_CONF_PERPACKET_CW;
+		/* retry limit IS on per-packet basis.
+		 * the short and long retry limit in TX_CONF
+		 * reg are ignored
+		 */
+		reg |= RTL818X_CW_CONF_PERPACKET_RETRY;
 		rtl818x_iowrite8(priv, &priv->map->CW_CONF, reg);
 
 		reg = rtl818x_ioread8(priv, &priv->map->TX_AGC_CTL);
-		reg &= ~RTL818X_TX_AGC_CTL_PERPACKET_GAIN_SHIFT;
-		reg &= ~RTL818X_TX_AGC_CTL_PERPACKET_ANTSEL_SHIFT;
+		/* TX antenna and TX gain are not on per-packet basis.
+		 * TX Antenna is selected by ANTSEL reg (RX in BB regs).
+		 * TX gain is selected with CCK_TX_AGC and OFDM_TX_AGC regs
+		 */
+		reg &= ~RTL818X_TX_AGC_CTL_PERPACKET_GAIN;
+		reg &= ~RTL818X_TX_AGC_CTL_PERPACKET_ANTSEL;
 		reg |=  RTL818X_TX_AGC_CTL_FEEDBACK_ANT;
 		rtl818x_iowrite8(priv, &priv->map->TX_AGC_CTL, reg);
 
@@ -625,6 +673,8 @@ static int rtl8180_start(struct ieee80211_hw *dev)
 	else
 		reg &= ~RTL818X_TX_CONF_HW_SEQNUM;
 
+	reg &= ~RTL818X_TX_CONF_DISCW;
+
 	/* different meaning, same value on both rtl8185 and rtl8180 */
 	reg &= ~RTL818X_TX_CONF_SAT_HWPLCP;
 
@@ -1118,7 +1168,7 @@ static int rtl8180_probe(struct pci_dev *pdev,
 	return 0;
 
  err_iounmap:
-	iounmap(priv->map);
+	pci_iounmap(pdev, priv->map);
 
  err_free_dev:
 	ieee80211_free_hw(dev);
diff --git a/drivers/net/wireless/rtl818x/rtl8187/dev.c b/drivers/net/wireless/rtl818x/rtl8187/dev.c
index fd78df813a85..c981bcfb6cef 100644
--- a/drivers/net/wireless/rtl818x/rtl8187/dev.c
+++ b/drivers/net/wireless/rtl818x/rtl8187/dev.c
@@ -785,7 +785,7 @@ static int rtl8187b_init_hw(struct ieee80211_hw *dev)
 	rtl818x_iowrite16(priv, (__le16 *)0xFF34, 0x0FFF);
 
 	reg = rtl818x_ioread8(priv, &priv->map->CW_CONF);
-	reg |= RTL818X_CW_CONF_PERPACKET_RETRY_SHIFT;
+	reg |= RTL818X_CW_CONF_PERPACKET_RETRY;
 	rtl818x_iowrite8(priv, &priv->map->CW_CONF, reg);
 
 	/* Auto Rate Fallback Register (ARFR): 1M-54M setting */
@@ -943,8 +943,8 @@ static int rtl8187_start(struct ieee80211_hw *dev)
 		rtl818x_iowrite32(priv, &priv->map->RX_CONF, reg);
 
 		reg = rtl818x_ioread8(priv, &priv->map->TX_AGC_CTL);
-		reg &= ~RTL818X_TX_AGC_CTL_PERPACKET_GAIN_SHIFT;
-		reg &= ~RTL818X_TX_AGC_CTL_PERPACKET_ANTSEL_SHIFT;
+		reg &= ~RTL818X_TX_AGC_CTL_PERPACKET_GAIN;
+		reg &= ~RTL818X_TX_AGC_CTL_PERPACKET_ANTSEL;
 		reg &= ~RTL818X_TX_AGC_CTL_FEEDBACK_ANT;
 		rtl818x_iowrite8(priv, &priv->map->TX_AGC_CTL, reg);
 
@@ -986,13 +986,13 @@ static int rtl8187_start(struct ieee80211_hw *dev)
 	rtl818x_iowrite32(priv, &priv->map->RX_CONF, reg);
 
 	reg = rtl818x_ioread8(priv, &priv->map->CW_CONF);
-	reg &= ~RTL818X_CW_CONF_PERPACKET_CW_SHIFT;
-	reg |= RTL818X_CW_CONF_PERPACKET_RETRY_SHIFT;
+	reg &= ~RTL818X_CW_CONF_PERPACKET_CW;
+	reg |= RTL818X_CW_CONF_PERPACKET_RETRY;
 	rtl818x_iowrite8(priv, &priv->map->CW_CONF, reg);
 
 	reg = rtl818x_ioread8(priv, &priv->map->TX_AGC_CTL);
-	reg &= ~RTL818X_TX_AGC_CTL_PERPACKET_GAIN_SHIFT;
-	reg &= ~RTL818X_TX_AGC_CTL_PERPACKET_ANTSEL_SHIFT;
+	reg &= ~RTL818X_TX_AGC_CTL_PERPACKET_GAIN;
+	reg &= ~RTL818X_TX_AGC_CTL_PERPACKET_ANTSEL;
 	reg &= ~RTL818X_TX_AGC_CTL_FEEDBACK_ANT;
 	rtl818x_iowrite8(priv, &priv->map->TX_AGC_CTL, reg);
 
diff --git a/drivers/net/wireless/rtl818x/rtl8187/rtl8187.h b/drivers/net/wireless/rtl818x/rtl8187/rtl8187.h
index 56aee067f324..a6ad79f61bf9 100644
--- a/drivers/net/wireless/rtl818x/rtl8187/rtl8187.h
+++ b/drivers/net/wireless/rtl818x/rtl8187/rtl8187.h
@@ -15,6 +15,8 @@
 #ifndef RTL8187_H
 #define RTL8187_H
 
+#include <linux/cache.h>
+
 #include "rtl818x.h"
 #include "leds.h"
 
@@ -139,7 +141,10 @@ struct rtl8187_priv {
 	u8 aifsn[4];
 	u8 rfkill_mask;
 	struct {
-		__le64 buf;
+		union {
+			__le64 buf;
+			u8 dummy1[L1_CACHE_BYTES];
+		} ____cacheline_aligned;
 		struct sk_buff_head queue;
 	} b_tx_status; /* This queue is used by both -b and non-b devices */
 	struct mutex io_mutex;
@@ -147,7 +152,8 @@ struct rtl8187_priv {
 		u8 bits8;
 		__le16 bits16;
 		__le32 bits32;
-	} *io_dmabuf;
+		u8 dummy2[L1_CACHE_BYTES];
+	} *io_dmabuf ____cacheline_aligned;
 	bool rfkill_off;
 	u16 seqno;
 };
diff --git a/drivers/net/wireless/rtl818x/rtl818x.h b/drivers/net/wireless/rtl818x/rtl818x.h
index ce23dfd42381..fa7f7f61ea26 100644
--- a/drivers/net/wireless/rtl818x/rtl818x.h
+++ b/drivers/net/wireless/rtl818x/rtl818x.h
@@ -144,9 +144,9 @@ struct rtl818x_csr {
 	__le32	HSSI_PARA;
 	u8	reserved_13[4];
 	u8	TX_AGC_CTL;
-#define RTL818X_TX_AGC_CTL_PERPACKET_GAIN_SHIFT		(1 << 0)
-#define RTL818X_TX_AGC_CTL_PERPACKET_ANTSEL_SHIFT	(1 << 1)
-#define RTL818X_TX_AGC_CTL_FEEDBACK_ANT			(1 << 2)
+#define RTL818X_TX_AGC_CTL_PERPACKET_GAIN	(1 << 0)
+#define RTL818X_TX_AGC_CTL_PERPACKET_ANTSEL	(1 << 1)
+#define RTL818X_TX_AGC_CTL_FEEDBACK_ANT		(1 << 2)
 	u8	TX_GAIN_CCK;
 	u8	TX_GAIN_OFDM;
 	u8	TX_ANTENNA;
@@ -158,8 +158,8 @@ struct rtl818x_csr {
 	u8	SLOT;
 	u8	reserved_16[5];
 	u8	CW_CONF;
-#define RTL818X_CW_CONF_PERPACKET_CW_SHIFT	(1 << 0)
-#define RTL818X_CW_CONF_PERPACKET_RETRY_SHIFT	(1 << 1)
+#define RTL818X_CW_CONF_PERPACKET_CW	(1 << 0)
+#define RTL818X_CW_CONF_PERPACKET_RETRY	(1 << 1)
 	u8	CW_VAL;
 	u8	RATE_FALLBACK;
 #define RTL818X_RATE_FALLBACK_ENABLE	(1 << 7)
diff --git a/drivers/net/wireless/rtlwifi/Kconfig b/drivers/net/wireless/rtlwifi/Kconfig
index c2ffce7a907c..bf3cf124e4ea 100644
--- a/drivers/net/wireless/rtlwifi/Kconfig
+++ b/drivers/net/wireless/rtlwifi/Kconfig
@@ -5,7 +5,7 @@ menuconfig RTL_CARDS
 	---help---
 	  This option will enable support for the Realtek mac80211-based
 	  wireless drivers. Drivers rtl8192ce, rtl8192cu, rtl8192se, rtl8192de,
-	  rtl8723eu, and rtl8188eu share some common code.
+	  rtl8723ae, rtl8723be, and rtl8188ae share some common code.
 
 if RTL_CARDS
 
@@ -48,12 +48,27 @@ config RTL8723AE
 	depends on PCI
 	select RTLWIFI
 	select RTLWIFI_PCI
+	select RTL8723_COMMON
+	select RTLBTCOEXIST
 	---help---
 	This is the driver for Realtek RTL8723AE 802.11n PCIe
 	wireless network adapters.
 
 	If you choose to build it as a module, it will be called rtl8723ae
 
+config RTL8723BE
+	tristate "Realtek RTL8723BE PCIe Wireless Network Adapter"
+	depends on PCI
+	select RTLWIFI
+	select RTLWIFI_PCI
+	select RTL8723_COMMON
+	select RTLBTCOEXIST
+	---help---
+	This is the driver for Realtek RTL8723BE 802.11n PCIe
+	wireless network adapters.
+
+	If you choose to build it as a module, it will be called rtl8723be
+
 config RTL8188EE
 	tristate "Realtek RTL8188EE Wireless Network Adapter"
 	depends on PCI
@@ -101,4 +116,14 @@ config RTL8192C_COMMON
 	depends on RTL8192CE || RTL8192CU
 	default y
 
+config RTL8723_COMMON
+	tristate
+	depends on RTL8723AE || RTL8723BE
+	default y
+
+config RTLBTCOEXIST
+	tristate
+	depends on RTL8723AE || RTL8723BE
+	default y
+
 endif
diff --git a/drivers/net/wireless/rtlwifi/Makefile b/drivers/net/wireless/rtlwifi/Makefile
index d56f023a4b90..bba36a06abcc 100644
--- a/drivers/net/wireless/rtlwifi/Makefile
+++ b/drivers/net/wireless/rtlwifi/Makefile
@@ -24,6 +24,9 @@ obj-$(CONFIG_RTL8192CU)		+= rtl8192cu/
 obj-$(CONFIG_RTL8192SE)		+= rtl8192se/
 obj-$(CONFIG_RTL8192DE)		+= rtl8192de/
 obj-$(CONFIG_RTL8723AE)		+= rtl8723ae/
+obj-$(CONFIG_RTL8723BE)		+= rtl8723be/
 obj-$(CONFIG_RTL8188EE)		+= rtl8188ee/
+obj-$(CONFIG_RTLBTCOEXIST)	+= btcoexist/
+obj-$(CONFIG_RTL8723_COMMON)	+= rtl8723com/
 
 ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/net/wireless/rtlwifi/btcoexist/Makefile b/drivers/net/wireless/rtlwifi/btcoexist/Makefile
new file mode 100644
index 000000000000..47ceecfcb7dc
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/btcoexist/Makefile
@@ -0,0 +1,7 @@
+btcoexist-objs :=	halbtc8723b2ant.o	\
+			halbtcoutsrc.o		\
+			rtl_btc.o
+
+obj-$(CONFIG_RTLBTCOEXIST) += btcoexist.o
+
+ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/net/wireless/rtlwifi/btcoexist/halbt_precomp.h b/drivers/net/wireless/rtlwifi/btcoexist/halbt_precomp.h
new file mode 100644
index 000000000000..d76684eb24d0
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/btcoexist/halbt_precomp.h
@@ -0,0 +1,75 @@
+/******************************************************************************
+ *
+ * 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 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	__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 "halbtc8723b2ant.h"
+
+#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
+
+#endif	/* __HALBT_PRECOMP_H__ */
diff --git a/drivers/net/wireless/rtlwifi/btcoexist/halbtc8723b2ant.c b/drivers/net/wireless/rtlwifi/btcoexist/halbtc8723b2ant.c
new file mode 100644
index 000000000000..d916ab9f3c38
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/btcoexist/halbtc8723b2ant.c
@@ -0,0 +1,3698 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2012  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+/***************************************************************
+ * Description:
+ *
+ * This file is for 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 = 20130731;
+static u32 glcoex_ver_8723b_2ant = 0x3b;
+
+/**************************************************************
+ * local function proto type if needed
+ **************************************************************/
+/**************************************************************
+ * local function start with btc8723b2ant_
+ **************************************************************/
+static u8 btc8723b2ant_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 btc8723b2ant_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 btc8723b2ant_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 bool btc8723b2ant_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 btc8723b2ant_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 btc8723b2ant_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 btc8723b_need_dec_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 = btc8723b2ant_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 btc8723b2ant_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 btc8723b2ant_set_fw_dec_bt_pwr(struct btc_coexist *btcoexist,
+					   bool dec_bt_pwr)
+{
+	u8 h2c_parameter[1] = {0};
+
+	h2c_parameter[0] = 0;
+
+	if (dec_bt_pwr)
+		h2c_parameter[0] |= BIT1;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], decrease Bt Power : %s, FW write 0x62=0x%x\n",
+		  (dec_bt_pwr ? "Yes!!" : "No!!"), h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x62, 1, h2c_parameter);
+}
+
+static void btc8723b2ant_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;
+	}
+	btc8723b2ant_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 btc8723b2ant_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;
+	}
+
+	btc8723b2ant_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 btc8723b2ant_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 btc8723b2ant_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;
+	}
+	btc8723b2ant_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 btc8723b_set_penalty_txrate(struct btc_coexist *btcoexist,
+					bool low_penalty_ra)
+{
+	u8 h2c_parameter[6] = {0};
+
+	h2c_parameter[0] = 0x6;	/* opCode, 0x6= Retry_Penalty*/
+
+	if (low_penalty_ra) {
+		h2c_parameter[1] |= BIT0;
+		/*normal rate except MCS7/6/5, OFDM54/48/36*/
+		h2c_parameter[2] = 0x00;
+		h2c_parameter[3] = 0xf7;  /*MCS7 or OFDM54*/
+		h2c_parameter[4] = 0xf8;  /*MCS6 or OFDM48*/
+		h2c_parameter[5] = 0xf9;  /*MCS5 or OFDM36*/
+	}
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], set WiFi Low-Penalty Retry: %s",
+		  (low_penalty_ra ? "ON!!" : "OFF!!"));
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x69, 6, h2c_parameter);
+}
+
+static void btc8723b2ant_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;
+	}
+	btc8723b_set_penalty_txrate(btcoexist, coex_dm->cur_low_penalty_ra);
+
+	coex_dm->pre_low_penalty_ra = coex_dm->cur_low_penalty_ra;
+}
+
+static void btc8723b2ant_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 btc8723b2ant_set_sw_fulltime_dac_swing(struct btc_coexist *btcoex,
+						   bool sw_dac_swing_on,
+						   u32 sw_dac_swing_lvl)
+{
+	if (sw_dac_swing_on)
+		btc8723b2ant_set_dac_swing_reg(btcoex, sw_dac_swing_lvl);
+	else
+		btc8723b2ant_set_dac_swing_reg(btcoex, 0x18);
+}
+
+
+static void btc8723b2ant_dac_swing(struct btc_coexist *btcoexist,
+				   bool force_exec, bool dac_swing_on,
+				   u32 dac_swing_lvl)
+{
+	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);
+	btc8723b2ant_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 btc8723b2ant_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 btc8723b2ant_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;
+	}
+	btc8723b2ant_set_agc_table(btcoexist, agc_table_en);
+
+	coex_dm->pre_agc_table_en = coex_dm->cur_agc_table_en;
+}
+
+static void btc8723b2ant_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 btc8723b2ant_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;
+	}
+	btc8723b2ant_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 btc8723b_coex_tbl_type(struct btc_coexist *btcoexist,
+				   bool force_exec, u8 type)
+{
+	switch (type) {
+	case 0:
+		btc8723b2ant_coex_table(btcoexist, force_exec, 0x55555555,
+					0x55555555, 0xffff, 0x3);
+		break;
+	case 1:
+		btc8723b2ant_coex_table(btcoexist, force_exec, 0x55555555,
+					0x5afa5afa, 0xffff, 0x3);
+		break;
+	case 2:
+		btc8723b2ant_coex_table(btcoexist, force_exec, 0x5a5a5a5a,
+					0x5a5a5a5a, 0xffff, 0x3);
+		break;
+	case 3:
+		btc8723b2ant_coex_table(btcoexist, force_exec, 0xaaaaaaaa,
+					0xaaaaaaaa, 0xffff, 0x3);
+		break;
+	case 4:
+		btc8723b2ant_coex_table(btcoexist, force_exec, 0xffffffff,
+					0xffffffff, 0xffff, 0x3);
+		break;
+	case 5:
+		btc8723b2ant_coex_table(btcoexist, force_exec, 0x5fff5fff,
+					0x5fff5fff, 0xffff, 0x3);
+		break;
+	case 6:
+		btc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
+					0x5a5a5a5a, 0xffff, 0x3);
+		break;
+	case 7:
+		btc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
+					0x5afa5afa, 0xffff, 0x3);
+		break;
+	case 8:
+		btc8723b2ant_coex_table(btcoexist, force_exec, 0x5aea5aea,
+					0x5aea5aea, 0xffff, 0x3);
+		break;
+	case 9:
+		btc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
+					0x5aea5aea, 0xffff, 0x3);
+		break;
+	case 10:
+		btc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
+					0x5aff5aff, 0xffff, 0x3);
+		break;
+	case 11:
+		btc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
+					0x5a5f5a5f, 0xffff, 0x3);
+		break;
+	case 12:
+		btc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
+					0x5f5f5f5f, 0xffff, 0x3);
+		break;
+	default:
+		break;
+	}
+}
+
+static void btc8723b2ant_set_fw_ignore_wlan_act(struct btc_coexist *btcoexist,
+						bool enable)
+{
+	u8 h2c_parameter[1] = {0};
+
+	if (enable)
+		h2c_parameter[0] |= BIT0;/* function enable*/
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], set FW for BT Ignore Wlan_Act, "
+		  "FW write 0x63=0x%x\n", h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x63, 1, h2c_parameter);
+}
+
+static void btc8723b2ant_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;
+	}
+	btc8723b2ant_set_fw_ignore_wlan_act(btcoexist, enable);
+
+	coex_dm->pre_ignore_wlan_act = coex_dm->cur_ignore_wlan_act;
+}
+
+static void btc8723b2ant_set_fw_ps_tdma(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 btc8723b2ant_sw_mechanism1(struct btc_coexist *btcoexist,
+				       bool shrink_rx_lpf, bool low_penalty_ra,
+				       bool limited_dig, bool bt_lna_constrain)
+{
+	btc8723b2ant_rf_shrink(btcoexist, NORMAL_EXEC, shrink_rx_lpf);
+	btc8723b2ant_low_penalty_ra(btcoexist, NORMAL_EXEC, low_penalty_ra);
+}
+
+static void btc8723b2ant_sw_mechanism2(struct btc_coexist *btcoexist,
+				       bool agc_table_shift, bool adc_backoff,
+				       bool sw_dac_swing, u32 dac_swing_lvl)
+{
+	btc8723b2ant_agc_table(btcoexist, NORMAL_EXEC, agc_table_shift);
+	btc8723b2ant_dac_swing(btcoexist, NORMAL_EXEC, sw_dac_swing,
+			       dac_swing_lvl);
+}
+
+static void btc8723b2ant_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:
+			btc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x1a,
+						    0x1a, 0xe1, 0x90);
+			break;
+		case 2:
+			btc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x12,
+						    0x12, 0xe1, 0x90);
+			break;
+		case 3:
+			btc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x1c,
+						    0x3, 0xf1, 0x90);
+			break;
+		case 4:
+			btc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x10,
+						    0x03, 0xf1, 0x90);
+			break;
+		case 5:
+			btc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x1a,
+						    0x1a, 0x60, 0x90);
+			break;
+		case 6:
+			btc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x12,
+						    0x12, 0x60, 0x90);
+			break;
+		case 7:
+			btc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x1c,
+						    0x3, 0x70, 0x90);
+			break;
+		case 8:
+			btc8723b2ant_set_fw_ps_tdma(btcoexist, 0xa3, 0x10,
+						    0x3, 0x70, 0x90);
+			break;
+		case 9:
+			btc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x1a,
+						    0x1a, 0xe1, 0x90);
+			break;
+		case 10:
+			btc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x12,
+						    0x12, 0xe1, 0x90);
+			break;
+		case 11:
+			btc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0xa,
+						    0xa, 0xe1, 0x90);
+			break;
+		case 12:
+			btc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x5,
+						    0x5, 0xe1, 0x90);
+			break;
+		case 13:
+			btc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x1a,
+						    0x1a, 0x60, 0x90);
+			break;
+		case 14:
+			btc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x12,
+						    0x12, 0x60, 0x90);
+			break;
+		case 15:
+			btc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0xa,
+						    0xa, 0x60, 0x90);
+			break;
+		case 16:
+			btc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x5,
+						    0x5, 0x60, 0x90);
+			break;
+		case 17:
+			btc8723b2ant_set_fw_ps_tdma(btcoexist, 0xa3, 0x2f,
+						    0x2f, 0x60, 0x90);
+			break;
+		case 18:
+			btc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x5,
+						    0x5, 0xe1, 0x90);
+			break;
+		case 19:
+			btc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x25,
+						    0x25, 0xe1, 0x90);
+			break;
+		case 20:
+			btc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x25,
+						    0x25, 0x60, 0x90);
+			break;
+		case 21:
+			btc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x15,
+						    0x03, 0x70, 0x90);
+			break;
+		case 71:
+			btc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x1a,
+						    0x1a, 0xe1, 0x90);
+			break;
+		}
+	} else {
+		/* disable PS tdma */
+		switch (type) {
+		case 0:
+			btc8723b2ant_set_fw_ps_tdma(btcoexist, 0x0, 0x0, 0x0,
+						    0x40, 0x0);
+			break;
+		case 1:
+			btc8723b2ant_set_fw_ps_tdma(btcoexist, 0x0, 0x0, 0x0,
+						    0x48, 0x0);
+			break;
+		default:
+			btc8723b2ant_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 btc8723b2ant_coex_alloff(struct btc_coexist *btcoexist)
+{
+	/* fw all off */
+	btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
+	btc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+	btc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	/* sw all off */
+	btc8723b2ant_sw_mechanism1(btcoexist, false, false, false, false);
+	btc8723b2ant_sw_mechanism2(btcoexist, false, false, false, 0x18);
+
+	/* hw all off */
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+	btc8723b_coex_tbl_type(btcoexist, NORMAL_EXEC, 0);
+}
+
+static void btc8723b2ant_init_coex_dm(struct btc_coexist *btcoexist)
+{
+	/* force to reset coex mechanism*/
+
+	btc8723b2ant_ps_tdma(btcoexist, FORCE_EXEC, false, 1);
+	btc8723b2ant_fw_dac_swing_lvl(btcoexist, FORCE_EXEC, 6);
+	btc8723b2ant_dec_bt_pwr(btcoexist, FORCE_EXEC, false);
+
+	btc8723b2ant_sw_mechanism1(btcoexist, false, false, false, false);
+	btc8723b2ant_sw_mechanism2(btcoexist, false, false, false, 0x18);
+}
+
+static void btc8723b2ant_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) {
+		btc8723b_coex_tbl_type(btcoexist, NORMAL_EXEC, 7);
+		btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 3);
+	} else {
+		btc8723b_coex_tbl_type(btcoexist, NORMAL_EXEC, 0);
+		btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
+	}
+	btc8723b2ant_fw_dac_swing_lvl(btcoexist, FORCE_EXEC, 6);
+	btc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btc8723b2ant_sw_mechanism1(btcoexist, false, false, false, false);
+	btc8723b2ant_sw_mechanism2(btcoexist, false, false, false, 0x18);
+
+	coex_dm->need_recover_0x948 = true;
+	coex_dm->backup_0x948 = btcoexist->btc_read_2byte(btcoexist, 0x948);
+
+	btcoexist->btc_write_2byte(btcoexist, 0x948, 0x280);
+}
+
+static bool btc8723b2ant_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);
+		btc8723b_coex_tbl_type(btcoexist, NORMAL_EXEC, 0);
+		btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
+		btc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+		btc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+		btc8723b2ant_sw_mechanism1(btcoexist, false, false, false,
+					   false);
+		btc8723b2ant_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);
+			btc8723b_coex_tbl_type(btcoexist, NORMAL_EXEC, 0);
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
+			btc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC,
+						      0xb);
+			btc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC,
+						false);
+
+			btc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						   false, false);
+			btc8723b2ant_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);
+			btc8723b_coex_tbl_type(btcoexist, NORMAL_EXEC, 0);
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
+			btc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC,
+						      0xb);
+			btc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC,
+						false);
+
+			btc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						   false, false);
+			btc8723b2ant_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);
+				btc8723b_coex_tbl_type(btcoexist, NORMAL_EXEC,
+						       7);
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 21);
+				btc8723b2ant_fw_dac_swing_lvl(btcoexist,
+							      NORMAL_EXEC,
+							      0xb);
+				if (btc8723b_need_dec_pwr(btcoexist))
+					btc8723b2ant_dec_bt_pwr(btcoexist,
+								NORMAL_EXEC,
+								true);
+				else
+					btc8723b2ant_dec_bt_pwr(btcoexist,
+								NORMAL_EXEC,
+								false);
+				btc8723b2ant_sw_mechanism1(btcoexist, false,
+							   false, false,
+							   false);
+				btc8723b2ant_sw_mechanism2(btcoexist, false,
+							   false, false,
+							   0x18);
+				common = true;
+			}
+		}
+	}
+
+	return common;
+}
+
+static void set_tdma_int1(struct btc_coexist *btcoexist, bool tx_pause,
+			  s32 result)
+{
+	/* Set PS TDMA for 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) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+					     true, 5);
+			coex_dm->tdma_adj_type = 5;
+		} else if (coex_dm->cur_ps_tdma == 1) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+					     true, 5);
+			coex_dm->tdma_adj_type = 5;
+		} else if (coex_dm->cur_ps_tdma == 2) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+					     true, 6);
+			coex_dm->tdma_adj_type = 6;
+		} else if (coex_dm->cur_ps_tdma == 3) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+					     true, 7);
+			coex_dm->tdma_adj_type = 7;
+		} else if (coex_dm->cur_ps_tdma == 4) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+					     true, 8);
+			coex_dm->tdma_adj_type = 8;
+		} else if (coex_dm->cur_ps_tdma == 9) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+					     true, 13);
+			coex_dm->tdma_adj_type = 13;
+		} else if (coex_dm->cur_ps_tdma == 10) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+					     true, 14);
+			coex_dm->tdma_adj_type = 14;
+		} else if (coex_dm->cur_ps_tdma == 11) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+					     true, 15);
+			coex_dm->tdma_adj_type = 15;
+		} else if (coex_dm->cur_ps_tdma == 12) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+					     true, 16);
+			coex_dm->tdma_adj_type = 16;
+		}
+
+		if (result == -1) {
+			if (coex_dm->cur_ps_tdma == 5) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 6);
+				coex_dm->tdma_adj_type = 6;
+			} else if (coex_dm->cur_ps_tdma == 6) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 7);
+				coex_dm->tdma_adj_type = 7;
+			} else if (coex_dm->cur_ps_tdma == 7) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 8);
+				coex_dm->tdma_adj_type = 8;
+			} else if (coex_dm->cur_ps_tdma == 13) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 14);
+				coex_dm->tdma_adj_type = 14;
+			} else if (coex_dm->cur_ps_tdma == 14) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 15);
+				coex_dm->tdma_adj_type = 15;
+			} else if (coex_dm->cur_ps_tdma == 15) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 16);
+				coex_dm->tdma_adj_type = 16;
+			}
+		}  else if (result == 1) {
+			if (coex_dm->cur_ps_tdma == 8) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 7);
+				coex_dm->tdma_adj_type = 7;
+			} else if (coex_dm->cur_ps_tdma == 7) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 6);
+				coex_dm->tdma_adj_type = 6;
+			} else if (coex_dm->cur_ps_tdma == 6) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 5);
+				coex_dm->tdma_adj_type = 5;
+			} else if (coex_dm->cur_ps_tdma == 16) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 15);
+				coex_dm->tdma_adj_type = 15;
+			} else if (coex_dm->cur_ps_tdma == 15) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 14);
+				coex_dm->tdma_adj_type = 14;
+			} else if (coex_dm->cur_ps_tdma == 14) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 13);
+				coex_dm->tdma_adj_type = 13;
+			}
+		}
+	} else {
+		BTC_PRINT(BTC_MSG_ALGORITHM,
+			  ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], TxPause = 0\n");
+		if (coex_dm->cur_ps_tdma == 5) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 71);
+			coex_dm->tdma_adj_type = 71;
+		} else if (coex_dm->cur_ps_tdma == 6) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 2);
+			coex_dm->tdma_adj_type = 2;
+		} else if (coex_dm->cur_ps_tdma == 7) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 3);
+			coex_dm->tdma_adj_type = 3;
+		} else if (coex_dm->cur_ps_tdma == 8) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 4);
+			coex_dm->tdma_adj_type = 4;
+		} else if (coex_dm->cur_ps_tdma == 13) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
+			coex_dm->tdma_adj_type = 9;
+		} else if (coex_dm->cur_ps_tdma == 14) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 10);
+			coex_dm->tdma_adj_type = 10;
+		} else if (coex_dm->cur_ps_tdma == 15) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 11);
+			coex_dm->tdma_adj_type = 11;
+		} else if (coex_dm->cur_ps_tdma == 16) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 12);
+			coex_dm->tdma_adj_type = 12;
+		}
+
+		if (result == -1) {
+			if (coex_dm->cur_ps_tdma == 71) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 1);
+				coex_dm->tdma_adj_type = 1;
+			} else if (coex_dm->cur_ps_tdma == 1) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 2);
+				coex_dm->tdma_adj_type = 2;
+			} else if (coex_dm->cur_ps_tdma == 2) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 3);
+				coex_dm->tdma_adj_type = 3;
+			} else if (coex_dm->cur_ps_tdma == 3) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 4);
+				coex_dm->tdma_adj_type = 4;
+			} else if (coex_dm->cur_ps_tdma == 9) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 10);
+				coex_dm->tdma_adj_type = 10;
+			} else if (coex_dm->cur_ps_tdma == 10) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 11);
+				coex_dm->tdma_adj_type = 11;
+			} else if (coex_dm->cur_ps_tdma == 11) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 12);
+				coex_dm->tdma_adj_type = 12;
+			}
+		}  else if (result == 1) {
+			int tmp = coex_dm->cur_ps_tdma;
+			switch (tmp) {
+			case 4:
+			case 3:
+			case 2:
+			case 12:
+			case 11:
+			case 10:
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, tmp - 1);
+				coex_dm->tdma_adj_type = tmp - 1;
+				break;
+			case 1:
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 71);
+				coex_dm->tdma_adj_type = 71;
+				break;
+			}
+		}
+	}
+}
+
+static void set_tdma_int2(struct btc_coexist *btcoexist, bool tx_pause,
+			  s32 result)
+{
+	/* Set PS TDMA for 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) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 6);
+			coex_dm->tdma_adj_type = 6;
+		} else if (coex_dm->cur_ps_tdma == 2) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 6);
+			coex_dm->tdma_adj_type = 6;
+		} else if (coex_dm->cur_ps_tdma == 3) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 7);
+			coex_dm->tdma_adj_type = 7;
+		} else if (coex_dm->cur_ps_tdma == 4) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 8);
+			coex_dm->tdma_adj_type = 8;
+		} else if (coex_dm->cur_ps_tdma == 9) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 14);
+			coex_dm->tdma_adj_type = 14;
+		} else if (coex_dm->cur_ps_tdma == 10) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 14);
+			coex_dm->tdma_adj_type = 14;
+		} else if (coex_dm->cur_ps_tdma == 11) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 15);
+			coex_dm->tdma_adj_type = 15;
+		} else if (coex_dm->cur_ps_tdma == 12) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 16);
+			coex_dm->tdma_adj_type = 16;
+		}
+		if (result == -1) {
+			if (coex_dm->cur_ps_tdma == 5) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 6);
+				coex_dm->tdma_adj_type = 6;
+			} else if (coex_dm->cur_ps_tdma == 6) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 7);
+				coex_dm->tdma_adj_type = 7;
+			} else if (coex_dm->cur_ps_tdma == 7) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 8);
+				coex_dm->tdma_adj_type = 8;
+			} else if (coex_dm->cur_ps_tdma == 13) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 14);
+				coex_dm->tdma_adj_type = 14;
+			} else if (coex_dm->cur_ps_tdma == 14) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 15);
+				coex_dm->tdma_adj_type = 15;
+			} else if (coex_dm->cur_ps_tdma == 15) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 16);
+				coex_dm->tdma_adj_type = 16;
+			}
+		}  else if (result == 1) {
+			if (coex_dm->cur_ps_tdma == 8) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 7);
+				coex_dm->tdma_adj_type = 7;
+			} else if (coex_dm->cur_ps_tdma == 7) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 6);
+				coex_dm->tdma_adj_type = 6;
+			} else if (coex_dm->cur_ps_tdma == 6) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 6);
+				coex_dm->tdma_adj_type = 6;
+			} else if (coex_dm->cur_ps_tdma == 16) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 15);
+				coex_dm->tdma_adj_type = 15;
+			} else if (coex_dm->cur_ps_tdma == 15) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 14);
+				coex_dm->tdma_adj_type = 14;
+			} else if (coex_dm->cur_ps_tdma == 14) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 14);
+				coex_dm->tdma_adj_type = 14;
+			}
+		}
+	} else {
+		BTC_PRINT(BTC_MSG_ALGORITHM,
+			  ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], TxPause = 0\n");
+		if (coex_dm->cur_ps_tdma == 5) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 2);
+			coex_dm->tdma_adj_type = 2;
+		} else if (coex_dm->cur_ps_tdma == 6) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 2);
+			coex_dm->tdma_adj_type = 2;
+		} else if (coex_dm->cur_ps_tdma == 7) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 3);
+			coex_dm->tdma_adj_type = 3;
+		} else if (coex_dm->cur_ps_tdma == 8) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 4);
+			coex_dm->tdma_adj_type = 4;
+		} else if (coex_dm->cur_ps_tdma == 13) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 10);
+			coex_dm->tdma_adj_type = 10;
+		} else if (coex_dm->cur_ps_tdma == 14) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 10);
+			coex_dm->tdma_adj_type = 10;
+		} else if (coex_dm->cur_ps_tdma == 15) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 11);
+			coex_dm->tdma_adj_type = 11;
+		} else if (coex_dm->cur_ps_tdma == 16) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 12);
+			coex_dm->tdma_adj_type = 12;
+		}
+		if (result == -1) {
+			if (coex_dm->cur_ps_tdma == 1) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 2);
+				coex_dm->tdma_adj_type = 2;
+			} else if (coex_dm->cur_ps_tdma == 2) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 3);
+				coex_dm->tdma_adj_type = 3;
+			} else if (coex_dm->cur_ps_tdma == 3) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 4);
+				coex_dm->tdma_adj_type = 4;
+			} else if (coex_dm->cur_ps_tdma == 9) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 10);
+				coex_dm->tdma_adj_type = 10;
+			} else if (coex_dm->cur_ps_tdma == 10) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 11);
+				coex_dm->tdma_adj_type = 11;
+			} else if (coex_dm->cur_ps_tdma == 11) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 12);
+				coex_dm->tdma_adj_type = 12;
+			}
+		} else if (result == 1) {
+			if (coex_dm->cur_ps_tdma == 4) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 3);
+				coex_dm->tdma_adj_type = 3;
+			} else if (coex_dm->cur_ps_tdma == 3) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 2);
+				coex_dm->tdma_adj_type = 2;
+			} else if (coex_dm->cur_ps_tdma == 2) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 2);
+				coex_dm->tdma_adj_type = 2;
+			} else if (coex_dm->cur_ps_tdma == 12) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 11);
+				coex_dm->tdma_adj_type = 11;
+			} else if (coex_dm->cur_ps_tdma == 11) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 10);
+				coex_dm->tdma_adj_type = 10;
+			} else if (coex_dm->cur_ps_tdma == 10) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 10);
+				coex_dm->tdma_adj_type = 10;
+			}
+		}
+	}
+}
+
+static void set_tdma_int3(struct btc_coexist *btcoexist, bool tx_pause,
+			  s32 result)
+{
+	/* Set PS TDMA for 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) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 7);
+			coex_dm->tdma_adj_type = 7;
+		} else if (coex_dm->cur_ps_tdma == 2) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 7);
+			coex_dm->tdma_adj_type = 7;
+		} else if (coex_dm->cur_ps_tdma == 3) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 7);
+			coex_dm->tdma_adj_type = 7;
+		} else if (coex_dm->cur_ps_tdma == 4) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 8);
+			coex_dm->tdma_adj_type = 8;
+		} else if (coex_dm->cur_ps_tdma == 9) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 15);
+			coex_dm->tdma_adj_type = 15;
+		} else if (coex_dm->cur_ps_tdma == 10) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 15);
+			coex_dm->tdma_adj_type = 15;
+		} else if (coex_dm->cur_ps_tdma == 11) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 15);
+			coex_dm->tdma_adj_type = 15;
+		} else if (coex_dm->cur_ps_tdma == 12) {
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 16);
+			coex_dm->tdma_adj_type = 16;
+		}
+		if (result == -1) {
+			if (coex_dm->cur_ps_tdma == 5) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 7);
+				coex_dm->tdma_adj_type = 7;
+			} else if (coex_dm->cur_ps_tdma == 6) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 7);
+				coex_dm->tdma_adj_type = 7;
+			} else if (coex_dm->cur_ps_tdma == 7) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 8);
+				coex_dm->tdma_adj_type = 8;
+			} else if (coex_dm->cur_ps_tdma == 13) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 15);
+				coex_dm->tdma_adj_type = 15;
+			} else if (coex_dm->cur_ps_tdma == 14) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 15);
+				coex_dm->tdma_adj_type = 15;
+			} else if (coex_dm->cur_ps_tdma == 15) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 16);
+				coex_dm->tdma_adj_type = 16;
+			}
+		}  else if (result == 1) {
+			if (coex_dm->cur_ps_tdma == 8) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 7);
+				coex_dm->tdma_adj_type = 7;
+			} else if (coex_dm->cur_ps_tdma == 7) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 7);
+				coex_dm->tdma_adj_type = 7;
+			} else if (coex_dm->cur_ps_tdma == 6) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 7);
+				coex_dm->tdma_adj_type = 7;
+			} else if (coex_dm->cur_ps_tdma == 16) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 15);
+				coex_dm->tdma_adj_type = 15;
+			} else if (coex_dm->cur_ps_tdma == 15) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 15);
+				coex_dm->tdma_adj_type = 15;
+			} else if (coex_dm->cur_ps_tdma == 14) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 15);
+				coex_dm->tdma_adj_type = 15;
+			}
+		}
+	} else {
+		BTC_PRINT(BTC_MSG_ALGORITHM,
+			  ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], TxPause = 0\n");
+		switch (coex_dm->cur_ps_tdma) {
+		case 5:
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 3);
+			coex_dm->tdma_adj_type = 3;
+			break;
+		case 6:
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 3);
+			coex_dm->tdma_adj_type = 3;
+			break;
+		case 7:
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 3);
+			coex_dm->tdma_adj_type = 3;
+			break;
+		case 8:
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 4);
+			coex_dm->tdma_adj_type = 4;
+			break;
+		case 13:
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 11);
+			coex_dm->tdma_adj_type = 11;
+			break;
+		case 14:
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 11);
+			coex_dm->tdma_adj_type = 11;
+			break;
+		case 15:
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 11);
+			coex_dm->tdma_adj_type = 11;
+			break;
+		case 16:
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 12);
+			coex_dm->tdma_adj_type = 12;
+			break;
+		}
+		if (result == -1) {
+			switch (coex_dm->cur_ps_tdma) {
+			case 1:
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 3);
+				coex_dm->tdma_adj_type = 3;
+				break;
+			case 2:
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 3);
+				coex_dm->tdma_adj_type = 3;
+				break;
+			case 3:
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 4);
+				coex_dm->tdma_adj_type = 4;
+				break;
+			case 9:
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 11);
+				coex_dm->tdma_adj_type = 11;
+				break;
+			case 10:
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 11);
+				coex_dm->tdma_adj_type = 11;
+				break;
+			case 11:
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 12);
+				coex_dm->tdma_adj_type = 12;
+				break;
+			}
+		} else if (result == 1) {
+			switch (coex_dm->cur_ps_tdma) {
+			case 4:
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 3);
+				coex_dm->tdma_adj_type = 3;
+				break;
+			case 3:
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 3);
+				coex_dm->tdma_adj_type = 3;
+				break;
+			case 2:
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 3);
+				coex_dm->tdma_adj_type = 3;
+				break;
+			case 12:
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 11);
+				coex_dm->tdma_adj_type = 11;
+				break;
+			case 11:
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 11);
+				coex_dm->tdma_adj_type = 11;
+				break;
+			case 10:
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 11);
+				coex_dm->tdma_adj_type = 11;
+			}
+		}
+	}
+}
+
+static void btc8723b2ant_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 retry_count = 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) {
+					btc8723b2ant_ps_tdma(btcoexist,
+							     NORMAL_EXEC,
+							     true, 13);
+					coex_dm->tdma_adj_type = 13;
+				} else if (max_interval == 2) {
+					btc8723b2ant_ps_tdma(btcoexist,
+							     NORMAL_EXEC,
+							     true, 14);
+					coex_dm->tdma_adj_type = 14;
+				} else if (max_interval == 3) {
+					btc8723b2ant_ps_tdma(btcoexist,
+							     NORMAL_EXEC,
+							     true, 15);
+					coex_dm->tdma_adj_type = 15;
+				} else {
+					btc8723b2ant_ps_tdma(btcoexist,
+							     NORMAL_EXEC,
+							     true, 15);
+					coex_dm->tdma_adj_type = 15;
+				}
+			} else {
+				if (max_interval == 1) {
+					btc8723b2ant_ps_tdma(btcoexist,
+							     NORMAL_EXEC,
+							     true, 9);
+					coex_dm->tdma_adj_type = 9;
+				} else if (max_interval == 2) {
+					btc8723b2ant_ps_tdma(btcoexist,
+							     NORMAL_EXEC,
+							     true, 10);
+					coex_dm->tdma_adj_type = 10;
+				} else if (max_interval == 3) {
+					btc8723b2ant_ps_tdma(btcoexist,
+							     NORMAL_EXEC,
+							     true, 11);
+					coex_dm->tdma_adj_type = 11;
+				} else {
+					btc8723b2ant_ps_tdma(btcoexist,
+							     NORMAL_EXEC,
+							     true, 11);
+					coex_dm->tdma_adj_type = 11;
+				}
+			}
+		} else {
+			if (tx_pause) {
+				if (max_interval == 1) {
+					btc8723b2ant_ps_tdma(btcoexist,
+							     NORMAL_EXEC,
+							     true, 5);
+					coex_dm->tdma_adj_type = 5;
+				} else if (max_interval == 2) {
+					btc8723b2ant_ps_tdma(btcoexist,
+							     NORMAL_EXEC,
+							     true, 6);
+					coex_dm->tdma_adj_type = 6;
+				} else if (max_interval == 3) {
+					btc8723b2ant_ps_tdma(btcoexist,
+							     NORMAL_EXEC,
+							     true, 7);
+					coex_dm->tdma_adj_type = 7;
+				} else {
+					btc8723b2ant_ps_tdma(btcoexist,
+							     NORMAL_EXEC,
+							     true, 7);
+					coex_dm->tdma_adj_type = 7;
+				}
+			} else {
+				if (max_interval == 1) {
+					btc8723b2ant_ps_tdma(btcoexist,
+							     NORMAL_EXEC,
+							     true, 1);
+					coex_dm->tdma_adj_type = 1;
+				} else if (max_interval == 2) {
+					btc8723b2ant_ps_tdma(btcoexist,
+							     NORMAL_EXEC,
+							     true, 2);
+					coex_dm->tdma_adj_type = 2;
+				} else if (max_interval == 3) {
+					btc8723b2ant_ps_tdma(btcoexist,
+							     NORMAL_EXEC,
+							     true, 3);
+					coex_dm->tdma_adj_type = 3;
+				} else {
+					btc8723b2ant_ps_tdma(btcoexist,
+							     NORMAL_EXEC,
+							     true, 3);
+					coex_dm->tdma_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",
+			  up, dn, m, n, wait_count);
+		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");
+			} /* <=3 retry in the last 2-second duration*/
+		} 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 retry_counter<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 retry_counter>3!!\n");
+		}
+
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], max Interval = %d\n", max_interval);
+		if (max_interval == 1)
+			set_tdma_int1(btcoexist, tx_pause, result);
+		else if (max_interval == 2)
+			set_tdma_int2(btcoexist, tx_pause, result);
+		else if (max_interval == 3)
+			set_tdma_int3(btcoexist, tx_pause, result);
+	}
+
+	/*if current PsTdma not match with the recorded one (when scan, dhcp..),
+	 *then we have to adjust it back to the previous recorded one.
+	 */
+	if (coex_dm->cur_ps_tdma != coex_dm->tdma_adj_type) {
+		bool scan = false, link = false, roam = false;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], PsTdma type dismatch!!!, "
+			  "curPsTdma=%d, recordPsTdma=%d\n",
+			  coex_dm->cur_ps_tdma, coex_dm->tdma_adj_type);
+
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
+
+		if (!scan && !link && !roam)
+			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true,
+					     coex_dm->tdma_adj_type);
+		else
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+				  "[BTCoex], roaming/link/scan is under"
+				  " progress, will adjust next time!!!\n");
+	}
+}
+
+/* SCO only or SCO+PAN(HS) */
+static void btc8723b2ant_action_sco(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = btc8723b2ant_wifi_rssi_state(btcoexist,
+						       0, 2, 15, 0);
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	btc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 4);
+
+	if (btc8723b_need_dec_pwr(btcoexist))
+		btc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		btc8723b2ant_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)
+		btc8723b_coex_tbl_type(btcoexist, NORMAL_EXEC, 2);
+	else  /*for SCO quality & wifi performance balance at 11n mode*/
+		btc8723b_coex_tbl_type(btcoexist, NORMAL_EXEC, 8);
+
+	/*for voice quality */
+	btc8723b2ant_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)) {
+			btc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						   true, 0x4);
+		} else {
+			btc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						   true, 0x4);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			btc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						   true, 0x4);
+		} else {
+			btc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						   true, 0x4);
+		}
+	}
+}
+
+static void btc8723b2ant_action_hid(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = btc8723b2ant_wifi_rssi_state(btcoexist,
+						       0, 2, 15, 0);
+	bt_rssi_state = btc8723b2ant_bt_rssi_state(2, 35, 0);
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	btc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (btc8723b_need_dec_pwr(btcoexist))
+		btc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		btc8723b2ant_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*/
+		btc8723b_coex_tbl_type(btcoexist, NORMAL_EXEC, 7);
+	else  /*for HID quality & wifi performance balance at 11n mode*/
+		btc8723b_coex_tbl_type(btcoexist, NORMAL_EXEC, 9);
+
+	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+		btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
+	else
+		btc8723b2ant_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)) {
+			btc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						   false, 0x18);
+		} else {
+			btc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						   false, 0x18);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			btc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						   false, 0x18);
+		} else {
+			btc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						   false, 0x18);
+		}
+	}
+}
+
+/*A2DP only / PAN(EDR) only/ A2DP+PAN(HS)*/
+static void btc8723b2ant_action_a2dp(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = btc8723b2ant_wifi_rssi_state(btcoexist,
+						       0, 2, 15, 0);
+	bt_rssi_state = btc8723b2ant_bt_rssi_state(2, 35, 0);
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	btc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (btc8723b_need_dec_pwr(btcoexist))
+		btc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		btc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btc8723b_coex_tbl_type(btcoexist, NORMAL_EXEC, 7);
+
+	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+		btc8723b2ant_tdma_duration_adjust(btcoexist, false,
+						  false, 1);
+	else
+		btc8723b2ant_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)) {
+			btc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						   false, 0x18);
+		} else {
+			btc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						   false, 0x18);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			btc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						   false, 0x18);
+		} else {
+			btc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						   false, 0x18);
+		}
+	}
+}
+
+static void btc8723b2ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = btc8723b2ant_wifi_rssi_state(btcoexist,
+						       0, 2, 15, 0);
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	btc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (btc8723b_need_dec_pwr(btcoexist))
+		btc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		btc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btc8723b_coex_tbl_type(btcoexist, NORMAL_EXEC, 7);
+
+	btc8723b2ant_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)) {
+			btc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						   false, 0x18);
+		} else {
+			btc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						   false, 0x18);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			btc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						   false, 0x18);
+		} else {
+			btc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						   false, 0x18);
+		}
+	}
+}
+
+static void btc8723b2ant_action_pan_edr(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = btc8723b2ant_wifi_rssi_state(btcoexist,
+						       0, 2, 15, 0);
+	bt_rssi_state = btc8723b2ant_bt_rssi_state(2, 35, 0);
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	btc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (btc8723b_need_dec_pwr(btcoexist))
+		btc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		btc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btc8723b_coex_tbl_type(btcoexist, NORMAL_EXEC, 10);
+
+	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+		btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 1);
+	else
+		btc8723b2ant_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)) {
+			btc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						   false, 0x18);
+		} else {
+			btc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						   false, 0x18);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			btc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						   false, 0x18);
+		} else {
+			btc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						   false, 0x18);
+		}
+	}
+}
+
+/*PAN(HS) only*/
+static void btc8723b2ant_action_pan_hs(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = btc8723b2ant_wifi_rssi_state(btcoexist,
+						       0, 2, 15, 0);
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	btc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+	    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+		btc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		btc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btc8723b_coex_tbl_type(btcoexist, NORMAL_EXEC, 7);
+
+	btc8723b2ant_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)) {
+			btc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						   false, 0x18);
+		} else {
+			btc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						   false, 0x18);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			btc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						   false, 0x18);
+		} else {
+			btc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						   false, 0x18);
+		}
+	}
+}
+
+/*PAN(EDR)+A2DP*/
+static void btc8723b2ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = btc8723b2ant_wifi_rssi_state(btcoexist,
+						       0, 2, 15, 0);
+	bt_rssi_state = btc8723b2ant_bt_rssi_state(2, 35, 0);
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	btc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (btc8723b_need_dec_pwr(btcoexist))
+		btc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		btc8723b2ant_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)) {
+		btc8723b_coex_tbl_type(btcoexist, NORMAL_EXEC, 12);
+		if (BTC_WIFI_BW_HT40 == wifi_bw)
+			btc8723b2ant_tdma_duration_adjust(btcoexist, false,
+							  true, 3);
+		else
+			btc8723b2ant_tdma_duration_adjust(btcoexist, false,
+							  false, 3);
+	} else {
+		btc8723b_coex_tbl_type(btcoexist, NORMAL_EXEC, 7);
+		btc8723b2ant_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)) {
+			btc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						   false, 0x18);
+		} else {
+			btc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						   false, 0x18);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			btc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						   false, 0x18);
+		} else {
+			btc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						   false, 0x18);
+		}
+	}
+}
+
+static void btc8723b2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = btc8723b2ant_wifi_rssi_state(btcoexist,
+						       0, 2, 15, 0);
+	bt_rssi_state = btc8723b2ant_bt_rssi_state(2, 35, 0);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if (btc8723b_need_dec_pwr(btcoexist))
+		btc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		btc8723b2ant_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) {
+			btc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC,
+						      3);
+			btc8723b_coex_tbl_type(btcoexist, NORMAL_EXEC, 11);
+			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1,
+						  0xfffff, 0x780);
+		} else {
+			btc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC,
+						      6);
+			btc8723b_coex_tbl_type(btcoexist, NORMAL_EXEC, 7);
+			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1,
+						  0xfffff, 0x0);
+		}
+		btc8723b2ant_tdma_duration_adjust(btcoexist, true, false, 2);
+	} else {
+		btc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+		btc8723b_coex_tbl_type(btcoexist, NORMAL_EXEC, 11);
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff,
+					  0x0);
+		btc8723b2ant_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)) {
+			btc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						   false, 0x18);
+		} else {
+			btc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						   false, 0x18);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			btc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						   false, 0x18);
+		} else {
+			btc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						   false, 0x18);
+		}
+	}
+}
+
+/* HID+A2DP+PAN(EDR) */
+static void btc8723b2ant_action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = btc8723b2ant_wifi_rssi_state(btcoexist,
+						       0, 2, 15, 0);
+	bt_rssi_state = btc8723b2ant_bt_rssi_state(2, 35, 0);
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	btc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (btc8723b_need_dec_pwr(btcoexist))
+		btc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		btc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	btc8723b_coex_tbl_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)
+			btc8723b2ant_tdma_duration_adjust(btcoexist, true,
+							  true, 2);
+		else
+			btc8723b2ant_tdma_duration_adjust(btcoexist, true,
+							  false, 3);
+	} else {
+		btc8723b2ant_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)) {
+			btc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						   false, 0x18);
+		} else {
+			btc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						   false, 0x18);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			btc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						   false, 0x18);
+		} else {
+			btc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						   false, 0x18);
+		}
+	}
+}
+
+static void btc8723b2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = btc8723b2ant_wifi_rssi_state(btcoexist,
+							  0, 2, 15, 0);
+	bt_rssi_state = btc8723b2ant_bt_rssi_state(2, 35, 0);
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	btc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (btc8723b_need_dec_pwr(btcoexist))
+		btc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		btc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	btc8723b_coex_tbl_type(btcoexist, NORMAL_EXEC, 7);
+
+	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+		btc8723b2ant_tdma_duration_adjust(btcoexist, true, false, 2);
+	else
+		btc8723b2ant_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)) {
+			btc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						   false, 0x18);
+		} else {
+			btc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						   false, 0x18);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			btc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						   false, 0x18);
+		} else {
+			btc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						   false, 0x18);
+		}
+	}
+}
+
+static void btc8723b2ant_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 = btc8723b2ant_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");
+		btc8723b2ant_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 (btc8723b2ant_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");
+			btc8723b2ant_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");
+			btc8723b2ant_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");
+			btc8723b2ant_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");
+			btc8723b2ant_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");
+			btc8723b2ant_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");
+			btc8723b2ant_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");
+			btc8723b2ant_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");
+			btc8723b2ant_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");
+			btc8723b2ant_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");
+			btc8723b2ant_action_hid_a2dp(btcoexist);
+			break;
+		default:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, "
+				  "algorithm = coexist All Off!!\n");
+			btc8723b2ant_coex_alloff(btcoexist);
+			break;
+		}
+		coex_dm->pre_algorithm = coex_dm->cur_algorithm;
+	}
+}
+
+
+
+/*********************************************************************
+ *  work around function start with wa_btc8723b2ant_
+ *********************************************************************/
+/*********************************************************************
+ *  extern function start with EXbtc8723b2ant_
+ *********************************************************************/
+void ex_halbtc8723b2ant_init_hwconfig(struct btc_coexist *btcoexist)
+{
+	struct btc_board_info *board_info = &btcoexist->board_info;
+	u32 u32tmp = 0, fw_ver;
+	u8 u8tmp = 0;
+	u8 h2c_parameter[2] = {0};
+
+
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+		  "[BTCoex], 2Ant Init HW Config!!\n");
+
+	/* backup rf 0x1e value */
+	coex_dm->bt_rf0x1e_backup = btcoexist->btc_get_rf_reg(btcoexist,
+							      BTC_RF_A, 0x1e,
+							      0xfffff);
+
+	/* 0x4c[23]=0, 0x4c[24]=1  Antenna control by WL/BT */
+	u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
+	u32tmp &= ~BIT23;
+	u32tmp |= BIT24;
+	btcoexist->btc_write_4byte(btcoexist, 0x4c, u32tmp);
+
+	btcoexist->btc_write_1byte(btcoexist, 0x974, 0xff);
+	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x944, 0x3, 0x3);
+	btcoexist->btc_write_1byte(btcoexist, 0x930, 0x77);
+	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x67, 0x20, 0x1);
+
+	/* Antenna switch control parameter */
+	/* btcoexist->btc_write_4byte(btcoexist, 0x858, 0x55555555);*/
+
+	/*Force GNT_BT to low*/
+	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x765, 0x18, 0x0);
+	btcoexist->btc_write_2byte(btcoexist, 0x948, 0x0);
+
+	/* 0x790[5:0]=0x5 */
+	u8tmp = btcoexist->btc_read_1byte(btcoexist, 0x790);
+	u8tmp &= 0xc0;
+	u8tmp |= 0x5;
+	btcoexist->btc_write_1byte(btcoexist, 0x790, u8tmp);
+
+
+	/*Antenna config	*/
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
+
+ /*ext switch for fw ver < 0xc */
+	if (fw_ver < 0xc00) {
+		if (board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT) {
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x92c,
+							   0x3, 0x1);
+			/*Main Ant to  BT for IPS case 0x4c[23]=1*/
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x64, 0x1,
+							   0x1);
+
+			/*tell firmware "no antenna inverse"*/
+			h2c_parameter[0] = 0;
+			h2c_parameter[1] = 1;  /* ext switch type */
+			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
+						h2c_parameter);
+		} else {
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x92c,
+							   0x3, 0x2);
+			/*Aux Ant to  BT for IPS case 0x4c[23]=1*/
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x64, 0x1,
+							   0x0);
+
+			/*tell firmware "antenna inverse"*/
+			h2c_parameter[0] = 1;
+			h2c_parameter[1] = 1;  /*ext switch type*/
+			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
+						h2c_parameter);
+		}
+	} else {
+		/*ext switch always at s1 (if exist) */
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x92c, 0x3, 0x1);
+		/*Main Ant to  BT for IPS case 0x4c[23]=1*/
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x64, 0x1, 0x1);
+
+		if (board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT) {
+			/*tell firmware "no antenna inverse"*/
+			h2c_parameter[0] = 0;
+			h2c_parameter[1] = 0;  /*ext switch type*/
+			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
+						h2c_parameter);
+		} else {
+			/*tell firmware "antenna inverse"*/
+			h2c_parameter[0] = 1;
+			h2c_parameter[1] = 0;  /*ext switch type*/
+			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
+						h2c_parameter);
+		}
+	}
+
+	/* PTA parameter */
+	btc8723b_coex_tbl_type(btcoexist, FORCE_EXEC, 0);
+
+	/* Enable counter statistics */
+	/*0x76e[3] =1, WLAN_Act control by PTA*/
+	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
+	btcoexist->btc_write_1byte(btcoexist, 0x778, 0x3);
+	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x40, 0x20, 0x1);
+}
+
+void ex_halbtc8723b2ant_init_coex_dm(struct btc_coexist *btcoexist)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+		  "[BTCoex], Coex Mechanism Init!!\n");
+	btc8723b2ant_init_coex_dm(btcoexist);
+}
+
+void ex_halbtc8723b2ant_display_coex_info(struct btc_coexist *btcoexist)
+{
+	struct btc_board_info *board_info = &btcoexist->board_info;
+	struct btc_stack_info *stack_info = &btcoexist->stack_info;
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	u8 *cli_buf = btcoexist->cli_buf;
+	u8 u8tmp[4], i, bt_info_ext, ps_tdma_case = 0;
+	u32 u32tmp[4];
+	bool roam = false, scan = false;
+	bool link = false, wifi_under_5g = false;
+	bool bt_hs_on = false, wifi_busy = false;
+	s32 wifi_rssi = 0, bt_hs_rssi = 0;
+	u32 wifi_bw, wifi_traffic_dir, fa_ofdm, fa_cck;
+	u8 wifi_dot11_chnl, wifi_hs_chnl;
+	u32 fw_ver = 0, bt_patch_ver = 0;
+
+	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/ FwVer/ PatchVer",
+		   glcoex_ver_date_8723b_2ant, glcoex_ver_8723b_2ant,
+		   fw_ver, bt_patch_ver, bt_patch_ver);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_DOT11_CHNL,
+			   &wifi_dot11_chnl);
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifi_hs_chnl);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d(%d)",
+		   "Dot11 channel / HsChnl(HsMode)",
+		   wifi_dot11_chnl, wifi_hs_chnl, bt_hs_on);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x ",
+		   "H2C Wifi inform bt chnl Info", coex_dm->wifi_chnl_info[0],
+		   coex_dm->wifi_chnl_info[1], coex_dm->wifi_chnl_info[2]);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
+	btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
+		   "Wifi rssi/ HS rssi", wifi_rssi, bt_hs_rssi);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ",
+		   "Wifi link/ roam/ scan", link, roam, scan);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under_5g);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION,
+			   &wifi_traffic_dir);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %s/ %s ",
+		   "Wifi status", (wifi_under_5g ? "5G" : "2.4G"),
+		   ((BTC_WIFI_BW_LEGACY == wifi_bw) ? "Legacy" :
+		   (((BTC_WIFI_BW_HT40 == wifi_bw) ? "HT40" : "HT20"))),
+		   ((!wifi_busy) ? "idle" :
+		   ((BTC_WIFI_TRAFFIC_TX == wifi_traffic_dir) ?
+		   "uplink" : "downlink")));
+	CL_PRINTF(cli_buf);
+
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d / %d / %d",
+		   "SCO/HID/PAN/A2DP",
+		   bt_link_info->sco_exist, bt_link_info->hid_exist,
+		   bt_link_info->pan_exist, bt_link_info->a2dp_exist);
+	CL_PRINTF(cli_buf);
+	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_BT_LINK_INFO);
+
+	bt_info_ext = coex_sta->bt_info_ext;
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s",
+		   "BT Info A2DP rate",
+		   (bt_info_ext&BIT0) ? "Basic rate" : "EDR rate");
+	CL_PRINTF(cli_buf);
+
+	for (i = 0; i < BT_INFO_SRC_8723B_2ANT_MAX; i++) {
+		if (coex_sta->bt_info_c2h_cnt[i]) {
+			CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+				   "\r\n %-35s = %02x %02x %02x "
+				   "%02x %02x %02x %02x(%d)",
+				   glbt_info_src_8723b_2ant[i],
+				   coex_sta->bt_info_c2h[i][0],
+				   coex_sta->bt_info_c2h[i][1],
+				   coex_sta->bt_info_c2h[i][2],
+				   coex_sta->bt_info_c2h[i][3],
+				   coex_sta->bt_info_c2h[i][4],
+				   coex_sta->bt_info_c2h[i][5],
+				   coex_sta->bt_info_c2h[i][6],
+				   coex_sta->bt_info_c2h_cnt[i]);
+			CL_PRINTF(cli_buf);
+		}
+	}
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s/%s",
+		   "PS state, IPS/LPS",
+		   ((coex_sta->under_ips ? "IPS ON" : "IPS OFF")),
+		   ((coex_sta->under_lps ? "LPS ON" : "LPS OFF")));
+	CL_PRINTF(cli_buf);
+	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_FW_PWR_MODE_CMD);
+
+	/* Sw mechanism	*/
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n %-35s", "============[Sw mechanism]============");
+	CL_PRINTF(cli_buf);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ",
+		   "SM1[ShRf/ LpRA/ LimDig]", coex_dm->cur_rf_rx_lpf_shrink,
+		   coex_dm->cur_low_penalty_ra, coex_dm->limited_dig);
+	CL_PRINTF(cli_buf);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d(0x%x) ",
+		   "SM2[AgcT/ AdcB/ SwDacSwing(lvl)]",
+		   coex_dm->cur_agc_table_en, coex_dm->cur_adc_back_off,
+		   coex_dm->cur_dac_swing_on, coex_dm->cur_dac_swing_lvl);
+	CL_PRINTF(cli_buf);
+
+	/* Fw mechanism	*/
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
+		   "============[Fw mechanism]============");
+	CL_PRINTF(cli_buf);
+
+	ps_tdma_case = coex_dm->cur_ps_tdma;
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n %-35s = %02x %02x %02x %02x %02x case-%d (auto:%d)",
+		   "PS TDMA", coex_dm->ps_tdma_para[0],
+		   coex_dm->ps_tdma_para[1], coex_dm->ps_tdma_para[2],
+		   coex_dm->ps_tdma_para[3], coex_dm->ps_tdma_para[4],
+		   ps_tdma_case, coex_dm->auto_tdma_adjust);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ",
+		   "DecBtPwr/ IgnWlanAct", coex_dm->cur_dec_bt_pwr,
+		   coex_dm->cur_ignore_wlan_act);
+	CL_PRINTF(cli_buf);
+
+	/* Hw setting */
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
+		   "============[Hw setting]============");
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x",
+		   "RF-A, 0x1e initVal", coex_dm->bt_rf0x1e_backup);
+	CL_PRINTF(cli_buf);
+
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x778);
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x880);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
+		   "0x778/0x880[29:25]", u8tmp[0],
+		   (u32tmp[0]&0x3e000000) >> 25);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x948);
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x67);
+	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x765);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
+		   "0x948/ 0x67[5] / 0x765",
+		   u32tmp[0], ((u8tmp[0]&0x20) >> 5), u8tmp[1]);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x92c);
+	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x930);
+	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x944);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
+		   "0x92c[1:0]/ 0x930[7:0]/0x944[1:0]",
+		   u32tmp[0]&0x3, u32tmp[1]&0xff, u32tmp[2]&0x3);
+	CL_PRINTF(cli_buf);
+
+
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x39);
+	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x40);
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x4c);
+	u8tmp[2] = btcoexist->btc_read_1byte(btcoexist, 0x64);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x",
+		   "0x38[11]/0x40/0x4c[24:23]/0x64[0]",
+		   ((u8tmp[0] & 0x8)>>3), u8tmp[1],
+		   ((u32tmp[0]&0x01800000)>>23), u8tmp[2]&0x1);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x550);
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x522);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
+		   "0x550(bcn ctrl)/0x522", u32tmp[0], u8tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xc50);
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x49c);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
+		   "0xc50(dig)/0x49c(null-drop)", u32tmp[0]&0xff, u8tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xda0);
+	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0xda4);
+	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0xda8);
+	u32tmp[3] = btcoexist->btc_read_4byte(btcoexist, 0xcf0);
+
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0xa5b);
+	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0xa5c);
+
+	fa_ofdm = ((u32tmp[0]&0xffff0000) >> 16) +
+		  ((u32tmp[1]&0xffff0000) >> 16) +
+		   (u32tmp[1] & 0xffff) +
+		   (u32tmp[2] & 0xffff) +
+		  ((u32tmp[3]&0xffff0000) >> 16) +
+		   (u32tmp[3] & 0xffff);
+	fa_cck = (u8tmp[0] << 8) + u8tmp[1];
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
+		   "OFDM-CCA/OFDM-FA/CCK-FA",
+		   u32tmp[0]&0xffff, fa_ofdm, fa_cck);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x6c0);
+	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x6c4);
+	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x6c8);
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x6cc);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x",
+		   "0x6c0/0x6c4/0x6c8/0x6cc(coexTable)",
+		   u32tmp[0], u32tmp[1], u32tmp[2], u8tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
+		   "0x770(high-pri rx/tx)",
+		   coex_sta->high_priority_rx, coex_sta->high_priority_tx);
+	CL_PRINTF(cli_buf);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
+		   "0x774(low-pri rx/tx)", coex_sta->low_priority_rx,
+		   coex_sta->low_priority_tx);
+	CL_PRINTF(cli_buf);
+#if (BT_AUTO_REPORT_ONLY_8723B_2ANT == 1)
+	btc8723b2ant_monitor_bt_ctr(btcoexist);
+#endif
+	btcoexist->btc_disp_dbg_msg(btcoexist,
+	BTC_DBG_DISP_COEX_STATISTICS);
+}
+
+
+void ex_halbtc8723b2ant_ips_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	if (BTC_IPS_ENTER == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], IPS ENTER notify\n");
+		coex_sta->under_ips = true;
+		btc8723b2ant_coex_alloff(btcoexist);
+	} else if (BTC_IPS_LEAVE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], IPS LEAVE notify\n");
+		coex_sta->under_ips = false;
+	}
+}
+
+void ex_halbtc8723b2ant_lps_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	if (BTC_LPS_ENABLE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], LPS ENABLE notify\n");
+		coex_sta->under_lps = true;
+	} else if (BTC_LPS_DISABLE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], LPS DISABLE notify\n");
+		coex_sta->under_lps = false;
+	}
+}
+
+void ex_halbtc8723b2ant_scan_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	if (BTC_SCAN_START == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], SCAN START notify\n");
+	else if (BTC_SCAN_FINISH == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], SCAN FINISH notify\n");
+}
+
+void ex_halbtc8723b2ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	if (BTC_ASSOCIATE_START == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], CONNECT START notify\n");
+	else if (BTC_ASSOCIATE_FINISH == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], CONNECT FINISH notify\n");
+}
+
+void btc8723b_med_stat_notify(struct btc_coexist *btcoexist,
+					    u8 type)
+{
+	u8 h2c_parameter[3] = {0};
+	u32 wifi_bw;
+	u8 wifi_central_chnl;
+
+	if (BTC_MEDIA_CONNECT == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], MEDIA connect notify\n");
+	else
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], MEDIA disconnect notify\n");
+
+	/* only 2.4G we need to inform bt the chnl mask */
+	btcoexist->btc_get(btcoexist,
+		BTC_GET_U1_WIFI_CENTRAL_CHNL, &wifi_central_chnl);
+	if ((BTC_MEDIA_CONNECT == type) &&
+	    (wifi_central_chnl <= 14)) {
+		h2c_parameter[0] = 0x1;
+		h2c_parameter[1] = wifi_central_chnl;
+		btcoexist->btc_get(btcoexist,
+			BTC_GET_U4_WIFI_BW, &wifi_bw);
+		if (BTC_WIFI_BW_HT40 == wifi_bw)
+			h2c_parameter[2] = 0x30;
+		else
+			h2c_parameter[2] = 0x20;
+	}
+
+	coex_dm->wifi_chnl_info[0] = h2c_parameter[0];
+	coex_dm->wifi_chnl_info[1] = h2c_parameter[1];
+	coex_dm->wifi_chnl_info[2] = h2c_parameter[2];
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], FW write 0x66=0x%x\n",
+		  h2c_parameter[0] << 16 | h2c_parameter[1] << 8 |
+		  h2c_parameter[2]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x66, 3, h2c_parameter);
+}
+
+void ex_halbtc8723b2ant_special_packet_notify(struct btc_coexist *btcoexist,
+					      u8 type)
+{
+	if (type == BTC_PACKET_DHCP)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], DHCP Packet notify\n");
+}
+
+void ex_halbtc8723b2ant_bt_info_notify(struct btc_coexist *btcoexist,
+				       u8 *tmpbuf, u8 length)
+{
+	u8 bt_info = 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)
+			bt_info = tmpbuf[i];
+		if (i == length-1)
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+				  "0x%02x]\n", tmpbuf[i]);
+		else
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+				  "0x%02x, ", tmpbuf[i]);
+	}
+
+	if (btcoexist->manual_control) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), "
+			  "return for Manual CTRL<===\n");
+		return;
+	}
+
+	if (BT_INFO_SRC_8723B_2ANT_WIFI_FW != rsp_source) {
+		coex_sta->bt_retry_cnt =	/* [3:0]*/
+			coex_sta->bt_info_c2h[rsp_source][2] & 0xf;
+
+		coex_sta->bt_rssi =
+			coex_sta->bt_info_c2h[rsp_source][3] * 2 + 10;
+
+		coex_sta->bt_info_ext =
+			coex_sta->bt_info_c2h[rsp_source][4];
+
+		/* Here we need to resend some wifi info to BT
+		 * because bt is reset and loss of the info.
+		 */
+		if ((coex_sta->bt_info_ext & BIT1)) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], BT ext info bit1 check,"
+				  " send wifi BW&Chnl to BT!!\n");
+			btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+					   &wifi_connected);
+			if (wifi_connected)
+				btc8723b_med_stat_notify(btcoexist,
+							 BTC_MEDIA_CONNECT);
+			else
+				btc8723b_med_stat_notify(btcoexist,
+							 BTC_MEDIA_DISCONNECT);
+		}
+
+		if ((coex_sta->bt_info_ext & BIT3)) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], BT ext info bit3 check, "
+				  "set BT NOT to ignore Wlan active!!\n");
+			btc8723b2ant_ignore_wlan_act(btcoexist, FORCE_EXEC,
+						     false);
+		} else {
+			/* BT already NOT ignore Wlan active, do nothing here.*/
+		}
+#if (BT_AUTO_REPORT_ONLY_8723B_2ANT == 0)
+		if ((coex_sta->bt_info_ext & BIT4)) {
+			/* BT auto report already enabled, do nothing*/
+		} else {
+			btc8723b2ant_bt_auto_report(btcoexist, FORCE_EXEC,
+						    true);
+		}
+#endif
+	}
+
+	/* check BIT2 first ==> check if bt is under inquiry or page scan*/
+	if (bt_info & BT_INFO_8723B_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_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 (bt_info & BT_INFO_8723B_2ANT_B_FTP)
+			coex_sta->pan_exist = true;
+		else
+			coex_sta->pan_exist = false;
+		if (bt_info & BT_INFO_8723B_2ANT_B_A2DP)
+			coex_sta->a2dp_exist = true;
+		else
+			coex_sta->a2dp_exist = false;
+		if (bt_info & BT_INFO_8723B_2ANT_B_HID)
+			coex_sta->hid_exist = true;
+		else
+			coex_sta->hid_exist = false;
+		if (bt_info & BT_INFO_8723B_2ANT_B_SCO_ESCO)
+			coex_sta->sco_exist = true;
+		else
+			coex_sta->sco_exist = false;
+	}
+
+	btc8723b2ant_update_bt_link_info(btcoexist);
+
+	if (!(bt_info & 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 (bt_info == 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 ((bt_info & BT_INFO_8723B_2ANT_B_SCO_ESCO) ||
+		   (bt_info & 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 (bt_info & BT_INFO_8723B_2ANT_B_ACL_BUSY) {
+		coex_dm->bt_status = BT_8723B_2ANT_BT_STATUS_ACL_BUSY;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), BT ACL busy!!!\n");
+	} 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);
+
+	btc8723b2ant_run_coexist_mechanism(btcoexist);
+}
+
+void ex_halbtc8723b2ant_stack_operation_notify(struct btc_coexist *btcoexist,
+					       u8 type)
+{
+	if (BTC_STACK_OP_INQ_PAGE_PAIR_START == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex],StackOP Inquiry/page/pair start notify\n");
+	else if (BTC_STACK_OP_INQ_PAGE_PAIR_FINISH == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex],StackOP Inquiry/page/pair finish notify\n");
+}
+
+void ex_halbtc8723b2ant_halt_notify(struct btc_coexist *btcoexist)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, "[BTCoex], Halt notify\n");
+
+	btc8723b2ant_ignore_wlan_act(btcoexist, FORCE_EXEC, true);
+	btc8723b_med_stat_notify(btcoexist, BTC_MEDIA_DISCONNECT);
+}
+
+void ex_halbtc8723b2ant_periodical(struct btc_coexist *btcoexist)
+{
+	struct btc_board_info *board_info = &btcoexist->board_info;
+	struct btc_stack_info *stack_info = &btcoexist->stack_info;
+	static u8 dis_ver_info_cnt;
+	u32 fw_ver = 0, bt_patch_ver = 0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+		  "[BTCoex], =========================="
+		  "Periodical===========================\n");
+
+	if (dis_ver_info_cnt <= 5) {
+		dis_ver_info_cnt += 1;
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], ****************************"
+			  "************************************\n");
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], Ant PG Num/ Ant Mech/ "
+			  "Ant Pos = %d/ %d/ %d\n", board_info->pg_ant_num,
+			  board_info->btdm_ant_num, board_info->btdm_ant_pos);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], BT stack/ hci ext ver = %s / %d\n",
+			  ((stack_info->profile_notified) ? "Yes" : "No"),
+			  stack_info->hci_version);
+		btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER,
+				   &bt_patch_ver);
+		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], CoexVer/ FwVer/ PatchVer = "
+			  "%d_%x/ 0x%x/ 0x%x(%d)\n",
+			  glcoex_ver_date_8723b_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)
+	btc8723b2ant_query_bt_info(btcoexist);
+	btc8723b2ant_monitor_bt_ctr(btcoexist);
+	btc8723b2ant_monitor_bt_enable_disable(btcoexist);
+#else
+	if (btc8723b2ant_is_wifi_status_changed(btcoexist) ||
+	    coex_dm->auto_tdma_adjust)
+		btc8723b2ant_run_coexist_mechanism(btcoexist);
+#endif
+}
diff --git a/drivers/net/wireless/rtlwifi/btcoexist/halbtc8723b2ant.h b/drivers/net/wireless/rtlwifi/btcoexist/halbtc8723b2ant.h
new file mode 100644
index 000000000000..e0ad8e545f82
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/btcoexist/halbtc8723b2ant.h
@@ -0,0 +1,173 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2012  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+#ifndef _HAL8723B_2_ANT
+#define _HAL8723B_2_ANT
+
+/************************************************************************
+ * The following is for 8723B 2Ant BT Co-exist definition
+ ************************************************************************/
+#define	BT_AUTO_REPORT_ONLY_8723B_2ANT			1
+
+#define	BT_INFO_8723B_2ANT_B_FTP			BIT7
+#define	BT_INFO_8723B_2ANT_B_A2DP			BIT6
+#define	BT_INFO_8723B_2ANT_B_HID			BIT5
+#define	BT_INFO_8723B_2ANT_B_SCO_BUSY			BIT4
+#define	BT_INFO_8723B_2ANT_B_ACL_BUSY			BIT3
+#define	BT_INFO_8723B_2ANT_B_INQ_PAGE			BIT2
+#define	BT_INFO_8723B_2ANT_B_SCO_ESCO			BIT1
+#define	BT_INFO_8723B_2ANT_B_CONNECTION			BIT0
+
+#define BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT		2
+
+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 tdma_adj_type;
+	bool reset_tdma_adjust;
+	bool auto_tdma_adjust;
+	bool pre_ps_tdma_on;
+	bool cur_ps_tdma_on;
+	bool pre_bt_auto_report;
+	bool cur_bt_auto_report;
+
+	/* sw mechanism */
+	bool pre_rf_rx_lpf_shrink;
+	bool cur_rf_rx_lpf_shrink;
+	u32 bt_rf0x1e_backup;
+	bool pre_low_penalty_ra;
+	bool cur_low_penalty_ra;
+	bool pre_dac_swing_on;
+	u32 pre_dac_swing_lvl;
+	bool cur_dac_swing_on;
+	u32 cur_dac_swing_lvl;
+	bool pre_adc_back_off;
+	bool cur_adc_back_off;
+	bool pre_agc_table_en;
+	bool cur_agc_table_en;
+	u32 pre_val0x6c0;
+	u32 cur_val0x6c0;
+	u32 pre_val0x6c4;
+	u32 cur_val0x6c4;
+	u32 pre_val0x6c8;
+	u32 cur_val0x6c8;
+	u8 pre_val0x6cc;
+	u8 cur_val0x6cc;
+	bool limited_dig;
+
+	/* algorithm related */
+	u8 pre_algorithm;
+	u8 cur_algorithm;
+	u8 bt_status;
+	u8 wifi_chnl_info[3];
+
+	bool need_recover_0x948;
+	u16 backup_0x948;
+};
+
+struct coex_sta_8723b_2ant {
+	bool bt_link_exist;
+	bool sco_exist;
+	bool a2dp_exist;
+	bool hid_exist;
+	bool pan_exist;
+
+	bool under_lps;
+	bool under_ips;
+	u32 high_priority_tx;
+	u32 high_priority_rx;
+	u32 low_priority_tx;
+	u32 low_priority_rx;
+	u8 bt_rssi;
+	u8 pre_bt_rssi_state;
+	u8 pre_wifi_rssi_state[4];
+	bool c2h_bt_info_req_sent;
+	u8 bt_info_c2h[BT_INFO_SRC_8723B_2ANT_MAX][10];
+	u32 bt_info_c2h_cnt[BT_INFO_SRC_8723B_2ANT_MAX];
+	bool c2h_bt_inquiry_page;
+	u8 bt_retry_cnt;
+	u8 bt_info_ext;
+};
+
+/*********************************************************************
+ * The following is interface which will notify coex module.
+ *********************************************************************/
+void ex_halbtc8723b2ant_init_hwconfig(struct btc_coexist *btcoexist);
+void ex_halbtc8723b2ant_init_coex_dm(struct btc_coexist *btcoexist);
+void ex_halbtc8723b2ant_ips_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_halbtc8723b2ant_lps_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_halbtc8723b2ant_scan_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_halbtc8723b2ant_connect_notify(struct btc_coexist *btcoexist, u8 type);
+void btc8723b_med_stat_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_halbtc8723b2ant_special_packet_notify(struct btc_coexist *btcoexist,
+					      u8 type);
+void ex_halbtc8723b2ant_bt_info_notify(struct btc_coexist *btcoexist,
+				       u8 *tmpbuf, u8 length);
+void ex_halbtc8723b2ant_stack_operation_notify(struct btc_coexist *btcoexist,
+					       u8 type);
+void ex_halbtc8723b2ant_halt_notify(struct btc_coexist *btcoexist);
+void ex_halbtc8723b2ant_periodical(struct btc_coexist *btcoexist);
+void ex_halbtc8723b2ant_display_coex_info(struct btc_coexist *btcoexist);
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/btcoexist/halbtcoutsrc.c b/drivers/net/wireless/rtlwifi/btcoexist/halbtcoutsrc.c
new file mode 100644
index 000000000000..b6722de64a31
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/btcoexist/halbtcoutsrc.c
@@ -0,0 +1,1011 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ * 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 "halbt_precomp.h"
+
+/***********************************************
+ *		Global variables
+ ***********************************************/
+
+struct btc_coexist gl_bt_coexist;
+
+u32 btc_dbg_type[BTC_MSG_MAX];
+static u8 btc_dbg_buf[100];
+
+/***************************************************
+ *		Debug related function
+ ***************************************************/
+static bool halbtc_is_bt_coexist_available(struct btc_coexist *btcoexist)
+{
+	if (!btcoexist->binded || NULL == btcoexist->adapter)
+		return false;
+
+	return true;
+}
+
+static bool halbtc_is_wifi_busy(struct rtl_priv *rtlpriv)
+{
+	if (rtlpriv->link_info.busytraffic)
+		return true;
+	else
+		return false;
+}
+
+static void halbtc_dbg_init(void)
+{
+	u8 i;
+
+	for (i = 0; i < BTC_MSG_MAX; i++)
+		btc_dbg_type[i] = 0;
+
+	btc_dbg_type[BTC_MSG_INTERFACE] =
+/*			INTF_INIT				| */
+/*			INTF_NOTIFY				| */
+			0;
+
+	btc_dbg_type[BTC_MSG_ALGORITHM] =
+/*			ALGO_BT_RSSI_STATE			| */
+/*			ALGO_WIFI_RSSI_STATE			| */
+/*			ALGO_BT_MONITOR				| */
+/*			ALGO_TRACE				| */
+/*			ALGO_TRACE_FW				| */
+/*			ALGO_TRACE_FW_DETAIL			| */
+/*			ALGO_TRACE_FW_EXEC			| */
+/*			ALGO_TRACE_SW				| */
+/*			ALGO_TRACE_SW_DETAIL			| */
+/*			ALGO_TRACE_SW_EXEC			| */
+			0;
+}
+
+static bool halbtc_is_bt40(struct rtl_priv *adapter)
+{
+	struct rtl_priv *rtlpriv = adapter;
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	bool is_ht40 = true;
+	enum ht_channel_width bw = rtlphy->current_chan_bw;
+
+	if (bw == HT_CHANNEL_WIDTH_20)
+		is_ht40 = false;
+	else if (bw == HT_CHANNEL_WIDTH_20_40)
+		is_ht40 = true;
+
+	return is_ht40;
+}
+
+static bool halbtc_legacy(struct rtl_priv *adapter)
+{
+	struct rtl_priv *rtlpriv = adapter;
+	struct rtl_mac *mac = rtl_mac(rtlpriv);
+
+	bool is_legacy = false;
+
+	if ((mac->mode == WIRELESS_MODE_B) || (mac->mode == WIRELESS_MODE_B))
+		is_legacy = true;
+
+	return is_legacy;
+}
+
+bool halbtc_is_wifi_uplink(struct rtl_priv *adapter)
+{
+	struct rtl_priv *rtlpriv = adapter;
+
+	if (rtlpriv->link_info.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,
+		  "static 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_info("halbtc_leave_lps()<--dont leave lps under AP mode\n");
+		return;
+	}
+
+	btcoexist->bt_info.bt_ctrl_lps = true;
+	btcoexist->bt_info.bt_lps_on = false;
+}
+
+static void halbtc_enter_lps(struct btc_coexist *btcoexist)
+{
+	struct rtl_priv *rtlpriv;
+	struct rtl_ps_ctl *ppsc;
+	bool ap_enable = false;
+
+	rtlpriv = btcoexist->adapter;
+	ppsc = rtl_psc(rtlpriv);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_AP_MODE_ENABLE,
+			   &ap_enable);
+
+	if (ap_enable) {
+		pr_info("halbtc_enter_lps()<--dont enter lps under AP mode\n");
+		return;
+	}
+
+	btcoexist->bt_info.bt_ctrl_lps = true;
+	btcoexist->bt_info.bt_lps_on = false;
+}
+
+static void halbtc_normal_lps(struct btc_coexist *btcoexist)
+{
+	if (btcoexist->bt_info.bt_ctrl_lps) {
+		btcoexist->bt_info.bt_lps_on = false;
+		btcoexist->bt_info.bt_ctrl_lps = false;
+	}
+}
+
+static void halbtc_leave_low_power(void)
+{
+}
+
+static void halbtc_nomal_low_power(void)
+{
+}
+
+static void halbtc_disable_low_power(void)
+{
+}
+
+static void halbtc_aggregation_check(void)
+{
+}
+
+static u32 halbtc_get_bt_patch_version(struct btc_coexist *btcoexist)
+{
+	return 0;
+}
+
+static s32 halbtc_get_wifi_rssi(struct rtl_priv *adapter)
+{
+	struct rtl_priv *rtlpriv = adapter;
+	s32	undec_sm_pwdb = 0;
+
+	if (rtlpriv->mac80211.link_state >= MAC80211_LINKED)
+		undec_sm_pwdb = rtlpriv->dm.undec_sm_pwdb;
+	else /* associated entry pwdb */
+		undec_sm_pwdb = rtlpriv->dm.undec_sm_pwdb;
+	return undec_sm_pwdb;
+}
+
+static bool halbtc_get(void *void_btcoexist, u8 get_type, void *out_buf)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)void_btcoexist;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_mac *mac = rtl_mac(rtlpriv);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	bool *bool_tmp = (bool *)out_buf;
+	int *s32_tmp = (int *)out_buf;
+	u32 *u32_tmp = (u32 *)out_buf;
+	u8 *u8_tmp = (u8 *)out_buf;
+	bool tmp = false;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return false;
+
+	switch (get_type) {
+	case BTC_GET_BL_HS_OPERATION:
+		*bool_tmp = false;
+		break;
+	case BTC_GET_BL_HS_CONNECTING:
+		*bool_tmp = false;
+		break;
+	case BTC_GET_BL_WIFI_CONNECTED:
+		if (rtlpriv->mac80211.link_state >= MAC80211_LINKED)
+			tmp = true;
+		*bool_tmp = tmp;
+		break;
+	case BTC_GET_BL_WIFI_BUSY:
+		if (halbtc_is_wifi_busy(rtlpriv))
+			*bool_tmp = true;
+		else
+			*bool_tmp = false;
+		break;
+	case BTC_GET_BL_WIFI_SCAN:
+		if (mac->act_scanning)
+			*bool_tmp = true;
+		else
+			*bool_tmp = false;
+		break;
+	case BTC_GET_BL_WIFI_LINK:
+		if (mac->link_state == MAC80211_LINKING)
+			*bool_tmp = true;
+		else
+			*bool_tmp = false;
+		break;
+	case BTC_GET_BL_WIFI_ROAM:	/*TODO*/
+		if (mac->link_state == MAC80211_LINKING)
+			*bool_tmp = true;
+		else
+			*bool_tmp = false;
+		break;
+	case BTC_GET_BL_WIFI_4_WAY_PROGRESS:	/*TODO*/
+			*bool_tmp = false;
+
+		break;
+	case BTC_GET_BL_WIFI_UNDER_5G:
+		*bool_tmp = false; /*TODO*/
+
+	case BTC_GET_BL_WIFI_DHCP:	/*TODO*/
+		break;
+	case BTC_GET_BL_WIFI_SOFTAP_IDLE:
+		*bool_tmp = true;
+		break;
+	case BTC_GET_BL_WIFI_SOFTAP_LINKING:
+		*bool_tmp = false;
+		break;
+	case BTC_GET_BL_WIFI_IN_EARLY_SUSPEND:
+		*bool_tmp = false;
+		break;
+	case BTC_GET_BL_WIFI_AP_MODE_ENABLE:
+		*bool_tmp = false;
+		break;
+	case BTC_GET_BL_WIFI_ENABLE_ENCRYPTION:
+		if (NO_ENCRYPTION == rtlpriv->sec.pairwise_enc_algorithm)
+			*bool_tmp = false;
+		else
+			*bool_tmp = true;
+		break;
+	case BTC_GET_BL_WIFI_UNDER_B_MODE:
+		*bool_tmp = false; /*TODO*/
+		break;
+	case BTC_GET_BL_EXT_SWITCH:
+		*bool_tmp = false;
+		break;
+	case BTC_GET_S4_WIFI_RSSI:
+		*s32_tmp = halbtc_get_wifi_rssi(rtlpriv);
+		break;
+	case BTC_GET_S4_HS_RSSI:	/*TODO*/
+		*s32_tmp = halbtc_get_wifi_rssi(rtlpriv);
+		break;
+	case BTC_GET_U4_WIFI_BW:
+		*u32_tmp = halbtc_get_wifi_bw(btcoexist);
+		break;
+	case BTC_GET_U4_WIFI_TRAFFIC_DIRECTION:
+		if (halbtc_is_wifi_uplink(rtlpriv))
+			*u32_tmp = BTC_WIFI_TRAFFIC_TX;
+		else
+			*u32_tmp = BTC_WIFI_TRAFFIC_RX;
+		break;
+	case BTC_GET_U4_WIFI_FW_VER:
+		*u32_tmp = rtlhal->fw_version;
+		break;
+	case BTC_GET_U4_BT_PATCH_VER:
+		*u32_tmp = halbtc_get_bt_patch_version(btcoexist);
+		break;
+	case BTC_GET_U1_WIFI_DOT11_CHNL:
+		*u8_tmp = rtlphy->current_channel;
+		break;
+	case BTC_GET_U1_WIFI_CENTRAL_CHNL:
+		*u8_tmp = halbtc_get_wifi_central_chnl(btcoexist);
+		break;
+	case BTC_GET_U1_WIFI_HS_CHNL:
+		*u8_tmp = 1;/*BT_OperateChnl(rtlpriv);*/
+		break;
+	case BTC_GET_U1_MAC_PHY_MODE:
+		*u8_tmp = BTC_MP_UNKNOWN;
+		break;
+
+		/************* 1Ant **************/
+	case BTC_GET_U1_LPS_MODE:
+		*u8_tmp = btcoexist->pwr_mode_val[0];
+		break;
+
+	default:
+		break;
+	}
+
+	return true;
+}
+
+static bool halbtc_set(void *void_btcoexist, u8 set_type, void *in_buf)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)void_btcoexist;
+	bool *bool_tmp = (bool *)in_buf;
+	u8 *u8_tmp = (u8 *)in_buf;
+	u32 *u32_tmp = (u32 *)in_buf;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return false;
+
+	switch (set_type) {
+	/* set some bool type variables. */
+	case BTC_SET_BL_BT_DISABLE:
+		btcoexist->bt_info.bt_disabled = *bool_tmp;
+		break;
+	case BTC_SET_BL_BT_TRAFFIC_BUSY:
+		btcoexist->bt_info.bt_busy = *bool_tmp;
+		break;
+	case BTC_SET_BL_BT_LIMITED_DIG:
+		btcoexist->bt_info.limited_dig = *bool_tmp;
+		break;
+	case BTC_SET_BL_FORCE_TO_ROAM:
+		btcoexist->bt_info.force_to_roam = *bool_tmp;
+		break;
+	case BTC_SET_BL_TO_REJ_AP_AGG_PKT:
+		btcoexist->bt_info.reject_agg_pkt = *bool_tmp;
+		break;
+	case BTC_SET_BL_BT_CTRL_AGG_SIZE:
+		btcoexist->bt_info.b_bt_ctrl_buf_size = *bool_tmp;
+		break;
+	case BTC_SET_BL_INC_SCAN_DEV_NUM:
+		btcoexist->bt_info.increase_scan_dev_num = *bool_tmp;
+		break;
+		/* set some u1Byte type variables. */
+	case BTC_SET_U1_RSSI_ADJ_VAL_FOR_AGC_TABLE_ON:
+		btcoexist->bt_info.rssi_adjust_for_agc_table_on = *u8_tmp;
+		break;
+	case BTC_SET_U1_AGG_BUF_SIZE:
+		btcoexist->bt_info.agg_buf_size = *u8_tmp;
+		break;
+		/* the following are some action which will be triggered */
+	case BTC_SET_ACT_GET_BT_RSSI:
+		/*BTHCI_SendGetBtRssiEvent(rtlpriv);*/
+		break;
+	case BTC_SET_ACT_AGGREGATE_CTRL:
+		halbtc_aggregation_check();
+		break;
+
+		/* 1Ant */
+	case BTC_SET_U1_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE:
+		btcoexist->bt_info.rssi_adjust_for_1ant_coex_type = *u8_tmp;
+		break;
+	case BTC_SET_UI_SCAN_SIG_COMPENSATION:
+	/*	rtlpriv->mlmepriv.scan_compensation = *u8_tmp;  */
+		break;
+	case BTC_SET_U1_1ANT_LPS:
+		btcoexist->bt_info.lps_1ant = *u8_tmp;
+		break;
+	case BTC_SET_U1_1ANT_RPWM:
+		btcoexist->bt_info.rpwm_1ant = *u8_tmp;
+		break;
+	/* the following are some action which will be triggered  */
+	case BTC_SET_ACT_LEAVE_LPS:
+		halbtc_leave_lps(btcoexist);
+		break;
+	case BTC_SET_ACT_ENTER_LPS:
+		halbtc_enter_lps(btcoexist);
+		break;
+	case BTC_SET_ACT_NORMAL_LPS:
+		halbtc_normal_lps(btcoexist);
+		break;
+	case BTC_SET_ACT_DISABLE_LOW_POWER:
+		halbtc_disable_low_power();
+		break;
+	case BTC_SET_ACT_UPDATE_ra_mask:
+		btcoexist->bt_info.ra_mask = *u32_tmp;
+		break;
+	case BTC_SET_ACT_SEND_MIMO_PS:
+		break;
+	case BTC_SET_ACT_INC_FORCE_EXEC_PWR_CMD_CNT:
+		btcoexist->bt_info.force_exec_pwr_cmd_cnt++;
+		break;
+	case BTC_SET_ACT_CTRL_BT_INFO: /*wait for 8812/8821*/
+		break;
+	case BTC_SET_ACT_CTRL_BT_COEX:
+		break;
+	default:
+		break;
+	}
+
+	return true;
+}
+
+static void halbtc_display_coex_statistics(struct btc_coexist *btcoexist)
+{
+}
+
+static void halbtc_display_bt_link_info(struct btc_coexist *btcoexist)
+{
+}
+
+static void halbtc_display_bt_fw_info(struct btc_coexist *btcoexist)
+{
+}
+
+static void halbtc_display_fw_pwr_mode_cmd(struct btc_coexist *btcoexist)
+{
+}
+
+/************************************************************
+ *		IO related function
+ ************************************************************/
+static u8 halbtc_read_1byte(void *bt_context, u32 reg_addr)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+
+	return	rtl_read_byte(rtlpriv, reg_addr);
+}
+
+static u16 halbtc_read_2byte(void *bt_context, u32 reg_addr)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+
+	return	rtl_read_word(rtlpriv, reg_addr);
+}
+
+static u32 halbtc_read_4byte(void *bt_context, u32 reg_addr)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+
+	return	rtl_read_dword(rtlpriv, reg_addr);
+}
+
+static void halbtc_write_1byte(void *bt_context, u32 reg_addr, u8 data)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+
+	rtl_write_byte(rtlpriv, reg_addr, data);
+}
+
+static void halbtc_bitmask_write_1byte(void *bt_context, u32 reg_addr,
+				       u32 bit_mask, u8 data)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	u8 original_value, bit_shift = 0;
+	u8 i;
+
+	if (bit_mask != MASKDWORD) {/*if not "double word" write*/
+		original_value = rtl_read_byte(rtlpriv, reg_addr);
+		for (i = 0; i <= 7; i++) {
+			if ((bit_mask>>i) & 0x1)
+				break;
+		}
+		bit_shift = i;
+		data = (original_value & (~bit_mask)) |
+			((data << bit_shift) & bit_mask);
+	}
+	rtl_write_byte(rtlpriv, reg_addr, data);
+}
+
+static void halbtc_write_2byte(void *bt_context, u32 reg_addr, u16 data)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+
+	rtl_write_word(rtlpriv, reg_addr, data);
+}
+
+static void halbtc_write_4byte(void *bt_context, u32 reg_addr, u32 data)
+{
+	struct btc_coexist *btcoexist =
+		(struct btc_coexist *)bt_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+
+	rtl_write_dword(rtlpriv, reg_addr, data);
+}
+
+static void halbtc_set_bbreg(void *bt_context, u32 reg_addr, u32 bit_mask,
+			     u32 data)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+
+	rtl_set_bbreg(rtlpriv->mac80211.hw, reg_addr, bit_mask, data);
+}
+
+static u32 halbtc_get_bbreg(void *bt_context, u32 reg_addr, u32 bit_mask)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+
+	return rtl_get_bbreg(rtlpriv->mac80211.hw, reg_addr, bit_mask);
+}
+
+static void halbtc_set_rfreg(void *bt_context, u8 rf_path, u32 reg_addr,
+			     u32 bit_mask, u32 data)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+
+	rtl_set_rfreg(rtlpriv->mac80211.hw, rf_path, reg_addr, bit_mask, data);
+}
+
+static u32 halbtc_get_rfreg(void *bt_context, u8 rf_path, u32 reg_addr,
+			    u32 bit_mask)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+
+	return rtl_get_rfreg(rtlpriv->mac80211.hw, rf_path, reg_addr, bit_mask);
+}
+
+static void halbtc_fill_h2c_cmd(void *bt_context, u8 element_id,
+				u32 cmd_len, u8 *cmd_buf)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+
+	rtlpriv->cfg->ops->fill_h2c_cmd(rtlpriv->mac80211.hw, element_id,
+					cmd_len, cmd_buf);
+}
+
+static void halbtc_display_dbg_msg(void *bt_context, u8 disp_type)
+{
+	struct btc_coexist *btcoexist =	(struct btc_coexist *)bt_context;
+	switch (disp_type) {
+	case BTC_DBG_DISP_COEX_STATISTICS:
+		halbtc_display_coex_statistics(btcoexist);
+		break;
+	case BTC_DBG_DISP_BT_LINK_INFO:
+		halbtc_display_bt_link_info(btcoexist);
+		break;
+	case BTC_DBG_DISP_BT_FW_VER:
+		halbtc_display_bt_fw_info(btcoexist);
+		break;
+	case BTC_DBG_DISP_FW_PWR_MODE_CMD:
+		halbtc_display_fw_pwr_mode_cmd(btcoexist);
+		break;
+	default:
+		break;
+	}
+}
+
+/*****************************************************************
+ *         Extern functions called by other module
+ *****************************************************************/
+bool exhalbtc_initlize_variables(struct rtl_priv *adapter)
+{
+	struct btc_coexist *btcoexist = &gl_bt_coexist;
+
+	btcoexist->statistics.cnt_bind++;
+
+	halbtc_dbg_init();
+
+	if (btcoexist->binded)
+		return false;
+	else
+		btcoexist->binded = true;
+
+#if (defined(CONFIG_PCI_HCI))
+	btcoexist->chip_interface = BTC_INTF_PCI;
+#elif (defined(CONFIG_USB_HCI))
+	btcoexist->chip_interface = BTC_INTF_USB;
+#elif (defined(CONFIG_SDIO_HCI))
+	btcoexist->chip_interface = BTC_INTF_SDIO;
+#elif (defined(CONFIG_GSPI_HCI))
+	btcoexist->chip_interface = BTC_INTF_GSPI;
+#else
+	btcoexist->chip_interface = BTC_INTF_UNKNOWN;
+#endif
+
+	if (NULL == btcoexist->adapter)
+		btcoexist->adapter = adapter;
+
+	btcoexist->stack_info.profile_notified = false;
+
+	btcoexist->btc_read_1byte = halbtc_read_1byte;
+	btcoexist->btc_write_1byte = halbtc_write_1byte;
+	btcoexist->btc_write_1byte_bitmask = halbtc_bitmask_write_1byte;
+	btcoexist->btc_read_2byte = halbtc_read_2byte;
+	btcoexist->btc_write_2byte = halbtc_write_2byte;
+	btcoexist->btc_read_4byte = halbtc_read_4byte;
+	btcoexist->btc_write_4byte = halbtc_write_4byte;
+
+	btcoexist->btc_set_bb_reg = halbtc_set_bbreg;
+	btcoexist->btc_get_bb_reg = halbtc_get_bbreg;
+
+	btcoexist->btc_set_rf_reg = halbtc_set_rfreg;
+	btcoexist->btc_get_rf_reg = halbtc_get_rfreg;
+
+	btcoexist->btc_fill_h2c = halbtc_fill_h2c_cmd;
+	btcoexist->btc_disp_dbg_msg = halbtc_display_dbg_msg;
+
+	btcoexist->btc_get = halbtc_get;
+	btcoexist->btc_set = halbtc_set;
+
+	btcoexist->cli_buf = &btc_dbg_buf[0];
+
+	btcoexist->bt_info.b_bt_ctrl_buf_size = false;
+	btcoexist->bt_info.agg_buf_size = 5;
+
+	btcoexist->bt_info.increase_scan_dev_num = false;
+	return true;
+}
+
+void exhalbtc_init_hw_config(struct btc_coexist *btcoexist)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+
+	btcoexist->statistics.cnt_init_hw_config++;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
+		ex_halbtc8723b2ant_init_hwconfig(btcoexist);
+}
+
+void exhalbtc_init_coex_dm(struct btc_coexist *btcoexist)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+
+	btcoexist->statistics.cnt_init_coex_dm++;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
+		ex_halbtc8723b2ant_init_coex_dm(btcoexist);
+
+	btcoexist->initilized = true;
+}
+
+void exhalbtc_ips_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	u8 ips_type;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_ips_notify++;
+	if (btcoexist->manual_control)
+		return;
+
+	if (ERFOFF == type)
+		ips_type = BTC_IPS_ENTER;
+	else
+		ips_type = BTC_IPS_LEAVE;
+
+	halbtc_leave_low_power();
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
+		ex_halbtc8723b2ant_ips_notify(btcoexist, ips_type);
+
+	halbtc_nomal_low_power();
+}
+
+void exhalbtc_lps_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	u8 lps_type;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_lps_notify++;
+	if (btcoexist->manual_control)
+		return;
+
+	if (EACTIVE == type)
+		lps_type = BTC_LPS_DISABLE;
+	else
+		lps_type = BTC_LPS_ENABLE;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
+		ex_halbtc8723b2ant_lps_notify(btcoexist, lps_type);
+}
+
+void exhalbtc_scan_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	u8 scan_type;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_scan_notify++;
+	if (btcoexist->manual_control)
+		return;
+
+	if (type)
+		scan_type = BTC_SCAN_START;
+	else
+		scan_type = BTC_SCAN_FINISH;
+
+	halbtc_leave_low_power();
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
+		ex_halbtc8723b2ant_scan_notify(btcoexist, scan_type);
+
+	halbtc_nomal_low_power();
+}
+
+void exhalbtc_connect_notify(struct btc_coexist *btcoexist, u8 action)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	u8 asso_type;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_connect_notify++;
+	if (btcoexist->manual_control)
+		return;
+
+	if (action)
+		asso_type = BTC_ASSOCIATE_START;
+	else
+		asso_type = BTC_ASSOCIATE_FINISH;
+
+	halbtc_leave_low_power();
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
+		ex_halbtc8723b2ant_connect_notify(btcoexist, asso_type);
+}
+
+void exhalbtc_mediastatus_notify(struct btc_coexist *btcoexist,
+				 enum _RT_MEDIA_STATUS media_status)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	u8 status;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_media_status_notify++;
+	if (btcoexist->manual_control)
+		return;
+
+	if (RT_MEDIA_CONNECT == media_status)
+		status = BTC_MEDIA_CONNECT;
+	else
+		status = BTC_MEDIA_DISCONNECT;
+
+	halbtc_leave_low_power();
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
+		btc8723b_med_stat_notify(btcoexist, status);
+
+	halbtc_nomal_low_power();
+}
+
+void exhalbtc_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;
+
+	packet_type = BTC_PACKET_DHCP;
+
+	halbtc_leave_low_power();
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
+		ex_halbtc8723b2ant_special_packet_notify(btcoexist,
+							 packet_type);
+
+	halbtc_nomal_low_power();
+}
+
+void exhalbtc_bt_info_notify(struct btc_coexist *btcoexist,
+			     u8 *tmp_buf, u8 length)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_bt_info_notify++;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
+		ex_halbtc8723b2ant_bt_info_notify(btcoexist, tmp_buf, length);
+}
+
+void exhalbtc_stack_operation_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	u8 stack_op_type;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_stack_operation_notify++;
+	if (btcoexist->manual_control)
+		return;
+
+	stack_op_type = BTC_STACK_OP_NONE;
+
+	halbtc_leave_low_power();
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
+		ex_halbtc8723b2ant_stack_operation_notify(btcoexist,
+							  stack_op_type);
+
+	halbtc_nomal_low_power();
+}
+
+void exhalbtc_halt_notify(struct btc_coexist *btcoexist)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
+		ex_halbtc8723b2ant_halt_notify(btcoexist);
+}
+
+void exhalbtc_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state)
+{
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+}
+
+void exhalbtc_periodical(struct btc_coexist *btcoexist)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_periodical++;
+
+	halbtc_leave_low_power();
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
+		ex_halbtc8723b2ant_periodical(btcoexist);
+
+	halbtc_nomal_low_power();
+}
+
+void exhalbtc_dbg_control(struct btc_coexist *btcoexist,
+			  u8 code, u8 len, u8 *data)
+{
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_dbg_ctrl++;
+}
+
+void exhalbtc_stack_update_profile_info(void)
+{
+}
+
+void exhalbtc_update_min_bt_rssi(char bt_rssi)
+{
+	struct btc_coexist *btcoexist = &gl_bt_coexist;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+
+	btcoexist->stack_info.min_bt_rssi = bt_rssi;
+}
+
+void exhalbtc_set_hci_version(u16 hci_version)
+{
+	struct btc_coexist *btcoexist = &gl_bt_coexist;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+
+	btcoexist->stack_info.hci_version = hci_version;
+}
+
+void exhalbtc_set_bt_patch_version(u16 bt_hci_version, u16 bt_patch_version)
+{
+	struct btc_coexist *btcoexist = &gl_bt_coexist;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+
+	btcoexist->bt_info.bt_real_fw_ver = bt_patch_version;
+	btcoexist->bt_info.bt_hci_ver = bt_hci_version;
+}
+
+void exhalbtc_set_bt_exist(bool bt_exist)
+{
+	gl_bt_coexist.board_info.bt_exist = bt_exist;
+}
+
+void exhalbtc_set_chip_type(u8 chip_type)
+{
+	switch (chip_type) {
+	default:
+	case BT_2WIRE:
+	case BT_ISSC_3WIRE:
+	case BT_ACCEL:
+	case BT_RTL8756:
+		gl_bt_coexist.board_info.bt_chip_type = BTC_CHIP_UNDEF;
+		break;
+	case BT_CSR_BC4:
+		gl_bt_coexist.board_info.bt_chip_type = BTC_CHIP_CSR_BC4;
+		break;
+	case BT_CSR_BC8:
+		gl_bt_coexist.board_info.bt_chip_type = BTC_CHIP_CSR_BC8;
+		break;
+	case BT_RTL8723A:
+		gl_bt_coexist.board_info.bt_chip_type = BTC_CHIP_RTL8723A;
+		break;
+	case BT_RTL8821A:
+		gl_bt_coexist.board_info.bt_chip_type = BTC_CHIP_RTL8821;
+		break;
+	case BT_RTL8723B:
+		gl_bt_coexist.board_info.bt_chip_type = BTC_CHIP_RTL8723B;
+		break;
+	}
+}
+
+void exhalbtc_set_ant_num(u8 type, u8 ant_num)
+{
+	if (BT_COEX_ANT_TYPE_PG == type) {
+		gl_bt_coexist.board_info.pg_ant_num = ant_num;
+		gl_bt_coexist.board_info.btdm_ant_num = ant_num;
+	} else if (BT_COEX_ANT_TYPE_ANTDIV == type) {
+		gl_bt_coexist.board_info.btdm_ant_num = ant_num;
+	}
+}
+
+void exhalbtc_display_bt_coex_info(struct btc_coexist *btcoexist)
+{
+	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)
+		ex_halbtc8723b2ant_display_coex_info(btcoexist);
+}
diff --git a/drivers/net/wireless/rtlwifi/btcoexist/halbtcoutsrc.h b/drivers/net/wireless/rtlwifi/btcoexist/halbtcoutsrc.h
new file mode 100644
index 000000000000..871fc3c6d559
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/btcoexist/halbtcoutsrc.h
@@ -0,0 +1,559 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2012  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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	__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
+
+#define		BTC_ANT_PATH_WIFI			0
+#define		BTC_ANT_PATH_BT				1
+#define		BTC_ANT_PATH_PTA			2
+
+enum btc_chip_interface {
+	BTC_INTF_UNKNOWN	= 0,
+	BTC_INTF_PCI		= 1,
+	BTC_INTF_USB		= 2,
+	BTC_INTF_SDIO		= 3,
+	BTC_INTF_GSPI		= 4,
+	BTC_INTF_MAX
+};
+
+enum BTC_CHIP_TYPE {
+	BTC_CHIP_UNDEF		= 0,
+	BTC_CHIP_CSR_BC4	= 1,
+	BTC_CHIP_CSR_BC8	= 2,
+	BTC_CHIP_RTL8723A	= 3,
+	BTC_CHIP_RTL8821	= 4,
+	BTC_CHIP_RTL8723B	= 5,
+	BTC_CHIP_MAX
+};
+
+enum BTC_MSG_TYPE {
+	BTC_MSG_INTERFACE	= 0x0,
+	BTC_MSG_ALGORITHM	= 0x1,
+	BTC_MSG_MAX
+};
+extern u32 btc_dbg_type[];
+
+/* following is for BTC_MSG_INTERFACE */
+#define		INTF_INIT				BIT0
+#define		INTF_NOTIFY				BIT2
+
+/* following is for BTC_ALGORITHM */
+#define		ALGO_BT_RSSI_STATE			BIT0
+#define		ALGO_WIFI_RSSI_STATE			BIT1
+#define		ALGO_BT_MONITOR				BIT2
+#define		ALGO_TRACE				BIT3
+#define		ALGO_TRACE_FW				BIT4
+#define		ALGO_TRACE_FW_DETAIL			BIT5
+#define		ALGO_TRACE_FW_EXEC			BIT6
+#define		ALGO_TRACE_SW				BIT7
+#define		ALGO_TRACE_SW_DETAIL			BIT8
+#define		ALGO_TRACE_SW_EXEC			BIT9
+
+#define		BT_COEX_ANT_TYPE_PG			0
+#define		BT_COEX_ANT_TYPE_ANTDIV			1
+#define		BT_COEX_ANT_TYPE_DETECTED		2
+#define		BTC_MIMO_PS_STATIC			0
+#define		BTC_MIMO_PS_DYNAMIC			1
+#define		BTC_RATE_DISABLE			0
+#define		BTC_RATE_ENABLE				1
+#define		BTC_ANT_PATH_WIFI			0
+#define		BTC_ANT_PATH_BT				1
+#define		BTC_ANT_PATH_PTA			2
+
+
+#define	CL_SPRINTF	snprintf
+#define	CL_PRINTF	printk
+
+#define	BTC_PRINT(dbgtype, dbgflag, printstr, ...)		\
+	do {							\
+		if (unlikely(btc_dbg_type[dbgtype] & dbgflag)) {\
+			printk(printstr, ##__VA_ARGS__);	\
+		}						\
+	} while (0)
+
+#define	BTC_PRINT_F(dbgtype, dbgflag, printstr, ...)		\
+	do {							\
+		if (unlikely(btc_dbg_type[dbgtype] & dbgflag)) {\
+			pr_info("%s: ", __func__);	\
+			printk(printstr, ##__VA_ARGS__);	\
+		}						\
+	} while (0)
+
+#define	BTC_PRINT_ADDR(dbgtype, dbgflag, printstr, _ptr)	\
+	do {							\
+		if (unlikely(btc_dbg_type[dbgtype] & dbgflag)) {	\
+			int __i;				\
+			u8 *__ptr = (u8 *)_ptr;			\
+			printk printstr;			\
+			for (__i = 0; __i < 6; __i++)		\
+				printk("%02X%s", __ptr[__i], (__i == 5) ? \
+				       "" : "-");		\
+			pr_info("\n");				\
+		}						\
+	} while (0)
+
+#define BTC_PRINT_DATA(dbgtype, dbgflag, _titlestring, _hexdata, _hexdatalen) \
+	do {								\
+		if (unlikely(btc_dbg_type[dbgtype] & dbgflag))	{	\
+			int __i;					\
+			u8 *__ptr = (u8 *)_hexdata;			\
+			printk(_titlestring);				\
+			for (__i = 0; __i < (int)_hexdatalen; __i++) {	\
+				printk("%02X%s", __ptr[__i], (((__i + 1) % 4) \
+							== 0) ? "  " : " ");\
+				if (((__i + 1) % 16) == 0)		\
+					printk("\n");			\
+			}						\
+			pr_debug("\n");					\
+		}							\
+	} while (0)
+
+#define BTC_ANT_PATH_WIFI	0
+#define BTC_ANT_PATH_BT		1
+#define BTC_ANT_PATH_PTA	2
+
+enum btc_power_save_type {
+	BTC_PS_WIFI_NATIVE = 0,
+	BTC_PS_LPS_ON = 1,
+	BTC_PS_LPS_OFF = 2,
+	BTC_PS_LPS_MAX
+};
+
+struct btc_board_info {
+	/* The following is some board information */
+	u8 bt_chip_type;
+	u8 pg_ant_num;	/* pg ant number */
+	u8 btdm_ant_num;	/* ant number for btdm */
+	u8 btdm_ant_pos;
+	bool bt_exist;
+};
+
+enum btc_dbg_opcode {
+	BTC_DBG_SET_COEX_NORMAL = 0x0,
+	BTC_DBG_SET_COEX_WIFI_ONLY = 0x1,
+	BTC_DBG_SET_COEX_BT_ONLY = 0x2,
+	BTC_DBG_MAX
+};
+
+enum btc_rssi_state {
+	BTC_RSSI_STATE_HIGH = 0x0,
+	BTC_RSSI_STATE_MEDIUM = 0x1,
+	BTC_RSSI_STATE_LOW = 0x2,
+	BTC_RSSI_STATE_STAY_HIGH = 0x3,
+	BTC_RSSI_STATE_STAY_MEDIUM = 0x4,
+	BTC_RSSI_STATE_STAY_LOW = 0x5,
+	BTC_RSSI_MAX
+};
+
+enum btc_wifi_role {
+	BTC_ROLE_STATION = 0x0,
+	BTC_ROLE_AP = 0x1,
+	BTC_ROLE_IBSS = 0x2,
+	BTC_ROLE_HS_MODE = 0x3,
+	BTC_ROLE_MAX
+};
+
+enum btc_wifi_bw_mode {
+	BTC_WIFI_BW_LEGACY = 0x0,
+	BTC_WIFI_BW_HT20 = 0x1,
+	BTC_WIFI_BW_HT40 = 0x2,
+	BTC_WIFI_BW_MAX
+};
+
+enum btc_wifi_traffic_dir {
+	BTC_WIFI_TRAFFIC_TX = 0x0,
+	BTC_WIFI_TRAFFIC_RX = 0x1,
+	BTC_WIFI_TRAFFIC_MAX
+};
+
+enum btc_wifi_pnp {
+	BTC_WIFI_PNP_WAKE_UP = 0x0,
+	BTC_WIFI_PNP_SLEEP = 0x1,
+	BTC_WIFI_PNP_MAX
+};
+
+
+enum btc_get_type {
+	/* type bool */
+	BTC_GET_BL_HS_OPERATION,
+	BTC_GET_BL_HS_CONNECTING,
+	BTC_GET_BL_WIFI_CONNECTED,
+	BTC_GET_BL_WIFI_BUSY,
+	BTC_GET_BL_WIFI_SCAN,
+	BTC_GET_BL_WIFI_LINK,
+	BTC_GET_BL_WIFI_DHCP,
+	BTC_GET_BL_WIFI_SOFTAP_IDLE,
+	BTC_GET_BL_WIFI_SOFTAP_LINKING,
+	BTC_GET_BL_WIFI_IN_EARLY_SUSPEND,
+	BTC_GET_BL_WIFI_ROAM,
+	BTC_GET_BL_WIFI_4_WAY_PROGRESS,
+	BTC_GET_BL_WIFI_UNDER_5G,
+	BTC_GET_BL_WIFI_AP_MODE_ENABLE,
+	BTC_GET_BL_WIFI_ENABLE_ENCRYPTION,
+	BTC_GET_BL_WIFI_UNDER_B_MODE,
+	BTC_GET_BL_EXT_SWITCH,
+
+	/* type s4Byte */
+	BTC_GET_S4_WIFI_RSSI,
+	BTC_GET_S4_HS_RSSI,
+
+	/* type u32 */
+	BTC_GET_U4_WIFI_BW,
+	BTC_GET_U4_WIFI_TRAFFIC_DIRECTION,
+	BTC_GET_U4_WIFI_FW_VER,
+	BTC_GET_U4_BT_PATCH_VER,
+
+	/* type u1Byte */
+	BTC_GET_U1_WIFI_DOT11_CHNL,
+	BTC_GET_U1_WIFI_CENTRAL_CHNL,
+	BTC_GET_U1_WIFI_HS_CHNL,
+	BTC_GET_U1_MAC_PHY_MODE,
+
+	/* 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_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE,
+	BTC_SET_UI_SCAN_SIG_COMPENSATION,
+	BTC_SET_U1_AGG_BUF_SIZE,
+
+	/* type trigger some action */
+	BTC_SET_ACT_GET_BT_RSSI,
+	BTC_SET_ACT_AGGREGATE_CTRL,
+
+	/********* for 1Ant **********/
+	/* type bool */
+	BTC_SET_BL_BT_SCO_BUSY,
+	/* type u1Byte */
+	BTC_SET_U1_1ANT_LPS,
+	BTC_SET_U1_1ANT_RPWM,
+	/* type trigger some action */
+	BTC_SET_ACT_LEAVE_LPS,
+	BTC_SET_ACT_ENTER_LPS,
+	BTC_SET_ACT_NORMAL_LPS,
+	BTC_SET_ACT_INC_FORCE_EXEC_PWR_CMD_CNT,
+	BTC_SET_ACT_DISABLE_LOW_POWER,
+	BTC_SET_ACT_UPDATE_ra_mask,
+	BTC_SET_ACT_SEND_MIMO_PS,
+	/* BT Coex related */
+	BTC_SET_ACT_CTRL_BT_INFO,
+	BTC_SET_ACT_CTRL_BT_COEX,
+	/***************************/
+	BTC_SET_MAX
+};
+
+enum btc_dbg_disp_type {
+	BTC_DBG_DISP_COEX_STATISTICS = 0x0,
+	BTC_DBG_DISP_BT_LINK_INFO = 0x1,
+	BTC_DBG_DISP_BT_FW_VER = 0x2,
+	BTC_DBG_DISP_FW_PWR_MODE_CMD = 0x3,
+	BTC_DBG_DISP_MAX
+};
+
+enum btc_notify_type_ips {
+	BTC_IPS_LEAVE = 0x0,
+	BTC_IPS_ENTER = 0x1,
+	BTC_IPS_MAX
+};
+
+enum btc_notify_type_lps {
+	BTC_LPS_DISABLE = 0x0,
+	BTC_LPS_ENABLE = 0x1,
+	BTC_LPS_MAX
+};
+
+enum btc_notify_type_scan {
+	BTC_SCAN_FINISH = 0x0,
+	BTC_SCAN_START = 0x1,
+	BTC_SCAN_MAX
+};
+
+enum btc_notify_type_associate {
+	BTC_ASSOCIATE_FINISH = 0x0,
+	BTC_ASSOCIATE_START = 0x1,
+	BTC_ASSOCIATE_MAX
+};
+
+enum btc_notify_type_media_status {
+	BTC_MEDIA_DISCONNECT = 0x0,
+	BTC_MEDIA_CONNECT = 0x1,
+	BTC_MEDIA_MAX
+};
+
+enum btc_notify_type_special_packet {
+	BTC_PACKET_UNKNOWN = 0x0,
+	BTC_PACKET_DHCP = 0x1,
+	BTC_PACKET_ARP = 0x2,
+	BTC_PACKET_EAPOL = 0x3,
+	BTC_PACKET_MAX
+};
+
+enum btc_notify_type_stack_operation {
+	BTC_STACK_OP_NONE = 0x0,
+	BTC_STACK_OP_INQ_PAGE_PAIR_START = 0x1,
+	BTC_STACK_OP_INQ_PAGE_PAIR_FINISH = 0x2,
+	BTC_STACK_OP_MAX
+};
+
+
+typedef u8 (*bfp_btc_r1)(void *btc_context, u32 reg_addr);
+
+typedef u16 (*bfp_btc_r2)(void *btc_context, u32 reg_addr);
+
+typedef u32 (*bfp_btc_r4)(void *btc_context, u32 reg_addr);
+
+typedef void (*bfp_btc_w1)(void *btc_context, u32 reg_addr, u8 data);
+
+typedef void (*bfp_btc_w1_bit_mak)(void *btc_context, u32 reg_addr,
+				   u32 bit_mask, u8 data1b);
+
+typedef void (*bfp_btc_w2)(void *btc_context, u32 reg_addr, u16 data);
+
+typedef void (*bfp_btc_w4)(void *btc_context, u32 reg_addr, u32 data);
+
+typedef void (*bfp_btc_wr_1byte_bit_mask)(void *btc_context, u32 reg_addr,
+					  u8 bit_mask, u8 data);
+
+typedef void (*bfp_btc_set_bb_reg)(void *btc_context, u32 reg_addr,
+				   u32 bit_mask, u32 data);
+
+typedef u32 (*bfp_btc_get_bb_reg)(void *btc_context, u32 reg_addr,
+				  u32 bit_mask);
+
+typedef void (*bfp_btc_set_rf_reg)(void *btc_context, u8 rf_path, u32 reg_addr,
+				   u32 bit_mask, u32 data);
+
+typedef u32 (*bfp_btc_get_rf_reg)(void *btc_context, u8 rf_path,
+				  u32 reg_addr, u32 bit_mask);
+
+typedef void (*bfp_btc_fill_h2c)(void *btc_context, u8 element_id,
+				 u32 cmd_len, u8 *cmd_buffer);
+
+typedef	bool (*bfp_btc_get)(void *btcoexist, u8 get_type, void *out_buf);
+
+typedef	bool (*bfp_btc_set)(void *btcoexist, u8 set_type, void *in_buf);
+
+typedef void (*bfp_btc_disp_dbg_msg)(void *btcoexist, u8 disp_type);
+
+struct btc_bt_info {
+	bool bt_disabled;
+	u8 rssi_adjust_for_agc_table_on;
+	u8 rssi_adjust_for_1ant_coex_type;
+	bool bt_busy;
+	u8 agg_buf_size;
+	bool limited_dig;
+	bool reject_agg_pkt;
+	bool b_bt_ctrl_buf_size;
+	bool increase_scan_dev_num;
+	u16 bt_hci_ver;
+	u16 bt_real_fw_ver;
+	u8 bt_fw_ver;
+
+	/* the following is for 1Ant solution */
+	bool bt_ctrl_lps;
+	bool bt_pwr_save_mode;
+	bool bt_lps_on;
+	bool force_to_roam;
+	u8 force_exec_pwr_cmd_cnt;
+	u8 lps_1ant;
+	u8 rpwm_1ant;
+	u32 ra_mask;
+};
+
+struct btc_stack_info {
+	bool profile_notified;
+	u16 hci_version;	/* stack hci version */
+	u8 num_of_link;
+	bool bt_link_exist;
+	bool sco_exist;
+	bool acl_exist;
+	bool a2dp_exist;
+	bool hid_exist;
+	u8 num_of_hid;
+	bool pan_exist;
+	bool unknown_acl_exist;
+	char min_bt_rssi;
+};
+
+struct btc_statistics {
+	u32 cnt_bind;
+	u32 cnt_init_hw_config;
+	u32 cnt_init_coex_dm;
+	u32 cnt_ips_notify;
+	u32 cnt_lps_notify;
+	u32 cnt_scan_notify;
+	u32 cnt_connect_notify;
+	u32 cnt_media_status_notify;
+	u32 cnt_special_packet_notify;
+	u32 cnt_bt_info_notify;
+	u32 cnt_periodical;
+	u32 cnt_stack_operation_notify;
+	u32 cnt_dbg_ctrl;
+};
+
+struct btc_bt_link_info {
+	bool bt_link_exist;
+	bool sco_exist;
+	bool sco_only;
+	bool a2dp_exist;
+	bool a2dp_only;
+	bool hid_exist;
+	bool hid_only;
+	bool pan_exist;
+	bool pan_only;
+};
+
+enum btc_antenna_pos {
+	BTC_ANTENNA_AT_MAIN_PORT = 0x1,
+	BTC_ANTENNA_AT_AUX_PORT = 0x2,
+};
+
+struct btc_coexist {
+	/* make sure only one adapter can bind the data context  */
+	bool binded;
+	/* default adapter */
+	void *adapter;
+	struct btc_board_info board_info;
+	/* some bt info referenced by non-bt module */
+	struct btc_bt_info bt_info;
+	struct btc_stack_info stack_info;
+	enum btc_chip_interface	chip_interface;
+	struct btc_bt_link_info bt_link_info;
+
+	bool initilized;
+	bool stop_coex_dm;
+	bool manual_control;
+	u8 *cli_buf;
+	struct btc_statistics statistics;
+	u8 pwr_mode_val[10];
+
+	/* function pointers - io related */
+	bfp_btc_r1 btc_read_1byte;
+	bfp_btc_w1 btc_write_1byte;
+	bfp_btc_w1_bit_mak btc_write_1byte_bitmask;
+	bfp_btc_r2 btc_read_2byte;
+	bfp_btc_w2 btc_write_2byte;
+	bfp_btc_r4 btc_read_4byte;
+	bfp_btc_w4 btc_write_4byte;
+
+	bfp_btc_set_bb_reg btc_set_bb_reg;
+	bfp_btc_get_bb_reg btc_get_bb_reg;
+
+
+	bfp_btc_set_rf_reg btc_set_rf_reg;
+	bfp_btc_get_rf_reg btc_get_rf_reg;
+
+	bfp_btc_fill_h2c btc_fill_h2c;
+
+	bfp_btc_disp_dbg_msg btc_disp_dbg_msg;
+
+	bfp_btc_get btc_get;
+	bfp_btc_set btc_set;
+};
+
+bool halbtc_is_wifi_uplink(struct rtl_priv *adapter);
+
+extern struct btc_coexist gl_bt_coexist;
+
+bool exhalbtc_initlize_variables(struct rtl_priv *adapter);
+void exhalbtc_init_hw_config(struct btc_coexist *btcoexist);
+void exhalbtc_init_coex_dm(struct btc_coexist *btcoexist);
+void exhalbtc_ips_notify(struct btc_coexist *btcoexist, u8 type);
+void exhalbtc_lps_notify(struct btc_coexist *btcoexist, u8 type);
+void exhalbtc_scan_notify(struct btc_coexist *btcoexist, u8 type);
+void exhalbtc_connect_notify(struct btc_coexist *btcoexist, u8 action);
+void exhalbtc_mediastatus_notify(struct btc_coexist *btcoexist,
+				 enum _RT_MEDIA_STATUS media_status);
+void exhalbtc_special_packet_notify(struct btc_coexist *btcoexist, u8 pkt_type);
+void exhalbtc_bt_info_notify(struct btc_coexist *btcoexist, u8 *tmp_buf,
+			     u8 length);
+void exhalbtc_stack_operation_notify(struct btc_coexist *btcoexist, u8 type);
+void exhalbtc_halt_notify(struct btc_coexist *btcoexist);
+void exhalbtc_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state);
+void exhalbtc_periodical(struct btc_coexist *btcoexist);
+void exhalbtc_dbg_control(struct btc_coexist *btcoexist, u8 code, u8 len,
+			  u8 *data);
+void exhalbtc_stack_update_profile_info(void);
+void exhalbtc_set_hci_version(u16 hci_version);
+void exhalbtc_set_bt_patch_version(u16 bt_hci_version, u16 bt_patch_version);
+void exhalbtc_update_min_bt_rssi(char bt_rssi);
+void exhalbtc_set_bt_exist(bool bt_exist);
+void exhalbtc_set_chip_type(u8 chip_type);
+void exhalbtc_set_ant_num(u8 type, u8 ant_num);
+void exhalbtc_display_bt_coex_info(struct btc_coexist *btcoexist);
+void exhalbtc_signal_compensation(struct btc_coexist *btcoexist,
+				  u8 *rssi_wifi, u8 *rssi_bt);
+void exhalbtc_lps_leave(struct btc_coexist *btcoexist);
+void exhalbtc_low_wifi_traffic_notify(struct btc_coexist *btcoexist);
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/btcoexist/rtl_btc.c b/drivers/net/wireless/rtlwifi/btcoexist/rtl_btc.c
new file mode 100644
index 000000000000..0ab94fe4cbbe
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/btcoexist/rtl_btc.c
@@ -0,0 +1,218 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2013  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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 "rtl_btc.h"
+#include "halbt_precomp.h"
+
+#include <linux/vmalloc.h>
+#include <linux/module.h>
+
+static struct rtl_btc_ops rtl_btc_operation = {
+	.btc_init_variables = rtl_btc_init_variables,
+	.btc_init_hal_vars = rtl_btc_init_hal_vars,
+	.btc_init_hw_config = rtl_btc_init_hw_config,
+	.btc_ips_notify = rtl_btc_ips_notify,
+	.btc_scan_notify = rtl_btc_scan_notify,
+	.btc_connect_notify = rtl_btc_connect_notify,
+	.btc_mediastatus_notify = rtl_btc_mediastatus_notify,
+	.btc_periodical = rtl_btc_periodical,
+	.btc_halt_notify = rtl_btc_halt_notify,
+	.btc_btinfo_notify = rtl_btc_btinfo_notify,
+	.btc_is_limited_dig = rtl_btc_is_limited_dig,
+	.btc_is_disable_edca_turbo = rtl_btc_is_disable_edca_turbo,
+	.btc_is_bt_disabled = rtl_btc_is_bt_disabled,
+};
+
+void rtl_btc_init_variables(struct rtl_priv *rtlpriv)
+{
+	exhalbtc_initlize_variables(rtlpriv);
+}
+
+void rtl_btc_init_hal_vars(struct rtl_priv *rtlpriv)
+{
+	u8 ant_num;
+	u8 bt_exist;
+	u8 bt_type;
+
+	ant_num = rtl_get_hwpg_ant_num(rtlpriv);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		 "%s, antNum is %d\n", __func__, ant_num);
+
+	bt_exist = rtl_get_hwpg_bt_exist(rtlpriv);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		 "%s, bt_exist is %d\n", __func__, bt_exist);
+	exhalbtc_set_bt_exist(bt_exist);
+
+	bt_type = rtl_get_hwpg_bt_type(rtlpriv);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "%s, bt_type is %d\n",
+		 __func__, bt_type);
+	exhalbtc_set_chip_type(bt_type);
+
+	exhalbtc_set_ant_num(BT_COEX_ANT_TYPE_PG, ant_num);
+}
+
+void rtl_btc_init_hw_config(struct rtl_priv *rtlpriv)
+{
+	exhalbtc_init_hw_config(&gl_bt_coexist);
+	exhalbtc_init_coex_dm(&gl_bt_coexist);
+}
+
+void rtl_btc_ips_notify(struct rtl_priv *rtlpriv, u8 type)
+{
+	exhalbtc_ips_notify(&gl_bt_coexist, type);
+}
+
+void rtl_btc_scan_notify(struct rtl_priv *rtlpriv, u8 scantype)
+{
+	exhalbtc_scan_notify(&gl_bt_coexist, scantype);
+}
+
+void rtl_btc_connect_notify(struct rtl_priv *rtlpriv, u8 action)
+{
+	exhalbtc_connect_notify(&gl_bt_coexist, action);
+}
+
+void rtl_btc_mediastatus_notify(struct rtl_priv *rtlpriv,
+				enum _RT_MEDIA_STATUS mstatus)
+{
+	exhalbtc_mediastatus_notify(&gl_bt_coexist, mstatus);
+}
+
+void rtl_btc_periodical(struct rtl_priv *rtlpriv)
+{
+	exhalbtc_periodical(&gl_bt_coexist);
+}
+
+void rtl_btc_halt_notify(void)
+{
+	exhalbtc_halt_notify(&gl_bt_coexist);
+}
+
+void rtl_btc_btinfo_notify(struct rtl_priv *rtlpriv, u8 *tmp_buf, u8 length)
+{
+	exhalbtc_bt_info_notify(&gl_bt_coexist, tmp_buf, length);
+}
+
+bool rtl_btc_is_limited_dig(struct rtl_priv *rtlpriv)
+{
+	return gl_bt_coexist.bt_info.limited_dig;
+}
+
+bool rtl_btc_is_disable_edca_turbo(struct rtl_priv *rtlpriv)
+{
+	bool bt_change_edca = false;
+	u32 cur_edca_val;
+	u32 edca_bt_hs_uplink = 0x5ea42b, edca_bt_hs_downlink = 0x5ea42b;
+	u32 edca_hs;
+	u32 edca_addr = 0x504;
+
+	cur_edca_val = rtl_read_dword(rtlpriv, edca_addr);
+	if (halbtc_is_wifi_uplink(rtlpriv)) {
+		if (cur_edca_val != edca_bt_hs_uplink) {
+			edca_hs = edca_bt_hs_uplink;
+			bt_change_edca = true;
+		}
+	} else {
+		if (cur_edca_val != edca_bt_hs_downlink) {
+			edca_hs = edca_bt_hs_downlink;
+			bt_change_edca = true;
+		}
+	}
+
+	if (bt_change_edca)
+		rtl_write_dword(rtlpriv, edca_addr, edca_hs);
+
+	return true;
+}
+
+bool rtl_btc_is_bt_disabled(struct rtl_priv *rtlpriv)
+{
+	if (gl_bt_coexist.bt_info.bt_disabled)
+		return true;
+	else
+		return false;
+}
+
+struct rtl_btc_ops *rtl_btc_get_ops_pointer(void)
+{
+	return &rtl_btc_operation;
+}
+EXPORT_SYMBOL(rtl_btc_get_ops_pointer);
+
+u8 rtl_get_hwpg_ant_num(struct rtl_priv *rtlpriv)
+{
+	u8 num;
+
+	if (rtlpriv->btcoexist.btc_info.ant_num == ANT_X2)
+		num = 2;
+	else
+		num = 1;
+
+	return num;
+}
+
+enum _RT_MEDIA_STATUS mgnt_link_status_query(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	enum _RT_MEDIA_STATUS    m_status = RT_MEDIA_DISCONNECT;
+
+	u8 bibss = (mac->opmode == NL80211_IFTYPE_ADHOC) ? 1 : 0;
+
+	if (bibss || rtlpriv->mac80211.link_state >= MAC80211_LINKED)
+		m_status = RT_MEDIA_CONNECT;
+
+	return m_status;
+}
+
+u8 rtl_get_hwpg_bt_exist(struct rtl_priv *rtlpriv)
+{
+	return rtlpriv->btcoexist.btc_info.btcoexist;
+}
+
+u8 rtl_get_hwpg_bt_type(struct rtl_priv *rtlpriv)
+{
+	return rtlpriv->btcoexist.btc_info.bt_type;
+}
+
+MODULE_AUTHOR("Page He	<page_he@realsil.com.cn>");
+MODULE_AUTHOR("Realtek WlanFAE	<wlanfae@realtek.com>");
+MODULE_AUTHOR("Larry Finger	<Larry.FInger@lwfinger.net>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Realtek 802.11n PCI wireless core");
+
+static int __init rtl_btcoexist_module_init(void)
+{
+	return 0;
+}
+
+static void __exit rtl_btcoexist_module_exit(void)
+{
+	return;
+}
+
+module_init(rtl_btcoexist_module_init);
+module_exit(rtl_btcoexist_module_exit);
diff --git a/drivers/net/wireless/rtlwifi/btcoexist/rtl_btc.h b/drivers/net/wireless/rtlwifi/btcoexist/rtl_btc.h
new file mode 100644
index 000000000000..805b22cc8fc8
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/btcoexist/rtl_btc.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_BTC_H__
+#define __RTL_BTC_H__
+
+#include "halbt_precomp.h"
+
+void rtl_btc_init_variables(struct rtl_priv *rtlpriv);
+void rtl_btc_init_hal_vars(struct rtl_priv *rtlpriv);
+void rtl_btc_init_hw_config(struct rtl_priv *rtlpriv);
+void rtl_btc_ips_notify(struct rtl_priv *rtlpriv, u8 type);
+void rtl_btc_scan_notify(struct rtl_priv *rtlpriv, u8 scantype);
+void rtl_btc_connect_notify(struct rtl_priv *rtlpriv, u8 action);
+void rtl_btc_mediastatus_notify(struct rtl_priv *rtlpriv,
+				enum _RT_MEDIA_STATUS mstatus);
+void rtl_btc_periodical(struct rtl_priv *rtlpriv);
+void rtl_btc_halt_notify(void);
+void rtl_btc_btinfo_notify(struct rtl_priv *rtlpriv, u8 *tmpbuf, u8 length);
+bool rtl_btc_is_limited_dig(struct rtl_priv *rtlpriv);
+bool rtl_btc_is_disable_edca_turbo(struct rtl_priv *rtlpriv);
+bool rtl_btc_is_bt_disabled(struct rtl_priv *rtlpriv);
+
+struct rtl_btc_ops *rtl_btc_get_ops_pointer(void);
+
+u8 rtl_get_hwpg_ant_num(struct rtl_priv *rtlpriv);
+u8 rtl_get_hwpg_bt_exist(struct rtl_priv *rtlpriv);
+u8 rtl_get_hwpg_bt_type(struct rtl_priv *rtlpriv);
+enum _RT_MEDIA_STATUS mgnt_link_status_query(struct ieee80211_hw *hw);
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/core.c b/drivers/net/wireless/rtlwifi/core.c
index 2d337a0c3df0..ded691f76f2f 100644
--- a/drivers/net/wireless/rtlwifi/core.c
+++ b/drivers/net/wireless/rtlwifi/core.c
@@ -36,6 +36,66 @@
 
 #include <linux/export.h>
 
+void rtl_addr_delay(u32 addr)
+{
+	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);
+}
+EXPORT_SYMBOL(rtl_addr_delay);
+
+void rtl_rfreg_delay(struct ieee80211_hw *hw, enum radio_path rfpath, u32 addr,
+		     u32 mask, u32 data)
+{
+	if (addr == 0xfe) {
+		mdelay(50);
+	} else if (addr == 0xfd) {
+		mdelay(5);
+	} else if (addr == 0xfc) {
+		mdelay(1);
+	} else if (addr == 0xfb) {
+		udelay(50);
+	} else if (addr == 0xfa) {
+		udelay(5);
+	} else if (addr == 0xf9) {
+		udelay(1);
+	} else {
+		rtl_set_rfreg(hw, rfpath, addr, mask, data);
+		udelay(1);
+	}
+}
+EXPORT_SYMBOL(rtl_rfreg_delay);
+
+void rtl_bb_delay(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);
+	}
+}
+EXPORT_SYMBOL(rtl_bb_delay);
+
 void rtl_fw_cb(const struct firmware *firmware, void *context)
 {
 	struct ieee80211_hw *hw = context;
@@ -475,20 +535,40 @@ static void rtl_op_configure_filter(struct ieee80211_hw *hw,
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u32 rx_conf;
 
 	*new_flags &= RTL_SUPPORTED_FILTERS;
 	if (!changed_flags)
 		return;
 
+	/* if ssid not set to hw don't check bssid
+	 * here just used for linked scanning, & linked
+	 * and nolink check bssid is set in set network_type */
+	if ((changed_flags & FIF_BCN_PRBRESP_PROMISC) &&
+		(mac->link_state >= MAC80211_LINKED)) {
+		if (mac->opmode != NL80211_IFTYPE_AP &&
+		    mac->opmode != NL80211_IFTYPE_MESH_POINT) {
+			if (*new_flags & FIF_BCN_PRBRESP_PROMISC) {
+				rtlpriv->cfg->ops->set_chk_bssid(hw, false);
+			} else {
+				rtlpriv->cfg->ops->set_chk_bssid(hw, true);
+			}
+		}
+	}
+
+	/* must be called after set_chk_bssid since that function modifies the
+	 * RCR register too. */
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_RCR, (u8 *)(&rx_conf));
+
 	/*TODO: we disable broadcase now, so enable here */
 	if (changed_flags & FIF_ALLMULTI) {
 		if (*new_flags & FIF_ALLMULTI) {
-			mac->rx_conf |= rtlpriv->cfg->maps[MAC_RCR_AM] |
+			rx_conf |= rtlpriv->cfg->maps[MAC_RCR_AM] |
 			    rtlpriv->cfg->maps[MAC_RCR_AB];
 			RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD,
 				 "Enable receive multicast frame\n");
 		} else {
-			mac->rx_conf &= ~(rtlpriv->cfg->maps[MAC_RCR_AM] |
+			rx_conf &= ~(rtlpriv->cfg->maps[MAC_RCR_AM] |
 					  rtlpriv->cfg->maps[MAC_RCR_AB]);
 			RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD,
 				 "Disable receive multicast frame\n");
@@ -497,39 +577,25 @@ static void rtl_op_configure_filter(struct ieee80211_hw *hw,
 
 	if (changed_flags & FIF_FCSFAIL) {
 		if (*new_flags & FIF_FCSFAIL) {
-			mac->rx_conf |= rtlpriv->cfg->maps[MAC_RCR_ACRC32];
+			rx_conf |= rtlpriv->cfg->maps[MAC_RCR_ACRC32];
 			RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD,
 				 "Enable receive FCS error frame\n");
 		} else {
-			mac->rx_conf &= ~rtlpriv->cfg->maps[MAC_RCR_ACRC32];
+			rx_conf &= ~rtlpriv->cfg->maps[MAC_RCR_ACRC32];
 			RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD,
 				 "Disable receive FCS error frame\n");
 		}
 	}
 
-	/* if ssid not set to hw don't check bssid
-	 * here just used for linked scanning, & linked
-	 * and nolink check bssid is set in set network_type */
-	if ((changed_flags & FIF_BCN_PRBRESP_PROMISC) &&
-		(mac->link_state >= MAC80211_LINKED)) {
-		if (mac->opmode != NL80211_IFTYPE_AP &&
-		    mac->opmode != NL80211_IFTYPE_MESH_POINT) {
-			if (*new_flags & FIF_BCN_PRBRESP_PROMISC) {
-				rtlpriv->cfg->ops->set_chk_bssid(hw, false);
-			} else {
-				rtlpriv->cfg->ops->set_chk_bssid(hw, true);
-			}
-		}
-	}
 
 	if (changed_flags & FIF_CONTROL) {
 		if (*new_flags & FIF_CONTROL) {
-			mac->rx_conf |= rtlpriv->cfg->maps[MAC_RCR_ACF];
+			rx_conf |= rtlpriv->cfg->maps[MAC_RCR_ACF];
 
 			RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD,
 				 "Enable receive control frame\n");
 		} else {
-			mac->rx_conf &= ~rtlpriv->cfg->maps[MAC_RCR_ACF];
+			rx_conf &= ~rtlpriv->cfg->maps[MAC_RCR_ACF];
 			RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD,
 				 "Disable receive control frame\n");
 		}
@@ -537,15 +603,17 @@ static void rtl_op_configure_filter(struct ieee80211_hw *hw,
 
 	if (changed_flags & FIF_OTHER_BSS) {
 		if (*new_flags & FIF_OTHER_BSS) {
-			mac->rx_conf |= rtlpriv->cfg->maps[MAC_RCR_AAP];
+			rx_conf |= rtlpriv->cfg->maps[MAC_RCR_AAP];
 			RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD,
 				 "Enable receive other BSS's frame\n");
 		} else {
-			mac->rx_conf &= ~rtlpriv->cfg->maps[MAC_RCR_AAP];
+			rx_conf &= ~rtlpriv->cfg->maps[MAC_RCR_AAP];
 			RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD,
 				 "Disable receive other BSS's frame\n");
 		}
 	}
+
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_RCR, (u8 *)(&rx_conf));
 }
 static int rtl_op_sta_add(struct ieee80211_hw *hw,
 			 struct ieee80211_vif *vif,
@@ -738,6 +806,11 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 				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);
@@ -892,7 +965,7 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 
 			mac->basic_rates = basic_rates;
 			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BASIC_RATE,
-					(u8 *) (&basic_rates));
+					(u8 *)(&basic_rates));
 		}
 		rcu_read_unlock();
 	}
@@ -906,6 +979,11 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 		if (bss_conf->assoc) {
 			if (ppsc->fwctrl_lps) {
 				u8 mstatus = RT_MEDIA_CONNECT;
+				u8 keep_alive = 10;
+				rtlpriv->cfg->ops->set_hw_reg(hw,
+						 HW_VAR_KEEP_ALIVE,
+						 (u8 *)(&keep_alive));
+
 				rtlpriv->cfg->ops->set_hw_reg(hw,
 						      HW_VAR_H2C_FW_JOINBSSRPT,
 						      &mstatus);
diff --git a/drivers/net/wireless/rtlwifi/core.h b/drivers/net/wireless/rtlwifi/core.h
index 2fe46a1b4f1f..027e75374dcc 100644
--- a/drivers/net/wireless/rtlwifi/core.h
+++ b/drivers/net/wireless/rtlwifi/core.h
@@ -41,5 +41,9 @@
 
 extern const struct ieee80211_ops rtl_ops;
 void rtl_fw_cb(const struct firmware *firmware, void *context);
+void rtl_addr_delay(u32 addr);
+void rtl_rfreg_delay(struct ieee80211_hw *hw, enum radio_path rfpath, u32 addr,
+		     u32 mask, u32 data);
+void rtl_bb_delay(struct ieee80211_hw *hw, u32 addr, u32 data);
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/pci.c b/drivers/net/wireless/rtlwifi/pci.c
index d7aa165fe677..f26f4ffc771d 100644
--- a/drivers/net/wireless/rtlwifi/pci.c
+++ b/drivers/net/wireless/rtlwifi/pci.c
@@ -811,19 +811,19 @@ done:
 		if (pci_dma_mapping_error(rtlpci->pdev, bufferaddress))
 			return;
 		tmp_one = 1;
-		rtlpriv->cfg->ops->set_desc((u8 *) pdesc, false,
+		rtlpriv->cfg->ops->set_desc(hw, (u8 *)pdesc, false,
 					    HW_DESC_RXBUFF_ADDR,
 					    (u8 *)&bufferaddress);
-		rtlpriv->cfg->ops->set_desc((u8 *)pdesc, false,
+		rtlpriv->cfg->ops->set_desc(hw, (u8 *)pdesc, false,
 					    HW_DESC_RXPKT_LEN,
 					    (u8 *)&rtlpci->rxbuffersize);
 
 		if (index == rtlpci->rxringcount - 1)
-			rtlpriv->cfg->ops->set_desc((u8 *)pdesc, false,
+			rtlpriv->cfg->ops->set_desc(hw, (u8 *)pdesc, false,
 						    HW_DESC_RXERO,
 						    &tmp_one);
 
-		rtlpriv->cfg->ops->set_desc((u8 *)pdesc, false, HW_DESC_RXOWN,
+		rtlpriv->cfg->ops->set_desc(hw, (u8 *)pdesc, false, HW_DESC_RXOWN,
 					    &tmp_one);
 
 		index = (index + 1) % rtlpci->rxringcount;
@@ -983,6 +983,8 @@ static void _rtl_pci_prepare_bcn_tasklet(struct ieee80211_hw *hw)
 	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));
@@ -1004,11 +1006,12 @@ static void _rtl_pci_prepare_bcn_tasklet(struct ieee80211_hw *hw)
 	info = IEEE80211_SKB_CB(pskb);
 	pdesc = &ring->desc[0];
 	rtlpriv->cfg->ops->fill_tx_desc(hw, hdr, (u8 *) pdesc,
-		info, NULL, pskb, BEACON_QUEUE, &tcb_desc);
+					(u8 *)pbuffer_desc, info, NULL, pskb,
+					BEACON_QUEUE, &tcb_desc);
 
 	__skb_queue_tail(&ring->queue, pskb);
 
-	rtlpriv->cfg->ops->set_desc((u8 *) pdesc, true, HW_DESC_OWN,
+	rtlpriv->cfg->ops->set_desc(hw, (u8 *)pdesc, true, HW_DESC_OWN,
 				    &temp_one);
 
 	return;
@@ -1066,7 +1069,7 @@ static void _rtl_pci_init_struct(struct ieee80211_hw *hw,
 	mac->current_ampdu_factor = 3;
 
 	/*QOS*/
-	rtlpci->acm_method = eAcmWay2_SW;
+	rtlpci->acm_method = EACMWAY2_SW;
 
 	/*task */
 	tasklet_init(&rtlpriv->works.irq_tasklet,
@@ -1113,7 +1116,7 @@ static int _rtl_pci_init_tx_ring(struct ieee80211_hw *hw,
 					      ((i + 1) % entries) *
 					      sizeof(*ring);
 
-		rtlpriv->cfg->ops->set_desc((u8 *)&(ring[i]),
+		rtlpriv->cfg->ops->set_desc(hw, (u8 *)&(ring[i]),
 					    true, HW_DESC_TX_NEXTDESC_ADDR,
 					    (u8 *)&nextdescaddress);
 	}
@@ -1188,19 +1191,19 @@ static int _rtl_pci_init_rx_ring(struct ieee80211_hw *hw)
 				dev_kfree_skb_any(skb);
 				return 1;
 			}
-			rtlpriv->cfg->ops->set_desc((u8 *)entry, false,
+			rtlpriv->cfg->ops->set_desc(hw, (u8 *)entry, false,
 						    HW_DESC_RXBUFF_ADDR,
 						    (u8 *)&bufferaddress);
-			rtlpriv->cfg->ops->set_desc((u8 *)entry, false,
+			rtlpriv->cfg->ops->set_desc(hw, (u8 *)entry, false,
 						    HW_DESC_RXPKT_LEN,
 						    (u8 *)&rtlpci->
 						    rxbuffersize);
-			rtlpriv->cfg->ops->set_desc((u8 *) entry, false,
+			rtlpriv->cfg->ops->set_desc(hw, (u8 *)entry, false,
 						    HW_DESC_RXOWN,
 						    &tmp_one);
 		}
 
-		rtlpriv->cfg->ops->set_desc((u8 *) entry, false,
+		rtlpriv->cfg->ops->set_desc(hw, (u8 *)entry, false,
 					    HW_DESC_RXERO, &tmp_one);
 	}
 	return 0;
@@ -1331,7 +1334,7 @@ int rtl_pci_reset_trx_ring(struct ieee80211_hw *hw)
 
 			for (i = 0; i < rtlpci->rxringcount; i++) {
 				entry = &rtlpci->rx_ring[rx_queue_idx].desc[i];
-				rtlpriv->cfg->ops->set_desc((u8 *) entry,
+				rtlpriv->cfg->ops->set_desc(hw, (u8 *)entry,
 							    false,
 							    HW_DESC_RXOWN,
 							    &tmp_one);
@@ -1424,6 +1427,7 @@ static int rtl_pci_tx(struct ieee80211_hw *hw,
 	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;
 	u8 idx;
 	u8 hw_queue = _rtl_mac_to_hwqueue(hw, skb);
 	unsigned long flags;
@@ -1464,17 +1468,22 @@ static int rtl_pci_tx(struct ieee80211_hw *hw,
 		idx = 0;
 
 	pdesc = &ring->desc[idx];
-	own = (u8) rtlpriv->cfg->ops->get_desc((u8 *) pdesc,
-			true, HW_DESC_OWN);
+	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(rtlpriv, 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));
+		if ((own == 1) && (hw_queue != BEACON_QUEUE)) {
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+				 "No more TX desc@%d, ring->idx = %d, idx = %d, skb_queue_len = 0x%d\n",
+				 hw_queue, ring->idx, idx,
+				 skb_queue_len(&ring->queue));
 
-		spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
-		return skb->len;
+			spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock,
+					       flags);
+			return skb->len;
+		}
 	}
 
 	if (ieee80211_is_data_qos(fc)) {
@@ -1494,17 +1503,20 @@ static int rtl_pci_tx(struct ieee80211_hw *hw,
 		rtlpriv->cfg->ops->led_control(hw, LED_CTL_TX);
 
 	rtlpriv->cfg->ops->fill_tx_desc(hw, hdr, (u8 *)pdesc,
-			info, sta, skb, hw_queue, ptcb_desc);
+			(u8 *)ptx_bd_desc, info, sta, skb, hw_queue, ptcb_desc);
 
 	__skb_queue_tail(&ring->queue, skb);
 
-	rtlpriv->cfg->ops->set_desc((u8 *)pdesc, true,
-				    HW_DESC_OWN, &temp_one);
-
+	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(rtlpriv, 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,
diff --git a/drivers/net/wireless/rtlwifi/pci.h b/drivers/net/wireless/rtlwifi/pci.h
index d3262ec45d23..90174a814a6d 100644
--- a/drivers/net/wireless/rtlwifi/pci.h
+++ b/drivers/net/wireless/rtlwifi/pci.h
@@ -137,12 +137,22 @@ struct rtl_tx_cmd_desc {
 	u32 dword[16];
 } __packed;
 
+/* In new TRX flow, Buffer_desc is new concept
+ * But TX wifi info == TX descriptor in old flow
+ * RX wifi info == RX descriptor in old flow
+ */
+struct rtl_tx_buffer_desc {
+	u32 dword[8]; /*seg = 4*/
+} __packed;
+
 struct rtl8192_tx_ring {
 	struct rtl_tx_desc *desc;
 	dma_addr_t dma;
 	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*/
 };
 
 struct rtl8192_rx_ring {
@@ -199,6 +209,10 @@ struct rtl_pci {
 
 	u16 shortretry_limit;
 	u16 longretry_limit;
+
+	/* MSI support */
+	bool msi_support;
+	bool using_msi;
 };
 
 struct mp_adapter {
diff --git a/drivers/net/wireless/rtlwifi/ps.c b/drivers/net/wireless/rtlwifi/ps.c
index deedae3c5449..de7f05f848ef 100644
--- a/drivers/net/wireless/rtlwifi/ps.c
+++ b/drivers/net/wireless/rtlwifi/ps.c
@@ -32,6 +32,106 @@
 #include "base.h"
 #include "ps.h"
 
+/*	Description:
+ *		This routine deals with the Power Configuration CMD
+ *		 parsing for RTL8723/RTL8188E Series IC.
+ *	Assumption:
+ *		We should follow specific format that was released from HW SD.
+ */
+bool rtl_hal_pwrseqcmdparsing(struct rtl_priv *rtlpriv, u8 cut_version,
+			      u8 faversion, u8 interface_type,
+			      struct wlan_pwr_cfg pwrcfgcmd[])
+{
+	struct wlan_pwr_cfg cfg_cmd = {0};
+	bool polling_bit = false;
+	u32 ary_idx = 0;
+	u8 value = 0;
+	u32 offset = 0;
+	u32 polling_count = 0;
+	u32 max_polling_cnt = 5000;
+
+	do {
+		cfg_cmd = pwrcfgcmd[ary_idx];
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			"rtl_hal_pwrseqcmdparsing(): offset(%#x),cut_msk(%#x), famsk(%#x),"
+			"interface_msk(%#x), base(%#x), cmd(%#x), msk(%#x), value(%#x)\n",
+			GET_PWR_CFG_OFFSET(cfg_cmd),
+					   GET_PWR_CFG_CUT_MASK(cfg_cmd),
+			GET_PWR_CFG_FAB_MASK(cfg_cmd),
+					     GET_PWR_CFG_INTF_MASK(cfg_cmd),
+			GET_PWR_CFG_BASE(cfg_cmd), GET_PWR_CFG_CMD(cfg_cmd),
+			GET_PWR_CFG_MASK(cfg_cmd), GET_PWR_CFG_VALUE(cfg_cmd));
+
+		if ((GET_PWR_CFG_FAB_MASK(cfg_cmd)&faversion) &&
+		    (GET_PWR_CFG_CUT_MASK(cfg_cmd)&cut_version) &&
+		    (GET_PWR_CFG_INTF_MASK(cfg_cmd)&interface_type)) {
+			switch (GET_PWR_CFG_CMD(cfg_cmd)) {
+			case PWR_CMD_READ:
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+					"rtl_hal_pwrseqcmdparsing(): PWR_CMD_READ\n");
+				break;
+			case PWR_CMD_WRITE:
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+					"rtl_hal_pwrseqcmdparsing(): PWR_CMD_WRITE\n");
+				offset = GET_PWR_CFG_OFFSET(cfg_cmd);
+
+				/*Read the value from system register*/
+				value = rtl_read_byte(rtlpriv, offset);
+				value &= (~(GET_PWR_CFG_MASK(cfg_cmd)));
+				value |= (GET_PWR_CFG_VALUE(cfg_cmd) &
+					  GET_PWR_CFG_MASK(cfg_cmd));
+
+				/*Write the value back to sytem register*/
+				rtl_write_byte(rtlpriv, offset, value);
+				break;
+			case PWR_CMD_POLLING:
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+					"rtl_hal_pwrseqcmdparsing(): PWR_CMD_POLLING\n");
+				polling_bit = false;
+				offset = GET_PWR_CFG_OFFSET(cfg_cmd);
+
+				do {
+					value = rtl_read_byte(rtlpriv, offset);
+
+					value &= GET_PWR_CFG_MASK(cfg_cmd);
+					if (value ==
+					    (GET_PWR_CFG_VALUE(cfg_cmd)
+					    & GET_PWR_CFG_MASK(cfg_cmd)))
+						polling_bit = true;
+					else
+						udelay(10);
+
+					if (polling_count++ > max_polling_cnt)
+						return false;
+				} while (!polling_bit);
+				break;
+			case PWR_CMD_DELAY:
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+					"rtl_hal_pwrseqcmdparsing(): PWR_CMD_DELAY\n");
+				if (GET_PWR_CFG_VALUE(cfg_cmd) ==
+				    PWRSEQ_DELAY_US)
+					udelay(GET_PWR_CFG_OFFSET(cfg_cmd));
+				else
+					mdelay(GET_PWR_CFG_OFFSET(cfg_cmd));
+				break;
+			case PWR_CMD_END:
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+					 "rtl_hal_pwrseqcmdparsing(): PWR_CMD_END\n");
+				return true;
+			default:
+				RT_ASSERT(false,
+					 "rtl_hal_pwrseqcmdparsing(): Unknown CMD!!\n");
+				break;
+			}
+
+		}
+		ary_idx++;
+	} while (1);
+
+	return true;
+}
+EXPORT_SYMBOL(rtl_hal_pwrseqcmdparsing);
+
 bool rtl_ps_enable_nic(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -48,7 +148,7 @@ bool rtl_ps_enable_nic(struct ieee80211_hw *hw)
 
 	/*<2> Enable Adapter */
 	if (rtlpriv->cfg->ops->hw_init(hw))
-		return 1;
+		return false;
 	RT_CLEAR_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
 
 	/*<3> Enable Interrupt */
diff --git a/drivers/net/wireless/rtlwifi/ps.h b/drivers/net/wireless/rtlwifi/ps.h
index 88bd76ea88f7..3bd41f958974 100644
--- a/drivers/net/wireless/rtlwifi/ps.h
+++ b/drivers/net/wireless/rtlwifi/ps.h
@@ -32,6 +32,66 @@
 
 #define MAX_SW_LPS_SLEEP_INTV	5
 
+/*---------------------------------------------
+ * 3 The value of cmd: 4 bits
+ *---------------------------------------------
+ */
+#define    PWR_CMD_READ		0x00
+#define    PWR_CMD_WRITE	0x01
+#define    PWR_CMD_POLLING	0x02
+#define    PWR_CMD_DELAY	0x03
+#define    PWR_CMD_END		0x04
+
+/* define the base address of each block */
+#define	PWR_BASEADDR_MAC	0x00
+#define	PWR_BASEADDR_USB	0x01
+#define	PWR_BASEADDR_PCIE	0x02
+#define	PWR_BASEADDR_SDIO	0x03
+
+#define	PWR_FAB_ALL_MSK		(BIT(0)|BIT(1)|BIT(2)|BIT(3))
+#define	PWR_CUT_TESTCHIP_MSK	BIT(0)
+#define	PWR_CUT_A_MSK		BIT(1)
+#define	PWR_CUT_B_MSK		BIT(2)
+#define	PWR_CUT_C_MSK		BIT(3)
+#define	PWR_CUT_D_MSK		BIT(4)
+#define	PWR_CUT_E_MSK		BIT(5)
+#define	PWR_CUT_F_MSK		BIT(6)
+#define	PWR_CUT_G_MSK		BIT(7)
+#define	PWR_CUT_ALL_MSK		0xFF
+#define PWR_INTF_SDIO_MSK	BIT(0)
+#define PWR_INTF_USB_MSK	BIT(1)
+#define PWR_INTF_PCI_MSK	BIT(2)
+#define PWR_INTF_ALL_MSK	(BIT(0)|BIT(1)|BIT(2)|BIT(3))
+
+enum pwrseq_delay_unit {
+	PWRSEQ_DELAY_US,
+	PWRSEQ_DELAY_MS,
+};
+
+struct wlan_pwr_cfg {
+	u16 offset;
+	u8 cut_msk;
+	u8 fab_msk:4;
+	u8 interface_msk:4;
+	u8 base:4;
+	u8 cmd:4;
+	u8 msk;
+	u8 value;
+};
+
+#define	GET_PWR_CFG_OFFSET(__PWR_CMD)	(__PWR_CMD.offset)
+#define	GET_PWR_CFG_CUT_MASK(__PWR_CMD)	(__PWR_CMD.cut_msk)
+#define	GET_PWR_CFG_FAB_MASK(__PWR_CMD)	(__PWR_CMD.fab_msk)
+#define	GET_PWR_CFG_INTF_MASK(__PWR_CMD)	(__PWR_CMD.interface_msk)
+#define	GET_PWR_CFG_BASE(__PWR_CMD)	(__PWR_CMD.base)
+#define	GET_PWR_CFG_CMD(__PWR_CMD)	(__PWR_CMD.cmd)
+#define	GET_PWR_CFG_MASK(__PWR_CMD)	(__PWR_CMD.msk)
+#define	GET_PWR_CFG_VALUE(__PWR_CMD)	(__PWR_CMD.value)
+
+bool rtl_hal_pwrseqcmdparsing(struct rtl_priv *rtlpriv, u8 cut_version,
+			      u8 fab_version, u8 interface_type,
+			      struct wlan_pwr_cfg pwrcfgcmd[]);
+
 bool rtl_ps_set_rf_state(struct ieee80211_hw *hw,
 			 enum rf_pwrstate state_toset, u32 changesource);
 bool rtl_ps_enable_nic(struct ieee80211_hw *hw);
diff --git a/drivers/net/wireless/rtlwifi/rc.c b/drivers/net/wireless/rtlwifi/rc.c
index a98acefb8c06..ee28a1a3d010 100644
--- a/drivers/net/wireless/rtlwifi/rc.c
+++ b/drivers/net/wireless/rtlwifi/rc.c
@@ -260,8 +260,7 @@ static void rtl_rate_free_sta(void *rtlpriv,
 	kfree(rate_priv);
 }
 
-static struct rate_control_ops rtl_rate_ops = {
-	.module = NULL,
+static const struct rate_control_ops rtl_rate_ops = {
 	.name = "rtl_rc",
 	.alloc = rtl_rate_alloc,
 	.free = rtl_rate_free,
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/Makefile b/drivers/net/wireless/rtlwifi/rtl8188ee/Makefile
index 5b194e97f4b3..a85419a37651 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/Makefile
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/Makefile
@@ -5,7 +5,6 @@ rtl8188ee-objs :=		\
 		led.o		\
 		phy.o		\
 		pwrseq.o	\
-		pwrseqcmd.o	\
 		rf.o		\
 		sw.o		\
 		table.o		\
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/dm.c b/drivers/net/wireless/rtlwifi/rtl8188ee/dm.c
index a6184b6e1d57..97bc9aa5e1d1 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/dm.c
@@ -235,7 +235,7 @@ void rtl88e_dm_txpower_track_adjust(struct ieee80211_hw *hw,
 	u8 pwr_val = 0;
 	u8 cck_base = rtldm->swing_idx_cck_base;
 	u8 cck_val = rtldm->swing_idx_cck;
-	u8 ofdm_base = rtldm->swing_idx_ofdm_base;
+	u8 ofdm_base = rtldm->swing_idx_ofdm_base[0];
 	u8 ofdm_val = rtlpriv->dm.swing_idx_ofdm[RF90_PATH_A];
 
 	if (type == 0) {
@@ -726,7 +726,7 @@ static void rtl88e_dm_pwdb_monitor(struct ieee80211_hw *hw)
 	static u64 last_rx;
 	long tmp_entry_max_pwdb = 0, tmp_entry_min_pwdb = 0xff;
 
-	if (rtlhal->oem_id == RT_CID_819x_HP) {
+	if (rtlhal->oem_id == RT_CID_819X_HP) {
 		u64 cur_txok_cnt = 0;
 		u64 cur_rxok_cnt = 0;
 		cur_txok_cnt = rtlpriv->stats.txbytesunicast - last_txok;
@@ -912,7 +912,7 @@ static void rtl88e_dm_txpower_tracking_callback_thermalmeter(struct ieee80211_hw
 	for (i = 0; i < OFDM_TABLE_LENGTH; i++) {
 		if (ele_d == (ofdmswing_table[i] & MASKOFDM_D)) {
 			ofdm_old[0] = (u8) i;
-			rtldm->swing_idx_ofdm_base = (u8)i;
+			rtldm->swing_idx_ofdm_base[0] = (u8)i;
 			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
 				 "Initial pathA ele_d reg0x%x = 0x%lx, ofdm_index = 0x%x\n",
 				 ROFDM0_XATXIQIMBAL,
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/hw.c b/drivers/net/wireless/rtlwifi/rtl8188ee/hw.c
index e06971be7df7..bd2a26bafb69 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/hw.c
@@ -41,7 +41,6 @@
 #include "fw.h"
 #include "led.h"
 #include "hw.h"
-#include "pwrseqcmd.h"
 #include "pwrseq.h"
 
 #define LLT_CONFIG		5
@@ -509,7 +508,7 @@ void rtl88ee_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 		u8 e_aci = *((u8 *)val);
 		rtl88e_dm_init_edca_turbo(hw);
 
-		if (rtlpci->acm_method != eAcmWay2_SW)
+		if (rtlpci->acm_method != EACMWAY2_SW)
 			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ACM_CTRL,
 						      (u8 *)(&e_aci));
 		break; }
@@ -815,11 +814,11 @@ static bool _rtl88ee_init_mac(struct ieee80211_hw *hw)
 
 	rtl_write_byte(rtlpriv, REG_RSV_CTRL, 0x00);
 	/* HW Power on sequence */
-	if (!rtl88_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK,
-					PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,
-					Rtl8188E_NIC_ENABLE_FLOW)) {
+	if (!rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK,
+				      PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,
+				      Rtl8188E_NIC_ENABLE_FLOW)) {
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-			 "init MAC Fail as rtl88_hal_pwrseqcmdparsing\n");
+			 "init MAC Fail as rtl_hal_pwrseqcmdparsing\n");
 		return false;
 	}
 
@@ -1025,9 +1024,20 @@ int rtl88ee_hw_init(struct ieee80211_hw *hw)
 	bool rtstatus = true;
 	int err = 0;
 	u8 tmp_u1b, u1byte;
+	unsigned long flags;
 
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "Rtl8188EE hw init\n");
 	rtlpriv->rtlhal.being_init_adapter = true;
+	/* As this function can take a very long time (up to 350 ms)
+	 * and can be called with irqs disabled, reenable the irqs
+	 * to let the other devices continue being serviced.
+	 *
+	 * It is safe doing so since our own interrupts will only be enabled
+	 * in a subsequent step.
+	 */
+	local_save_flags(flags);
+	local_irq_enable();
+
 	rtlpriv->intf_ops->disable_aspm(hw);
 
 	tmp_u1b = rtl_read_byte(rtlpriv, REG_SYS_CLKR+1);
@@ -1043,7 +1053,7 @@ int rtl88ee_hw_init(struct ieee80211_hw *hw)
 	if (rtstatus != true) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "Init MAC failed\n");
 		err = 1;
-		return err;
+		goto exit;
 	}
 
 	err = rtl88e_download_fw(hw, false);
@@ -1051,8 +1061,7 @@ int rtl88ee_hw_init(struct ieee80211_hw *hw)
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
 			 "Failed to download FW. Init HW without FW now..\n");
 		err = 1;
-		rtlhal->fw_ready = false;
-		return err;
+		goto exit;
 	} else {
 		rtlhal->fw_ready = true;
 	}
@@ -1097,7 +1106,7 @@ int rtl88ee_hw_init(struct ieee80211_hw *hw)
 	if (ppsc->rfpwr_state == ERFON) {
 		if ((rtlefuse->antenna_div_type == CGCS_RX_HW_ANTDIV) ||
 		    ((rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV) &&
-		    (rtlhal->oem_id == RT_CID_819x_HP))) {
+		    (rtlhal->oem_id == RT_CID_819X_HP))) {
 			rtl88e_phy_set_rfpath_switch(hw, true);
 			rtlpriv->dm.fat_table.rx_idle_ant = MAIN_ANT;
 		} else {
@@ -1135,10 +1144,12 @@ int rtl88ee_hw_init(struct ieee80211_hw *hw)
 	}
 	rtl_write_byte(rtlpriv, REG_NAV_CTRL+2,  ((30000+127)/128));
 	rtl88e_dm_init(hw);
+exit:
+	local_irq_restore(flags);
 	rtlpriv->rtlhal.being_init_adapter = false;
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "end of Rtl8188EE hw init %x\n",
 		 err);
-	return 0;
+	return err;
 }
 
 static enum version_8188e _rtl88ee_read_chip_version(struct ieee80211_hw *hw)
@@ -1235,12 +1246,13 @@ static int _rtl88ee_set_media_status(struct ieee80211_hw *hw,
 void rtl88ee_set_check_bssid(struct ieee80211_hw *hw, bool check_bssid)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	u32 reg_rcr = rtlpci->receive_config;
+	u32 reg_rcr;
 
 	if (rtlpriv->psc.rfpwr_state != ERFON)
 		return;
 
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_RCR, (u8 *)(&reg_rcr));
+
 	if (check_bssid == true) {
 		reg_rcr |= (RCR_CBSSID_DATA | RCR_CBSSID_BCN);
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_RCR,
@@ -1345,9 +1357,9 @@ static void _rtl88ee_poweroff_adapter(struct ieee80211_hw *hw)
 	}
 	rtl_write_byte(rtlpriv, REG_PCIE_CTRL_REG+1, 0xFF);
 
-	rtl88_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
-				   PWR_INTF_PCI_MSK,
-				   Rtl8188E_NIC_LPS_ENTER_FLOW);
+	rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
+				 PWR_INTF_PCI_MSK,
+				 Rtl8188E_NIC_LPS_ENTER_FLOW);
 
 	rtl_write_byte(rtlpriv, REG_RF_CTRL, 0x00);
 
@@ -1361,8 +1373,8 @@ static void _rtl88ee_poweroff_adapter(struct ieee80211_hw *hw)
 	u1b_tmp = rtl_read_byte(rtlpriv, REG_32K_CTRL);
 	rtl_write_byte(rtlpriv, REG_32K_CTRL, (u1b_tmp & (~BIT(0))));
 
-	rtl88_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
-				   PWR_INTF_PCI_MSK, Rtl8188E_NIC_DISABLE_FLOW);
+	rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
+				 PWR_INTF_PCI_MSK, Rtl8188E_NIC_DISABLE_FLOW);
 
 	u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL+1);
 	rtl_write_byte(rtlpriv, REG_RSV_CTRL+1, (u1b_tmp & (~BIT(3))));
@@ -1872,15 +1884,15 @@ static void _rtl88ee_read_adapter_info(struct ieee80211_hw *hw)
 		case EEPROM_CID_DEFAULT:
 			if (rtlefuse->eeprom_did == 0x8179) {
 				if (rtlefuse->eeprom_svid == 0x1025) {
-					rtlhal->oem_id = RT_CID_819x_Acer;
+					rtlhal->oem_id = RT_CID_819X_ACER;
 				} else if ((rtlefuse->eeprom_svid == 0x10EC &&
 					    rtlefuse->eeprom_smid == 0x0179) ||
 					    (rtlefuse->eeprom_svid == 0x17AA &&
 					    rtlefuse->eeprom_smid == 0x0179)) {
-					rtlhal->oem_id = RT_CID_819x_Lenovo;
+					rtlhal->oem_id = RT_CID_819X_LENOVO;
 				} else if (rtlefuse->eeprom_svid == 0x103c &&
 					 rtlefuse->eeprom_smid == 0x197d) {
-					rtlhal->oem_id = RT_CID_819x_HP;
+					rtlhal->oem_id = RT_CID_819X_HP;
 				} else {
 					rtlhal->oem_id = RT_CID_DEFAULT;
 				}
@@ -1892,7 +1904,7 @@ static void _rtl88ee_read_adapter_info(struct ieee80211_hw *hw)
 			rtlhal->oem_id = RT_CID_TOSHIBA;
 			break;
 		case EEPROM_CID_QMI:
-			rtlhal->oem_id = RT_CID_819x_QMI;
+			rtlhal->oem_id = RT_CID_819X_QMI;
 			break;
 		case EEPROM_CID_WHQL:
 		default:
@@ -1911,14 +1923,14 @@ static void _rtl88ee_hal_customized_behavior(struct ieee80211_hw *hw)
 	pcipriv->ledctl.led_opendrain = true;
 
 	switch (rtlhal->oem_id) {
-	case RT_CID_819x_HP:
+	case RT_CID_819X_HP:
 		pcipriv->ledctl.led_opendrain = true;
 		break;
-	case RT_CID_819x_Lenovo:
+	case RT_CID_819X_LENOVO:
 	case RT_CID_DEFAULT:
 	case RT_CID_TOSHIBA:
 	case RT_CID_CCX:
-	case RT_CID_819x_Acer:
+	case RT_CID_819X_ACER:
 	case RT_CID_WHQL:
 	default:
 		break;
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/phy.c b/drivers/net/wireless/rtlwifi/rtl8188ee/phy.c
index d67f9c731cc4..1cd6c16d597e 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/phy.c
@@ -29,6 +29,7 @@
 
 #include "../wifi.h"
 #include "../pci.h"
+#include "../core.h"
 #include "../ps.h"
 #include "reg.h"
 #include "def.h"
@@ -151,18 +152,7 @@ static bool config_bb_with_pgheader(struct ieee80211_hw *hw,
 			v2 = table_pg[i + 1];
 
 			if (v1 < 0xcdcdcdcd) {
-				if (table_pg[i] == 0xfe)
-					mdelay(50);
-				else if (table_pg[i] == 0xfd)
-					mdelay(5);
-				else if (table_pg[i] == 0xfc)
-					mdelay(1);
-				else if (table_pg[i] == 0xfb)
-					udelay(50);
-				else if (table_pg[i] == 0xfa)
-					udelay(5);
-				else if (table_pg[i] == 0xf9)
-					udelay(1);
+				rtl_addr_delay(table_pg[i]);
 
 				store_pwrindex_offset(hw, table_pg[i],
 						      table_pg[i + 1],
@@ -672,24 +662,9 @@ static void _rtl8188e_config_rf_reg(struct ieee80211_hw *hw,
 				    u32 addr, u32 data, enum radio_path rfpath,
 				    u32 regaddr)
 {
-	if (addr == 0xffe) {
-		mdelay(50);
-	} else if (addr == 0xfd) {
-		mdelay(5);
-	} else if (addr == 0xfc) {
-		mdelay(1);
-	} else if (addr == 0xfb) {
-		udelay(50);
-	} else if (addr == 0xfa) {
-		udelay(5);
-	} else if (addr == 0xf9) {
-		udelay(1);
-	} else {
-		rtl_set_rfreg(hw, rfpath, regaddr,
-			      RFREG_OFFSET_MASK,
-			      data);
-		udelay(1);
-	}
+	rtl_rfreg_delay(hw, rfpath, regaddr,
+			RFREG_OFFSET_MASK,
+			data);
 }
 
 static void rtl88_config_s(struct ieee80211_hw *hw,
@@ -702,28 +677,6 @@ static void rtl88_config_s(struct ieee80211_hw *hw,
 				addr | maskforphyset);
 }
 
-static void _rtl8188e_config_bb_reg(struct ieee80211_hw *hw,
-				    u32 addr, u32 data)
-{
-	if (addr == 0xfe) {
-		mdelay(50);
-	} else if (addr == 0xfd) {
-		mdelay(5);
-	} else if (addr == 0xfc) {
-		mdelay(1);
-	} else if (addr == 0xfb) {
-		udelay(50);
-	} else if (addr == 0xfa) {
-		udelay(5);
-	} else if (addr == 0xf9) {
-		udelay(1);
-	} else {
-		rtl_set_bbreg(hw, addr, MASKDWORD, data);
-		udelay(1);
-	}
-}
-
-
 #define NEXT_PAIR(v1, v2, i)				\
 	do {						\
 		i += 2; v1 = array_table[i];		\
@@ -795,7 +748,7 @@ static void set_baseband_phy_config(struct ieee80211_hw *hw)
 		v1 = array_table[i];
 		v2 = array_table[i + 1];
 		if (v1 < 0xcdcdcdcd) {
-			_rtl8188e_config_bb_reg(hw, v1, v2);
+			rtl_bb_delay(hw, v1, v2);
 		} else {/*This line is the start line of branch.*/
 			if (!check_cond(hw, array_table[i])) {
 				/*Discard the following (offset, data) pairs*/
@@ -811,7 +764,7 @@ static void set_baseband_phy_config(struct ieee80211_hw *hw)
 				while (v2 != 0xDEAD &&
 				       v2 != 0xCDEF &&
 				       v2 != 0xCDCD && i < arraylen - 2) {
-					_rtl8188e_config_bb_reg(hw, v1, v2);
+					rtl_bb_delay(hw, v1, v2);
 					NEXT_PAIR(v1, v2, i);
 				}
 
@@ -1002,7 +955,7 @@ bool rtl88e_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
 			}
 		}
 
-		if (rtlhal->oem_id == RT_CID_819x_HP)
+		if (rtlhal->oem_id == RT_CID_819X_HP)
 			rtl88_config_s(hw, 0x52, 0x7E4BD);
 
 		break;
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseq.h b/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseq.h
index 028ec6dd52b4..32e135ab9a63 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseq.h
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseq.h
@@ -30,7 +30,6 @@
 #ifndef __RTL8723E_PWRSEQ_H__
 #define __RTL8723E_PWRSEQ_H__
 
-#include "pwrseqcmd.h"
 /*
 	Check document WM-20110607-Paul-RTL8188E_Power_Architecture-R02.vsd
 	There are 6 HW Power States:
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/reg.h b/drivers/net/wireless/rtlwifi/rtl8188ee/reg.h
index d849abf7d94a..7af85cfa8f87 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/reg.h
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/reg.h
@@ -2215,22 +2215,6 @@
 #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
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/trx.c b/drivers/net/wireless/rtlwifi/rtl8188ee/trx.c
index aece6c9cccf1..2ba6f510d884 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/trx.c
@@ -452,7 +452,7 @@ bool rtl88ee_rx_query_desc(struct ieee80211_hw *hw,
 			/* During testing, hdr was NULL */
 			return false;
 		}
-		if ((ieee80211_is_robust_mgmt_frame(hdr)) &&
+		if ((_ieee80211_is_robust_mgmt_frame(hdr)) &&
 		    (ieee80211_has_protected(hdr->frame_control)))
 			rx_status->flag &= ~RX_FLAG_DECRYPTED;
 		else
@@ -489,9 +489,8 @@ bool rtl88ee_rx_query_desc(struct ieee80211_hw *hw,
 
 void rtl88ee_tx_fill_desc(struct ieee80211_hw *hw,
 			  struct ieee80211_hdr *hdr, u8 *pdesc_tx,
-			  struct ieee80211_tx_info *info,
-			  struct ieee80211_sta *sta,
-			  struct sk_buff *skb,
+			  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);
@@ -734,7 +733,8 @@ void rtl88ee_tx_fill_cmddesc(struct ieee80211_hw *hw,
 		      pdesc, TX_DESC_SIZE);
 }
 
-void rtl88ee_set_desc(u8 *pdesc, bool istx, u8 desc_name, u8 *val)
+void rtl88ee_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
+		      u8 desc_name, u8 *val)
 {
 	if (istx == true) {
 		switch (desc_name) {
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/trx.h b/drivers/net/wireless/rtlwifi/rtl8188ee/trx.h
index 21ca33a7c770..8c2609412d2c 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/trx.h
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/trx.h
@@ -777,15 +777,15 @@ struct rx_desc_88e {
 
 void rtl88ee_tx_fill_desc(struct ieee80211_hw *hw,
 			  struct ieee80211_hdr *hdr, u8 *pdesc_tx,
-			  struct ieee80211_tx_info *info,
-			  struct ieee80211_sta *sta,
-			  struct sk_buff *skb,
+			  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 rtl88ee_rx_query_desc(struct ieee80211_hw *hw,
 			   struct rtl_stats *status,
 			   struct ieee80211_rx_status *rx_status,
 			   u8 *pdesc, struct sk_buff *skb);
-void rtl88ee_set_desc(u8 *pdesc, bool istx, u8 desc_name, u8 *val);
+void rtl88ee_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
+		      u8 desc_name, u8 *val);
 u32 rtl88ee_get_desc(u8 *pdesc, bool istx, u8 desc_name);
 void rtl88ee_tx_polling(struct ieee80211_hw *hw, u8 hw_queue);
 void rtl88ee_tx_fill_cmddesc(struct ieee80211_hw *hw, u8 *pdesc,
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c b/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c
index a82b30a1996c..4ae51d5b436f 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c
@@ -319,7 +319,7 @@ void rtl92ce_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			u8 e_aci = *(val);
 			rtl92c_dm_init_edca_turbo(hw);
 
-			if (rtlpci->acm_method != eAcmWay2_SW)
+			if (rtlpci->acm_method != EACMWAY2_SW)
 				rtlpriv->cfg->ops->set_hw_reg(hw,
 							      HW_VAR_ACM_CTRL,
 							      (&e_aci));
@@ -542,9 +542,11 @@ void rtl92ce_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 						(u8 *)(&fw_current_inps));
 			}
 		break; }
+	case HW_VAR_KEEP_ALIVE:
+		break;
 	default:
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "switch case not processed\n");
+			 "switch case %d not processed\n", variable);
 		break;
 	}
 }
@@ -937,14 +939,26 @@ int rtl92ce_hw_init(struct ieee80211_hw *hw)
 	bool is92c;
 	int err;
 	u8 tmp_u1b;
+	unsigned long flags;
 
 	rtlpci->being_init_adapter = true;
+
+	/* Since this function can take a very long time (up to 350 ms)
+	 * and can be called with irqs disabled, reenable the irqs
+	 * to let the other devices continue being serviced.
+	 *
+	 * It is safe doing so since our own interrupts will only be enabled
+	 * in a subsequent step.
+	 */
+	local_save_flags(flags);
+	local_irq_enable();
+
 	rtlpriv->intf_ops->disable_aspm(hw);
 	rtstatus = _rtl92ce_init_mac(hw);
 	if (!rtstatus) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "Init MAC failed\n");
 		err = 1;
-		return err;
+		goto exit;
 	}
 
 	err = rtl92c_download_fw(hw);
@@ -952,7 +966,7 @@ int rtl92ce_hw_init(struct ieee80211_hw *hw)
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
 			 "Failed to download FW. Init HW without FW now..\n");
 		err = 1;
-		return err;
+		goto exit;
 	}
 
 	rtlhal->last_hmeboxnum = 0;
@@ -1032,6 +1046,8 @@ int rtl92ce_hw_init(struct ieee80211_hw *hw)
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "under 1.5V\n");
 	}
 	rtl92c_dm_init(hw);
+exit:
+	local_irq_restore(flags);
 	rtlpci->being_init_adapter = false;
 	return err;
 }
@@ -1200,11 +1216,13 @@ static int _rtl92ce_set_media_status(struct ieee80211_hw *hw,
 void rtl92ce_set_check_bssid(struct ieee80211_hw *hw, bool check_bssid)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 reg_rcr = rtl_read_dword(rtlpriv, REG_RCR);
+	u32 reg_rcr;
 
 	if (rtlpriv->psc.rfpwr_state != ERFON)
 		return;
 
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_RCR, (u8 *)(&reg_rcr));
+
 	if (check_bssid) {
 		reg_rcr |= (RCR_CBSSID_DATA | RCR_CBSSID_BCN);
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_RCR,
@@ -1720,7 +1738,7 @@ static void _rtl92ce_read_adapter_info(struct ieee80211_hw *hw)
 			if (rtlefuse->eeprom_did == 0x8176) {
 				if ((rtlefuse->eeprom_svid == 0x103C &&
 				     rtlefuse->eeprom_smid == 0x1629))
-					rtlhal->oem_id = RT_CID_819x_HP;
+					rtlhal->oem_id = RT_CID_819X_HP;
 				else
 					rtlhal->oem_id = RT_CID_DEFAULT;
 			} else {
@@ -1731,7 +1749,7 @@ static void _rtl92ce_read_adapter_info(struct ieee80211_hw *hw)
 			rtlhal->oem_id = RT_CID_TOSHIBA;
 			break;
 		case EEPROM_CID_QMI:
-			rtlhal->oem_id = RT_CID_819x_QMI;
+			rtlhal->oem_id = RT_CID_819X_QMI;
 			break;
 		case EEPROM_CID_WHQL:
 		default:
@@ -1750,14 +1768,14 @@ static void _rtl92ce_hal_customized_behavior(struct ieee80211_hw *hw)
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 
 	switch (rtlhal->oem_id) {
-	case RT_CID_819x_HP:
+	case RT_CID_819X_HP:
 		pcipriv->ledctl.led_opendrain = true;
 		break;
-	case RT_CID_819x_Lenovo:
+	case RT_CID_819X_LENOVO:
 	case RT_CID_DEFAULT:
 	case RT_CID_TOSHIBA:
 	case RT_CID_CCX:
-	case RT_CID_819x_Acer:
+	case RT_CID_819X_ACER:
 	case RT_CID_WHQL:
 	default:
 		break;
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/phy.c b/drivers/net/wireless/rtlwifi/rtl8192ce/phy.c
index 73262ca3864b..98b22303c84d 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/phy.c
@@ -30,6 +30,7 @@
 #include "../wifi.h"
 #include "../pci.h"
 #include "../ps.h"
+#include "../core.h"
 #include "reg.h"
 #include "def.h"
 #include "hw.h"
@@ -198,18 +199,7 @@ bool _rtl92ce_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
 	}
 	if (configtype == BASEBAND_CONFIG_PHY_REG) {
 		for (i = 0; i < phy_reg_arraylen; i = i + 2) {
-			if (phy_regarray_table[i] == 0xfe)
-				mdelay(50);
-			else if (phy_regarray_table[i] == 0xfd)
-				mdelay(5);
-			else if (phy_regarray_table[i] == 0xfc)
-				mdelay(1);
-			else if (phy_regarray_table[i] == 0xfb)
-				udelay(50);
-			else if (phy_regarray_table[i] == 0xfa)
-				udelay(5);
-			else if (phy_regarray_table[i] == 0xf9)
-				udelay(1);
+			rtl_addr_delay(phy_regarray_table[i]);
 			rtl_set_bbreg(hw, phy_regarray_table[i], MASKDWORD,
 				      phy_regarray_table[i + 1]);
 			udelay(1);
@@ -245,18 +235,7 @@ bool _rtl92ce_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
 
 	if (configtype == BASEBAND_CONFIG_PHY_REG) {
 		for (i = 0; i < phy_regarray_pg_len; i = i + 3) {
-			if (phy_regarray_table_pg[i] == 0xfe)
-				mdelay(50);
-			else if (phy_regarray_table_pg[i] == 0xfd)
-				mdelay(5);
-			else if (phy_regarray_table_pg[i] == 0xfc)
-				mdelay(1);
-			else if (phy_regarray_table_pg[i] == 0xfb)
-				udelay(50);
-			else if (phy_regarray_table_pg[i] == 0xfa)
-				udelay(5);
-			else if (phy_regarray_table_pg[i] == 0xf9)
-				udelay(1);
+			rtl_addr_delay(phy_regarray_table_pg[i]);
 
 			_rtl92c_store_pwrIndex_diffrate_offset(hw,
 					       phy_regarray_table_pg[i],
@@ -305,46 +284,16 @@ bool rtl92c_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
 	switch (rfpath) {
 	case RF90_PATH_A:
 		for (i = 0; i < radioa_arraylen; i = i + 2) {
-			if (radioa_array_table[i] == 0xfe)
-				mdelay(50);
-			else if (radioa_array_table[i] == 0xfd)
-				mdelay(5);
-			else if (radioa_array_table[i] == 0xfc)
-				mdelay(1);
-			else if (radioa_array_table[i] == 0xfb)
-				udelay(50);
-			else if (radioa_array_table[i] == 0xfa)
-				udelay(5);
-			else if (radioa_array_table[i] == 0xf9)
-				udelay(1);
-			else {
-				rtl_set_rfreg(hw, rfpath, radioa_array_table[i],
-					      RFREG_OFFSET_MASK,
-					      radioa_array_table[i + 1]);
-				udelay(1);
-			}
+			rtl_rfreg_delay(hw, rfpath, radioa_array_table[i],
+					RFREG_OFFSET_MASK,
+					radioa_array_table[i + 1]);
 		}
 		break;
 	case RF90_PATH_B:
 		for (i = 0; i < radiob_arraylen; i = i + 2) {
-			if (radiob_array_table[i] == 0xfe) {
-				mdelay(50);
-			} else if (radiob_array_table[i] == 0xfd)
-				mdelay(5);
-			else if (radiob_array_table[i] == 0xfc)
-				mdelay(1);
-			else if (radiob_array_table[i] == 0xfb)
-				udelay(50);
-			else if (radiob_array_table[i] == 0xfa)
-				udelay(5);
-			else if (radiob_array_table[i] == 0xf9)
-				udelay(1);
-			else {
-				rtl_set_rfreg(hw, rfpath, radiob_array_table[i],
-					      RFREG_OFFSET_MASK,
-					      radiob_array_table[i + 1]);
-				udelay(1);
-			}
+			rtl_rfreg_delay(hw, rfpath, radiob_array_table[i],
+					RFREG_OFFSET_MASK,
+					radiob_array_table[i + 1]);
 		}
 		break;
 	case RF90_PATH_C:
@@ -355,6 +304,8 @@ bool rtl92c_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 			 "switch case not processed\n");
 		break;
+	default:
+		break;
 	}
 	return true;
 }
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/reg.h b/drivers/net/wireless/rtlwifi/rtl8192ce/reg.h
index 8922ecb47ad2..ed703a1b3b7c 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/reg.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/reg.h
@@ -2044,22 +2044,6 @@
 #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
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/trx.c b/drivers/net/wireless/rtlwifi/rtl8192ce/trx.c
index 52abf0a862fa..8f04817cb7ec 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/trx.c
@@ -393,7 +393,7 @@ bool rtl92ce_rx_query_desc(struct ieee80211_hw *hw,
 			/* In testing, hdr was NULL here */
 			return false;
 		}
-		if ((ieee80211_is_robust_mgmt_frame(hdr)) &&
+		if ((_ieee80211_is_robust_mgmt_frame(hdr)) &&
 		    (ieee80211_has_protected(hdr->frame_control)))
 			rx_status->flag &= ~RX_FLAG_DECRYPTED;
 		else
@@ -426,7 +426,7 @@ bool rtl92ce_rx_query_desc(struct ieee80211_hw *hw,
 
 void rtl92ce_tx_fill_desc(struct ieee80211_hw *hw,
 			  struct ieee80211_hdr *hdr, u8 *pdesc_tx,
-			  struct ieee80211_tx_info *info,
+			  u8 *pbd_desc_tx, struct ieee80211_tx_info *info,
 			  struct ieee80211_sta *sta,
 			  struct sk_buff *skb,
 			  u8 hw_queue, struct rtl_tcb_desc *tcb_desc)
@@ -666,7 +666,8 @@ void rtl92ce_tx_fill_cmddesc(struct ieee80211_hw *hw,
 		      "H2C Tx Cmd Content", pdesc, TX_DESC_SIZE);
 }
 
-void rtl92ce_set_desc(u8 *pdesc, bool istx, u8 desc_name, u8 *val)
+void rtl92ce_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
+		      u8 desc_name, u8 *val)
 {
 	if (istx) {
 		switch (desc_name) {
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/trx.h b/drivers/net/wireless/rtlwifi/rtl8192ce/trx.h
index a7cdd514cb2e..9a39ec4204dd 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/trx.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/trx.h
@@ -711,8 +711,8 @@ struct rx_desc_92c {
 } __packed;
 
 void rtl92ce_tx_fill_desc(struct ieee80211_hw *hw,
-			  struct ieee80211_hdr *hdr,
-			  u8 *pdesc, struct ieee80211_tx_info *info,
+			  struct ieee80211_hdr *hdr, u8 *pdesc,
+			  u8 *pbd_desc_tx, struct ieee80211_tx_info *info,
 			  struct ieee80211_sta *sta,
 			  struct sk_buff *skb, u8 hw_queue,
 			  struct rtl_tcb_desc *ptcb_desc);
@@ -720,7 +720,8 @@ bool rtl92ce_rx_query_desc(struct ieee80211_hw *hw,
 			   struct rtl_stats *stats,
 			   struct ieee80211_rx_status *rx_status,
 			   u8 *pdesc, struct sk_buff *skb);
-void rtl92ce_set_desc(u8 *pdesc, bool istx, u8 desc_name, u8 *val);
+void rtl92ce_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
+		      u8 desc_name, u8 *val);
 u32 rtl92ce_get_desc(u8 *pdesc, bool istx, u8 desc_name);
 void rtl92ce_tx_polling(struct ieee80211_hw *hw, u8 hw_queue);
 void rtl92ce_tx_fill_cmddesc(struct ieee80211_hw *hw, u8 *pdesc,
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c b/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
index 468bf73cc883..68b5c7e92cfb 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
@@ -394,7 +394,7 @@ static void _rtl92cu_read_adapter_info(struct ieee80211_hw *hw)
 			if (rtlefuse->eeprom_did == 0x8176) {
 				if ((rtlefuse->eeprom_svid == 0x103C &&
 				     rtlefuse->eeprom_smid == 0x1629))
-					rtlhal->oem_id = RT_CID_819x_HP;
+					rtlhal->oem_id = RT_CID_819X_HP;
 				else
 					rtlhal->oem_id = RT_CID_DEFAULT;
 			} else {
@@ -405,7 +405,7 @@ static void _rtl92cu_read_adapter_info(struct ieee80211_hw *hw)
 			rtlhal->oem_id = RT_CID_TOSHIBA;
 			break;
 		case EEPROM_CID_QMI:
-			rtlhal->oem_id = RT_CID_819x_QMI;
+			rtlhal->oem_id = RT_CID_819X_QMI;
 			break;
 		case EEPROM_CID_WHQL:
 		default:
@@ -423,14 +423,14 @@ static void _rtl92cu_hal_customized_behavior(struct ieee80211_hw *hw)
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 
 	switch (rtlhal->oem_id) {
-	case RT_CID_819x_HP:
+	case RT_CID_819X_HP:
 		usb_priv->ledctl.led_opendrain = true;
 		break;
-	case RT_CID_819x_Lenovo:
+	case RT_CID_819X_LENOVO:
 	case RT_CID_DEFAULT:
 	case RT_CID_TOSHIBA:
 	case RT_CID_CCX:
-	case RT_CID_819x_Acer:
+	case RT_CID_819X_ACER:
 	case RT_CID_WHQL:
 	default:
 		break;
@@ -985,6 +985,17 @@ int rtl92cu_hw_init(struct ieee80211_hw *hw)
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 	int err = 0;
 	static bool iqk_initialized;
+	unsigned long flags;
+
+	/* As this function can take a very long time (up to 350 ms)
+	 * and can be called with irqs disabled, reenable the irqs
+	 * to let the other devices continue being serviced.
+	 *
+	 * It is safe doing so since our own interrupts will only be enabled
+	 * in a subsequent step.
+	 */
+	local_save_flags(flags);
+	local_irq_enable();
 
 	rtlhal->hw_type = HARDWARE_TYPE_RTL8192CU;
 	err = _rtl92cu_init_mac(hw);
@@ -997,7 +1008,7 @@ int rtl92cu_hw_init(struct ieee80211_hw *hw)
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
 			 "Failed to download FW. Init HW without FW now..\n");
 		err = 1;
-		return err;
+		goto exit;
 	}
 	rtlhal->last_hmeboxnum = 0; /* h2c */
 	_rtl92cu_phy_param_tab_init(hw);
@@ -1034,6 +1045,8 @@ int rtl92cu_hw_init(struct ieee80211_hw *hw)
 	_InitPABias(hw);
 	_update_mac_setting(hw);
 	rtl92c_dm_init(hw);
+exit:
+	local_irq_restore(flags);
 	return err;
 }
 
@@ -1379,11 +1392,13 @@ void rtl92cu_set_check_bssid(struct ieee80211_hw *hw, bool check_bssid)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	u32 reg_rcr = rtl_read_dword(rtlpriv, REG_RCR);
+	u32 reg_rcr;
 
 	if (rtlpriv->psc.rfpwr_state != ERFON)
 		return;
 
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_RCR, (u8 *)(&reg_rcr));
+
 	if (check_bssid) {
 		u8 tmp;
 		if (IS_NORMAL_CHIP(rtlhal->version)) {
@@ -1795,7 +1810,7 @@ void rtl92cu_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 					  e_aci);
 				break;
 			}
-			if (rtlusb->acm_method != eAcmWay2_SW)
+			if (rtlusb->acm_method != EACMWAY2_SW)
 				rtlpriv->cfg->ops->set_hw_reg(hw,
 					 HW_VAR_ACM_CTRL, &e_aci);
 			break;
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/phy.c b/drivers/net/wireless/rtlwifi/rtl8192cu/phy.c
index 0c09240eadcc..9831ff1128ca 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/phy.c
@@ -30,6 +30,7 @@
 #include "../wifi.h"
 #include "../pci.h"
 #include "../ps.h"
+#include "../core.h"
 #include "reg.h"
 #include "def.h"
 #include "phy.h"
@@ -188,18 +189,7 @@ bool _rtl92cu_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
 	}
 	if (configtype == BASEBAND_CONFIG_PHY_REG) {
 		for (i = 0; i < phy_reg_arraylen; i = i + 2) {
-			if (phy_regarray_table[i] == 0xfe)
-				mdelay(50);
-			else if (phy_regarray_table[i] == 0xfd)
-				mdelay(5);
-			else if (phy_regarray_table[i] == 0xfc)
-				mdelay(1);
-			else if (phy_regarray_table[i] == 0xfb)
-				udelay(50);
-			else if (phy_regarray_table[i] == 0xfa)
-				udelay(5);
-			else if (phy_regarray_table[i] == 0xf9)
-				udelay(1);
+			rtl_addr_delay(phy_regarray_table[i]);
 			rtl_set_bbreg(hw, phy_regarray_table[i], MASKDWORD,
 				      phy_regarray_table[i + 1]);
 			udelay(1);
@@ -236,18 +226,7 @@ bool _rtl92cu_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
 	phy_regarray_table_pg = rtlphy->hwparam_tables[PHY_REG_PG].pdata;
 	if (configtype == BASEBAND_CONFIG_PHY_REG) {
 		for (i = 0; i < phy_regarray_pg_len; i = i + 3) {
-			if (phy_regarray_table_pg[i] == 0xfe)
-				mdelay(50);
-			else if (phy_regarray_table_pg[i] == 0xfd)
-				mdelay(5);
-			else if (phy_regarray_table_pg[i] == 0xfc)
-				mdelay(1);
-			else if (phy_regarray_table_pg[i] == 0xfb)
-				udelay(50);
-			else if (phy_regarray_table_pg[i] == 0xfa)
-				udelay(5);
-			else if (phy_regarray_table_pg[i] == 0xf9)
-				udelay(1);
+			rtl_addr_delay(phy_regarray_table_pg[i]);
 			_rtl92c_store_pwrIndex_diffrate_offset(hw,
 						  phy_regarray_table_pg[i],
 						  phy_regarray_table_pg[i + 1],
@@ -294,46 +273,16 @@ bool rtl92cu_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
 	switch (rfpath) {
 	case RF90_PATH_A:
 		for (i = 0; i < radioa_arraylen; i = i + 2) {
-			if (radioa_array_table[i] == 0xfe)
-				mdelay(50);
-			else if (radioa_array_table[i] == 0xfd)
-				mdelay(5);
-			else if (radioa_array_table[i] == 0xfc)
-				mdelay(1);
-			else if (radioa_array_table[i] == 0xfb)
-				udelay(50);
-			else if (radioa_array_table[i] == 0xfa)
-				udelay(5);
-			else if (radioa_array_table[i] == 0xf9)
-				udelay(1);
-			else {
-				rtl_set_rfreg(hw, rfpath, radioa_array_table[i],
-					      RFREG_OFFSET_MASK,
-					      radioa_array_table[i + 1]);
-				udelay(1);
-			}
+			rtl_rfreg_delay(hw, rfpath, radioa_array_table[i],
+					RFREG_OFFSET_MASK,
+					radioa_array_table[i + 1]);
 		}
 		break;
 	case RF90_PATH_B:
 		for (i = 0; i < radiob_arraylen; i = i + 2) {
-			if (radiob_array_table[i] == 0xfe) {
-				mdelay(50);
-			} else if (radiob_array_table[i] == 0xfd)
-				mdelay(5);
-			else if (radiob_array_table[i] == 0xfc)
-				mdelay(1);
-			else if (radiob_array_table[i] == 0xfb)
-				udelay(50);
-			else if (radiob_array_table[i] == 0xfa)
-				udelay(5);
-			else if (radiob_array_table[i] == 0xf9)
-				udelay(1);
-			else {
-				rtl_set_rfreg(hw, rfpath, radiob_array_table[i],
-					      RFREG_OFFSET_MASK,
-					      radiob_array_table[i + 1]);
-				udelay(1);
-			}
+			rtl_rfreg_delay(hw, rfpath, radiob_array_table[i],
+					RFREG_OFFSET_MASK,
+					radiob_array_table[i + 1]);
 		}
 		break;
 	case RF90_PATH_C:
@@ -344,6 +293,8 @@ bool rtl92cu_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 			 "switch case not processed\n");
 		break;
+	default:
+		break;
 	}
 	return true;
 }
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/trx.c b/drivers/net/wireless/rtlwifi/rtl8192cu/trx.c
index 1bc21ccfa71b..035e0dc3922c 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/trx.c
@@ -495,7 +495,7 @@ static void _rtl_tx_desc_checksum(u8 *txdesc)
 
 void rtl92cu_tx_fill_desc(struct ieee80211_hw *hw,
 			  struct ieee80211_hdr *hdr, u8 *pdesc_tx,
-			  struct ieee80211_tx_info *info,
+			  u8 *pbd_desc_tx, struct ieee80211_tx_info *info,
 			  struct ieee80211_sta *sta,
 			  struct sk_buff *skb,
 			  u8 queue_index,
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/trx.h b/drivers/net/wireless/rtlwifi/rtl8192cu/trx.h
index 725c53accc58..fd8051dcd98a 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/trx.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/trx.h
@@ -420,7 +420,7 @@ struct sk_buff *rtl8192c_tx_aggregate_hdl(struct ieee80211_hw *,
 					   struct sk_buff_head *);
 void rtl92cu_tx_fill_desc(struct ieee80211_hw *hw,
 			  struct ieee80211_hdr *hdr, u8 *pdesc_tx,
-			  struct ieee80211_tx_info *info,
+			  u8 *pbd_desc_tx, struct ieee80211_tx_info *info,
 			  struct ieee80211_sta *sta,
 			  struct sk_buff *skb,
 			  u8 queue_index,
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/dm.c b/drivers/net/wireless/rtlwifi/rtl8192de/dm.c
index 7908e1c85819..304c443b89b2 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/dm.c
@@ -194,15 +194,15 @@ static void rtl92d_dm_false_alarm_counter_statistics(struct ieee80211_hw *hw)
 	rtl_set_bbreg(hw, ROFDM0_LSTF, BIT(31), 1); /* hold page C counter */
 	rtl_set_bbreg(hw, ROFDM1_LSTF, BIT(31), 1); /*hold page D counter */
 
-	ret_value = rtl_get_bbreg(hw, ROFDM0_FRAMESYNC, BMASKDWORD);
+	ret_value = rtl_get_bbreg(hw, ROFDM0_FRAMESYNC, 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, ROFDM_PHYCOUNTER1, BMASKDWORD);
+	ret_value = rtl_get_bbreg(hw, ROFDM_PHYCOUNTER1, MASKDWORD);
 	falsealm_cnt->cnt_parity_fail = ((ret_value & 0xffff0000) >> 16);
-	ret_value = rtl_get_bbreg(hw, ROFDM_PHYCOUNTER2, BMASKDWORD);
+	ret_value = rtl_get_bbreg(hw, ROFDM_PHYCOUNTER2, MASKDWORD);
 	falsealm_cnt->cnt_rate_illegal = (ret_value & 0xffff);
 	falsealm_cnt->cnt_crc8_fail = ((ret_value & 0xffff0000) >> 16);
-	ret_value = rtl_get_bbreg(hw, ROFDM_PHYCOUNTER3, BMASKDWORD);
+	ret_value = rtl_get_bbreg(hw, ROFDM_PHYCOUNTER3, MASKDWORD);
 	falsealm_cnt->cnt_mcs_fail = (ret_value & 0xffff);
 	falsealm_cnt->cnt_ofdm_fail = falsealm_cnt->cnt_parity_fail +
 				      falsealm_cnt->cnt_rate_illegal +
@@ -214,9 +214,9 @@ static void rtl92d_dm_false_alarm_counter_statistics(struct ieee80211_hw *hw)
 	if (rtlpriv->rtlhal.current_bandtype != BAND_ON_5G) {
 		/* hold cck counter */
 		rtl92d_acquire_cckandrw_pagea_ctl(hw, &flag);
-		ret_value = rtl_get_bbreg(hw, RCCK0_FACOUNTERLOWER, BMASKBYTE0);
+		ret_value = rtl_get_bbreg(hw, RCCK0_FACOUNTERLOWER, MASKBYTE0);
 		falsealm_cnt->cnt_cck_fail = ret_value;
-		ret_value = rtl_get_bbreg(hw, RCCK0_FACOUNTERUPPER, BMASKBYTE3);
+		ret_value = rtl_get_bbreg(hw, RCCK0_FACOUNTERUPPER, MASKBYTE3);
 		falsealm_cnt->cnt_cck_fail += (ret_value & 0xff) << 8;
 		rtl92d_release_cckandrw_pagea_ctl(hw, &flag);
 	} else {
@@ -331,11 +331,11 @@ static void rtl92d_dm_cck_packet_detection_thresh(struct ieee80211_hw *hw)
 	if (de_digtable->pre_cck_pd_state != de_digtable->cur_cck_pd_state) {
 		if (de_digtable->cur_cck_pd_state == CCK_PD_STAGE_LOWRSSI) {
 			rtl92d_acquire_cckandrw_pagea_ctl(hw, &flag);
-			rtl_set_bbreg(hw, RCCK0_CCA, BMASKBYTE2, 0x83);
+			rtl_set_bbreg(hw, RCCK0_CCA, MASKBYTE2, 0x83);
 			rtl92d_release_cckandrw_pagea_ctl(hw, &flag);
 		} else {
 			rtl92d_acquire_cckandrw_pagea_ctl(hw, &flag);
-			rtl_set_bbreg(hw, RCCK0_CCA, BMASKBYTE2, 0xcd);
+			rtl_set_bbreg(hw, RCCK0_CCA, MASKBYTE2, 0xcd);
 			rtl92d_release_cckandrw_pagea_ctl(hw, &flag);
 		}
 		de_digtable->pre_cck_pd_state = de_digtable->cur_cck_pd_state;
@@ -722,7 +722,7 @@ static void rtl92d_dm_rxgain_tracking_thermalmeter(struct ieee80211_hw *hw)
 	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
 		 "===> Rx Gain %x\n", u4tmp);
 	for (i = RF90_PATH_A; i < rtlpriv->phy.num_total_rfpath; i++)
-		rtl_set_rfreg(hw, i, 0x3C, BRFREGOFFSETMASK,
+		rtl_set_rfreg(hw, i, 0x3C, RFREG_OFFSET_MASK,
 			      (rtlpriv->phy.reg_rf3c[i] & (~(0xF000))) | u4tmp);
 }
 
@@ -737,7 +737,7 @@ static void rtl92d_bandtype_2_4G(struct ieee80211_hw *hw, long *temp_cckg,
 	/* Query CCK default setting From 0xa24 */
 	rtl92d_acquire_cckandrw_pagea_ctl(hw, &flag);
 	temp_cck = rtl_get_bbreg(hw, RCCK0_TXFILTER2,
-				 BMASKDWORD) & BMASKCCK;
+				 MASKDWORD) & MASKCCK;
 	rtl92d_release_cckandrw_pagea_ctl(hw, &flag);
 	for (i = 0; i < CCK_TABLE_LENGTH; i++) {
 		if (rtlpriv->dm.cck_inch14) {
@@ -896,9 +896,9 @@ static void rtl92d_dm_txpower_tracking_callback_thermalmeter(
 		rf = 1;
 	if (thermalvalue) {
 		ele_d = rtl_get_bbreg(hw, ROFDM0_XATxIQIMBALANCE,
-				      BMASKDWORD) & BMASKOFDM_D;
+				      MASKDWORD) & MASKOFDM_D;
 		for (i = 0; i < OFDM_TABLE_SIZE_92D; i++) {
-			if (ele_d == (ofdmswing_table[i] & BMASKOFDM_D)) {
+			if (ele_d == (ofdmswing_table[i] & MASKOFDM_D)) {
 				ofdm_index_old[0] = (u8) i;
 
 				RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
@@ -910,10 +910,10 @@ static void rtl92d_dm_txpower_tracking_callback_thermalmeter(
 		}
 		if (is2t) {
 			ele_d = rtl_get_bbreg(hw, ROFDM0_XBTxIQIMBALANCE,
-					      BMASKDWORD) & BMASKOFDM_D;
+					      MASKDWORD) & MASKOFDM_D;
 			for (i = 0; i < OFDM_TABLE_SIZE_92D; i++) {
 				if (ele_d ==
-				    (ofdmswing_table[i] & BMASKOFDM_D)) {
+				    (ofdmswing_table[i] & MASKOFDM_D)) {
 					ofdm_index_old[1] = (u8) i;
 					RT_TRACE(rtlpriv, COMP_POWER_TRACKING,
 						 DBG_LOUD,
@@ -1091,10 +1091,10 @@ static void rtl92d_dm_txpower_tracking_callback_thermalmeter(
 				value32 = (ele_d << 22) | ((ele_c & 0x3F) <<
 					  16) | ele_a;
 				rtl_set_bbreg(hw, ROFDM0_XATxIQIMBALANCE,
-					      BMASKDWORD, value32);
+					      MASKDWORD, value32);
 
 				value32 = (ele_c & 0x000003C0) >> 6;
-				rtl_set_bbreg(hw, ROFDM0_XCTxAFE, BMASKH4BITS,
+				rtl_set_bbreg(hw, ROFDM0_XCTxAFE, MASKH4BITS,
 					      value32);
 
 				value32 = ((val_x * ele_d) >> 7) & 0x01;
@@ -1103,10 +1103,10 @@ static void rtl92d_dm_txpower_tracking_callback_thermalmeter(
 
 			} else {
 				rtl_set_bbreg(hw, ROFDM0_XATxIQIMBALANCE,
-					      BMASKDWORD,
+					      MASKDWORD,
 					      ofdmswing_table
 					      [(u8)ofdm_index[0]]);
-				rtl_set_bbreg(hw, ROFDM0_XCTxAFE, BMASKH4BITS,
+				rtl_set_bbreg(hw, ROFDM0_XCTxAFE, MASKH4BITS,
 					      0x00);
 				rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD,
 					      BIT(24), 0x00);
@@ -1204,21 +1204,21 @@ static void rtl92d_dm_txpower_tracking_callback_thermalmeter(
 						  ele_a;
 					rtl_set_bbreg(hw,
 						      ROFDM0_XBTxIQIMBALANCE,
-						      BMASKDWORD, value32);
+						      MASKDWORD, value32);
 					value32 = (ele_c & 0x000003C0) >> 6;
 					rtl_set_bbreg(hw, ROFDM0_XDTxAFE,
-						      BMASKH4BITS, value32);
+						      MASKH4BITS, value32);
 					value32 = ((val_x * ele_d) >> 7) & 0x01;
 					rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD,
 						      BIT(28), value32);
 				} else {
 					rtl_set_bbreg(hw,
 						      ROFDM0_XBTxIQIMBALANCE,
-						      BMASKDWORD,
+						      MASKDWORD,
 						      ofdmswing_table
 						      [(u8) ofdm_index[1]]);
 					rtl_set_bbreg(hw, ROFDM0_XDTxAFE,
-						      BMASKH4BITS, 0x00);
+						      MASKH4BITS, 0x00);
 					rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD,
 						      BIT(28), 0x00);
 				}
@@ -1229,10 +1229,10 @@ static void rtl92d_dm_txpower_tracking_callback_thermalmeter(
 			}
 			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
 				 "TxPwrTracking 0xc80 = 0x%x, 0xc94 = 0x%x RF 0x24 = 0x%x\n",
-				 rtl_get_bbreg(hw, 0xc80, BMASKDWORD),
-				 rtl_get_bbreg(hw, 0xc94, BMASKDWORD),
+				 rtl_get_bbreg(hw, 0xc80, MASKDWORD),
+				 rtl_get_bbreg(hw, 0xc94, MASKDWORD),
 				 rtl_get_rfreg(hw, RF90_PATH_A, 0x24,
-					       BRFREGOFFSETMASK));
+					       RFREG_OFFSET_MASK));
 		}
 		if ((delta_iqk > rtlefuse->delta_iqk) &&
 		    (rtlefuse->delta_iqk != 0)) {
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/hw.c b/drivers/net/wireless/rtlwifi/rtl8192de/hw.c
index c4a7db9135d6..2b08671004a0 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/hw.c
@@ -318,7 +318,7 @@ void rtl92de_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 	case HW_VAR_AC_PARAM: {
 		u8 e_aci = *val;
 		rtl92d_dm_init_edca_turbo(hw);
-		if (rtlpci->acm_method != eAcmWay2_SW)
+		if (rtlpci->acm_method != EACMWAY2_SW)
 			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ACM_CTRL,
 						      &e_aci);
 		break;
@@ -985,9 +985,9 @@ int rtl92de_hw_init(struct ieee80211_hw *hw)
 	/* set default value after initialize RF,  */
 	rtl_set_bbreg(hw, RFPGA0_ANALOGPARAMETER4, 0x00f00000, 0);
 	rtlphy->rfreg_chnlval[0] = rtl_get_rfreg(hw, (enum radio_path)0,
-			RF_CHNLBW, BRFREGOFFSETMASK);
+			RF_CHNLBW, RFREG_OFFSET_MASK);
 	rtlphy->rfreg_chnlval[1] = rtl_get_rfreg(hw, (enum radio_path)1,
-			RF_CHNLBW, BRFREGOFFSETMASK);
+			RF_CHNLBW, RFREG_OFFSET_MASK);
 
 	/*---- Set CCK and OFDM Block "ON"----*/
 	if (rtlhal->current_bandtype == BAND_ON_2_4G)
@@ -1035,7 +1035,7 @@ int rtl92de_hw_init(struct ieee80211_hw *hw)
 
 				tmp_rega = rtl_get_rfreg(hw,
 						  (enum radio_path)RF90_PATH_A,
-						  0x2a, BMASKDWORD);
+						  0x2a, MASKDWORD);
 
 				if (((tmp_rega & BIT(11)) == BIT(11)))
 					break;
@@ -1138,11 +1138,13 @@ static int _rtl92de_set_media_status(struct ieee80211_hw *hw,
 void rtl92de_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;
+	u32 reg_rcr;
 
 	if (rtlpriv->psc.rfpwr_state != ERFON)
 		return;
+
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_RCR, (u8 *)(&reg_rcr));
+
 	if (check_bssid) {
 		reg_rcr |= (RCR_CBSSID_DATA | RCR_CBSSID_BCN);
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_RCR, (u8 *)(&reg_rcr));
@@ -1332,13 +1334,13 @@ void rtl92de_card_disable(struct ieee80211_hw *hw)
 	/* c. ========RF OFF sequence==========  */
 	/* 0x88c[23:20] = 0xf. */
 	rtl_set_bbreg(hw, RFPGA0_ANALOGPARAMETER4, 0x00f00000, 0xf);
-	rtl_set_rfreg(hw, RF90_PATH_A, 0x00, BRFREGOFFSETMASK, 0x00);
+	rtl_set_rfreg(hw, RF90_PATH_A, 0x00, RFREG_OFFSET_MASK, 0x00);
 
 	/* APSD_CTRL 0x600[7:0] = 0x40 */
 	rtl_write_byte(rtlpriv, REG_APSD_CTRL, 0x40);
 
 	/* Close antenna 0,0xc04,0xd04 */
-	rtl_set_bbreg(hw, ROFDM0_TRXPATHENABLE, BMASKBYTE0, 0);
+	rtl_set_bbreg(hw, ROFDM0_TRXPATHENABLE, MASKBYTE0, 0);
 	rtl_set_bbreg(hw, ROFDM1_TRXPATHENABLE, BDWORD, 0);
 
 	/*  SYS_FUNC_EN 0x02[7:0] = 0xE2   reset BB state machine */
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/phy.c b/drivers/net/wireless/rtlwifi/rtl8192de/phy.c
index 13196cc4b1d3..3d1f0dd4e52d 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/phy.c
@@ -30,6 +30,7 @@
 #include "../wifi.h"
 #include "../pci.h"
 #include "../ps.h"
+#include "../core.h"
 #include "reg.h"
 #include "def.h"
 #include "phy.h"
@@ -242,7 +243,7 @@ void rtl92d_phy_set_bb_reg(struct ieee80211_hw *hw,
 	else if (rtlhal->during_mac0init_radiob)
 		/* mac0 use phy1 write radio_b. */
 		dbi_direct = BIT(3) | BIT(2);
-	if (bitmask != BMASKDWORD) {
+	if (bitmask != MASKDWORD) {
 		if (rtlhal->during_mac1init_radioa ||
 		    rtlhal->during_mac0init_radiob)
 			originalvalue = rtl92de_read_dword_dbi(hw,
@@ -275,20 +276,20 @@ static u32 _rtl92d_phy_rf_serial_read(struct ieee80211_hw *hw,
 	u32 retvalue;
 
 	newoffset = offset;
-	tmplong = rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, BMASKDWORD);
+	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, BMASKDWORD);
+		tmplong2 = rtl_get_bbreg(hw, pphyreg->rfhssi_para2, MASKDWORD);
 	tmplong2 = (tmplong2 & (~BLSSIREADADDRESS)) |
 		(newoffset << 23) | BLSSIREADEDGE;
-	rtl_set_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, BMASKDWORD,
+	rtl_set_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD,
 		tmplong & (~BLSSIREADEDGE));
 	udelay(10);
-	rtl_set_bbreg(hw, pphyreg->rfhssi_para2, BMASKDWORD, tmplong2);
+	rtl_set_bbreg(hw, pphyreg->rfhssi_para2, MASKDWORD, tmplong2);
 	udelay(50);
 	udelay(50);
-	rtl_set_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, BMASKDWORD,
+	rtl_set_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD,
 		tmplong | BLSSIREADEDGE);
 	udelay(10);
 	if (rfpath == RF90_PATH_A)
@@ -321,7 +322,7 @@ static void _rtl92d_phy_rf_serial_write(struct ieee80211_hw *hw,
 	newoffset = offset;
 	/* T65 RF */
 	data_and_addr = ((newoffset << 20) | (data & 0x000fffff)) & 0x0fffffff;
-	rtl_set_bbreg(hw, pphyreg->rf3wire_offset, BMASKDWORD, data_and_addr);
+	rtl_set_bbreg(hw, pphyreg->rf3wire_offset, MASKDWORD, data_and_addr);
 	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE, "RFW-%d Addr[0x%x]=0x%x\n",
 		 rfpath, pphyreg->rf3wire_offset, data_and_addr);
 }
@@ -362,7 +363,7 @@ void rtl92d_phy_set_rf_reg(struct ieee80211_hw *hw, enum radio_path rfpath,
 		return;
 	spin_lock_irqsave(&rtlpriv->locks.rf_lock, flags);
 	if (rtlphy->rf_mode != RF_OP_BY_FW) {
-		if (bitmask != BRFREGOFFSETMASK) {
+		if (bitmask != RFREG_OFFSET_MASK) {
 			original_value = _rtl92d_phy_rf_serial_read(hw,
 				rfpath, regaddr);
 			bitshift = _rtl92d_phy_calculate_bit_shift(bitmask);
@@ -567,19 +568,8 @@ static bool _rtl92d_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
 		 " ===> phy:Rtl819XPHY_REG_Array_PG\n");
 	if (configtype == BASEBAND_CONFIG_PHY_REG) {
 		for (i = 0; i < phy_reg_arraylen; i = i + 2) {
-			if (phy_regarray_table[i] == 0xfe)
-				mdelay(50);
-			else if (phy_regarray_table[i] == 0xfd)
-				mdelay(5);
-			else if (phy_regarray_table[i] == 0xfc)
-				mdelay(1);
-			else if (phy_regarray_table[i] == 0xfb)
-				udelay(50);
-			else if (phy_regarray_table[i] == 0xfa)
-				udelay(5);
-			else if (phy_regarray_table[i] == 0xf9)
-				udelay(1);
-			rtl_set_bbreg(hw, phy_regarray_table[i], BMASKDWORD,
+			rtl_addr_delay(phy_regarray_table[i]);
+			rtl_set_bbreg(hw, phy_regarray_table[i], MASKDWORD,
 				      phy_regarray_table[i + 1]);
 			udelay(1);
 			RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
@@ -591,7 +581,7 @@ static bool _rtl92d_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
 		if (rtlhal->interfaceindex == 0) {
 			for (i = 0; i < agctab_arraylen; i = i + 2) {
 				rtl_set_bbreg(hw, agctab_array_table[i],
-					BMASKDWORD,
+					MASKDWORD,
 					agctab_array_table[i + 1]);
 				/* Add 1us delay between BB/RF register
 				 * setting. */
@@ -607,7 +597,7 @@ static bool _rtl92d_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
 			if (rtlhal->current_bandtype == BAND_ON_2_4G) {
 				for (i = 0; i < agctab_arraylen; i = i + 2) {
 					rtl_set_bbreg(hw, agctab_array_table[i],
-						BMASKDWORD,
+						MASKDWORD,
 						agctab_array_table[i + 1]);
 					/* Add 1us delay between BB/RF register
 					 * setting. */
@@ -623,7 +613,7 @@ static bool _rtl92d_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
 				for (i = 0; i < agctab_5garraylen; i = i + 2) {
 					rtl_set_bbreg(hw,
 						agctab_5garray_table[i],
-						BMASKDWORD,
+						MASKDWORD,
 						agctab_5garray_table[i + 1]);
 					/* Add 1us delay between BB/RF registeri
 					 * setting. */
@@ -705,18 +695,7 @@ static bool _rtl92d_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
 	phy_regarray_table_pg = rtl8192de_phy_reg_array_pg;
 	if (configtype == BASEBAND_CONFIG_PHY_REG) {
 		for (i = 0; i < phy_regarray_pg_len; i = i + 3) {
-			if (phy_regarray_table_pg[i] == 0xfe)
-				mdelay(50);
-			else if (phy_regarray_table_pg[i] == 0xfd)
-				mdelay(5);
-			else if (phy_regarray_table_pg[i] == 0xfc)
-				mdelay(1);
-			else if (phy_regarray_table_pg[i] == 0xfb)
-				udelay(50);
-			else if (phy_regarray_table_pg[i] == 0xfa)
-				udelay(5);
-			else if (phy_regarray_table_pg[i] == 0xf9)
-				udelay(1);
+			rtl_addr_delay(phy_regarray_table_pg[i]);
 			_rtl92d_store_pwrindex_diffrate_offset(hw,
 				phy_regarray_table_pg[i],
 				phy_regarray_table_pg[i + 1],
@@ -843,54 +822,16 @@ bool rtl92d_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
 	switch (rfpath) {
 	case RF90_PATH_A:
 		for (i = 0; i < radioa_arraylen; i = i + 2) {
-			if (radioa_array_table[i] == 0xfe) {
-				mdelay(50);
-			} else if (radioa_array_table[i] == 0xfd) {
-				/* delay_ms(5); */
-				mdelay(5);
-			} else if (radioa_array_table[i] == 0xfc) {
-				/* delay_ms(1); */
-				mdelay(1);
-			} else if (radioa_array_table[i] == 0xfb) {
-				udelay(50);
-			} else if (radioa_array_table[i] == 0xfa) {
-				udelay(5);
-			} else if (radioa_array_table[i] == 0xf9) {
-				udelay(1);
-			} else {
-				rtl_set_rfreg(hw, rfpath, radioa_array_table[i],
-					      BRFREGOFFSETMASK,
-					      radioa_array_table[i + 1]);
-				/*  Add 1us delay between BB/RF register set. */
-				udelay(1);
-			}
+			rtl_rfreg_delay(hw, rfpath, radioa_array_table[i],
+					RFREG_OFFSET_MASK,
+					radioa_array_table[i + 1]);
 		}
 		break;
 	case RF90_PATH_B:
 		for (i = 0; i < radiob_arraylen; i = i + 2) {
-			if (radiob_array_table[i] == 0xfe) {
-				/* Delay specific ms. Only RF configuration
-				 * requires delay. */
-				mdelay(50);
-			} else if (radiob_array_table[i] == 0xfd) {
-				/* delay_ms(5); */
-				mdelay(5);
-			} else if (radiob_array_table[i] == 0xfc) {
-				/* delay_ms(1); */
-				mdelay(1);
-			} else if (radiob_array_table[i] == 0xfb) {
-				udelay(50);
-			} else if (radiob_array_table[i] == 0xfa) {
-				udelay(5);
-			} else if (radiob_array_table[i] == 0xf9) {
-				udelay(1);
-			} else {
-				rtl_set_rfreg(hw, rfpath, radiob_array_table[i],
-					      BRFREGOFFSETMASK,
-					      radiob_array_table[i + 1]);
-				/*  Add 1us delay between BB/RF register set. */
-				udelay(1);
-			}
+			rtl_rfreg_delay(hw, rfpath, radiob_array_table[i],
+					RFREG_OFFSET_MASK,
+					radiob_array_table[i + 1]);
 		}
 		break;
 	case RF90_PATH_C:
@@ -911,13 +852,13 @@ void rtl92d_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw)
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
 
 	rtlphy->default_initialgain[0] =
-	    (u8) rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, BMASKBYTE0);
+	    (u8) rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, MASKBYTE0);
 	rtlphy->default_initialgain[1] =
-	    (u8) rtl_get_bbreg(hw, ROFDM0_XBAGCCORE1, BMASKBYTE0);
+	    (u8) rtl_get_bbreg(hw, ROFDM0_XBAGCCORE1, MASKBYTE0);
 	rtlphy->default_initialgain[2] =
-	    (u8) rtl_get_bbreg(hw, ROFDM0_XCAGCCORE1, BMASKBYTE0);
+	    (u8) rtl_get_bbreg(hw, ROFDM0_XCAGCCORE1, MASKBYTE0);
 	rtlphy->default_initialgain[3] =
-	    (u8) rtl_get_bbreg(hw, ROFDM0_XDAGCCORE1, BMASKBYTE0);
+	    (u8) rtl_get_bbreg(hw, ROFDM0_XDAGCCORE1, MASKBYTE0);
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 		 "Default initial gain (c50=0x%x, c58=0x%x, c60=0x%x, c68=0x%x\n",
 		 rtlphy->default_initialgain[0],
@@ -925,9 +866,9 @@ void rtl92d_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw)
 		 rtlphy->default_initialgain[2],
 		 rtlphy->default_initialgain[3]);
 	rtlphy->framesync = (u8)rtl_get_bbreg(hw, ROFDM0_RXDETECTOR3,
-					      BMASKBYTE0);
+					      MASKBYTE0);
 	rtlphy->framesync_c34 = rtl_get_bbreg(hw, ROFDM0_RXDETECTOR2,
-					      BMASKDWORD);
+					      MASKDWORD);
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 		 "Default framesync (0x%x) = 0x%x\n",
 		 ROFDM0_RXDETECTOR3, rtlphy->framesync);
@@ -1106,7 +1047,7 @@ static void _rtl92d_phy_stop_trx_before_changeband(struct ieee80211_hw *hw)
 {
 	rtl_set_bbreg(hw, RFPGA0_RFMOD, BCCKEN, 0);
 	rtl_set_bbreg(hw, RFPGA0_RFMOD, BOFDMEN, 0);
-	rtl_set_bbreg(hw, ROFDM0_TRXPATHENABLE, BMASKBYTE0, 0x00);
+	rtl_set_bbreg(hw, ROFDM0_TRXPATHENABLE, MASKBYTE0, 0x00);
 	rtl_set_bbreg(hw, ROFDM1_TRXPATHENABLE, BDWORD, 0x0);
 }
 
@@ -1168,7 +1109,7 @@ static void _rtl92d_phy_reload_imr_setting(struct ieee80211_hw *hw,
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u32 imr_num = MAX_RF_IMR_INDEX;
-	u32 rfmask = BRFREGOFFSETMASK;
+	u32 rfmask = RFREG_OFFSET_MASK;
 	u8 group, i;
 	unsigned long flag = 0;
 
@@ -1211,7 +1152,7 @@ static void _rtl92d_phy_reload_imr_setting(struct ieee80211_hw *hw,
 			for (i = 0; i < imr_num; i++) {
 				rtl_set_rfreg(hw, (enum radio_path)rfpath,
 					      rf_reg_for_5g_swchnl_normal[i],
-					      BRFREGOFFSETMASK,
+					      RFREG_OFFSET_MASK,
 					      rf_imr_param_normal[0][0][i]);
 			}
 			rtl_set_bbreg(hw, RFPGA0_ANALOGPARAMETER4,
@@ -1329,7 +1270,7 @@ static void _rtl92d_phy_switch_rf_setting(struct ieee80211_hw *hw, u8 channel)
 			if (i == 0 && (rtlhal->macphymode == DUALMAC_DUALPHY)) {
 				rtl_set_rfreg(hw, (enum radio_path)path,
 					      rf_reg_for_c_cut_5g[i],
-					      BRFREGOFFSETMASK, 0xE439D);
+					      RFREG_OFFSET_MASK, 0xE439D);
 			} else if (rf_reg_for_c_cut_5g[i] == RF_SYN_G4) {
 				u4tmp2 = (rf_reg_pram_c_5g[index][i] &
 				     0x7FF) | (u4tmp << 11);
@@ -1337,11 +1278,11 @@ static void _rtl92d_phy_switch_rf_setting(struct ieee80211_hw *hw, u8 channel)
 					u4tmp2 &= ~(BIT(7) | BIT(6));
 				rtl_set_rfreg(hw, (enum radio_path)path,
 					      rf_reg_for_c_cut_5g[i],
-					      BRFREGOFFSETMASK, u4tmp2);
+					      RFREG_OFFSET_MASK, u4tmp2);
 			} else {
 				rtl_set_rfreg(hw, (enum radio_path)path,
 					      rf_reg_for_c_cut_5g[i],
-					      BRFREGOFFSETMASK,
+					      RFREG_OFFSET_MASK,
 					      rf_reg_pram_c_5g[index][i]);
 			}
 			RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
@@ -1351,7 +1292,7 @@ static void _rtl92d_phy_switch_rf_setting(struct ieee80211_hw *hw, u8 channel)
 				 path, index,
 				 rtl_get_rfreg(hw, (enum radio_path)path,
 					       rf_reg_for_c_cut_5g[i],
-					       BRFREGOFFSETMASK));
+					       RFREG_OFFSET_MASK));
 		}
 		if (need_pwr_down)
 			_rtl92d_phy_restore_rf_env(hw, path, &u4regvalue);
@@ -1381,7 +1322,7 @@ static void _rtl92d_phy_switch_rf_setting(struct ieee80211_hw *hw, u8 channel)
 				     i++) {
 					rtl_set_rfreg(hw, rfpath,
 						rf_for_c_cut_5g_internal_pa[i],
-						BRFREGOFFSETMASK,
+						RFREG_OFFSET_MASK,
 						rf_pram_c_5g_int_pa[index][i]);
 					RT_TRACE(rtlpriv, COMP_RF, DBG_LOUD,
 						 "offset 0x%x value 0x%x path %d index %d\n",
@@ -1422,13 +1363,13 @@ static void _rtl92d_phy_switch_rf_setting(struct ieee80211_hw *hw, u8 channel)
 			if (rf_reg_for_c_cut_2g[i] == RF_SYN_G7)
 				rtl_set_rfreg(hw, (enum radio_path)path,
 					rf_reg_for_c_cut_2g[i],
-					BRFREGOFFSETMASK,
+					RFREG_OFFSET_MASK,
 					(rf_reg_param_for_c_cut_2g[index][i] |
 					BIT(17)));
 			else
 				rtl_set_rfreg(hw, (enum radio_path)path,
 					      rf_reg_for_c_cut_2g[i],
-					      BRFREGOFFSETMASK,
+					      RFREG_OFFSET_MASK,
 					      rf_reg_param_for_c_cut_2g
 					      [index][i]);
 			RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
@@ -1438,14 +1379,14 @@ static void _rtl92d_phy_switch_rf_setting(struct ieee80211_hw *hw, u8 channel)
 				 rf_reg_mask_for_c_cut_2g[i], path, index,
 				 rtl_get_rfreg(hw, (enum radio_path)path,
 					       rf_reg_for_c_cut_2g[i],
-					       BRFREGOFFSETMASK));
+					       RFREG_OFFSET_MASK));
 		}
 		RTPRINT(rtlpriv, FINIT, INIT_IQK,
 			"cosa ver 3 set RF-B, 2G, 0x28 = 0x%x !!\n",
 			rf_syn_g4_for_c_cut_2g | (u4tmp << 11));
 
 		rtl_set_rfreg(hw, (enum radio_path)path, RF_SYN_G4,
-			      BRFREGOFFSETMASK,
+			      RFREG_OFFSET_MASK,
 			      rf_syn_g4_for_c_cut_2g | (u4tmp << 11));
 		if (need_pwr_down)
 			_rtl92d_phy_restore_rf_env(hw, path, &u4regvalue);
@@ -1493,41 +1434,41 @@ static u8 _rtl92d_phy_patha_iqk(struct ieee80211_hw *hw, bool configpathb)
 	/* path-A IQK setting */
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,  "Path-A IQK setting!\n");
 	if (rtlhal->interfaceindex == 0) {
-		rtl_set_bbreg(hw, 0xe30, BMASKDWORD, 0x10008c1f);
-		rtl_set_bbreg(hw, 0xe34, BMASKDWORD, 0x10008c1f);
+		rtl_set_bbreg(hw, 0xe30, MASKDWORD, 0x10008c1f);
+		rtl_set_bbreg(hw, 0xe34, MASKDWORD, 0x10008c1f);
 	} else {
-		rtl_set_bbreg(hw, 0xe30, BMASKDWORD, 0x10008c22);
-		rtl_set_bbreg(hw, 0xe34, BMASKDWORD, 0x10008c22);
+		rtl_set_bbreg(hw, 0xe30, MASKDWORD, 0x10008c22);
+		rtl_set_bbreg(hw, 0xe34, MASKDWORD, 0x10008c22);
 	}
-	rtl_set_bbreg(hw, 0xe38, BMASKDWORD, 0x82140102);
-	rtl_set_bbreg(hw, 0xe3c, BMASKDWORD, 0x28160206);
+	rtl_set_bbreg(hw, 0xe38, MASKDWORD, 0x82140102);
+	rtl_set_bbreg(hw, 0xe3c, MASKDWORD, 0x28160206);
 	/* path-B IQK setting */
 	if (configpathb) {
-		rtl_set_bbreg(hw, 0xe50, BMASKDWORD, 0x10008c22);
-		rtl_set_bbreg(hw, 0xe54, BMASKDWORD, 0x10008c22);
-		rtl_set_bbreg(hw, 0xe58, BMASKDWORD, 0x82140102);
-		rtl_set_bbreg(hw, 0xe5c, BMASKDWORD, 0x28160206);
+		rtl_set_bbreg(hw, 0xe50, MASKDWORD, 0x10008c22);
+		rtl_set_bbreg(hw, 0xe54, MASKDWORD, 0x10008c22);
+		rtl_set_bbreg(hw, 0xe58, MASKDWORD, 0x82140102);
+		rtl_set_bbreg(hw, 0xe5c, MASKDWORD, 0x28160206);
 	}
 	/* LO calibration setting */
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,  "LO calibration setting!\n");
-	rtl_set_bbreg(hw, 0xe4c, BMASKDWORD, 0x00462911);
+	rtl_set_bbreg(hw, 0xe4c, MASKDWORD, 0x00462911);
 	/* One shot, path A LOK & IQK */
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,  "One shot, path A LOK & IQK!\n");
-	rtl_set_bbreg(hw, 0xe48, BMASKDWORD, 0xf9000000);
-	rtl_set_bbreg(hw, 0xe48, BMASKDWORD, 0xf8000000);
+	rtl_set_bbreg(hw, 0xe48, MASKDWORD, 0xf9000000);
+	rtl_set_bbreg(hw, 0xe48, MASKDWORD, 0xf8000000);
 	/* delay x ms */
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,
 		"Delay %d ms for One shot, path A LOK & IQK\n",
 		IQK_DELAY_TIME);
 	mdelay(IQK_DELAY_TIME);
 	/* Check failed */
-	regeac = rtl_get_bbreg(hw, 0xeac, BMASKDWORD);
+	regeac = rtl_get_bbreg(hw, 0xeac, MASKDWORD);
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,  "0xeac = 0x%x\n", regeac);
-	rege94 = rtl_get_bbreg(hw, 0xe94, BMASKDWORD);
+	rege94 = rtl_get_bbreg(hw, 0xe94, MASKDWORD);
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,  "0xe94 = 0x%x\n", rege94);
-	rege9c = rtl_get_bbreg(hw, 0xe9c, BMASKDWORD);
+	rege9c = rtl_get_bbreg(hw, 0xe9c, MASKDWORD);
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,  "0xe9c = 0x%x\n", rege9c);
-	regea4 = rtl_get_bbreg(hw, 0xea4, BMASKDWORD);
+	regea4 = rtl_get_bbreg(hw, 0xea4, MASKDWORD);
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,  "0xea4 = 0x%x\n", regea4);
 	if (!(regeac & BIT(28)) && (((rege94 & 0x03FF0000) >> 16) != 0x142) &&
 	    (((rege9c & 0x03FF0000) >> 16) != 0x42))
@@ -1563,42 +1504,42 @@ static u8 _rtl92d_phy_patha_iqk_5g_normal(struct ieee80211_hw *hw,
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,  "Path A IQK!\n");
 	/* path-A IQK setting */
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,  "Path-A IQK setting!\n");
-	rtl_set_bbreg(hw, 0xe30, BMASKDWORD, 0x18008c1f);
-	rtl_set_bbreg(hw, 0xe34, BMASKDWORD, 0x18008c1f);
-	rtl_set_bbreg(hw, 0xe38, BMASKDWORD, 0x82140307);
-	rtl_set_bbreg(hw, 0xe3c, BMASKDWORD, 0x68160960);
+	rtl_set_bbreg(hw, 0xe30, MASKDWORD, 0x18008c1f);
+	rtl_set_bbreg(hw, 0xe34, MASKDWORD, 0x18008c1f);
+	rtl_set_bbreg(hw, 0xe38, MASKDWORD, 0x82140307);
+	rtl_set_bbreg(hw, 0xe3c, MASKDWORD, 0x68160960);
 	/* path-B IQK setting */
 	if (configpathb) {
-		rtl_set_bbreg(hw, 0xe50, BMASKDWORD, 0x18008c2f);
-		rtl_set_bbreg(hw, 0xe54, BMASKDWORD, 0x18008c2f);
-		rtl_set_bbreg(hw, 0xe58, BMASKDWORD, 0x82110000);
-		rtl_set_bbreg(hw, 0xe5c, BMASKDWORD, 0x68110000);
+		rtl_set_bbreg(hw, 0xe50, MASKDWORD, 0x18008c2f);
+		rtl_set_bbreg(hw, 0xe54, MASKDWORD, 0x18008c2f);
+		rtl_set_bbreg(hw, 0xe58, MASKDWORD, 0x82110000);
+		rtl_set_bbreg(hw, 0xe5c, MASKDWORD, 0x68110000);
 	}
 	/* LO calibration setting */
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,  "LO calibration setting!\n");
-	rtl_set_bbreg(hw, 0xe4c, BMASKDWORD, 0x00462911);
+	rtl_set_bbreg(hw, 0xe4c, MASKDWORD, 0x00462911);
 	/* path-A PA on */
-	rtl_set_bbreg(hw, RFPGA0_XAB_RFINTERFACESW, BMASKDWORD, 0x07000f60);
-	rtl_set_bbreg(hw, RFPGA0_XA_RFINTERFACEOE, BMASKDWORD, 0x66e60e30);
+	rtl_set_bbreg(hw, RFPGA0_XAB_RFINTERFACESW, MASKDWORD, 0x07000f60);
+	rtl_set_bbreg(hw, RFPGA0_XA_RFINTERFACEOE, MASKDWORD, 0x66e60e30);
 	for (i = 0; i < retrycount; i++) {
 		/* One shot, path A LOK & IQK */
 		RTPRINT(rtlpriv, FINIT, INIT_IQK,
 			"One shot, path A LOK & IQK!\n");
-		rtl_set_bbreg(hw, 0xe48, BMASKDWORD, 0xf9000000);
-		rtl_set_bbreg(hw, 0xe48, BMASKDWORD, 0xf8000000);
+		rtl_set_bbreg(hw, 0xe48, MASKDWORD, 0xf9000000);
+		rtl_set_bbreg(hw, 0xe48, MASKDWORD, 0xf8000000);
 		/* delay x ms */
 		RTPRINT(rtlpriv, FINIT, INIT_IQK,
 			"Delay %d ms for One shot, path A LOK & IQK.\n",
 			IQK_DELAY_TIME);
 		mdelay(IQK_DELAY_TIME * 10);
 		/* Check failed */
-		regeac = rtl_get_bbreg(hw, 0xeac, BMASKDWORD);
+		regeac = rtl_get_bbreg(hw, 0xeac, MASKDWORD);
 		RTPRINT(rtlpriv, FINIT, INIT_IQK,  "0xeac = 0x%x\n", regeac);
-		rege94 = rtl_get_bbreg(hw, 0xe94, BMASKDWORD);
+		rege94 = rtl_get_bbreg(hw, 0xe94, MASKDWORD);
 		RTPRINT(rtlpriv, FINIT, INIT_IQK,  "0xe94 = 0x%x\n", rege94);
-		rege9c = rtl_get_bbreg(hw, 0xe9c, BMASKDWORD);
+		rege9c = rtl_get_bbreg(hw, 0xe9c, MASKDWORD);
 		RTPRINT(rtlpriv, FINIT, INIT_IQK,  "0xe9c = 0x%x\n", rege9c);
-		regea4 = rtl_get_bbreg(hw, 0xea4, BMASKDWORD);
+		regea4 = rtl_get_bbreg(hw, 0xea4, MASKDWORD);
 		RTPRINT(rtlpriv, FINIT, INIT_IQK,  "0xea4 = 0x%x\n", regea4);
 		if (!(regeac & TxOKBit) &&
 		     (((rege94 & 0x03FF0000) >> 16) != 0x142)) {
@@ -1620,9 +1561,9 @@ static u8 _rtl92d_phy_patha_iqk_5g_normal(struct ieee80211_hw *hw,
 		}
 	}
 	/* path A PA off */
-	rtl_set_bbreg(hw, RFPGA0_XAB_RFINTERFACESW, BMASKDWORD,
+	rtl_set_bbreg(hw, RFPGA0_XAB_RFINTERFACESW, MASKDWORD,
 		      rtlphy->iqk_bb_backup[0]);
-	rtl_set_bbreg(hw, RFPGA0_XA_RFINTERFACEOE, BMASKDWORD,
+	rtl_set_bbreg(hw, RFPGA0_XA_RFINTERFACEOE, MASKDWORD,
 		      rtlphy->iqk_bb_backup[1]);
 	return result;
 }
@@ -1637,22 +1578,22 @@ static u8 _rtl92d_phy_pathb_iqk(struct ieee80211_hw *hw)
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,  "Path B IQK!\n");
 	/* One shot, path B LOK & IQK */
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,  "One shot, path A LOK & IQK!\n");
-	rtl_set_bbreg(hw, 0xe60, BMASKDWORD, 0x00000002);
-	rtl_set_bbreg(hw, 0xe60, BMASKDWORD, 0x00000000);
+	rtl_set_bbreg(hw, 0xe60, MASKDWORD, 0x00000002);
+	rtl_set_bbreg(hw, 0xe60, MASKDWORD, 0x00000000);
 	/* delay x ms  */
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,
 		"Delay %d ms for One shot, path B LOK & IQK\n", IQK_DELAY_TIME);
 	mdelay(IQK_DELAY_TIME);
 	/* Check failed */
-	regeac = rtl_get_bbreg(hw, 0xeac, BMASKDWORD);
+	regeac = rtl_get_bbreg(hw, 0xeac, MASKDWORD);
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,  "0xeac = 0x%x\n", regeac);
-	regeb4 = rtl_get_bbreg(hw, 0xeb4, BMASKDWORD);
+	regeb4 = rtl_get_bbreg(hw, 0xeb4, MASKDWORD);
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,  "0xeb4 = 0x%x\n", regeb4);
-	regebc = rtl_get_bbreg(hw, 0xebc, BMASKDWORD);
+	regebc = rtl_get_bbreg(hw, 0xebc, MASKDWORD);
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,  "0xebc = 0x%x\n", regebc);
-	regec4 = rtl_get_bbreg(hw, 0xec4, BMASKDWORD);
+	regec4 = rtl_get_bbreg(hw, 0xec4, MASKDWORD);
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,  "0xec4 = 0x%x\n", regec4);
-	regecc = rtl_get_bbreg(hw, 0xecc, BMASKDWORD);
+	regecc = rtl_get_bbreg(hw, 0xecc, MASKDWORD);
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,  "0xecc = 0x%x\n", regecc);
 	if (!(regeac & BIT(31)) && (((regeb4 & 0x03FF0000) >> 16) != 0x142) &&
 	    (((regebc & 0x03FF0000) >> 16) != 0x42))
@@ -1680,31 +1621,31 @@ static u8 _rtl92d_phy_pathb_iqk_5g_normal(struct ieee80211_hw *hw)
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,  "Path B IQK!\n");
 	/* path-A IQK setting */
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,  "Path-A IQK setting!\n");
-	rtl_set_bbreg(hw, 0xe30, BMASKDWORD, 0x18008c1f);
-	rtl_set_bbreg(hw, 0xe34, BMASKDWORD, 0x18008c1f);
-	rtl_set_bbreg(hw, 0xe38, BMASKDWORD, 0x82110000);
-	rtl_set_bbreg(hw, 0xe3c, BMASKDWORD, 0x68110000);
+	rtl_set_bbreg(hw, 0xe30, MASKDWORD, 0x18008c1f);
+	rtl_set_bbreg(hw, 0xe34, MASKDWORD, 0x18008c1f);
+	rtl_set_bbreg(hw, 0xe38, MASKDWORD, 0x82110000);
+	rtl_set_bbreg(hw, 0xe3c, MASKDWORD, 0x68110000);
 
 	/* path-B IQK setting */
-	rtl_set_bbreg(hw, 0xe50, BMASKDWORD, 0x18008c2f);
-	rtl_set_bbreg(hw, 0xe54, BMASKDWORD, 0x18008c2f);
-	rtl_set_bbreg(hw, 0xe58, BMASKDWORD, 0x82140307);
-	rtl_set_bbreg(hw, 0xe5c, BMASKDWORD, 0x68160960);
+	rtl_set_bbreg(hw, 0xe50, MASKDWORD, 0x18008c2f);
+	rtl_set_bbreg(hw, 0xe54, MASKDWORD, 0x18008c2f);
+	rtl_set_bbreg(hw, 0xe58, MASKDWORD, 0x82140307);
+	rtl_set_bbreg(hw, 0xe5c, MASKDWORD, 0x68160960);
 
 	/* LO calibration setting */
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,  "LO calibration setting!\n");
-	rtl_set_bbreg(hw, 0xe4c, BMASKDWORD, 0x00462911);
+	rtl_set_bbreg(hw, 0xe4c, MASKDWORD, 0x00462911);
 
 	/* path-B PA on */
-	rtl_set_bbreg(hw, RFPGA0_XAB_RFINTERFACESW, BMASKDWORD, 0x0f600700);
-	rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE, BMASKDWORD, 0x061f0d30);
+	rtl_set_bbreg(hw, RFPGA0_XAB_RFINTERFACESW, MASKDWORD, 0x0f600700);
+	rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE, MASKDWORD, 0x061f0d30);
 
 	for (i = 0; i < retrycount; i++) {
 		/* One shot, path B LOK & IQK */
 		RTPRINT(rtlpriv, FINIT, INIT_IQK,
 			"One shot, path A LOK & IQK!\n");
-		rtl_set_bbreg(hw, 0xe48, BMASKDWORD, 0xfa000000);
-		rtl_set_bbreg(hw, 0xe48, BMASKDWORD, 0xf8000000);
+		rtl_set_bbreg(hw, 0xe48, MASKDWORD, 0xfa000000);
+		rtl_set_bbreg(hw, 0xe48, MASKDWORD, 0xf8000000);
 
 		/* delay x ms */
 		RTPRINT(rtlpriv, FINIT, INIT_IQK,
@@ -1712,15 +1653,15 @@ static u8 _rtl92d_phy_pathb_iqk_5g_normal(struct ieee80211_hw *hw)
 		mdelay(IQK_DELAY_TIME * 10);
 
 		/* Check failed */
-		regeac = rtl_get_bbreg(hw, 0xeac, BMASKDWORD);
+		regeac = rtl_get_bbreg(hw, 0xeac, MASKDWORD);
 		RTPRINT(rtlpriv, FINIT, INIT_IQK,  "0xeac = 0x%x\n", regeac);
-		regeb4 = rtl_get_bbreg(hw, 0xeb4, BMASKDWORD);
+		regeb4 = rtl_get_bbreg(hw, 0xeb4, MASKDWORD);
 		RTPRINT(rtlpriv, FINIT, INIT_IQK,  "0xeb4 = 0x%x\n", regeb4);
-		regebc = rtl_get_bbreg(hw, 0xebc, BMASKDWORD);
+		regebc = rtl_get_bbreg(hw, 0xebc, MASKDWORD);
 		RTPRINT(rtlpriv, FINIT, INIT_IQK,  "0xebc = 0x%x\n", regebc);
-		regec4 = rtl_get_bbreg(hw, 0xec4, BMASKDWORD);
+		regec4 = rtl_get_bbreg(hw, 0xec4, MASKDWORD);
 		RTPRINT(rtlpriv, FINIT, INIT_IQK,  "0xec4 = 0x%x\n", regec4);
-		regecc = rtl_get_bbreg(hw, 0xecc, BMASKDWORD);
+		regecc = rtl_get_bbreg(hw, 0xecc, MASKDWORD);
 		RTPRINT(rtlpriv, FINIT, INIT_IQK,  "0xecc = 0x%x\n", regecc);
 		if (!(regeac & BIT(31)) &&
 		    (((regeb4 & 0x03FF0000) >> 16) != 0x142))
@@ -1738,9 +1679,9 @@ static u8 _rtl92d_phy_pathb_iqk_5g_normal(struct ieee80211_hw *hw)
 	}
 
 	/* path B PA off */
-	rtl_set_bbreg(hw, RFPGA0_XAB_RFINTERFACESW, BMASKDWORD,
+	rtl_set_bbreg(hw, RFPGA0_XAB_RFINTERFACESW, MASKDWORD,
 		      rtlphy->iqk_bb_backup[0]);
-	rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE, BMASKDWORD,
+	rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE, MASKDWORD,
 		      rtlphy->iqk_bb_backup[2]);
 	return result;
 }
@@ -1754,7 +1695,7 @@ static void _rtl92d_phy_save_adda_registers(struct ieee80211_hw *hw,
 
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,  "Save ADDA parameters.\n");
 	for (i = 0; i < regnum; i++)
-		adda_backup[i] = rtl_get_bbreg(hw, adda_reg[i], BMASKDWORD);
+		adda_backup[i] = rtl_get_bbreg(hw, adda_reg[i], MASKDWORD);
 }
 
 static void _rtl92d_phy_save_mac_registers(struct ieee80211_hw *hw,
@@ -1779,7 +1720,7 @@ static void _rtl92d_phy_reload_adda_registers(struct ieee80211_hw *hw,
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,
 		"Reload ADDA power saving parameters !\n");
 	for (i = 0; i < regnum; i++)
-		rtl_set_bbreg(hw, adda_reg[i], BMASKDWORD, adda_backup[i]);
+		rtl_set_bbreg(hw, adda_reg[i], MASKDWORD, adda_backup[i]);
 }
 
 static void _rtl92d_phy_reload_mac_registers(struct ieee80211_hw *hw,
@@ -1807,7 +1748,7 @@ static void _rtl92d_phy_path_adda_on(struct ieee80211_hw *hw,
 		pathon = rtlpriv->rtlhal.interfaceindex == 0 ?
 		    0x04db25a4 : 0x0b1b25a4;
 	for (i = 0; i < IQK_ADDA_REG_NUM; i++)
-		rtl_set_bbreg(hw, adda_reg[i], BMASKDWORD, pathon);
+		rtl_set_bbreg(hw, adda_reg[i], MASKDWORD, pathon);
 }
 
 static void _rtl92d_phy_mac_setting_calibration(struct ieee80211_hw *hw,
@@ -1830,9 +1771,9 @@ static void _rtl92d_phy_patha_standby(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,  "Path-A standby mode!\n");
 
-	rtl_set_bbreg(hw, 0xe28, BMASKDWORD, 0x0);
-	rtl_set_bbreg(hw, RFPGA0_XA_LSSIPARAMETER, BMASKDWORD, 0x00010000);
-	rtl_set_bbreg(hw, 0xe28, BMASKDWORD, 0x80800000);
+	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x0);
+	rtl_set_bbreg(hw, RFPGA0_XA_LSSIPARAMETER, MASKDWORD, 0x00010000);
+	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x80800000);
 }
 
 static void _rtl92d_phy_pimode_switch(struct ieee80211_hw *hw, bool pi_mode)
@@ -1843,8 +1784,8 @@ static void _rtl92d_phy_pimode_switch(struct ieee80211_hw *hw, bool pi_mode)
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,
 		"BB Switch to %s mode!\n", pi_mode ? "PI" : "SI");
 	mode = pi_mode ? 0x01000100 : 0x01000000;
-	rtl_set_bbreg(hw, 0x820, BMASKDWORD, mode);
-	rtl_set_bbreg(hw, 0x828, BMASKDWORD, mode);
+	rtl_set_bbreg(hw, 0x820, MASKDWORD, mode);
+	rtl_set_bbreg(hw, 0x828, MASKDWORD, mode);
 }
 
 static void _rtl92d_phy_iq_calibrate(struct ieee80211_hw *hw, long result[][8],
@@ -1875,7 +1816,7 @@ static void _rtl92d_phy_iq_calibrate(struct ieee80211_hw *hw, long result[][8],
 
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,  "IQK for 2.4G :Start!!!\n");
 	if (t == 0) {
-		bbvalue = rtl_get_bbreg(hw, RFPGA0_RFMOD, BMASKDWORD);
+		bbvalue = rtl_get_bbreg(hw, RFPGA0_RFMOD, MASKDWORD);
 		RTPRINT(rtlpriv, FINIT, INIT_IQK,  "==>0x%08x\n", bbvalue);
 		RTPRINT(rtlpriv, FINIT, INIT_IQK, "IQ Calibration for %s\n",
 			is2t ? "2T2R" : "1T1R");
@@ -1898,40 +1839,40 @@ static void _rtl92d_phy_iq_calibrate(struct ieee80211_hw *hw, long result[][8],
 		_rtl92d_phy_pimode_switch(hw, true);
 
 	rtl_set_bbreg(hw, RFPGA0_RFMOD, BIT(24), 0x00);
-	rtl_set_bbreg(hw, ROFDM0_TRXPATHENABLE, BMASKDWORD, 0x03a05600);
-	rtl_set_bbreg(hw, ROFDM0_TRMUXPAR, BMASKDWORD, 0x000800e4);
-	rtl_set_bbreg(hw, RFPGA0_XCD_RFINTERFACESW, BMASKDWORD, 0x22204000);
+	rtl_set_bbreg(hw, ROFDM0_TRXPATHENABLE, MASKDWORD, 0x03a05600);
+	rtl_set_bbreg(hw, ROFDM0_TRMUXPAR, MASKDWORD, 0x000800e4);
+	rtl_set_bbreg(hw, RFPGA0_XCD_RFINTERFACESW, MASKDWORD, 0x22204000);
 	rtl_set_bbreg(hw, RFPGA0_ANALOGPARAMETER4, 0xf00000, 0x0f);
 	if (is2t) {
-		rtl_set_bbreg(hw, RFPGA0_XA_LSSIPARAMETER, BMASKDWORD,
+		rtl_set_bbreg(hw, RFPGA0_XA_LSSIPARAMETER, MASKDWORD,
 			      0x00010000);
-		rtl_set_bbreg(hw, RFPGA0_XB_LSSIPARAMETER, BMASKDWORD,
+		rtl_set_bbreg(hw, RFPGA0_XB_LSSIPARAMETER, MASKDWORD,
 			      0x00010000);
 	}
 	/* MAC settings */
 	_rtl92d_phy_mac_setting_calibration(hw, iqk_mac_reg,
 					    rtlphy->iqk_mac_backup);
 	/* Page B init */
-	rtl_set_bbreg(hw, 0xb68, BMASKDWORD, 0x0f600000);
+	rtl_set_bbreg(hw, 0xb68, MASKDWORD, 0x0f600000);
 	if (is2t)
-		rtl_set_bbreg(hw, 0xb6c, BMASKDWORD, 0x0f600000);
+		rtl_set_bbreg(hw, 0xb6c, MASKDWORD, 0x0f600000);
 	/* IQ calibration setting */
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,  "IQK setting!\n");
-	rtl_set_bbreg(hw, 0xe28, BMASKDWORD, 0x80800000);
-	rtl_set_bbreg(hw, 0xe40, BMASKDWORD, 0x01007c00);
-	rtl_set_bbreg(hw, 0xe44, BMASKDWORD, 0x01004800);
+	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x80800000);
+	rtl_set_bbreg(hw, 0xe40, MASKDWORD, 0x01007c00);
+	rtl_set_bbreg(hw, 0xe44, MASKDWORD, 0x01004800);
 	for (i = 0; i < retrycount; i++) {
 		patha_ok = _rtl92d_phy_patha_iqk(hw, is2t);
 		if (patha_ok == 0x03) {
 			RTPRINT(rtlpriv, FINIT, INIT_IQK,
 				"Path A IQK Success!!\n");
-			result[t][0] = (rtl_get_bbreg(hw, 0xe94, BMASKDWORD) &
+			result[t][0] = (rtl_get_bbreg(hw, 0xe94, MASKDWORD) &
 					0x3FF0000) >> 16;
-			result[t][1] = (rtl_get_bbreg(hw, 0xe9c, BMASKDWORD) &
+			result[t][1] = (rtl_get_bbreg(hw, 0xe9c, MASKDWORD) &
 					0x3FF0000) >> 16;
-			result[t][2] = (rtl_get_bbreg(hw, 0xea4, BMASKDWORD) &
+			result[t][2] = (rtl_get_bbreg(hw, 0xea4, MASKDWORD) &
 					0x3FF0000) >> 16;
-			result[t][3] = (rtl_get_bbreg(hw, 0xeac, BMASKDWORD) &
+			result[t][3] = (rtl_get_bbreg(hw, 0xeac, MASKDWORD) &
 					0x3FF0000) >> 16;
 			break;
 		} else if (i == (retrycount - 1) && patha_ok == 0x01) {
@@ -1939,9 +1880,9 @@ static void _rtl92d_phy_iq_calibrate(struct ieee80211_hw *hw, long result[][8],
 			RTPRINT(rtlpriv, FINIT, INIT_IQK,
 				"Path A IQK Only  Tx Success!!\n");
 
-			result[t][0] = (rtl_get_bbreg(hw, 0xe94, BMASKDWORD) &
+			result[t][0] = (rtl_get_bbreg(hw, 0xe94, MASKDWORD) &
 					0x3FF0000) >> 16;
-			result[t][1] = (rtl_get_bbreg(hw, 0xe9c, BMASKDWORD) &
+			result[t][1] = (rtl_get_bbreg(hw, 0xe9c, MASKDWORD) &
 					0x3FF0000) >> 16;
 		}
 	}
@@ -1957,22 +1898,22 @@ static void _rtl92d_phy_iq_calibrate(struct ieee80211_hw *hw, long result[][8],
 				RTPRINT(rtlpriv, FINIT, INIT_IQK,
 					"Path B IQK Success!!\n");
 				result[t][4] = (rtl_get_bbreg(hw, 0xeb4,
-					       BMASKDWORD) & 0x3FF0000) >> 16;
+					       MASKDWORD) & 0x3FF0000) >> 16;
 				result[t][5] = (rtl_get_bbreg(hw, 0xebc,
-					       BMASKDWORD) & 0x3FF0000) >> 16;
+					       MASKDWORD) & 0x3FF0000) >> 16;
 				result[t][6] = (rtl_get_bbreg(hw, 0xec4,
-					       BMASKDWORD) & 0x3FF0000) >> 16;
+					       MASKDWORD) & 0x3FF0000) >> 16;
 				result[t][7] = (rtl_get_bbreg(hw, 0xecc,
-					       BMASKDWORD) & 0x3FF0000) >> 16;
+					       MASKDWORD) & 0x3FF0000) >> 16;
 				break;
 			} else if (i == (retrycount - 1) && pathb_ok == 0x01) {
 				/* Tx IQK OK */
 				RTPRINT(rtlpriv, FINIT, INIT_IQK,
 					"Path B Only Tx IQK Success!!\n");
 				result[t][4] = (rtl_get_bbreg(hw, 0xeb4,
-					       BMASKDWORD) & 0x3FF0000) >> 16;
+					       MASKDWORD) & 0x3FF0000) >> 16;
 				result[t][5] = (rtl_get_bbreg(hw, 0xebc,
-					       BMASKDWORD) & 0x3FF0000) >> 16;
+					       MASKDWORD) & 0x3FF0000) >> 16;
 			}
 		}
 		if (0x00 == pathb_ok)
@@ -1984,7 +1925,7 @@ static void _rtl92d_phy_iq_calibrate(struct ieee80211_hw *hw, long result[][8],
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,
 		"IQK:Back to BB mode, load original value!\n");
 
-	rtl_set_bbreg(hw, 0xe28, BMASKDWORD, 0);
+	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0);
 	if (t != 0) {
 		/* Switch back BB to SI mode after finish IQ Calibration. */
 		if (!rtlphy->rfpi_enable)
@@ -2004,8 +1945,8 @@ static void _rtl92d_phy_iq_calibrate(struct ieee80211_hw *hw, long result[][8],
 							  rtlphy->iqk_bb_backup,
 							  IQK_BB_REG_NUM - 1);
 		/* load 0xe30 IQC default value */
-		rtl_set_bbreg(hw, 0xe30, BMASKDWORD, 0x01008c00);
-		rtl_set_bbreg(hw, 0xe34, BMASKDWORD, 0x01008c00);
+		rtl_set_bbreg(hw, 0xe30, MASKDWORD, 0x01008c00);
+		rtl_set_bbreg(hw, 0xe34, MASKDWORD, 0x01008c00);
 	}
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,  "<==\n");
 }
@@ -2042,7 +1983,7 @@ static void _rtl92d_phy_iq_calibrate_5g_normal(struct ieee80211_hw *hw,
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,  "IQK for 5G NORMAL:Start!!!\n");
 	mdelay(IQK_DELAY_TIME * 20);
 	if (t == 0) {
-		bbvalue = rtl_get_bbreg(hw, RFPGA0_RFMOD, BMASKDWORD);
+		bbvalue = rtl_get_bbreg(hw, RFPGA0_RFMOD, MASKDWORD);
 		RTPRINT(rtlpriv, FINIT, INIT_IQK,  "==>0x%08x\n", bbvalue);
 		RTPRINT(rtlpriv, FINIT, INIT_IQK, "IQ Calibration for %s\n",
 			is2t ? "2T2R" : "1T1R");
@@ -2072,38 +2013,38 @@ static void _rtl92d_phy_iq_calibrate_5g_normal(struct ieee80211_hw *hw,
 	if (!rtlphy->rfpi_enable)
 		_rtl92d_phy_pimode_switch(hw, true);
 	rtl_set_bbreg(hw, RFPGA0_RFMOD, BIT(24), 0x00);
-	rtl_set_bbreg(hw, ROFDM0_TRXPATHENABLE, BMASKDWORD, 0x03a05600);
-	rtl_set_bbreg(hw, ROFDM0_TRMUXPAR, BMASKDWORD, 0x000800e4);
-	rtl_set_bbreg(hw, RFPGA0_XCD_RFINTERFACESW, BMASKDWORD, 0x22208000);
+	rtl_set_bbreg(hw, ROFDM0_TRXPATHENABLE, MASKDWORD, 0x03a05600);
+	rtl_set_bbreg(hw, ROFDM0_TRMUXPAR, MASKDWORD, 0x000800e4);
+	rtl_set_bbreg(hw, RFPGA0_XCD_RFINTERFACESW, MASKDWORD, 0x22208000);
 	rtl_set_bbreg(hw, RFPGA0_ANALOGPARAMETER4, 0xf00000, 0x0f);
 
 	/* Page B init */
-	rtl_set_bbreg(hw, 0xb68, BMASKDWORD, 0x0f600000);
+	rtl_set_bbreg(hw, 0xb68, MASKDWORD, 0x0f600000);
 	if (is2t)
-		rtl_set_bbreg(hw, 0xb6c, BMASKDWORD, 0x0f600000);
+		rtl_set_bbreg(hw, 0xb6c, MASKDWORD, 0x0f600000);
 	/* IQ calibration setting  */
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,  "IQK setting!\n");
-	rtl_set_bbreg(hw, 0xe28, BMASKDWORD, 0x80800000);
-	rtl_set_bbreg(hw, 0xe40, BMASKDWORD, 0x10007c00);
-	rtl_set_bbreg(hw, 0xe44, BMASKDWORD, 0x01004800);
+	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x80800000);
+	rtl_set_bbreg(hw, 0xe40, MASKDWORD, 0x10007c00);
+	rtl_set_bbreg(hw, 0xe44, MASKDWORD, 0x01004800);
 	patha_ok = _rtl92d_phy_patha_iqk_5g_normal(hw, is2t);
 	if (patha_ok == 0x03) {
 		RTPRINT(rtlpriv, FINIT, INIT_IQK,  "Path A IQK Success!!\n");
-		result[t][0] = (rtl_get_bbreg(hw, 0xe94, BMASKDWORD) &
+		result[t][0] = (rtl_get_bbreg(hw, 0xe94, MASKDWORD) &
 				0x3FF0000) >> 16;
-		result[t][1] = (rtl_get_bbreg(hw, 0xe9c, BMASKDWORD) &
+		result[t][1] = (rtl_get_bbreg(hw, 0xe9c, MASKDWORD) &
 				0x3FF0000) >> 16;
-		result[t][2] = (rtl_get_bbreg(hw, 0xea4, BMASKDWORD) &
+		result[t][2] = (rtl_get_bbreg(hw, 0xea4, MASKDWORD) &
 				0x3FF0000) >> 16;
-		result[t][3] = (rtl_get_bbreg(hw, 0xeac, BMASKDWORD) &
+		result[t][3] = (rtl_get_bbreg(hw, 0xeac, MASKDWORD) &
 				0x3FF0000) >> 16;
 	} else if (patha_ok == 0x01) {	/* Tx IQK OK */
 		RTPRINT(rtlpriv, FINIT, INIT_IQK,
 			"Path A IQK Only  Tx Success!!\n");
 
-		result[t][0] = (rtl_get_bbreg(hw, 0xe94, BMASKDWORD) &
+		result[t][0] = (rtl_get_bbreg(hw, 0xe94, MASKDWORD) &
 				0x3FF0000) >> 16;
-		result[t][1] = (rtl_get_bbreg(hw, 0xe9c, BMASKDWORD) &
+		result[t][1] = (rtl_get_bbreg(hw, 0xe9c, MASKDWORD) &
 				0x3FF0000) >> 16;
 	} else {
 		RTPRINT(rtlpriv, FINIT, INIT_IQK,  "Path A IQK Fail!!\n");
@@ -2116,20 +2057,20 @@ static void _rtl92d_phy_iq_calibrate_5g_normal(struct ieee80211_hw *hw,
 		if (pathb_ok == 0x03) {
 			RTPRINT(rtlpriv, FINIT, INIT_IQK,
 				"Path B IQK Success!!\n");
-			result[t][4] = (rtl_get_bbreg(hw, 0xeb4, BMASKDWORD) &
+			result[t][4] = (rtl_get_bbreg(hw, 0xeb4, MASKDWORD) &
 			     0x3FF0000) >> 16;
-			result[t][5] = (rtl_get_bbreg(hw, 0xebc, BMASKDWORD) &
+			result[t][5] = (rtl_get_bbreg(hw, 0xebc, MASKDWORD) &
 			     0x3FF0000) >> 16;
-			result[t][6] = (rtl_get_bbreg(hw, 0xec4, BMASKDWORD) &
+			result[t][6] = (rtl_get_bbreg(hw, 0xec4, MASKDWORD) &
 			     0x3FF0000) >> 16;
-			result[t][7] = (rtl_get_bbreg(hw, 0xecc, BMASKDWORD) &
+			result[t][7] = (rtl_get_bbreg(hw, 0xecc, MASKDWORD) &
 			     0x3FF0000) >> 16;
 		} else if (pathb_ok == 0x01) { /* Tx IQK OK */
 			RTPRINT(rtlpriv, FINIT, INIT_IQK,
 				"Path B Only Tx IQK Success!!\n");
-			result[t][4] = (rtl_get_bbreg(hw, 0xeb4, BMASKDWORD) &
+			result[t][4] = (rtl_get_bbreg(hw, 0xeb4, MASKDWORD) &
 			     0x3FF0000) >> 16;
-			result[t][5] = (rtl_get_bbreg(hw, 0xebc, BMASKDWORD) &
+			result[t][5] = (rtl_get_bbreg(hw, 0xebc, MASKDWORD) &
 			     0x3FF0000) >> 16;
 		} else {
 			RTPRINT(rtlpriv, FINIT, INIT_IQK,
@@ -2140,7 +2081,7 @@ static void _rtl92d_phy_iq_calibrate_5g_normal(struct ieee80211_hw *hw,
 	/* Back to BB mode, load original value */
 	RTPRINT(rtlpriv, FINIT, INIT_IQK,
 		"IQK:Back to BB mode, load original value!\n");
-	rtl_set_bbreg(hw, 0xe28, BMASKDWORD, 0);
+	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0);
 	if (t != 0) {
 		if (is2t)
 			_rtl92d_phy_reload_adda_registers(hw, iqk_bb_reg,
@@ -2240,7 +2181,7 @@ static void _rtl92d_phy_patha_fill_iqk_matrix(struct ieee80211_hw *hw,
 		return;
 	} else if (iqk_ok) {
 		oldval_0 = (rtl_get_bbreg(hw, ROFDM0_XATxIQIMBALANCE,
-			BMASKDWORD) >> 22) & 0x3FF;	/* OFDM0_D */
+			MASKDWORD) >> 22) & 0x3FF;	/* OFDM0_D */
 		val_x = result[final_candidate][0];
 		if ((val_x & 0x00000200) != 0)
 			val_x = val_x | 0xFFFFFC00;
@@ -2271,7 +2212,7 @@ static void _rtl92d_phy_patha_fill_iqk_matrix(struct ieee80211_hw *hw,
 				      ((val_y * oldval_0 >> 7) & 0x1));
 		RTPRINT(rtlpriv, FINIT, INIT_IQK, "0xC80 = 0x%x\n",
 			rtl_get_bbreg(hw, ROFDM0_XATxIQIMBALANCE,
-				      BMASKDWORD));
+				      MASKDWORD));
 		if (txonly) {
 			RTPRINT(rtlpriv, FINIT, INIT_IQK,  "only Tx OK\n");
 			return;
@@ -2299,7 +2240,7 @@ static void _rtl92d_phy_pathb_fill_iqk_matrix(struct ieee80211_hw *hw,
 		return;
 	} else if (iqk_ok) {
 		oldval_1 = (rtl_get_bbreg(hw, ROFDM0_XBTxIQIMBALANCE,
-					  BMASKDWORD) >> 22) & 0x3FF;
+					  MASKDWORD) >> 22) & 0x3FF;
 		val_x = result[final_candidate][4];
 		if ((val_x & 0x00000200) != 0)
 			val_x = val_x | 0xFFFFFC00;
@@ -2657,7 +2598,7 @@ static void _rtl92d_phy_lc_calibrate_sw(struct ieee80211_hw *hw, bool is2t)
 		rf_mode[index] = rtl_read_byte(rtlpriv, offset);
 		/* 2. Set RF mode = standby mode */
 		rtl_set_rfreg(hw, (enum radio_path)index, RF_AC,
-			      BRFREGOFFSETMASK, 0x010000);
+			      RFREG_OFFSET_MASK, 0x010000);
 		if (rtlpci->init_ready) {
 			/* switch CV-curve control by LC-calibration */
 			rtl_set_rfreg(hw, (enum radio_path)index, RF_SYN_G7,
@@ -2667,16 +2608,16 @@ static void _rtl92d_phy_lc_calibrate_sw(struct ieee80211_hw *hw, bool is2t)
 				      0x08000, 0x01);
 		}
 		u4tmp = rtl_get_rfreg(hw, (enum radio_path)index, RF_SYN_G6,
-				  BRFREGOFFSETMASK);
+				  RFREG_OFFSET_MASK);
 		while ((!(u4tmp & BIT(11))) && timecount <= timeout) {
 			mdelay(50);
 			timecount += 50;
 			u4tmp = rtl_get_rfreg(hw, (enum radio_path)index,
-					      RF_SYN_G6, BRFREGOFFSETMASK);
+					      RF_SYN_G6, RFREG_OFFSET_MASK);
 		}
 		RTPRINT(rtlpriv, FINIT, INIT_IQK,
 			"PHY_LCK finish delay for %d ms=2\n", timecount);
-		u4tmp = rtl_get_rfreg(hw, index, RF_SYN_G4, BRFREGOFFSETMASK);
+		u4tmp = rtl_get_rfreg(hw, index, RF_SYN_G4, RFREG_OFFSET_MASK);
 		if (index == 0 && rtlhal->interfaceindex == 0) {
 			RTPRINT(rtlpriv, FINIT, INIT_IQK,
 				"path-A / 5G LCK\n");
@@ -2696,9 +2637,9 @@ static void _rtl92d_phy_lc_calibrate_sw(struct ieee80211_hw *hw, bool is2t)
 				      0x7f, i);
 
 			rtl_set_rfreg(hw, (enum radio_path)index, 0x4D,
-				BRFREGOFFSETMASK, 0x0);
+				RFREG_OFFSET_MASK, 0x0);
 			readval = rtl_get_rfreg(hw, (enum radio_path)index,
-					  0x4F, BRFREGOFFSETMASK);
+					  0x4F, RFREG_OFFSET_MASK);
 			curvecount_val[2 * i + 1] = (readval & 0xfffe0) >> 5;
 			/* reg 0x4f [4:0] */
 			/* reg 0x50 [19:10] */
@@ -2912,7 +2853,7 @@ static bool _rtl92d_phy_sw_chnl_step_by_step(struct ieee80211_hw *hw,
 				}
 				rtl_set_rfreg(hw, (enum radio_path)rfpath,
 					      currentcmd->para1,
-					      BRFREGOFFSETMASK,
+					      RFREG_OFFSET_MASK,
 					      rtlphy->rfreg_chnlval[rfpath]);
 				_rtl92d_phy_reload_imr_setting(hw, channel,
 							       rfpath);
@@ -2960,7 +2901,7 @@ u8 rtl92d_phy_sw_chnl(struct ieee80211_hw *hw)
 	if (rtlhal->macphymode == SINGLEMAC_SINGLEPHY &&
 	    rtlhal->bandset == BAND_ON_BOTH) {
 		ret_value = rtl_get_bbreg(hw, RFPGA0_XAB_RFPARAMETER,
-					  BMASKDWORD);
+					  MASKDWORD);
 		if (rtlphy->current_channel > 14 && !(ret_value & BIT(0)))
 			rtl92d_phy_switch_wirelessband(hw, BAND_ON_5G);
 		else if (rtlphy->current_channel <= 14 && (ret_value & BIT(0)))
@@ -3112,7 +3053,7 @@ static void _rtl92d_phy_set_rfsleep(struct ieee80211_hw *hw)
 	/* a.   TXPAUSE 0x522[7:0] = 0xFF  Pause MAC TX queue  */
 	rtl_write_byte(rtlpriv, REG_TXPAUSE, 0xFF);
 	/* b.   RF path 0 offset 0x00 = 0x00  disable RF  */
-	rtl_set_rfreg(hw, RF90_PATH_A, 0x00, BRFREGOFFSETMASK, 0x00);
+	rtl_set_rfreg(hw, RF90_PATH_A, 0x00, RFREG_OFFSET_MASK, 0x00);
 	/* c.   APSD_CTRL 0x600[7:0] = 0x40 */
 	rtl_write_byte(rtlpriv, REG_APSD_CTRL, 0x40);
 	/* d. APSD_CTRL 0x600[7:0] = 0x00
@@ -3120,12 +3061,12 @@ static void _rtl92d_phy_set_rfsleep(struct ieee80211_hw *hw)
 	 * RF path 0 offset 0x00 = 0x00
 	 * APSD_CTRL 0x600[7:0] = 0x40
 	 * */
-	u4btmp = rtl_get_rfreg(hw, RF90_PATH_A, 0, BRFREGOFFSETMASK);
+	u4btmp = rtl_get_rfreg(hw, RF90_PATH_A, 0, RFREG_OFFSET_MASK);
 	while (u4btmp != 0 && delay > 0) {
 		rtl_write_byte(rtlpriv, REG_APSD_CTRL, 0x0);
-		rtl_set_rfreg(hw, RF90_PATH_A, 0x00, BRFREGOFFSETMASK, 0x00);
+		rtl_set_rfreg(hw, RF90_PATH_A, 0x00, RFREG_OFFSET_MASK, 0x00);
 		rtl_write_byte(rtlpriv, REG_APSD_CTRL, 0x40);
-		u4btmp = rtl_get_rfreg(hw, RF90_PATH_A, 0, BRFREGOFFSETMASK);
+		u4btmp = rtl_get_rfreg(hw, RF90_PATH_A, 0, RFREG_OFFSET_MASK);
 		delay--;
 	}
 	if (delay == 0) {
@@ -3468,9 +3409,9 @@ void rtl92d_update_bbrf_configuration(struct ieee80211_hw *hw)
 		/* 5G LAN ON */
 		rtl_set_bbreg(hw, 0xB30, 0x00F00000, 0xa);
 		/* TX BB gain shift*1,Just for testchip,0xc80,0xc88 */
-		rtl_set_bbreg(hw, ROFDM0_XATxIQIMBALANCE, BMASKDWORD,
+		rtl_set_bbreg(hw, ROFDM0_XATxIQIMBALANCE, MASKDWORD,
 			      0x40000100);
-		rtl_set_bbreg(hw, ROFDM0_XBTxIQIMBALANCE, BMASKDWORD,
+		rtl_set_bbreg(hw, ROFDM0_XBTxIQIMBALANCE, MASKDWORD,
 			      0x40000100);
 		if (rtlhal->macphymode == DUALMAC_DUALPHY) {
 			rtl_set_bbreg(hw, RFPGA0_XAB_RFINTERFACESW,
@@ -3524,16 +3465,16 @@ void rtl92d_update_bbrf_configuration(struct ieee80211_hw *hw)
 		rtl_set_bbreg(hw, 0xB30, 0x00F00000, 0x0);
 		/* TX BB gain shift,Just for testchip,0xc80,0xc88 */
 		if (rtlefuse->internal_pa_5g[0])
-			rtl_set_bbreg(hw, ROFDM0_XATxIQIMBALANCE, BMASKDWORD,
+			rtl_set_bbreg(hw, ROFDM0_XATxIQIMBALANCE, MASKDWORD,
 				      0x2d4000b5);
 		else
-			rtl_set_bbreg(hw, ROFDM0_XATxIQIMBALANCE, BMASKDWORD,
+			rtl_set_bbreg(hw, ROFDM0_XATxIQIMBALANCE, MASKDWORD,
 				      0x20000080);
 		if (rtlefuse->internal_pa_5g[1])
-			rtl_set_bbreg(hw, ROFDM0_XBTxIQIMBALANCE, BMASKDWORD,
+			rtl_set_bbreg(hw, ROFDM0_XBTxIQIMBALANCE, MASKDWORD,
 				      0x2d4000b5);
 		else
-			rtl_set_bbreg(hw, ROFDM0_XBTxIQIMBALANCE, BMASKDWORD,
+			rtl_set_bbreg(hw, ROFDM0_XBTxIQIMBALANCE, MASKDWORD,
 				      0x20000080);
 		if (rtlhal->macphymode == DUALMAC_DUALPHY) {
 			rtl_set_bbreg(hw, RFPGA0_XAB_RFINTERFACESW,
@@ -3560,8 +3501,8 @@ void rtl92d_update_bbrf_configuration(struct ieee80211_hw *hw)
 		}
 	}
 	/* update IQK related settings */
-	rtl_set_bbreg(hw, ROFDM0_XARXIQIMBALANCE, BMASKDWORD, 0x40000100);
-	rtl_set_bbreg(hw, ROFDM0_XBRXIQIMBALANCE, BMASKDWORD, 0x40000100);
+	rtl_set_bbreg(hw, ROFDM0_XARXIQIMBALANCE, MASKDWORD, 0x40000100);
+	rtl_set_bbreg(hw, ROFDM0_XBRXIQIMBALANCE, MASKDWORD, 0x40000100);
 	rtl_set_bbreg(hw, ROFDM0_XCTxAFE, 0xF0000000, 0x00);
 	rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD, BIT(30) | BIT(28) |
 		      BIT(26) | BIT(24), 0x00);
@@ -3590,7 +3531,7 @@ void rtl92d_update_bbrf_configuration(struct ieee80211_hw *hw)
 	/* DMDP */
 	if (rtlphy->rf_type == RF_1T1R) {
 		/* Use antenna 0,0xc04,0xd04 */
-		rtl_set_bbreg(hw, ROFDM0_TRXPATHENABLE, BMASKBYTE0, 0x11);
+		rtl_set_bbreg(hw, ROFDM0_TRXPATHENABLE, MASKBYTE0, 0x11);
 		rtl_set_bbreg(hw, ROFDM1_TRXPATHENABLE, BDWORD, 0x1);
 
 		/* enable ad/da clock1 for dual-phy reg0x888 */
@@ -3612,7 +3553,7 @@ void rtl92d_update_bbrf_configuration(struct ieee80211_hw *hw)
 	} else {
 		/* Single PHY */
 		/* Use antenna 0 & 1,0xc04,0xd04 */
-		rtl_set_bbreg(hw, ROFDM0_TRXPATHENABLE, BMASKBYTE0, 0x33);
+		rtl_set_bbreg(hw, ROFDM0_TRXPATHENABLE, MASKBYTE0, 0x33);
 		rtl_set_bbreg(hw, ROFDM1_TRXPATHENABLE, BDWORD, 0x3);
 		/* disable ad/da clock1,0x888 */
 		rtl_set_bbreg(hw, RFPGA0_ADDALLOCKEN, BIT(12) | BIT(13), 0);
@@ -3620,9 +3561,9 @@ void rtl92d_update_bbrf_configuration(struct ieee80211_hw *hw)
 	for (rfpath = RF90_PATH_A; rfpath < rtlphy->num_total_rfpath;
 	     rfpath++) {
 		rtlphy->rfreg_chnlval[rfpath] = rtl_get_rfreg(hw, rfpath,
-						RF_CHNLBW, BRFREGOFFSETMASK);
+						RF_CHNLBW, RFREG_OFFSET_MASK);
 		rtlphy->reg_rf3c[rfpath] = rtl_get_rfreg(hw, rfpath, 0x3C,
-			BRFREGOFFSETMASK);
+			RFREG_OFFSET_MASK);
 	}
 	for (i = 0; i < 2; i++)
 		RT_TRACE(rtlpriv, COMP_RF, DBG_LOUD, "RF 0x18 = 0x%x\n",
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/reg.h b/drivers/net/wireless/rtlwifi/rtl8192de/reg.h
index b7498c5bafc5..7f29b8d765b3 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/reg.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/reg.h
@@ -1295,18 +1295,4 @@
 #define	BWORD1					0xc
 #define	BDWORD					0xf
 
-#define	BMASKBYTE0				0xff
-#define	BMASKBYTE1				0xff00
-#define	BMASKBYTE2				0xff0000
-#define	BMASKBYTE3				0xff000000
-#define	BMASKHWORD				0xffff0000
-#define	BMASKLWORD				0x0000ffff
-#define	BMASKDWORD				0xffffffff
-#define	BMASK12BITS				0xfff
-#define	BMASKH4BITS				0xf0000000
-#define BMASKOFDM_D				0xffc00000
-#define	BMASKCCK				0x3f3f3f3f
-
-#define BRFREGOFFSETMASK			0xfffff
-
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/rf.c b/drivers/net/wireless/rtlwifi/rtl8192de/rf.c
index 20144e0b4142..6a6ac540d5b5 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/rf.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/rf.c
@@ -125,7 +125,7 @@ void rtl92d_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
 	}
 
 	tmpval = tx_agc[RF90_PATH_A] & 0xff;
-	rtl_set_bbreg(hw, RTXAGC_A_CCK1_MCS32, BMASKBYTE1, tmpval);
+	rtl_set_bbreg(hw, RTXAGC_A_CCK1_MCS32, MASKBYTE1, tmpval);
 	RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
 		"CCK PWR 1M (rf-A) = 0x%x (reg 0x%x)\n",
 		tmpval, RTXAGC_A_CCK1_MCS32);
@@ -135,7 +135,7 @@ void rtl92d_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
 		"CCK PWR 2~11M (rf-A) = 0x%x (reg 0x%x)\n",
 		tmpval, RTXAGC_B_CCK11_A_CCK2_11);
 	tmpval = tx_agc[RF90_PATH_B] >> 24;
-	rtl_set_bbreg(hw, RTXAGC_B_CCK11_A_CCK2_11, BMASKBYTE0, tmpval);
+	rtl_set_bbreg(hw, RTXAGC_B_CCK11_A_CCK2_11, MASKBYTE0, tmpval);
 	RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
 		"CCK PWR 11M (rf-B) = 0x%x (reg 0x%x)\n",
 		tmpval, RTXAGC_B_CCK11_A_CCK2_11);
@@ -360,7 +360,7 @@ static void _rtl92d_write_ofdm_power_reg(struct ieee80211_hw *hw,
 			regoffset = regoffset_a[index];
 		else
 			regoffset = regoffset_b[index];
-		rtl_set_bbreg(hw, regoffset, BMASKDWORD, writeval);
+		rtl_set_bbreg(hw, regoffset, MASKDWORD, writeval);
 		RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
 			"Set 0x%x = %08x\n", regoffset, writeval);
 		if (((get_rf_type(rtlphy) == RF_2T2R) &&
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/trx.c b/drivers/net/wireless/rtlwifi/rtl8192de/trx.c
index 0eb0f4ae5920..99c2ab5dfceb 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/trx.c
@@ -545,7 +545,7 @@ static void _rtl92de_insert_emcontent(struct rtl_tcb_desc *ptcb_desc,
 
 void rtl92de_tx_fill_desc(struct ieee80211_hw *hw,
 			  struct ieee80211_hdr *hdr, u8 *pdesc_tx,
-			  struct ieee80211_tx_info *info,
+			  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)
@@ -786,7 +786,8 @@ void rtl92de_tx_fill_cmddesc(struct ieee80211_hw *hw,
 	SET_TX_DESC_OWN(pdesc, 1);
 }
 
-void rtl92de_set_desc(u8 *pdesc, bool istx, u8 desc_name, u8 *val)
+void rtl92de_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
+		      u8 desc_name, u8 *val)
 {
 	if (istx) {
 		switch (desc_name) {
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/trx.h b/drivers/net/wireless/rtlwifi/rtl8192de/trx.h
index c1b5dfb79d53..fb5cf0634e8d 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/trx.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/trx.h
@@ -728,8 +728,8 @@ struct rx_desc_92d {
 } __packed;
 
 void rtl92de_tx_fill_desc(struct ieee80211_hw *hw,
-			  struct ieee80211_hdr *hdr,
-			  u8 *pdesc, struct ieee80211_tx_info *info,
+			  struct ieee80211_hdr *hdr, u8 *pdesc,
+			  u8 *pbd_desc_tx, struct ieee80211_tx_info *info,
 			  struct ieee80211_sta *sta,
 			  struct sk_buff *skb, u8 hw_queue,
 			  struct rtl_tcb_desc *ptcb_desc);
@@ -737,7 +737,8 @@ bool rtl92de_rx_query_desc(struct ieee80211_hw *hw,
 			   struct rtl_stats *stats,
 			   struct ieee80211_rx_status *rx_status,
 			   u8 *pdesc, struct sk_buff *skb);
-void rtl92de_set_desc(u8 *pdesc, bool istx, u8 desc_name, u8 *val);
+void rtl92de_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
+		      u8 desc_name, u8 *val);
 u32 rtl92de_get_desc(u8 *pdesc, bool istx, u8 desc_name);
 void rtl92de_tx_polling(struct ieee80211_hw *hw, u8 hw_queue);
 void rtl92de_tx_fill_cmddesc(struct ieee80211_hw *hw, u8 *pdesc,
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/hw.c b/drivers/net/wireless/rtlwifi/rtl8192se/hw.c
index 4f461786a7eb..3015af167b2b 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/hw.c
@@ -251,7 +251,7 @@ void rtl92se_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			u8 e_aci = *val;
 			rtl92s_dm_init_edca_turbo(hw);
 
-			if (rtlpci->acm_method != eAcmWay2_SW)
+			if (rtlpci->acm_method != EACMWAY2_SW)
 				rtlpriv->cfg->ops->set_hw_reg(hw,
 						 HW_VAR_ACM_CTRL,
 						 &e_aci);
@@ -955,7 +955,7 @@ int rtl92se_hw_init(struct ieee80211_hw *hw)
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	u8 tmp_byte = 0;
-
+	unsigned long flags;
 	bool rtstatus = true;
 	u8 tmp_u1b;
 	int err = false;
@@ -967,6 +967,16 @@ int rtl92se_hw_init(struct ieee80211_hw *hw)
 
 	rtlpci->being_init_adapter = true;
 
+	/* As this function can take a very long time (up to 350 ms)
+	 * and can be called with irqs disabled, reenable the irqs
+	 * to let the other devices continue being serviced.
+	 *
+	 * It is safe doing so since our own interrupts will only be enabled
+	 * in a subsequent step.
+	 */
+	local_save_flags(flags);
+	local_irq_enable();
+
 	rtlpriv->intf_ops->disable_aspm(hw);
 
 	/* 1. MAC Initialize */
@@ -984,7 +994,8 @@ int rtl92se_hw_init(struct ieee80211_hw *hw)
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
 			 "Failed to download FW. Init HW without FW now... "
 			 "Please copy FW into /lib/firmware/rtlwifi\n");
-		return 1;
+		err = 1;
+		goto exit;
 	}
 
 	/* After FW download, we have to reset MAC register */
@@ -997,7 +1008,8 @@ int rtl92se_hw_init(struct ieee80211_hw *hw)
 	/* 3. Initialize MAC/PHY Config by MACPHY_reg.txt */
 	if (!rtl92s_phy_mac_config(hw)) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "MAC Config failed\n");
-		return rtstatus;
+		err = rtstatus;
+		goto exit;
 	}
 
 	/* because last function modify RCR, so we update
@@ -1016,7 +1028,8 @@ int rtl92se_hw_init(struct ieee80211_hw *hw)
 	/* 4. Initialize BB After MAC Config PHY_reg.txt, AGC_Tab.txt */
 	if (!rtl92s_phy_bb_config(hw)) {
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_EMERG, "BB Config failed\n");
-		return rtstatus;
+		err = rtstatus;
+		goto exit;
 	}
 
 	/* 5. Initiailze RF RAIO_A.txt RF RAIO_B.txt */
@@ -1033,7 +1046,8 @@ int rtl92se_hw_init(struct ieee80211_hw *hw)
 
 	if (!rtl92s_phy_rf_config(hw)) {
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "RF Config failed\n");
-		return rtstatus;
+		err = rtstatus;
+		goto exit;
 	}
 
 	/* After read predefined TXT, we must set BB/MAC/RF
@@ -1122,8 +1136,9 @@ int rtl92se_hw_init(struct ieee80211_hw *hw)
 
 	rtlpriv->cfg->ops->led_control(hw, LED_CTL_POWER_ON);
 	rtl92s_dm_init(hw);
+exit:
+	local_irq_restore(flags);
 	rtlpci->being_init_adapter = false;
-
 	return err;
 }
 
@@ -1135,12 +1150,13 @@ void rtl92se_set_mac_addr(struct rtl_io *io, const u8 *addr)
 void rtl92se_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;
+	u32 reg_rcr;
 
 	if (rtlpriv->psc.rfpwr_state != ERFON)
 		return;
 
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_RCR, (u8 *)(&reg_rcr));
+
 	if (check_bssid) {
 		reg_rcr |= (RCR_CBSSID);
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_RCR, (u8 *)(&reg_rcr));
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/phy.c b/drivers/net/wireless/rtlwifi/rtl8192se/phy.c
index 9c092e6eb3fe..77c5b5f35244 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/phy.c
@@ -30,6 +30,7 @@
 #include "../wifi.h"
 #include "../pci.h"
 #include "../ps.h"
+#include "../core.h"
 #include "reg.h"
 #include "def.h"
 #include "phy.h"
@@ -833,18 +834,7 @@ static bool _rtl92s_phy_config_bb(struct ieee80211_hw *hw, u8 configtype)
 
 	if (configtype == BASEBAND_CONFIG_PHY_REG) {
 		for (i = 0; i < phy_reg_len; i = i + 2) {
-			if (phy_reg_table[i] == 0xfe)
-				mdelay(50);
-			else if (phy_reg_table[i] == 0xfd)
-				mdelay(5);
-			else if (phy_reg_table[i] == 0xfc)
-				mdelay(1);
-			else if (phy_reg_table[i] == 0xfb)
-				udelay(50);
-			else if (phy_reg_table[i] == 0xfa)
-				udelay(5);
-			else if (phy_reg_table[i] == 0xf9)
-				udelay(1);
+			rtl_addr_delay(phy_reg_table[i]);
 
 			/* Add delay for ECS T20 & LG malow platform, */
 			udelay(1);
@@ -886,18 +876,7 @@ static bool _rtl92s_phy_set_bb_to_diff_rf(struct ieee80211_hw *hw,
 
 	if (configtype == BASEBAND_CONFIG_PHY_REG) {
 		for (i = 0; i < phy_regarray2xtxr_len; i = i + 3) {
-			if (phy_regarray2xtxr_table[i] == 0xfe)
-				mdelay(50);
-			else if (phy_regarray2xtxr_table[i] == 0xfd)
-				mdelay(5);
-			else if (phy_regarray2xtxr_table[i] == 0xfc)
-				mdelay(1);
-			else if (phy_regarray2xtxr_table[i] == 0xfb)
-				udelay(50);
-			else if (phy_regarray2xtxr_table[i] == 0xfa)
-				udelay(5);
-			else if (phy_regarray2xtxr_table[i] == 0xf9)
-				udelay(1);
+			rtl_addr_delay(phy_regarray2xtxr_table[i]);
 
 			rtl92s_phy_set_bb_reg(hw, phy_regarray2xtxr_table[i],
 				phy_regarray2xtxr_table[i + 1],
@@ -920,18 +899,7 @@ static bool _rtl92s_phy_config_bb_with_pg(struct ieee80211_hw *hw,
 
 	if (configtype == BASEBAND_CONFIG_PHY_REG) {
 		for (i = 0; i < phy_pg_len; i = i + 3) {
-			if (phy_table_pg[i] == 0xfe)
-				mdelay(50);
-			else if (phy_table_pg[i] == 0xfd)
-				mdelay(5);
-			else if (phy_table_pg[i] == 0xfc)
-				mdelay(1);
-			else if (phy_table_pg[i] == 0xfb)
-				udelay(50);
-			else if (phy_table_pg[i] == 0xfa)
-				udelay(5);
-			else if (phy_table_pg[i] == 0xf9)
-				udelay(1);
+			rtl_addr_delay(phy_table_pg[i]);
 
 			_rtl92s_store_pwrindex_diffrate_offset(hw,
 					phy_table_pg[i],
@@ -1034,28 +1002,9 @@ u8 rtl92s_phy_config_rf(struct ieee80211_hw *hw, enum radio_path rfpath)
 	switch (rfpath) {
 	case RF90_PATH_A:
 		for (i = 0; i < radio_a_tblen; i = i + 2) {
-			if (radio_a_table[i] == 0xfe)
-				/* Delay specific ms. Only RF configuration
-				 * requires delay. */
-				mdelay(50);
-			else if (radio_a_table[i] == 0xfd)
-				mdelay(5);
-			else if (radio_a_table[i] == 0xfc)
-				mdelay(1);
-			else if (radio_a_table[i] == 0xfb)
-				udelay(50);
-			else if (radio_a_table[i] == 0xfa)
-				udelay(5);
-			else if (radio_a_table[i] == 0xf9)
-				udelay(1);
-			else
-				rtl92s_phy_set_rf_reg(hw, rfpath,
-						      radio_a_table[i],
-						      MASK20BITS,
-						      radio_a_table[i + 1]);
+			rtl_rfreg_delay(hw, rfpath, radio_a_table[i],
+					MASK20BITS, radio_a_table[i + 1]);
 
-			/* Add delay for ECS T20 & LG malow platform */
-			udelay(1);
 		}
 
 		/* PA Bias current for inferiority IC */
@@ -1063,28 +1012,8 @@ u8 rtl92s_phy_config_rf(struct ieee80211_hw *hw, enum radio_path rfpath)
 		break;
 	case RF90_PATH_B:
 		for (i = 0; i < radio_b_tblen; i = i + 2) {
-			if (radio_b_table[i] == 0xfe)
-				/* Delay specific ms. Only RF configuration
-				 * requires delay.*/
-				mdelay(50);
-			else if (radio_b_table[i] == 0xfd)
-				mdelay(5);
-			else if (radio_b_table[i] == 0xfc)
-				mdelay(1);
-			else if (radio_b_table[i] == 0xfb)
-				udelay(50);
-			else if (radio_b_table[i] == 0xfa)
-				udelay(5);
-			else if (radio_b_table[i] == 0xf9)
-				udelay(1);
-			else
-				rtl92s_phy_set_rf_reg(hw, rfpath,
-						      radio_b_table[i],
-						      MASK20BITS,
-						      radio_b_table[i + 1]);
-
-			/* Add delay for ECS T20 & LG malow platform */
-			udelay(1);
+			rtl_rfreg_delay(hw, rfpath, radio_b_table[i],
+					MASK20BITS, radio_b_table[i + 1]);
 		}
 		break;
 	case RF90_PATH_C:
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/reg.h b/drivers/net/wireless/rtlwifi/rtl8192se/reg.h
index c81c83591940..e13043479b71 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/reg.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/reg.h
@@ -1165,16 +1165,4 @@
 
 #define	BTX_AGCRATECCK				0x7f00
 
-#define	MASKBYTE0				0xff
-#define	MASKBYTE1				0xff00
-#define	MASKBYTE2				0xff0000
-#define	MASKBYTE3				0xff000000
-#define	MASKHWORD				0xffff0000
-#define	MASKLWORD				0x0000ffff
-#define	MASKDWORD				0xffffffff
-
-#define	MAKS12BITS				0xfffff
-#define	MASK20BITS				0xfffff
-#define RFREG_OFFSET_MASK			0xfffff
-
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/rf.c b/drivers/net/wireless/rtlwifi/rtl8192se/rf.c
index 92d38ab3c60e..78a81c1e390b 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/rf.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/rf.c
@@ -52,7 +52,7 @@ static void _rtl92s_get_powerbase(struct ieee80211_hw *hw, u8 *p_pwrlevel,
 	/* We only care about the path A for legacy. */
 	if (rtlefuse->eeprom_version < 2) {
 		pwrbase0 = pwrlevel[0] + (rtlefuse->legacy_httxpowerdiff & 0xf);
-	} else if (rtlefuse->eeprom_version >= 2) {
+	} else {
 		legacy_pwrdiff = rtlefuse->txpwr_legacyhtdiff
 						[RF90_PATH_A][chnl - 1];
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/trx.c b/drivers/net/wireless/rtlwifi/rtl8192se/trx.c
index 27efbcdac6a9..36b48be8329c 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/trx.c
@@ -310,7 +310,7 @@ bool rtl92se_rx_query_desc(struct ieee80211_hw *hw, struct rtl_stats *stats,
 			/* during testing, hdr was NULL here */
 			return false;
 		}
-		if ((ieee80211_is_robust_mgmt_frame(hdr)) &&
+		if ((_ieee80211_is_robust_mgmt_frame(hdr)) &&
 			(ieee80211_has_protected(hdr->frame_control)))
 			rx_status->flag &= ~RX_FLAG_DECRYPTED;
 		else
@@ -336,7 +336,7 @@ bool rtl92se_rx_query_desc(struct ieee80211_hw *hw, struct rtl_stats *stats,
 
 void rtl92se_tx_fill_desc(struct ieee80211_hw *hw,
 		struct ieee80211_hdr *hdr, u8 *pdesc_tx,
-		struct ieee80211_tx_info *info,
+		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)
@@ -573,7 +573,8 @@ void rtl92se_tx_fill_cmddesc(struct ieee80211_hw *hw, u8 *pdesc,
 	}
 }
 
-void rtl92se_set_desc(u8 *pdesc, bool istx, u8 desc_name, u8 *val)
+void rtl92se_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
+		      u8 desc_name, u8 *val)
 {
 	if (istx) {
 		switch (desc_name) {
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/trx.h b/drivers/net/wireless/rtlwifi/rtl8192se/trx.h
index 64dd66f287c1..5a13f17e3b41 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/trx.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/trx.h
@@ -29,8 +29,9 @@
 #ifndef __REALTEK_PCI92SE_TRX_H__
 #define __REALTEK_PCI92SE_TRX_H__
 
-void rtl92se_tx_fill_desc(struct ieee80211_hw *hw, struct ieee80211_hdr *hdr,
-			  u8 *pdesc, struct ieee80211_tx_info *info,
+void rtl92se_tx_fill_desc(struct ieee80211_hw *hw,
+			  struct ieee80211_hdr *hdr, u8 *pdesc,
+			  u8 *pbd_desc_tx, struct ieee80211_tx_info *info,
 			  struct ieee80211_sta *sta,
 			  struct sk_buff *skb, u8 hw_queue,
 			  struct rtl_tcb_desc *ptcb_desc);
@@ -39,7 +40,8 @@ void rtl92se_tx_fill_cmddesc(struct ieee80211_hw *hw, u8 *pdesc, bool firstseg,
 bool rtl92se_rx_query_desc(struct ieee80211_hw *hw, struct rtl_stats *stats,
 			   struct ieee80211_rx_status *rx_status, u8 *pdesc,
 			   struct sk_buff *skb);
-void rtl92se_set_desc(u8 *pdesc, bool istx, u8 desc_name, u8 *val);
+void rtl92se_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
+		      u8 desc_name, u8 *val);
 u32 rtl92se_get_desc(u8 *pdesc, bool istx, u8 desc_name);
 void rtl92se_tx_polling(struct ieee80211_hw *hw, u8 hw_queue);
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/Makefile b/drivers/net/wireless/rtlwifi/rtl8723ae/Makefile
index 4ed731f09b1f..9c34a85fdb89 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/Makefile
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/Makefile
@@ -10,7 +10,6 @@ rtl8723ae-objs :=		\
 		led.o		\
 		phy.o		\
 		pwrseq.o	\
-		pwrseqcmd.o	\
 		rf.o		\
 		sw.o		\
 		table.o		\
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/def.h b/drivers/net/wireless/rtlwifi/rtl8723ae/def.h
index 8c110356dff9..debe261a7eeb 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/def.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/def.h
@@ -46,11 +46,6 @@
 #define E_CUT_VERSION			BIT(14)
 #define	RF_RL_ID			(BIT(31)|BIT(30)|BIT(29)|BIT(28))
 
-enum version_8723e {
-	VERSION_TEST_UMC_CHIP_8723 = 0x0081,
-	VERSION_NORMAL_UMC_CHIP_8723_1T1R_A_CUT = 0x0089,
-	VERSION_NORMAL_UMC_CHIP_8723_1T1R_B_CUT = 0x1089,
-};
 
 /* MASK */
 #define IC_TYPE_MASK			(BIT(0)|BIT(1)|BIT(2))
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/dm.c b/drivers/net/wireless/rtlwifi/rtl8723ae/dm.c
index a36eee28f9e7..863ddb3e2888 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/dm.c
@@ -35,6 +35,7 @@
 #include "def.h"
 #include "phy.h"
 #include "dm.h"
+#include "../rtl8723com/dm_common.h"
 #include "fw.h"
 #include "hal_btc.h"
 
@@ -483,16 +484,6 @@ static void rtl8723ae_dm_dig(struct ieee80211_hw *hw)
 	rtl8723ae_dm_ctrl_initgain_by_twoport(hw);
 }
 
-static void rtl8723ae_dm_init_dynamic_txpower(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	rtlpriv->dm.dynamic_txpower_enable = false;
-
-	rtlpriv->dm.last_dtp_lvl = TXHIGHPWRLEVEL_NORMAL;
-	rtlpriv->dm.dynamic_txhighpower_lvl = TXHIGHPWRLEVEL_NORMAL;
-}
-
 static void rtl8723ae_dm_dynamic_txpower(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -585,19 +576,6 @@ void rtl8723ae_dm_write_dig(struct ieee80211_hw *hw)
 	}
 }
 
-static void rtl8723ae_dm_pwdmonitor(struct ieee80211_hw *hw)
-{
-}
-
-void rtl8723ae_dm_init_edca_turbo(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	rtlpriv->dm.current_turbo_edca = false;
-	rtlpriv->dm.is_any_nonbepkts = false;
-	rtlpriv->dm.is_cur_rdlstate = false;
-}
-
 static void rtl8723ae_dm_check_edca_turbo(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -778,17 +756,6 @@ static void rtl8723ae_dm_refresh_rate_adaptive_mask(struct ieee80211_hw *hw)
 	}
 }
 
-static void rtl8723ae_dm_init_dynamic_bpowersaving(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	rtlpriv->dm_pstable.pre_ccastate = CCA_MAX;
-	rtlpriv->dm_pstable.cur_ccasate = CCA_MAX;
-	rtlpriv->dm_pstable.pre_rfstate = RF_MAX;
-	rtlpriv->dm_pstable.cur_rfstate = RF_MAX;
-	rtlpriv->dm_pstable.rssi_val_min = 0;
-}
-
 void rtl8723ae_dm_rf_saving(struct ieee80211_hw *hw, u8 force_in_normal)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -905,11 +872,11 @@ void rtl8723ae_dm_init(struct ieee80211_hw *hw)
 
 	rtlpriv->dm.dm_type = DM_TYPE_BYDRIVER;
 	rtl8723ae_dm_diginit(hw);
-	rtl8723ae_dm_init_dynamic_txpower(hw);
-	rtl8723ae_dm_init_edca_turbo(hw);
+	rtl8723_dm_init_dynamic_txpower(hw);
+	rtl8723_dm_init_edca_turbo(hw);
 	rtl8723ae_dm_init_rate_adaptive_mask(hw);
 	rtl8723ae_dm_initialize_txpower_tracking(hw);
-	rtl8723ae_dm_init_dynamic_bpowersaving(hw);
+	rtl8723_dm_init_dynamic_bb_powersaving(hw);
 }
 
 void rtl8723ae_dm_watchdog(struct ieee80211_hw *hw)
@@ -930,7 +897,6 @@ void rtl8723ae_dm_watchdog(struct ieee80211_hw *hw)
 	if ((ppsc->rfpwr_state == ERFON) &&
 	    ((!fw_current_inpsmode) && fw_ps_awake) &&
 	    (!ppsc->rfchange_inprogress)) {
-		rtl8723ae_dm_pwdmonitor(hw);
 		rtl8723ae_dm_dig(hw);
 		rtl8723ae_dm_false_alarm_counter_statistics(hw);
 		rtl8723ae_dm_dynamic_bpowersaving(hw);
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/dm.h b/drivers/net/wireless/rtlwifi/rtl8723ae/dm.h
index a372b0204456..d253bb53d03e 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/dm.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/dm.h
@@ -147,7 +147,6 @@ enum dm_dig_connect_e {
 void rtl8723ae_dm_init(struct ieee80211_hw *hw);
 void rtl8723ae_dm_watchdog(struct ieee80211_hw *hw);
 void rtl8723ae_dm_write_dig(struct ieee80211_hw *hw);
-void rtl8723ae_dm_init_edca_turbo(struct ieee80211_hw *hw);
 void rtl8723ae_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw);
 void rtl8723ae_dm_rf_saving(struct ieee80211_hw *hw, u8 bforce_in_normal);
 void rtl8723ae_dm_bt_coexist(struct ieee80211_hw *hw);
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/fw.c b/drivers/net/wireless/rtlwifi/rtl8723ae/fw.c
index ba1502b172a6..728b7563ad36 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/fw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/fw.c
@@ -34,199 +34,7 @@
 #include "reg.h"
 #include "def.h"
 #include "fw.h"
-
-static void _rtl8723ae_enable_fw_download(struct ieee80211_hw *hw, bool enable)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 tmp;
-	if (enable) {
-		tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
-		rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN + 1, tmp | 0x04);
-
-		tmp = rtl_read_byte(rtlpriv, REG_MCUFWDL);
-		rtl_write_byte(rtlpriv, REG_MCUFWDL, tmp | 0x01);
-
-		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);
-
-		rtl_write_byte(rtlpriv, REG_MCUFWDL + 1, 0x00);
-	}
-}
-
-static void _rtl8723ae_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 _rtl8723ae_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);
-	_rtl8723ae_fw_block_write(hw, buffer, size);
-}
-
-static void _rtl8723ae_write_fw(struct ieee80211_hw *hw,
-				enum version_8723e version, u8 *buffer,
-				u32 size)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 *bufferPtr = (u8 *) buffer;
-	u32 page_nums, remain_size;
-	u32 page, offset;
-
-	RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE, "FW size is %d bytes,\n", size);
-
-	page_nums = size / FW_8192C_PAGE_SIZE;
-	remain_size = size % FW_8192C_PAGE_SIZE;
-
-	if (page_nums > 6) {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "Page numbers should not be greater then 6\n");
-	}
-
-	for (page = 0; page < page_nums; page++) {
-		offset = page * FW_8192C_PAGE_SIZE;
-		_rtl8723ae_fw_page_write(hw, page, (bufferPtr + offset),
-					 FW_8192C_PAGE_SIZE);
-	}
-
-	if (remain_size) {
-		offset = page_nums * FW_8192C_PAGE_SIZE;
-		page = page_nums;
-		_rtl8723ae_fw_page_write(hw, page, (bufferPtr + offset),
-					 remain_size);
-	}
-
-	RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE, "FW write done.\n");
-}
-
-static int _rtl8723ae_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(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "chksum report faill ! REG_MCUFWDL:0x%08x .\n",
-			 value32);
-		goto exit;
-	}
-
-	RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
-		 "Checksum report OK ! REG_MCUFWDL:0x%08x .\n", value32);
-
-	value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
-	value32 |= MCUFWDL_RDY;
-	value32 &= ~WINTINI_RDY;
-	rtl_write_dword(rtlpriv, REG_MCUFWDL, value32);
-
-	counter = 0;
-
-	do {
-		value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
-		if (value32 & WINTINI_RDY) {
-			RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
-				 "Polling FW ready success!! REG_MCUFWDL:0x%08x .\n",
-				 value32);
-			err = 0;
-			goto exit;
-		}
-
-		mdelay(FW_8192C_POLLING_DELAY);
-
-	} while (counter++ < FW_8192C_POLLING_TIMEOUT_COUNT);
-
-	RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-		 "Polling FW ready fail!! REG_MCUFWDL:0x%08x .\n", value32);
-
-exit:
-	return err;
-}
-
-int rtl8723ae_download_fw(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl8723ae_firmware_header *pfwheader;
-	u8 *pfwdata;
-	u32 fwsize;
-	int err;
-	enum version_8723e version = rtlhal->version;
-
-	if (!rtlhal->pfirmware)
-		return 1;
-
-	pfwheader = (struct rtl8723ae_firmware_header *)rtlhal->pfirmware;
-	pfwdata = (u8 *) rtlhal->pfirmware;
-	fwsize = rtlhal->fwsize;
-
-	if (IS_FW_HEADER_EXIST(pfwheader)) {
-		RT_TRACE(rtlpriv, COMP_FW, DBG_DMESG,
-			 "Firmware Version(%d), Signature(%#x),Size(%d)\n",
-			 pfwheader->version, pfwheader->signature,
-			 (int)sizeof(struct rtl8723ae_firmware_header));
-
-		pfwdata = pfwdata + sizeof(struct rtl8723ae_firmware_header);
-		fwsize = fwsize - sizeof(struct rtl8723ae_firmware_header);
-	}
-
-	if (rtl_read_byte(rtlpriv, REG_MCUFWDL)&BIT(7)) {
-		rtl8723ae_firmware_selfreset(hw);
-		rtl_write_byte(rtlpriv, REG_MCUFWDL, 0x00);
-	}
-	_rtl8723ae_enable_fw_download(hw, true);
-	_rtl8723ae_write_fw(hw, version, pfwdata, fwsize);
-	_rtl8723ae_enable_fw_download(hw, false);
-
-	err = _rtl8723ae_fw_free_to_go(hw);
-	if (err) {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "Firmware is not ready to run!\n");
-	} else {
-		RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
-			 "Firmware is ready to run!\n");
-	}
-	return 0;
-}
+#include "../rtl8723com/fw_common.h"
 
 static bool rtl8723ae_check_fw_read_last_h2c(struct ieee80211_hw *hw, u8 boxnum)
 {
@@ -463,50 +271,6 @@ void rtl8723ae_fill_h2c_cmd(struct ieee80211_hw *hw,
 	return;
 }
 
-void rtl8723ae_firmware_selfreset(struct ieee80211_hw *hw)
-{
-	u8 u1tmp;
-	u8 delay = 100;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	rtl_write_byte(rtlpriv, REG_HMETFR + 3, 0x20);
-	u1tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
-
-	while (u1tmp & BIT(2)) {
-		delay--;
-		if (delay == 0)
-			break;
-		udelay(50);
-		u1tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
-	}
-	if (delay == 0) {
-		u1tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
-		rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN + 1, u1tmp&(~BIT(2)));
-	}
-}
-
-void rtl8723ae_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 u1_h2c_set_pwrmode[3] = { 0 };
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-
-	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "FW LPS mode = %d\n", mode);
-
-	SET_H2CCMD_PWRMODE_PARM_MODE(u1_h2c_set_pwrmode, mode);
-	SET_H2CCMD_PWRMODE_PARM_SMART_PS(u1_h2c_set_pwrmode,
-					 (rtlpriv->mac80211.p2p) ?
-					 ppsc->smart_ps : 1);
-	SET_H2CCMD_PWRMODE_PARM_BCN_PASS_TIME(u1_h2c_set_pwrmode,
-					      ppsc->reg_max_lps_awakeintvl);
-
-	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
-		      "rtl8723ae_set_fw_rsvdpagepkt(): u1_h2c_set_pwrmode\n",
-		      u1_h2c_set_pwrmode, 3);
-	rtl8723ae_fill_h2c_cmd(hw, H2C_SETPWRMODE, 3, u1_h2c_set_pwrmode);
-
-}
-
 static bool _rtl8723ae_cmd_send_packet(struct ieee80211_hw *hw,
 				       struct sk_buff *skb)
 {
@@ -812,7 +576,6 @@ void rtl8723ae_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
 			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;
@@ -836,3 +599,24 @@ void rtl8723ae_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
 	}
 	rtl8723ae_fill_h2c_cmd(hw, H2C_P2P_PS_OFFLOAD, 1, (u8 *)p2p_ps_offload);
 }
+
+void rtl8723ae_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 u1_h2c_set_pwrmode[3] = { 0 };
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+
+	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "FW LPS mode = %d\n", mode);
+
+	SET_H2CCMD_PWRMODE_PARM_MODE(u1_h2c_set_pwrmode, mode);
+	SET_H2CCMD_PWRMODE_PARM_SMART_PS_23A(u1_h2c_set_pwrmode,
+					     (rtlpriv->mac80211.p2p) ?
+					     ppsc->smart_ps : 1);
+	SET_H2CCMD_PWRMODE_PARM_BCN_PASS_TIME(u1_h2c_set_pwrmode,
+					      ppsc->reg_max_lps_awakeintvl);
+
+	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
+		      "rtl8723ae_set_fw_rsvdpagepkt(): u1_h2c_set_pwrmode\n",
+		      u1_h2c_set_pwrmode, 3);
+	rtl8723ae_fill_h2c_cmd(hw, H2C_SETPWRMODE, 3, u1_h2c_set_pwrmode);
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/fw.h b/drivers/net/wireless/rtlwifi/rtl8723ae/fw.h
index ed3b795e6980..d355b85dd9fe 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/fw.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/fw.h
@@ -34,7 +34,7 @@
 #define FW_8192C_END_ADDRESS			0x3FFF
 #define FW_8192C_PAGE_SIZE			4096
 #define FW_8192C_POLLING_DELAY			5
-#define FW_8192C_POLLING_TIMEOUT_COUNT		1000
+#define FW_8192C_POLLING_TIMEOUT_COUNT		6000
 
 #define BEACON_PG				0
 #define PSPOLL_PG				2
@@ -65,21 +65,9 @@ struct rtl8723ae_firmware_header {
 	u32 rsvd5;
 };
 
-enum rtl8192c_h2c_cmd {
-	H2C_AP_OFFLOAD = 0,
-	H2C_SETPWRMODE = 1,
-	H2C_JOINBSSRPT = 2,
-	H2C_RSVDPAGE = 3,
-	H2C_RSSI_REPORT = 4,
-	H2C_P2P_PS_CTW_CMD = 5,
-	H2C_P2P_PS_OFFLOAD = 6,
-	H2C_RA_MASK = 7,
-	MAX_H2CCMD
-};
-
 #define SET_H2CCMD_PWRMODE_PARM_MODE(__ph2ccmd, __val)			\
 	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 8, __val)
-#define SET_H2CCMD_PWRMODE_PARM_SMART_PS(__ph2ccmd, __val)		\
+#define SET_H2CCMD_PWRMODE_PARM_SMART_PS_23A(__ph2ccmd, __val)		\
 	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+1, 0, 8, __val)
 #define SET_H2CCMD_PWRMODE_PARM_BCN_PASS_TIME(__ph2ccmd, __val)	\
 	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+2, 0, 8, __val)
@@ -92,10 +80,8 @@ enum rtl8192c_h2c_cmd {
 #define SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(__ph2ccmd, __val)		\
 	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+2, 0, 8, __val)
 
-int rtl8723ae_download_fw(struct ieee80211_hw *hw);
 void rtl8723ae_fill_h2c_cmd(struct ieee80211_hw *hw, u8 element_id,
 			    u32 cmd_len, u8 *p_cmdbuffer);
-void rtl8723ae_firmware_selfreset(struct ieee80211_hw *hw);
 void rtl8723ae_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode);
 void rtl8723ae_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished);
 void rtl8723ae_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus);
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/hal_bt_coexist.c b/drivers/net/wireless/rtlwifi/rtl8723ae/hal_bt_coexist.c
index 3d092e4b0b7f..48fee1be78c2 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/hal_bt_coexist.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/hal_bt_coexist.c
@@ -31,6 +31,8 @@
 #include "../pci.h"
 #include "dm.h"
 #include "fw.h"
+#include "../rtl8723com/fw_common.h"
+#include "../rtl8723com/fw_common.h"
 #include "phy.h"
 #include "reg.h"
 #include "hal_btc.h"
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/hal_btc.c b/drivers/net/wireless/rtlwifi/rtl8723ae/hal_btc.c
index 68c28340f791..5d534df8d90c 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/hal_btc.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/hal_btc.c
@@ -30,7 +30,9 @@
 #include "hal_btc.h"
 #include "../pci.h"
 #include "phy.h"
+#include "../rtl8723com/phy_common.h"
 #include "fw.h"
+#include "../rtl8723com/fw_common.h"
 #include "reg.h"
 #include "def.h"
 
@@ -391,13 +393,13 @@ static void rtl8723ae_dm_bt_set_sw_full_time_dac_swing(struct ieee80211_hw *hw,
 	if (sw_dac_swing_on) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 			 "[BTCoex], SwDacSwing = 0x%x\n", sw_dac_swing_lvl);
-		rtl8723ae_phy_set_bb_reg(hw, 0x880, 0xff000000,
-					 sw_dac_swing_lvl);
+		rtl8723_phy_set_bb_reg(hw, 0x880, 0xff000000,
+				       sw_dac_swing_lvl);
 		rtlpcipriv->bt_coexist.sw_coexist_all_off = false;
 	} else {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 			 "[BTCoex], SwDacSwing Off!\n");
-		rtl8723ae_phy_set_bb_reg(hw, 0x880, 0xff000000, 0xc0);
+		rtl8723_phy_set_bb_reg(hw, 0x880, 0xff000000, 0xc0);
 	}
 }
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/hw.c b/drivers/net/wireless/rtlwifi/rtl8723ae/hw.c
index c333dfd116b8..f4c9852d1e1e 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/hw.c
@@ -38,10 +38,11 @@
 #include "def.h"
 #include "phy.h"
 #include "dm.h"
+#include "../rtl8723com/dm_common.h"
 #include "fw.h"
+#include "../rtl8723com/fw_common.h"
 #include "led.h"
 #include "hw.h"
-#include "pwrseqcmd.h"
 #include "pwrseq.h"
 #include "btc.h"
 
@@ -304,9 +305,9 @@ void rtl8723ae_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 		break; }
 	case HW_VAR_AC_PARAM:{
 		u8 e_aci = *((u8 *) val);
-		rtl8723ae_dm_init_edca_turbo(hw);
+		rtl8723_dm_init_edca_turbo(hw);
 
-		if (rtlpci->acm_method != eAcmWay2_SW)
+		if (rtlpci->acm_method != EACMWAY2_SW)
 			rtlpriv->cfg->ops->set_hw_reg(hw,
 						      HW_VAR_ACM_CTRL,
 						      (u8 *) (&e_aci));
@@ -880,23 +881,33 @@ int rtl8723ae_hw_init(struct ieee80211_hw *hw)
 	bool rtstatus = true;
 	int err;
 	u8 tmp_u1b;
+	unsigned long flags;
 
 	rtlpriv->rtlhal.being_init_adapter = true;
+	/* As this function can take a very long time (up to 350 ms)
+	 * and can be called with irqs disabled, reenable the irqs
+	 * to let the other devices continue being serviced.
+	 *
+	 * It is safe doing so since our own interrupts will only be enabled
+	 * in a subsequent step.
+	 */
+	local_save_flags(flags);
+	local_irq_enable();
+
 	rtlpriv->intf_ops->disable_aspm(hw);
 	rtstatus = _rtl8712e_init_mac(hw);
 	if (rtstatus != true) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "Init MAC failed\n");
 		err = 1;
-		return err;
+		goto exit;
 	}
 
-	err = rtl8723ae_download_fw(hw);
+	err = rtl8723_download_fw(hw, false);
 	if (err) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
 			 "Failed to download FW. Init HW without FW now..\n");
 		err = 1;
-		rtlhal->fw_ready = false;
-		return err;
+		goto exit;
 	} else {
 		rtlhal->fw_ready = true;
 	}
@@ -971,6 +982,8 @@ int rtl8723ae_hw_init(struct ieee80211_hw *hw)
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "under 1.5V\n");
 	}
 	rtl8723ae_dm_init(hw);
+exit:
+	local_irq_restore(flags);
 	rtlpriv->rtlhal.being_init_adapter = false;
 	return err;
 }
@@ -1112,12 +1125,13 @@ static int _rtl8723ae_set_media_status(struct ieee80211_hw *hw,
 void rtl8723ae_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;
+	u32 reg_rcr;
 
 	if (rtlpriv->psc.rfpwr_state != ERFON)
 		return;
 
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_RCR, (u8 *)(&reg_rcr));
+
 	if (check_bssid == true) {
 		reg_rcr |= (RCR_CBSSID_DATA | RCR_CBSSID_BCN);
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_RCR,
@@ -1153,7 +1167,7 @@ void rtl8723ae_set_qos(struct ieee80211_hw *hw, int aci)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
-	rtl8723ae_dm_init_edca_turbo(hw);
+	rtl8723_dm_init_edca_turbo(hw);
 	switch (aci) {
 	case AC1_BK:
 		rtl_write_dword(rtlpriv, REG_EDCA_BK_PARAM, 0xa44f);
@@ -1655,7 +1669,7 @@ static void _rtl8723ae_read_adapter_info(struct ieee80211_hw *hw,
 				    CHK_SVID_SMID(0x10EC, 0x9185))
 					rtlhal->oem_id = RT_CID_TOSHIBA;
 				else if (rtlefuse->eeprom_svid == 0x1025)
-					rtlhal->oem_id = RT_CID_819x_Acer;
+					rtlhal->oem_id = RT_CID_819X_ACER;
 				else if (CHK_SVID_SMID(0x10EC, 0x6191) ||
 					 CHK_SVID_SMID(0x10EC, 0x6192) ||
 					 CHK_SVID_SMID(0x10EC, 0x6193) ||
@@ -1665,7 +1679,7 @@ static void _rtl8723ae_read_adapter_info(struct ieee80211_hw *hw,
 					 CHK_SVID_SMID(0x10EC, 0x8191) ||
 					 CHK_SVID_SMID(0x10EC, 0x8192) ||
 					 CHK_SVID_SMID(0x10EC, 0x8193))
-					rtlhal->oem_id = RT_CID_819x_SAMSUNG;
+					rtlhal->oem_id = RT_CID_819X_SAMSUNG;
 				else if (CHK_SVID_SMID(0x10EC, 0x8195) ||
 					 CHK_SVID_SMID(0x10EC, 0x9195) ||
 					 CHK_SVID_SMID(0x10EC, 0x7194) ||
@@ -1673,24 +1687,24 @@ static void _rtl8723ae_read_adapter_info(struct ieee80211_hw *hw,
 					 CHK_SVID_SMID(0x10EC, 0x8201) ||
 					 CHK_SVID_SMID(0x10EC, 0x8202) ||
 					 CHK_SVID_SMID(0x10EC, 0x9200))
-					rtlhal->oem_id = RT_CID_819x_Lenovo;
+					rtlhal->oem_id = RT_CID_819X_LENOVO;
 				else if (CHK_SVID_SMID(0x10EC, 0x8197) ||
 					 CHK_SVID_SMID(0x10EC, 0x9196))
-					rtlhal->oem_id = RT_CID_819x_CLEVO;
+					rtlhal->oem_id = RT_CID_819X_CLEVO;
 				else if (CHK_SVID_SMID(0x1028, 0x8194) ||
 					 CHK_SVID_SMID(0x1028, 0x8198) ||
 					 CHK_SVID_SMID(0x1028, 0x9197) ||
 					 CHK_SVID_SMID(0x1028, 0x9198))
-					rtlhal->oem_id = RT_CID_819x_DELL;
+					rtlhal->oem_id = RT_CID_819X_DELL;
 				else if (CHK_SVID_SMID(0x103C, 0x1629))
-					rtlhal->oem_id = RT_CID_819x_HP;
+					rtlhal->oem_id = RT_CID_819X_HP;
 				else if (CHK_SVID_SMID(0x1A32, 0x2315))
-					rtlhal->oem_id = RT_CID_819x_QMI;
+					rtlhal->oem_id = RT_CID_819X_QMI;
 				else if (CHK_SVID_SMID(0x10EC, 0x8203))
-					rtlhal->oem_id = RT_CID_819x_PRONETS;
+					rtlhal->oem_id = RT_CID_819X_PRONETS;
 				else if (CHK_SVID_SMID(0x1043, 0x84B5))
 					rtlhal->oem_id =
-						 RT_CID_819x_Edimax_ASUS;
+						 RT_CID_819X_EDIMAX_ASUS;
 				else
 					rtlhal->oem_id = RT_CID_DEFAULT;
 			} else if (rtlefuse->eeprom_did == 0x8178) {
@@ -1712,12 +1726,12 @@ static void _rtl8723ae_read_adapter_info(struct ieee80211_hw *hw,
 				    CHK_SVID_SMID(0x10EC, 0x9185))
 					rtlhal->oem_id = RT_CID_TOSHIBA;
 				else if (rtlefuse->eeprom_svid == 0x1025)
-					rtlhal->oem_id = RT_CID_819x_Acer;
+					rtlhal->oem_id = RT_CID_819X_ACER;
 				else if (CHK_SVID_SMID(0x10EC, 0x8186))
-					rtlhal->oem_id = RT_CID_819x_PRONETS;
+					rtlhal->oem_id = RT_CID_819X_PRONETS;
 				else if (CHK_SVID_SMID(0x1043, 0x8486))
 					rtlhal->oem_id =
-						     RT_CID_819x_Edimax_ASUS;
+						     RT_CID_819X_EDIMAX_ASUS;
 				else
 					rtlhal->oem_id = RT_CID_DEFAULT;
 			} else {
@@ -1731,7 +1745,7 @@ static void _rtl8723ae_read_adapter_info(struct ieee80211_hw *hw,
 			rtlhal->oem_id = RT_CID_CCX;
 			break;
 		case EEPROM_CID_QMI:
-			rtlhal->oem_id = RT_CID_819x_QMI;
+			rtlhal->oem_id = RT_CID_819X_QMI;
 			break;
 		case EEPROM_CID_WHQL:
 				break;
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/phy.c b/drivers/net/wireless/rtlwifi/rtl8723ae/phy.c
index 5d318a85eda4..3ea78afdec73 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/phy.c
@@ -30,12 +30,14 @@
 #include "../wifi.h"
 #include "../pci.h"
 #include "../ps.h"
+#include "../core.h"
 #include "reg.h"
 #include "def.h"
 #include "phy.h"
 #include "rf.h"
 #include "dm.h"
 #include "table.h"
+#include "../rtl8723com/phy_common.h"
 
 /* static forward definitions */
 static u32 _phy_fw_rf_serial_read(struct ieee80211_hw *hw,
@@ -43,72 +45,17 @@ static u32 _phy_fw_rf_serial_read(struct ieee80211_hw *hw,
 static void _phy_fw_rf_serial_write(struct ieee80211_hw *hw,
 				    enum radio_path rfpath,
 				    u32 offset, u32 data);
-static u32 _phy_rf_serial_read(struct ieee80211_hw *hw,
-			       enum radio_path rfpath, u32 offset);
-static void _phy_rf_serial_write(struct ieee80211_hw *hw,
-				 enum radio_path rfpath, u32 offset, u32 data);
-static u32 _phy_calculate_bit_shift(u32 bitmask);
 static bool _phy_bb8192c_config_parafile(struct ieee80211_hw *hw);
 static bool _phy_cfg_mac_w_header(struct ieee80211_hw *hw);
 static bool _phy_cfg_bb_w_header(struct ieee80211_hw *hw, u8 configtype);
 static bool _phy_cfg_bb_w_pgheader(struct ieee80211_hw *hw, u8 configtype);
-static void _phy_init_bb_rf_reg_def(struct ieee80211_hw *hw);
-static bool _phy_set_sw_chnl_cmdarray(struct swchnlcmd *cmdtable,
-				      u32 cmdtableidx, u32 cmdtablesz,
-				      enum swchnlcmd_id cmdid,
-				      u32 para1, u32 para2,
-				      u32 msdelay);
 static bool _phy_sw_chnl_step_by_step(struct ieee80211_hw *hw, u8 channel,
 				      u8 *stage, u8 *step, u32 *delay);
 static u8 _phy_dbm_to_txpwr_Idx(struct ieee80211_hw *hw,
 				enum wireless_mode wirelessmode,
 				long power_indbm);
-static long _phy_txpwr_idx_to_dbm(struct ieee80211_hw *hw,
-				  enum wireless_mode wirelessmode, u8 txpwridx);
 static void rtl8723ae_phy_set_io(struct ieee80211_hw *hw);
 
-u32 rtl8723ae_phy_query_bb_reg(struct ieee80211_hw *hw, u32 regaddr,
-			       u32 bitmask)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 returnvalue, originalvalue, bitshift;
-
-	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
-		 "regaddr(%#x), bitmask(%#x)\n", regaddr, bitmask);
-	originalvalue = rtl_read_dword(rtlpriv, regaddr);
-	bitshift = _phy_calculate_bit_shift(bitmask);
-	returnvalue = (originalvalue & bitmask) >> bitshift;
-
-	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
-		 "BBR MASK=0x%x Addr[0x%x]=0x%x\n", bitmask, regaddr,
-		 originalvalue);
-
-	return returnvalue;
-}
-
-void rtl8723ae_phy_set_bb_reg(struct ieee80211_hw *hw,
-			      u32 regaddr, u32 bitmask, u32 data)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 originalvalue, bitshift;
-
-	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
-		 "regaddr(%#x), bitmask(%#x), data(%#x)\n", regaddr,
-		 bitmask, data);
-
-	if (bitmask != MASKDWORD) {
-		originalvalue = rtl_read_dword(rtlpriv, regaddr);
-		bitshift = _phy_calculate_bit_shift(bitmask);
-		data = ((originalvalue & (~bitmask)) | (data << bitshift));
-	}
-
-	rtl_write_dword(rtlpriv, regaddr, data);
-
-	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
-		 "regaddr(%#x), bitmask(%#x), data(%#x)\n",
-		 regaddr, bitmask, data);
-}
-
 u32 rtl8723ae_phy_query_rf_reg(struct ieee80211_hw *hw,
 			       enum radio_path rfpath, u32 regaddr, u32 bitmask)
 {
@@ -124,11 +71,11 @@ u32 rtl8723ae_phy_query_rf_reg(struct ieee80211_hw *hw,
 	spin_lock_irqsave(&rtlpriv->locks.rf_lock, flags);
 
 	if (rtlphy->rf_mode != RF_OP_BY_FW)
-		original_value = _phy_rf_serial_read(hw, rfpath, regaddr);
+		original_value = rtl8723_phy_rf_serial_read(hw, rfpath, regaddr);
 	else
 		original_value = _phy_fw_rf_serial_read(hw, rfpath, regaddr);
 
-	bitshift = _phy_calculate_bit_shift(bitmask);
+	bitshift = rtl8723_phy_calculate_bit_shift(bitmask);
 	readback_value = (original_value & bitmask) >> bitshift;
 
 	spin_unlock_irqrestore(&rtlpriv->locks.rf_lock, flags);
@@ -157,19 +104,19 @@ void rtl8723ae_phy_set_rf_reg(struct ieee80211_hw *hw,
 
 	if (rtlphy->rf_mode != RF_OP_BY_FW) {
 		if (bitmask != RFREG_OFFSET_MASK) {
-			original_value = _phy_rf_serial_read(hw, rfpath,
-							     regaddr);
-			bitshift = _phy_calculate_bit_shift(bitmask);
+			original_value = rtl8723_phy_rf_serial_read(hw, rfpath,
+								    regaddr);
+			bitshift = rtl8723_phy_calculate_bit_shift(bitmask);
 			data = ((original_value & (~bitmask)) |
 			       (data << bitshift));
 		}
 
-		_phy_rf_serial_write(hw, rfpath, regaddr, data);
+		rtl8723_phy_rf_serial_write(hw, rfpath, regaddr, data);
 	} else {
 		if (bitmask != RFREG_OFFSET_MASK) {
 			original_value = _phy_fw_rf_serial_read(hw, rfpath,
 								regaddr);
-			bitshift = _phy_calculate_bit_shift(bitmask);
+			bitshift = rtl8723_phy_calculate_bit_shift(bitmask);
 			data = ((original_value & (~bitmask)) |
 			       (data << bitshift));
 		}
@@ -197,87 +144,6 @@ static void _phy_fw_rf_serial_write(struct ieee80211_hw *hw,
 	RT_ASSERT(false, "deprecated!\n");
 }
 
-static u32 _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 &= 0x3f;
-	newoffset = offset;
-	if (RT_CANNOT_IO(hw)) {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "return all one\n");
-		return 0xFFFFFFFF;
-	}
-	tmplong = rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD);
-	if (rfpath == RF90_PATH_A)
-		tmplong2 = tmplong;
-	else
-		tmplong2 = rtl_get_bbreg(hw, pphyreg->rfhssi_para2, MASKDWORD);
-	tmplong2 = (tmplong2 & (~BLSSIREADADDRESS)) |
-	    (newoffset << 23) | BLSSIREADEDGE;
-	rtl_set_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD,
-		      tmplong & (~BLSSIREADEDGE));
-	mdelay(1);
-	rtl_set_bbreg(hw, pphyreg->rfhssi_para2, MASKDWORD, tmplong2);
-	mdelay(1);
-	rtl_set_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD,
-		      tmplong | BLSSIREADEDGE);
-	mdelay(1);
-	if (rfpath == RF90_PATH_A)
-		rfpi_enable = (u8) rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER1,
-						 BIT(8));
-	else if (rfpath == RF90_PATH_B)
-		rfpi_enable = (u8) rtl_get_bbreg(hw, RFPGA0_XB_HSSIPARAMETER1,
-						 BIT(8));
-	if (rfpi_enable)
-		retvalue = rtl_get_bbreg(hw, pphyreg->rf_rbpi,
-					 BLSSIREADBACKDATA);
-	else
-		retvalue = rtl_get_bbreg(hw, pphyreg->rf_rb,
-					 BLSSIREADBACKDATA);
-	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE, "RFR-%d Addr[0x%x]=0x%x\n",
-		 rfpath, pphyreg->rf_rb, retvalue);
-	return retvalue;
-}
-
-static void _phy_rf_serial_write(struct ieee80211_hw *hw,
-				 enum radio_path rfpath, u32 offset, u32 data)
-{
-	u32 data_and_addr;
-	u32 newoffset;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct bb_reg_def *pphyreg = &rtlphy->phyreg_def[rfpath];
-
-	if (RT_CANNOT_IO(hw)) {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "stop\n");
-		return;
-	}
-	offset &= 0x3f;
-	newoffset = offset;
-	data_and_addr = ((newoffset << 20) | (data & 0x000fffff)) & 0x0fffffff;
-	rtl_set_bbreg(hw, pphyreg->rf3wire_offset, MASKDWORD, data_and_addr);
-	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE, "RFW-%d Addr[0x%x]=0x%x\n",
-		 rfpath, pphyreg->rf3wire_offset, data_and_addr);
-}
-
-static u32 _phy_calculate_bit_shift(u32 bitmask)
-{
-	u32 i;
-
-	for (i = 0; i <= 31; i++) {
-		if (((bitmask >> i) & 0x1) == 1)
-			break;
-	}
-	return i;
-}
-
 static void _rtl8723ae_phy_bb_config_1t(struct ieee80211_hw *hw)
 {
 	rtl_set_bbreg(hw, RFPGA0_TXINFO, 0x3, 0x2);
@@ -307,7 +173,7 @@ bool rtl8723ae_phy_bb_config(struct ieee80211_hw *hw)
 	u8 tmpu1b;
 	u8 reg_hwparafile = 1;
 
-	_phy_init_bb_rf_reg_def(hw);
+	rtl8723_phy_init_bb_rf_reg_def(hw);
 
 	/* 1. 0x28[1] = 1 */
 	tmpu1b = rtl_read_byte(rtlpriv, REG_AFE_PLL_CTRL);
@@ -412,18 +278,7 @@ static bool _phy_cfg_bb_w_header(struct ieee80211_hw *hw, u8 configtype)
 	phy_regarray_table = RTL8723EPHY_REG_1TARRAY;
 	if (configtype == BASEBAND_CONFIG_PHY_REG) {
 		for (i = 0; i < phy_reg_arraylen; i = i + 2) {
-			if (phy_regarray_table[i] == 0xfe)
-				mdelay(50);
-			else if (phy_regarray_table[i] == 0xfd)
-				mdelay(5);
-			else if (phy_regarray_table[i] == 0xfc)
-				mdelay(1);
-			else if (phy_regarray_table[i] == 0xfb)
-				udelay(50);
-			else if (phy_regarray_table[i] == 0xfa)
-				udelay(5);
-			else if (phy_regarray_table[i] == 0xf9)
-				udelay(1);
+			rtl_addr_delay(phy_regarray_table[i]);
 			rtl_set_bbreg(hw, phy_regarray_table[i], MASKDWORD,
 				      phy_regarray_table[i + 1]);
 			udelay(1);
@@ -585,18 +440,7 @@ static bool _phy_cfg_bb_w_pgheader(struct ieee80211_hw *hw, u8 configtype)
 
 	if (configtype == BASEBAND_CONFIG_PHY_REG) {
 		for (i = 0; i < phy_regarray_pg_len; i = i + 3) {
-			if (phy_regarray_table_pg[i] == 0xfe)
-				mdelay(50);
-			else if (phy_regarray_table_pg[i] == 0xfd)
-				mdelay(5);
-			else if (phy_regarray_table_pg[i] == 0xfc)
-				mdelay(1);
-			else if (phy_regarray_table_pg[i] == 0xfb)
-				udelay(50);
-			else if (phy_regarray_table_pg[i] == 0xfa)
-				udelay(5);
-			else if (phy_regarray_table_pg[i] == 0xf9)
-				udelay(1);
+			rtl_addr_delay(phy_regarray_table_pg[i]);
 
 			_st_pwrIdx_dfrate_off(hw, phy_regarray_table_pg[i],
 					      phy_regarray_table_pg[i + 1],
@@ -623,24 +467,9 @@ bool rtl8723ae_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
 	switch (rfpath) {
 	case RF90_PATH_A:
 		for (i = 0; i < radioa_arraylen; i = i + 2) {
-			if (radioa_array_table[i] == 0xfe)
-				mdelay(50);
-			else if (radioa_array_table[i] == 0xfd)
-				mdelay(5);
-			else if (radioa_array_table[i] == 0xfc)
-				mdelay(1);
-			else if (radioa_array_table[i] == 0xfb)
-				udelay(50);
-			else if (radioa_array_table[i] == 0xfa)
-				udelay(5);
-			else if (radioa_array_table[i] == 0xf9)
-				udelay(1);
-			else {
-				rtl_set_rfreg(hw, rfpath, radioa_array_table[i],
-					      RFREG_OFFSET_MASK,
-					      radioa_array_table[i + 1]);
-				udelay(1);
-			}
+			rtl_rfreg_delay(hw, rfpath, radioa_array_table[i],
+					RFREG_OFFSET_MASK,
+					radioa_array_table[i + 1]);
 		}
 		break;
 	case RF90_PATH_B:
@@ -690,92 +519,6 @@ void rtl8723ae_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw)
 		 ROFDM0_RXDETECTOR3, rtlphy->framesync);
 }
 
-static void _phy_init_bb_rf_reg_def(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-
-	rtlphy->phyreg_def[RF90_PATH_A].rfintfs = RFPGA0_XAB_RFINTERFACESW;
-	rtlphy->phyreg_def[RF90_PATH_B].rfintfs = RFPGA0_XAB_RFINTERFACESW;
-	rtlphy->phyreg_def[RF90_PATH_C].rfintfs = RFPGA0_XCD_RFINTERFACESW;
-	rtlphy->phyreg_def[RF90_PATH_D].rfintfs = RFPGA0_XCD_RFINTERFACESW;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rfintfi = RFPGA0_XAB_RFINTERFACERB;
-	rtlphy->phyreg_def[RF90_PATH_B].rfintfi = RFPGA0_XAB_RFINTERFACERB;
-	rtlphy->phyreg_def[RF90_PATH_C].rfintfi = RFPGA0_XCD_RFINTERFACERB;
-	rtlphy->phyreg_def[RF90_PATH_D].rfintfi = RFPGA0_XCD_RFINTERFACERB;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rfintfo = RFPGA0_XA_RFINTERFACEOE;
-	rtlphy->phyreg_def[RF90_PATH_B].rfintfo = RFPGA0_XB_RFINTERFACEOE;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rfintfe = RFPGA0_XA_RFINTERFACEOE;
-	rtlphy->phyreg_def[RF90_PATH_B].rfintfe = RFPGA0_XB_RFINTERFACEOE;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rf3wire_offset =
-			    RFPGA0_XA_LSSIPARAMETER;
-	rtlphy->phyreg_def[RF90_PATH_B].rf3wire_offset =
-			    RFPGA0_XB_LSSIPARAMETER;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rflssi_select = rFPGA0_XAB_RFPARAMETER;
-	rtlphy->phyreg_def[RF90_PATH_B].rflssi_select = rFPGA0_XAB_RFPARAMETER;
-	rtlphy->phyreg_def[RF90_PATH_C].rflssi_select = rFPGA0_XCD_RFPARAMETER;
-	rtlphy->phyreg_def[RF90_PATH_D].rflssi_select = rFPGA0_XCD_RFPARAMETER;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rftxgain_stage = RFPGA0_TXGAINSTAGE;
-	rtlphy->phyreg_def[RF90_PATH_B].rftxgain_stage = RFPGA0_TXGAINSTAGE;
-	rtlphy->phyreg_def[RF90_PATH_C].rftxgain_stage = RFPGA0_TXGAINSTAGE;
-	rtlphy->phyreg_def[RF90_PATH_D].rftxgain_stage = RFPGA0_TXGAINSTAGE;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rfhssi_para1 = RFPGA0_XA_HSSIPARAMETER1;
-	rtlphy->phyreg_def[RF90_PATH_B].rfhssi_para1 = RFPGA0_XB_HSSIPARAMETER1;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rfhssi_para2 = RFPGA0_XA_HSSIPARAMETER2;
-	rtlphy->phyreg_def[RF90_PATH_B].rfhssi_para2 = RFPGA0_XB_HSSIPARAMETER2;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rfsw_ctrl = RFPGA0_XAB_SWITCHCONTROL;
-	rtlphy->phyreg_def[RF90_PATH_B].rfsw_ctrl = RFPGA0_XAB_SWITCHCONTROL;
-	rtlphy->phyreg_def[RF90_PATH_C].rfsw_ctrl = RFPGA0_XCD_SWITCHCONTROL;
-	rtlphy->phyreg_def[RF90_PATH_D].rfsw_ctrl = RFPGA0_XCD_SWITCHCONTROL;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rfagc_control1 = ROFDM0_XAAGCCORE1;
-	rtlphy->phyreg_def[RF90_PATH_B].rfagc_control1 = ROFDM0_XBAGCCORE1;
-	rtlphy->phyreg_def[RF90_PATH_C].rfagc_control1 = ROFDM0_XCAGCCORE1;
-	rtlphy->phyreg_def[RF90_PATH_D].rfagc_control1 = ROFDM0_XDAGCCORE1;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rfagc_control2 = ROFDM0_XAAGCCORE2;
-	rtlphy->phyreg_def[RF90_PATH_B].rfagc_control2 = ROFDM0_XBAGCCORE2;
-	rtlphy->phyreg_def[RF90_PATH_C].rfagc_control2 = ROFDM0_XCAGCCORE2;
-	rtlphy->phyreg_def[RF90_PATH_D].rfagc_control2 = ROFDM0_XDAGCCORE2;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rfrxiq_imbal = ROFDM0_XARXIQIMBALANCE;
-	rtlphy->phyreg_def[RF90_PATH_B].rfrxiq_imbal = ROFDM0_XBRXIQIMBALANCE;
-	rtlphy->phyreg_def[RF90_PATH_C].rfrxiq_imbal = ROFDM0_XCRXIQIMBANLANCE;
-	rtlphy->phyreg_def[RF90_PATH_D].rfrxiq_imbal = ROFDM0_XDRXIQIMBALANCE;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rfrx_afe = ROFDM0_XARXAFE;
-	rtlphy->phyreg_def[RF90_PATH_B].rfrx_afe = ROFDM0_XBRXAFE;
-	rtlphy->phyreg_def[RF90_PATH_C].rfrx_afe = ROFDM0_XCRXAFE;
-	rtlphy->phyreg_def[RF90_PATH_D].rfrx_afe = ROFDM0_XDRXAFE;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rftxiq_imbal = ROFDM0_XATXIQIMBALANCE;
-	rtlphy->phyreg_def[RF90_PATH_B].rftxiq_imbal = ROFDM0_XBTXIQIMBALANCE;
-	rtlphy->phyreg_def[RF90_PATH_C].rftxiq_imbal = ROFDM0_XCTXIQIMBALANCE;
-	rtlphy->phyreg_def[RF90_PATH_D].rftxiq_imbal = ROFDM0_XDTXIQIMBALANCE;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rftx_afe = ROFDM0_XATXAFE;
-	rtlphy->phyreg_def[RF90_PATH_B].rftx_afe = ROFDM0_XBTXAFE;
-	rtlphy->phyreg_def[RF90_PATH_C].rftx_afe = ROFDM0_XCTXAFE;
-	rtlphy->phyreg_def[RF90_PATH_D].rftx_afe = ROFDM0_XDTXAFE;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rf_rb = RFPGA0_XA_LSSIREADBACK;
-	rtlphy->phyreg_def[RF90_PATH_B].rf_rb = RFPGA0_XB_LSSIREADBACK;
-	rtlphy->phyreg_def[RF90_PATH_C].rf_rb = RFPGA0_XC_LSSIREADBACK;
-	rtlphy->phyreg_def[RF90_PATH_D].rf_rb = RFPGA0_XD_LSSIREADBACK;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rf_rbpi = TRANSCEIVEA_HSPI_READBACK;
-	rtlphy->phyreg_def[RF90_PATH_B].rf_rbpi = TRANSCEIVEB_HSPI_READBACK;
-}
-
 void rtl8723ae_phy_get_txpower_level(struct ieee80211_hw *hw, long *powerlevel)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -785,17 +528,17 @@ void rtl8723ae_phy_get_txpower_level(struct ieee80211_hw *hw, long *powerlevel)
 	long txpwr_dbm;
 
 	txpwr_level = rtlphy->cur_cck_txpwridx;
-	txpwr_dbm = _phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_B, txpwr_level);
+	txpwr_dbm = rtl8723_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_B, txpwr_level);
 	txpwr_level = rtlphy->cur_ofdm24g_txpwridx +
 	    rtlefuse->legacy_ht_txpowerdiff;
-	if (_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_G, txpwr_level) > txpwr_dbm)
-		txpwr_dbm = _phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_G,
+	if (rtl8723_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_G, txpwr_level) > txpwr_dbm)
+		txpwr_dbm = rtl8723_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_G,
 						  txpwr_level);
 	txpwr_level = rtlphy->cur_ofdm24g_txpwridx;
-	if (_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_N_24G, txpwr_level) >
+	if (rtl8723_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_N_24G, txpwr_level) >
 	    txpwr_dbm)
-		txpwr_dbm = _phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_N_24G,
-						  txpwr_level);
+		txpwr_dbm = rtl8723_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_N_24G,
+							 txpwr_level);
 	*powerlevel = txpwr_dbm;
 }
 
@@ -912,28 +655,6 @@ static u8 _phy_dbm_to_txpwr_Idx(struct ieee80211_hw *hw,
 	return txpwridx;
 }
 
-static long _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 rtl8723ae_phy_set_bw_mode_callback(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -1117,26 +838,26 @@ static bool _phy_sw_chnl_step_by_step(struct ieee80211_hw *hw, u8 channel,
 	u8 num_total_rfpath = rtlphy->num_total_rfpath;
 
 	precommoncmdcnt = 0;
-	_phy_set_sw_chnl_cmdarray(precommoncmd, precommoncmdcnt++,
-				  MAX_PRECMD_CNT, CMDID_SET_TXPOWEROWER_LEVEL,
-				  0, 0, 0);
-	_phy_set_sw_chnl_cmdarray(precommoncmd, precommoncmdcnt++,
-				  MAX_PRECMD_CNT, CMDID_END, 0, 0, 0);
+	rtl8723_phy_set_sw_chnl_cmdarray(precommoncmd, precommoncmdcnt++,
+					 MAX_PRECMD_CNT, CMDID_SET_TXPOWEROWER_LEVEL,
+					 0, 0, 0);
+	rtl8723_phy_set_sw_chnl_cmdarray(precommoncmd, precommoncmdcnt++,
+					 MAX_PRECMD_CNT, CMDID_END, 0, 0, 0);
 	postcommoncmdcnt = 0;
 
-	_phy_set_sw_chnl_cmdarray(postcommoncmd, postcommoncmdcnt++,
-				  MAX_POSTCMD_CNT, CMDID_END, 0, 0, 0);
+	rtl8723_phy_set_sw_chnl_cmdarray(postcommoncmd, postcommoncmdcnt++,
+					 MAX_POSTCMD_CNT, CMDID_END, 0, 0, 0);
 	rfdependcmdcnt = 0;
 
 	RT_ASSERT((channel >= 1 && channel <= 14),
 		  "illegal channel for Zebra: %d\n", channel);
 
-	_phy_set_sw_chnl_cmdarray(rfdependcmd, rfdependcmdcnt++,
-				  MAX_RFDEPENDCMD_CNT, CMDID_RF_WRITEREG,
+	rtl8723_phy_set_sw_chnl_cmdarray(rfdependcmd, rfdependcmdcnt++,
+					 MAX_RFDEPENDCMD_CNT, CMDID_RF_WRITEREG,
 				  RF_CHNLBW, channel, 10);
 
-	_phy_set_sw_chnl_cmdarray(rfdependcmd, rfdependcmdcnt++,
-				  MAX_RFDEPENDCMD_CNT, CMDID_END, 0, 0, 0);
+	rtl8723_phy_set_sw_chnl_cmdarray(rfdependcmd, rfdependcmdcnt++,
+					 MAX_RFDEPENDCMD_CNT, CMDID_END, 0, 0, 0);
 
 	do {
 		switch (*stage) {
@@ -1204,29 +925,6 @@ static bool _phy_sw_chnl_step_by_step(struct ieee80211_hw *hw, u8 channel,
 	return false;
 }
 
-static bool _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 _rtl8723ae_phy_path_a_iqk(struct ieee80211_hw *hw, bool config_pathb)
 {
 	u32 reg_eac, reg_e94, reg_e9c, reg_ea4;
@@ -1297,136 +995,6 @@ static u8 _rtl8723ae_phy_path_b_iqk(struct ieee80211_hw *hw)
 	return result;
 }
 
-static void phy_path_a_fill_iqk_matrix(struct ieee80211_hw *hw, bool iqk_ok,
-				       long result[][8], u8 final_candidate,
-				       bool btxonly)
-{
-	u32 oldval_0, x, tx0_a, reg;
-	long y, tx0_c;
-
-	if (final_candidate == 0xFF) {
-		return;
-	} else if (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, 0xca0, 0xF0000000, reg);
-	}
-}
-
-static void phy_save_adda_regs(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 phy_save_mac_regs(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 phy_reload_adda_regs(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 phy_reload_mac_regs(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 _rtl8723ae_phy_path_adda_on(struct ieee80211_hw *hw,
-					u32 *addareg, bool is_patha_on,
-					bool is2t)
-{
-	u32 pathOn;
-	u32 i;
-
-	pathOn = is_patha_on ? 0x04db25a4 : 0x0b1b25a4;
-	if (false == is2t) {
-		pathOn = 0x0bdb25a0;
-		rtl_set_bbreg(hw, addareg[0], MASKDWORD, 0x0b1b25a0);
-	} 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 _rtl8723ae_phy_mac_setting_calibration(struct ieee80211_hw *hw,
-						   u32 *macreg, u32 *macbackup)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 i = 0;
-
-	rtl_write_byte(rtlpriv, macreg[i], 0x3F);
-
-	for (i = 1; i < (IQK_MAC_REG_NUM - 1); i++)
-		rtl_write_byte(rtlpriv, macreg[i],
-			       (u8) (macbackup[i] & (~BIT(3))));
-	rtl_write_byte(rtlpriv, macreg[i], (u8) (macbackup[i] & (~BIT(5))));
-}
-
-static void _rtl8723ae_phy_path_a_standby(struct ieee80211_hw *hw)
-{
-	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x0);
-	rtl_set_bbreg(hw, 0x840, MASKDWORD, 0x00010000);
-	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x80800000);
-}
-
-static void _rtl8723ae_phy_pi_mode_switch(struct ieee80211_hw *hw, bool pi_mode)
-{
-	u32 mode;
-
-	mode = pi_mode ? 0x01000100 : 0x01000000;
-	rtl_set_bbreg(hw, 0x820, MASKDWORD, mode);
-	rtl_set_bbreg(hw, 0x828, MASKDWORD, mode);
-}
-
 static bool phy_simularity_comp(struct ieee80211_hw *hw, long result[][8],
 				u8 c1, u8 c2)
 {
@@ -1498,10 +1066,12 @@ static void _rtl8723ae_phy_iq_calibrate(struct ieee80211_hw *hw,
 	const u32 retrycount = 2;
 
 	if (t == 0) {
-		phy_save_adda_regs(hw, adda_reg, rtlphy->adda_backup, 16);
-		phy_save_mac_regs(hw, iqk_mac_reg, rtlphy->iqk_mac_backup);
+		rtl8723_save_adda_registers(hw, adda_reg, rtlphy->adda_backup,
+					    16);
+		rtl8723_phy_save_mac_registers(hw, iqk_mac_reg,
+					       rtlphy->iqk_mac_backup);
 	}
-	_rtl8723ae_phy_path_adda_on(hw, adda_reg, true, is2t);
+	rtl8723_phy_path_adda_on(hw, adda_reg, true, is2t);
 	if (t == 0) {
 		rtlphy->rfpi_enable = (u8) rtl_get_bbreg(hw,
 						 RFPGA0_XA_HSSIPARAMETER1,
@@ -1509,7 +1079,7 @@ static void _rtl8723ae_phy_iq_calibrate(struct ieee80211_hw *hw,
 	}
 
 	if (!rtlphy->rfpi_enable)
-		_rtl8723ae_phy_pi_mode_switch(hw, true);
+		rtl8723_phy_pi_mode_switch(hw, true);
 	if (t == 0) {
 		rtlphy->reg_c04 = rtl_get_bbreg(hw, 0xc04, MASKDWORD);
 		rtlphy->reg_c08 = rtl_get_bbreg(hw, 0xc08, MASKDWORD);
@@ -1522,7 +1092,7 @@ static void _rtl8723ae_phy_iq_calibrate(struct ieee80211_hw *hw,
 		rtl_set_bbreg(hw, 0x840, MASKDWORD, 0x00010000);
 		rtl_set_bbreg(hw, 0x844, MASKDWORD, 0x00010000);
 	}
-	_rtl8723ae_phy_mac_setting_calibration(hw, iqk_mac_reg,
+	rtl8723_phy_mac_setting_calibration(hw, iqk_mac_reg,
 					    rtlphy->iqk_mac_backup);
 	rtl_set_bbreg(hw, 0xb68, MASKDWORD, 0x00080000);
 	if (is2t)
@@ -1552,8 +1122,8 @@ static void _rtl8723ae_phy_iq_calibrate(struct ieee80211_hw *hw,
 	}
 
 	if (is2t) {
-		_rtl8723ae_phy_path_a_standby(hw);
-		_rtl8723ae_phy_path_adda_on(hw, adda_reg, false, is2t);
+		rtl8723_phy_path_a_standby(hw);
+		rtl8723_phy_path_adda_on(hw, adda_reg, false, is2t);
 		for (i = 0; i < retrycount; i++) {
 			pathb_ok = _rtl8723ae_phy_path_b_iqk(hw);
 			if (pathb_ok == 0x03) {
@@ -1588,9 +1158,11 @@ static void _rtl8723ae_phy_iq_calibrate(struct ieee80211_hw *hw,
 		rtl_set_bbreg(hw, 0x844, MASKDWORD, 0x00032ed3);
 	if (t != 0) {
 		if (!rtlphy->rfpi_enable)
-			_rtl8723ae_phy_pi_mode_switch(hw, false);
-		phy_reload_adda_regs(hw, adda_reg, rtlphy->adda_backup, 16);
-		phy_reload_mac_regs(hw, iqk_mac_reg, rtlphy->iqk_mac_backup);
+			rtl8723_phy_pi_mode_switch(hw, false);
+		rtl8723_phy_reload_adda_registers(hw, adda_reg,
+						  rtlphy->adda_backup, 16);
+		rtl8723_phy_reload_mac_registers(hw, iqk_mac_reg,
+						 rtlphy->iqk_mac_backup);
 	}
 }
 
@@ -1691,7 +1263,8 @@ void rtl8723ae_phy_iq_calibrate(struct ieee80211_hw *hw, bool recovery)
 	};
 
 	if (recovery) {
-		phy_reload_adda_regs(hw, iqk_bb_reg, rtlphy->iqk_bb_backup, 10);
+		rtl8723_phy_reload_adda_registers(hw, iqk_bb_reg,
+						  rtlphy->iqk_bb_backup, 10);
 		return;
 	}
 	if (start_conttx || singletone)
@@ -1756,9 +1329,10 @@ void rtl8723ae_phy_iq_calibrate(struct ieee80211_hw *hw, bool recovery)
 		rtlphy->reg_e9c = rtlphy->reg_ebc = 0x0;
 	}
 	if (reg_e94 != 0) /*&&(reg_ea4 != 0) */
-		phy_path_a_fill_iqk_matrix(hw, patha_ok, result,
-					   final_candidate, (reg_ea4 == 0));
-	phy_save_adda_regs(hw, iqk_bb_reg, rtlphy->iqk_bb_backup, 10);
+		rtl8723_phy_path_a_fill_iqk_matrix(hw, patha_ok, result,
+						   final_candidate,
+						   (reg_ea4 == 0));
+	rtl8723_save_adda_registers(hw, iqk_bb_reg, rtlphy->iqk_bb_backup, 10);
 }
 
 void rtl8723ae_phy_lc_calibrate(struct ieee80211_hw *hw)
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/phy.h b/drivers/net/wireless/rtlwifi/rtl8723ae/phy.h
index 007ebdbbe108..cd43139ed332 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/phy.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/phy.h
@@ -76,23 +76,6 @@
 
 #define RTL92C_MAX_PATH_NUM			2
 
-enum swchnlcmd_id {
-	CMDID_END,
-	CMDID_SET_TXPOWEROWER_LEVEL,
-	CMDID_BBREGWRITE10,
-	CMDID_WRITEPORT_ULONG,
-	CMDID_WRITEPORT_USHORT,
-	CMDID_WRITEPORT_UCHAR,
-	CMDID_RF_WRITEREG,
-};
-
-struct swchnlcmd {
-	enum swchnlcmd_id cmdid;
-	u32 para1;
-	u32 para2;
-	u32 msdelay;
-};
-
 enum hw90_block_e {
 	HW90_BLOCK_MAC = 0,
 	HW90_BLOCK_PHY0 = 1,
@@ -183,10 +166,6 @@ struct tx_power_struct {
 	u32 mcs_original_offset[4][16];
 };
 
-u32 rtl8723ae_phy_query_bb_reg(struct ieee80211_hw *hw,
-			       u32 regaddr, u32 bitmask);
-void rtl8723ae_phy_set_bb_reg(struct ieee80211_hw *hw,
-			      u32 regaddr, u32 bitmask, u32 data);
 u32 rtl8723ae_phy_query_rf_reg(struct ieee80211_hw *hw,
 			       enum radio_path rfpath, u32 regaddr,
 			       u32 bitmask);
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/pwrseq.h b/drivers/net/wireless/rtlwifi/rtl8723ae/pwrseq.h
index 7a46f9fdf558..a418acb4d0ca 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/pwrseq.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/pwrseq.h
@@ -30,7 +30,6 @@
 #ifndef __RTL8723E_PWRSEQ_H__
 #define __RTL8723E_PWRSEQ_H__
 
-#include "pwrseqcmd.h"
 /*
 	Check document WM-20110607-Paul-RTL8723A_Power_Architecture-R02.vsd
 	There are 6 HW Power States:
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/reg.h b/drivers/net/wireless/rtlwifi/rtl8723ae/reg.h
index 199da366c6da..64376b38708b 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/reg.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/reg.h
@@ -2059,22 +2059,6 @@
 #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
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/sw.c b/drivers/net/wireless/rtlwifi/rtl8723ae/sw.c
index 62b204faf773..1087a3bd07fa 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/sw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/sw.c
@@ -37,8 +37,11 @@
 #include "reg.h"
 #include "def.h"
 #include "phy.h"
+#include "../rtl8723com/phy_common.h"
 #include "dm.h"
 #include "hw.h"
+#include "fw.h"
+#include "../rtl8723com/fw_common.h"
 #include "sw.h"
 #include "trx.h"
 #include "led.h"
@@ -193,6 +196,11 @@ void rtl8723ae_deinit_sw_vars(struct ieee80211_hw *hw)
 	}
 }
 
+static bool is_fw_header(struct rtl92c_firmware_header *hdr)
+{
+	return (hdr->signature & 0xfff0) == 0x2300;
+}
+
 static struct rtl_hal_ops rtl8723ae_hal_ops = {
 	.init_sw_vars = rtl8723ae_init_sw_vars,
 	.deinit_sw_vars = rtl8723ae_deinit_sw_vars,
@@ -231,13 +239,14 @@ static struct rtl_hal_ops rtl8723ae_hal_ops = {
 	.set_key = rtl8723ae_set_key,
 	.init_sw_leds = rtl8723ae_init_sw_leds,
 	.allow_all_destaddr = rtl8723ae_allow_all_destaddr,
-	.get_bbreg = rtl8723ae_phy_query_bb_reg,
-	.set_bbreg = rtl8723ae_phy_set_bb_reg,
+	.get_bbreg = rtl8723_phy_query_bb_reg,
+	.set_bbreg = rtl8723_phy_set_bb_reg,
 	.get_rfreg = rtl8723ae_phy_query_rf_reg,
 	.set_rfreg = rtl8723ae_phy_set_rf_reg,
 	.c2h_command_handle = rtl_8723e_c2h_command_handle,
 	.bt_wifi_media_status_notify = rtl_8723e_bt_wifi_media_status_notify,
 	.bt_coex_off_before_lps = rtl8723ae_bt_coex_off_before_lps,
+	.is_fw_header = is_fw_header,
 };
 
 static struct rtl_mod_params rtl8723ae_mod_params = {
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/trx.c b/drivers/net/wireless/rtlwifi/rtl8723ae/trx.c
index 50b7be3f3a60..29adf55c6fd3 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/trx.c
@@ -334,7 +334,7 @@ bool rtl8723ae_rx_query_desc(struct ieee80211_hw *hw,
 			/* during testing, hdr could be NULL here */
 			return false;
 		}
-		if ((ieee80211_is_robust_mgmt_frame(hdr)) &&
+		if ((_ieee80211_is_robust_mgmt_frame(hdr)) &&
 			(ieee80211_has_protected(hdr->frame_control)))
 			rx_status->flag &= ~RX_FLAG_DECRYPTED;
 		else
@@ -365,7 +365,7 @@ bool rtl8723ae_rx_query_desc(struct ieee80211_hw *hw,
 
 void rtl8723ae_tx_fill_desc(struct ieee80211_hw *hw,
 			    struct ieee80211_hdr *hdr, u8 *pdesc_tx,
-			    struct ieee80211_tx_info *info,
+			    u8 *pbd_desc_tx, struct ieee80211_tx_info *info,
 			    struct ieee80211_sta *sta,
 			    struct sk_buff *skb, u8 hw_queue,
 			    struct rtl_tcb_desc *ptcdesc)
@@ -597,7 +597,8 @@ void rtl8723ae_tx_fill_cmddesc(struct ieee80211_hw *hw,
 		      pdesc, TX_DESC_SIZE);
 }
 
-void rtl8723ae_set_desc(u8 *pdesc, bool istx, u8 desc_name, u8 *val)
+void rtl8723ae_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
+			u8 desc_name, u8 *val)
 {
 	if (istx == true) {
 		switch (desc_name) {
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/trx.h b/drivers/net/wireless/rtlwifi/rtl8723ae/trx.h
index ad05b54bc0f1..4380b7d3a91a 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/trx.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/trx.h
@@ -521,12 +521,6 @@ do {							\
 		memset(__pdesc, 0, _size);		\
 } while (0)
 
-#define RTL8723E_RX_HAL_IS_CCK_RATE(rxmcs)		\
-	((rxmcs) == DESC92_RATE1M ||			\
-	 (rxmcs) == DESC92_RATE2M ||			\
-	 (rxmcs) == DESC92_RATE5_5M ||			\
-	 (rxmcs) == DESC92_RATE11M)
-
 struct rx_fwinfo_8723e {
 	u8 gain_trsw[4];
 	u8 pwdb_all;
@@ -706,8 +700,8 @@ struct rx_desc_8723e {
 } __packed;
 
 void rtl8723ae_tx_fill_desc(struct ieee80211_hw *hw,
-			    struct ieee80211_hdr *hdr, u8 *pdesc_tx,
-			    struct ieee80211_tx_info *info,
+			    struct ieee80211_hdr *hdr, u8 *pdesc,
+			    u8 *pbd_desc_tx, struct ieee80211_tx_info *info,
 			    struct ieee80211_sta *sta,
 			    struct sk_buff *skb, u8 hw_queue,
 			    struct rtl_tcb_desc *ptcb_desc);
@@ -715,7 +709,8 @@ bool rtl8723ae_rx_query_desc(struct ieee80211_hw *hw,
 			     struct rtl_stats *status,
 			     struct ieee80211_rx_status *rx_status,
 			     u8 *pdesc, struct sk_buff *skb);
-void rtl8723ae_set_desc(u8 *pdesc, bool istx, u8 desc_name, u8 *val);
+void rtl8723ae_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
+			u8 desc_name, u8 *val);
 u32 rtl8723ae_get_desc(u8 *pdesc, bool istx, u8 desc_name);
 void rtl8723ae_tx_polling(struct ieee80211_hw *hw, u8 hw_queue);
 void rtl8723ae_tx_fill_cmddesc(struct ieee80211_hw *hw, u8 *pdesc,
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/Makefile b/drivers/net/wireless/rtlwifi/rtl8723be/Makefile
new file mode 100644
index 000000000000..59e416abd93a
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/Makefile
@@ -0,0 +1,19 @@
+obj-m := rtl8723be.o
+
+
+rtl8723be-objs :=		\
+		dm.o		\
+		fw.o		\
+		hw.o		\
+		led.o		\
+		phy.o		\
+		pwrseq.o	\
+		rf.o		\
+		sw.o		\
+		table.o		\
+		trx.o		\
+
+
+obj-$(CONFIG_RTL8723BE) += rtl8723be.o
+
+ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/def.h b/drivers/net/wireless/rtlwifi/rtl8723be/def.h
new file mode 100644
index 000000000000..3c30b74e983d
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/def.h
@@ -0,0 +1,248 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL8723BE_DEF_H__
+#define __RTL8723BE_DEF_H__
+
+#define HAL_RETRY_LIMIT_INFRA				48
+#define HAL_RETRY_LIMIT_AP_ADHOC			7
+
+#define RESET_DELAY_8185				20
+
+#define RT_IBSS_INT_MASKS	(IMR_BCNINT | IMR_TBDOK | IMR_TBDER)
+#define RT_AC_INT_MASKS		(IMR_VIDOK | IMR_VODOK | IMR_BEDOK|IMR_BKDOK)
+
+#define NUM_OF_FIRMWARE_QUEUE			10
+#define NUM_OF_PAGES_IN_FW			0x100
+#define NUM_OF_PAGE_IN_FW_QUEUE_BK		0x07
+#define NUM_OF_PAGE_IN_FW_QUEUE_BE		0x07
+#define NUM_OF_PAGE_IN_FW_QUEUE_VI		0x07
+#define NUM_OF_PAGE_IN_FW_QUEUE_VO		0x07
+#define NUM_OF_PAGE_IN_FW_QUEUE_HCCA		0x0
+#define NUM_OF_PAGE_IN_FW_QUEUE_CMD		0x0
+#define NUM_OF_PAGE_IN_FW_QUEUE_MGNT		0x02
+#define NUM_OF_PAGE_IN_FW_QUEUE_HIGH		0x02
+#define NUM_OF_PAGE_IN_FW_QUEUE_BCN		0x2
+#define NUM_OF_PAGE_IN_FW_QUEUE_PUB		0xA1
+
+#define NUM_OF_PAGE_IN_FW_QUEUE_BK_DTM		0x026
+#define NUM_OF_PAGE_IN_FW_QUEUE_BE_DTM		0x048
+#define NUM_OF_PAGE_IN_FW_QUEUE_VI_DTM		0x048
+#define NUM_OF_PAGE_IN_FW_QUEUE_VO_DTM		0x026
+#define NUM_OF_PAGE_IN_FW_QUEUE_PUB_DTM		0x00
+
+#define MAX_LINES_HWCONFIG_TXT			1000
+#define MAX_BYTES_LINE_HWCONFIG_TXT		256
+
+#define SW_THREE_WIRE				0
+#define HW_THREE_WIRE				2
+
+#define BT_DEMO_BOARD				0
+#define BT_QA_BOARD				1
+#define BT_FPGA					2
+
+#define HAL_PRIME_CHNL_OFFSET_DONT_CARE		0
+#define HAL_PRIME_CHNL_OFFSET_LOWER		1
+#define HAL_PRIME_CHNL_OFFSET_UPPER		2
+
+#define MAX_H2C_QUEUE_NUM			10
+
+#define RX_MPDU_QUEUE				0
+#define RX_CMD_QUEUE				1
+#define RX_MAX_QUEUE				2
+#define AC2QUEUEID(_AC)				(_AC)
+
+#define	C2H_RX_CMD_HDR_LEN			8
+#define	GET_C2H_CMD_CMD_LEN(__prxhdr)		\
+	LE_BITS_TO_4BYTE((__prxhdr), 0, 16)
+#define	GET_C2H_CMD_ELEMENT_ID(__prxhdr)	\
+	LE_BITS_TO_4BYTE((__prxhdr), 16, 8)
+#define	GET_C2H_CMD_CMD_SEQ(__prxhdr)		\
+	LE_BITS_TO_4BYTE((__prxhdr), 24, 7)
+#define	GET_C2H_CMD_CONTINUE(__prxhdr)		\
+	LE_BITS_TO_4BYTE((__prxhdr), 31, 1)
+#define	GET_C2H_CMD_CONTENT(__prxhdr)		\
+	((u8 *)(__prxhdr) + C2H_RX_CMD_HDR_LEN)
+
+#define	GET_C2H_CMD_FEEDBACK_ELEMENT_ID(__pcmdfbhdr)	\
+	LE_BITS_TO_4BYTE((__pcmdfbhdr), 0, 8)
+#define	GET_C2H_CMD_FEEDBACK_CCX_LEN(__pcmdfbhdr)	\
+	LE_BITS_TO_4BYTE((__pcmdfbhdr), 8, 8)
+#define	GET_C2H_CMD_FEEDBACK_CCX_CMD_CNT(__pcmdfbhdr)	\
+	LE_BITS_TO_4BYTE((__pcmdfbhdr), 16, 16)
+#define	GET_C2H_CMD_FEEDBACK_CCX_MAC_ID(__pcmdfbhdr)	\
+	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 0, 5)
+#define	GET_C2H_CMD_FEEDBACK_CCX_VALID(__pcmdfbhdr)	\
+	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 7, 1)
+#define	GET_C2H_CMD_FEEDBACK_CCX_RETRY_CNT(__pcmdfbhdr)	\
+	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 8, 5)
+#define	GET_C2H_CMD_FEEDBACK_CCX_TOK(__pcmdfbhdr)	\
+	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 15, 1)
+#define	GET_C2H_CMD_FEEDBACK_CCX_QSEL(__pcmdfbhdr)	\
+	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 16, 4)
+#define	GET_C2H_CMD_FEEDBACK_CCX_SEQ(__pcmdfbhdr)	\
+	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 20, 12)
+
+#define CHIP_BONDING_IDENTIFIER(_value)	(((_value)>>22)&0x3)
+#define	CHIP_BONDING_92C_1T2R		0x1
+
+#define CHIP_8723			BIT(0)
+#define CHIP_8723B			(BIT(1) | BIT(2))
+#define NORMAL_CHIP			BIT(3)
+#define RF_TYPE_1T1R			(~(BIT(4) | BIT(5) | BIT(6)))
+#define RF_TYPE_1T2R			BIT(4)
+#define RF_TYPE_2T2R			BIT(5)
+#define CHIP_VENDOR_UMC			BIT(7)
+#define B_CUT_VERSION			BIT(12)
+#define C_CUT_VERSION			BIT(13)
+#define D_CUT_VERSION			((BIT(12) | BIT(13)))
+#define E_CUT_VERSION			BIT(14)
+#define	RF_RL_ID			(BIT(31) | BIT(30) | BIT(29) | BIT(28))
+
+/* MASK */
+#define IC_TYPE_MASK			(BIT(0) | BIT(1) | BIT(2))
+#define CHIP_TYPE_MASK			BIT(3)
+#define RF_TYPE_MASK			(BIT(4) | BIT(5) | BIT(6))
+#define MANUFACTUER_MASK		BIT(7)
+#define ROM_VERSION_MASK		(BIT(11) | BIT(10) | BIT(9) | BIT(8))
+#define CUT_VERSION_MASK		(BIT(15) | BIT(14) | BIT(13) | BIT(12))
+
+/* Get element */
+#define GET_CVID_IC_TYPE(version)	((version) & IC_TYPE_MASK)
+#define GET_CVID_CHIP_TYPE(version)	((version) & CHIP_TYPE_MASK)
+#define GET_CVID_RF_TYPE(version)	((version) & RF_TYPE_MASK)
+#define GET_CVID_MANUFACTUER(version)	((version) & MANUFACTUER_MASK)
+#define GET_CVID_ROM_VERSION(version)	((version) & ROM_VERSION_MASK)
+#define GET_CVID_CUT_VERSION(version)	((version) & CUT_VERSION_MASK)
+
+#define IS_92C_SERIAL(version)   ((IS_81XXC(version) && IS_2T2R(version)) ?\
+								true : false)
+#define IS_81XXC(version)	((GET_CVID_IC_TYPE(version) == 0) ?\
+							true : false)
+#define IS_8723_SERIES(version)	((GET_CVID_IC_TYPE(version) == CHIP_8723) ?\
+							true : false)
+#define IS_1T1R(version)	((GET_CVID_RF_TYPE(version)) ? false : true)
+#define IS_1T2R(version)	((GET_CVID_RF_TYPE(version) == RF_TYPE_1T2R)\
+							? true : false)
+#define IS_2T2R(version)	((GET_CVID_RF_TYPE(version) == RF_TYPE_2T2R)\
+							? true : false)
+enum rf_optype {
+	RF_OP_BY_SW_3WIRE = 0,
+	RF_OP_BY_FW,
+	RF_OP_MAX
+};
+
+enum rf_power_state {
+	RF_ON,
+	RF_OFF,
+	RF_SLEEP,
+	RF_SHUT_DOWN,
+};
+
+enum power_save_mode {
+	POWER_SAVE_MODE_ACTIVE,
+	POWER_SAVE_MODE_SAVE,
+};
+
+enum power_polocy_config {
+	POWERCFG_MAX_POWER_SAVINGS,
+	POWERCFG_GLOBAL_POWER_SAVINGS,
+	POWERCFG_LOCAL_POWER_SAVINGS,
+	POWERCFG_LENOVO,
+};
+
+enum interface_select_pci {
+	INTF_SEL1_MINICARD = 0,
+	INTF_SEL0_PCIE = 1,
+	INTF_SEL2_RSV = 2,
+	INTF_SEL3_RSV = 3,
+};
+
+enum 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_desc8723e_rate {
+	DESC92C_RATE1M = 0x00,
+	DESC92C_RATE2M = 0x01,
+	DESC92C_RATE5_5M = 0x02,
+	DESC92C_RATE11M = 0x03,
+
+	DESC92C_RATE6M = 0x04,
+	DESC92C_RATE9M = 0x05,
+	DESC92C_RATE12M = 0x06,
+	DESC92C_RATE18M = 0x07,
+	DESC92C_RATE24M = 0x08,
+	DESC92C_RATE36M = 0x09,
+	DESC92C_RATE48M = 0x0a,
+	DESC92C_RATE54M = 0x0b,
+
+	DESC92C_RATEMCS0 = 0x0c,
+	DESC92C_RATEMCS1 = 0x0d,
+	DESC92C_RATEMCS2 = 0x0e,
+	DESC92C_RATEMCS3 = 0x0f,
+	DESC92C_RATEMCS4 = 0x10,
+	DESC92C_RATEMCS5 = 0x11,
+	DESC92C_RATEMCS6 = 0x12,
+	DESC92C_RATEMCS7 = 0x13,
+	DESC92C_RATEMCS8 = 0x14,
+	DESC92C_RATEMCS9 = 0x15,
+	DESC92C_RATEMCS10 = 0x16,
+	DESC92C_RATEMCS11 = 0x17,
+	DESC92C_RATEMCS12 = 0x18,
+	DESC92C_RATEMCS13 = 0x19,
+	DESC92C_RATEMCS14 = 0x1a,
+	DESC92C_RATEMCS15 = 0x1b,
+	DESC92C_RATEMCS15_SG = 0x1c,
+	DESC92C_RATEMCS32 = 0x20,
+};
+
+enum rx_packet_type {
+	NORMAL_RX,
+	TX_REPORT1,
+	TX_REPORT2,
+	HIS_REPORT,
+};
+
+struct phy_sts_cck_8723e_t {
+	u8 adc_pwdb_X[4];
+	u8 sq_rpt;
+	u8 cck_agc_rpt;
+};
+
+struct h2c_cmd_8723e {
+	u8 element_id;
+	u32 cmd_len;
+	u8 *p_cmdbuffer;
+};
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/dm.c b/drivers/net/wireless/rtlwifi/rtl8723be/dm.c
new file mode 100644
index 000000000000..736bfcb7938a
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/dm.c
@@ -0,0 +1,1325 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../base.h"
+#include "../pci.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "dm.h"
+#include "../rtl8723com/dm_common.h"
+#include "fw.h"
+#include "../rtl8723com/fw_common.h"
+#include "trx.h"
+#include "../btcoexist/rtl_btc.h"
+
+static const u32 ofdmswing_table[] = {
+	0x0b40002d, /* 0,  -15.0dB */
+	0x0c000030, /* 1,  -14.5dB */
+	0x0cc00033, /* 2,  -14.0dB */
+	0x0d800036, /* 3,  -13.5dB */
+	0x0e400039, /* 4,  -13.0dB */
+	0x0f00003c, /* 5,  -12.5dB */
+	0x10000040, /* 6,  -12.0dB */
+	0x11000044, /* 7,  -11.5dB */
+	0x12000048, /* 8,  -11.0dB */
+	0x1300004c, /* 9,  -10.5dB */
+	0x14400051, /* 10, -10.0dB */
+	0x15800056, /* 11, -9.5dB */
+	0x16c0005b, /* 12, -9.0dB */
+	0x18000060, /* 13, -8.5dB */
+	0x19800066, /* 14, -8.0dB */
+	0x1b00006c, /* 15, -7.5dB */
+	0x1c800072, /* 16, -7.0dB */
+	0x1e400079, /* 17, -6.5dB */
+	0x20000080, /* 18, -6.0dB */
+	0x22000088, /* 19, -5.5dB */
+	0x24000090, /* 20, -5.0dB */
+	0x26000098, /* 21, -4.5dB */
+	0x288000a2, /* 22, -4.0dB */
+	0x2ac000ab, /* 23, -3.5dB */
+	0x2d4000b5, /* 24, -3.0dB */
+	0x300000c0, /* 25, -2.5dB */
+	0x32c000cb, /* 26, -2.0dB */
+	0x35c000d7, /* 27, -1.5dB */
+	0x390000e4, /* 28, -1.0dB */
+	0x3c8000f2, /* 29, -0.5dB */
+	0x40000100, /* 30, +0dB */
+	0x43c0010f, /* 31, +0.5dB */
+	0x47c0011f, /* 32, +1.0dB */
+	0x4c000130, /* 33, +1.5dB */
+	0x50800142, /* 34, +2.0dB */
+	0x55400155, /* 35, +2.5dB */
+	0x5a400169, /* 36, +3.0dB */
+	0x5fc0017f, /* 37, +3.5dB */
+	0x65400195, /* 38, +4.0dB */
+	0x6b8001ae, /* 39, +4.5dB */
+	0x71c001c7, /* 40, +5.0dB */
+	0x788001e2, /* 41, +5.5dB */
+	0x7f8001fe  /* 42, +6.0dB */
+};
+
+static const u8 cckswing_table_ch1ch13[CCK_TABLE_SIZE][8] = {
+	{0x09, 0x08, 0x07, 0x06, 0x04, 0x03, 0x01, 0x01}, /*  0, -16.0dB */
+	{0x09, 0x09, 0x08, 0x06, 0x05, 0x03, 0x01, 0x01}, /*  1, -15.5dB */
+	{0x0a, 0x09, 0x08, 0x07, 0x05, 0x03, 0x02, 0x01}, /*  2, -15.0dB */
+	{0x0a, 0x0a, 0x09, 0x07, 0x05, 0x03, 0x02, 0x01}, /*  3, -14.5dB */
+	{0x0b, 0x0a, 0x09, 0x08, 0x06, 0x04, 0x02, 0x01}, /*  4, -14.0dB */
+	{0x0b, 0x0b, 0x0a, 0x08, 0x06, 0x04, 0x02, 0x01}, /*  5, -13.5dB */
+	{0x0c, 0x0c, 0x0a, 0x09, 0x06, 0x04, 0x02, 0x01}, /*  6, -13.0dB */
+	{0x0d, 0x0c, 0x0b, 0x09, 0x07, 0x04, 0x02, 0x01}, /*  7, -12.5dB */
+	{0x0d, 0x0d, 0x0c, 0x0a, 0x07, 0x05, 0x02, 0x01}, /*  8, -12.0dB */
+	{0x0e, 0x0e, 0x0c, 0x0a, 0x08, 0x05, 0x02, 0x01}, /*  9, -11.5dB */
+	{0x0f, 0x0f, 0x0d, 0x0b, 0x08, 0x05, 0x03, 0x01}, /* 10, -11.0dB */
+	{0x10, 0x10, 0x0e, 0x0b, 0x08, 0x05, 0x03, 0x01}, /* 11, -10.5dB */
+	{0x11, 0x11, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01}, /* 12, -10.0dB */
+	{0x12, 0x12, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01}, /* 13, -9.5dB */
+	{0x13, 0x13, 0x10, 0x0d, 0x0a, 0x06, 0x03, 0x01}, /* 14, -9.0dB */
+	{0x14, 0x14, 0x11, 0x0e, 0x0b, 0x07, 0x03, 0x02}, /* 15, -8.5dB */
+	{0x16, 0x15, 0x12, 0x0f, 0x0b, 0x07, 0x04, 0x01}, /* 16, -8.0dB */
+	{0x17, 0x16, 0x13, 0x10, 0x0c, 0x08, 0x04, 0x02}, /* 17, -7.5dB */
+	{0x18, 0x17, 0x15, 0x11, 0x0c, 0x08, 0x04, 0x02}, /* 18, -7.0dB */
+	{0x1a, 0x19, 0x16, 0x12, 0x0d, 0x09, 0x04, 0x02}, /* 19, -6.5dB */
+	{0x1b, 0x1a, 0x17, 0x13, 0x0e, 0x09, 0x04, 0x02}, /* 20, -6.0dB */
+	{0x1d, 0x1c, 0x18, 0x14, 0x0f, 0x0a, 0x05, 0x02}, /* 21, -5.5dB */
+	{0x1f, 0x1e, 0x1a, 0x15, 0x10, 0x0a, 0x05, 0x02}, /* 22, -5.0dB */
+	{0x20, 0x20, 0x1b, 0x16, 0x11, 0x08, 0x05, 0x02}, /* 23, -4.5dB */
+	{0x22, 0x21, 0x1d, 0x18, 0x11, 0x0b, 0x06, 0x02}, /* 24, -4.0dB */
+	{0x24, 0x23, 0x1f, 0x19, 0x13, 0x0c, 0x06, 0x03}, /* 25, -3.5dB */
+	{0x26, 0x25, 0x21, 0x1b, 0x14, 0x0d, 0x06, 0x03}, /* 26, -3.0dB */
+	{0x28, 0x28, 0x22, 0x1c, 0x15, 0x0d, 0x07, 0x03}, /* 27, -2.5dB */
+	{0x2b, 0x2a, 0x25, 0x1e, 0x16, 0x0e, 0x07, 0x03}, /* 28, -2.0dB */
+	{0x2d, 0x2d, 0x27, 0x1f, 0x18, 0x0f, 0x08, 0x03}, /* 29, -1.5dB */
+	{0x30, 0x2f, 0x29, 0x21, 0x19, 0x10, 0x08, 0x03}, /* 30, -1.0dB */
+	{0x33, 0x32, 0x2b, 0x23, 0x1a, 0x11, 0x08, 0x04}, /* 31, -0.5dB */
+	{0x36, 0x35, 0x2e, 0x25, 0x1c, 0x12, 0x09, 0x04}  /* 32, +0dB */
+};
+
+static const u8 cckswing_table_ch14[CCK_TABLE_SIZE][8] = {
+	{0x09, 0x08, 0x07, 0x04, 0x00, 0x00, 0x00, 0x00}, /*  0, -16.0dB */
+	{0x09, 0x09, 0x08, 0x05, 0x00, 0x00, 0x00, 0x00}, /*  1, -15.5dB */
+	{0x0a, 0x09, 0x08, 0x05, 0x00, 0x00, 0x00, 0x00}, /*  2, -15.0dB */
+	{0x0a, 0x0a, 0x09, 0x05, 0x00, 0x00, 0x00, 0x00}, /*  3, -14.5dB */
+	{0x0b, 0x0a, 0x09, 0x05, 0x00, 0x00, 0x00, 0x00}, /*  4, -14.0dB */
+	{0x0b, 0x0b, 0x0a, 0x06, 0x00, 0x00, 0x00, 0x00}, /*  5, -13.5dB */
+	{0x0c, 0x0c, 0x0a, 0x06, 0x00, 0x00, 0x00, 0x00}, /*  6, -13.0dB */
+	{0x0d, 0x0c, 0x0b, 0x06, 0x00, 0x00, 0x00, 0x00}, /*  7, -12.5dB */
+	{0x0d, 0x0d, 0x0c, 0x07, 0x00, 0x00, 0x00, 0x00}, /*  8, -12.0dB */
+	{0x0e, 0x0e, 0x0c, 0x07, 0x00, 0x00, 0x00, 0x00}, /*  9, -11.5dB */
+	{0x0f, 0x0f, 0x0d, 0x08, 0x00, 0x00, 0x00, 0x00}, /* 10, -11.0dB */
+	{0x10, 0x10, 0x0e, 0x08, 0x00, 0x00, 0x00, 0x00}, /* 11, -10.5dB */
+	{0x11, 0x11, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00}, /* 12, -10.0dB */
+	{0x12, 0x12, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00}, /* 13, -9.5dB */
+	{0x13, 0x13, 0x10, 0x0a, 0x00, 0x00, 0x00, 0x00}, /* 14, -9.0dB */
+	{0x14, 0x14, 0x11, 0x0a, 0x00, 0x00, 0x00, 0x00}, /* 15, -8.5dB */
+	{0x16, 0x15, 0x12, 0x0b, 0x00, 0x00, 0x00, 0x00}, /* 16, -8.0dB */
+	{0x17, 0x16, 0x13, 0x0b, 0x00, 0x00, 0x00, 0x00}, /* 17, -7.5dB */
+	{0x18, 0x17, 0x15, 0x0c, 0x00, 0x00, 0x00, 0x00}, /* 18, -7.0dB */
+	{0x1a, 0x19, 0x16, 0x0d, 0x00, 0x00, 0x00, 0x00}, /* 19, -6.5dB */
+	{0x1b, 0x1a, 0x17, 0x0e, 0x00, 0x00, 0x00, 0x00}, /* 20, -6.0dB */
+	{0x1d, 0x1c, 0x18, 0x0e, 0x00, 0x00, 0x00, 0x00}, /* 21, -5.5dB */
+	{0x1f, 0x1e, 0x1a, 0x0f, 0x00, 0x00, 0x00, 0x00}, /* 22, -5.0dB */
+	{0x20, 0x20, 0x1b, 0x10, 0x00, 0x00, 0x00, 0x00}, /* 23, -4.5dB */
+	{0x22, 0x21, 0x1d, 0x11, 0x00, 0x00, 0x00, 0x00}, /* 24, -4.0dB */
+	{0x24, 0x23, 0x1f, 0x12, 0x00, 0x00, 0x00, 0x00}, /* 25, -3.5dB */
+	{0x26, 0x25, 0x21, 0x13, 0x00, 0x00, 0x00, 0x00}, /* 26, -3.0dB */
+	{0x28, 0x28, 0x24, 0x14, 0x00, 0x00, 0x00, 0x00}, /* 27, -2.5dB */
+	{0x2b, 0x2a, 0x25, 0x15, 0x00, 0x00, 0x00, 0x00}, /* 28, -2.0dB */
+	{0x2d, 0x2d, 0x17, 0x17, 0x00, 0x00, 0x00, 0x00}, /* 29, -1.5dB */
+	{0x30, 0x2f, 0x29, 0x18, 0x00, 0x00, 0x00, 0x00}, /* 30, -1.0dB */
+	{0x33, 0x32, 0x2b, 0x19, 0x00, 0x00, 0x00, 0x00}, /* 31, -0.5dB */
+	{0x36, 0x35, 0x2e, 0x1b, 0x00, 0x00, 0x00, 0x00}  /* 32, +0dB */
+};
+
+static const u32 edca_setting_dl[PEER_MAX] = {
+	0xa44f,		/* 0 UNKNOWN */
+	0x5ea44f,	/* 1 REALTEK_90 */
+	0x5e4322,	/* 2 REALTEK_92SE */
+	0x5ea42b,	/* 3 BROAD */
+	0xa44f,		/* 4 RAL */
+	0xa630,		/* 5 ATH */
+	0x5ea630,	/* 6 CISCO */
+	0x5ea42b,	/* 7 MARVELL */
+};
+
+static const u32 edca_setting_ul[PEER_MAX] = {
+	0x5e4322,	/* 0 UNKNOWN */
+	0xa44f,		/* 1 REALTEK_90 */
+	0x5ea44f,	/* 2 REALTEK_92SE */
+	0x5ea32b,	/* 3 BROAD */
+	0x5ea422,	/* 4 RAL */
+	0x5ea322,	/* 5 ATH */
+	0x3ea430,	/* 6 CISCO */
+	0x5ea44f,	/* 7 MARV */
+};
+
+void rtl8723be_dm_txpower_track_adjust(struct ieee80211_hw *hw, u8 type,
+				       u8 *pdirection, u32 *poutwrite_val)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	u8 pwr_val = 0;
+	u8 ofdm_base = rtlpriv->dm.swing_idx_ofdm_base[RF90_PATH_A];
+	u8 ofdm_val = rtlpriv->dm.swing_idx_ofdm[RF90_PATH_A];
+	u8 cck_base = rtldm->swing_idx_cck_base;
+	u8 cck_val = rtldm->swing_idx_cck;
+
+	if (type == 0) {
+		if (ofdm_val <= ofdm_base) {
+			*pdirection = 1;
+			pwr_val = ofdm_base - ofdm_val;
+		} else {
+			*pdirection = 2;
+			pwr_val = ofdm_val - ofdm_base;
+		}
+	} else if (type == 1) {
+		if (cck_val <= cck_base) {
+			*pdirection = 1;
+			pwr_val = cck_base - cck_val;
+		} else {
+			*pdirection = 2;
+			pwr_val = cck_val - cck_base;
+		}
+	}
+
+	if (pwr_val >= TXPWRTRACK_MAX_IDX && (*pdirection == 1))
+		pwr_val = TXPWRTRACK_MAX_IDX;
+
+	*poutwrite_val = pwr_val | (pwr_val << 8) |
+			(pwr_val << 16) | (pwr_val << 24);
+}
+
+static void rtl8723be_dm_diginit(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
+
+	dm_digtable->dig_enable_flag = true;
+	dm_digtable->cur_igvalue = rtl_get_bbreg(hw,
+		ROFDM0_XAAGCCORE1, 0x7f);
+	dm_digtable->rssi_lowthresh = DM_DIG_THRESH_LOW;
+	dm_digtable->rssi_highthresh = DM_DIG_THRESH_HIGH;
+	dm_digtable->fa_lowthresh = DM_FALSEALARM_THRESH_LOW;
+	dm_digtable->fa_highthresh = DM_FALSEALARM_THRESH_HIGH;
+	dm_digtable->rx_gain_max = DM_DIG_MAX;
+	dm_digtable->rx_gain_min = DM_DIG_MIN;
+	dm_digtable->back_val = DM_DIG_BACKOFF_DEFAULT;
+	dm_digtable->back_range_max = DM_DIG_BACKOFF_MAX;
+	dm_digtable->back_range_min = DM_DIG_BACKOFF_MIN;
+	dm_digtable->pre_cck_cca_thres = 0xff;
+	dm_digtable->cur_cck_cca_thres = 0x83;
+	dm_digtable->forbidden_igi = DM_DIG_MIN;
+	dm_digtable->large_fa_hit = 0;
+	dm_digtable->recover_cnt = 0;
+	dm_digtable->dig_min_0 = DM_DIG_MIN;
+	dm_digtable->dig_min_1 = DM_DIG_MIN;
+	dm_digtable->media_connect_0 = false;
+	dm_digtable->media_connect_1 = false;
+	rtlpriv->dm.dm_initialgain_enable = true;
+	dm_digtable->bt30_cur_igi = 0x32;
+}
+
+void rtl8723be_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rate_adaptive *ra = &(rtlpriv->ra);
+
+	ra->ratr_state = DM_RATR_STA_INIT;
+	ra->pre_ratr_state = DM_RATR_STA_INIT;
+
+	if (rtlpriv->dm.dm_type == DM_TYPE_BYDRIVER)
+		rtlpriv->dm.useramask = true;
+	else
+		rtlpriv->dm.useramask = false;
+
+	ra->high_rssi_thresh_for_ra = 50;
+	ra->low_rssi_thresh_for_ra40m = 20;
+}
+
+static void rtl8723be_dm_init_txpower_tracking(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->dm.txpower_tracking = true;
+	rtlpriv->dm.txpower_track_control = true;
+	rtlpriv->dm.thermalvalue = 0;
+
+	rtlpriv->dm.ofdm_index[0] = 30;
+	rtlpriv->dm.cck_index = 20;
+
+	rtlpriv->dm.swing_idx_cck_base = rtlpriv->dm.cck_index;
+
+	rtlpriv->dm.swing_idx_ofdm_base[0] = rtlpriv->dm.ofdm_index[0];
+	rtlpriv->dm.delta_power_index[RF90_PATH_A] = 0;
+	rtlpriv->dm.delta_power_index_last[RF90_PATH_A] = 0;
+	rtlpriv->dm.power_index_offset[RF90_PATH_A] = 0;
+
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+		 "  rtlpriv->dm.txpower_tracking = %d\n",
+		 rtlpriv->dm.txpower_tracking);
+}
+
+static void rtl8723be_dm_init_dynamic_atc_switch(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->dm.crystal_cap = rtlpriv->efuse.crystalcap;
+	rtlpriv->dm.atc_status = rtl_get_bbreg(hw, ROFDM1_CFOTRACKING, 0x800);
+	rtlpriv->dm.cfo_threshold = CFO_THRESHOLD_XTAL;
+}
+
+void rtl8723be_dm_init(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->dm.dm_type = DM_TYPE_BYDRIVER;
+	rtl8723be_dm_diginit(hw);
+	rtl8723be_dm_init_rate_adaptive_mask(hw);
+	rtl8723_dm_init_edca_turbo(hw);
+	rtl8723_dm_init_dynamic_bb_powersaving(hw);
+	rtl8723_dm_init_dynamic_txpower(hw);
+	rtl8723be_dm_init_txpower_tracking(hw);
+	rtl8723be_dm_init_dynamic_atc_switch(hw);
+}
+
+static void rtl8723be_dm_find_minimum_rssi(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *rtl_dm_dig = &(rtlpriv->dm_digtable);
+	struct rtl_mac *mac = rtl_mac(rtlpriv);
+
+	/* Determine the minimum RSSI  */
+	if ((mac->link_state < MAC80211_LINKED) &&
+	    (rtlpriv->dm.entry_min_undec_sm_pwdb == 0)) {
+		rtl_dm_dig->min_undec_pwdb_for_dm = 0;
+		RT_TRACE(rtlpriv, COMP_BB_POWERSAVING, DBG_LOUD,
+			 "Not connected to any\n");
+	}
+	if (mac->link_state >= MAC80211_LINKED) {
+		if (mac->opmode == NL80211_IFTYPE_AP ||
+		    mac->opmode == NL80211_IFTYPE_ADHOC) {
+			rtl_dm_dig->min_undec_pwdb_for_dm =
+			    rtlpriv->dm.entry_min_undec_sm_pwdb;
+			RT_TRACE(rtlpriv, COMP_BB_POWERSAVING, DBG_LOUD,
+				 "AP Client PWDB = 0x%lx\n",
+				 rtlpriv->dm.entry_min_undec_sm_pwdb);
+		} else {
+			rtl_dm_dig->min_undec_pwdb_for_dm =
+			    rtlpriv->dm.undec_sm_pwdb;
+			RT_TRACE(rtlpriv, COMP_BB_POWERSAVING, DBG_LOUD,
+				 "STA Default Port PWDB = 0x%x\n",
+				 rtl_dm_dig->min_undec_pwdb_for_dm);
+		}
+	} else {
+		rtl_dm_dig->min_undec_pwdb_for_dm =
+				rtlpriv->dm.entry_min_undec_sm_pwdb;
+		RT_TRACE(rtlpriv, COMP_BB_POWERSAVING, DBG_LOUD,
+			 "AP Ext Port or disconnet PWDB = 0x%x\n",
+			 rtl_dm_dig->min_undec_pwdb_for_dm);
+	}
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD, "MinUndecoratedPWDBForDM =%d\n",
+		 rtl_dm_dig->min_undec_pwdb_for_dm);
+}
+
+static void rtl8723be_dm_check_rssi_monitor(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_sta_info *drv_priv;
+	u8 h2c_parameter[3] = { 0 };
+	long tmp_entry_max_pwdb = 0, tmp_entry_min_pwdb = 0xff;
+
+	/* AP & ADHOC & MESH */
+	spin_lock_bh(&rtlpriv->locks.entry_list_lock);
+	list_for_each_entry(drv_priv, &rtlpriv->entry_list, list) {
+		if (drv_priv->rssi_stat.undec_sm_pwdb <
+						tmp_entry_min_pwdb)
+			tmp_entry_min_pwdb =
+				drv_priv->rssi_stat.undec_sm_pwdb;
+		if (drv_priv->rssi_stat.undec_sm_pwdb >
+						tmp_entry_max_pwdb)
+			tmp_entry_max_pwdb =
+				drv_priv->rssi_stat.undec_sm_pwdb;
+	}
+	spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
+
+	/* If associated entry is found */
+	if (tmp_entry_max_pwdb != 0) {
+		rtlpriv->dm.entry_max_undec_sm_pwdb = tmp_entry_max_pwdb;
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 "EntryMaxPWDB = 0x%lx(%ld)\n",
+			 tmp_entry_max_pwdb, tmp_entry_max_pwdb);
+	} else {
+		rtlpriv->dm.entry_max_undec_sm_pwdb = 0;
+	}
+	/* If associated entry is found */
+	if (tmp_entry_min_pwdb != 0xff) {
+		rtlpriv->dm.entry_min_undec_sm_pwdb = tmp_entry_min_pwdb;
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 "EntryMinPWDB = 0x%lx(%ld)\n",
+			 tmp_entry_min_pwdb, tmp_entry_min_pwdb);
+	} else {
+		rtlpriv->dm.entry_min_undec_sm_pwdb = 0;
+	}
+	/* Indicate Rx signal strength to FW. */
+	if (rtlpriv->dm.useramask) {
+		h2c_parameter[2] = (u8) (rtlpriv->dm.undec_sm_pwdb & 0xFF);
+		h2c_parameter[1] = 0x20;
+		h2c_parameter[0] = 0;
+		rtl8723be_fill_h2c_cmd(hw, H2C_RSSI_REPORT, 3, h2c_parameter);
+	} else {
+		rtl_write_byte(rtlpriv, 0x4fe, rtlpriv->dm.undec_sm_pwdb);
+	}
+	rtl8723be_dm_find_minimum_rssi(hw);
+	rtlpriv->dm_digtable.rssi_val_min =
+		rtlpriv->dm_digtable.min_undec_pwdb_for_dm;
+}
+
+void rtl8723be_dm_write_dig(struct ieee80211_hw *hw, u8 current_igi)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (rtlpriv->dm_digtable.cur_igvalue != current_igi) {
+		rtl_set_bbreg(hw, ROFDM0_XAAGCCORE1, 0x7f, current_igi);
+		if (rtlpriv->phy.rf_type != RF_1T1R)
+			rtl_set_bbreg(hw, ROFDM0_XBAGCCORE1, 0x7f, current_igi);
+	}
+	rtlpriv->dm_digtable.pre_igvalue = rtlpriv->dm_digtable.cur_igvalue;
+	rtlpriv->dm_digtable.cur_igvalue = current_igi;
+}
+
+static void rtl8723be_dm_dig(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct dig_t *dm_digtable = &(rtlpriv->dm_digtable);
+	u8 dig_dynamic_min, dig_maxofmin;
+	bool firstconnect, firstdisconnect;
+	u8 dm_dig_max, dm_dig_min;
+	u8 current_igi = dm_digtable->cur_igvalue;
+	u8 offset;
+
+	/* AP, BT */
+	if (mac->act_scanning)
+		return;
+
+	dig_dynamic_min = dm_digtable->dig_min_0;
+	firstconnect = (mac->link_state >= MAC80211_LINKED) &&
+			!dm_digtable->media_connect_0;
+	firstdisconnect = (mac->link_state < MAC80211_LINKED) &&
+			   dm_digtable->media_connect_0;
+
+	dm_dig_max = 0x5a;
+	dm_dig_min = DM_DIG_MIN;
+	dig_maxofmin = DM_DIG_MAX_AP;
+
+	if (mac->link_state >= MAC80211_LINKED) {
+		if ((dm_digtable->rssi_val_min + 10) > dm_dig_max)
+			dm_digtable->rx_gain_max = dm_dig_max;
+		else if ((dm_digtable->rssi_val_min + 10) < dm_dig_min)
+			dm_digtable->rx_gain_max = dm_dig_min;
+		else
+			dm_digtable->rx_gain_max =
+				dm_digtable->rssi_val_min + 10;
+
+		if (rtlpriv->dm.one_entry_only) {
+			offset = 12;
+			if (dm_digtable->rssi_val_min - offset < dm_dig_min)
+				dig_dynamic_min = dm_dig_min;
+			else if (dm_digtable->rssi_val_min - offset >
+							dig_maxofmin)
+				dig_dynamic_min = dig_maxofmin;
+			else
+				dig_dynamic_min =
+					dm_digtable->rssi_val_min - offset;
+		} else {
+			dig_dynamic_min = dm_dig_min;
+		}
+	} else {
+		dm_digtable->rx_gain_max = dm_dig_max;
+		dig_dynamic_min = dm_dig_min;
+		RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD, "no link\n");
+	}
+
+	if (rtlpriv->falsealm_cnt.cnt_all > 10000) {
+		if (dm_digtable->large_fa_hit != 3)
+			dm_digtable->large_fa_hit++;
+		if (dm_digtable->forbidden_igi < current_igi) {
+			dm_digtable->forbidden_igi = current_igi;
+			dm_digtable->large_fa_hit = 1;
+		}
+
+		if (dm_digtable->large_fa_hit >= 3) {
+			if ((dm_digtable->forbidden_igi + 1) >
+			     dm_digtable->rx_gain_max)
+				dm_digtable->rx_gain_min =
+						dm_digtable->rx_gain_max;
+			else
+				dm_digtable->rx_gain_min =
+						dm_digtable->forbidden_igi + 1;
+			dm_digtable->recover_cnt = 3600;
+		}
+	} else {
+		if (dm_digtable->recover_cnt != 0) {
+			dm_digtable->recover_cnt--;
+		} else {
+			if (dm_digtable->large_fa_hit < 3) {
+				if ((dm_digtable->forbidden_igi - 1) <
+				     dig_dynamic_min) {
+					dm_digtable->forbidden_igi =
+							dig_dynamic_min;
+					dm_digtable->rx_gain_min =
+							dig_dynamic_min;
+				} else {
+					dm_digtable->forbidden_igi--;
+					dm_digtable->rx_gain_min =
+						dm_digtable->forbidden_igi + 1;
+				}
+			} else {
+				dm_digtable->large_fa_hit = 0;
+			}
+		}
+	}
+	if (dm_digtable->rx_gain_min > dm_digtable->rx_gain_max)
+		dm_digtable->rx_gain_min = dm_digtable->rx_gain_max;
+
+	if (mac->link_state >= MAC80211_LINKED) {
+		if (firstconnect) {
+			if (dm_digtable->rssi_val_min <= dig_maxofmin)
+				current_igi = dm_digtable->rssi_val_min;
+			else
+				current_igi = dig_maxofmin;
+
+			dm_digtable->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;
+		}
+	} else {
+		if (firstdisconnect) {
+			current_igi = dm_digtable->rx_gain_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_digtable->rx_gain_max)
+		current_igi = dm_digtable->rx_gain_max;
+	else if (current_igi < dm_digtable->rx_gain_min)
+		current_igi = dm_digtable->rx_gain_min;
+
+	rtl8723be_dm_write_dig(hw, current_igi);
+	dm_digtable->media_connect_0 =
+		((mac->link_state >= MAC80211_LINKED) ? true : false);
+	dm_digtable->dig_min_0 = dig_dynamic_min;
+}
+
+static void rtl8723be_dm_false_alarm_counter_statistics(struct ieee80211_hw *hw)
+{
+	u32 ret_value;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct false_alarm_statistics *falsealm_cnt = &(rtlpriv->falsealm_cnt);
+
+	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;
+
+	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_RST_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;
+
+	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);
+
+	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);
+
+	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);
+
+	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(rtlpriv, COMP_DIG, DBG_TRACE,
+		 "cnt_parity_fail = %d, cnt_rate_illegal = %d, "
+		 "cnt_crc8_fail = %d, cnt_mcs_fail = %d\n",
+		 falsealm_cnt->cnt_parity_fail,
+		 falsealm_cnt->cnt_rate_illegal,
+		 falsealm_cnt->cnt_crc8_fail,
+		 falsealm_cnt->cnt_mcs_fail);
+
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
+		 "cnt_ofdm_fail = %x, cnt_cck_fail = %x,"
+		 " cnt_all = %x\n",
+		 falsealm_cnt->cnt_ofdm_fail,
+		 falsealm_cnt->cnt_cck_fail,
+		 falsealm_cnt->cnt_all);
+}
+
+static void rtl8723be_dm_dynamic_txpower(struct ieee80211_hw *hw)
+{
+	/* 8723BE does not support ODM_BB_DYNAMIC_TXPWR*/
+	return;
+}
+
+static void rtl8723be_set_iqk_matrix(struct ieee80211_hw *hw, u8 ofdm_index,
+				     u8 rfpath, long iqk_result_x,
+				     long iqk_result_y)
+{
+	long ele_a = 0, ele_d, ele_c = 0, value32;
+
+	if (ofdm_index >= 43)
+		ofdm_index = 43 - 1;
+
+	ele_d = (ofdmswing_table[ofdm_index] & 0xFFC00000) >> 22;
+
+	if (iqk_result_x != 0) {
+		if ((iqk_result_x & 0x00000200) != 0)
+			iqk_result_x = iqk_result_x | 0xFFFFFC00;
+		ele_a = ((iqk_result_x * ele_d) >> 8) & 0x000003FF;
+
+		if ((iqk_result_y & 0x00000200) != 0)
+			iqk_result_y = iqk_result_y | 0xFFFFFC00;
+		ele_c = ((iqk_result_y * ele_d) >> 8) & 0x000003FF;
+
+		switch (rfpath) {
+		case RF90_PATH_A:
+			value32 = (ele_d << 22) |
+				((ele_c & 0x3F) << 16) | ele_a;
+			rtl_set_bbreg(hw, ROFDM0_XATXIQIMBALANCE, MASKDWORD,
+				      value32);
+			value32 = (ele_c & 0x000003C0) >> 6;
+			rtl_set_bbreg(hw, ROFDM0_XCTXAFE, MASKH4BITS, value32);
+			value32 = ((iqk_result_x * ele_d) >> 7) & 0x01;
+			rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD, BIT(24),
+				      value32);
+			break;
+		default:
+			break;
+		}
+	} else {
+		switch (rfpath) {
+		case RF90_PATH_A:
+			rtl_set_bbreg(hw, ROFDM0_XATXIQIMBALANCE, MASKDWORD,
+				      ofdmswing_table[ofdm_index]);
+			rtl_set_bbreg(hw, ROFDM0_XCTXAFE, MASKH4BITS, 0x00);
+			rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD, BIT(24), 0x00);
+			break;
+		default:
+			break;
+		}
+	}
+}
+
+static void rtl8723be_dm_tx_power_track_set_power(struct ieee80211_hw *hw,
+					enum pwr_track_control_method method,
+					u8 rfpath, u8 idx)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	u8 swing_idx_ofdm_limit = 36;
+
+	if (method == TXAGC) {
+		rtl8723be_phy_set_txpower_level(hw, rtlphy->current_channel);
+	} else if (method == BBSWING) {
+		if (rtldm->swing_idx_cck >= CCK_TABLE_SIZE)
+			rtldm->swing_idx_cck = CCK_TABLE_SIZE - 1;
+
+		if (!rtldm->cck_inch14) {
+			rtl_write_byte(rtlpriv, 0xa22,
+			    cckswing_table_ch1ch13[rtldm->swing_idx_cck][0]);
+			rtl_write_byte(rtlpriv, 0xa23,
+			    cckswing_table_ch1ch13[rtldm->swing_idx_cck][1]);
+			rtl_write_byte(rtlpriv, 0xa24,
+			    cckswing_table_ch1ch13[rtldm->swing_idx_cck][2]);
+			rtl_write_byte(rtlpriv, 0xa25,
+			    cckswing_table_ch1ch13[rtldm->swing_idx_cck][3]);
+			rtl_write_byte(rtlpriv, 0xa26,
+			    cckswing_table_ch1ch13[rtldm->swing_idx_cck][4]);
+			rtl_write_byte(rtlpriv, 0xa27,
+			    cckswing_table_ch1ch13[rtldm->swing_idx_cck][5]);
+			rtl_write_byte(rtlpriv, 0xa28,
+			    cckswing_table_ch1ch13[rtldm->swing_idx_cck][6]);
+			rtl_write_byte(rtlpriv, 0xa29,
+			    cckswing_table_ch1ch13[rtldm->swing_idx_cck][7]);
+		} else {
+			rtl_write_byte(rtlpriv, 0xa22,
+			    cckswing_table_ch14[rtldm->swing_idx_cck][0]);
+			rtl_write_byte(rtlpriv, 0xa23,
+			    cckswing_table_ch14[rtldm->swing_idx_cck][1]);
+			rtl_write_byte(rtlpriv, 0xa24,
+			    cckswing_table_ch14[rtldm->swing_idx_cck][2]);
+			rtl_write_byte(rtlpriv, 0xa25,
+			    cckswing_table_ch14[rtldm->swing_idx_cck][3]);
+			rtl_write_byte(rtlpriv, 0xa26,
+			    cckswing_table_ch14[rtldm->swing_idx_cck][4]);
+			rtl_write_byte(rtlpriv, 0xa27,
+			    cckswing_table_ch14[rtldm->swing_idx_cck][5]);
+			rtl_write_byte(rtlpriv, 0xa28,
+			    cckswing_table_ch14[rtldm->swing_idx_cck][6]);
+			rtl_write_byte(rtlpriv, 0xa29,
+			    cckswing_table_ch14[rtldm->swing_idx_cck][7]);
+		}
+
+		if (rfpath == RF90_PATH_A) {
+			if (rtldm->swing_idx_ofdm[RF90_PATH_A] <
+			    swing_idx_ofdm_limit)
+				swing_idx_ofdm_limit =
+					rtldm->swing_idx_ofdm[RF90_PATH_A];
+
+			rtl8723be_set_iqk_matrix(hw,
+				rtldm->swing_idx_ofdm[rfpath], rfpath,
+				rtlphy->iqk_matrix[idx].value[0][0],
+				rtlphy->iqk_matrix[idx].value[0][1]);
+		} else if (rfpath == RF90_PATH_B) {
+			if (rtldm->swing_idx_ofdm[RF90_PATH_B] <
+			    swing_idx_ofdm_limit)
+				swing_idx_ofdm_limit =
+					rtldm->swing_idx_ofdm[RF90_PATH_B];
+
+			rtl8723be_set_iqk_matrix(hw,
+				rtldm->swing_idx_ofdm[rfpath], rfpath,
+				rtlphy->iqk_matrix[idx].value[0][4],
+				rtlphy->iqk_matrix[idx].value[0][5]);
+		}
+	} else {
+		return;
+	}
+}
+
+static void txpwr_track_cb_therm(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_dm	*rtldm = rtl_dm(rtl_priv(hw));
+	u8 thermalvalue = 0, delta, delta_lck, delta_iqk;
+	u8 thermalvalue_avg_count = 0;
+	u32 thermalvalue_avg = 0;
+	int i = 0;
+
+	u8 ofdm_min_index = 6;
+	u8 index = 0;
+
+	char delta_swing_table_idx_tup_a[] = {
+		0, 0, 1, 2, 2, 2, 3, 3, 3, 4,  5,
+		5, 6, 6, 7, 7, 8, 8, 9, 9, 9, 10,
+		10, 11, 11, 12, 12, 13, 14, 15};
+	char delta_swing_table_idx_tdown_a[] = {
+		0, 0, 1, 2, 2, 2, 3, 3, 3, 4,  5,
+		5, 6, 6, 6, 6, 7, 7, 7, 8, 8,  9,
+		9, 10, 10, 11, 12, 13, 14, 15};
+
+	/*Initilization ( 7 steps in total)*/
+	rtlpriv->dm.txpower_trackinginit = true;
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+		 "rtl8723be_dm_txpower_tracking"
+		 "_callback_thermalmeter\n");
+
+	thermalvalue = (u8)rtl_get_rfreg(hw, RF90_PATH_A, RF_T_METER, 0xfc00);
+	if (!rtlpriv->dm.txpower_track_control || thermalvalue == 0 ||
+	    rtlefuse->eeprom_thermalmeter == 0xFF)
+		return;
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+		 "Readback Thermal Meter = 0x%x pre thermal meter 0x%x "
+		 "eeprom_thermalmeter 0x%x\n",
+		 thermalvalue, rtldm->thermalvalue,
+		 rtlefuse->eeprom_thermalmeter);
+	/*3 Initialize ThermalValues of RFCalibrateInfo*/
+	if (!rtldm->thermalvalue) {
+		rtlpriv->dm.thermalvalue_lck = thermalvalue;
+		rtlpriv->dm.thermalvalue_iqk = thermalvalue;
+	}
+
+	/*4 Calculate average thermal meter*/
+	rtldm->thermalvalue_avg[rtldm->thermalvalue_avg_index] = thermalvalue;
+	rtldm->thermalvalue_avg_index++;
+	if (rtldm->thermalvalue_avg_index == AVG_THERMAL_NUM_8723BE)
+		rtldm->thermalvalue_avg_index = 0;
+
+	for (i = 0; i < AVG_THERMAL_NUM_8723BE; i++) {
+		if (rtldm->thermalvalue_avg[i]) {
+			thermalvalue_avg += rtldm->thermalvalue_avg[i];
+			thermalvalue_avg_count++;
+		}
+	}
+
+	if (thermalvalue_avg_count)
+		thermalvalue = (u8)(thermalvalue_avg / thermalvalue_avg_count);
+
+	/* 5 Calculate delta, delta_LCK, delta_IQK.*/
+	delta = (thermalvalue > rtlpriv->dm.thermalvalue) ?
+		(thermalvalue - rtlpriv->dm.thermalvalue) :
+		(rtlpriv->dm.thermalvalue - thermalvalue);
+	delta_lck = (thermalvalue > rtlpriv->dm.thermalvalue_lck) ?
+		    (thermalvalue - rtlpriv->dm.thermalvalue_lck) :
+		    (rtlpriv->dm.thermalvalue_lck - thermalvalue);
+	delta_iqk = (thermalvalue > rtlpriv->dm.thermalvalue_iqk) ?
+		    (thermalvalue - rtlpriv->dm.thermalvalue_iqk) :
+		    (rtlpriv->dm.thermalvalue_iqk - thermalvalue);
+
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+		 "Readback Thermal Meter = 0x%x pre thermal meter 0x%x "
+		 "eeprom_thermalmeter 0x%x delta 0x%x "
+		 "delta_lck 0x%x delta_iqk 0x%x\n",
+		 thermalvalue, rtlpriv->dm.thermalvalue,
+		 rtlefuse->eeprom_thermalmeter, delta, delta_lck, delta_iqk);
+	/* 6 If necessary, do LCK.*/
+	if (delta_lck >= IQK_THRESHOLD) {
+		rtlpriv->dm.thermalvalue_lck = thermalvalue;
+		rtl8723be_phy_lc_calibrate(hw);
+	}
+
+	/* 7 If necessary, move the index of
+	 * swing table to adjust Tx power.
+	 */
+	if (delta > 0 && rtlpriv->dm.txpower_track_control) {
+		delta = (thermalvalue > rtlefuse->eeprom_thermalmeter) ?
+			(thermalvalue - rtlefuse->eeprom_thermalmeter) :
+			(rtlefuse->eeprom_thermalmeter - thermalvalue);
+
+		if (delta >= TXSCALE_TABLE_SIZE)
+			delta = TXSCALE_TABLE_SIZE - 1;
+		/* 7.1 Get the final CCK_index and
+		 * OFDM_index for each swing table.
+		 */
+		if (thermalvalue > rtlefuse->eeprom_thermalmeter) {
+			rtldm->delta_power_index_last[RF90_PATH_A] =
+					rtldm->delta_power_index[RF90_PATH_A];
+			rtldm->delta_power_index[RF90_PATH_A] =
+					delta_swing_table_idx_tup_a[delta];
+		} else {
+			rtldm->delta_power_index_last[RF90_PATH_A] =
+					rtldm->delta_power_index[RF90_PATH_A];
+			rtldm->delta_power_index[RF90_PATH_A] =
+				-1 * delta_swing_table_idx_tdown_a[delta];
+		}
+
+		/* 7.2 Handle boundary conditions of index.*/
+		if (rtldm->delta_power_index[RF90_PATH_A] ==
+		    rtldm->delta_power_index_last[RF90_PATH_A])
+			rtldm->power_index_offset[RF90_PATH_A] = 0;
+		else
+			rtldm->power_index_offset[RF90_PATH_A] =
+				rtldm->delta_power_index[RF90_PATH_A] -
+				rtldm->delta_power_index_last[RF90_PATH_A];
+
+		rtldm->ofdm_index[0] =
+			rtldm->swing_idx_ofdm_base[RF90_PATH_A] +
+			rtldm->power_index_offset[RF90_PATH_A];
+		rtldm->cck_index = rtldm->swing_idx_cck_base +
+				   rtldm->power_index_offset[RF90_PATH_A];
+
+		rtldm->swing_idx_cck = rtldm->cck_index;
+		rtldm->swing_idx_ofdm[0] = rtldm->ofdm_index[0];
+
+		if (rtldm->ofdm_index[0] > OFDM_TABLE_SIZE - 1)
+			rtldm->ofdm_index[0] = OFDM_TABLE_SIZE - 1;
+		else if (rtldm->ofdm_index[0] < ofdm_min_index)
+			rtldm->ofdm_index[0] = ofdm_min_index;
+
+		if (rtldm->cck_index > CCK_TABLE_SIZE - 1)
+			rtldm->cck_index = CCK_TABLE_SIZE - 1;
+		else if (rtldm->cck_index < 0)
+			rtldm->cck_index = 0;
+	} else {
+		rtldm->power_index_offset[RF90_PATH_A] = 0;
+	}
+
+	if ((rtldm->power_index_offset[RF90_PATH_A] != 0) &&
+	    (rtldm->txpower_track_control)) {
+		rtldm->done_txpower = true;
+		if (thermalvalue > rtlefuse->eeprom_thermalmeter)
+			rtl8723be_dm_tx_power_track_set_power(hw, BBSWING, 0,
+							      index);
+		else
+			rtl8723be_dm_tx_power_track_set_power(hw, BBSWING, 0,
+							      index);
+
+		rtldm->swing_idx_cck_base = rtldm->swing_idx_cck;
+		rtldm->swing_idx_ofdm_base[RF90_PATH_A] =
+						rtldm->swing_idx_ofdm[0];
+		rtldm->thermalvalue = thermalvalue;
+	}
+
+	if (delta_iqk >= IQK_THRESHOLD) {
+		rtldm->thermalvalue_iqk = thermalvalue;
+		rtl8723be_phy_iq_calibrate(hw, false);
+	}
+
+	rtldm->txpowercount = 0;
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD, "end\n");
+}
+
+void rtl8723be_dm_check_txpower_tracking(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	static u8 tm_trigger;
+
+	if (!rtlpriv->dm.txpower_tracking)
+		return;
+
+	if (!tm_trigger) {
+		rtl_set_rfreg(hw, RF90_PATH_A, RF_T_METER, BIT(17) | BIT(16),
+			      0x03);
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 "Trigger 8723be Thermal Meter!!\n");
+		tm_trigger = 1;
+		return;
+	} else {
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 "Schedule TxPowerTracking !!\n");
+		txpwr_track_cb_therm(hw);
+		tm_trigger = 0;
+	}
+}
+
+static void rtl8723be_dm_refresh_rate_adaptive_mask(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rate_adaptive *ra = &(rtlpriv->ra);
+	struct ieee80211_sta *sta = NULL;
+	u32 low_rssithresh_for_ra = ra->low2high_rssi_thresh_for_ra40m;
+	u32 high_rssithresh_for_ra = ra->high_rssi_thresh_for_ra;
+	u8 go_up_gap = 5;
+
+	if (is_hal_stop(rtlhal)) {
+		RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
+			 "driver is going to unload\n");
+		return;
+	}
+
+	if (!rtlpriv->dm.useramask) {
+		RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
+			 "driver does not control rate adaptive mask\n");
+		return;
+	}
+
+	if (mac->link_state == MAC80211_LINKED &&
+	    mac->opmode == NL80211_IFTYPE_STATION) {
+		switch (ra->pre_ratr_state) {
+		case DM_RATR_STA_MIDDLE:
+			high_rssithresh_for_ra += go_up_gap;
+			break;
+		case DM_RATR_STA_LOW:
+			high_rssithresh_for_ra += go_up_gap;
+			low_rssithresh_for_ra += go_up_gap;
+			break;
+		default:
+			break;
+		}
+
+		if (rtlpriv->dm.undec_sm_pwdb >
+		    (long)high_rssithresh_for_ra)
+			ra->ratr_state = DM_RATR_STA_HIGH;
+		else if (rtlpriv->dm.undec_sm_pwdb >
+			 (long)low_rssithresh_for_ra)
+			ra->ratr_state = DM_RATR_STA_MIDDLE;
+		else
+			ra->ratr_state = DM_RATR_STA_LOW;
+
+		if (ra->pre_ratr_state != ra->ratr_state) {
+			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
+				 "RSSI = %ld\n",
+				 rtlpriv->dm.undec_sm_pwdb);
+			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
+				 "RSSI_LEVEL = %d\n", ra->ratr_state);
+			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
+				 "PreState = %d, CurState = %d\n",
+				 ra->pre_ratr_state, ra->ratr_state);
+
+			rcu_read_lock();
+			sta = rtl_find_sta(hw, mac->bssid);
+			if (sta)
+				rtlpriv->cfg->ops->update_rate_tbl(hw, sta,
+							   ra->ratr_state);
+			rcu_read_unlock();
+
+			ra->pre_ratr_state = ra->ratr_state;
+		}
+	}
+}
+
+static bool rtl8723be_dm_is_edca_turbo_disable(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (rtlpriv->cfg->ops->get_btc_status()) {
+		if (rtlpriv->btcoexist.btc_ops->btc_is_disable_edca_turbo(rtlpriv))
+			return true;
+	}
+	if (rtlpriv->mac80211.mode == WIRELESS_MODE_B)
+		return true;
+
+	return false;
+}
+
+static void rtl8723be_dm_check_edca_turbo(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	static u64 last_txok_cnt;
+	static u64 last_rxok_cnt;
+	u64 cur_txok_cnt = 0;
+	u64 cur_rxok_cnt = 0;
+	u32 edca_be_ul = 0x6ea42b;
+	u32 edca_be_dl = 0x6ea42b;/*not sure*/
+	u32 edca_be = 0x5ea42b;
+	u32 iot_peer = 0;
+	bool is_cur_rdlstate;
+	bool last_is_cur_rdlstate = false;
+	bool bias_on_rx = false;
+	bool edca_turbo_on = false;
+
+	last_is_cur_rdlstate = rtlpriv->dm.is_cur_rdlstate;
+
+	cur_txok_cnt = rtlpriv->stats.txbytesunicast - last_txok_cnt;
+	cur_rxok_cnt = rtlpriv->stats.rxbytesunicast - last_rxok_cnt;
+
+	iot_peer = rtlpriv->mac80211.vendor;
+	bias_on_rx = (iot_peer == PEER_RAL || iot_peer == PEER_ATH) ?
+		     true : false;
+	edca_turbo_on = ((!rtlpriv->dm.is_any_nonbepkts) &&
+			 (!rtlpriv->dm.disable_framebursting)) ?
+			 true : false;
+
+	if ((iot_peer == PEER_CISCO) &&
+	    (mac->mode == WIRELESS_MODE_N_24G)) {
+		edca_be_dl = edca_setting_dl[iot_peer];
+		edca_be_ul = edca_setting_ul[iot_peer];
+	}
+	if (rtl8723be_dm_is_edca_turbo_disable(hw))
+		goto exit;
+
+	if (edca_turbo_on) {
+		if (bias_on_rx)
+			is_cur_rdlstate = (cur_txok_cnt > cur_rxok_cnt * 4) ?
+					  false : true;
+		else
+			is_cur_rdlstate = (cur_rxok_cnt > cur_txok_cnt * 4) ?
+					  true : false;
+
+		edca_be = (is_cur_rdlstate) ? edca_be_dl : edca_be_ul;
+		rtl_write_dword(rtlpriv, REG_EDCA_BE_PARAM, edca_be);
+		rtlpriv->dm.is_cur_rdlstate = is_cur_rdlstate;
+		rtlpriv->dm.current_turbo_edca = true;
+	} else {
+		if (rtlpriv->dm.current_turbo_edca) {
+			u8 tmp = AC0_BE;
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AC_PARAM,
+						      (u8 *)(&tmp));
+		}
+		rtlpriv->dm.current_turbo_edca = false;
+	}
+
+exit:
+	rtlpriv->dm.is_any_nonbepkts = false;
+	last_txok_cnt = rtlpriv->stats.txbytesunicast;
+	last_rxok_cnt = rtlpriv->stats.rxbytesunicast;
+}
+
+static void rtl8723be_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 (rtlpriv->dm_digtable.rssi_val_min > 25) {
+			cur_cck_cca_thresh = 0xcd;
+		} else if ((rtlpriv->dm_digtable.rssi_val_min <= 25) &&
+			   (rtlpriv->dm_digtable.rssi_val_min > 10)) {
+			cur_cck_cca_thresh = 0x83;
+		} else {
+			if (rtlpriv->falsealm_cnt.cnt_cck_fail > 1000)
+				cur_cck_cca_thresh = 0x83;
+			else
+				cur_cck_cca_thresh = 0x40;
+		}
+	} else {
+		if (rtlpriv->falsealm_cnt.cnt_cck_fail > 1000)
+			cur_cck_cca_thresh = 0x83;
+		else
+			cur_cck_cca_thresh = 0x40;
+	}
+
+	if (rtlpriv->dm_digtable.cur_cck_cca_thres != cur_cck_cca_thresh)
+		rtl_set_bbreg(hw, RCCK0_CCA, MASKBYTE2, cur_cck_cca_thresh);
+
+	rtlpriv->dm_digtable.pre_cck_cca_thres = rtlpriv->dm_digtable.cur_cck_cca_thres;
+	rtlpriv->dm_digtable.cur_cck_cca_thres = cur_cck_cca_thresh;
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
+		 "CCK cca thresh hold =%x\n",
+		 rtlpriv->dm_digtable.cur_cck_cca_thres);
+}
+
+static void rtl8723be_dm_dynamic_edcca(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 reg_c50, reg_c58;
+	bool fw_current_in_ps_mode = false;
+
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
+				      (u8 *)(&fw_current_in_ps_mode));
+	if (fw_current_in_ps_mode)
+		return;
+
+	reg_c50 = rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, MASKBYTE0);
+	reg_c58 = rtl_get_bbreg(hw, ROFDM0_XBAGCCORE1, MASKBYTE0);
+
+	if (reg_c50 > 0x28 && reg_c58 > 0x28) {
+		if (!rtlpriv->rtlhal.pre_edcca_enable) {
+			rtl_write_byte(rtlpriv, ROFDM0_ECCATHRESHOLD, 0x03);
+			rtl_write_byte(rtlpriv, ROFDM0_ECCATHRESHOLD + 2, 0x00);
+		}
+	} else if (reg_c50 < 0x25 && reg_c58 < 0x25) {
+		if (rtlpriv->rtlhal.pre_edcca_enable) {
+			rtl_write_byte(rtlpriv, ROFDM0_ECCATHRESHOLD, 0x7f);
+			rtl_write_byte(rtlpriv, ROFDM0_ECCATHRESHOLD + 2, 0x7f);
+		}
+	}
+}
+
+static void rtl8723be_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;
+		}
+		if (rtlpriv->cfg->ops->get_btc_status()) {
+			if (!rtlpriv->btcoexist.btc_ops->btc_is_bt_disabled(rtlpriv)) {
+				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+					 "odm_DynamicATCSwitch(): Disable"
+					 " CFO tracking for BT!!\n");
+				return;
+			}
+		}
+
+		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) >> 1) + 1;
+		else if ((cfo_ave < -rtlpriv->dm.cfo_threshold) &&
+					rtlpriv->dm.crystal_cap > 0)
+			adjust_xtal = ((cfo_ave + CFO_THRESHOLD_XTAL) >> 1) - 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 rtl8723be_dm_common_info_self_update(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_sta_info *drv_priv;
+	u8 cnt = 0;
+
+	rtlpriv->dm.one_entry_only = false;
+
+	if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_STATION &&
+	    rtlpriv->mac80211.link_state >= MAC80211_LINKED) {
+		rtlpriv->dm.one_entry_only = true;
+		return;
+	}
+
+	if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_AP ||
+	    rtlpriv->mac80211.opmode == NL80211_IFTYPE_ADHOC ||
+	    rtlpriv->mac80211.opmode == NL80211_IFTYPE_MESH_POINT) {
+		spin_lock_bh(&rtlpriv->locks.entry_list_lock);
+		list_for_each_entry(drv_priv, &rtlpriv->entry_list, list) {
+			cnt++;
+		}
+		spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
+
+		if (cnt == 1)
+			rtlpriv->dm.one_entry_only = true;
+	}
+}
+
+void rtl8723be_dm_watchdog(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	bool fw_current_inpsmode = false;
+	bool fw_ps_awake = true;
+
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
+				      (u8 *)(&fw_current_inpsmode));
+
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FWLPS_RF_ON,
+				      (u8 *)(&fw_ps_awake));
+
+	if (ppsc->p2p_ps_info.p2p_ps_mode)
+		fw_ps_awake = false;
+
+	if ((ppsc->rfpwr_state == ERFON) &&
+	    ((!fw_current_inpsmode) && fw_ps_awake) &&
+	    (!ppsc->rfchange_inprogress)) {
+		rtl8723be_dm_common_info_self_update(hw);
+		rtl8723be_dm_false_alarm_counter_statistics(hw);
+		rtl8723be_dm_check_rssi_monitor(hw);
+		rtl8723be_dm_dig(hw);
+		rtl8723be_dm_dynamic_edcca(hw);
+		rtl8723be_dm_cck_packet_detection_thresh(hw);
+		rtl8723be_dm_refresh_rate_adaptive_mask(hw);
+		rtl8723be_dm_check_edca_turbo(hw);
+		rtl8723be_dm_dynamic_atc_switch(hw);
+		rtl8723be_dm_check_txpower_tracking(hw);
+		rtl8723be_dm_dynamic_txpower(hw);
+		if (rtlpriv->cfg->ops->get_btc_status())
+			rtlpriv->btcoexist.btc_ops->btc_periodical(rtlpriv);
+	}
+	rtlpriv->dm.dbginfo.num_qry_beacon_pkt = 0;
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/dm.h b/drivers/net/wireless/rtlwifi/rtl8723be/dm.h
new file mode 100644
index 000000000000..c6c2f2a78a66
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/dm.h
@@ -0,0 +1,310 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef	__RTL8723BE_DM_H__
+#define __RTL8723BE_DM_H__
+
+#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
+
+#define	TXSCALE_TABLE_SIZE	30
+
+/*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_88E_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_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				37
+#define CCK_TABLE_SIZE				33
+
+#define BW_AUTO_SWITCH_HIGH_LOW			25
+#define BW_AUTO_SWITCH_LOW_HIGH			30
+
+#define DM_DIG_THRESH_HIGH			40
+#define DM_DIG_THRESH_LOW			35
+
+#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_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 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 */
+
+enum FAT_STATE {
+	FAT_NORMAL_STATE	= 0,
+	FAT_TRAINING_STATE	= 1,
+};
+
+enum tag_dynamic_init_gain_operation_type_definition {
+	DIG_TYPE_THRESH_HIGH	= 0,
+	DIG_TYPE_THRESH_LOW	= 1,
+	DIG_TYPE_BACKOFF	= 2,
+	DIG_TYPE_RX_GAIN_MIN	= 3,
+	DIG_TYPE_RX_GAIN_MAX	= 4,
+	DIG_TYPE_ENABLE		= 5,
+	DIG_TYPE_DISABLE	= 6,
+	DIG_OP_TYPE_MAX
+};
+
+enum dm_1r_cca_e {
+	CCA_1R		= 0,
+	CCA_2R		= 1,
+	CCA_MAX		= 2,
+};
+
+enum dm_rf_e {
+	RF_SAVE		= 0,
+	RF_NORMAL	= 1,
+	RF_MAX		= 2,
+};
+
+enum dm_sw_ant_switch_e {
+	ANS_ANTENNA_B	= 1,
+	ANS_ANTENNA_A	= 2,
+	ANS_ANTENNA_MAX	= 3,
+};
+
+enum dm_dig_ext_port_alg_e {
+	DIG_EXT_PORT_STAGE_0	= 0,
+	DIG_EXT_PORT_STAGE_1	= 1,
+	DIG_EXT_PORT_STAGE_2	= 2,
+	DIG_EXT_PORT_STAGE_3	= 3,
+	DIG_EXT_PORT_STAGE_MAX	= 4,
+};
+
+enum dm_dig_connect_e {
+	DIG_STA_DISCONNECT	= 0,
+	DIG_STA_CONNECT		= 1,
+	DIG_STA_BEFORE_CONNECT	= 2,
+	DIG_MULTISTA_DISCONNECT	= 3,
+	DIG_MULTISTA_CONNECT	= 4,
+	DIG_CONNECT_MAX
+};
+
+enum pwr_track_control_method {
+	BBSWING,
+	TXAGC
+};
+
+#define BT_RSSI_STATE_NORMAL_POWER      BIT_OFFSET_LEN_MASK_32(0, 1)
+#define BT_RSSI_STATE_AMDPU_OFF         BIT_OFFSET_LEN_MASK_32(1, 1)
+#define BT_RSSI_STATE_SPECIAL_LOW       BIT_OFFSET_LEN_MASK_32(2, 1)
+#define BT_RSSI_STATE_BG_EDCA_LOW       BIT_OFFSET_LEN_MASK_32(3, 1)
+#define BT_RSSI_STATE_TXPOWER_LOW       BIT_OFFSET_LEN_MASK_32(4, 1)
+
+void rtl8723be_dm_set_tx_ant_by_tx_info(struct ieee80211_hw *hw, u8 *pdesc,
+					u32 mac_id);
+void rtl8723be_dm_ant_sel_statistics(struct ieee80211_hw *hw, u8 antsel_tr_mux,
+				     u32 mac_id, u32 rx_pwdb_all);
+void rtl8723be_dm_fast_antenna_trainning_callback(unsigned long data);
+void rtl8723be_dm_init(struct ieee80211_hw *hw);
+void rtl8723be_dm_watchdog(struct ieee80211_hw *hw);
+void rtl8723be_dm_write_dig(struct ieee80211_hw *hw, u8 current_igi);
+void rtl8723be_dm_check_txpower_tracking(struct ieee80211_hw *hw);
+void rtl8723be_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw);
+void rtl8723be_dm_txpower_track_adjust(struct ieee80211_hw *hw, u8 type,
+				       u8 *pdirection, u32 *poutwrite_val);
+void rtl8723be_dm_init_edca_turbo(struct ieee80211_hw *hw);
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/fw.c b/drivers/net/wireless/rtlwifi/rtl8723be/fw.c
new file mode 100644
index 000000000000..f856be6fc138
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/fw.c
@@ -0,0 +1,620 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../pci.h"
+#include "../base.h"
+#include "reg.h"
+#include "def.h"
+#include "fw.h"
+#include "../rtl8723com/fw_common.h"
+
+static bool _rtl8723be_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 _rtl8723be_fill_h2c_command(struct ieee80211_hw *hw, u8 element_id,
+					u32 cmd_len, u8 *p_cmdbuffer)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 boxnum;
+	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_limit = 100;
+	u8 wait_writeh2c_limit = 100;
+	u8 boxcontent[4], boxextcontent[4];
+	u32 h2c_waitcounter = 0;
+	unsigned long flag;
+	u8 idx;
+
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD, "come in\n");
+
+	while (true) {
+		spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
+		if (rtlhal->h2c_setinprogress) {
+			RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
+				 "H2C set in progress! Wait to set.."
+				 "element_id(%d).\n", element_id);
+
+			while (rtlhal->h2c_setinprogress) {
+				spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock,
+						       flag);
+				h2c_waitcounter++;
+				RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
+					 "Wait 100 us (%d times)...\n",
+					 h2c_waitcounter);
+				udelay(100);
+
+				if (h2c_waitcounter > 1000)
+					return;
+				spin_lock_irqsave(&rtlpriv->locks.h2c_lock,
+						  flag);
+			}
+			spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
+		} else {
+			rtlhal->h2c_setinprogress = true;
+			spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
+			break;
+		}
+	}
+	while (!bwrite_sucess) {
+		wait_writeh2c_limit--;
+		if (wait_writeh2c_limit == 0) {
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "Write H2C fail because no trigger "
+				 "for FW INT!\n");
+			break;
+		}
+		boxnum = rtlhal->last_hmeboxnum;
+		switch (boxnum) {
+		case 0:
+			box_reg = REG_HMEBOX_0;
+			box_extreg = REG_HMEBOX_EXT_0;
+			break;
+		case 1:
+			box_reg = REG_HMEBOX_1;
+			box_extreg = REG_HMEBOX_EXT_1;
+			break;
+		case 2:
+			box_reg = REG_HMEBOX_2;
+			box_extreg = REG_HMEBOX_EXT_2;
+			break;
+		case 3:
+			box_reg = REG_HMEBOX_3;
+			box_extreg = REG_HMEBOX_EXT_3;
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "switch case not processed\n");
+			break;
+		}
+		isfw_read = _rtl8723be_check_fw_read_last_h2c(hw, boxnum);
+		while (!isfw_read) {
+			wait_h2c_limit--;
+			if (wait_h2c_limit == 0) {
+				RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
+					 "Wating too long for FW read "
+					 "clear HMEBox(%d)!\n", boxnum);
+				break;
+			}
+			udelay(10);
+
+			isfw_read = _rtl8723be_check_fw_read_last_h2c(hw,
+								boxnum);
+			u1b_tmp = rtl_read_byte(rtlpriv, 0x130);
+			RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
+				 "Wating for FW read clear HMEBox(%d)!!! 0x130 = %2x\n",
+				 boxnum, u1b_tmp);
+		}
+		if (!isfw_read) {
+			RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
+				 "Write H2C register BOX[%d] fail!!!!! "
+				 "Fw do not read.\n", boxnum);
+			break;
+		}
+		memset(boxcontent, 0, sizeof(boxcontent));
+		memset(boxextcontent, 0, sizeof(boxextcontent));
+		boxcontent[0] = element_id;
+		RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
+			 "Write element_id box_reg(%4x) = %2x\n",
+			 box_reg, element_id);
+
+		switch (cmd_len) {
+		case 1:
+		case 2:
+		case 3:
+			/*boxcontent[0] &= ~(BIT(7));*/
+			memcpy((u8 *)(boxcontent) + 1,
+			       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(rtlpriv, 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(rtlpriv, COMP_CMD, DBG_LOUD,
+			 "pHalData->last_hmeboxnum  = %d\n",
+			 rtlhal->last_hmeboxnum);
+	}
+	spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
+	rtlhal->h2c_setinprogress = false;
+	spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
+
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD, "go out\n");
+}
+
+void rtl8723be_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->fw_ready) {
+		RT_ASSERT(false,
+			  "return H2C cmd because of Fw download fail!!!\n");
+		return;
+	}
+	memset(tmp_cmdbuf, 0, 8);
+	memcpy(tmp_cmdbuf, p_cmdbuffer, cmd_len);
+	_rtl8723be_fill_h2c_command(hw, element_id, cmd_len,
+				    (u8 *)&tmp_cmdbuf);
+	return;
+}
+
+void rtl8723be_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 u1_h2c_set_pwrmode[H2C_8723BE_PWEMODE_LENGTH] = { 0 };
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	u8 rlbm, power_state = 0;
+	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "FW LPS mode = %d\n", mode);
+
+	SET_H2CCMD_PWRMODE_PARM_MODE(u1_h2c_set_pwrmode, ((mode) ? 1 : 0));
+	rlbm = 0;/*YJ, temp, 120316. FW now not support RLBM = 2.*/
+	SET_H2CCMD_PWRMODE_PARM_RLBM(u1_h2c_set_pwrmode, rlbm);
+	SET_H2CCMD_PWRMODE_PARM_SMART_PS(u1_h2c_set_pwrmode,
+					 (rtlpriv->mac80211.p2p) ?
+					 ppsc->smart_ps : 1);
+	SET_H2CCMD_PWRMODE_PARM_AWAKE_INTERVAL(u1_h2c_set_pwrmode,
+					       ppsc->reg_max_lps_awakeintvl);
+	SET_H2CCMD_PWRMODE_PARM_ALL_QUEUE_UAPSD(u1_h2c_set_pwrmode, 0);
+	if (mode == FW_PS_ACTIVE_MODE)
+		power_state |= FW_PWR_STATE_ACTIVE;
+	else
+		power_state |= FW_PWR_STATE_RF_OFF;
+	SET_H2CCMD_PWRMODE_PARM_PWR_STATE(u1_h2c_set_pwrmode, power_state);
+
+	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
+		      "rtl92c_set_fw_pwrmode(): u1_h2c_set_pwrmode\n",
+		      u1_h2c_set_pwrmode, H2C_8723BE_PWEMODE_LENGTH);
+	rtl8723be_fill_h2c_cmd(hw, H2C_8723BE_SETPWRMODE,
+			       H2C_8723BE_PWEMODE_LENGTH,
+			       u1_h2c_set_pwrmode);
+}
+
+static bool _rtl8723be_cmd_send_packet(struct ieee80211_hw *hw,
+				       struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl8192_tx_ring *ring;
+	struct rtl_tx_desc *pdesc;
+	struct sk_buff *pskb = NULL;
+	u8 own;
+	unsigned long flags;
+
+	ring = &rtlpci->tx_ring[BEACON_QUEUE];
+
+	pskb = __skb_dequeue(&ring->queue);
+	if (pskb)
+		kfree_skb(pskb);
+
+	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
+
+	pdesc = &ring->desc[0];
+	own = (u8) rtlpriv->cfg->ops->get_desc((u8 *)pdesc, true, HW_DESC_OWN);
+
+	rtlpriv->cfg->ops->fill_tx_cmddesc(hw, (u8 *)pdesc, 1, 1, skb);
+
+	__skb_queue_tail(&ring->queue, skb);
+
+	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
+
+	rtlpriv->cfg->ops->tx_polling(hw, BEACON_QUEUE);
+
+	return true;
+}
+#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 rtl8723be_set_fw_rsvdpagepkt(struct ieee80211_hw *hw,
+				  bool 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 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,
+		      "rtl8723be_set_fw_rsvdpagepkt(): "
+		      "HW_VAR_SET_TX_CMD: ALL\n",
+		      &reserved_page_packet[0], totalpacketlen);
+	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
+		      "rtl8723be_set_fw_rsvdpagepkt(): "
+		      "HW_VAR_SET_TX_CMD: ALL\n", u1rsvdpageloc, 3);
+
+
+	skb = dev_alloc_skb(totalpacketlen);
+	memcpy((u8 *)skb_put(skb, totalpacketlen),
+	       &reserved_page_packet, totalpacketlen);
+
+	rtstatus = _rtl8723be_cmd_send_packet(hw, skb);
+
+	if (rtstatus)
+		dlok = true;
+
+	if (dlok) {
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+			 "Set RSVD page location to Fw.\n");
+		RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG, "H2C_RSVDPAGE:\n",
+			      u1rsvdpageloc, 3);
+		rtl8723be_fill_h2c_cmd(hw, H2C_8723BE_RSVDPAGE,
+				       sizeof(u1rsvdpageloc), u1rsvdpageloc);
+	} else {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+			 "Set RSVD page location to Fw FAIL!!!!!!.\n");
+	}
+}
+
+/*Should check FW support p2p or not.*/
+static void rtl8723be_set_p2p_ctw_period_cmd(struct ieee80211_hw *hw,
+					     u8 ctwindow)
+{
+	u8 u1_ctwindow_period[1] = {ctwindow};
+
+	rtl8723be_fill_h2c_cmd(hw, H2C_8723BE_P2P_PS_CTW_CMD, 1,
+			       u1_ctwindow_period);
+}
+
+void rtl8723be_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw,
+				      u8 p2p_ps_state)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *rtlps = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_p2p_ps_info *p2pinfo = &(rtlps->p2p_ps_info);
+	struct p2p_ps_offload_t *p2p_ps_offload = &rtlhal->p2p_ps_offload;
+	u8 i;
+	u16 ctwindow;
+	u32 start_time, tsf_low;
+
+	switch (p2p_ps_state) {
+	case P2P_PS_DISABLE:
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_DISABLE\n");
+		memset(p2p_ps_offload, 0, sizeof(struct p2p_ps_offload_t));
+		break;
+	case P2P_PS_ENABLE:
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_ENABLE\n");
+		/* update CTWindow value. */
+		if (p2pinfo->ctwindow > 0) {
+			p2p_ps_offload->ctwindow_en = 1;
+			ctwindow = p2pinfo->ctwindow;
+			rtl8723be_set_p2p_ctw_period_cmd(hw, ctwindow);
+		}
+		/* hw only support 2 set of NoA */
+		for (i = 0; i < p2pinfo->noa_num; i++) {
+			/* To control the register setting
+			 * for which NOA
+			 */
+			rtl_write_byte(rtlpriv, 0x5cf, (i << 4));
+			if (i == 0)
+				p2p_ps_offload->noa0_en = 1;
+			else
+				p2p_ps_offload->noa1_en = 1;
+
+			/* config P2P NoA Descriptor Register */
+			rtl_write_dword(rtlpriv, 0x5E0,
+					p2pinfo->noa_duration[i]);
+			rtl_write_dword(rtlpriv, 0x5E4,
+					p2pinfo->noa_interval[i]);
+
+			/*Get Current TSF value */
+			tsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
+
+			start_time = p2pinfo->noa_start_time[i];
+			if (p2pinfo->noa_count_type[i] != 1) {
+				while (start_time <= (tsf_low + (50 * 1024))) {
+					start_time += p2pinfo->noa_interval[i];
+					if (p2pinfo->noa_count_type[i] != 255)
+						p2pinfo->noa_count_type[i]--;
+				}
+			}
+			rtl_write_dword(rtlpriv, 0x5E8, start_time);
+			rtl_write_dword(rtlpriv, 0x5EC,
+					p2pinfo->noa_count_type[i]);
+		}
+		if ((p2pinfo->opp_ps == 1) ||
+		    (p2pinfo->noa_num > 0)) {
+			/* rst p2p circuit */
+			rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, BIT(4));
+
+			p2p_ps_offload->offload_en = 1;
+
+			if (P2P_ROLE_GO == rtlpriv->mac80211.p2p) {
+				p2p_ps_offload->role = 1;
+				p2p_ps_offload->allstasleep = 0;
+			} else {
+				p2p_ps_offload->role = 0;
+			}
+			p2p_ps_offload->discovery = 0;
+		}
+		break;
+	case P2P_PS_SCAN:
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_SCAN\n");
+		p2p_ps_offload->discovery = 1;
+		break;
+	case P2P_PS_SCAN_DONE:
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_SCAN_DONE\n");
+		p2p_ps_offload->discovery = 0;
+		p2pinfo->p2p_ps_state = P2P_PS_ENABLE;
+		break;
+	default:
+		break;
+	}
+	rtl8723be_fill_h2c_cmd(hw, H2C_8723BE_P2P_PS_OFFLOAD, 1,
+			       (u8 *)p2p_ps_offload);
+}
+
+void rtl8723be_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus)
+{
+	u8 u1_joinbssrpt_parm[1] = { 0 };
+
+	SET_H2CCMD_JOINBSSRPT_PARM_OPMODE(u1_joinbssrpt_parm, mstatus);
+
+	rtl8723be_fill_h2c_cmd(hw, H2C_8723BE_JOINBSSRPT, 1,
+			       u1_joinbssrpt_parm);
+}
+
+void rtl8723be_set_fw_ap_off_load_cmd(struct ieee80211_hw *hw,
+				      u8 ap_offload_enable)
+{
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u8 u1_apoffload_parm[H2C_8723BE_AP_OFFLOAD_LENGTH] = { 0 };
+
+	SET_H2CCMD_AP_OFFLOAD_ON(u1_apoffload_parm, ap_offload_enable);
+	SET_H2CCMD_AP_OFFLOAD_HIDDEN(u1_apoffload_parm, mac->hiddenssid);
+	SET_H2CCMD_AP_OFFLOAD_DENYANY(u1_apoffload_parm, 0);
+
+	rtl8723be_fill_h2c_cmd(hw, H2C_8723BE_AP_OFFLOAD,
+			       H2C_8723BE_AP_OFFLOAD_LENGTH, u1_apoffload_parm);
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/fw.h b/drivers/net/wireless/rtlwifi/rtl8723be/fw.h
new file mode 100644
index 000000000000..31eec281e446
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/fw.h
@@ -0,0 +1,248 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL8723BE__FW__H__
+#define __RTL8723BE__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		6000
+
+#define IS_FW_HEADER_EXIST(_pfwhdr)	\
+	((_pfwhdr->signature&0xFFF0) == 0x5300)
+#define USE_OLD_WOWLAN_DEBUG_FW			0
+
+#define H2C_8723BE_RSVDPAGE_LOC_LEN		5
+#define H2C_8723BE_PWEMODE_LENGTH		5
+#define H2C_8723BE_JOINBSSRPT_LENGTH		1
+#define H2C_8723BE_AP_OFFLOAD_LENGTH		3
+#define H2C_8723BE_WOWLAN_LENGTH		3
+#define H2C_8723BE_KEEP_ALIVE_CTRL_LENGTH	3
+#if (USE_OLD_WOWLAN_DEBUG_FW == 0)
+#define H2C_8723BE_REMOTE_WAKE_CTRL_LEN		1
+#else
+#define H2C_8723BE_REMOTE_WAKE_CTRL_LEN		3
+#endif
+#define H2C_8723BE_AOAC_GLOBAL_INFO_LEN		2
+#define H2C_8723BE_AOAC_RSVDPAGE_LOC_LEN	7
+
+
+/* Fw PS state for RPWM.
+*BIT[2:0] = HW state
+*BIT[3] = Protocol PS state, 1: register active state , 0: register sleep state
+*BIT[4] = sub-state
+*/
+#define	FW_PS_GO_ON		BIT(0)
+#define	FW_PS_TX_NULL		BIT(1)
+#define	FW_PS_RF_ON		BIT(2)
+#define	FW_PS_REGISTER_ACTIVE	BIT(3)
+
+#define	FW_PS_DPS		BIT(0)
+#define	FW_PS_LCLK		(FW_PS_DPS)
+#define	FW_PS_RF_OFF		BIT(1)
+#define	FW_PS_ALL_ON		BIT(2)
+#define	FW_PS_ST_ACTIVE	BIT(3)
+#define	FW_PS_ISR_ENABLE	BIT(4)
+#define	FW_PS_IMR_ENABLE	BIT(5)
+
+
+#define	FW_PS_ACK		BIT(6)
+#define	FW_PS_TOGGLE		BIT(7)
+
+ /* 88E RPWM value*/
+ /* 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)
+/*ISR_ENABLE, IMR_ENABLE, and PS mode should be inherited.*/
+#define	FW_PS_STATE_INT_MASK	(0x3F)
+
+#define	FW_PS_STATE(x)	(FW_PS_STATE_MASK & (x))
+#define	FW_PS_STATE_HW(x)	(FW_PS_STATE_HW_MASK & (x))
+#define	FW_PS_STATE_INT(x)	(FW_PS_STATE_INT_MASK & (x))
+#define	FW_PS_ISR_VAL(x)	((x) & 0x70)
+#define	FW_PS_IMR_MASK(x)	((x) & 0xDF)
+#define	FW_PS_KEEP_IMR(x)	((x) & 0x20)
+
+
+#define	FW_PS_STATE_S0		(FW_PS_DPS)
+#define	FW_PS_STATE_S1		(FW_PS_LCLK)
+#define	FW_PS_STATE_S2		(FW_PS_RF_OFF)
+#define	FW_PS_STATE_S3		(FW_PS_ALL_ON)
+#define	FW_PS_STATE_S4		((FW_PS_ST_ACTIVE) | (FW_PS_ALL_ON))
+
+/* ((FW_PS_RF_ON) | (FW_PS_REGISTER_ACTIVE))*/
+#define	FW_PS_STATE_ALL_ON_88E	(FW_PS_CLOCK_ON)
+/* (FW_PS_RF_ON)*/
+#define	FW_PS_STATE_RF_ON_88E	(FW_PS_CLOCK_ON)
+/* 0x0*/
+#define	FW_PS_STATE_RF_OFF_88E	(FW_PS_CLOCK_ON)
+/* (FW_PS_STATE_RF_OFF)*/
+#define	FW_PS_STATE_RF_OFF_LOW_PWR_88E	(FW_PS_CLOCK_OFF)
+
+#define	FW_PS_STATE_ALL_ON_92C	(FW_PS_STATE_S4)
+#define	FW_PS_STATE_RF_ON_92C		(FW_PS_STATE_S3)
+#define	FW_PS_STATE_RF_OFF_92C	(FW_PS_STATE_S2)
+#define	FW_PS_STATE_RF_OFF_LOW_PWR_92C	(FW_PS_STATE_S1)
+
+
+/* For 88E H2C PwrMode Cmd ID 5.*/
+#define	FW_PWR_STATE_ACTIVE	((FW_PS_RF_ON) | (FW_PS_REGISTER_ACTIVE))
+#define	FW_PWR_STATE_RF_OFF	0
+
+#define	FW_PS_IS_ACK(x)	((x) & FW_PS_ACK)
+#define	FW_PS_IS_CLK_ON(x)	((x) & (FW_PS_RF_OFF | FW_PS_ALL_ON))
+#define	FW_PS_IS_RF_ON(x)	((x) & (FW_PS_ALL_ON))
+#define	FW_PS_IS_ACTIVE(x)	((x) & (FW_PS_ST_ACTIVE))
+#define	FW_PS_IS_CPWM_INT(x)	((x) & 0x40)
+
+#define	FW_CLR_PS_STATE(x)	((x) = ((x) & (0xF0)))
+
+#define	IS_IN_LOW_POWER_STATE_88E(fwpsstate)		\
+			(FW_PS_STATE(fwpsstate) == FW_PS_CLOCK_OFF)
+
+#define	FW_PWR_STATE_ACTIVE	((FW_PS_RF_ON) | (FW_PS_REGISTER_ACTIVE))
+#define	FW_PWR_STATE_RF_OFF	0
+
+#define pagenum_128(_len)	(u32)(((_len)>>7) + ((_len)&0x7F ? 1 : 0))
+
+#define SET_88E_H2CCMD_WOWLAN_FUNC_ENABLE(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 1, __val)
+#define SET_88E_H2CCMD_WOWLAN_PATTERN_MATCH_ENABLE(__ph2ccmd, __val)	\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 1, 1, __val)
+#define SET_88E_H2CCMD_WOWLAN_MAGIC_PKT_ENABLE(__ph2ccmd, __val)	\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 2, 1, __val)
+#define SET_88E_H2CCMD_WOWLAN_UNICAST_PKT_ENABLE(__ph2ccmd, __val)	\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 3, 1, __val)
+#define SET_88E_H2CCMD_WOWLAN_ALL_PKT_DROP(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 4, 1, __val)
+#define SET_88E_H2CCMD_WOWLAN_GPIO_ACTIVE(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 5, 1, __val)
+#define SET_88E_H2CCMD_WOWLAN_REKEY_WAKE_UP(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 6, 1, __val)
+#define SET_88E_H2CCMD_WOWLAN_DISCONNECT_WAKE_UP(__ph2ccmd, __val)	\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 7, 1, __val)
+#define SET_88E_H2CCMD_WOWLAN_GPIONUM(__ph2ccmd, __val)			\
+	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+1, 0, 8, __val)
+#define SET_88E_H2CCMD_WOWLAN_GPIO_DURATION(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+2, 0, 8, __val)
+
+
+#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_88E_H2CCMD_PWRMODE_PARM_MODE(__ph2ccmd)			\
+	LE_BITS_TO_1BYTE(__ph2ccmd, 0, 8)
+
+#define SET_H2CCMD_JOINBSSRPT_PARM_OPMODE(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 8, __val)
+#define SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 8, __val)
+#define SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+1, 0, 8, __val)
+#define SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+2, 0, 8, __val)
+
+/* AP_OFFLOAD */
+#define SET_H2CCMD_AP_OFFLOAD_ON(__ph2ccmd, __val)			\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 8, __val)
+#define SET_H2CCMD_AP_OFFLOAD_HIDDEN(__ph2ccmd, __val)			\
+	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+1, 0, 8, __val)
+#define SET_H2CCMD_AP_OFFLOAD_DENYANY(__ph2ccmd, __val)			\
+	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+2, 0, 8, __val)
+#define SET_H2CCMD_AP_OFFLOAD_WAKEUP_EVT_RPT(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+3, 0, 8, __val)
+
+/* Keep Alive Control*/
+#define SET_88E_H2CCMD_KEEP_ALIVE_ENABLE(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 1, __val)
+#define SET_88E_H2CCMD_KEEP_ALIVE_ACCPEPT_USER_DEFINED(__ph2ccmd, __val)\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 1, 1, __val)
+#define SET_88E_H2CCMD_KEEP_ALIVE_PERIOD(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+1, 0, 8, __val)
+
+/*REMOTE_WAKE_CTRL */
+#define SET_88E_H2CCMD_REMOTE_WAKE_CTRL_EN(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 1, __val)
+#if (USE_OLD_WOWLAN_DEBUG_FW == 0)
+#define SET_88E_H2CCMD_REMOTE_WAKE_CTRL_ARP_OFFLOAD_EN(__ph2ccmd, __val)\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 1, 1, __val)
+#define SET_88E_H2CCMD_REMOTE_WAKE_CTRL_NDP_OFFLOAD_EN(__ph2ccmd, __val)\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 2, 1, __val)
+#define SET_88E_H2CCMD_REMOTE_WAKE_CTRL_GTK_OFFLOAD_EN(__ph2ccmd, __val)\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 3, 1, __val)
+#else
+#define SET_88E_H2CCMD_REMOTE_WAKE_CTRL_PAIRWISE_ENC_ALG(__ph2ccmd, __val)\
+	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+1, 0, 8, __val)
+#define SET_88E_H2CCMD_REMOTE_WAKE_CTRL_GROUP_ENC_ALG(__ph2ccmd, __val)	\
+	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+2, 0, 8, __val)
+#endif
+
+/* GTK_OFFLOAD */
+#define SET_88E_H2CCMD_AOAC_GLOBAL_INFO_PAIRWISE_ENC_ALG(__ph2ccmd, __val)\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 8, __val)
+#define SET_88E_H2CCMD_AOAC_GLOBAL_INFO_GROUP_ENC_ALG(__ph2ccmd, __val)	\
+	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+1, 0, 8, __val)
+
+/* AOAC_RSVDPAGE_LOC */
+#define SET_88E_H2CCMD_AOAC_RSVDPAGE_LOC_REM_WAKE_CTRL_INFO(__ph2ccmd, __val)\
+	SET_BITS_TO_LE_1BYTE((__ph2ccmd), 0, 8, __val)
+#define SET_88E_H2CCMD_AOAC_RSVDPAGE_LOC_ARP_RSP(__ph2ccmd, __val)	\
+	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+1, 0, 8, __val)
+#define SET_88E_H2CCMD_AOAC_RSVDPAGE_LOC_NEIGHBOR_ADV(__ph2ccmd, __val)	\
+	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+2, 0, 8, __val)
+#define SET_88E_H2CCMD_AOAC_RSVDPAGE_LOC_GTK_RSP(__ph2ccmd, __val)	\
+	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+3, 0, 8, __val)
+#define SET_88E_H2CCMD_AOAC_RSVDPAGE_LOC_GTK_INFO(__ph2ccmd, __val)	\
+	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+4, 0, 8, __val)
+
+void rtl8723be_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode);
+void rtl8723be_set_fw_ap_off_load_cmd(struct ieee80211_hw *hw,
+				      u8 ap_offload_enable);
+void rtl8723be_fill_h2c_cmd(struct ieee80211_hw *hw, u8 element_id,
+			    u32 cmd_len, u8 *p_cmdbuffer);
+void rtl8723be_firmware_selfreset(struct ieee80211_hw *hw);
+void rtl8723be_set_fw_rsvdpagepkt(struct ieee80211_hw *hw,
+				  bool dl_finished);
+void rtl8723be_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus);
+int rtl8723be_download_fw(struct ieee80211_hw *hw,
+			  bool buse_wake_on_wlan_fw);
+void rtl8723be_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw,
+				      u8 p2p_ps_state);
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/hw.c b/drivers/net/wireless/rtlwifi/rtl8723be/hw.c
new file mode 100644
index 000000000000..7e70c7108d91
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/hw.c
@@ -0,0 +1,2527 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../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 "../rtl8723com/dm_common.h"
+#include "fw.h"
+#include "../rtl8723com/fw_common.h"
+#include "led.h"
+#include "hw.h"
+#include "pwrseq.h"
+#include "../btcoexist/rtl_btc.h"
+
+#define LLT_CONFIG	5
+
+static void _rtl8723be_return_beacon_queue_skb(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[BEACON_QUEUE];
+
+	while (skb_queue_len(&ring->queue)) {
+		struct rtl_tx_desc *entry = &ring->desc[ring->idx];
+		struct sk_buff *skb = __skb_dequeue(&ring->queue);
+
+		pci_unmap_single(rtlpci->pdev,
+				 rtlpriv->cfg->ops->get_desc(
+				 (u8 *)entry, true, HW_DESC_TXBUFF_ADDR),
+				 skb->len, PCI_DMA_TODEVICE);
+		kfree_skb(skb);
+		ring->idx = (ring->idx + 1) % ring->entries;
+	}
+}
+
+static void _rtl8723be_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 _rtl8723be_stop_tx_beacon(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmp1byte;
+
+	tmp1byte = rtl_read_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2);
+	rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2, tmp1byte & (~BIT(6)));
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 1, 0x64);
+	tmp1byte = rtl_read_byte(rtlpriv, REG_TBTT_PROHIBIT + 2);
+	tmp1byte &= ~(BIT(0));
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 2, tmp1byte);
+}
+
+static void _rtl8723be_resume_tx_beacon(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmp1byte;
+
+	tmp1byte = rtl_read_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2);
+	rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2, tmp1byte | BIT(6));
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 1, 0xff);
+	tmp1byte = rtl_read_byte(rtlpriv, REG_TBTT_PROHIBIT + 2);
+	tmp1byte |= BIT(1);
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 2, tmp1byte);
+}
+
+static void _rtl8723be_enable_bcn_sub_func(struct ieee80211_hw *hw)
+{
+	_rtl8723be_set_bcn_ctrl_reg(hw, 0, BIT(1));
+}
+
+static void _rtl8723be_disable_bcn_sub_func(struct ieee80211_hw *hw)
+{
+	_rtl8723be_set_bcn_ctrl_reg(hw, BIT(1), 0);
+}
+
+static void _rtl8723be_set_fw_clock_on(struct ieee80211_hw *hw, u8 rpwm_val,
+				       bool need_turn_off_ckk)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool support_remote_wake_up;
+	u32 count = 0, isr_regaddr, content;
+	bool schedule_timer = need_turn_off_ckk;
+	rtlpriv->cfg->ops->get_hw_reg(hw, HAL_DEF_WOWLAN,
+				      (u8 *)(&support_remote_wake_up));
+
+	if (!rtlhal->fw_ready)
+		return;
+	if (!rtlpriv->psc.fw_current_inpsmode)
+		return;
+
+	while (1) {
+		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+		if (rtlhal->fw_clk_change_in_progress) {
+			while (rtlhal->fw_clk_change_in_progress) {
+				spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+				count++;
+				udelay(100);
+				if (count > 1000)
+					return;
+				spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+			}
+			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+		} else {
+			rtlhal->fw_clk_change_in_progress = false;
+			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+			break;
+		}
+	}
+	if (IS_IN_LOW_POWER_STATE_88E(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_88E;
+				RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+					 "Receive CPWM INT!!! Set "
+					 "pHalData->FwPSState = %X\n",
+					 rtlhal->fw_ps_state);
+			}
+		}
+		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+		rtlhal->fw_clk_change_in_progress = false;
+		spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+		if (schedule_timer) {
+			mod_timer(&rtlpriv->works.fw_clockoff_timer,
+				  jiffies + MSECS(10));
+		}
+	} else  {
+		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+		rtlhal->fw_clk_change_in_progress = false;
+		spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+	}
+}
+
+static void _rtl8723be_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 schedule_timer = false;
+	u8 queue;
+
+	if (!rtlhal->fw_ready)
+		return;
+	if (!rtlpriv->psc.fw_current_inpsmode)
+		return;
+	if (!rtlhal->allow_sw_to_change_hwclc)
+		return;
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_RF_STATE, (u8 *)(&rtstate));
+	if (rtstate == ERFOFF || rtlpriv->psc.inactive_pwrstate == ERFOFF)
+		return;
+
+	for (queue = 0; queue < RTL_PCI_MAX_TX_QUEUE_COUNT; queue++) {
+		ring = &rtlpci->tx_ring[queue];
+		if (skb_queue_len(&ring->queue)) {
+			schedule_timer = true;
+			break;
+		}
+	}
+	if (schedule_timer) {
+		mod_timer(&rtlpriv->works.fw_clockoff_timer,
+			  jiffies + MSECS(10));
+		return;
+	}
+	if (FW_PS_STATE(rtlhal->fw_ps_state) !=
+	    FW_PS_STATE_RF_OFF_LOW_PWR_88E) {
+		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+		if (!rtlhal->fw_clk_change_in_progress) {
+			rtlhal->fw_clk_change_in_progress = true;
+			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+			rtlhal->fw_ps_state = FW_PS_STATE(rpwm_val);
+			rtl_write_word(rtlpriv, REG_HISR, 0x0100);
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
+						      (u8 *)(&rpwm_val));
+			spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+			rtlhal->fw_clk_change_in_progress = false;
+			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+		} else {
+			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+			mod_timer(&rtlpriv->works.fw_clockoff_timer,
+				  jiffies + MSECS(10));
+		}
+	}
+}
+
+static void _rtl8723be_set_fw_ps_rf_on(struct ieee80211_hw *hw)
+{
+	u8 rpwm_val = 0;
+	rpwm_val |= (FW_PS_STATE_RF_OFF_88E | FW_PS_ACK);
+	_rtl8723be_set_fw_clock_on(hw, rpwm_val, true);
+}
+
+static void _rtl8723be_fwlps_leave(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool fw_current_inps = false;
+	u8 rpwm_val = 0, fw_pwrmode = FW_PS_ACTIVE_MODE;
+
+	if (ppsc->low_power_enable) {
+		rpwm_val = (FW_PS_STATE_ALL_ON_88E | FW_PS_ACK);/* RF on */
+		_rtl8723be_set_fw_clock_on(hw, rpwm_val, false);
+		rtlhal->allow_sw_to_change_hwclc = false;
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_PWRMODE,
+					      (u8 *)(&fw_pwrmode));
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
+					      (u8 *)(&fw_current_inps));
+	} else {
+		rpwm_val = FW_PS_STATE_ALL_ON_88E;	/* RF on */
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
+					      (u8 *)(&rpwm_val));
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_PWRMODE,
+					      (u8 *)(&fw_pwrmode));
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
+					      (u8 *)(&fw_current_inps));
+	}
+}
+
+static void _rtl8723be_fwlps_enter(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool fw_current_inps = true;
+	u8 rpwm_val;
+
+	if (ppsc->low_power_enable) {
+		rpwm_val = FW_PS_STATE_RF_OFF_LOW_PWR_88E;	/* RF off */
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
+					      (u8 *)(&fw_current_inps));
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_PWRMODE,
+					      (u8 *)(&ppsc->fwctrl_psmode));
+		rtlhal->allow_sw_to_change_hwclc = true;
+		_rtl8723be_set_fw_clock_off(hw, rpwm_val);
+
+	} else {
+		rpwm_val = FW_PS_STATE_RF_OFF_88E;	/* RF off */
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
+					      (u8 *)(&fw_current_inps));
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_PWRMODE,
+					      (u8 *)(&ppsc->fwctrl_psmode));
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
+					      (u8 *)(&rpwm_val));
+	}
+}
+
+void rtl8723be_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->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(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "switch case not process %x\n", variable);
+		break;
+	}
+}
+
+void rtl8723be_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	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 rate_cfg = ((u16 *)val)[0];
+		u8 rate_index = 0;
+		rate_cfg = rate_cfg & 0x15f;
+		rate_cfg |= 0x01;
+		rtl_write_byte(rtlpriv, REG_RRSR, rate_cfg & 0xff);
+		rtl_write_byte(rtlpriv, REG_RRSR + 1, (rate_cfg >> 8) & 0xff);
+		while (rate_cfg > 0x1) {
+			rate_cfg = (rate_cfg >> 1);
+			rate_index++;
+		}
+		rtl_write_byte(rtlpriv, REG_INIRTS_RATE_SEL, rate_index);
+		break; }
+	case HW_VAR_BSSID:
+		for (idx = 0; idx < ETH_ALEN; idx++)
+			rtl_write_byte(rtlpriv, (REG_BSSID + idx), val[idx]);
+		break;
+	case HW_VAR_SIFS:
+		rtl_write_byte(rtlpriv, REG_SIFS_CTX + 1, val[0]);
+		rtl_write_byte(rtlpriv, REG_SIFS_TRX + 1, val[1]);
+
+		rtl_write_byte(rtlpriv, REG_SPEC_SIFS + 1, val[0]);
+		rtl_write_byte(rtlpriv, REG_MAC_SPEC_SIFS + 1, val[0]);
+
+		if (!mac->ht_enable)
+			rtl_write_word(rtlpriv, REG_RESP_SIFS_OFDM, 0x0e0e);
+		else
+			rtl_write_word(rtlpriv, REG_RESP_SIFS_OFDM,
+				       *((u16 *)val));
+		break;
+	case HW_VAR_SLOT_TIME: {
+		u8 e_aci;
+
+		RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
+			 "HW_VAR_SLOT_TIME %x\n", val[0]);
+
+		rtl_write_byte(rtlpriv, REG_SLOT, val[0]);
+
+		for (e_aci = 0; e_aci < AC_MAX; e_aci++) {
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AC_PARAM,
+						      (u8 *)(&e_aci));
+		}
+		break; }
+	case HW_VAR_ACK_PREAMBLE: {
+		u8 reg_tmp;
+		u8 short_preamble = (bool) (*(u8 *)val);
+		reg_tmp = rtl_read_byte(rtlpriv, REG_TRXPTCL_CTL + 2);
+		if (short_preamble) {
+			reg_tmp |= 0x02;
+			rtl_write_byte(rtlpriv, REG_TRXPTCL_CTL + 2, reg_tmp);
+		} else {
+			reg_tmp &= 0xFD;
+			rtl_write_byte(rtlpriv, REG_TRXPTCL_CTL + 2, reg_tmp);
+		}
+		break; }
+	case HW_VAR_WPA_CONFIG:
+		rtl_write_byte(rtlpriv, REG_SECCFG, *((u8 *)val));
+		break;
+	case HW_VAR_AMPDU_MIN_SPACE: {
+		u8 min_spacing_to_set;
+		u8 sec_min_space;
+
+		min_spacing_to_set = *((u8 *)val);
+		if (min_spacing_to_set <= 7) {
+			sec_min_space = 0;
+
+			if (min_spacing_to_set < sec_min_space)
+				min_spacing_to_set = sec_min_space;
+
+			mac->min_space_cfg = ((mac->min_space_cfg & 0xf8) |
+					      min_spacing_to_set);
+
+			*val = min_spacing_to_set;
+
+			RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
+				 "Set HW_VAR_AMPDU_MIN_SPACE: %#x\n",
+				 mac->min_space_cfg);
+
+			rtl_write_byte(rtlpriv, REG_AMPDU_MIN_SPACE,
+				       mac->min_space_cfg);
+		}
+		break; }
+	case HW_VAR_SHORTGI_DENSITY: {
+		u8 density_to_set;
+
+		density_to_set = *((u8 *)val);
+		mac->min_space_cfg |= (density_to_set << 3);
+
+		RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
+			 "Set HW_VAR_SHORTGI_DENSITY: %#x\n",
+			 mac->min_space_cfg);
+
+		rtl_write_byte(rtlpriv, REG_AMPDU_MIN_SPACE,
+			       mac->min_space_cfg);
+		break; }
+	case HW_VAR_AMPDU_FACTOR: {
+		u8 regtoset_normal[4] = {0x41, 0xa8, 0x72, 0xb9};
+		u8 factor_toset;
+		u8 *p_regtoset = NULL;
+		u8 index = 0;
+
+		p_regtoset = regtoset_normal;
+
+		factor_toset = *((u8 *)val);
+		if (factor_toset <= 3) {
+			factor_toset = (1 << (factor_toset + 2));
+			if (factor_toset > 0xf)
+				factor_toset = 0xf;
+
+			for (index = 0; index < 4; index++) {
+				if ((p_regtoset[index] & 0xf0) >
+				    (factor_toset << 4))
+					p_regtoset[index] =
+						(p_regtoset[index] & 0x0f) |
+						(factor_toset << 4);
+
+				if ((p_regtoset[index] & 0x0f) > factor_toset)
+					p_regtoset[index] =
+						(p_regtoset[index] & 0xf0) |
+						(factor_toset);
+
+				rtl_write_byte(rtlpriv,
+					       (REG_AGGLEN_LMT + index),
+					       p_regtoset[index]);
+			}
+			RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
+				 "Set HW_VAR_AMPDU_FACTOR: %#x\n",
+				 factor_toset);
+		}
+		break; }
+	case HW_VAR_AC_PARAM: {
+		u8 e_aci = *((u8 *)val);
+		rtl8723_dm_init_edca_turbo(hw);
+
+		if (rtlpci->acm_method != EACMWAY2_SW)
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ACM_CTRL,
+						      (u8 *)(&e_aci));
+		break; }
+	case HW_VAR_ACM_CTRL: {
+		u8 e_aci = *((u8 *)val);
+		union aci_aifsn *p_aci_aifsn =
+				(union aci_aifsn *)(&(mac->ac[0].aifs));
+		u8 acm = p_aci_aifsn->f.acm;
+		u8 acm_ctrl = rtl_read_byte(rtlpriv, REG_ACMHWCTRL);
+
+		acm_ctrl =
+		    acm_ctrl | ((rtlpci->acm_method == 2) ? 0x0 : 0x1);
+
+		if (acm) {
+			switch (e_aci) {
+			case AC0_BE:
+				acm_ctrl |= ACMHW_BEQEN;
+				break;
+			case AC2_VI:
+				acm_ctrl |= ACMHW_VIQEN;
+				break;
+			case AC3_VO:
+				acm_ctrl |= ACMHW_VOQEN;
+				break;
+			default:
+				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+					 "HW_VAR_ACM_CTRL acm set "
+					  "failed: eACI is %d\n", acm);
+				break;
+			}
+		} else {
+			switch (e_aci) {
+			case AC0_BE:
+				acm_ctrl &= (~ACMHW_BEQEN);
+				break;
+			case AC2_VI:
+				acm_ctrl &= (~ACMHW_VIQEN);
+				break;
+			case AC3_VO:
+				acm_ctrl &= (~ACMHW_BEQEN);
+				break;
+			default:
+				RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+					 "switch case not process\n");
+				break;
+			}
+		}
+		RT_TRACE(rtlpriv, COMP_QOS, DBG_TRACE,
+			 "SetHwReg8190pci(): [HW_VAR_ACM_CTRL] "
+			 "Write 0x%X\n", acm_ctrl);
+		rtl_write_byte(rtlpriv, REG_ACMHWCTRL, acm_ctrl);
+		break; }
+	case HW_VAR_RCR:
+		rtl_write_dword(rtlpriv, REG_RCR, ((u32 *)(val))[0]);
+		rtlpci->receive_config = ((u32 *)(val))[0];
+		break;
+	case HW_VAR_RETRY_LIMIT: {
+		u8 retry_limit = ((u8 *)(val))[0];
+
+		rtl_write_word(rtlpriv, REG_RL,
+			       retry_limit << RETRY_LIMIT_SHORT_SHIFT |
+			       retry_limit << RETRY_LIMIT_LONG_SHIFT);
+		break; }
+	case HW_VAR_DUAL_TSF_RST:
+		rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, (BIT(0) | BIT(1)));
+		break;
+	case HW_VAR_EFUSE_BYTES:
+		rtlefuse->efuse_usedbytes = *((u16 *)val);
+		break;
+	case HW_VAR_EFUSE_USAGE:
+		rtlefuse->efuse_usedpercentage = *((u8 *)val);
+		break;
+	case HW_VAR_IO_CMD:
+		rtl8723be_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:
+		rtl8723be_set_fw_pwrmode_cmd(hw, (*(u8 *)val));
+		break;
+	case HW_VAR_FW_PSMODE_STATUS:
+		ppsc->fw_current_inpsmode = *((bool *)val);
+		break;
+	case HW_VAR_RESUME_CLK_ON:
+		_rtl8723be_set_fw_ps_rf_on(hw);
+		break;
+	case HW_VAR_FW_LPS_ACTION: {
+		bool enter_fwlps = *((bool *)val);
+
+		if (enter_fwlps)
+			_rtl8723be_fwlps_enter(hw);
+		else
+			_rtl8723be_fwlps_leave(hw);
+
+		break; }
+	case HW_VAR_H2C_FW_JOINBSSRPT: {
+		u8 mstatus = (*(u8 *)val);
+		u8 tmp_regcr, tmp_reg422, bcnvalid_reg;
+		u8 count = 0, dlbcn_count = 0;
+		bool recover = false;
+
+		if (mstatus == RT_MEDIA_CONNECT) {
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AID, NULL);
+
+			tmp_regcr = rtl_read_byte(rtlpriv, REG_CR + 1);
+			rtl_write_byte(rtlpriv, REG_CR + 1,
+				       (tmp_regcr | BIT(0)));
+
+			_rtl8723be_set_bcn_ctrl_reg(hw, 0, BIT(3));
+			_rtl8723be_set_bcn_ctrl_reg(hw, BIT(4), 0);
+
+			tmp_reg422 = rtl_read_byte(rtlpriv,
+						   REG_FWHW_TXQ_CTRL + 2);
+			rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2,
+				       tmp_reg422 & (~BIT(6)));
+			if (tmp_reg422 & BIT(6))
+				recover = true;
+
+			do {
+				bcnvalid_reg = rtl_read_byte(rtlpriv,
+							     REG_TDECTRL + 2);
+				rtl_write_byte(rtlpriv, REG_TDECTRL + 2,
+					       (bcnvalid_reg | BIT(0)));
+				_rtl8723be_return_beacon_queue_skb(hw);
+
+				rtl8723be_set_fw_rsvdpagepkt(hw, 0);
+				bcnvalid_reg = rtl_read_byte(rtlpriv,
+							     REG_TDECTRL + 2);
+				count = 0;
+				while (!(bcnvalid_reg & BIT(0)) && count < 20) {
+					count++;
+					udelay(10);
+					bcnvalid_reg = rtl_read_byte(rtlpriv,
+							       REG_TDECTRL + 2);
+				}
+				dlbcn_count++;
+			} while (!(bcnvalid_reg & BIT(0)) && dlbcn_count < 5);
+
+			if (bcnvalid_reg & BIT(0))
+				rtl_write_byte(rtlpriv, REG_TDECTRL+2, BIT(0));
+
+			_rtl8723be_set_bcn_ctrl_reg(hw, BIT(3), 0);
+			_rtl8723be_set_bcn_ctrl_reg(hw, 0, BIT(4));
+
+			if (recover) {
+				rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2,
+					       tmp_reg422);
+			}
+			rtl_write_byte(rtlpriv, REG_CR + 1,
+				       (tmp_regcr & ~(BIT(0))));
+		}
+		rtl8723be_set_fw_joinbss_report_cmd(hw, (*(u8 *)val));
+		break; }
+	case HW_VAR_H2C_FW_P2P_PS_OFFLOAD:
+		rtl8723be_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)
+			_rtl8723be_stop_tx_beacon(hw);
+
+		_rtl8723be_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));
+
+		_rtl8723be_set_bcn_ctrl_reg(hw, BIT(3), 0);
+
+		if (btype_ibss)
+			_rtl8723be_resume_tx_beacon(hw);
+		break; }
+	case HW_VAR_KEEP_ALIVE: {
+		u8 array[2];
+		array[0] = 0xff;
+		array[1] = *((u8 *)val);
+		rtl8723be_fill_h2c_cmd(hw, H2C_8723BE_KEEP_ALIVE_CTRL,
+				       2, array);
+		break; }
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "switch case not process %x\n",
+			 variable);
+		break;
+	}
+}
+
+static bool _rtl8723be_llt_write(struct ieee80211_hw *hw, u32 address, u32 data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	bool status = true;
+	int count = 0;
+	u32 value = _LLT_INIT_ADDR(address) | _LLT_INIT_DATA(data) |
+		    _LLT_OP(_LLT_WRITE_ACCESS);
+
+	rtl_write_dword(rtlpriv, REG_LLT_INIT, value);
+
+	do {
+		value = rtl_read_dword(rtlpriv, REG_LLT_INIT);
+		if (_LLT_NO_ACTIVE == _LLT_OP_VALUE(value))
+			break;
+
+		if (count > POLLING_LLT_THRESHOLD) {
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "Failed to polling write LLT done at "
+				  "address %d!\n", address);
+			status = false;
+			break;
+		}
+	} while (++count);
+
+	return status;
+}
+
+static bool _rtl8723be_llt_table_init(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	unsigned short i;
+	u8 txpktbuf_bndy;
+	u8 maxpage;
+	bool status;
+
+	maxpage = 255;
+	txpktbuf_bndy = 245;
+
+	rtl_write_dword(rtlpriv, REG_TRXFF_BNDY,
+			(0x27FF0000 | txpktbuf_bndy));
+	rtl_write_byte(rtlpriv, REG_TDECTRL + 1, txpktbuf_bndy);
+
+	rtl_write_byte(rtlpriv, REG_TXPKTBUF_BCNQ_BDNY, txpktbuf_bndy);
+	rtl_write_byte(rtlpriv, REG_TXPKTBUF_MGQ_BDNY, txpktbuf_bndy);
+
+	rtl_write_byte(rtlpriv, 0x45D, txpktbuf_bndy);
+	rtl_write_byte(rtlpriv, REG_PBP, 0x31);
+	rtl_write_byte(rtlpriv, REG_RX_DRVINFO_SZ, 0x4);
+
+	for (i = 0; i < (txpktbuf_bndy - 1); i++) {
+		status = _rtl8723be_llt_write(hw, i, i + 1);
+		if (!status)
+			return status;
+	}
+	status = _rtl8723be_llt_write(hw, (txpktbuf_bndy - 1), 0xFF);
+
+	if (!status)
+		return status;
+
+	for (i = txpktbuf_bndy; i < maxpage; i++) {
+		status = _rtl8723be_llt_write(hw, i, (i + 1));
+		if (!status)
+			return status;
+	}
+	status = _rtl8723be_llt_write(hw, maxpage, txpktbuf_bndy);
+	if (!status)
+		return status;
+
+	rtl_write_dword(rtlpriv, REG_RQPN, 0x80e40808);
+	rtl_write_byte(rtlpriv, REG_RQPN_NPQ, 0x00);
+
+	return true;
+}
+
+static void _rtl8723be_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)
+		rtl8723be_sw_led_on(hw, pled0);
+	else if (ppsc->rfoff_reason == RF_CHANGE_BY_INIT)
+		rtl8723be_sw_led_on(hw, pled0);
+	else
+		rtl8723be_sw_led_off(hw, pled0);
+}
+
+static bool _rtl8723be_init_mac(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	unsigned char bytetmp;
+	unsigned short wordtmp;
+	u16 retry = 0;
+	bool mac_func_enable;
+
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL, 0x00);
+
+	/*Auto Power Down to CHIP-off State*/
+	bytetmp = rtl_read_byte(rtlpriv, REG_APS_FSMCO + 1) & (~BIT(7));
+	rtl_write_byte(rtlpriv, REG_APS_FSMCO + 1, bytetmp);
+
+	bytetmp = rtl_read_byte(rtlpriv, REG_CR);
+	if (bytetmp == 0xFF)
+		mac_func_enable = true;
+	else
+		mac_func_enable = false;
+
+	/* HW Power on sequence */
+	if (!rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK,
+				      PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,
+				      RTL8723_NIC_ENABLE_FLOW)) {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "init MAC Fail as power on failure\n");
+		return false;
+	}
+	bytetmp = rtl_read_byte(rtlpriv, REG_APS_FSMCO) | BIT(4);
+	rtl_write_byte(rtlpriv, REG_APS_FSMCO, bytetmp);
+
+	bytetmp = rtl_read_byte(rtlpriv, REG_CR);
+	bytetmp = 0xff;
+	rtl_write_byte(rtlpriv, REG_CR, bytetmp);
+	mdelay(2);
+
+	bytetmp = rtl_read_byte(rtlpriv, REG_HWSEQ_CTRL);
+	bytetmp |= 0x7f;
+	rtl_write_byte(rtlpriv, REG_HWSEQ_CTRL, bytetmp);
+	mdelay(2);
+
+	bytetmp = rtl_read_byte(rtlpriv, REG_SYS_CFG + 3);
+	if (bytetmp & BIT(0)) {
+		bytetmp = rtl_read_byte(rtlpriv, 0x7c);
+		bytetmp |= BIT(6);
+		rtl_write_byte(rtlpriv, 0x7c, bytetmp);
+	}
+	bytetmp = rtl_read_byte(rtlpriv, REG_SYS_CLKR);
+	bytetmp |= BIT(3);
+	rtl_write_byte(rtlpriv, REG_SYS_CLKR, bytetmp);
+	bytetmp = rtl_read_byte(rtlpriv, REG_GPIO_MUXCFG + 1);
+	bytetmp &= ~BIT(4);
+	rtl_write_byte(rtlpriv, REG_GPIO_MUXCFG + 1, bytetmp);
+
+	bytetmp = rtl_read_byte(rtlpriv, REG_PCIE_CTRL_REG+3);
+	rtl_write_byte(rtlpriv, REG_PCIE_CTRL_REG+3, bytetmp | 0x77);
+
+	rtl_write_word(rtlpriv, REG_CR, 0x2ff);
+
+	if (!mac_func_enable) {
+		if (!_rtl8723be_llt_table_init(hw))
+			return false;
+	}
+	rtl_write_dword(rtlpriv, REG_HISR, 0xffffffff);
+	rtl_write_dword(rtlpriv, REG_HISRE, 0xffffffff);
+
+	/* Enable FW Beamformer Interrupt */
+	bytetmp = rtl_read_byte(rtlpriv, REG_FWIMR + 3);
+	rtl_write_byte(rtlpriv, REG_FWIMR + 3, bytetmp | BIT(6));
+
+	wordtmp = rtl_read_word(rtlpriv, REG_TRXDMA_CTRL);
+	wordtmp &= 0xf;
+	wordtmp |= 0xF5B1;
+	rtl_write_word(rtlpriv, REG_TRXDMA_CTRL, wordtmp);
+
+	rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 1, 0x1F);
+	rtl_write_dword(rtlpriv, REG_RCR, rtlpci->receive_config);
+	rtl_write_word(rtlpriv, REG_RXFLTMAP2, 0xFFFF);
+	rtl_write_dword(rtlpriv, REG_TCR, rtlpci->transmit_config);
+
+	rtl_write_byte(rtlpriv, 0x4d0, 0x0);
+
+	rtl_write_dword(rtlpriv, REG_BCNQ_DESA,
+			((u64) rtlpci->tx_ring[BEACON_QUEUE].dma) &
+			DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_MGQ_DESA,
+			(u64) rtlpci->tx_ring[MGNT_QUEUE].dma &
+			DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_VOQ_DESA,
+			(u64) rtlpci->tx_ring[VO_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_VIQ_DESA,
+			(u64) rtlpci->tx_ring[VI_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_BEQ_DESA,
+			(u64) rtlpci->tx_ring[BE_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_BKQ_DESA,
+			(u64) rtlpci->tx_ring[BK_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_HQ_DESA,
+			(u64) rtlpci->tx_ring[HIGH_QUEUE].dma &
+			DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_RX_DESA,
+			(u64) rtlpci->rx_ring[RX_MPDU_QUEUE].dma &
+			DMA_BIT_MASK(32));
+
+	bytetmp = rtl_read_byte(rtlpriv, REG_PCIE_CTRL_REG + 3);
+	rtl_write_byte(rtlpriv, REG_PCIE_CTRL_REG + 3, bytetmp | 0x77);
+
+	rtl_write_dword(rtlpriv, REG_INT_MIG, 0);
+
+	bytetmp = rtl_read_byte(rtlpriv, REG_APSD_CTRL);
+	rtl_write_byte(rtlpriv, REG_APSD_CTRL, bytetmp & ~BIT(6));
+
+	rtl_write_byte(rtlpriv, REG_SECONDARY_CCA_CTRL, 0x3);
+
+	do {
+		retry++;
+		bytetmp = rtl_read_byte(rtlpriv, REG_APSD_CTRL);
+	} while ((retry < 200) && (bytetmp & BIT(7)));
+
+	_rtl8723be_gen_refresh_led_state(hw);
+
+	rtl_write_dword(rtlpriv, REG_MCUTST_1, 0x0);
+
+	bytetmp = rtl_read_byte(rtlpriv, REG_RXDMA_CONTROL);
+	rtl_write_byte(rtlpriv, REG_RXDMA_CONTROL, bytetmp & ~BIT(2));
+
+	return true;
+}
+
+static void _rtl8723be_hw_configure(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 reg_bw_opmode;
+	u32 reg_ratr, reg_prsr;
+
+	reg_bw_opmode = BW_OPMODE_20MHZ;
+	reg_ratr = RATE_ALL_CCK | RATE_ALL_OFDM_AG |
+		   RATE_ALL_OFDM_1SS | RATE_ALL_OFDM_2SS;
+	reg_prsr = RATE_ALL_CCK | RATE_ALL_OFDM_AG;
+
+	rtl_write_dword(rtlpriv, REG_RRSR, reg_prsr);
+	rtl_write_byte(rtlpriv, REG_HWSEQ_CTRL, 0xFF);
+}
+
+static void _rtl8723be_enable_aspm_back_door(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+
+	rtl_write_byte(rtlpriv, 0x34b, 0x93);
+	rtl_write_word(rtlpriv, 0x350, 0x870c);
+	rtl_write_byte(rtlpriv, 0x352, 0x1);
+
+	if (ppsc->support_backdoor)
+		rtl_write_byte(rtlpriv, 0x349, 0x1b);
+	else
+		rtl_write_byte(rtlpriv, 0x349, 0x03);
+
+	rtl_write_word(rtlpriv, 0x350, 0x2718);
+	rtl_write_byte(rtlpriv, 0x352, 0x1);
+}
+
+void rtl8723be_enable_hw_security_config(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 sec_reg_value;
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		 "PairwiseEncAlgorithm = %d GroupEncAlgorithm = %d\n",
+		 rtlpriv->sec.pairwise_enc_algorithm,
+		 rtlpriv->sec.group_enc_algorithm);
+
+	if (rtlpriv->cfg->mod_params->sw_crypto || rtlpriv->sec.use_sw_sec) {
+		RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+			 "not open hw encryption\n");
+		return;
+	}
+	sec_reg_value = SCR_TXENCENABLE | SCR_RXDECENABLE;
+
+	if (rtlpriv->sec.use_defaultkey) {
+		sec_reg_value |= SCR_TXUSEDK;
+		sec_reg_value |= SCR_RXUSEDK;
+	}
+	sec_reg_value |= (SCR_RXBCUSEDK | SCR_TXBCUSEDK);
+
+	rtl_write_byte(rtlpriv, REG_CR + 1, 0x02);
+
+	RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG, "The SECR-value %x\n",
+		 sec_reg_value);
+
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_WPA_CONFIG, &sec_reg_value);
+}
+
+int rtl8723be_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));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	bool rtstatus = true;
+	int err;
+	u8 tmp_u1b;
+	unsigned long flags;
+
+	/* reenable interrupts to not interfere with other devices */
+	local_save_flags(flags);
+	local_irq_enable();
+
+	rtlpriv->rtlhal.being_init_adapter = true;
+	rtlpriv->intf_ops->disable_aspm(hw);
+	rtstatus = _rtl8723be_init_mac(hw);
+	if (!rtstatus) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "Init MAC failed\n");
+		err = 1;
+		goto exit;
+	}
+	tmp_u1b = rtl_read_byte(rtlpriv, REG_SYS_CFG);
+	tmp_u1b &= 0x7F;
+	rtl_write_byte(rtlpriv, REG_SYS_CFG, tmp_u1b);
+
+	err = rtl8723_download_fw(hw, true);
+	if (err) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+			 "Failed to download FW. Init HW without FW now..\n");
+		err = 1;
+		rtlhal->fw_ready = false;
+		goto exit;
+	} else {
+		rtlhal->fw_ready = true;
+	}
+	rtlhal->last_hmeboxnum = 0;
+	rtl8723be_phy_mac_config(hw);
+	/* because last function modify RCR, so we update
+	 * rcr var here, or TP will unstable for receive_config
+	 * is wrong, RX RCR_ACRC32 will cause TP unstabel & Rx
+	 * RCR_APP_ICV will cause mac80211 unassoc for cisco 1252
+	 */
+	rtlpci->receive_config = rtl_read_dword(rtlpriv, REG_RCR);
+	rtlpci->receive_config &= ~(RCR_ACRC32 | RCR_AICV);
+	rtl_write_dword(rtlpriv, REG_RCR, rtlpci->receive_config);
+
+	rtl8723be_phy_bb_config(hw);
+	rtlphy->rf_mode = RF_OP_BY_SW_3WIRE;
+	rtl8723be_phy_rf_config(hw);
+
+	rtlphy->rfreg_chnlval[0] = rtl_get_rfreg(hw, (enum radio_path)0,
+						 RF_CHNLBW, RFREG_OFFSET_MASK);
+	rtlphy->rfreg_chnlval[1] = rtl_get_rfreg(hw, (enum radio_path)1,
+						 RF_CHNLBW, RFREG_OFFSET_MASK);
+	rtlphy->rfreg_chnlval[0] &= 0xFFF03FF;
+	rtlphy->rfreg_chnlval[0] |= (BIT(10) | BIT(11));
+
+	rtl_set_bbreg(hw, RFPGA0_RFMOD, BCCKEN, 0x1);
+	rtl_set_bbreg(hw, RFPGA0_RFMOD, BOFDMEN, 0x1);
+	rtl_set_bbreg(hw, RFPGA0_ANALOGPARAMETER2, BIT(10), 1);
+	_rtl8723be_hw_configure(hw);
+	rtl_cam_reset_all_entry(hw);
+	rtl8723be_enable_hw_security_config(hw);
+
+	ppsc->rfpwr_state = ERFON;
+
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ETHER_ADDR, mac->mac_addr);
+	_rtl8723be_enable_aspm_back_door(hw);
+	rtlpriv->intf_ops->enable_aspm(hw);
+
+	rtl8723be_bt_hw_init(hw);
+
+	rtl_set_bbreg(hw, 0x64, BIT(20), 0);
+	rtl_set_bbreg(hw, 0x64, BIT(24), 0);
+
+	rtl_set_bbreg(hw, 0x40, BIT(4), 0);
+	rtl_set_bbreg(hw, 0x40, BIT(3), 1);
+
+	rtl_set_bbreg(hw, 0x944, BIT(0)|BIT(1), 0x3);
+	rtl_set_bbreg(hw, 0x930, 0xff, 0x77);
+
+	rtl_set_bbreg(hw, 0x38, BIT(11), 0x1);
+
+	rtl_set_bbreg(hw, 0xb2c, 0xffffffff, 0x80000000);
+
+	if (ppsc->rfpwr_state == ERFON) {
+		rtl8723be_dm_check_txpower_tracking(hw);
+		rtl8723be_phy_lc_calibrate(hw);
+	}
+	tmp_u1b = efuse_read_1byte(hw, 0x1FA);
+	if (!(tmp_u1b & BIT(0))) {
+		rtl_set_rfreg(hw, RF90_PATH_A, 0x15, 0x0F, 0x05);
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "PA BIAS path A\n");
+	}
+	if (!(tmp_u1b & BIT(4))) {
+		tmp_u1b = rtl_read_byte(rtlpriv, 0x16);
+		tmp_u1b &= 0x0F;
+		rtl_write_byte(rtlpriv, 0x16, tmp_u1b | 0x80);
+		udelay(10);
+		rtl_write_byte(rtlpriv, 0x16, tmp_u1b | 0x90);
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "under 1.5V\n");
+	}
+	rtl8723be_dm_init(hw);
+exit:
+	local_irq_restore(flags);
+	rtlpriv->rtlhal.being_init_adapter = false;
+	return err;
+}
+
+static enum version_8723e _rtl8723be_read_chip_version(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	enum version_8723e version = VERSION_UNKNOWN;
+	u8 count = 0;
+	u8 value8;
+	u32 value32;
+
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL, 0);
+
+	value8 = rtl_read_byte(rtlpriv, REG_APS_FSMCO + 2);
+	rtl_write_byte(rtlpriv, REG_APS_FSMCO + 2, value8 | BIT(0));
+
+	value8 = rtl_read_byte(rtlpriv, REG_APS_FSMCO + 1);
+	rtl_write_byte(rtlpriv, REG_APS_FSMCO + 1, value8 | BIT(0));
+
+	value8 = rtl_read_byte(rtlpriv, REG_APS_FSMCO + 1);
+	while (((value8 & BIT(0))) && (count++ < 100)) {
+		udelay(10);
+		value8 = rtl_read_byte(rtlpriv, REG_APS_FSMCO + 1);
+	}
+	count = 0;
+	value8 = rtl_read_byte(rtlpriv, REG_ROM_VERSION);
+	while ((value8 == 0) && (count++ < 50)) {
+		value8 = rtl_read_byte(rtlpriv, REG_ROM_VERSION);
+		mdelay(1);
+	}
+	value32 = rtl_read_dword(rtlpriv, REG_SYS_CFG1);
+	if ((value32 & (CHIP_8723B)) != CHIP_8723B)
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "unkown chip version\n");
+	else
+		version = (enum version_8723e) VERSION_TEST_CHIP_1T1R_8723B;
+
+		rtlphy->rf_type = RF_1T1R;
+
+	value8 = rtl_read_byte(rtlpriv, REG_ROM_VERSION);
+	if (value8 >= 0x02)
+		version |= BIT(3);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "Chip RF Type: %s\n", (rtlphy->rf_type == RF_2T2R) ?
+		 "RF_2T2R" : "RF_1T1R");
+
+	return version;
+}
+
+static int _rtl8723be_set_media_status(struct ieee80211_hw *hw,
+				       enum nl80211_iftype type)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 bt_msr = rtl_read_byte(rtlpriv, MSR) & 0xfc;
+	enum led_ctl_mode ledaction = LED_CTL_NO_LINK;
+
+	rtl_write_dword(rtlpriv, REG_BCN_CTRL, 0);
+	RT_TRACE(rtlpriv, COMP_BEACON, DBG_LOUD,
+		 "clear 0x550 when set HW_VAR_MEDIA_STATUS\n");
+
+	if (type == NL80211_IFTYPE_UNSPECIFIED ||
+	    type == NL80211_IFTYPE_STATION) {
+		_rtl8723be_stop_tx_beacon(hw);
+		_rtl8723be_enable_bcn_sub_func(hw);
+	} else if (type == NL80211_IFTYPE_ADHOC || type == NL80211_IFTYPE_AP) {
+		_rtl8723be_resume_tx_beacon(hw);
+		_rtl8723be_disable_bcn_sub_func(hw);
+	} else {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+			 "Set HW_VAR_MEDIA_STATUS: "
+			 "No such media status(%x).\n", type);
+	}
+	switch (type) {
+	case NL80211_IFTYPE_UNSPECIFIED:
+		bt_msr |= MSR_NOLINK;
+		ledaction = LED_CTL_LINK;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "Set Network type to NO LINK!\n");
+		break;
+	case NL80211_IFTYPE_ADHOC:
+		bt_msr |= MSR_ADHOC;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "Set Network type to Ad Hoc!\n");
+		break;
+	case NL80211_IFTYPE_STATION:
+		bt_msr |= MSR_INFRA;
+		ledaction = LED_CTL_LINK;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "Set Network type to STA!\n");
+		break;
+	case NL80211_IFTYPE_AP:
+		bt_msr |= MSR_AP;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "Set Network type to AP!\n");
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "Network type %d not support!\n", type);
+		return 1;
+	}
+	rtl_write_byte(rtlpriv, (MSR), bt_msr);
+	rtlpriv->cfg->ops->led_control(hw, ledaction);
+	if ((bt_msr & 0x03) == MSR_AP)
+		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x00);
+	else
+		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x66);
+	return 0;
+}
+
+void rtl8723be_set_check_bssid(struct ieee80211_hw *hw, bool check_bssid)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u32 reg_rcr = rtlpci->receive_config;
+
+	if (rtlpriv->psc.rfpwr_state != ERFON)
+		return;
+
+	if (check_bssid) {
+		reg_rcr |= (RCR_CBSSID_DATA | RCR_CBSSID_BCN);
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_RCR,
+					      (u8 *)(&reg_rcr));
+		_rtl8723be_set_bcn_ctrl_reg(hw, 0, BIT(4));
+	} else if (!check_bssid) {
+		reg_rcr &= (~(RCR_CBSSID_DATA | RCR_CBSSID_BCN));
+		_rtl8723be_set_bcn_ctrl_reg(hw, BIT(4), 0);
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_RCR,
+					      (u8 *)(&reg_rcr));
+	}
+}
+
+int rtl8723be_set_network_type(struct ieee80211_hw *hw,
+			       enum nl80211_iftype type)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (_rtl8723be_set_media_status(hw, type))
+		return -EOPNOTSUPP;
+
+	if (rtlpriv->mac80211.link_state == MAC80211_LINKED) {
+		if (type != NL80211_IFTYPE_AP)
+			rtl8723be_set_check_bssid(hw, true);
+	} else {
+		rtl8723be_set_check_bssid(hw, false);
+	}
+	return 0;
+}
+
+/* don't set REG_EDCA_BE_PARAM here
+ * because mac80211 will send pkt when scan
+ */
+void rtl8723be_set_qos(struct ieee80211_hw *hw, int aci)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	rtl8723_dm_init_edca_turbo(hw);
+	switch (aci) {
+	case AC1_BK:
+		rtl_write_dword(rtlpriv, REG_EDCA_BK_PARAM, 0xa44f);
+		break;
+	case AC0_BE:
+		break;
+	case AC2_VI:
+		rtl_write_dword(rtlpriv, REG_EDCA_VI_PARAM, 0x5e4322);
+		break;
+	case AC3_VO:
+		rtl_write_dword(rtlpriv, REG_EDCA_VO_PARAM, 0x2f3222);
+		break;
+	default:
+		RT_ASSERT(false, "invalid aci: %d !\n", aci);
+		break;
+	}
+}
+
+void rtl8723be_enable_interrupt(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	rtl_write_dword(rtlpriv, REG_HIMR, rtlpci->irq_mask[0] & 0xFFFFFFFF);
+	rtl_write_dword(rtlpriv, REG_HIMRE, rtlpci->irq_mask[1] & 0xFFFFFFFF);
+	rtlpci->irq_enabled = true;
+	/* there are some C2H CMDs have been sent
+	 * before system interrupt is enabled, e.g., C2H, CPWM.
+	 * So we need to clear all C2H events that FW has notified,
+	 * otherwise FW won't schedule any commands anymore.
+	 */
+	rtl_write_byte(rtlpriv, REG_C2HEVT_CLEAR, 0);
+	/*enable system interrupt*/
+	rtl_write_dword(rtlpriv, REG_HSIMR, rtlpci->sys_irq_mask & 0xFFFFFFFF);
+}
+
+void rtl8723be_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 _rtl8723be_poweroff_adapter(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 u1b_tmp;
+
+	/* Combo (PCIe + USB) Card and PCIe-MF Card */
+	/* 1. Run LPS WL RFOFF flow */
+	rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
+				 PWR_INTF_PCI_MSK, RTL8723_NIC_LPS_ENTER_FLOW);
+
+	/* 2. 0x1F[7:0] = 0 */
+	/* turn off RF */
+	rtl_write_byte(rtlpriv, REG_RF_CTRL, 0x00);
+	if ((rtl_read_byte(rtlpriv, REG_MCUFWDL) & BIT(7)) &&
+	    rtlhal->fw_ready)
+		rtl8723be_firmware_selfreset(hw);
+
+	/* Reset MCU. Suggested by Filen. */
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN + 1, (u1b_tmp & (~BIT(2))));
+
+	/* g.	MCUFWDL 0x80[1:0]= 0	 */
+	/* reset MCU ready status */
+	rtl_write_byte(rtlpriv, REG_MCUFWDL, 0x00);
+
+	/* HW card disable configuration. */
+	rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
+				 PWR_INTF_PCI_MSK, RTL8723_NIC_DISABLE_FLOW);
+
+	/* Reset MCU IO Wrapper */
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL + 1);
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL + 1, (u1b_tmp & (~BIT(0))));
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL + 1);
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL + 1, u1b_tmp | BIT(0));
+
+	/* 7. RSV_CTRL 0x1C[7:0] = 0x0E */
+	/* lock ISO/CLK/Power control register */
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL, 0x0e);
+}
+
+void rtl8723be_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;
+
+	mac->link_state = MAC80211_NOLINK;
+	opmode = NL80211_IFTYPE_UNSPECIFIED;
+	_rtl8723be_set_media_status(hw, opmode);
+	if (rtlpriv->rtlhal.driver_is_goingto_unload ||
+	    ppsc->rfoff_reason > RF_CHANGE_BY_PS)
+		rtlpriv->cfg->ops->led_control(hw, LED_CTL_POWER_OFF);
+	RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
+	_rtl8723be_poweroff_adapter(hw);
+
+	/* after power off we should do iqk again */
+	rtlpriv->phy.iqk_initialized = false;
+}
+
+void rtl8723be_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 rtl8723be_set_beacon_related_registers(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u16 bcn_interval, atim_window;
+
+	bcn_interval = mac->beacon_interval;
+	atim_window = 2;	/*FIX MERGE */
+	rtl8723be_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);
+	rtl8723be_enable_interrupt(hw);
+}
+
+void rtl8723be_set_beacon_interval(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u16 bcn_interval = mac->beacon_interval;
+
+	RT_TRACE(rtlpriv, COMP_BEACON, DBG_DMESG,
+		 "beacon_interval:%d\n", bcn_interval);
+	rtl8723be_disable_interrupt(hw);
+	rtl_write_word(rtlpriv, REG_BCN_INTERVAL, bcn_interval);
+	rtl8723be_enable_interrupt(hw);
+}
+
+void rtl8723be_update_interrupt_mask(struct ieee80211_hw *hw,
+				   u32 add_msr, u32 rm_msr)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	RT_TRACE(rtlpriv, COMP_INTR, DBG_LOUD,
+		 "add_msr:%x, rm_msr:%x\n", add_msr, rm_msr);
+
+	if (add_msr)
+		rtlpci->irq_mask[0] |= add_msr;
+	if (rm_msr)
+		rtlpci->irq_mask[0] &= (~rm_msr);
+	rtl8723be_disable_interrupt(hw);
+	rtl8723be_enable_interrupt(hw);
+}
+
+static u8 _rtl8723be_get_chnl_group(u8 chnl)
+{
+	u8 group;
+
+	if (chnl < 3)
+		group = 0;
+	else if (chnl < 9)
+		group = 1;
+	else
+		group = 2;
+	return group;
+}
+
+static void _rtl8723be_read_power_value_fromprom(struct ieee80211_hw *hw,
+					struct txpower_info_2g *pw2g,
+					struct txpower_info_5g *pw5g,
+					bool autoload_fail, u8 *hwinfo)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 path, addr = EEPROM_TX_PWR_INX, group, cnt = 0;
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "hal_ReadPowerValueFromPROM8723BE(): "
+		 "PROMContent[0x%x]= 0x%x\n",
+		 (addr + 1), hwinfo[addr + 1]);
+	if (0xFF == hwinfo[addr + 1])  /*YJ, add, 120316*/
+		autoload_fail = true;
+
+	if (autoload_fail) {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "auto load fail : Use Default value!\n");
+		for (path = 0; path < MAX_RF_PATH; path++) {
+			/* 2.4G default value */
+			for (group = 0; group < MAX_CHNL_GROUP_24G; group++) {
+				pw2g->index_cck_base[path][group] = 0x2D;
+				pw2g->index_bw40_base[path][group] = 0x2D;
+			}
+			for (cnt = 0; cnt < MAX_TX_COUNT; cnt++) {
+				if (cnt == 0) {
+					pw2g->bw20_diff[path][0] = 0x02;
+					pw2g->ofdm_diff[path][0] = 0x04;
+				} else {
+					pw2g->bw20_diff[path][cnt] = 0xFE;
+					pw2g->bw40_diff[path][cnt] = 0xFE;
+					pw2g->cck_diff[path][cnt] = 0xFE;
+					pw2g->ofdm_diff[path][cnt] = 0xFE;
+				}
+			}
+		}
+		return;
+	}
+	for (path = 0; path < MAX_RF_PATH; path++) {
+		/*2.4G default value*/
+		for (group = 0; group < MAX_CHNL_GROUP_24G; group++) {
+			pw2g->index_cck_base[path][group] = hwinfo[addr++];
+			if (pw2g->index_cck_base[path][group] == 0xFF)
+				pw2g->index_cck_base[path][group] = 0x2D;
+		}
+		for (group = 0; group < MAX_CHNL_GROUP_24G - 1; group++) {
+			pw2g->index_bw40_base[path][group] = hwinfo[addr++];
+			if (pw2g->index_bw40_base[path][group] == 0xFF)
+				pw2g->index_bw40_base[path][group] = 0x2D;
+		}
+		for (cnt = 0; cnt < MAX_TX_COUNT; cnt++) {
+			if (cnt == 0) {
+				pw2g->bw40_diff[path][cnt] = 0;
+				if (hwinfo[addr] == 0xFF) {
+					pw2g->bw20_diff[path][cnt] = 0x02;
+				} else {
+					pw2g->bw20_diff[path][cnt] =
+						(hwinfo[addr] & 0xf0) >> 4;
+					/*bit sign number to 8 bit sign number*/
+					if (pw2g->bw20_diff[path][cnt] & BIT(3))
+						pw2g->bw20_diff[path][cnt] |= 0xF0;
+				}
+				if (hwinfo[addr] == 0xFF) {
+					pw2g->ofdm_diff[path][cnt] = 0x04;
+				} else {
+					pw2g->ofdm_diff[path][cnt] =
+							(hwinfo[addr] & 0x0f);
+					/*bit sign number to 8 bit sign number*/
+					if (pw2g->ofdm_diff[path][cnt] & BIT(3))
+						pw2g->ofdm_diff[path][cnt] |=
+									  0xF0;
+				}
+				pw2g->cck_diff[path][cnt] = 0;
+				addr++;
+			} else {
+				if (hwinfo[addr] == 0xFF) {
+					pw2g->bw40_diff[path][cnt] = 0xFE;
+				} else {
+					pw2g->bw40_diff[path][cnt] =
+						(hwinfo[addr] & 0xf0) >> 4;
+					if (pw2g->bw40_diff[path][cnt] & BIT(3))
+						pw2g->bw40_diff[path][cnt] |=
+									  0xF0;
+				}
+				if (hwinfo[addr] == 0xFF) {
+					pw2g->bw20_diff[path][cnt] = 0xFE;
+				} else {
+					pw2g->bw20_diff[path][cnt] =
+							(hwinfo[addr] & 0x0f);
+					if (pw2g->bw20_diff[path][cnt] & BIT(3))
+						pw2g->bw20_diff[path][cnt] |=
+									  0xF0;
+				}
+				addr++;
+
+				if (hwinfo[addr] == 0xFF) {
+					pw2g->ofdm_diff[path][cnt] = 0xFE;
+				} else {
+					pw2g->ofdm_diff[path][cnt] =
+						(hwinfo[addr] & 0xf0) >> 4;
+					if (pw2g->ofdm_diff[path][cnt] & BIT(3))
+						pw2g->ofdm_diff[path][cnt] |=
+									  0xF0;
+				}
+				if (hwinfo[addr] == 0xFF) {
+					pw2g->cck_diff[path][cnt] = 0xFE;
+				} else {
+					pw2g->cck_diff[path][cnt] =
+							(hwinfo[addr] & 0x0f);
+					if (pw2g->cck_diff[path][cnt] & BIT(3))
+						pw2g->cck_diff[path][cnt] |=
+									 0xF0;
+				}
+				addr++;
+			}
+		}
+		/*5G default value*/
+		for (group = 0; group < MAX_CHNL_GROUP_5G; group++) {
+			pw5g->index_bw40_base[path][group] = hwinfo[addr++];
+			if (pw5g->index_bw40_base[path][group] == 0xFF)
+				pw5g->index_bw40_base[path][group] = 0xFE;
+		}
+		for (cnt = 0; cnt < MAX_TX_COUNT; cnt++) {
+			if (cnt == 0) {
+				pw5g->bw40_diff[path][cnt] = 0;
+
+				if (hwinfo[addr] == 0xFF) {
+					pw5g->bw20_diff[path][cnt] = 0;
+				} else {
+					pw5g->bw20_diff[path][0] =
+						(hwinfo[addr] & 0xf0) >> 4;
+					if (pw5g->bw20_diff[path][cnt] & BIT(3))
+						pw5g->bw20_diff[path][cnt] |=
+									  0xF0;
+				}
+				if (hwinfo[addr] == 0xFF) {
+					pw5g->ofdm_diff[path][cnt] = 0x04;
+				} else {
+					pw5g->ofdm_diff[path][0] =
+							(hwinfo[addr] & 0x0f);
+					if (pw5g->ofdm_diff[path][cnt] & BIT(3))
+						pw5g->ofdm_diff[path][cnt] |=
+									  0xF0;
+				}
+				addr++;
+			} else {
+				if (hwinfo[addr] == 0xFF) {
+					pw5g->bw40_diff[path][cnt] = 0xFE;
+				} else {
+					pw5g->bw40_diff[path][cnt] =
+						(hwinfo[addr] & 0xf0) >> 4;
+					if (pw5g->bw40_diff[path][cnt] & BIT(3))
+						pw5g->bw40_diff[path][cnt] |= 0xF0;
+				}
+				if (hwinfo[addr] == 0xFF) {
+					pw5g->bw20_diff[path][cnt] = 0xFE;
+				} else {
+					pw5g->bw20_diff[path][cnt] =
+							(hwinfo[addr] & 0x0f);
+					if (pw5g->bw20_diff[path][cnt] & BIT(3))
+						pw5g->bw20_diff[path][cnt] |= 0xF0;
+				}
+				addr++;
+			}
+		}
+		if (hwinfo[addr] == 0xFF) {
+			pw5g->ofdm_diff[path][1] = 0xFE;
+			pw5g->ofdm_diff[path][2] = 0xFE;
+		} else {
+			pw5g->ofdm_diff[path][1] = (hwinfo[addr] & 0xf0) >> 4;
+			pw5g->ofdm_diff[path][2] = (hwinfo[addr] & 0x0f);
+		}
+		addr++;
+
+		if (hwinfo[addr] == 0xFF)
+			pw5g->ofdm_diff[path][3] = 0xFE;
+		else
+			pw5g->ofdm_diff[path][3] = (hwinfo[addr] & 0x0f);
+		addr++;
+
+		for (cnt = 1; cnt < MAX_TX_COUNT; cnt++) {
+			if (pw5g->ofdm_diff[path][cnt] == 0xFF)
+				pw5g->ofdm_diff[path][cnt] = 0xFE;
+			else if (pw5g->ofdm_diff[path][cnt] & BIT(3))
+				pw5g->ofdm_diff[path][cnt] |= 0xF0;
+		}
+	}
+}
+
+static void _rtl8723be_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
+						   bool autoload_fail,
+						   u8 *hwinfo)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct txpower_info_2g pw2g;
+	struct txpower_info_5g pw5g;
+	u8 rf_path, index;
+	u8 i;
+
+	_rtl8723be_read_power_value_fromprom(hw, &pw2g, &pw5g, autoload_fail,
+					     hwinfo);
+
+	for (rf_path = 0; rf_path < 2; rf_path++) {
+		for (i = 0; i < 14; i++) {
+			index = _rtl8723be_get_chnl_group(i+1);
+
+			rtlefuse->txpwrlevel_cck[rf_path][i] =
+					pw2g.index_cck_base[rf_path][index];
+			rtlefuse->txpwrlevel_ht40_1s[rf_path][i] =
+					pw2g.index_bw40_base[rf_path][index];
+		}
+		for (i = 0; i < MAX_TX_COUNT; i++) {
+			rtlefuse->txpwr_ht20diff[rf_path][i] =
+						pw2g.bw20_diff[rf_path][i];
+			rtlefuse->txpwr_ht40diff[rf_path][i] =
+						pw2g.bw40_diff[rf_path][i];
+			rtlefuse->txpwr_legacyhtdiff[rf_path][i] =
+						pw2g.ofdm_diff[rf_path][i];
+		}
+		for (i = 0; i < 14; i++) {
+			RTPRINT(rtlpriv, FINIT, INIT_EEPROM,
+				"RF(%d)-Ch(%d) [CCK / HT40_1S ] = "
+				"[0x%x / 0x%x ]\n", rf_path, i,
+				rtlefuse->txpwrlevel_cck[rf_path][i],
+				rtlefuse->txpwrlevel_ht40_1s[rf_path][i]);
+		}
+	}
+	if (!autoload_fail)
+		rtlefuse->eeprom_thermalmeter =
+					hwinfo[EEPROM_THERMAL_METER_88E];
+	else
+		rtlefuse->eeprom_thermalmeter = EEPROM_DEFAULT_THERMALMETER;
+
+	if (rtlefuse->eeprom_thermalmeter == 0xff || autoload_fail) {
+		rtlefuse->apk_thermalmeterignore = true;
+		rtlefuse->eeprom_thermalmeter = EEPROM_DEFAULT_THERMALMETER;
+	}
+	rtlefuse->thermalmeter[0] = rtlefuse->eeprom_thermalmeter;
+	RTPRINT(rtlpriv, FINIT, INIT_EEPROM,
+		"thermalmeter = 0x%x\n", rtlefuse->eeprom_thermalmeter);
+
+	if (!autoload_fail) {
+		rtlefuse->eeprom_regulatory =
+			hwinfo[EEPROM_RF_BOARD_OPTION_88E] & 0x07;/*bit0~2*/
+		if (hwinfo[EEPROM_RF_BOARD_OPTION_88E] == 0xFF)
+			rtlefuse->eeprom_regulatory = 0;
+	} else {
+		rtlefuse->eeprom_regulatory = 0;
+	}
+	RTPRINT(rtlpriv, FINIT, INIT_EEPROM,
+		"eeprom_regulatory = 0x%x\n", rtlefuse->eeprom_regulatory);
+}
+
+static void _rtl8723be_read_adapter_info(struct ieee80211_hw *hw,
+					 bool pseudo_test)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u16 i, usvalue;
+	u8 hwinfo[HWSET_MAX_SIZE];
+	u16 eeprom_id;
+	bool is_toshiba_smid1 = false;
+	bool is_toshiba_smid2 = false;
+	bool is_samsung_smid = false;
+	bool is_lenovo_smid = false;
+	u16 toshiba_smid1[] = {
+		0x6151, 0x6152, 0x6154, 0x6155, 0x6177, 0x6178, 0x6179, 0x6180,
+		0x7151, 0x7152, 0x7154, 0x7155, 0x7177, 0x7178, 0x7179, 0x7180,
+		0x8151, 0x8152, 0x8154, 0x8155, 0x8181, 0x8182, 0x8184, 0x8185,
+		0x9151, 0x9152, 0x9154, 0x9155, 0x9181, 0x9182, 0x9184, 0x9185
+	};
+	u16 toshiba_smid2[] = {
+		0x6181, 0x6184, 0x6185, 0x7181, 0x7182, 0x7184, 0x7185, 0x8181,
+		0x8182, 0x8184, 0x8185, 0x9181, 0x9182, 0x9184, 0x9185
+	};
+	u16 samsung_smid[] = {
+		0x6191, 0x6192, 0x6193, 0x7191, 0x7192, 0x7193, 0x8191, 0x8192,
+		0x8193, 0x9191, 0x9192, 0x9193
+	};
+	u16 lenovo_smid[] = {
+		0x8195, 0x9195, 0x7194, 0x8200, 0x8201, 0x8202, 0x9199, 0x9200
+	};
+
+	if (pseudo_test) {
+		/* needs to be added */
+		return;
+	}
+	if (rtlefuse->epromtype == EEPROM_BOOT_EFUSE) {
+		rtl_efuse_shadow_map_update(hw);
+
+		memcpy(hwinfo, &rtlefuse->efuse_map[EFUSE_INIT_MAP][0],
+		       HWSET_MAX_SIZE);
+	} else if (rtlefuse->epromtype == EEPROM_93C46) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "RTL819X Not boot from eeprom, check it !!");
+	}
+	RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_DMESG, ("MAP\n"),
+		      hwinfo, HWSET_MAX_SIZE);
+
+	eeprom_id = *((u16 *)&hwinfo[0]);
+	if (eeprom_id != RTL8723BE_EEPROM_ID) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+			 "EEPROM ID(%#x) is invalid!!\n", eeprom_id);
+		rtlefuse->autoload_failflag = true;
+	} else {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "Autoload OK\n");
+		rtlefuse->autoload_failflag = false;
+	}
+	if (rtlefuse->autoload_failflag)
+		return;
+
+	rtlefuse->eeprom_vid = *(u16 *)&hwinfo[EEPROM_VID];
+	rtlefuse->eeprom_did = *(u16 *)&hwinfo[EEPROM_DID];
+	rtlefuse->eeprom_svid = *(u16 *)&hwinfo[EEPROM_SVID];
+	rtlefuse->eeprom_smid = *(u16 *)&hwinfo[EEPROM_SMID];
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROMId = 0x%4x\n", eeprom_id);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROM VID = 0x%4x\n", rtlefuse->eeprom_vid);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROM DID = 0x%4x\n", rtlefuse->eeprom_did);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROM SVID = 0x%4x\n", rtlefuse->eeprom_svid);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROM SMID = 0x%4x\n", rtlefuse->eeprom_smid);
+
+	for (i = 0; i < 6; i += 2) {
+		usvalue = *(u16 *)&hwinfo[EEPROM_MAC_ADDR + i];
+		*((u16 *)(&rtlefuse->dev_addr[i])) = usvalue;
+	}
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "dev_addr: %pM\n",
+		 rtlefuse->dev_addr);
+
+	/*parse xtal*/
+	rtlefuse->crystalcap = hwinfo[EEPROM_XTAL_8723BE];
+	if (rtlefuse->crystalcap == 0xFF)
+		rtlefuse->crystalcap = 0x20;
+
+	_rtl8723be_read_txpower_info_from_hwpg(hw, rtlefuse->autoload_failflag,
+					       hwinfo);
+
+	rtl8723be_read_bt_coexist_info_from_hwpg(hw,
+						 rtlefuse->autoload_failflag,
+						 hwinfo);
+
+	rtlefuse->eeprom_channelplan = *(u8 *)&hwinfo[EEPROM_CHANNELPLAN];
+	rtlefuse->eeprom_version = *(u16 *)&hwinfo[EEPROM_VERSION];
+	rtlefuse->txpwr_fromeprom = true;
+	rtlefuse->eeprom_oemid = *(u8 *)&hwinfo[EEPROM_CUSTOMER_ID];
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROM Customer ID: 0x%2x\n", rtlefuse->eeprom_oemid);
+
+	/* set channel plan to world wide 13 */
+	rtlefuse->channel_plan = COUNTRY_CODE_WORLD_WIDE_13;
+
+	if (rtlhal->oem_id == RT_CID_DEFAULT) {
+		/* Does this one have a Toshiba SMID from group 1? */
+		for (i = 0; i < sizeof(toshiba_smid1) / sizeof(u16); i++) {
+			if (rtlefuse->eeprom_smid == toshiba_smid1[i]) {
+				is_toshiba_smid1 = true;
+				break;
+			}
+		}
+		/* Does this one have a Toshiba SMID from group 2? */
+		for (i = 0; i < sizeof(toshiba_smid2) / sizeof(u16); i++) {
+			if (rtlefuse->eeprom_smid == toshiba_smid2[i]) {
+				is_toshiba_smid2 = true;
+				break;
+			}
+		}
+		/* Does this one have a Samsung SMID? */
+		for (i = 0; i < sizeof(samsung_smid) / sizeof(u16); i++) {
+			if (rtlefuse->eeprom_smid == samsung_smid[i]) {
+				is_samsung_smid = true;
+				break;
+			}
+		}
+		/* Does this one have a Lenovo SMID? */
+		for (i = 0; i < sizeof(lenovo_smid) / sizeof(u16); i++) {
+			if (rtlefuse->eeprom_smid == lenovo_smid[i]) {
+				is_lenovo_smid = true;
+				break;
+			}
+		}
+		switch (rtlefuse->eeprom_oemid) {
+		case EEPROM_CID_DEFAULT:
+			if (rtlefuse->eeprom_did == 0x8176) {
+				if (rtlefuse->eeprom_svid == 0x10EC &&
+				    is_toshiba_smid1) {
+					rtlhal->oem_id = RT_CID_TOSHIBA;
+				} else if (rtlefuse->eeprom_svid == 0x1025) {
+					rtlhal->oem_id = RT_CID_819X_ACER;
+				} else if (rtlefuse->eeprom_svid == 0x10EC &&
+					   is_samsung_smid) {
+					rtlhal->oem_id = RT_CID_819X_SAMSUNG;
+				} else if (rtlefuse->eeprom_svid == 0x10EC &&
+					   is_lenovo_smid) {
+					rtlhal->oem_id = RT_CID_819X_LENOVO;
+				} else if ((rtlefuse->eeprom_svid == 0x10EC &&
+					    rtlefuse->eeprom_smid == 0x8197) ||
+					   (rtlefuse->eeprom_svid == 0x10EC &&
+					    rtlefuse->eeprom_smid == 0x9196)) {
+					rtlhal->oem_id = RT_CID_819X_CLEVO;
+				} else if ((rtlefuse->eeprom_svid == 0x1028 &&
+					    rtlefuse->eeprom_smid == 0x8194) ||
+					   (rtlefuse->eeprom_svid == 0x1028 &&
+					    rtlefuse->eeprom_smid == 0x8198) ||
+					   (rtlefuse->eeprom_svid == 0x1028 &&
+					    rtlefuse->eeprom_smid == 0x9197) ||
+					   (rtlefuse->eeprom_svid == 0x1028 &&
+					    rtlefuse->eeprom_smid == 0x9198)) {
+					rtlhal->oem_id = RT_CID_819X_DELL;
+				} else if ((rtlefuse->eeprom_svid == 0x103C &&
+					    rtlefuse->eeprom_smid == 0x1629)) {
+					rtlhal->oem_id = RT_CID_819X_HP;
+				} else if ((rtlefuse->eeprom_svid == 0x1A32 &&
+					   rtlefuse->eeprom_smid == 0x2315)) {
+					rtlhal->oem_id = RT_CID_819X_QMI;
+				} else if ((rtlefuse->eeprom_svid == 0x10EC &&
+					   rtlefuse->eeprom_smid == 0x8203)) {
+					rtlhal->oem_id = RT_CID_819X_PRONETS;
+				} else if ((rtlefuse->eeprom_svid == 0x1043 &&
+					   rtlefuse->eeprom_smid == 0x84B5)) {
+					rtlhal->oem_id = RT_CID_819X_EDIMAX_ASUS;
+				} else {
+					rtlhal->oem_id = RT_CID_DEFAULT;
+				}
+			} else if (rtlefuse->eeprom_did == 0x8178) {
+				if (rtlefuse->eeprom_svid == 0x10EC &&
+				    is_toshiba_smid2)
+					rtlhal->oem_id = RT_CID_TOSHIBA;
+				else if (rtlefuse->eeprom_svid == 0x1025)
+					rtlhal->oem_id = RT_CID_819X_ACER;
+				else if ((rtlefuse->eeprom_svid == 0x10EC &&
+					  rtlefuse->eeprom_smid == 0x8186))
+					rtlhal->oem_id = RT_CID_819X_PRONETS;
+				else if ((rtlefuse->eeprom_svid == 0x1043 &&
+					  rtlefuse->eeprom_smid == 0x84B6))
+					rtlhal->oem_id =
+							RT_CID_819X_EDIMAX_ASUS;
+				else
+					rtlhal->oem_id = RT_CID_DEFAULT;
+			} else {
+					rtlhal->oem_id = RT_CID_DEFAULT;
+			}
+			break;
+		case EEPROM_CID_TOSHIBA:
+			rtlhal->oem_id = RT_CID_TOSHIBA;
+			break;
+		case EEPROM_CID_CCX:
+			rtlhal->oem_id = RT_CID_CCX;
+			break;
+		case EEPROM_CID_QMI:
+			rtlhal->oem_id = RT_CID_819X_QMI;
+			break;
+		case EEPROM_CID_WHQL:
+			break;
+		default:
+			rtlhal->oem_id = RT_CID_DEFAULT;
+			break;
+		}
+	}
+}
+
+static void _rtl8723be_hal_customized_behavior(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	pcipriv->ledctl.led_opendrain = true;
+	switch (rtlhal->oem_id) {
+	case RT_CID_819X_HP:
+		pcipriv->ledctl.led_opendrain = true;
+		break;
+	case RT_CID_819X_LENOVO:
+	case RT_CID_DEFAULT:
+	case RT_CID_TOSHIBA:
+	case RT_CID_CCX:
+	case RT_CID_819X_ACER:
+	case RT_CID_WHQL:
+	default:
+		break;
+	}
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		 "RT Customized ID: 0x%02X\n", rtlhal->oem_id);
+}
+
+void rtl8723be_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 = _rtl8723be_read_chip_version(hw);
+	if (get_rf_type(rtlphy) == RF_1T1R)
+		rtlpriv->dm.rfpath_rxenable[0] = true;
+	else
+		rtlpriv->dm.rfpath_rxenable[0] =
+		    rtlpriv->dm.rfpath_rxenable[1] = true;
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "VersionID = 0x%4x\n",
+		 rtlhal->version);
+	tmp_u1b = rtl_read_byte(rtlpriv, REG_9346CR);
+	if (tmp_u1b & BIT(4)) {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "Boot from EEPROM\n");
+		rtlefuse->epromtype = EEPROM_93C46;
+	} else {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "Boot from EFUSE\n");
+		rtlefuse->epromtype = EEPROM_BOOT_EFUSE;
+	}
+	if (tmp_u1b & BIT(5)) {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "Autoload OK\n");
+		rtlefuse->autoload_failflag = false;
+		_rtl8723be_read_adapter_info(hw, false);
+	} else {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "Autoload ERR!!\n");
+	}
+	_rtl8723be_hal_customized_behavior(hw);
+}
+
+static void rtl8723be_update_hal_rate_table(struct ieee80211_hw *hw,
+					    struct ieee80211_sta *sta)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u32 ratr_value;
+	u8 ratr_index = 0;
+	u8 nmode = mac->ht_enable;
+	u8 mimo_ps = IEEE80211_SMPS_OFF;
+	u16 shortgi_rate;
+	u32 tmp_ratr_value;
+	u8 curtxbw_40mhz = mac->bw_40;
+	u8 curshortgi_40mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40) ?
+			       1 : 0;
+	u8 curshortgi_20mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20) ?
+			       1 : 0;
+	enum wireless_mode wirelessmode = mac->mode;
+
+	if (rtlhal->current_bandtype == BAND_ON_5G)
+		ratr_value = sta->supp_rates[1] << 4;
+	else
+		ratr_value = sta->supp_rates[0];
+	if (mac->opmode == NL80211_IFTYPE_ADHOC)
+		ratr_value = 0xfff;
+	ratr_value |= (sta->ht_cap.mcs.rx_mask[1] << 20 |
+		       sta->ht_cap.mcs.rx_mask[0] << 12);
+	switch (wirelessmode) {
+	case WIRELESS_MODE_B:
+		if (ratr_value & 0x0000000c)
+			ratr_value &= 0x0000000d;
+		else
+			ratr_value &= 0x0000000f;
+		break;
+	case WIRELESS_MODE_G:
+		ratr_value &= 0x00000FF5;
+		break;
+	case WIRELESS_MODE_N_24G:
+	case WIRELESS_MODE_N_5G:
+		nmode = 1;
+		if (mimo_ps == IEEE80211_SMPS_STATIC) {
+			ratr_value &= 0x0007F005;
+		} else {
+			u32 ratr_mask;
+
+			if (get_rf_type(rtlphy) == RF_1T2R ||
+			    get_rf_type(rtlphy) == RF_1T1R)
+				ratr_mask = 0x000ff005;
+			else
+				ratr_mask = 0x0f0ff005;
+			ratr_value &= ratr_mask;
+		}
+		break;
+	default:
+		if (rtlphy->rf_type == RF_1T2R)
+			ratr_value &= 0x000ff0ff;
+		else
+			ratr_value &= 0x0f0ff0ff;
+		break;
+	}
+	if ((rtlpriv->btcoexist.bt_coexistence) &&
+	    (rtlpriv->btcoexist.bt_coexist_type == BT_CSR_BC4) &&
+	    (rtlpriv->btcoexist.bt_cur_state) &&
+	    (rtlpriv->btcoexist.bt_ant_isolation) &&
+	    ((rtlpriv->btcoexist.bt_service == BT_SCO) ||
+	     (rtlpriv->btcoexist.bt_service == BT_BUSY)))
+		ratr_value &= 0x0fffcfc0;
+	else
+		ratr_value &= 0x0FFFFFFF;
+
+	if (nmode && ((curtxbw_40mhz && curshortgi_40mhz) ||
+		      (!curtxbw_40mhz && curshortgi_20mhz))) {
+		ratr_value |= 0x10000000;
+		tmp_ratr_value = (ratr_value >> 12);
+
+		for (shortgi_rate = 15; shortgi_rate > 0; shortgi_rate--) {
+			if ((1 << shortgi_rate) & tmp_ratr_value)
+				break;
+		}
+		shortgi_rate = (shortgi_rate << 12) | (shortgi_rate << 8) |
+			       (shortgi_rate << 4) | (shortgi_rate);
+	}
+	rtl_write_dword(rtlpriv, REG_ARFR0 + ratr_index * 4, ratr_value);
+
+	RT_TRACE(rtlpriv, COMP_RATR, DBG_DMESG,
+		 "%x\n", rtl_read_dword(rtlpriv, REG_ARFR0));
+}
+
+static u8 _rtl8723be_mrate_idx_to_arfr_id(struct ieee80211_hw *hw,
+					  u8 rate_index)
+{
+	u8 ret = 0;
+
+	switch (rate_index) {
+	case RATR_INX_WIRELESS_NGB:
+		ret = 1;
+		break;
+	case RATR_INX_WIRELESS_N:
+	case RATR_INX_WIRELESS_NG:
+		ret = 5;
+		break;
+	case RATR_INX_WIRELESS_NB:
+		ret = 3;
+		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 rtl8723be_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 curtxbw_40mhz = (sta->ht_cap.cap &
+			    IEEE80211_HT_CAP_SUP_WIDTH_20_40) ? 1 : 0;
+	u8 curshortgi_40mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40) ?
+			       1 : 0;
+	u8 curshortgi_20mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20) ?
+			       1 : 0;
+	enum wireless_mode wirelessmode = 0;
+	bool shortgi = false;
+	u8 rate_mask[7];
+	u8 macid = 0;
+	u8 mimo_ps = IEEE80211_SMPS_OFF;
+
+	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+	wirelessmode = sta_entry->wireless_mode;
+	if (mac->opmode == NL80211_IFTYPE_STATION ||
+	    mac->opmode == NL80211_IFTYPE_MESH_POINT)
+		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_A:
+		ratr_index = RATR_INX_WIRELESS_A;
+		ratr_bitmap &= 0x00000ff0;
+		break;
+	case WIRELESS_MODE_N_24G:
+	case WIRELESS_MODE_N_5G:
+		ratr_index = RATR_INX_WIRELESS_NGB;
+
+		if (mimo_ps == IEEE80211_SMPS_STATIC  ||
+		    mimo_ps == IEEE80211_SMPS_DYNAMIC) {
+			if (rssi_level == 1)
+				ratr_bitmap &= 0x00070000;
+			else if (rssi_level == 2)
+				ratr_bitmap &= 0x0007f000;
+			else
+				ratr_bitmap &= 0x0007f005;
+		} else {
+			if (rtlphy->rf_type == RF_1T1R) {
+				if (curtxbw_40mhz) {
+					if (rssi_level == 1)
+						ratr_bitmap &= 0x000f0000;
+					else if (rssi_level == 2)
+						ratr_bitmap &= 0x000ff000;
+					else
+						ratr_bitmap &= 0x000ff015;
+				} else {
+					if (rssi_level == 1)
+						ratr_bitmap &= 0x000f0000;
+					else if (rssi_level == 2)
+						ratr_bitmap &= 0x000ff000;
+					else
+						ratr_bitmap &= 0x000ff005;
+				}
+			} else {
+				if (curtxbw_40mhz) {
+					if (rssi_level == 1)
+						ratr_bitmap &= 0x0f8f0000;
+					else if (rssi_level == 2)
+						ratr_bitmap &= 0x0f8ff000;
+					else
+						ratr_bitmap &= 0x0f8ff015;
+				} else {
+					if (rssi_level == 1)
+						ratr_bitmap &= 0x0f8f0000;
+					else if (rssi_level == 2)
+						ratr_bitmap &= 0x0f8ff000;
+					else
+						ratr_bitmap &= 0x0f8ff005;
+				}
+			}
+		}
+		if ((curtxbw_40mhz && curshortgi_40mhz) ||
+		    (!curtxbw_40mhz && curshortgi_20mhz)) {
+			if (macid == 0)
+				shortgi = true;
+			else if (macid == 1)
+				shortgi = false;
+		}
+		break;
+	default:
+		ratr_index = RATR_INX_WIRELESS_NGB;
+
+		if (rtlphy->rf_type == RF_1T2R)
+			ratr_bitmap &= 0x000ff0ff;
+		else
+			ratr_bitmap &= 0x0f0ff0ff;
+		break;
+	}
+	sta_entry->ratr_index = ratr_index;
+
+	RT_TRACE(rtlpriv, COMP_RATR, DBG_DMESG,
+		 "ratr_bitmap :%x\n", ratr_bitmap);
+	*(u32 *)&rate_mask = (ratr_bitmap & 0x0fffffff) | (ratr_index << 28);
+	rate_mask[0] = macid;
+	rate_mask[1] = _rtl8723be_mrate_idx_to_arfr_id(hw, ratr_index) |
+						       (shortgi ? 0x80 : 0x00);
+	rate_mask[2] = curtxbw_40mhz;
+	/* if (prox_priv->proxim_modeinfo->power_output > 0)
+	 *	rate_mask[2] |= BIT(6);
+	 */
+
+	rate_mask[3] = (u8)(ratr_bitmap & 0x000000ff);
+	rate_mask[4] = (u8)((ratr_bitmap & 0x0000ff00) >> 8);
+	rate_mask[5] = (u8)((ratr_bitmap & 0x00ff0000) >> 16);
+	rate_mask[6] = (u8)((ratr_bitmap & 0xff000000) >> 24);
+
+	RT_TRACE(rtlpriv, COMP_RATR, DBG_DMESG,
+		 "Rate_index:%x, ratr_val:%x, %x:%x:%x:%x:%x:%x:%x\n",
+		 ratr_index, ratr_bitmap,
+		 rate_mask[0], rate_mask[1],
+		 rate_mask[2], rate_mask[3],
+		 rate_mask[4], rate_mask[5],
+		 rate_mask[6]);
+	rtl8723be_fill_h2c_cmd(hw, H2C_8723BE_RA_MASK, 7, rate_mask);
+	_rtl8723be_set_bcn_ctrl_reg(hw, BIT(3), 0);
+}
+
+void rtl8723be_update_hal_rate_tbl(struct ieee80211_hw *hw,
+				   struct ieee80211_sta *sta,
+				   u8 rssi_level)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	if (rtlpriv->dm.useramask)
+		rtl8723be_update_hal_rate_mask(hw, sta, rssi_level);
+	else
+		rtl8723be_update_hal_rate_table(hw, sta);
+}
+
+void rtl8723be_update_channel_access_setting(struct ieee80211_hw *hw)
+{
+	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 rtl8723be_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 *valid)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	enum rf_pwrstate e_rfpowerstate_toset, cur_rfstate;
+	u8 u1tmp;
+	bool actuallyset = false;
+
+	if (rtlpriv->rtlhal.being_init_adapter)
+		return false;
+
+	if (ppsc->swrf_processing)
+		return false;
+
+	spin_lock(&rtlpriv->locks.rf_ps_lock);
+	if (ppsc->rfchange_inprogress) {
+		spin_unlock(&rtlpriv->locks.rf_ps_lock);
+		return false;
+	} else {
+		ppsc->rfchange_inprogress = true;
+		spin_unlock(&rtlpriv->locks.rf_ps_lock);
+	}
+	cur_rfstate = ppsc->rfpwr_state;
+
+	rtl_write_byte(rtlpriv, REG_GPIO_IO_SEL_2,
+		       rtl_read_byte(rtlpriv, REG_GPIO_IO_SEL_2) & ~(BIT(1)));
+
+	u1tmp = rtl_read_byte(rtlpriv, REG_GPIO_PIN_CTRL_2);
+
+	if (rtlphy->polarity_ctl)
+		e_rfpowerstate_toset = (u1tmp & BIT(1)) ? ERFOFF : ERFON;
+	else
+		e_rfpowerstate_toset = (u1tmp & BIT(1)) ? ERFON : ERFOFF;
+
+	if (ppsc->hwradiooff &&
+	    (e_rfpowerstate_toset == ERFON)) {
+		RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
+			 "GPIOChangeRF  - HW Radio ON, RF ON\n");
+
+		e_rfpowerstate_toset = ERFON;
+		ppsc->hwradiooff = false;
+		actuallyset = true;
+	} else if (!ppsc->hwradiooff &&
+		   (e_rfpowerstate_toset == ERFOFF)) {
+		RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
+			 "GPIOChangeRF  - HW Radio OFF, RF OFF\n");
+
+		e_rfpowerstate_toset = ERFOFF;
+		ppsc->hwradiooff = true;
+		actuallyset = true;
+	}
+	if (actuallyset) {
+		spin_lock(&rtlpriv->locks.rf_ps_lock);
+		ppsc->rfchange_inprogress = false;
+		spin_unlock(&rtlpriv->locks.rf_ps_lock);
+	} else {
+		if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_HALT_NIC)
+			RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
+
+		spin_lock(&rtlpriv->locks.rf_ps_lock);
+		ppsc->rfchange_inprogress = false;
+		spin_unlock(&rtlpriv->locks.rf_ps_lock);
+	}
+	*valid = 1;
+	return !ppsc->hwradiooff;
+}
+
+void rtl8723be_set_key(struct ieee80211_hw *hw, u32 key_index,
+		       u8 *p_macaddr, bool is_group, u8 enc_algo,
+		       bool is_wepkey, bool clear_all)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u8 *macaddr = p_macaddr;
+	u32 entry_id = 0;
+	bool is_pairwise = false;
+
+	static u8 cam_const_addr[4][6] = {
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x01},
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x02},
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x03}
+	};
+	static u8 cam_const_broad[] = {
+		0xff, 0xff, 0xff, 0xff, 0xff, 0xff
+	};
+
+	if (clear_all) {
+		u8 idx = 0;
+		u8 cam_offset = 0;
+		u8 clear_number = 5;
+
+		RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG, "clear_all\n");
+
+		for (idx = 0; idx < clear_number; idx++) {
+			rtl_cam_mark_invalid(hw, cam_offset + idx);
+			rtl_cam_empty_entry(hw, cam_offset + idx);
+
+			if (idx < 5) {
+				memset(rtlpriv->sec.key_buf[idx], 0,
+				       MAX_KEY_LEN);
+				rtlpriv->sec.key_len[idx] = 0;
+			}
+		}
+	} else {
+		switch (enc_algo) {
+		case WEP40_ENCRYPTION:
+			enc_algo = CAM_WEP40;
+			break;
+		case WEP104_ENCRYPTION:
+			enc_algo = CAM_WEP104;
+			break;
+		case TKIP_ENCRYPTION:
+			enc_algo = CAM_TKIP;
+			break;
+		case AESCCMP_ENCRYPTION:
+			enc_algo = CAM_AES;
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "switch case not process\n");
+			enc_algo = CAM_TKIP;
+			break;
+		}
+
+		if (is_wepkey || rtlpriv->sec.use_defaultkey) {
+			macaddr = cam_const_addr[key_index];
+			entry_id = key_index;
+		} else {
+			if (is_group) {
+				macaddr = cam_const_broad;
+				entry_id = key_index;
+			} else {
+				if (mac->opmode == NL80211_IFTYPE_AP) {
+					entry_id = rtl_cam_get_free_entry(hw,
+								p_macaddr);
+					if (entry_id >=  TOTAL_CAM_ENTRY) {
+						RT_TRACE(rtlpriv, 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(rtlpriv, COMP_SEC, DBG_DMESG,
+				 "delete one entry, entry_id is %d\n",
+				 entry_id);
+			if (mac->opmode == NL80211_IFTYPE_AP)
+				rtl_cam_del_entry(hw, p_macaddr);
+			rtl_cam_delete_one_entry(hw, p_macaddr, entry_id);
+		} else {
+			RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+				 "add one entry\n");
+			if (is_pairwise) {
+				RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+					 "set Pairwise key\n");
+
+				rtl_cam_add_one_entry(hw, macaddr, key_index,
+						      entry_id, enc_algo,
+						      CAM_CONFIG_NO_USEDK,
+						      rtlpriv->sec.key_buf[key_index]);
+			} else {
+				RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+					 "set group key\n");
+
+				if (mac->opmode == NL80211_IFTYPE_ADHOC) {
+					rtl_cam_add_one_entry(hw,
+						rtlefuse->dev_addr,
+						PAIRWISE_KEYIDX,
+						CAM_PAIRWISE_KEY_POSITION,
+						enc_algo,
+						CAM_CONFIG_NO_USEDK,
+						rtlpriv->sec.key_buf
+						[entry_id]);
+				}
+				rtl_cam_add_one_entry(hw, macaddr, key_index,
+						      entry_id, enc_algo,
+						      CAM_CONFIG_NO_USEDK,
+						      rtlpriv->sec.key_buf[entry_id]);
+			}
+		}
+	}
+}
+
+void rtl8723be_read_bt_coexist_info_from_hwpg(struct ieee80211_hw *hw,
+					      bool auto_load_fail, u8 *hwinfo)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 value;
+	u32 tmpu_32;
+
+	if (!auto_load_fail) {
+		tmpu_32 = rtl_read_dword(rtlpriv, REG_MULTI_FUNC_CTRL);
+		if (tmpu_32 & BIT(18))
+			rtlpriv->btcoexist.btc_info.btcoexist = 1;
+		else
+			rtlpriv->btcoexist.btc_info.btcoexist = 0;
+		value = hwinfo[RF_OPTION4];
+		rtlpriv->btcoexist.btc_info.bt_type = BT_RTL8723B;
+		rtlpriv->btcoexist.btc_info.ant_num = (value & 0x1);
+	} else {
+		rtlpriv->btcoexist.btc_info.btcoexist = 0;
+		rtlpriv->btcoexist.btc_info.bt_type = BT_RTL8723B;
+		rtlpriv->btcoexist.btc_info.ant_num = ANT_X2;
+	}
+}
+
+void rtl8723be_bt_reg_init(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	/* 0:Low, 1:High, 2:From Efuse. */
+	rtlpriv->btcoexist.reg_bt_iso = 2;
+	/* 0:Idle, 1:None-SCO, 2:SCO, 3:From Counter. */
+	rtlpriv->btcoexist.reg_bt_sco = 3;
+	/* 0:Disable BT control A-MPDU, 1:Enable BT control A-MPDU. */
+	rtlpriv->btcoexist.reg_bt_sco = 0;
+}
+
+void rtl8723be_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 rtl8723be_suspend(struct ieee80211_hw *hw)
+{
+}
+
+void rtl8723be_resume(struct ieee80211_hw *hw)
+{
+}
+
+/* Turn on AAP (RCR:bit 0) for promicuous mode. */
+void rtl8723be_allow_all_destaddr(struct ieee80211_hw *hw, bool allow_all_da,
+				  bool write_into_reg)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	if (allow_all_da) /* Set BIT0 */
+		rtlpci->receive_config |= RCR_AAP;
+	else /* Clear BIT0 */
+		rtlpci->receive_config &= ~RCR_AAP;
+
+	if (write_into_reg)
+		rtl_write_dword(rtlpriv, REG_RCR, rtlpci->receive_config);
+
+	RT_TRACE(rtlpriv, COMP_TURBO | COMP_INIT, DBG_LOUD,
+		 "receive_config = 0x%08X, write_into_reg =%d\n",
+		 rtlpci->receive_config, write_into_reg);
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/hw.h b/drivers/net/wireless/rtlwifi/rtl8723be/hw.h
new file mode 100644
index 000000000000..b7449a9b57e4
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/hw.h
@@ -0,0 +1,64 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL8723BE_HW_H__
+#define __RTL8723BE_HW_H__
+
+void rtl8723be_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val);
+void rtl8723be_read_eeprom_info(struct ieee80211_hw *hw);
+
+void rtl8723be_interrupt_recognized(struct ieee80211_hw *hw,
+				    u32 *p_inta, u32 *p_intb);
+int rtl8723be_hw_init(struct ieee80211_hw *hw);
+void rtl8723be_card_disable(struct ieee80211_hw *hw);
+void rtl8723be_enable_interrupt(struct ieee80211_hw *hw);
+void rtl8723be_disable_interrupt(struct ieee80211_hw *hw);
+int rtl8723be_set_network_type(struct ieee80211_hw *hw,
+			       enum nl80211_iftype type);
+void rtl8723be_set_check_bssid(struct ieee80211_hw *hw, bool check_bssid);
+void rtl8723be_set_qos(struct ieee80211_hw *hw, int aci);
+void rtl8723be_set_beacon_related_registers(struct ieee80211_hw *hw);
+void rtl8723be_set_beacon_interval(struct ieee80211_hw *hw);
+void rtl8723be_update_interrupt_mask(struct ieee80211_hw *hw,
+				     u32 add_msr, u32 rm_msr);
+void rtl8723be_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val);
+void rtl8723be_update_hal_rate_tbl(struct ieee80211_hw *hw,
+				   struct ieee80211_sta *sta,
+				   u8 rssi_level);
+void rtl8723be_update_channel_access_setting(struct ieee80211_hw *hw);
+bool rtl8723be_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 *valid);
+void rtl8723be_enable_hw_security_config(struct ieee80211_hw *hw);
+void rtl8723be_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 rtl8723be_read_bt_coexist_info_from_hwpg(struct ieee80211_hw *hw,
+					      bool autoload_fail, u8 *hwinfo);
+void rtl8723be_bt_reg_init(struct ieee80211_hw *hw);
+void rtl8723be_bt_hw_init(struct ieee80211_hw *hw);
+void rtl8723be_suspend(struct ieee80211_hw *hw);
+void rtl8723be_resume(struct ieee80211_hw *hw);
+void rtl8723be_allow_all_destaddr(struct ieee80211_hw *hw, bool allow_all_da,
+				  bool write_into_reg);
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/led.c b/drivers/net/wireless/rtlwifi/rtl8723be/led.c
new file mode 100644
index 000000000000..cb931a38dc48
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/led.c
@@ -0,0 +1,153 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../pci.h"
+#include "reg.h"
+#include "led.h"
+
+static void _rtl8723be_init_led(struct ieee80211_hw *hw,  struct rtl_led *pled,
+				enum rtl_led_pin ledpin)
+{
+	pled->hw = hw;
+	pled->ledpin = ledpin;
+	pled->ledon = false;
+}
+
+void rtl8723be_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled)
+{
+	u8 ledcfg;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	RT_TRACE(rtlpriv, COMP_LED, DBG_LOUD,
+		 "LedAddr:%X ledpin =%d\n", REG_LEDCFG2, pled->ledpin);
+
+	switch (pled->ledpin) {
+	case LED_PIN_GPIO0:
+		break;
+	case LED_PIN_LED0:
+		ledcfg = rtl_read_byte(rtlpriv, REG_LEDCFG2);
+		ledcfg &= ~BIT(6);
+		rtl_write_byte(rtlpriv, REG_LEDCFG2, (ledcfg & 0xf0) | BIT(5));
+		break;
+	case LED_PIN_LED1:
+		ledcfg = rtl_read_byte(rtlpriv, REG_LEDCFG1);
+		rtl_write_byte(rtlpriv, REG_LEDCFG1, ledcfg & 0x10);
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "switch case not process\n");
+		break;
+	}
+	pled->ledon = true;
+}
+
+void rtl8723be_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	u8 ledcfg;
+
+	RT_TRACE(rtlpriv, COMP_LED, DBG_LOUD,
+		 "LedAddr:%X ledpin =%d\n", REG_LEDCFG2, pled->ledpin);
+
+	ledcfg = rtl_read_byte(rtlpriv, REG_LEDCFG2);
+
+	switch (pled->ledpin) {
+	case LED_PIN_GPIO0:
+		break;
+	case LED_PIN_LED0:
+		ledcfg &= 0xf0;
+		if (pcipriv->ledctl.led_opendrain) {
+			ledcfg &= 0x90; /* Set to software control. */
+			rtl_write_byte(rtlpriv, REG_LEDCFG2, (ledcfg|BIT(3)));
+			ledcfg = rtl_read_byte(rtlpriv, REG_MAC_PINMUX_CFG);
+			ledcfg &= 0xFE;
+			rtl_write_byte(rtlpriv, REG_MAC_PINMUX_CFG, ledcfg);
+		} else {
+			ledcfg &= ~BIT(6);
+			rtl_write_byte(rtlpriv, REG_LEDCFG2,
+				       (ledcfg | BIT(3) | BIT(5)));
+		}
+		break;
+	case LED_PIN_LED1:
+		ledcfg = rtl_read_byte(rtlpriv, REG_LEDCFG1);
+		ledcfg &= 0x10; /* Set to software control. */
+		rtl_write_byte(rtlpriv, REG_LEDCFG1, ledcfg|BIT(3));
+
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "switch case not processed\n");
+		break;
+	}
+	pled->ledon = false;
+}
+
+void rtl8723be_init_sw_leds(struct ieee80211_hw *hw)
+{
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	_rtl8723be_init_led(hw, &(pcipriv->ledctl.sw_led0), LED_PIN_LED0);
+	_rtl8723be_init_led(hw, &(pcipriv->ledctl.sw_led1), LED_PIN_LED1);
+}
+
+static void _rtl8723be_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:
+		rtl8723be_sw_led_on(hw, pled0);
+		break;
+	case LED_CTL_POWER_OFF:
+		rtl8723be_sw_led_off(hw, pled0);
+		break;
+	default:
+		break;
+	}
+}
+
+void rtl8723be_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(rtlpriv, COMP_LED, DBG_LOUD, "ledaction %d,\n", ledaction);
+	_rtl8723be_sw_led_control(hw, ledaction);
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/led.h b/drivers/net/wireless/rtlwifi/rtl8723be/led.h
new file mode 100644
index 000000000000..c57de379ee8d
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/led.h
@@ -0,0 +1,35 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL8723BE_LED_H__
+#define __RTL8723BE_LED_H__
+
+void rtl8723be_init_sw_leds(struct ieee80211_hw *hw);
+void rtl8723be_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled);
+void rtl8723be_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled);
+void rtl8723be_led_control(struct ieee80211_hw *hw,
+			   enum led_ctl_mode ledaction);
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/phy.c b/drivers/net/wireless/rtlwifi/rtl8723be/phy.c
new file mode 100644
index 000000000000..1575ef9ece9f
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/phy.c
@@ -0,0 +1,2156 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../pci.h"
+#include "../ps.h"
+#include "../core.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "../rtl8723com/phy_common.h"
+#include "rf.h"
+#include "dm.h"
+#include "table.h"
+#include "trx.h"
+
+static bool _rtl8723be_phy_bb8723b_config_parafile(struct ieee80211_hw *hw);
+static bool _rtl8723be_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
+						       u8 configtype);
+static bool rtl8723be_phy_sw_chn_step_by_step(struct ieee80211_hw *hw,
+					      u8 channel, u8 *stage,
+					      u8 *step, u32 *delay);
+static bool _rtl8723be_check_condition(struct ieee80211_hw *hw,
+				       const u32  condition)
+{
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u32 _board = rtlefuse->board_type; /*need efuse define*/
+	u32 _interface = rtlhal->interface;
+	u32 _platform = 0x08;/*SupportPlatform */
+	u32 cond = condition;
+
+	if (condition == 0xCDCDCDCD)
+		return true;
+
+	cond = condition & 0xFF;
+	if ((_board & cond) == 0 && cond != 0x1F)
+		return false;
+
+	cond = condition & 0xFF00;
+	cond = cond >> 8;
+	if ((_interface & cond) == 0 && cond != 0x07)
+		return false;
+
+	cond = condition & 0xFF0000;
+	cond = cond >> 16;
+	if ((_platform & cond) == 0 && cond != 0x0F)
+		return false;
+	return true;
+}
+
+static bool _rtl8723be_phy_config_mac_with_headerfile(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i;
+	u32 arraylength;
+	u32 *ptrarray;
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "Read rtl8723beMACPHY_Array\n");
+	arraylength = RTL8723BEMAC_1T_ARRAYLEN;
+	ptrarray = RTL8723BEMAC_1T_ARRAY;
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "Img:RTL8723bEMAC_1T_ARRAY LEN %d\n", arraylength);
+	for (i = 0; i < arraylength; i = i + 2)
+		rtl_write_byte(rtlpriv, ptrarray[i], (u8) ptrarray[i + 1]);
+	return true;
+}
+
+static bool _rtl8723be_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
+						     u8 configtype)
+{
+	#define READ_NEXT_PAIR(v1, v2, i) \
+		do { \
+			i += 2; \
+			v1 = array_table[i];\
+			v2 = array_table[i+1]; \
+		} while (0)
+
+	int i;
+	u32 *array_table;
+	u16 arraylen;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 v1 = 0, v2 = 0;
+
+	if (configtype == BASEBAND_CONFIG_PHY_REG) {
+		arraylen = RTL8723BEPHY_REG_1TARRAYLEN;
+		array_table = RTL8723BEPHY_REG_1TARRAY;
+
+		for (i = 0; i < arraylen; i = i + 2) {
+			v1 = array_table[i];
+			v2 = array_table[i+1];
+			if (v1 < 0xcdcdcdcd) {
+				rtl_bb_delay(hw, v1, v2);
+			} else {/*This line is the start line of branch.*/
+				if (!_rtl8723be_check_condition(hw, array_table[i])) {
+					/*Discard the following (offset, data) pairs*/
+					READ_NEXT_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD &&
+					       i < arraylen - 2) {
+						READ_NEXT_PAIR(v1, v2, i);
+					}
+					i -= 2; /* prevent from for-loop += 2*/
+				/* Configure matched pairs and
+				 * skip to end of if-else.
+				 */
+				} else {
+					READ_NEXT_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD &&
+					       i < arraylen - 2) {
+						rtl_bb_delay(hw,
+								    v1, v2);
+						READ_NEXT_PAIR(v1, v2, i);
+					}
+
+					while (v2 != 0xDEAD && i < arraylen - 2)
+						READ_NEXT_PAIR(v1, v2, i);
+				}
+			}
+		}
+	} else if (configtype == BASEBAND_CONFIG_AGC_TAB) {
+		arraylen = RTL8723BEAGCTAB_1TARRAYLEN;
+		array_table = RTL8723BEAGCTAB_1TARRAY;
+
+		for (i = 0; i < arraylen; i = i + 2) {
+			v1 = array_table[i];
+			v2 = array_table[i+1];
+			if (v1 < 0xCDCDCDCD) {
+				rtl_set_bbreg(hw, array_table[i],
+					      MASKDWORD,
+					      array_table[i + 1]);
+				udelay(1);
+				continue;
+			} else {/*This line is the start line of branch.*/
+				if (!_rtl8723be_check_condition(hw, array_table[i])) {
+					/* Discard the following
+					 * (offset, data) pairs
+					 */
+					READ_NEXT_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD &&
+					       i < arraylen - 2) {
+						READ_NEXT_PAIR(v1, v2, i);
+					}
+					i -= 2; /* prevent from for-loop += 2*/
+				/*Configure matched pairs and
+				 *skip to end of if-else.
+				 */
+				} else {
+					READ_NEXT_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD &&
+					       i < arraylen - 2) {
+						rtl_set_bbreg(hw, array_table[i],
+							      MASKDWORD,
+							      array_table[i + 1]);
+						udelay(1);
+						READ_NEXT_PAIR(v1, v2, i);
+					}
+
+					while (v2 != 0xDEAD && i < arraylen - 2)
+						READ_NEXT_PAIR(v1, v2, i);
+				}
+			}
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+				 "The agctab_array_table[0] is "
+				  "%x Rtl818EEPHY_REGArray[1] is %x\n",
+				  array_table[i], array_table[i + 1]);
+		}
+	}
+	return true;
+}
+
+static u8 _rtl8723be_get_rate_section_index(u32 regaddr)
+{
+	u8 index = 0;
+
+	switch (regaddr) {
+	case RTXAGC_A_RATE18_06:
+	case RTXAGC_B_RATE18_06:
+		index = 0;
+		break;
+	case RTXAGC_A_RATE54_24:
+	case RTXAGC_B_RATE54_24:
+		index = 1;
+		break;
+	case RTXAGC_A_CCK1_MCS32:
+	case RTXAGC_B_CCK1_55_MCS32:
+		index = 2;
+		break;
+	case RTXAGC_B_CCK11_A_CCK2_11:
+		index = 3;
+		break;
+	case RTXAGC_A_MCS03_MCS00:
+	case RTXAGC_B_MCS03_MCS00:
+		index = 4;
+		break;
+	case RTXAGC_A_MCS07_MCS04:
+	case RTXAGC_B_MCS07_MCS04:
+		index = 5;
+		break;
+	case RTXAGC_A_MCS11_MCS08:
+	case RTXAGC_B_MCS11_MCS08:
+		index = 6;
+		break;
+	case RTXAGC_A_MCS15_MCS12:
+	case RTXAGC_B_MCS15_MCS12:
+		index = 7;
+		break;
+	default:
+		regaddr &= 0xFFF;
+		if (regaddr >= 0xC20 && regaddr <= 0xC4C)
+			index = (u8) ((regaddr - 0xC20) / 4);
+		else if (regaddr >= 0xE20 && regaddr <= 0xE4C)
+			index = (u8) ((regaddr - 0xE20) / 4);
+		break;
+	};
+	return index;
+}
+
+u32 rtl8723be_phy_query_rf_reg(struct ieee80211_hw *hw, enum radio_path rfpath,
+			       u32 regaddr, u32 bitmask)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 original_value, readback_value, bitshift;
+	unsigned long flags;
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "regaddr(%#x), rfpath(%#x), bitmask(%#x)\n",
+		  regaddr, rfpath, bitmask);
+
+	spin_lock_irqsave(&rtlpriv->locks.rf_lock, flags);
+
+	original_value = rtl8723_phy_rf_serial_read(hw, rfpath, regaddr);
+	bitshift = rtl8723_phy_calculate_bit_shift(bitmask);
+	readback_value = (original_value & bitmask) >> bitshift;
+
+	spin_unlock_irqrestore(&rtlpriv->locks.rf_lock, flags);
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "regaddr(%#x), rfpath(%#x), "
+		  "bitmask(%#x), original_value(%#x)\n",
+		  regaddr, rfpath, bitmask, original_value);
+
+	return readback_value;
+}
+
+void rtl8723be_phy_set_rf_reg(struct ieee80211_hw *hw, enum radio_path path,
+			      u32 regaddr, u32 bitmask, u32 data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 original_value, bitshift;
+	unsigned long flags;
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "regaddr(%#x), bitmask(%#x), data(%#x), rfpath(%#x)\n",
+		  regaddr, bitmask, data, path);
+
+	spin_lock_irqsave(&rtlpriv->locks.rf_lock, flags);
+
+	if (bitmask != RFREG_OFFSET_MASK) {
+			original_value = rtl8723_phy_rf_serial_read(hw, path,
+								    regaddr);
+			bitshift = rtl8723_phy_calculate_bit_shift(bitmask);
+			data = ((original_value & (~bitmask)) |
+				(data << bitshift));
+		}
+
+	rtl8723_phy_rf_serial_write(hw, path, regaddr, data);
+
+	spin_unlock_irqrestore(&rtlpriv->locks.rf_lock, flags);
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "regaddr(%#x), bitmask(%#x), data(%#x), rfpath(%#x)\n",
+		  regaddr, bitmask, data, path);
+}
+
+bool rtl8723be_phy_mac_config(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	bool rtstatus = _rtl8723be_phy_config_mac_with_headerfile(hw);
+
+	rtl_write_byte(rtlpriv, 0x04CA, 0x0B);
+	return rtstatus;
+}
+
+bool rtl8723be_phy_bb_config(struct ieee80211_hw *hw)
+{
+	bool rtstatus = true;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u16 regval;
+	u8 reg_hwparafile = 1;
+	u32 tmp;
+	u8 crystalcap = rtlpriv->efuse.crystalcap;
+	rtl8723_phy_init_bb_rf_reg_def(hw);
+	regval = rtl_read_word(rtlpriv, REG_SYS_FUNC_EN);
+	rtl_write_word(rtlpriv, REG_SYS_FUNC_EN,
+		       regval | BIT(13) | BIT(0) | BIT(1));
+
+	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);
+	tmp = rtl_read_dword(rtlpriv, 0x4c);
+	rtl_write_dword(rtlpriv, 0x4c, tmp | BIT(23));
+
+	rtl_write_byte(rtlpriv, REG_AFE_XTAL_CTRL + 1, 0x80);
+
+	if (reg_hwparafile == 1)
+		rtstatus = _rtl8723be_phy_bb8723b_config_parafile(hw);
+
+	crystalcap = crystalcap & 0x3F;
+	rtl_set_bbreg(hw, REG_MAC_PHY_CTRL, 0xFFF000,
+		      (crystalcap | crystalcap << 6));
+
+	return rtstatus;
+}
+
+bool rtl8723be_phy_rf_config(struct ieee80211_hw *hw)
+{
+	return rtl8723be_phy_rf6052_config(hw);
+}
+
+static void _rtl8723be_config_rf_reg(struct ieee80211_hw *hw, u32 addr,
+				     u32 data, enum radio_path rfpath,
+				     u32 regaddr)
+{
+	if (addr == 0xfe || addr == 0xffe) {
+		mdelay(50);
+	} else {
+		rtl_set_rfreg(hw, rfpath, regaddr, RFREG_OFFSET_MASK, data);
+		udelay(1);
+	}
+}
+
+static void _rtl8723be_config_rf_radio_a(struct ieee80211_hw *hw,
+					 u32 addr, u32 data)
+{
+	u32 content = 0x1000; /*RF Content: radio_a_txt*/
+	u32 maskforphyset = (u32)(content & 0xE000);
+
+	_rtl8723be_config_rf_reg(hw, addr, data, RF90_PATH_A,
+				 addr | maskforphyset);
+}
+
+static void _rtl8723be_phy_init_tx_power_by_rate(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	u8 band, path, txnum, section;
+
+	for (band = BAND_ON_2_4G; band <= BAND_ON_5G; ++band)
+		for (path = 0; path < TX_PWR_BY_RATE_NUM_RF; ++path)
+			for (txnum = 0; txnum < TX_PWR_BY_RATE_NUM_RF; ++txnum)
+				for (section = 0;
+				     section < TX_PWR_BY_RATE_NUM_SECTION;
+				     ++section)
+					rtlphy->tx_power_by_rate_offset[band]
+						[path][txnum][section] = 0;
+}
+
+static void phy_set_txpwr_by_rate_base(struct ieee80211_hw *hw, u8 band,
+				       u8 path, u8 rate_section,
+				       u8 txnum, u8 value)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	if (path > RF90_PATH_D) {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Invalid Rf Path %d in phy_SetTxPowerByRatBase()\n",
+			  path);
+		return;
+	}
+
+	if (band == BAND_ON_2_4G) {
+		switch (rate_section) {
+		case CCK:
+			rtlphy->txpwr_by_rate_base_24g[path][txnum][0] = value;
+			break;
+		case OFDM:
+			rtlphy->txpwr_by_rate_base_24g[path][txnum][1] = value;
+			break;
+		case HT_MCS0_MCS7:
+			rtlphy->txpwr_by_rate_base_24g[path][txnum][2] = value;
+			break;
+		case HT_MCS8_MCS15:
+			rtlphy->txpwr_by_rate_base_24g[path][txnum][3] = value;
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				 "Invalid RateSection %d in Band 2.4G, Rf Path"
+				  " %d, %dTx in PHY_SetTxPowerByRateBase()\n",
+				  rate_section, path, txnum);
+			break;
+		};
+	} else {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Invalid Band %d in PHY_SetTxPowerByRateBase()\n",
+			  band);
+	}
+}
+
+static u8 phy_get_txpwr_by_rate_base(struct ieee80211_hw *hw, u8 band, u8 path,
+				     u8 txnum, u8 rate_section)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 value = 0;
+	if (path > RF90_PATH_D) {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Invalid Rf Path %d in PHY_GetTxPowerByRateBase()\n",
+			  path);
+		return 0;
+	}
+
+	if (band == BAND_ON_2_4G) {
+		switch (rate_section) {
+		case CCK:
+			value = rtlphy->txpwr_by_rate_base_24g[path][txnum][0];
+			break;
+		case OFDM:
+			value = rtlphy->txpwr_by_rate_base_24g[path][txnum][1];
+			break;
+		case HT_MCS0_MCS7:
+			value = rtlphy->txpwr_by_rate_base_24g[path][txnum][2];
+			break;
+		case HT_MCS8_MCS15:
+			value = rtlphy->txpwr_by_rate_base_24g[path][txnum][3];
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				 "Invalid RateSection %d in Band 2.4G, Rf Path"
+				  " %d, %dTx in PHY_GetTxPowerByRateBase()\n",
+				  rate_section, path, txnum);
+			break;
+		};
+	} else {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Invalid Band %d in PHY_GetTxPowerByRateBase()\n",
+			  band);
+	}
+
+	return value;
+}
+
+static void _rtl8723be_phy_store_txpower_by_rate_base(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u16 raw_value = 0;
+	u8 base = 0, path = 0;
+
+	for (path = RF90_PATH_A; path <= RF90_PATH_B; ++path) {
+		if (path == RF90_PATH_A) {
+			raw_value = (u16) (rtlphy->tx_power_by_rate_offset
+				[BAND_ON_2_4G][path][RF_1TX][3] >> 24) & 0xFF;
+			base = (raw_value >> 4) * 10 + (raw_value & 0xF);
+			phy_set_txpwr_by_rate_base(hw, BAND_ON_2_4G, path, CCK,
+						   RF_1TX, base);
+		} else if (path == RF90_PATH_B) {
+			raw_value = (u16) (rtlphy->tx_power_by_rate_offset
+				[BAND_ON_2_4G][path][RF_1TX][3] >> 0) & 0xFF;
+			base = (raw_value >> 4) * 10 + (raw_value & 0xF);
+			phy_set_txpwr_by_rate_base(hw, BAND_ON_2_4G, path,
+						   CCK, RF_1TX, base);
+		}
+		raw_value = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G]
+					  [path][RF_1TX][1] >> 24) & 0xFF;
+		base = (raw_value >> 4) * 10 + (raw_value & 0xF);
+		phy_set_txpwr_by_rate_base(hw, BAND_ON_2_4G, path, OFDM, RF_1TX,
+					   base);
+
+		raw_value = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G]
+					  [path][RF_1TX][5] >> 24) & 0xFF;
+		base = (raw_value >> 4) * 10 + (raw_value & 0xF);
+		phy_set_txpwr_by_rate_base(hw, BAND_ON_2_4G, path, HT_MCS0_MCS7,
+					   RF_1TX, base);
+
+		raw_value = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G]
+					  [path][RF_2TX][7] >> 24) & 0xFF;
+		base = (raw_value >> 4) * 10 + (raw_value & 0xF);
+		phy_set_txpwr_by_rate_base(hw, BAND_ON_2_4G, path,
+					   HT_MCS8_MCS15, RF_2TX, base);
+	}
+}
+
+static void phy_conv_dbm_to_rel(u32 *data, u8 start, u8 end, u8 base_val)
+{
+	char i = 0;
+	u8 temp_value = 0;
+	u32 temp_data = 0;
+
+	for (i = 3; i >= 0; --i) {
+		if (i >= start && i <= end) {
+			/* Get the exact value */
+			temp_value = (u8) (*data >> (i * 8)) & 0xF;
+			temp_value += ((u8) ((*data >> (i*8 + 4)) & 0xF)) * 10;
+
+			/* Change the value to a relative value */
+			temp_value = (temp_value > base_val) ?
+				     temp_value - base_val :
+				     base_val - temp_value;
+		} else {
+			temp_value = (u8) (*data >> (i * 8)) & 0xFF;
+		}
+		temp_data <<= 8;
+		temp_data |= temp_value;
+	}
+	*data = temp_data;
+}
+
+static void conv_dbm_to_rel(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 base = 0, rfpath = RF90_PATH_A;
+
+	base = phy_get_txpwr_by_rate_base(hw, BAND_ON_2_4G, rfpath,
+					  RF_1TX, CCK);
+	phy_conv_dbm_to_rel(&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G]
+			    [rfpath][RF_1TX][2]), 1, 1, base);
+	phy_conv_dbm_to_rel(&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G]
+			    [rfpath][RF_1TX][3]), 1, 3, base);
+
+	base = phy_get_txpwr_by_rate_base(hw, BAND_ON_2_4G, rfpath,
+					  RF_1TX, OFDM);
+	phy_conv_dbm_to_rel(&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G]
+			    [rfpath][RF_1TX][0]), 0, 3, base);
+	phy_conv_dbm_to_rel(&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G]
+			    [rfpath][RF_1TX][1]), 0, 3, base);
+
+	base = phy_get_txpwr_by_rate_base(hw, BAND_ON_2_4G, rfpath,
+					  RF_1TX, HT_MCS0_MCS7);
+	phy_conv_dbm_to_rel(&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G]
+			    [rfpath][RF_1TX][4]), 0, 3, base);
+	phy_conv_dbm_to_rel(&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G]
+			    [rfpath][RF_1TX][5]), 0, 3, base);
+
+	base = phy_get_txpwr_by_rate_base(hw, BAND_ON_2_4G, rfpath,
+					  RF_2TX, HT_MCS8_MCS15);
+	phy_conv_dbm_to_rel(&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G]
+			    [rfpath][RF_2TX][6]), 0, 3, base);
+
+	phy_conv_dbm_to_rel(&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G]
+			    [rfpath][RF_2TX][7]), 0, 3, base);
+
+	RT_TRACE(rtlpriv, COMP_POWER, DBG_TRACE,
+		 "<=== conv_dbm_to_rel()\n");
+}
+
+static void _rtl8723be_phy_txpower_by_rate_configuration(
+							struct ieee80211_hw *hw)
+{
+	_rtl8723be_phy_store_txpower_by_rate_base(hw);
+	conv_dbm_to_rel(hw);
+}
+
+static bool _rtl8723be_phy_bb8723b_config_parafile(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	bool rtstatus;
+
+	rtstatus = _rtl8723be_phy_config_bb_with_headerfile(hw,
+						BASEBAND_CONFIG_PHY_REG);
+	if (!rtstatus) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "Write BB Reg Fail!!");
+		return false;
+	}
+	_rtl8723be_phy_init_tx_power_by_rate(hw);
+	if (!rtlefuse->autoload_failflag) {
+		rtlphy->pwrgroup_cnt = 0;
+		rtstatus = _rtl8723be_phy_config_bb_with_pgheaderfile(hw,
+						BASEBAND_CONFIG_PHY_REG);
+	}
+	_rtl8723be_phy_txpower_by_rate_configuration(hw);
+	if (!rtstatus) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "BB_PG Reg Fail!!");
+		return false;
+	}
+	rtstatus = _rtl8723be_phy_config_bb_with_headerfile(hw,
+						BASEBAND_CONFIG_AGC_TAB);
+	if (!rtstatus) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "AGC Table Fail\n");
+		return false;
+	}
+	rtlphy->cck_high_power = (bool) (rtl_get_bbreg(hw,
+						       RFPGA0_XA_HSSIPARAMETER2,
+						       0x200));
+	return true;
+}
+
+static void _rtl8723be_store_tx_power_by_rate(struct ieee80211_hw *hw,
+					      u32 band, u32 rfpath,
+					      u32 txnum, u32 regaddr,
+					      u32 bitmask, u32 data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 rate_section = _rtl8723be_get_rate_section_index(regaddr);
+
+	if (band != BAND_ON_2_4G && band != BAND_ON_5G) {
+		RT_TRACE(rtlpriv, COMP_POWER, PHY_TXPWR,
+			 "Invalid Band %d\n", band);
+		return;
+	}
+
+	if (rfpath > TX_PWR_BY_RATE_NUM_RF) {
+		RT_TRACE(rtlpriv, COMP_POWER, PHY_TXPWR,
+			 "Invalid RfPath %d\n", rfpath);
+		return;
+	}
+	if (txnum > TX_PWR_BY_RATE_NUM_RF) {
+		RT_TRACE(rtlpriv, COMP_POWER, PHY_TXPWR,
+			 "Invalid TxNum %d\n", txnum);
+		return;
+	}
+	rtlphy->tx_power_by_rate_offset[band][rfpath][txnum][rate_section] =
+									data;
+}
+
+static bool _rtl8723be_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
+						       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 = RTL8723BEPHY_REG_ARRAY_PGLEN;
+	phy_regarray_table_pg = RTL8723BEPHY_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) {
+				if (phy_regarray_table_pg[i] == 0xfe ||
+				    phy_regarray_table_pg[i] == 0xffe)
+					mdelay(50);
+				else
+					_rtl8723be_store_tx_power_by_rate(hw,
+							v1, v2, v3, v4, v5, v6);
+				continue;
+			} else {
+				/*don't need the hw_body*/
+				if (!_rtl8723be_check_condition(hw,
+						phy_regarray_table_pg[i])) {
+					i += 2; /* skip the pair of expression*/
+					v1 = phy_regarray_table_pg[i];
+					v2 = phy_regarray_table_pg[i+1];
+					v3 = phy_regarray_table_pg[i+2];
+					while (v2 != 0xDEAD) {
+						i += 3;
+						v1 = phy_regarray_table_pg[i];
+						v2 = phy_regarray_table_pg[i+1];
+						v3 = phy_regarray_table_pg[i+2];
+					}
+				}
+			}
+		}
+	} else {
+		RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
+			 "configtype != BaseBand_Config_PHY_REG\n");
+	}
+	return true;
+}
+
+bool rtl8723be_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
+					     enum radio_path rfpath)
+{
+	#define READ_NEXT_RF_PAIR(v1, v2, i) \
+		do { \
+			i += 2; \
+			v1 = radioa_array_table[i]; \
+			v2 = radioa_array_table[i+1]; \
+		} while (0)
+
+	int i;
+	bool rtstatus = true;
+	u32 *radioa_array_table;
+	u16 radioa_arraylen;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u32 v1 = 0, v2 = 0;
+
+	radioa_arraylen = RTL8723BE_RADIOA_1TARRAYLEN;
+	radioa_array_table = RTL8723BE_RADIOA_1TARRAY;
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "Radio_A:RTL8723BE_RADIOA_1TARRAY %d\n", radioa_arraylen);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "Radio No %x\n", rfpath);
+	rtstatus = true;
+	switch (rfpath) {
+	case RF90_PATH_A:
+		for (i = 0; i < radioa_arraylen; i = i + 2) {
+			v1 = radioa_array_table[i];
+			v2 = radioa_array_table[i+1];
+			if (v1 < 0xcdcdcdcd) {
+				_rtl8723be_config_rf_radio_a(hw, v1, v2);
+			} else { /*This line is the start line of branch.*/
+				if (!_rtl8723be_check_condition(hw,
+						radioa_array_table[i])) {
+					/* Discard the following
+					 * (offset, data) pairs
+					 */
+					READ_NEXT_RF_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD &&
+					       i < radioa_arraylen - 2)
+						READ_NEXT_RF_PAIR(v1, v2, i);
+					i -= 2; /* prevent from for-loop += 2*/
+				} else {
+					/* Configure matched pairs
+					 * and skip to end of if-else.
+					 */
+					READ_NEXT_RF_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD &&
+					       i < radioa_arraylen - 2) {
+						_rtl8723be_config_rf_radio_a(hw,
+									v1, v2);
+						READ_NEXT_RF_PAIR(v1, v2, i);
+					}
+
+					while (v2 != 0xDEAD &&
+					       i < radioa_arraylen - 2) {
+						READ_NEXT_RF_PAIR(v1, v2, i);
+					}
+				}
+			}
+		}
+
+		if (rtlhal->oem_id == RT_CID_819X_HP)
+			_rtl8723be_config_rf_radio_a(hw, 0x52, 0x7E4BD);
+
+		break;
+	case RF90_PATH_B:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "switch case not process\n");
+		break;
+	case RF90_PATH_C:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "switch case not process\n");
+		break;
+	case RF90_PATH_D:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "switch case not process\n");
+		break;
+	}
+	return true;
+}
+
+void rtl8723be_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	rtlphy->default_initialgain[0] =
+	    (u8) rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, MASKBYTE0);
+	rtlphy->default_initialgain[1] =
+	    (u8) rtl_get_bbreg(hw, ROFDM0_XBAGCCORE1, MASKBYTE0);
+	rtlphy->default_initialgain[2] =
+	    (u8) rtl_get_bbreg(hw, ROFDM0_XCAGCCORE1, MASKBYTE0);
+	rtlphy->default_initialgain[3] =
+	    (u8) rtl_get_bbreg(hw, ROFDM0_XDAGCCORE1, MASKBYTE0);
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+		 "Default initial gain (c50 = 0x%x, "
+		  "c58 = 0x%x, c60 = 0x%x, c68 = 0x%x\n",
+		  rtlphy->default_initialgain[0],
+		  rtlphy->default_initialgain[1],
+		  rtlphy->default_initialgain[2],
+		  rtlphy->default_initialgain[3]);
+
+	rtlphy->framesync = (u8) rtl_get_bbreg(hw, ROFDM0_RXDETECTOR3,
+					       MASKBYTE0);
+	rtlphy->framesync_c34 = rtl_get_bbreg(hw, ROFDM0_RXDETECTOR2,
+					      MASKDWORD);
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+		 "Default framesync (0x%x) = 0x%x\n",
+		  ROFDM0_RXDETECTOR3, rtlphy->framesync);
+}
+
+void rtl8723be_phy_get_txpower_level(struct ieee80211_hw *hw, long *powerlevel)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 txpwr_level;
+	long txpwr_dbm;
+
+	txpwr_level = rtlphy->cur_cck_txpwridx;
+	txpwr_dbm = rtl8723_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_B,
+						 txpwr_level);
+	txpwr_level = rtlphy->cur_ofdm24g_txpwridx;
+	if (rtl8723_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_G, txpwr_level) >
+	    txpwr_dbm)
+		txpwr_dbm =
+		    rtl8723_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_G,
+						 txpwr_level);
+	txpwr_level = rtlphy->cur_ofdm24g_txpwridx;
+	if (rtl8723_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_N_24G,
+					 txpwr_level) > txpwr_dbm)
+		txpwr_dbm =
+		    rtl8723_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_N_24G,
+						 txpwr_level);
+	*powerlevel = txpwr_dbm;
+}
+
+static u8 _rtl8723be_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 _rtl8723be_get_txpower_by_rate(struct ieee80211_hw *hw,
+					 enum band_type band,
+					 enum radio_path rfpath, u8 rate)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 shift = 0, rate_section, tx_num;
+	char tx_pwr_diff = 0;
+
+	rate_section = _rtl8723be_phy_get_ratesection_intxpower_byrate(rfpath,
+								       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_RATE6M:
+	case DESC92C_RATE24M:
+	case DESC92C_RATEMCS0:
+	case DESC92C_RATEMCS4:
+	case DESC92C_RATEMCS8:
+	case DESC92C_RATEMCS12:
+		shift = 0;
+		break;
+	case DESC92C_RATE1M:
+	case DESC92C_RATE2M:
+	case DESC92C_RATE9M:
+	case DESC92C_RATE36M:
+	case DESC92C_RATEMCS1:
+	case DESC92C_RATEMCS5:
+	case DESC92C_RATEMCS9:
+	case DESC92C_RATEMCS13:
+		shift = 8;
+		break;
+	case DESC92C_RATE5_5M:
+	case DESC92C_RATE12M:
+	case DESC92C_RATE48M:
+	case DESC92C_RATEMCS2:
+	case DESC92C_RATEMCS6:
+	case DESC92C_RATEMCS10:
+	case DESC92C_RATEMCS14:
+		shift = 16;
+		break;
+	case DESC92C_RATE11M:
+	case DESC92C_RATE18M:
+	case DESC92C_RATE54M:
+	case DESC92C_RATEMCS3:
+	case DESC92C_RATEMCS7:
+	case DESC92C_RATEMCS11:
+	case DESC92C_RATEMCS15:
+		shift = 24;
+		break;
+	default:
+		RT_ASSERT(true, "Rate_Section is Illegal\n");
+		break;
+	}
+	tx_pwr_diff = (u8)(rtlphy->tx_power_by_rate_offset[band][rfpath][tx_num]
+					  [rate_section] >> shift) & 0xff;
+
+	return	tx_pwr_diff;
+}
+
+static u8 _rtl8723be_get_txpower_index(struct ieee80211_hw *hw, u8 path,
+				       u8 rate, u8 bandwidth, u8 channel)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u8 index = (channel - 1);
+	u8 txpower;
+	u8 power_diff_byrate = 0;
+
+	if (channel > 14 || channel < 1) {
+		index = 0;
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 "Illegal channel!\n");
+	}
+	if (RTL8723E_RX_HAL_IS_CCK_RATE(rate))
+		txpower = rtlefuse->txpwrlevel_cck[path][index];
+	else if (DESC92C_RATE6M <= rate)
+		txpower = rtlefuse->txpwrlevel_ht40_1s[path][index];
+	else
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 "invalid rate\n");
+
+	if (DESC92C_RATE6M <= rate && rate <= DESC92C_RATE54M &&
+	    !RTL8723E_RX_HAL_IS_CCK_RATE(rate))
+		txpower += rtlefuse->txpwr_legacyhtdiff[0][TX_1S];
+
+	if (bandwidth == HT_CHANNEL_WIDTH_20) {
+		if (DESC92C_RATEMCS0 <= rate && rate <= DESC92C_RATEMCS15)
+			txpower += rtlefuse->txpwr_ht20diff[0][TX_1S];
+		if (DESC92C_RATEMCS8 <= rate && rate <= DESC92C_RATEMCS15)
+			txpower += rtlefuse->txpwr_ht20diff[0][TX_2S];
+	} else if (bandwidth == HT_CHANNEL_WIDTH_20_40) {
+		if (DESC92C_RATEMCS0 <= rate && rate <= DESC92C_RATEMCS15)
+			txpower += rtlefuse->txpwr_ht40diff[0][TX_1S];
+		if (DESC92C_RATEMCS8 <= rate && rate <= DESC92C_RATEMCS15)
+			txpower += rtlefuse->txpwr_ht40diff[0][TX_2S];
+	}
+	if (rtlefuse->eeprom_regulatory != 2)
+		power_diff_byrate = _rtl8723be_get_txpower_by_rate(hw,
+								   BAND_ON_2_4G,
+								   path, rate);
+
+	txpower += power_diff_byrate;
+
+	if (txpower > MAX_POWER_INDEX)
+		txpower = MAX_POWER_INDEX;
+
+	return txpower;
+}
+
+static void _rtl8723be_phy_set_txpower_index(struct ieee80211_hw *hw,
+					     u8 power_index, u8 path, u8 rate)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	if (path == RF90_PATH_A) {
+		switch (rate) {
+		case DESC92C_RATE1M:
+			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_CCK1_MCS32,
+					       MASKBYTE1, power_index);
+			break;
+		case DESC92C_RATE2M:
+			rtl8723_phy_set_bb_reg(hw, RTXAGC_B_CCK11_A_CCK2_11,
+					       MASKBYTE1, power_index);
+			break;
+		case DESC92C_RATE5_5M:
+			rtl8723_phy_set_bb_reg(hw, RTXAGC_B_CCK11_A_CCK2_11,
+					       MASKBYTE2, power_index);
+			break;
+		case DESC92C_RATE11M:
+			rtl8723_phy_set_bb_reg(hw, RTXAGC_B_CCK11_A_CCK2_11,
+					       MASKBYTE3, power_index);
+			break;
+		case DESC92C_RATE6M:
+			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_RATE18_06,
+					       MASKBYTE0, power_index);
+			break;
+		case DESC92C_RATE9M:
+			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_RATE18_06,
+					       MASKBYTE1, power_index);
+			break;
+		case DESC92C_RATE12M:
+			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_RATE18_06,
+					       MASKBYTE2, power_index);
+			break;
+		case DESC92C_RATE18M:
+			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_RATE18_06,
+					       MASKBYTE3, power_index);
+			break;
+		case DESC92C_RATE24M:
+			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_RATE54_24,
+					       MASKBYTE0, power_index);
+			break;
+		case DESC92C_RATE36M:
+			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_RATE54_24,
+					       MASKBYTE1, power_index);
+			break;
+		case DESC92C_RATE48M:
+			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_RATE54_24,
+					       MASKBYTE2, power_index);
+			break;
+		case DESC92C_RATE54M:
+			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_RATE54_24,
+					       MASKBYTE3, power_index);
+			break;
+		case DESC92C_RATEMCS0:
+			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS03_MCS00,
+					       MASKBYTE0, power_index);
+			break;
+		case DESC92C_RATEMCS1:
+			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS03_MCS00,
+					       MASKBYTE1, power_index);
+			break;
+		case DESC92C_RATEMCS2:
+			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS03_MCS00,
+					       MASKBYTE2, power_index);
+			break;
+		case DESC92C_RATEMCS3:
+			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS03_MCS00,
+					       MASKBYTE3, power_index);
+			break;
+		case DESC92C_RATEMCS4:
+			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS07_MCS04,
+					       MASKBYTE0, power_index);
+			break;
+		case DESC92C_RATEMCS5:
+			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS07_MCS04,
+					       MASKBYTE1, power_index);
+			break;
+		case DESC92C_RATEMCS6:
+			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS07_MCS04,
+					       MASKBYTE2, power_index);
+			break;
+		case DESC92C_RATEMCS7:
+			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS07_MCS04,
+					       MASKBYTE3, power_index);
+			break;
+		case DESC92C_RATEMCS8:
+			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS11_MCS08,
+					       MASKBYTE0, power_index);
+			break;
+		case DESC92C_RATEMCS9:
+			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS11_MCS08,
+					       MASKBYTE1, power_index);
+			break;
+		case DESC92C_RATEMCS10:
+			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS11_MCS08,
+					       MASKBYTE2, power_index);
+			break;
+		case DESC92C_RATEMCS11:
+			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS11_MCS08,
+					       MASKBYTE3, power_index);
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+				 "Invalid Rate!!\n");
+			break;
+		}
+	} else {
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "Invalid RFPath!!\n");
+	}
+}
+
+void rtl8723be_phy_set_txpower_level(struct ieee80211_hw *hw, u8 channel)
+{
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u8 cck_rates[]  = {DESC92C_RATE1M, DESC92C_RATE2M,
+			   DESC92C_RATE5_5M, DESC92C_RATE11M};
+	u8 ofdm_rates[]  = {DESC92C_RATE6M, DESC92C_RATE9M,
+			    DESC92C_RATE12M, DESC92C_RATE18M,
+			    DESC92C_RATE24M, DESC92C_RATE36M,
+			    DESC92C_RATE48M, DESC92C_RATE54M};
+	u8 ht_rates_1t[]  = {DESC92C_RATEMCS0, DESC92C_RATEMCS1,
+			     DESC92C_RATEMCS2, DESC92C_RATEMCS3,
+			     DESC92C_RATEMCS4, DESC92C_RATEMCS5,
+			     DESC92C_RATEMCS6, DESC92C_RATEMCS7};
+	u8 i, size;
+	u8 power_index;
+
+	if (!rtlefuse->txpwr_fromeprom)
+		return;
+
+	size = sizeof(cck_rates) / sizeof(u8);
+	for (i = 0; i < size; i++) {
+		power_index = _rtl8723be_get_txpower_index(hw, RF90_PATH_A,
+					cck_rates[i],
+					rtl_priv(hw)->phy.current_chan_bw,
+					channel);
+		_rtl8723be_phy_set_txpower_index(hw, power_index, RF90_PATH_A,
+						 cck_rates[i]);
+	}
+	size = sizeof(ofdm_rates) / sizeof(u8);
+	for (i = 0; i < size; i++) {
+		power_index = _rtl8723be_get_txpower_index(hw, RF90_PATH_A,
+					ofdm_rates[i],
+					rtl_priv(hw)->phy.current_chan_bw,
+					channel);
+		_rtl8723be_phy_set_txpower_index(hw, power_index, RF90_PATH_A,
+						 ofdm_rates[i]);
+	}
+	size = sizeof(ht_rates_1t) / sizeof(u8);
+	for (i = 0; i < size; i++) {
+		power_index = _rtl8723be_get_txpower_index(hw, RF90_PATH_A,
+					ht_rates_1t[i],
+					rtl_priv(hw)->phy.current_chan_bw,
+					channel);
+		_rtl8723be_phy_set_txpower_index(hw, power_index, RF90_PATH_A,
+						 ht_rates_1t[i]);
+	}
+}
+
+void rtl8723be_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:
+			iotype = IO_CMD_PAUSE_DM_BY_SCAN;
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_IO_CMD,
+						      (u8 *)&iotype);
+			break;
+		case SCAN_OPT_RESTORE:
+			iotype = IO_CMD_RESUME_DM_BY_SCAN;
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_IO_CMD,
+						      (u8 *)&iotype);
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "Unknown Scan Backup operation.\n");
+			break;
+		}
+	}
+}
+
+void rtl8723be_phy_set_bw_mode_callback(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u8 reg_bw_opmode;
+	u8 reg_prsr_rsc;
+
+	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE,
+		 "Switch to %s bandwidth\n",
+		 rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20 ?
+		 "20MHz" : "40MHz");
+
+	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(rtlpriv, 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);
+		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(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "unknown bandwidth: %#X\n", rtlphy->current_chan_bw);
+		break;
+	}
+	rtl8723be_phy_rf6052_set_bandwidth(hw, rtlphy->current_chan_bw);
+	rtlphy->set_bwmode_inprogress = false;
+	RT_TRACE(rtlpriv, COMP_SCAN, DBG_LOUD, "\n");
+}
+
+void rtl8723be_phy_set_bw_mode(struct ieee80211_hw *hw,
+			    enum nl80211_channel_type ch_type)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_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))) {
+		rtl8723be_phy_set_bw_mode_callback(hw);
+	} else {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+			 "false driver sleep or unload\n");
+		rtlphy->set_bwmode_inprogress = false;
+		rtlphy->current_chan_bw = tmp_bw;
+	}
+}
+
+void rtl8723be_phy_sw_chnl_callback(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u32 delay;
+
+	RT_TRACE(rtlpriv, 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 (!rtl8723be_phy_sw_chn_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(rtlpriv, COMP_SCAN, DBG_TRACE, "\n");
+}
+
+u8 rtl8723be_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))) {
+		rtl8723be_phy_sw_chnl_callback(hw);
+		RT_TRACE(rtlpriv, COMP_CHAN, DBG_LOUD,
+			 "sw_chnl_inprogress false schdule "
+			  "workitem current channel %d\n",
+			  rtlphy->current_channel);
+		rtlphy->sw_chnl_inprogress = false;
+	} else {
+		RT_TRACE(rtlpriv, COMP_CHAN, DBG_LOUD,
+			 "sw_chnl_inprogress false driver sleep or"
+			  " unload\n");
+		rtlphy->sw_chnl_inprogress = false;
+	}
+	return 1;
+}
+
+static bool rtl8723be_phy_sw_chn_step_by_step(struct ieee80211_hw *hw,
+					      u8 channel, u8 *stage,
+					      u8 *step, u32 *delay)
+{
+	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;
+	rtl8723_phy_set_sw_chnl_cmdarray(precommoncmd, precommoncmdcnt++,
+					 MAX_PRECMD_CNT,
+					 CMDID_SET_TXPOWEROWER_LEVEL,
+					 0, 0, 0);
+	rtl8723_phy_set_sw_chnl_cmdarray(precommoncmd, precommoncmdcnt++,
+					 MAX_PRECMD_CNT, CMDID_END, 0, 0, 0);
+	postcommoncmdcnt = 0;
+	rtl8723_phy_set_sw_chnl_cmdarray(postcommoncmd, postcommoncmdcnt++,
+					 MAX_POSTCMD_CNT, CMDID_END,
+					 0, 0, 0);
+	rfdependcmdcnt = 0;
+
+	RT_ASSERT((channel >= 1 && channel <= 14),
+		  "illegal channel for Zebra: %d\n", channel);
+
+	rtl8723_phy_set_sw_chnl_cmdarray(rfdependcmd, rfdependcmdcnt++,
+					 MAX_RFDEPENDCMD_CNT,
+					 CMDID_RF_WRITEREG,
+					 RF_CHNLBW, channel, 10);
+
+	rtl8723_phy_set_sw_chnl_cmdarray(rfdependcmd, rfdependcmdcnt++,
+					 MAX_RFDEPENDCMD_CNT,
+					 CMDID_END, 0, 0, 0);
+
+	do {
+		switch (*stage) {
+		case 0:
+			currentcmd = &precommoncmd[*step];
+			break;
+		case 1:
+			currentcmd = &rfdependcmd[*step];
+			break;
+		case 2:
+			currentcmd = &postcommoncmd[*step];
+			break;
+		}
+
+		if (currentcmd->cmdid == CMDID_END) {
+			if ((*stage) == 2) {
+				return true;
+			} else {
+				(*stage)++;
+				(*step) = 0;
+				continue;
+			}
+		}
+
+		switch (currentcmd->cmdid) {
+		case CMDID_SET_TXPOWEROWER_LEVEL:
+			rtl8723be_phy_set_txpower_level(hw, channel);
+			break;
+		case CMDID_WRITEPORT_ULONG:
+			rtl_write_dword(rtlpriv, currentcmd->para1,
+					currentcmd->para2);
+			break;
+		case CMDID_WRITEPORT_USHORT:
+			rtl_write_word(rtlpriv, currentcmd->para1,
+				       (u16) currentcmd->para2);
+			break;
+		case CMDID_WRITEPORT_UCHAR:
+			rtl_write_byte(rtlpriv, currentcmd->para1,
+				       (u8) currentcmd->para2);
+			break;
+		case CMDID_RF_WRITEREG:
+			for (rfpath = 0; rfpath < num_total_rfpath; rfpath++) {
+				rtlphy->rfreg_chnlval[rfpath] =
+				    ((rtlphy->rfreg_chnlval[rfpath] &
+				      0xfffffc00) | currentcmd->para2);
+
+				rtl_set_rfreg(hw, (enum radio_path)rfpath,
+					      currentcmd->para1,
+					      RFREG_OFFSET_MASK,
+					      rtlphy->rfreg_chnlval[rfpath]);
+			}
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "switch case not process\n");
+			break;
+		}
+
+		break;
+	} while (true);
+
+	(*delay) = currentcmd->msdelay;
+	(*step)++;
+	return false;
+}
+
+static u8 _rtl8723be_phy_path_a_iqk(struct ieee80211_hw *hw, bool config_pathb)
+{
+	u32 reg_eac, reg_e94, reg_e9c, reg_ea4;
+	u8 result = 0x00;
+
+	rtl_set_bbreg(hw, 0xe30, MASKDWORD, 0x10008c1c);
+	rtl_set_bbreg(hw, 0xe34, MASKDWORD, 0x30008c1c);
+	rtl_set_bbreg(hw, 0xe38, MASKDWORD, 0x8214032a);
+	rtl_set_bbreg(hw, 0xe3c, MASKDWORD, 0x28160000);
+
+	rtl_set_bbreg(hw, 0xe4c, MASKDWORD, 0x00462911);
+	rtl_set_bbreg(hw, 0xe48, MASKDWORD, 0xf9000000);
+	rtl_set_bbreg(hw, 0xe48, MASKDWORD, 0xf8000000);
+
+	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);
+	reg_ea4 = rtl_get_bbreg(hw, 0xea4, MASKDWORD);
+
+	if (!(reg_eac & BIT(28)) &&
+	    (((reg_e94 & 0x03FF0000) >> 16) != 0x142) &&
+	    (((reg_e9c & 0x03FF0000) >> 16) != 0x42))
+		result |= 0x01;
+	return result;
+}
+
+static bool phy_similarity_cmp(struct ieee80211_hw *hw, long result[][8],
+			       u8 c1, u8 c2)
+{
+	u32 i, j, diff, simularity_bitmap, bound;
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	u8 final_candidate[2] = { 0xFF, 0xFF };
+	bool bresult = true, is2t = IS_92C_SERIAL(rtlhal->version);
+
+	if (is2t)
+		bound = 8;
+	else
+		bound = 4;
+
+	simularity_bitmap = 0;
+
+	for (i = 0; i < bound; i++) {
+		diff = (result[c1][i] > result[c2][i]) ?
+		    (result[c1][i] - result[c2][i]) :
+		    (result[c2][i] - result[c1][i]);
+
+		if (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 & 0x0F)) {
+		for (i = 0; i < 4; i++)
+			result[3][i] = result[c1][i];
+		return false;
+	} else if (!(simularity_bitmap & 0xF0) && is2t) {
+		for (i = 4; i < 8; i++)
+			result[3][i] = result[c1][i];
+		return false;
+	} else {
+		return false;
+	}
+}
+
+static void _rtl8723be_phy_iq_calibrate(struct ieee80211_hw *hw,
+					long result[][8], u8 t, bool is2t)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u32 i;
+	u8 patha_ok;
+	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;
+	u32 path_sel_bb, path_sel_rf;
+	u8 tmp_reg_c50, tmp_reg_c58;
+
+	tmp_reg_c50 = rtl_get_bbreg(hw, 0xc50, MASKBYTE0);
+	tmp_reg_c58 = rtl_get_bbreg(hw, 0xc58, MASKBYTE0);
+
+	if (t == 0) {
+		rtl8723_save_adda_registers(hw, adda_reg,
+					    rtlphy->adda_backup, 16);
+		rtl8723_phy_save_mac_registers(hw, iqk_mac_reg,
+					       rtlphy->iqk_mac_backup);
+		rtl8723_save_adda_registers(hw, iqk_bb_reg,
+					    rtlphy->iqk_bb_backup,
+					    IQK_BB_REG_NUM);
+	}
+	rtl8723_phy_path_adda_on(hw, adda_reg, true, is2t);
+	if (t == 0) {
+		rtlphy->rfpi_enable = (u8) rtl_get_bbreg(hw,
+						RFPGA0_XA_HSSIPARAMETER1,
+						BIT(8));
+	}
+	if (!rtlphy->rfpi_enable)
+		rtl8723_phy_pi_mode_switch(hw, true);
+
+	path_sel_bb = rtl_get_bbreg(hw, 0x948, MASKDWORD);
+	path_sel_rf = rtl_get_rfreg(hw, RF90_PATH_A, 0xb0, 0xfffff);
+
+	/*BB Setting*/
+	rtl_set_bbreg(hw, 0x800, BIT(24), 0x00);
+	rtl_set_bbreg(hw, 0xc04, MASKDWORD, 0x03a05600);
+	rtl_set_bbreg(hw, 0xc08, MASKDWORD, 0x000800e4);
+	rtl_set_bbreg(hw, 0x874, MASKDWORD, 0x22204000);
+
+	rtl_set_bbreg(hw, 0x870, BIT(10), 0x01);
+	rtl_set_bbreg(hw, 0x870, BIT(26), 0x01);
+	rtl_set_bbreg(hw, 0x860, BIT(10), 0x00);
+	rtl_set_bbreg(hw, 0x864, BIT(10), 0x00);
+
+	if (is2t)
+		rtl_set_rfreg(hw, RF90_PATH_B, 0x00, MASKDWORD, 0x10000);
+	rtl8723_phy_mac_setting_calibration(hw, iqk_mac_reg,
+					    rtlphy->iqk_mac_backup);
+	rtl_set_bbreg(hw, 0xb68, MASKDWORD, 0x0f600000);
+
+	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x80800000);
+	rtl_set_bbreg(hw, 0xe40, MASKDWORD, 0x01007c00);
+	rtl_set_bbreg(hw, 0xe44, MASKDWORD, 0x81004800);
+	for (i = 0; i < retrycount; i++) {
+		patha_ok = _rtl8723be_phy_path_a_iqk(hw, is2t);
+		if (patha_ok == 0x01) {
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				 "Path A Tx IQK Success!!\n");
+			result[t][0] = (rtl_get_bbreg(hw, 0xe94, MASKDWORD) &
+					0x3FF0000) >> 16;
+			result[t][1] = (rtl_get_bbreg(hw, 0xe9c, MASKDWORD) &
+					0x3FF0000) >> 16;
+			break;
+		}
+	}
+
+	if (0 == patha_ok)
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Path A IQK Success!!\n");
+	if (is2t) {
+		rtl8723_phy_path_a_standby(hw);
+		rtl8723_phy_path_adda_on(hw, adda_reg, false, is2t);
+	}
+
+	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0);
+
+	if (t != 0) {
+		if (!rtlphy->rfpi_enable)
+			rtl8723_phy_pi_mode_switch(hw, false);
+		rtl8723_phy_reload_adda_registers(hw, adda_reg,
+						  rtlphy->adda_backup, 16);
+		rtl8723_phy_reload_mac_registers(hw, iqk_mac_reg,
+						 rtlphy->iqk_mac_backup);
+		rtl8723_phy_reload_adda_registers(hw, iqk_bb_reg,
+						  rtlphy->iqk_bb_backup,
+						  IQK_BB_REG_NUM);
+
+		rtl_set_bbreg(hw, 0x948, MASKDWORD, path_sel_bb);
+		rtl_set_rfreg(hw, RF90_PATH_B, 0xb0, 0xfffff, path_sel_rf);
+
+		rtl_set_bbreg(hw, 0xc50, MASKBYTE0, 0x50);
+		rtl_set_bbreg(hw, 0xc50, MASKBYTE0, tmp_reg_c50);
+		if (is2t) {
+			rtl_set_bbreg(hw, 0xc58, MASKBYTE0, 0x50);
+			rtl_set_bbreg(hw, 0xc58, MASKBYTE0, tmp_reg_c58);
+		}
+		rtl_set_bbreg(hw, 0xe30, MASKDWORD, 0x01008c00);
+		rtl_set_bbreg(hw, 0xe34, MASKDWORD, 0x01008c00);
+	}
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "8723be IQK Finish!!\n");
+}
+
+static void _rtl8723be_phy_lc_calibrate(struct ieee80211_hw *hw, bool is2t)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmpreg;
+	u32 rf_a_mode = 0, rf_b_mode = 0, lc_cal;
+
+	tmpreg = rtl_read_byte(rtlpriv, 0xd03);
+
+	if ((tmpreg & 0x70) != 0)
+		rtl_write_byte(rtlpriv, 0xd03, tmpreg & 0x8F);
+	else
+		rtl_write_byte(rtlpriv, REG_TXPAUSE, 0xFF);
+
+	if ((tmpreg & 0x70) != 0) {
+		rf_a_mode = rtl_get_rfreg(hw, RF90_PATH_A, 0x00, MASK12BITS);
+
+		if (is2t)
+			rf_b_mode = rtl_get_rfreg(hw, RF90_PATH_B, 0x00,
+						  MASK12BITS);
+
+		rtl_set_rfreg(hw, RF90_PATH_A, 0x00, MASK12BITS,
+			      (rf_a_mode & 0x8FFFF) | 0x10000);
+
+		if (is2t)
+			rtl_set_rfreg(hw, RF90_PATH_B, 0x00, MASK12BITS,
+				      (rf_b_mode & 0x8FFFF) | 0x10000);
+	}
+	lc_cal = rtl_get_rfreg(hw, RF90_PATH_A, 0x18, MASK12BITS);
+
+	rtl_set_rfreg(hw, RF90_PATH_A, 0xb0, RFREG_OFFSET_MASK, 0xdfbe0);
+	rtl_set_rfreg(hw, RF90_PATH_A, 0x18, MASK12BITS, 0x8c0a);
+
+	mdelay(100);
+
+	rtl_set_rfreg(hw, RF90_PATH_A, 0xb0, RFREG_OFFSET_MASK, 0xdffe0);
+
+	if ((tmpreg & 0x70) != 0) {
+		rtl_write_byte(rtlpriv, 0xd03, tmpreg);
+		rtl_set_rfreg(hw, RF90_PATH_A, 0x00, MASK12BITS, rf_a_mode);
+
+		if (is2t)
+			rtl_set_rfreg(hw, RF90_PATH_B, 0x00,
+				      MASK12BITS, rf_b_mode);
+	} else {
+		rtl_write_byte(rtlpriv, REG_TXPAUSE, 0x00);
+	}
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "\n");
+}
+
+static void _rtl8723be_phy_set_rfpath_switch(struct ieee80211_hw *hw,
+					     bool bmain, bool is2t)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "\n");
+
+	if (is_hal_stop(rtlhal)) {
+		u8 u1btmp;
+		u1btmp = rtl_read_byte(rtlpriv, REG_LEDCFG0);
+		rtl_write_byte(rtlpriv, REG_LEDCFG0, u1btmp | BIT(7));
+		rtl_set_bbreg(hw, RFPGA0_XAB_RFPARAMETER, BIT(13), 0x01);
+	}
+	if (is2t) {
+		if (bmain)
+			rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE,
+				      BIT(5) | BIT(6), 0x1);
+		else
+			rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE,
+				      BIT(5) | BIT(6), 0x2);
+	} else {
+		rtl_set_bbreg(hw, RFPGA0_XAB_RFINTERFACESW, BIT(8) | BIT(9), 0);
+		rtl_set_bbreg(hw, 0x914, MASKLWORD, 0x0201);
+
+		/* We use the RF definition of MAIN and AUX,
+		 * left antenna and right antenna repectively.
+		 * Default output at AUX.
+		 */
+		if (bmain) {
+			rtl_set_bbreg(hw, RFPGA0_XA_RFINTERFACEOE,
+				      BIT(14) | BIT(13) | BIT(12), 0);
+			rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE,
+				      BIT(5) | BIT(4) | BIT(3), 0);
+			if (rtlefuse->antenna_div_type == CGCS_RX_HW_ANTDIV)
+				rtl_set_bbreg(hw, CONFIG_RAM64X16, BIT(31), 0);
+		} else {
+			rtl_set_bbreg(hw, RFPGA0_XA_RFINTERFACEOE,
+				      BIT(14) | BIT(13) | BIT(12), 1);
+			rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE,
+				      BIT(5) | BIT(4) | BIT(3), 1);
+			if (rtlefuse->antenna_div_type == CGCS_RX_HW_ANTDIV)
+				rtl_set_bbreg(hw, CONFIG_RAM64X16, BIT(31), 1);
+		}
+	}
+}
+
+#undef IQK_ADDA_REG_NUM
+#undef IQK_DELAY_TIME
+
+void rtl8723be_phy_iq_calibrate(struct ieee80211_hw *hw, bool recovery)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	long result[4][8];
+	u8 i, final_candidate;
+	bool patha_ok, pathb_ok;
+	long reg_e94, reg_e9c, reg_ea4, reg_eac, reg_eb4, reg_ebc, reg_ec4,
+	    reg_ecc, reg_tmp = 0;
+	bool is12simular, is13simular, is23simular;
+	u32 iqk_bb_reg[9] = {
+		ROFDM0_XARXIQIMBALANCE,
+		ROFDM0_XBRXIQIMBALANCE,
+		ROFDM0_ECCATHRESHOLD,
+		ROFDM0_AGCRSSITABLE,
+		ROFDM0_XATXIQIMBALANCE,
+		ROFDM0_XBTXIQIMBALANCE,
+		ROFDM0_XCTXAFE,
+		ROFDM0_XDTXAFE,
+		ROFDM0_RXIQEXTANTA
+	};
+
+	if (recovery) {
+		rtl8723_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;
+		result[3][i] = 0;
+	}
+	final_candidate = 0xff;
+	patha_ok = false;
+	pathb_ok = false;
+	is12simular = false;
+	is23simular = false;
+	is13simular = false;
+	for (i = 0; i < 3; i++) {
+		if (get_rf_type(rtlphy) == RF_2T2R)
+			_rtl8723be_phy_iq_calibrate(hw, result, i, true);
+		else
+			_rtl8723be_phy_iq_calibrate(hw, result, i, false);
+		if (i == 1) {
+			is12simular = phy_similarity_cmp(hw, result, 0, 1);
+			if (is12simular) {
+				final_candidate = 0;
+				break;
+			}
+		}
+		if (i == 2) {
+			is13simular = phy_similarity_cmp(hw, result, 0, 2);
+			if (is13simular) {
+				final_candidate = 0;
+				break;
+			}
+			is23simular = phy_similarity_cmp(hw, result, 1, 2);
+			if (is23simular) {
+				final_candidate = 1;
+			} else {
+				for (i = 0; i < 8; i++)
+					reg_tmp += result[3][i];
+
+				if (reg_tmp != 0)
+					final_candidate = 3;
+				else
+					final_candidate = 0xFF;
+			}
+		}
+	}
+	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];
+		patha_ok = true;
+		pathb_ok = true;
+	} else {
+		rtlphy->reg_e94 = 0x100;
+		rtlphy->reg_eb4 = 0x100;
+		rtlphy->reg_e9c = 0x0;
+		rtlphy->reg_ebc = 0x0;
+	}
+	if (reg_e94 != 0) /*&&(reg_ea4 != 0) */
+		rtl8723_phy_path_a_fill_iqk_matrix(hw, patha_ok, result,
+						   final_candidate,
+						   (reg_ea4 == 0));
+	if (final_candidate != 0xFF) {
+		for (i = 0; i < IQK_MATRIX_REG_NUM; i++)
+			rtlphy->iqk_matrix[0].value[0][i] =
+						result[final_candidate][i];
+		rtlphy->iqk_matrix[0].iqk_done = true;
+	}
+	rtl8723_save_adda_registers(hw, iqk_bb_reg, rtlphy->iqk_bb_backup, 9);
+}
+
+void rtl8723be_phy_lc_calibrate(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_hal *rtlhal = &(rtlpriv->rtlhal);
+	u32 timeout = 2000, timecount = 0;
+
+	while (rtlpriv->mac80211.act_scanning && timecount < timeout) {
+		udelay(50);
+		timecount += 50;
+	}
+
+	rtlphy->lck_inprogress = true;
+	RTPRINT(rtlpriv, FINIT, INIT_EEPROM,
+		"LCK:Start!!! currentband %x delay %d ms\n",
+		rtlhal->current_bandtype, timecount);
+
+	_rtl8723be_phy_lc_calibrate(hw, false);
+
+	rtlphy->lck_inprogress = false;
+}
+
+void rtl23b_phy_ap_calibrate(struct ieee80211_hw *hw, char delta)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	if (rtlphy->apk_done)
+		return;
+
+	return;
+}
+
+void rtl8723be_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool bmain)
+{
+	_rtl8723be_phy_set_rfpath_switch(hw, bmain, false);
+}
+
+static void rtl8723be_phy_set_io(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+		 "--->Cmd(%#x), set_io_inprogress(%d)\n",
+		  rtlphy->current_io_type, rtlphy->set_io_inprogress);
+	switch (rtlphy->current_io_type) {
+	case IO_CMD_RESUME_DM_BY_SCAN:
+		rtlpriv->dm_digtable.cur_igvalue =
+				 rtlphy->initgain_backup.xaagccore1;
+		/*rtl92c_dm_write_dig(hw);*/
+		rtl8723be_phy_set_txpower_level(hw, rtlphy->current_channel);
+		rtl_set_bbreg(hw, RCCK0_CCA, 0xff0000, 0x83);
+		break;
+	case IO_CMD_PAUSE_DM_BY_SCAN:
+		rtlphy->initgain_backup.xaagccore1 =
+				 rtlpriv->dm_digtable.cur_igvalue;
+		rtlpriv->dm_digtable.cur_igvalue = 0x17;
+		rtl_set_bbreg(hw, RCCK0_CCA, 0xff0000, 0x40);
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "switch case not process\n");
+		break;
+	}
+	rtlphy->set_io_inprogress = false;
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+		 "(%#x)\n", rtlphy->current_io_type);
+}
+
+bool rtl8723be_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	bool postprocessing = false;
+
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+		 "-->IO Cmd(%#x), set_io_inprogress(%d)\n",
+		  iotype, rtlphy->set_io_inprogress);
+	do {
+		switch (iotype) {
+		case IO_CMD_RESUME_DM_BY_SCAN:
+			RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+				 "[IO CMD] Resume DM after scan.\n");
+			postprocessing = true;
+			break;
+		case IO_CMD_PAUSE_DM_BY_SCAN:
+			RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+				 "[IO CMD] Pause DM before scan.\n");
+			postprocessing = true;
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "switch case not process\n");
+			break;
+		}
+	} while (false);
+	if (postprocessing && !rtlphy->set_io_inprogress) {
+		rtlphy->set_io_inprogress = true;
+		rtlphy->current_io_type = iotype;
+	} else {
+		return false;
+	}
+	rtl8723be_phy_set_io(hw);
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE, "IO Type(%#x)\n", iotype);
+	return true;
+}
+
+static void rtl8723be_phy_set_rf_on(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtl_write_byte(rtlpriv, REG_SPS0_CTRL, 0x2b);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE3);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE2);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE3);
+	rtl_write_byte(rtlpriv, REG_TXPAUSE, 0x00);
+}
+
+static void _rtl8723be_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 _rtl8723be_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 initialize_count = 0;
+			do {
+				initialize_count++;
+				RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
+					 "IPS Set eRf nic enable\n");
+				rtstatus = rtl_ps_enable_nic(hw);
+			} while (!rtstatus && (initialize_count < 10));
+				RT_CLEAR_PS_LEVEL(ppsc,
+						  RT_RF_OFF_LEVL_HALT_NIC);
+		} else {
+			RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
+				 "Set ERFON sleeped:%d ms\n",
+				  jiffies_to_msecs(jiffies -
+						   ppsc->last_sleep_jiffies));
+			ppsc->last_awake_jiffies = jiffies;
+			rtl8723be_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(rtlpriv, COMP_ERR, DBG_WARNING,
+					 "eRf Off/Sleep: %d times "
+					  "TcbBusyQueue[%d] =%d before "
+					  "doze!\n", (i + 1), queue_id,
+					  skb_queue_len(&ring->queue));
+
+				udelay(10);
+				i++;
+			}
+			if (i >= MAX_DOZE_WAITING_TIMES_9x) {
+				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+					 "\n ERFSLEEP: %d times "
+					  "TcbBusyQueue[%d] = %d !\n",
+					  MAX_DOZE_WAITING_TIMES_9x,
+					  queue_id,
+					  skb_queue_len(&ring->queue));
+				break;
+			}
+		}
+
+		if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_HALT_NIC) {
+			RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
+				 "IPS Set eRf nic disable\n");
+			rtl_ps_disable_nic(hw);
+			RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
+		} else {
+			if (ppsc->rfoff_reason == RF_CHANGE_BY_IPS) {
+				rtlpriv->cfg->ops->led_control(hw,
+							       LED_CTL_NO_LINK);
+			} else {
+				rtlpriv->cfg->ops->led_control(hw,
+							     LED_CTL_POWER_OFF);
+			}
+		}
+		break;
+	case ERFSLEEP:
+		if (ppsc->rfpwr_state == ERFOFF)
+			break;
+		for (queue_id = 0, i = 0;
+		     queue_id < RTL_PCI_MAX_TX_QUEUE_COUNT;) {
+			ring = &pcipriv->dev.tx_ring[queue_id];
+			if (skb_queue_len(&ring->queue) == 0) {
+				queue_id++;
+				continue;
+			} else {
+				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+					 "eRf Off/Sleep: %d times "
+					  "TcbBusyQueue[%d] =%d before "
+					  "doze!\n", (i + 1), queue_id,
+					  skb_queue_len(&ring->queue));
+
+				udelay(10);
+				i++;
+			}
+			if (i >= MAX_DOZE_WAITING_TIMES_9x) {
+				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+					 "\n ERFSLEEP: %d times "
+					  "TcbBusyQueue[%d] = %d !\n",
+					  MAX_DOZE_WAITING_TIMES_9x,
+					  queue_id,
+					  skb_queue_len(&ring->queue));
+				break;
+			}
+		}
+		RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
+			 "Set ERFSLEEP awaked:%d ms\n",
+			  jiffies_to_msecs(jiffies -
+					   ppsc->last_awake_jiffies));
+		ppsc->last_sleep_jiffies = jiffies;
+		_rtl8723be_phy_set_rf_sleep(hw);
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "switch case not process\n");
+		bresult = false;
+		break;
+	}
+	if (bresult)
+		ppsc->rfpwr_state = rfpwr_state;
+	return bresult;
+}
+
+bool rtl8723be_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 = _rtl8723be_phy_set_rf_power_state(hw, rfpwr_state);
+	return bresult;
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/phy.h b/drivers/net/wireless/rtlwifi/rtl8723be/phy.h
new file mode 100644
index 000000000000..444ef95bb6af
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/phy.h
@@ -0,0 +1,217 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL8723BE_PHY_H__
+#define __RTL8723BE_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	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_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			1
+
+#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 IQK_ADDA_REG_NUM		16
+#define IQK_MAC_REG_NUM			4
+
+#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 hw90_block_e {
+	HW90_BLOCK_MAC = 0,
+	HW90_BLOCK_PHY0 = 1,
+	HW90_BLOCK_PHY1 = 2,
+	HW90_BLOCK_RF = 3,
+	HW90_BLOCK_MAXIMUM = 4,
+};
+
+enum baseband_config_type {
+	BASEBAND_CONFIG_PHY_REG = 0,
+	BASEBAND_CONFIG_AGC_TAB = 1,
+};
+
+enum ra_offset_area {
+	RA_OFFSET_LEGACY_OFDM1,
+	RA_OFFSET_LEGACY_OFDM2,
+	RA_OFFSET_HT_OFDM1,
+	RA_OFFSET_HT_OFDM2,
+	RA_OFFSET_HT_OFDM3,
+	RA_OFFSET_HT_OFDM4,
+	RA_OFFSET_HT_CCK,
+};
+
+enum antenna_path {
+	ANTENNA_NONE,
+	ANTENNA_D,
+	ANTENNA_C,
+	ANTENNA_CD,
+	ANTENNA_B,
+	ANTENNA_BD,
+	ANTENNA_BC,
+	ANTENNA_BCD,
+	ANTENNA_A,
+	ANTENNA_AD,
+	ANTENNA_AC,
+	ANTENNA_ACD,
+	ANTENNA_AB,
+	ANTENNA_ABD,
+	ANTENNA_ABC,
+	ANTENNA_ABCD
+};
+
+struct r_antenna_select_ofdm {
+	u32 r_tx_antenna:4;
+	u32 r_ant_l:4;
+	u32 r_ant_non_ht:4;
+	u32 r_ant_ht1:4;
+	u32 r_ant_ht2:4;
+	u32 r_ant_ht_s1:4;
+	u32 r_ant_non_ht_s1:4;
+	u32 ofdm_txsc:2;
+	u32 reserved:2;
+};
+
+struct r_antenna_select_cck {
+	u8 r_cckrx_enable_2:2;
+	u8 r_cckrx_enable:2;
+	u8 r_ccktx_enable:4;
+};
+
+
+struct efuse_contents {
+	u8 mac_addr[ETH_ALEN];
+	u8 cck_tx_power_idx[6];
+	u8 ht40_1s_tx_power_idx[6];
+	u8 ht40_2s_tx_power_idx_diff[3];
+	u8 ht20_tx_power_idx_diff[3];
+	u8 ofdm_tx_power_idx_diff[3];
+	u8 ht40_max_power_offset[3];
+	u8 ht20_max_power_offset[3];
+	u8 channel_plan;
+	u8 thermal_meter;
+	u8 rf_option[5];
+	u8 version;
+	u8 oem_id;
+	u8 regulatory;
+};
+
+struct tx_power_struct {
+	u8 cck[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
+	u8 ht40_1s[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
+	u8 ht40_2s[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
+	u8 ht20_diff[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
+	u8 legacy_ht_diff[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
+	u8 legacy_ht_txpowerdiff;
+	u8 groupht20[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
+	u8 groupht40[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
+	u8 pwrgroup_cnt;
+	u32 mcs_original_offset[4][16];
+};
+
+enum _ANT_DIV_TYPE {
+	NO_ANTDIV				= 0xFF,
+	CG_TRX_HW_ANTDIV		= 0x01,
+	CGCS_RX_HW_ANTDIV		= 0x02,
+	FIXED_HW_ANTDIV         = 0x03,
+	CG_TRX_SMART_ANTDIV		= 0x04,
+	CGCS_RX_SW_ANTDIV		= 0x05,
+};
+
+u32 rtl8723be_phy_query_rf_reg(struct ieee80211_hw *hw,
+			       enum radio_path rfpath,
+			       u32 regaddr, u32 bitmask);
+void rtl8723be_phy_set_rf_reg(struct ieee80211_hw *hw,
+			      enum radio_path rfpath,
+			      u32 regaddr, u32 bitmask, u32 data);
+bool rtl8723be_phy_mac_config(struct ieee80211_hw *hw);
+bool rtl8723be_phy_bb_config(struct ieee80211_hw *hw);
+bool rtl8723be_phy_rf_config(struct ieee80211_hw *hw);
+void rtl8723be_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw);
+void rtl8723be_phy_get_txpower_level(struct ieee80211_hw *hw,
+				     long *powerlevel);
+void rtl8723be_phy_set_txpower_level(struct ieee80211_hw *hw,
+				     u8 channel);
+void rtl8723be_phy_scan_operation_backup(struct ieee80211_hw *hw,
+					 u8 operation);
+void rtl8723be_phy_set_bw_mode_callback(struct ieee80211_hw *hw);
+void rtl8723be_phy_set_bw_mode(struct ieee80211_hw *hw,
+			       enum nl80211_channel_type ch_type);
+void rtl8723be_phy_sw_chnl_callback(struct ieee80211_hw *hw);
+u8 rtl8723be_phy_sw_chnl(struct ieee80211_hw *hw);
+void rtl8723be_phy_iq_calibrate(struct ieee80211_hw *hw,
+				bool b_recovery);
+void rtl23b_phy_ap_calibrate(struct ieee80211_hw *hw, char delta);
+void rtl8723be_phy_lc_calibrate(struct ieee80211_hw *hw);
+void rtl8723be_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool bmain);
+bool rtl8723be_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
+					     enum radio_path rfpath);
+bool rtl8723be_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype);
+bool rtl8723be_phy_set_rf_power_state(struct ieee80211_hw *hw,
+				      enum rf_pwrstate rfpwr_state);
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/pwrseq.c b/drivers/net/wireless/rtlwifi/rtl8723be/pwrseq.c
new file mode 100644
index 000000000000..b5167e73fecf
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/pwrseq.c
@@ -0,0 +1,106 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "pwrseqcmd.h"
+#include "pwrseq.h"
+
+
+/* drivers should parse below arrays and do the corresponding actions */
+/*3 Power on  Array*/
+struct wlan_pwr_cfg rtl8723B_power_on_flow[RTL8723B_TRANS_CARDEMU_TO_ACT_STEPS +
+					   RTL8723B_TRANS_END_STEPS] = {
+	RTL8723B_TRANS_CARDEMU_TO_ACT
+	RTL8723B_TRANS_END
+};
+
+/*3Radio off GPIO Array */
+struct wlan_pwr_cfg rtl8723B_radio_off_flow[RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS
+					    + RTL8723B_TRANS_END_STEPS] = {
+	RTL8723B_TRANS_ACT_TO_CARDEMU
+	RTL8723B_TRANS_END
+};
+
+/*3Card Disable Array*/
+struct wlan_pwr_cfg rtl8723B_card_disable_flow
+				[RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS +
+				 RTL8723B_TRANS_CARDEMU_TO_PDN_STEPS +
+				 RTL8723B_TRANS_END_STEPS] = {
+	RTL8723B_TRANS_ACT_TO_CARDEMU
+	RTL8723B_TRANS_CARDEMU_TO_CARDDIS
+	RTL8723B_TRANS_END
+};
+
+/*3 Card Enable Array*/
+struct wlan_pwr_cfg rtl8723B_card_enable_flow
+				[RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS +
+				 RTL8723B_TRANS_CARDEMU_TO_PDN_STEPS +
+				 RTL8723B_TRANS_END_STEPS] = {
+	RTL8723B_TRANS_CARDDIS_TO_CARDEMU
+	RTL8723B_TRANS_CARDEMU_TO_ACT
+	RTL8723B_TRANS_END
+};
+
+/*3Suspend Array*/
+struct wlan_pwr_cfg rtl8723B_suspend_flow[RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS +
+					  RTL8723B_TRANS_CARDEMU_TO_SUS_STEPS +
+					  RTL8723B_TRANS_END_STEPS] = {
+	RTL8723B_TRANS_ACT_TO_CARDEMU
+	RTL8723B_TRANS_CARDEMU_TO_SUS
+	RTL8723B_TRANS_END
+};
+
+/*3 Resume Array*/
+struct wlan_pwr_cfg rtl8723B_resume_flow[RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS +
+					 RTL8723B_TRANS_CARDEMU_TO_SUS_STEPS +
+					 RTL8723B_TRANS_END_STEPS] = {
+	RTL8723B_TRANS_SUS_TO_CARDEMU
+	RTL8723B_TRANS_CARDEMU_TO_ACT
+	RTL8723B_TRANS_END
+};
+
+/*3HWPDN Array*/
+struct wlan_pwr_cfg rtl8723B_hwpdn_flow[RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS +
+					RTL8723B_TRANS_CARDEMU_TO_PDN_STEPS +
+					RTL8723B_TRANS_END_STEPS] = {
+	RTL8723B_TRANS_ACT_TO_CARDEMU
+	RTL8723B_TRANS_CARDEMU_TO_PDN
+	RTL8723B_TRANS_END
+};
+
+/*3 Enter LPS */
+struct wlan_pwr_cfg rtl8723B_enter_lps_flow[RTL8723B_TRANS_ACT_TO_LPS_STEPS +
+					    RTL8723B_TRANS_END_STEPS] = {
+	/*FW behavior*/
+	RTL8723B_TRANS_ACT_TO_LPS
+	RTL8723B_TRANS_END
+};
+
+/*3 Leave LPS */
+struct wlan_pwr_cfg rtl8723B_leave_lps_flow[RTL8723B_TRANS_LPS_TO_ACT_STEPS +
+					    RTL8723B_TRANS_END_STEPS] = {
+	/*FW behavior*/
+	RTL8723B_TRANS_LPS_TO_ACT
+	RTL8723B_TRANS_END
+};
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/pwrseq.h b/drivers/net/wireless/rtlwifi/rtl8723be/pwrseq.h
new file mode 100644
index 000000000000..a62f43ed8d32
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/pwrseq.h
@@ -0,0 +1,304 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL8723BE_PWRSEQ_H__
+#define __RTL8723BE_PWRSEQ_H__
+
+/*	Check document WM-20130425-JackieLau-RTL8723B_Power_Architecture v05.vsd
+ *	There are 6 HW Power States:
+ *	0: POFF--Power Off
+ *	1: PDN--Power Down
+ *	2: CARDEMU--Card Emulation
+ *	3: ACT--Active Mode
+ *	4: LPS--Low Power State
+ *	5: SUS--Suspend
+ *
+ *	The transition from different states are defined below
+ *	TRANS_CARDEMU_TO_ACT
+ *	TRANS_ACT_TO_CARDEMU
+ *	TRANS_CARDEMU_TO_SUS
+ *	TRANS_SUS_TO_CARDEMU
+ *	TRANS_CARDEMU_TO_PDN
+ *	TRANS_ACT_TO_LPS
+ *	TRANS_LPS_TO_ACT
+ *
+ *	TRANS_END
+ */
+#define	RTL8723B_TRANS_CARDEMU_TO_ACT_STEPS	23
+#define	RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS	15
+#define	RTL8723B_TRANS_CARDEMU_TO_SUS_STEPS	15
+#define	RTL8723B_TRANS_SUS_TO_CARDEMU_STEPS	15
+#define	RTL8723B_TRANS_CARDEMU_TO_PDN_STEPS	15
+#define	RTL8723B_TRANS_PDN_TO_CARDEMU_STEPS	15
+#define	RTL8723B_TRANS_ACT_TO_LPS_STEPS		15
+#define	RTL8723B_TRANS_LPS_TO_ACT_STEPS		15
+#define	RTL8723B_TRANS_END_STEPS		1
+
+#define RTL8723B_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, BIT(0), BIT(0)},		\
+	{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},			\
+	{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},		\
+	{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},			\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, (BIT(4)|BIT(3)|BIT(2)), 0},	\
+	{0x0075, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0) , 0},			\
+	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(1), BIT(1)},		\
+	{0x0075, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0) , BIT(0)},		\
+	{0x0006, 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_WRITE, BIT(7), 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},		\
+	{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},			\
+	{0x0010, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(6), BIT(6)},		\
+	{0x0049, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), BIT(1)},		\
+	{0x0063, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), BIT(1)},		\
+	{0x0062, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), 0},			\
+	{0x0058, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), BIT(0)},		\
+	{0x005A, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), BIT(1)},		\
+	{0x0068, PWR_CUT_TESTCHIP_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3), BIT(3)},		\
+	{0x0069, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(6), BIT(6)},
+
+#define RTL8723B_TRANS_ACT_TO_CARDEMU					\
+	{0x001F, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0},			\
+	{0x004F, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), 0},			\
+	{0x0049, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), 0},			\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), BIT(1)},		\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(1), 0},			\
+	{0x0010, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(6), 0},			\
+	{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)},				\
+	{0x0020, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
+	 PWR_INTF_USB_MSK | PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC,	\
+	 PWR_CMD_WRITE, BIT(0), 0},
+
+#define RTL8723B_TRANS_CARDEMU_TO_SUS					\
+	{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))}, \
+	{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)},			\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), BIT(4)},		\
+	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x20},			\
+	{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)},\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT(0), BIT(0)},		\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT(1), 0},
+
+#define RTL8723B_TRANS_SUS_TO_CARDEMU					\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3) | BIT(7), 0},		\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT(0), 0},			\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT(1), BIT(1)},		\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), 0},			\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3)|BIT(4), 0},
+
+#define RTL8723B_TRANS_CARDEMU_TO_CARDDIS				\
+	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x20},			\
+	{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)},	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(2), BIT(2)},		\
+	{0x004A, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), 1},			\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), BIT(4)},		\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT(0), BIT(0)},		\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT(1), 0},
+
+#define RTL8723B_TRANS_CARDDIS_TO_CARDEMU				\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3) | BIT(7), 0},		\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT(0), 0},			\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT(1), BIT(1)},		\
+	{0x004A, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), 0},			\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3)|BIT(4), 0},		\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), 0},			\
+	{0x0301, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0},
+
+#define RTL8723B_TRANS_CARDEMU_TO_PDN					\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), BIT(4)},		\
+	{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},					\
+	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), 0},			\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), BIT(7)},
+
+#define RTL8723B_TRANS_PDN_TO_CARDEMU					\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), 0},
+
+#define RTL8723B_TRANS_ACT_TO_LPS					\
+	{0x0301, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0xFF},			\
+	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0xFF},			\
+	{0x05F8, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},			\
+	{0x05F9, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},			\
+	{0x05FA, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},			\
+	{0x05FB, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},			\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), 0},			\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_DELAY, 0, PWRSEQ_DELAY_US},		\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), 0},			\
+	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x03},			\
+	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), 0},			\
+	{0x0093, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x00},			\
+	{0x0553, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(5), BIT(5)},
+
+#define RTL8723B_TRANS_LPS_TO_ACT					\
+	{0x0080, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_SDIO, PWR_CMD_WRITE, 0xFF, 0x84},		\
+	{0xFE58, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x84},			\
+	{0x0361, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x84},			\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_DELAY, 0, PWRSEQ_DELAY_MS},		\
+	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), 0},			\
+	{0x0109, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(7), 0},		\
+	{0x0029, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(6)|BIT(7), 0},		\
+	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), BIT(1)},		\
+	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0xFF},			\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1) | BIT(0), BIT(1) | BIT(0)}, \
+	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0},
+
+#define RTL8723B_TRANS_END						\
+	{0xFFFF, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, 0,	\
+	 PWR_CMD_END, 0, 0},
+
+extern struct wlan_pwr_cfg rtl8723B_power_on_flow
+				[RTL8723B_TRANS_CARDEMU_TO_ACT_STEPS +
+				 RTL8723B_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8723B_radio_off_flow
+				[RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS +
+				 RTL8723B_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8723B_card_disable_flow
+				[RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS +
+				 RTL8723B_TRANS_CARDEMU_TO_PDN_STEPS +
+				 RTL8723B_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8723B_card_enable_flow
+				[RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS +
+				 RTL8723B_TRANS_CARDEMU_TO_PDN_STEPS +
+				 RTL8723B_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8723B_suspend_flow
+				[RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS +
+				 RTL8723B_TRANS_CARDEMU_TO_SUS_STEPS +
+				 RTL8723B_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8723B_resume_flow
+				[RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS +
+				 RTL8723B_TRANS_CARDEMU_TO_SUS_STEPS +
+				 RTL8723B_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8723B_hwpdn_flow
+				[RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS +
+				 RTL8723B_TRANS_CARDEMU_TO_PDN_STEPS +
+				 RTL8723B_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8723B_enter_lps_flow
+				[RTL8723B_TRANS_ACT_TO_LPS_STEPS +
+				 RTL8723B_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8723B_leave_lps_flow
+				[RTL8723B_TRANS_LPS_TO_ACT_STEPS +
+				 RTL8723B_TRANS_END_STEPS];
+
+/* RTL8723 Power Configuration CMDs for PCIe interface */
+#define RTL8723_NIC_PWR_ON_FLOW		rtl8723B_power_on_flow
+#define RTL8723_NIC_RF_OFF_FLOW		rtl8723B_radio_off_flow
+#define RTL8723_NIC_DISABLE_FLOW	rtl8723B_card_disable_flow
+#define RTL8723_NIC_ENABLE_FLOW		rtl8723B_card_enable_flow
+#define RTL8723_NIC_SUSPEND_FLOW	rtl8723B_suspend_flow
+#define RTL8723_NIC_RESUME_FLOW		rtl8723B_resume_flow
+#define RTL8723_NIC_PDN_FLOW		rtl8723B_hwpdn_flow
+#define RTL8723_NIC_LPS_ENTER_FLOW	rtl8723B_enter_lps_flow
+#define RTL8723_NIC_LPS_LEAVE_FLOW	rtl8723B_leave_lps_flow
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/pwrseqcmd.c b/drivers/net/wireless/rtlwifi/rtl8723be/pwrseqcmd.c
new file mode 100644
index 000000000000..e4a507a756fb
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/pwrseqcmd.c
@@ -0,0 +1,140 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "pwrseq.h"
+
+/*	Description:
+ *		This routine deal with the Power Configuration CMDs
+ *		 parsing for RTL8723/RTL8188E Series IC.
+ *	Assumption:
+ *		We should follow specific format which was released from HW SD.
+ *
+ *	2011.07.07, added by Roger.
+ */
+bool rtlbe_hal_pwrseqcmdparsing(struct rtl_priv *rtlpriv, u8 cut_version,
+				 u8 fab_version, u8 interface_type,
+				 struct wlan_pwr_cfg pwrcfgcmd[])
+
+{
+	struct wlan_pwr_cfg pwr_cfg_cmd = {0};
+	bool b_polling_bit = false;
+	u32 ary_idx = 0;
+	u8 value = 0;
+	u32 offset = 0;
+	u32 polling_count = 0;
+	u32 max_polling_cnt = 5000;
+
+	do {
+		pwr_cfg_cmd = pwrcfgcmd[ary_idx];
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "rtlbe_hal_pwrseqcmdparsing(): "
+			 "offset(%#x),cut_msk(%#x), fab_msk(%#x),"
+			 "interface_msk(%#x), base(%#x), "
+			 "cmd(%#x), msk(%#x), value(%#x)\n",
+			 GET_PWR_CFG_OFFSET(pwr_cfg_cmd),
+			 GET_PWR_CFG_CUT_MASK(pwr_cfg_cmd),
+			 GET_PWR_CFG_FAB_MASK(pwr_cfg_cmd),
+			 GET_PWR_CFG_INTF_MASK(pwr_cfg_cmd),
+			 GET_PWR_CFG_BASE(pwr_cfg_cmd),
+			 GET_PWR_CFG_CMD(pwr_cfg_cmd),
+			 GET_PWR_CFG_MASK(pwr_cfg_cmd),
+			 GET_PWR_CFG_VALUE(pwr_cfg_cmd));
+
+		if ((GET_PWR_CFG_FAB_MASK(pwr_cfg_cmd)&fab_version) &&
+		    (GET_PWR_CFG_CUT_MASK(pwr_cfg_cmd)&cut_version) &&
+		    (GET_PWR_CFG_INTF_MASK(pwr_cfg_cmd)&interface_type)) {
+			switch (GET_PWR_CFG_CMD(pwr_cfg_cmd)) {
+			case PWR_CMD_READ:
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+					 "rtlbe_hal_pwrseqcmdparsing(): "
+					  "PWR_CMD_READ\n");
+				break;
+			case PWR_CMD_WRITE:
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+					 "rtlbe_hal_pwrseqcmdparsing(): "
+					  "PWR_CMD_WRITE\n");
+				offset = GET_PWR_CFG_OFFSET(pwr_cfg_cmd);
+
+				/*Read the value from system register*/
+				value = rtl_read_byte(rtlpriv, offset);
+				value &= (~(GET_PWR_CFG_MASK(pwr_cfg_cmd)));
+				value = value | (GET_PWR_CFG_VALUE(pwr_cfg_cmd)
+					& GET_PWR_CFG_MASK(pwr_cfg_cmd));
+
+				/*Write the value back to sytem register*/
+				rtl_write_byte(rtlpriv, offset, value);
+				break;
+			case PWR_CMD_POLLING:
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+					 "rtlbe_hal_pwrseqcmdparsing(): "
+					  "PWR_CMD_POLLING\n");
+				b_polling_bit = false;
+				offset = GET_PWR_CFG_OFFSET(pwr_cfg_cmd);
+
+				do {
+					value = rtl_read_byte(rtlpriv, offset);
+
+					value &= GET_PWR_CFG_MASK(pwr_cfg_cmd);
+					if (value ==
+					    (GET_PWR_CFG_VALUE(pwr_cfg_cmd) &
+					     GET_PWR_CFG_MASK(pwr_cfg_cmd)))
+						b_polling_bit = true;
+					else
+						udelay(10);
+
+					if (polling_count++ > max_polling_cnt)
+						return false;
+
+				} while (!b_polling_bit);
+				break;
+			case PWR_CMD_DELAY:
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+					 "rtlbe_hal_pwrseqcmdparsing(): "
+					 "PWR_CMD_DELAY\n");
+				if (GET_PWR_CFG_VALUE(pwr_cfg_cmd) ==
+				    PWRSEQ_DELAY_US)
+					udelay(GET_PWR_CFG_OFFSET(pwr_cfg_cmd));
+				else
+					mdelay(GET_PWR_CFG_OFFSET(pwr_cfg_cmd));
+				break;
+			case PWR_CMD_END:
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+					 "rtlbe_hal_pwrseqcmdparsing(): "
+					 "PWR_CMD_END\n");
+				return true;
+				break;
+			default:
+				RT_ASSERT(false,
+					  "rtlbe_hal_pwrseqcmdparsing(): "
+					  "Unknown CMD!!\n");
+				break;
+			}
+		}
+
+		ary_idx++;
+	} while (1);
+
+	return true;
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/pwrseqcmd.h b/drivers/net/wireless/rtlwifi/rtl8723be/pwrseqcmd.h
new file mode 100644
index 000000000000..ce14a3b5cb71
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/pwrseqcmd.h
@@ -0,0 +1,95 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL8723BE_PWRSEQCMD_H__
+#define __RTL8723BE_PWRSEQCMD_H__
+
+#include "../wifi.h"
+/*---------------------------------------------*/
+/*The value of cmd: 4 bits */
+/*---------------------------------------------*/
+#define PWR_CMD_READ		0x00
+#define PWR_CMD_WRITE		0x01
+#define PWR_CMD_POLLING		0x02
+#define PWR_CMD_DELAY		0x03
+#define PWR_CMD_END		0x04
+
+/* define the base address of each block */
+#define PWR_BASEADDR_MAC	0x00
+#define PWR_BASEADDR_USB	0x01
+#define PWR_BASEADDR_PCIE	0x02
+#define PWR_BASEADDR_SDIO	0x03
+
+#define	PWR_INTF_SDIO_MSK	BIT(0)
+#define	PWR_INTF_USB_MSK	BIT(1)
+#define	PWR_INTF_PCI_MSK	BIT(2)
+#define	PWR_INTF_ALL_MSK	(BIT(0) | BIT(1) | BIT(2) | BIT(3))
+
+#define	PWR_FAB_TSMC_MSK	BIT(0)
+#define	PWR_FAB_UMC_MSK		BIT(1)
+#define	PWR_FAB_ALL_MSK		(BIT(0) | BIT(1) | BIT(2) | BIT(3))
+
+#define	PWR_CUT_TESTCHIP_MSK	BIT(0)
+#define	PWR_CUT_A_MSK		BIT(1)
+#define	PWR_CUT_B_MSK		BIT(2)
+#define	PWR_CUT_C_MSK		BIT(3)
+#define	PWR_CUT_D_MSK		BIT(4)
+#define	PWR_CUT_E_MSK		BIT(5)
+#define	PWR_CUT_F_MSK		BIT(6)
+#define	PWR_CUT_G_MSK		BIT(7)
+#define	PWR_CUT_ALL_MSK		0xFF
+
+
+enum pwrseq_delay_unit {
+	PWRSEQ_DELAY_US,
+	PWRSEQ_DELAY_MS,
+};
+
+struct wlan_pwr_cfg {
+	u16 offset;
+	u8 cut_msk;
+	u8 fab_msk:4;
+	u8 interface_msk:4;
+	u8 base:4;
+	u8 cmd:4;
+	u8 msk;
+	u8 value;
+
+};
+
+#define	GET_PWR_CFG_OFFSET(__PWR_CMD)	__PWR_CMD.offset
+#define	GET_PWR_CFG_CUT_MASK(__PWR_CMD)	__PWR_CMD.cut_msk
+#define	GET_PWR_CFG_FAB_MASK(__PWR_CMD)	__PWR_CMD.fab_msk
+#define	GET_PWR_CFG_INTF_MASK(__PWR_CMD)	__PWR_CMD.interface_msk
+#define	GET_PWR_CFG_BASE(__PWR_CMD)	__PWR_CMD.base
+#define	GET_PWR_CFG_CMD(__PWR_CMD)	__PWR_CMD.cmd
+#define	GET_PWR_CFG_MASK(__PWR_CMD)	__PWR_CMD.msk
+#define	GET_PWR_CFG_VALUE(__PWR_CMD)	__PWR_CMD.value
+
+bool rtlbe_hal_pwrseqcmdparsing(struct rtl_priv *rtlpriv, u8 cut_version,
+				 u8 fab_version, u8 interface_type,
+				 struct wlan_pwr_cfg pwrcfgcmd[]);
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/reg.h b/drivers/net/wireless/rtlwifi/rtl8723be/reg.h
new file mode 100644
index 000000000000..4c653fab8795
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/reg.h
@@ -0,0 +1,2277 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL8723BE_REG_H__
+#define __RTL8723BE_REG_H__
+
+#define TXPKT_BUF_SELECT			0x69
+#define RXPKT_BUF_SELECT			0xA5
+#define DISABLE_TRXPKT_BUF_ACCESS		0x0
+
+#define REG_SYS_ISO_CTRL			0x0000
+#define REG_SYS_FUNC_EN				0x0002
+#define REG_APS_FSMCO				0x0004
+#define REG_SYS_CLKR				0x0008
+#define REG_9346CR				0x000A
+#define REG_EE_VPD				0x000C
+#define REG_AFE_MISC				0x0010
+#define REG_SPS0_CTRL				0x0011
+#define REG_SPS_OCP_CFG				0x0018
+#define REG_RSV_CTRL				0x001C
+#define REG_RF_CTRL				0x001F
+#define REG_LDOA15_CTRL				0x0020
+#define REG_LDOV12D_CTRL			0x0021
+#define REG_LDOHCI12_CTRL			0x0022
+#define REG_LPLDO_CTRL				0x0023
+#define REG_AFE_XTAL_CTRL			0x0024
+/* 1.5v for 8188EE test chip, 1.4v for MP chip */
+#define REG_AFE_LDO_CTRL			0x0027
+#define REG_AFE_PLL_CTRL			0x0028
+#define REG_MAC_PHY_CTRL			0x002c
+#define REG_EFUSE_CTRL				0x0030
+#define REG_EFUSE_TEST				0x0034
+#define REG_PWR_DATA				0x0038
+#define REG_CAL_TIMER				0x003C
+#define REG_ACLK_MON				0x003E
+#define REG_GPIO_MUXCFG				0x0040
+#define REG_GPIO_IO_SEL				0x0042
+#define REG_MAC_PINMUX_CFG			0x0043
+#define REG_GPIO_PIN_CTRL			0x0044
+#define REG_GPIO_INTM				0x0048
+#define REG_LEDCFG0				0x004C
+#define REG_LEDCFG1				0x004D
+#define REG_LEDCFG2				0x004E
+#define REG_LEDCFG3				0x004F
+#define REG_FSIMR				0x0050
+#define REG_FSISR				0x0054
+#define REG_HSIMR				0x0058
+#define REG_HSISR				0x005c
+#define REG_GPIO_PIN_CTRL_2			0x0060
+#define REG_GPIO_IO_SEL_2			0x0062
+#define REG_MULTI_FUNC_CTRL			0x0068
+#define REG_GPIO_OUTPUT				0x006c
+#define REG_AFE_XTAL_CTRL_EXT			0x0078
+#define REG_XCK_OUT_CTRL			0x007c
+#define REG_MCUFWDL				0x0080
+#define REG_WOL_EVENT				0x0081
+#define REG_MCUTSTCFG				0x0084
+
+
+#define REG_HIMR				0x00B0
+#define REG_HISR				0x00B4
+#define REG_HIMRE				0x00B8
+#define REG_HISRE				0x00BC
+
+#define REG_EFUSE_ACCESS			0x00CF
+
+#define REG_BIST_SCAN				0x00D0
+#define REG_BIST_RPT				0x00D4
+#define REG_BIST_ROM_RPT			0x00D8
+#define REG_USB_SIE_INTF			0x00E0
+#define REG_PCIE_MIO_INTF			0x00E4
+#define REG_PCIE_MIO_INTD			0x00E8
+#define REG_HPON_FSM				0x00EC
+#define REG_SYS_CFG				0x00F0
+#define REG_GPIO_OUTSTS				0x00F4
+#define REG_SYS_CFG1				0x00F0
+#define REG_ROM_VERSION				0x00FD
+
+#define REG_CR					0x0100
+#define REG_PBP					0x0104
+#define REG_PKT_BUFF_ACCESS_CTRL		0x0106
+#define REG_TRXDMA_CTRL				0x010C
+#define REG_TRXFF_BNDY				0x0114
+#define REG_TRXFF_STATUS			0x0118
+#define REG_RXFF_PTR				0x011C
+
+#define REG_CPWM				0x012F
+#define REG_FWIMR				0x0130
+#define REG_FWISR				0x0134
+#define REG_PKTBUF_DBG_CTRL			0x0140
+#define REG_PKTBUF_DBG_DATA_L			0x0144
+#define REG_PKTBUF_DBG_DATA_H			0x0148
+#define REG_RXPKTBUF_CTRL			(REG_PKTBUF_DBG_CTRL + 2)
+
+#define REG_TC0_CTRL				0x0150
+#define REG_TC1_CTRL				0x0154
+#define REG_TC2_CTRL				0x0158
+#define REG_TC3_CTRL				0x015C
+#define REG_TC4_CTRL				0x0160
+#define REG_TCUNIT_BASE				0x0164
+#define REG_MBIST_START				0x0174
+#define REG_MBIST_DONE				0x0178
+#define REG_MBIST_FAIL				0x017C
+#define REG_32K_CTRL				0x0194
+#define REG_C2HEVT_MSG_NORMAL			0x01A0
+#define REG_C2HEVT_CLEAR			0x01AF
+#define REG_C2HEVT_MSG_TEST			0x01B8
+#define REG_MCUTST_1				0x01c0
+#define REG_FMETHR				0x01C8
+#define REG_HMETFR				0x01CC
+#define REG_HMEBOX_0				0x01D0
+#define REG_HMEBOX_1				0x01D4
+#define REG_HMEBOX_2				0x01D8
+#define REG_HMEBOX_3				0x01DC
+
+#define REG_LLT_INIT				0x01E0
+#define REG_BB_ACCEESS_CTRL			0x01E8
+#define REG_BB_ACCESS_DATA			0x01EC
+
+#define REG_HMEBOX_EXT_0			0x01F0
+#define REG_HMEBOX_EXT_1			0x01F4
+#define REG_HMEBOX_EXT_2			0x01F8
+#define REG_HMEBOX_EXT_3			0x01FC
+
+#define REG_RQPN				0x0200
+#define REG_FIFOPAGE				0x0204
+#define REG_TDECTRL				0x0208
+#define REG_TXDMA_OFFSET_CHK			0x020C
+#define REG_TXDMA_STATUS			0x0210
+#define REG_RQPN_NPQ				0x0214
+
+#define REG_RXDMA_AGG_PG_TH			0x0280
+/* FW shall update this register before FW write RXPKT_RELEASE_POLL to 1 */
+#define REG_FW_UPD_RDPTR			0x0284
+/* Control the RX DMA.*/
+#define REG_RXDMA_CONTROL			0x0286
+/* The number of packets in RXPKTBUF.	*/
+#define REG_RXPKT_NUM				0x0287
+
+#define	REG_PCIE_CTRL_REG			0x0300
+#define	REG_INT_MIG				0x0304
+#define	REG_BCNQ_DESA				0x0308
+#define	REG_HQ_DESA				0x0310
+#define	REG_MGQ_DESA				0x0318
+#define	REG_VOQ_DESA				0x0320
+#define	REG_VIQ_DESA				0x0328
+#define	REG_BEQ_DESA				0x0330
+#define	REG_BKQ_DESA				0x0338
+#define	REG_RX_DESA				0x0340
+
+#define	REG_DBI					0x0348
+#define	REG_MDIO				0x0354
+#define	REG_DBG_SEL				0x0360
+#define	REG_PCIE_HRPWM				0x0361
+#define	REG_PCIE_HCPWM				0x0363
+#define	REG_UART_CTRL				0x0364
+#define	REG_WATCH_DOG				0x0368
+#define	REG_UART_TX_DESA			0x0370
+#define	REG_UART_RX_DESA			0x0378
+
+
+#define	REG_HDAQ_DESA_NODEF			0x0000
+#define	REG_CMDQ_DESA_NODEF			0x0000
+
+#define REG_VOQ_INFORMATION			0x0400
+#define REG_VIQ_INFORMATION			0x0404
+#define REG_BEQ_INFORMATION			0x0408
+#define REG_BKQ_INFORMATION			0x040C
+#define REG_MGQ_INFORMATION			0x0410
+#define REG_HGQ_INFORMATION			0x0414
+#define REG_BCNQ_INFORMATION			0x0418
+#define REG_TXPKT_EMPTY				0x041A
+
+
+#define REG_CPU_MGQ_INFORMATION			0x041C
+#define REG_FWHW_TXQ_CTRL			0x0420
+#define REG_HWSEQ_CTRL				0x0423
+#define REG_TXPKTBUF_BCNQ_BDNY			0x0424
+#define REG_TXPKTBUF_MGQ_BDNY			0x0425
+#define REG_MULTI_BCNQ_EN			0x0426
+#define REG_MULTI_BCNQ_OFFSET			0x0427
+#define REG_SPEC_SIFS				0x0428
+#define REG_RL					0x042A
+#define REG_DARFRC				0x0430
+#define REG_RARFRC				0x0438
+#define REG_RRSR				0x0440
+#define REG_ARFR0				0x0444
+#define REG_ARFR1				0x0448
+#define REG_ARFR2				0x044C
+#define REG_ARFR3				0x0450
+#define REG_AMPDU_MAX_TIME			0x0456
+#define REG_AGGLEN_LMT				0x0458
+#define REG_AMPDU_MIN_SPACE			0x045C
+#define REG_TXPKTBUF_WMAC_LBK_BF_HD		0x045D
+#define REG_FAST_EDCA_CTRL			0x0460
+#define REG_RD_RESP_PKT_TH			0x0463
+#define REG_INIRTS_RATE_SEL			0x0480
+#define REG_INIDATA_RATE_SEL			0x0484
+#define REG_POWER_STATUS			0x04A4
+#define REG_POWER_STAGE1			0x04B4
+#define REG_POWER_STAGE2			0x04B8
+#define REG_PKT_LIFE_TIME			0x04C0
+#define REG_STBC_SETTING			0x04C4
+#define REG_PROT_MODE_CTRL			0x04C8
+#define REG_BAR_MODE_CTRL			0x04CC
+#define REG_RA_TRY_RATE_AGG_LMT			0x04CF
+#define REG_EARLY_MODE_CONTROL			0x04D0
+#define REG_NQOS_SEQ				0x04DC
+#define REG_QOS_SEQ				0x04DE
+#define REG_NEED_CPU_HANDLE			0x04E0
+#define REG_PKT_LOSE_RPT			0x04E1
+#define REG_PTCL_ERR_STATUS			0x04E2
+#define REG_TX_RPT_CTRL				0x04EC
+#define REG_TX_RPT_TIME				0x04F0
+#define REG_DUMMY				0x04FC
+
+#define REG_EDCA_VO_PARAM			0x0500
+#define REG_EDCA_VI_PARAM			0x0504
+#define REG_EDCA_BE_PARAM			0x0508
+#define REG_EDCA_BK_PARAM			0x050C
+#define REG_BCNTCFG				0x0510
+#define REG_PIFS				0x0512
+#define REG_RDG_PIFS				0x0513
+#define REG_SIFS_CTX				0x0514
+#define REG_SIFS_TRX				0x0516
+#define REG_AGGR_BREAK_TIME			0x051A
+#define REG_SLOT				0x051B
+#define REG_TX_PTCL_CTRL			0x0520
+#define REG_TXPAUSE				0x0522
+#define REG_DIS_TXREQ_CLR			0x0523
+#define REG_RD_CTRL				0x0524
+#define REG_TBTT_PROHIBIT			0x0540
+#define REG_RD_NAV_NXT				0x0544
+#define REG_NAV_PROT_LEN			0x0546
+#define REG_BCN_CTRL				0x0550
+#define REG_USTIME_TSF				0x0551
+#define REG_MBID_NUM				0x0552
+#define REG_DUAL_TSF_RST			0x0553
+#define REG_BCN_INTERVAL			0x0554
+#define REG_MBSSID_BCN_SPACE			0x0554
+#define REG_DRVERLYINT				0x0558
+#define REG_BCNDMATIM				0x0559
+#define REG_ATIMWND				0x055A
+#define REG_BCN_MAX_ERR				0x055D
+#define REG_RXTSF_OFFSET_CCK			0x055E
+#define REG_RXTSF_OFFSET_OFDM			0x055F
+#define REG_TSFTR				0x0560
+#define REG_INIT_TSFTR				0x0564
+#define REG_SECONDARY_CCA_CTRL			0x0577
+#define REG_PSTIMER				0x0580
+#define REG_TIMER0				0x0584
+#define REG_TIMER1				0x0588
+#define REG_ACMHWCTRL				0x05C0
+#define REG_ACMRSTCTRL				0x05C1
+#define REG_ACMAVG				0x05C2
+#define REG_VO_ADMTIME				0x05C4
+#define REG_VI_ADMTIME				0x05C6
+#define REG_BE_ADMTIME				0x05C8
+#define REG_EDCA_RANDOM_GEN			0x05CC
+#define REG_SCH_TXCMD				0x05D0
+
+#define REG_APSD_CTRL				0x0600
+#define REG_BWOPMODE				0x0603
+#define REG_TCR					0x0604
+#define REG_RCR					0x0608
+#define REG_RX_PKT_LIMIT			0x060C
+#define REG_RX_DLK_TIME				0x060D
+#define REG_RX_DRVINFO_SZ			0x060F
+
+#define REG_MACID				0x0610
+#define REG_BSSID				0x0618
+#define REG_MAR					0x0620
+#define REG_MBIDCAMCFG				0x0628
+
+#define REG_USTIME_EDCA				0x0638
+#define REG_MAC_SPEC_SIFS			0x063A
+#define REG_RESP_SIFS_CCK			0x063C
+#define REG_RESP_SIFS_OFDM			0x063E
+#define REG_ACKTO				0x0640
+#define REG_CTS2TO				0x0641
+#define REG_EIFS				0x0642
+
+#define REG_NAV_CTRL				0x0650
+#define REG_BACAMCMD				0x0654
+#define REG_BACAMCONTENT			0x0658
+#define REG_LBDLY				0x0660
+#define REG_FWDLY				0x0661
+#define REG_RXERR_RPT				0x0664
+#define REG_TRXPTCL_CTL				0x0668
+
+#define REG_CAMCMD				0x0670
+#define REG_CAMWRITE				0x0674
+#define REG_CAMREAD				0x0678
+#define REG_CAMDBG				0x067C
+#define REG_SECCFG				0x0680
+
+#define REG_WOW_CTRL				0x0690
+#define REG_PSSTATUS				0x0691
+#define REG_PS_RX_INFO				0x0692
+#define REG_UAPSD_TID				0x0693
+#define REG_LPNAV_CTRL				0x0694
+#define REG_WKFMCAM_NUM				0x0698
+#define REG_WKFMCAM_RWD				0x069C
+#define REG_RXFLTMAP0				0x06A0
+#define REG_RXFLTMAP1				0x06A2
+#define REG_RXFLTMAP2				0x06A4
+#define REG_BCN_PSR_RPT				0x06A8
+#define REG_CALB32K_CTRL			0x06AC
+#define REG_PKT_MON_CTRL			0x06B4
+#define REG_BT_COEX_TABLE			0x06C0
+#define REG_WMAC_RESP_TXINFO			0x06D8
+
+#define REG_USB_INFO				0xFE17
+#define REG_USB_SPECIAL_OPTION			0xFE55
+#define REG_USB_DMA_AGG_TO			0xFE5B
+#define REG_USB_AGG_TO				0xFE5C
+#define REG_USB_AGG_TH				0xFE5D
+
+#define REG_TEST_USB_TXQS			0xFE48
+#define REG_TEST_SIE_VID			0xFE60
+#define REG_TEST_SIE_PID			0xFE62
+#define REG_TEST_SIE_OPTIONAL			0xFE64
+#define REG_TEST_SIE_CHIRP_K			0xFE65
+#define REG_TEST_SIE_PHY			0xFE66
+#define REG_TEST_SIE_MAC_ADDR			0xFE70
+#define REG_TEST_SIE_STRING			0xFE80
+
+#define REG_NORMAL_SIE_VID			0xFE60
+#define REG_NORMAL_SIE_PID			0xFE62
+#define REG_NORMAL_SIE_OPTIONAL			0xFE64
+#define REG_NORMAL_SIE_EP			0xFE65
+#define REG_NORMAL_SIE_PHY			0xFE68
+#define REG_NORMAL_SIE_MAC_ADDR			0xFE70
+#define REG_NORMAL_SIE_STRING			0xFE80
+
+#define	CR9346					REG_9346CR
+#define	MSR					(REG_CR + 2)
+#define	ISR					REG_HISR
+#define	TSFR					REG_TSFTR
+
+#define	MACIDR0					REG_MACID
+#define	MACIDR4					(REG_MACID + 4)
+
+#define PBP					REG_PBP
+
+#define	IDR0					MACIDR0
+#define	IDR4					MACIDR4
+
+#define	UNUSED_REGISTER				0x1BF
+#define	DCAM					UNUSED_REGISTER
+#define	PSR					UNUSED_REGISTER
+#define BBADDR					UNUSED_REGISTER
+#define	PHYDATAR				UNUSED_REGISTER
+
+#define	INVALID_BBRF_VALUE			0x12345678
+
+#define	MAX_MSS_DENSITY_2T			0x13
+#define	MAX_MSS_DENSITY_1T			0x0A
+
+#define	CMDEEPROM_EN				BIT(5)
+#define	CMDEEPROM_SEL				BIT(4)
+#define	CMD9346CR_9356SEL			BIT(4)
+#define	AUTOLOAD_EEPROM				(CMDEEPROM_EN | CMDEEPROM_SEL)
+#define	AUTOLOAD_EFUSE				CMDEEPROM_EN
+
+#define	GPIOSEL_GPIO				0
+#define	GPIOSEL_ENBT				BIT(5)
+
+#define	GPIO_IN					REG_GPIO_PIN_CTRL
+#define	GPIO_OUT				(REG_GPIO_PIN_CTRL + 1)
+#define	GPIO_IO_SEL				(REG_GPIO_PIN_CTRL + 2)
+#define	GPIO_MOD				(REG_GPIO_PIN_CTRL + 3)
+
+/* 8723/8188E Host System Interrupt Mask Register (offset 0x58, 32 byte) */
+#define	HSIMR_GPIO12_0_INT_EN			BIT(0)
+#define	HSIMR_SPS_OCP_INT_EN			BIT(5)
+#define	HSIMR_RON_INT_EN			BIT(6)
+#define	HSIMR_PDN_INT_EN			BIT(7)
+#define	HSIMR_GPIO9_INT_EN			BIT(25)
+
+/* 8723/8188E Host System Interrupt Status Register (offset 0x5C, 32 byte) */
+
+#define	HSISR_GPIO12_0_INT			BIT(0)
+#define	HSISR_SPS_OCP_INT			BIT(5)
+#define	HSISR_RON_INT_EN			BIT(6)
+#define	HSISR_PDNINT				BIT(7)
+#define	HSISR_GPIO9_INT				BIT(25)
+
+#define	MSR_NOLINK				0x00
+#define	MSR_ADHOC				0x01
+#define	MSR_INFRA				0x02
+#define	MSR_AP					0x03
+
+#define	RRSR_RSC_OFFSET				21
+#define	RRSR_SHORT_OFFSET			23
+#define	RRSR_RSC_BW_40M				0x600000
+#define	RRSR_RSC_UPSUBCHNL			0x400000
+#define	RRSR_RSC_LOWSUBCHNL			0x200000
+#define	RRSR_SHORT				0x800000
+#define	RRSR_1M					BIT(0)
+#define	RRSR_2M					BIT(1)
+#define	RRSR_5_5M				BIT(2)
+#define	RRSR_11M				BIT(3)
+#define	RRSR_6M					BIT(4)
+#define	RRSR_9M					BIT(5)
+#define	RRSR_12M				BIT(6)
+#define	RRSR_18M				BIT(7)
+#define	RRSR_24M				BIT(8)
+#define	RRSR_36M				BIT(9)
+#define	RRSR_48M				BIT(10)
+#define	RRSR_54M				BIT(11)
+#define	RRSR_MCS0				BIT(12)
+#define	RRSR_MCS1				BIT(13)
+#define	RRSR_MCS2				BIT(14)
+#define	RRSR_MCS3				BIT(15)
+#define	RRSR_MCS4				BIT(16)
+#define	RRSR_MCS5				BIT(17)
+#define	RRSR_MCS6				BIT(18)
+#define	RRSR_MCS7				BIT(19)
+#define	BRSR_ACKSHORTPMB			BIT(23)
+
+#define	RATR_1M					0x00000001
+#define	RATR_2M					0x00000002
+#define	RATR_55M				0x00000004
+#define	RATR_11M				0x00000008
+#define	RATR_6M					0x00000010
+#define	RATR_9M					0x00000020
+#define	RATR_12M				0x00000040
+#define	RATR_18M				0x00000080
+#define	RATR_24M				0x00000100
+#define	RATR_36M				0x00000200
+#define	RATR_48M				0x00000400
+#define	RATR_54M				0x00000800
+#define	RATR_MCS0				0x00001000
+#define	RATR_MCS1				0x00002000
+#define	RATR_MCS2				0x00004000
+#define	RATR_MCS3				0x00008000
+#define	RATR_MCS4				0x00010000
+#define	RATR_MCS5				0x00020000
+#define	RATR_MCS6				0x00040000
+#define	RATR_MCS7				0x00080000
+#define	RATR_MCS8				0x00100000
+#define	RATR_MCS9				0x00200000
+#define	RATR_MCS10				0x00400000
+#define	RATR_MCS11				0x00800000
+#define	RATR_MCS12				0x01000000
+#define	RATR_MCS13				0x02000000
+#define	RATR_MCS14				0x04000000
+#define	RATR_MCS15				0x08000000
+
+#define RATE_1M					BIT(0)
+#define RATE_2M					BIT(1)
+#define RATE_5_5M				BIT(2)
+#define RATE_11M				BIT(3)
+#define RATE_6M					BIT(4)
+#define RATE_9M					BIT(5)
+#define RATE_12M				BIT(6)
+#define RATE_18M				BIT(7)
+#define RATE_24M				BIT(8)
+#define RATE_36M				BIT(9)
+#define RATE_48M				BIT(10)
+#define RATE_54M				BIT(11)
+#define RATE_MCS0				BIT(12)
+#define RATE_MCS1				BIT(13)
+#define RATE_MCS2				BIT(14)
+#define RATE_MCS3				BIT(15)
+#define RATE_MCS4				BIT(16)
+#define RATE_MCS5				BIT(17)
+#define RATE_MCS6				BIT(18)
+#define RATE_MCS7				BIT(19)
+#define RATE_MCS8				BIT(20)
+#define RATE_MCS9				BIT(21)
+#define RATE_MCS10				BIT(22)
+#define RATE_MCS11				BIT(23)
+#define RATE_MCS12				BIT(24)
+#define RATE_MCS13				BIT(25)
+#define RATE_MCS14				BIT(26)
+#define RATE_MCS15				BIT(27)
+
+#define	RATE_ALL_CCK		(RATR_1M | RATR_2M | RATR_55M | RATR_11M)
+#define	RATE_ALL_OFDM_AG	(RATR_6M | RATR_9M | RATR_12M | RATR_18M |\
+				 RATR_24M | RATR_36M | RATR_48M | RATR_54M)
+#define	RATE_ALL_OFDM_1SS	(RATR_MCS0 | RATR_MCS1 | RATR_MCS2 |\
+				 RATR_MCS3 | RATR_MCS4 | RATR_MCS5 |\
+				 RATR_MCS6 | RATR_MCS7)
+#define	RATE_ALL_OFDM_2SS	(RATR_MCS8 | RATR_MCS9 | RATR_MCS10 |\
+				 RATR_MCS11 | RATR_MCS12 | RATR_MCS13 |\
+				 RATR_MCS14 | RATR_MCS15)
+
+#define	BW_OPMODE_20MHZ				BIT(2)
+#define	BW_OPMODE_5G				BIT(1)
+#define	BW_OPMODE_11J				BIT(0)
+
+#define	CAM_VALID				BIT(15)
+#define	CAM_NOTVALID				0x0000
+#define	CAM_USEDK				BIT(5)
+
+#define	CAM_NONE				0x0
+#define	CAM_WEP40				0x01
+#define	CAM_TKIP				0x02
+#define	CAM_AES					0x04
+#define	CAM_WEP104				0x05
+
+#define	TOTAL_CAM_ENTRY				32
+#define	HALF_CAM_ENTRY				16
+
+#define	CAM_WRITE				BIT(16)
+#define	CAM_READ				0x00000000
+#define	CAM_POLLINIG				BIT(31)
+
+#define	SCR_USEDK				0x01
+#define	SCR_TXSEC_ENABLE			0x02
+#define	SCR_RXSEC_ENABLE			0x04
+
+#define	WOW_PMEN				BIT(0)
+#define	WOW_WOMEN				BIT(1)
+#define	WOW_MAGIC				BIT(2)
+#define	WOW_UWF					BIT(3)
+
+/*********************************************
+*       8723BE IMR/ISR bits
+**********************************************/
+#define	IMR_DISABLED				0x0
+/* IMR DW0(0x0060-0063) Bit 0-31 */
+#define	IMR_TXCCK		BIT(30)	/* TXRPT interrupt when
+					 * CCX bit of the packet is set
+					 */
+#define	IMR_PSTIMEOUT		BIT(29)	/* Power Save Time Out Interrupt */
+#define	IMR_GTINT4		BIT(28)	/* When GTIMER4 expires,
+					 * this bit is set to 1
+					 */
+#define	IMR_GTINT3		BIT(27)	/* When GTIMER3 expires,
+					 * this bit is set to 1
+					 */
+#define	IMR_TBDER		BIT(26)	/* Transmit Beacon0 Error */
+#define	IMR_TBDOK		BIT(25)	/* Transmit Beacon0 OK */
+#define	IMR_TSF_BIT32_TOGGLE	BIT(24)	/* TSF Timer BIT32 toggle
+					 * indication interrupt
+					 */
+#define	IMR_BCNDMAINT0		BIT(20)	/* Beacon DMA Interrupt 0 */
+#define	IMR_BCNDOK0		BIT(16)	/* Beacon Queue DMA OK0 */
+#define	IMR_HSISR_IND_ON_INT	BIT(15)	/* HSISR Indicator (HSIMR & HSISR is
+					 * true, this bit is set to 1)
+					 */
+#define	IMR_BCNDMAINT_E		BIT(14)	/* Beacon DMA Interrupt
+					 * Extension for Win7
+					 */
+#define	IMR_ATIMEND		BIT(12)	/* CTWidnow End or ATIM Window End */
+#define	IMR_HISR1_IND_INT	BIT(11)	/* HISR1 Indicator (HISR1 & HIMR1 is
+					 * true, this bit is set to 1)
+					 */
+#define	IMR_C2HCMD		BIT(10)	/* CPU to Host Command INT Status,
+					 * Write 1 clear
+					 */
+#define	IMR_CPWM2		BIT(9)	/* CPU power Mode exchange INT Status,
+					 * Write 1 clear
+					 */
+#define	IMR_CPWM		BIT(8)	/* CPU power Mode exchange INT Status,
+					 * Write 1 clear
+					 */
+#define	IMR_HIGHDOK		BIT(7)	/* High Queue DMA OK */
+#define	IMR_MGNTDOK		BIT(6)	/* Management Queue DMA OK */
+#define	IMR_BKDOK		BIT(5)	/* AC_BK DMA OK	*/
+#define	IMR_BEDOK		BIT(4)	/* AC_BE DMA OK	*/
+#define	IMR_VIDOK		BIT(3)	/* AC_VI DMA OK	*/
+#define	IMR_VODOK		BIT(2)	/* AC_VO DMA OK	*/
+#define	IMR_RDU			BIT(1)	/* Rx Descriptor Unavailable */
+#define	IMR_ROK			BIT(0)	/* Receive DMA OK */
+
+/* IMR DW1(0x00B4-00B7) Bit 0-31 */
+#define	IMR_BCNDMAINT7		BIT(27)	/* Beacon DMA Interrupt 7 */
+#define	IMR_BCNDMAINT6		BIT(26)	/* Beacon DMA Interrupt 6 */
+#define	IMR_BCNDMAINT5		BIT(25)	/* Beacon DMA Interrupt 5 */
+#define	IMR_BCNDMAINT4		BIT(24)	/* Beacon DMA Interrupt 4 */
+#define	IMR_BCNDMAINT3		BIT(23)	/* Beacon DMA Interrupt 3 */
+#define	IMR_BCNDMAINT2		BIT(22)	/* Beacon DMA Interrupt 2 */
+#define	IMR_BCNDMAINT1		BIT(21)	/* Beacon DMA Interrupt 1 */
+#define	IMR_BCNDOK7		BIT(20)	/* Beacon Queue DMA OK Interrup 7 */
+#define	IMR_BCNDOK6		BIT(19)	/* Beacon Queue DMA OK Interrup 6 */
+#define	IMR_BCNDOK5		BIT(18)	/* Beacon Queue DMA OK Interrup 5 */
+#define	IMR_BCNDOK4		BIT(17)	/* Beacon Queue DMA OK Interrup 4 */
+#define	IMR_BCNDOK3		BIT(16)	/* Beacon Queue DMA OK Interrup 3 */
+#define	IMR_BCNDOK2		BIT(15)	/* Beacon Queue DMA OK Interrup 2 */
+#define	IMR_BCNDOK1		BIT(14)	/* Beacon Queue DMA OK Interrup 1 */
+#define	IMR_ATIMEND_E		BIT(13)	/* ATIM Window End Extension for Win7 */
+#define	IMR_TXERR		BIT(11)	/* Tx Error Flag Interrupt Status,
+					 * write 1 clear.
+					 */
+#define	IMR_RXERR		BIT(10)	/* Rx Error Flag INT Status,
+					 * Write 1 clear
+					 */
+#define	IMR_TXFOVW		BIT(9)	/* Transmit FIFO Overflow */
+#define	IMR_RXFOVW		BIT(8)	/* Receive FIFO Overflow */
+
+#define	HWSET_MAX_SIZE			512
+#define EFUSE_MAX_SECTION		64
+#define EFUSE_REAL_CONTENT_LEN		256
+#define EFUSE_OOB_PROTECT_BYTES		18 /* PG data exclude header,
+					    * dummy 7 bytes frome CP test
+					    * and reserved 1byte.
+					    */
+
+#define	EEPROM_DEFAULT_TSSI			0x0
+#define EEPROM_DEFAULT_TXPOWERDIFF		0x0
+#define EEPROM_DEFAULT_CRYSTALCAP		0x5
+#define EEPROM_DEFAULT_BOARDTYPE		0x02
+#define EEPROM_DEFAULT_TXPOWER			0x1010
+#define	EEPROM_DEFAULT_HT2T_TXPWR		0x10
+
+#define	EEPROM_DEFAULT_LEGACYHTTXPOWERDIFF	0x3
+#define	EEPROM_DEFAULT_THERMALMETER		0x18
+#define	EEPROM_DEFAULT_ANTTXPOWERDIFF		0x0
+#define	EEPROM_DEFAULT_TXPWDIFF_CRYSTALCAP	0x5
+#define	EEPROM_DEFAULT_TXPOWERLEVEL		0x22
+#define	EEPROM_DEFAULT_HT40_2SDIFF		0x0
+#define EEPROM_DEFAULT_HT20_DIFF		2
+#define	EEPROM_DEFAULT_LEGACYHTTXPOWERDIFF	0x3
+#define EEPROM_DEFAULT_HT40_PWRMAXOFFSET	0
+#define EEPROM_DEFAULT_HT20_PWRMAXOFFSET	0
+
+#define RF_OPTION1				0x79
+#define RF_OPTION2				0x7A
+#define RF_OPTION3				0x7B
+#define RF_OPTION4				0xC3
+
+#define EEPROM_DEFAULT_PID			0x1234
+#define EEPROM_DEFAULT_VID			0x5678
+#define EEPROM_DEFAULT_CUSTOMERID		0xAB
+#define EEPROM_DEFAULT_SUBCUSTOMERID		0xCD
+#define EEPROM_DEFAULT_VERSION			0
+
+#define	EEPROM_CHANNEL_PLAN_FCC			0x0
+#define	EEPROM_CHANNEL_PLAN_IC			0x1
+#define	EEPROM_CHANNEL_PLAN_ETSI		0x2
+#define	EEPROM_CHANNEL_PLAN_SPAIN		0x3
+#define	EEPROM_CHANNEL_PLAN_FRANCE		0x4
+#define	EEPROM_CHANNEL_PLAN_MKK			0x5
+#define	EEPROM_CHANNEL_PLAN_MKK1		0x6
+#define	EEPROM_CHANNEL_PLAN_ISRAEL		0x7
+#define	EEPROM_CHANNEL_PLAN_TELEC		0x8
+#define	EEPROM_CHANNEL_PLAN_GLOBAL_DOMAIN	0x9
+#define	EEPROM_CHANNEL_PLAN_WORLD_WIDE_13	0xA
+#define	EEPROM_CHANNEL_PLAN_NCC			0xB
+#define	EEPROM_CHANNEL_PLAN_BY_HW_MASK		0x80
+
+#define EEPROM_CID_DEFAULT			0x0
+#define EEPROM_CID_TOSHIBA			0x4
+#define	EEPROM_CID_CCX				0x10
+#define	EEPROM_CID_QMI				0x0D
+#define EEPROM_CID_WHQL				0xFE
+
+#define	RTL8723BE_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_8723BE			0xB9
+#define	EEPROM_THERMAL_METER_88E		0xBA
+#define	EEPROM_IQK_LCK_88E			0xBB
+
+#define	EEPROM_RF_BOARD_OPTION_88E		0xC1
+#define	EEPROM_RF_FEATURE_OPTION_88E		0xC2
+#define	EEPROM_RF_BT_SETTING_88E		0xC3
+#define	EEPROM_VERSION				0xC4
+#define	EEPROM_CUSTOMER_ID			0xC5
+#define	EEPROM_RF_ANTENNA_OPT_88E		0xC9
+
+#define	EEPROM_MAC_ADDR				0xD0
+#define EEPROM_VID				0xD6
+#define EEPROM_DID				0xD8
+#define EEPROM_SVID				0xDA
+#define EEPROM_SMID				0xDC
+
+#define	STOPBECON				BIT(6)
+#define	STOPHIGHT				BIT(5)
+#define	STOPMGT					BIT(4)
+#define	STOPVO					BIT(3)
+#define	STOPVI					BIT(2)
+#define	STOPBE					BIT(1)
+#define	STOPBK					BIT(0)
+
+#define	RCR_APPFCS				BIT(31)
+#define	RCR_APP_MIC				BIT(30)
+#define	RCR_APP_ICV				BIT(29)
+#define	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 XCLK_VLD			BIT(0)
+#define ACLK_VLD			BIT(1)
+#define UCLK_VLD			BIT(2)
+#define PCLK_VLD			BIT(3)
+#define PCIRSTB				BIT(4)
+#define V15_VLD				BIT(5)
+#define TRP_B15V_EN			BIT(7)
+#define SIC_IDLE			BIT(8)
+#define BD_MAC2				BIT(9)
+#define BD_MAC1				BIT(10)
+#define IC_MACPHY_MODE			BIT(11)
+#define BT_FUNC				BIT(16)
+#define VENDOR_ID			BIT(19)
+#define PAD_HWPD_IDN			BIT(22)
+#define TRP_VAUX_EN			BIT(23)
+#define TRP_BT_EN			BIT(24)
+#define BD_PKG_SEL			BIT(25)
+#define BD_HCI_SEL			BIT(26)
+#define TYPE_ID				BIT(27)
+
+#define USB_IS_HIGH_SPEED		0
+#define USB_IS_FULL_SPEED		1
+#define USB_SPEED_MASK			BIT(5)
+
+#define USB_NORMAL_SIE_EP_MASK		0xF
+#define USB_NORMAL_SIE_EP_SHIFT		4
+
+#define USB_TEST_EP_MASK		0x30
+#define USB_TEST_EP_SHIFT		4
+
+#define USB_AGG_EN			BIT(3)
+
+#define MAC_ADDR_LEN			6
+#define LAST_ENTRY_OF_TX_PKT_BUFFER	175/*255    88e*/
+
+#define POLLING_LLT_THRESHOLD		20
+#define POLLING_READY_TIMEOUT_COUNT	3000
+
+#define	MAX_MSS_DENSITY_2T		0x13
+#define	MAX_MSS_DENSITY_1T		0x0A
+
+#define EPROM_CMD_OPERATING_MODE_MASK	((1<<7)|(1<<6))
+#define EPROM_CMD_CONFIG		0x3
+#define EPROM_CMD_LOAD			1
+
+#define	HWSET_MAX_SIZE_92S		HWSET_MAX_SIZE
+
+#define	HAL_8192C_HW_GPIO_WPS_BIT	BIT(2)
+
+#define	RPMAC_RESET			0x100
+#define	RPMAC_TXSTART			0x104
+#define	RPMAC_TXLEGACYSIG		0x108
+#define	RPMAC_TXHTSIG1			0x10c
+#define	RPMAC_TXHTSIG2			0x110
+#define	RPMAC_PHYDEBUG			0x114
+#define	RPMAC_TXPACKETNUM		0x118
+#define	RPMAC_TXIDLE			0x11c
+#define	RPMAC_TXMACHEADER0		0x120
+#define	RPMAC_TXMACHEADER1		0x124
+#define	RPMAC_TXMACHEADER2		0x128
+#define	RPMAC_TXMACHEADER3		0x12c
+#define	RPMAC_TXMACHEADER4		0x130
+#define	RPMAC_TXMACHEADER5		0x134
+#define	RPMAC_TXDADATYPE		0x138
+#define	RPMAC_TXRANDOMSEED		0x13c
+#define	RPMAC_CCKPLCPPREAMBLE		0x140
+#define	RPMAC_CCKPLCPHEADER		0x144
+#define	RPMAC_CCKCRC16			0x148
+#define	RPMAC_OFDMRXCRC32OK		0x170
+#define	RPMAC_OFDMRXCRC32ER		0x174
+#define	RPMAC_OFDMRXPARITYER		0x178
+#define	RPMAC_OFDMRXCRC8ER		0x17c
+#define	RPMAC_CCKCRXRC16ER		0x180
+#define	RPMAC_CCKCRXRC32ER		0x184
+#define	RPMAC_CCKCRXRC32OK		0x188
+#define	RPMAC_TXSTATUS			0x18c
+
+#define	RFPGA0_RFMOD			0x800
+
+#define	RFPGA0_TXINFO			0x804
+#define	RFPGA0_PSDFUNCTION		0x808
+
+#define	RFPGA0_TXGAINSTAGE		0x80c
+
+#define	RFPGA0_RFTIMING1		0x810
+#define	RFPGA0_RFTIMING2		0x814
+
+#define	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	CONFIG_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	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)
+
+/* 2 EFUSE_TEST (For RTL8723 partially) */
+#define EFUSE_SEL(x)				(((x) & 0x3) << 8)
+#define EFUSE_SEL_MASK				0x300
+#define EFUSE_WIFI_SEL_0			0x0
+
+#define	WL_HWPDN_EN	BIT(0)	/* Enable GPIO[9] as WiFi HW PDn source*/
+#define	WL_HWPDN_SL	BIT(1)	/* WiFi HW PDn polarity control*/
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/rf.c b/drivers/net/wireless/rtlwifi/rtl8723be/rf.c
new file mode 100644
index 000000000000..486294930a7b
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/rf.c
@@ -0,0 +1,504 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "rf.h"
+#include "dm.h"
+
+static bool _rtl8723be_phy_rf6052_config_parafile(struct ieee80211_hw *hw);
+
+void rtl8723be_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]);
+		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]);
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_EMERG,
+			 "unknown bandwidth: %#X\n", bandwidth);
+		break;
+	}
+}
+
+void rtl8723be_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
+					  u8 *ppowerlevel)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u32 tx_agc[2] = {0, 0}, tmpval;
+	bool turbo_scanoff = false;
+	u8 idx1, idx2;
+	u8 *ptr;
+	u8 direction;
+	u32 pwrtrac_value;
+
+	if (rtlefuse->eeprom_regulatory != 0)
+		turbo_scanoff = true;
+
+	if (mac->act_scanning) {
+		tx_agc[RF90_PATH_A] = 0x3f3f3f3f;
+		tx_agc[RF90_PATH_B] = 0x3f3f3f3f;
+
+		if (turbo_scanoff) {
+			for (idx1 = RF90_PATH_A; idx1 <= RF90_PATH_B; idx1++) {
+				tx_agc[idx1] = ppowerlevel[idx1] |
+					       (ppowerlevel[idx1] << 8) |
+					       (ppowerlevel[idx1] << 16) |
+					       (ppowerlevel[idx1] << 24);
+			}
+		}
+	} else {
+		for (idx1 = RF90_PATH_A; idx1 <= RF90_PATH_B; idx1++) {
+			tx_agc[idx1] = ppowerlevel[idx1] |
+				       (ppowerlevel[idx1] << 8) |
+				       (ppowerlevel[idx1] << 16) |
+				       (ppowerlevel[idx1] << 24);
+		}
+		if (rtlefuse->eeprom_regulatory == 0) {
+			tmpval =
+			    (rtlphy->mcs_offset[0][6]) +
+			    (rtlphy->mcs_offset[0][7] << 8);
+			tx_agc[RF90_PATH_A] += tmpval;
+
+			tmpval = (rtlphy->mcs_offset[0][14]) +
+				 (rtlphy->mcs_offset[0][15] <<
+				  24);
+			tx_agc[RF90_PATH_B] += tmpval;
+		}
+	}
+	for (idx1 = RF90_PATH_A; idx1 <= RF90_PATH_B; idx1++) {
+		ptr = (u8 *)(&(tx_agc[idx1]));
+		for (idx2 = 0; idx2 < 4; idx2++) {
+			if (*ptr > RF6052_MAX_TX_PWR)
+				*ptr = RF6052_MAX_TX_PWR;
+			ptr++;
+		}
+	}
+	rtl8723be_dm_txpower_track_adjust(hw, 1, &direction, &pwrtrac_value);
+	if (direction == 1) {
+		tx_agc[0] += pwrtrac_value;
+		tx_agc[1] += pwrtrac_value;
+	} else if (direction == 2) {
+		tx_agc[0] -= pwrtrac_value;
+		tx_agc[1] -= pwrtrac_value;
+	}
+	tmpval = tx_agc[RF90_PATH_A] & 0xff;
+	rtl_set_bbreg(hw, RTXAGC_A_CCK1_MCS32, MASKBYTE1, tmpval);
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "CCK PWR 1M (rf-A) = 0x%x (reg 0x%x)\n", tmpval,
+		 RTXAGC_A_CCK1_MCS32);
+
+	tmpval = tx_agc[RF90_PATH_A] >> 8;
+
+	rtl_set_bbreg(hw, RTXAGC_B_CCK11_A_CCK2_11, 0xffffff00, tmpval);
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "CCK PWR 2~11M (rf-A) = 0x%x (reg 0x%x)\n", tmpval,
+		 RTXAGC_B_CCK11_A_CCK2_11);
+
+	tmpval = tx_agc[RF90_PATH_B] >> 24;
+	rtl_set_bbreg(hw, RTXAGC_B_CCK11_A_CCK2_11, MASKBYTE0, tmpval);
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "CCK PWR 11M (rf-B) = 0x%x (reg 0x%x)\n", tmpval,
+		 RTXAGC_B_CCK11_A_CCK2_11);
+
+	tmpval = tx_agc[RF90_PATH_B] & 0x00ffffff;
+	rtl_set_bbreg(hw, RTXAGC_B_CCK1_55_MCS32, 0xffffff00, tmpval);
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "CCK PWR 1~5.5M (rf-B) = 0x%x (reg 0x%x)\n", tmpval,
+		 RTXAGC_B_CCK1_55_MCS32);
+}
+
+static void rtl8723be_phy_get_power_base(struct ieee80211_hw *hw,
+					 u8 *ppowerlevel_ofdm,
+					 u8 *ppowerlevel_bw20,
+					 u8 *ppowerlevel_bw40,
+					 u8 channel, u32 *ofdmbase,
+					 u32 *mcsbase)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u32 powerbase0, powerbase1;
+	u8 i, powerlevel[2];
+
+	for (i = 0; i < 2; i++) {
+		powerbase0 = ppowerlevel_ofdm[i];
+
+		powerbase0 = (powerbase0 << 24) | (powerbase0 << 16) |
+		    (powerbase0 << 8) | powerbase0;
+		*(ofdmbase + i) = powerbase0;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 " [OFDM power base index rf(%c) = 0x%x]\n",
+			 ((i == 0) ? 'A' : 'B'), *(ofdmbase + i));
+	}
+
+	for (i = 0; i < 2; i++) {
+		if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20)
+			powerlevel[i] = ppowerlevel_bw20[i];
+		else
+			powerlevel[i] = ppowerlevel_bw40[i];
+		powerbase1 = powerlevel[i];
+		powerbase1 = (powerbase1 << 24) | (powerbase1 << 16) |
+			     (powerbase1 << 8) | powerbase1;
+
+		*(mcsbase + i) = powerbase1;
+
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			" [MCS power base index rf(%c) = 0x%x]\n",
+			((i == 0) ? 'A' : 'B'), *(mcsbase + i));
+	}
+}
+
+static void txpwr_by_regulatory(struct ieee80211_hw *hw, u8 channel, u8 index,
+				u32 *powerbase0, u32 *powerbase1,
+				u32 *p_outwriteval)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u8 i, chnlgroup = 0, pwr_diff_limit[4];
+	u8 pwr_diff = 0, customer_pwr_diff;
+	u32 writeval, customer_limit, rf;
+
+	for (rf = 0; rf < 2; rf++) {
+		switch (rtlefuse->eeprom_regulatory) {
+		case 0:
+			chnlgroup = 0;
+
+			writeval =
+			    rtlphy->mcs_offset[chnlgroup][index + (rf ? 8 : 0)]
+			    + ((index < 2) ? powerbase0[rf] : powerbase1[rf]);
+
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				 "RTK better performance, "
+				 "writeval(%c) = 0x%x\n",
+				 ((rf == 0) ? 'A' : 'B'), writeval);
+			break;
+		case 1:
+			if (rtlphy->pwrgroup_cnt == 1) {
+				chnlgroup = 0;
+			} else {
+				if (channel < 3)
+					chnlgroup = 0;
+				else if (channel < 6)
+					chnlgroup = 1;
+				else if (channel < 9)
+					chnlgroup = 2;
+				else if (channel < 12)
+					chnlgroup = 3;
+				else if (channel < 14)
+					chnlgroup = 4;
+				else if (channel == 14)
+					chnlgroup = 5;
+			}
+			writeval = rtlphy->mcs_offset[chnlgroup]
+			    [index + (rf ? 8 : 0)] + ((index < 2) ?
+						      powerbase0[rf] :
+						      powerbase1[rf]);
+
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				 "Realtek regulatory, 20MHz, "
+				 "writeval(%c) = 0x%x\n",
+				 ((rf == 0) ? 'A' : 'B'), writeval);
+
+			break;
+		case 2:
+			writeval =
+			    ((index < 2) ? powerbase0[rf] : powerbase1[rf]);
+
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				 "Better regulatory, "
+				 "writeval(%c) = 0x%x\n",
+				 ((rf == 0) ? 'A' : 'B'), writeval);
+			break;
+		case 3:
+			chnlgroup = 0;
+
+			if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40) {
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+					 "customer's limit, 40MHz "
+					 "rf(%c) = 0x%x\n",
+					 ((rf == 0) ? 'A' : 'B'),
+					 rtlefuse->pwrgroup_ht40[rf]
+					 [channel-1]);
+			} else {
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+					 "customer's limit, 20MHz "
+					 "rf(%c) = 0x%x\n",
+					 ((rf == 0) ? 'A' : 'B'),
+					 rtlefuse->pwrgroup_ht20[rf]
+					 [channel-1]);
+			}
+
+			if (index < 2)
+				pwr_diff =
+				    rtlefuse->txpwr_legacyhtdiff[rf][channel-1];
+			else if (rtlphy->current_chan_bw ==
+				 HT_CHANNEL_WIDTH_20)
+				pwr_diff =
+				    rtlefuse->txpwr_ht20diff[rf][channel-1];
+
+			if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40)
+				customer_pwr_diff =
+					rtlefuse->pwrgroup_ht40[rf][channel-1];
+			else
+				customer_pwr_diff =
+					rtlefuse->pwrgroup_ht20[rf][channel-1];
+
+			if (pwr_diff > customer_pwr_diff)
+				pwr_diff = 0;
+			else
+				pwr_diff = customer_pwr_diff - pwr_diff;
+
+			for (i = 0; i < 4; i++) {
+				pwr_diff_limit[i] =
+				    (u8)((rtlphy->mcs_offset
+					  [chnlgroup][index + (rf ? 8 : 0)] &
+					  (0x7f << (i * 8))) >> (i * 8));
+
+					if (pwr_diff_limit[i] > pwr_diff)
+						pwr_diff_limit[i] = pwr_diff;
+			}
+
+			customer_limit = (pwr_diff_limit[3] << 24) |
+					 (pwr_diff_limit[2] << 16) |
+					 (pwr_diff_limit[1] << 8) |
+					 (pwr_diff_limit[0]);
+
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				"Customer's limit rf(%c) = 0x%x\n",
+				((rf == 0) ? 'A' : 'B'), customer_limit);
+
+			writeval = customer_limit + ((index < 2) ?
+						      powerbase0[rf] :
+						      powerbase1[rf]);
+
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				 "Customer, writeval rf(%c)= 0x%x\n",
+				 ((rf == 0) ? 'A' : 'B'), writeval);
+			break;
+		default:
+			chnlgroup = 0;
+			writeval =
+			    rtlphy->mcs_offset[chnlgroup]
+			    [index + (rf ? 8 : 0)]
+			    + ((index < 2) ? powerbase0[rf] : powerbase1[rf]);
+
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				 "RTK better performance, writeval "
+				 "rf(%c) = 0x%x\n",
+				 ((rf == 0) ? 'A' : 'B'), writeval);
+			break;
+		}
+
+		if (rtlpriv->dm.dynamic_txhighpower_lvl == TXHIGHPWRLEVEL_BT1)
+			writeval = writeval - 0x06060606;
+		else if (rtlpriv->dm.dynamic_txhighpower_lvl ==
+			 TXHIGHPWRLEVEL_BT2)
+			writeval = writeval - 0x0c0c0c0c;
+		*(p_outwriteval + rf) = writeval;
+	}
+}
+
+static void _rtl8723be_write_ofdm_power_reg(struct ieee80211_hw *hw,
+					 u8 index, u32 *value)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u16 regoffset_a[6] = {
+		RTXAGC_A_RATE18_06, RTXAGC_A_RATE54_24,
+		RTXAGC_A_MCS03_MCS00, RTXAGC_A_MCS07_MCS04,
+		RTXAGC_A_MCS11_MCS08, RTXAGC_A_MCS15_MCS12
+	};
+	u16 regoffset_b[6] = {
+		RTXAGC_B_RATE18_06, RTXAGC_B_RATE54_24,
+		RTXAGC_B_MCS03_MCS00, RTXAGC_B_MCS07_MCS04,
+		RTXAGC_B_MCS11_MCS08, RTXAGC_B_MCS15_MCS12
+	};
+	u8 i, rf, pwr_val[4];
+	u32 writeval;
+	u16 regoffset;
+
+	for (rf = 0; rf < 2; rf++) {
+		writeval = value[rf];
+		for (i = 0; i < 4; i++) {
+			pwr_val[i] = (u8) ((writeval & (0x7f <<
+							(i * 8))) >> (i * 8));
+
+			if (pwr_val[i] > RF6052_MAX_TX_PWR)
+				pwr_val[i] = RF6052_MAX_TX_PWR;
+		}
+		writeval = (pwr_val[3] << 24) | (pwr_val[2] << 16) |
+		    (pwr_val[1] << 8) | pwr_val[0];
+
+		if (rf == 0)
+			regoffset = regoffset_a[index];
+		else
+			regoffset = regoffset_b[index];
+		rtl_set_bbreg(hw, regoffset, MASKDWORD, writeval);
+
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Set 0x%x = %08x\n", regoffset, writeval);
+	}
+}
+
+void rtl8723be_phy_rf6052_set_ofdm_txpower(struct ieee80211_hw *hw,
+					   u8 *ppowerlevel_ofdm,
+					   u8 *ppowerlevel_bw20,
+					   u8 *ppowerlevel_bw40, u8 channel)
+{
+	u32 writeval[2], powerbase0[2], powerbase1[2];
+	u8 index;
+	u8 direction;
+	u32 pwrtrac_value;
+
+	rtl8723be_phy_get_power_base(hw, ppowerlevel_ofdm, ppowerlevel_bw20,
+				     ppowerlevel_bw40, channel,
+				     &powerbase0[0], &powerbase1[0]);
+
+	rtl8723be_dm_txpower_track_adjust(hw, 1, &direction, &pwrtrac_value);
+
+	for (index = 0; index < 6; index++) {
+		txpwr_by_regulatory(hw, channel, index, &powerbase0[0],
+				    &powerbase1[0], &writeval[0]);
+		if (direction == 1) {
+			writeval[0] += pwrtrac_value;
+			writeval[1] += pwrtrac_value;
+		} else if (direction == 2) {
+			writeval[0] -= pwrtrac_value;
+			writeval[1] -= pwrtrac_value;
+		}
+		_rtl8723be_write_ofdm_power_reg(hw, index, &writeval[0]);
+	}
+}
+
+bool rtl8723be_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 _rtl8723be_phy_rf6052_config_parafile(hw);
+}
+
+static bool _rtl8723be_phy_rf6052_config_parafile(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct bb_reg_def *pphyreg;
+	u32 u4_regvalue = 0;
+	u8 rfpath;
+	bool rtstatus = true;
+
+	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 = rtl8723be_phy_config_rf_with_headerfile(hw,
+						      (enum radio_path)rfpath);
+			break;
+		case RF90_PATH_B:
+			rtstatus = rtl8723be_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(rtlpriv, COMP_INIT, DBG_TRACE,
+				 "Radio[%d] Fail!!", rfpath);
+			return false;
+		}
+	}
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "\n");
+	return rtstatus;
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/rf.h b/drivers/net/wireless/rtlwifi/rtl8723be/rf.h
new file mode 100644
index 000000000000..a6fea106ced4
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/rf.h
@@ -0,0 +1,43 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL8723BE_RF_H__
+#define __RTL8723BE_RF_H__
+
+#define RF6052_MAX_TX_PWR		0x3F
+#define RF6052_MAX_REG			0x3F
+
+void rtl8723be_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw,
+					u8 bandwidth);
+void rtl8723be_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
+					  u8 *ppowerlevel);
+void rtl8723be_phy_rf6052_set_ofdm_txpower(struct ieee80211_hw *hw,
+					   u8 *ppowerlevel_ofdm,
+					   u8 *ppowerlevel_bw20,
+					   u8 *ppowerlevel_bw40,
+					   u8 channel);
+bool rtl8723be_phy_rf6052_config(struct ieee80211_hw *hw);
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/sw.c b/drivers/net/wireless/rtlwifi/rtl8723be/sw.c
new file mode 100644
index 000000000000..b4577ebc4bb0
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/sw.c
@@ -0,0 +1,384 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../core.h"
+#include "../pci.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "../rtl8723com/phy_common.h"
+#include "dm.h"
+#include "hw.h"
+#include "fw.h"
+#include "../rtl8723com/fw_common.h"
+#include "sw.h"
+#include "trx.h"
+#include "led.h"
+#include "table.h"
+#include "../btcoexist/rtl_btc.h"
+
+#include <linux/vmalloc.h>
+#include <linux/module.h>
+
+static void rtl8723be_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 rtl8723be_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));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+
+	rtl8723be_bt_reg_init(hw);
+	rtlpci->msi_support = true;
+	rtlpriv->btcoexist.btc_ops = rtl_btc_get_ops_pointer();
+
+	rtlpriv->dm.dm_initialgain_enable = 1;
+	rtlpriv->dm.dm_flag = 0;
+	rtlpriv->dm.disable_framebursting = 0;
+	rtlpriv->dm.thermalvalue = 0;
+	rtlpci->transmit_config = CFENDFORM | BIT(15) | BIT(24) | BIT(25);
+
+	mac->ht_enable = true;
+
+	/* compatible 5G band 88ce just 2.4G band & smsp */
+	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_AB		|
+				  RCR_AM		|
+				  RCR_APM		|
+				  0);
+
+	rtlpci->irq_mask[0] = (u32) (IMR_PSTIMEOUT	|
+				     IMR_HSISR_IND_ON_INT	|
+				     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.inactiveps = rtlpriv->cfg->mod_params->inactiveps;
+	rtlpriv->psc.swctrl_lps = rtlpriv->cfg->mod_params->swctrl_lps;
+	rtlpriv->psc.fwctrl_lps = rtlpriv->cfg->mod_params->fwctrl_lps;
+	rtlpriv->psc.reg_fwctrl_lps = 3;
+	rtlpriv->psc.reg_max_lps_awakeintvl = 5;
+	/* for ASPM, you can close aspm through
+	 * set const_support_pciaspm = 0
+	 */
+	rtl8723be_init_aspm_vars(hw);
+
+	if (rtlpriv->psc.reg_fwctrl_lps == 1)
+		rtlpriv->psc.fwctrl_psmode = FW_PS_MIN_MODE;
+	else if (rtlpriv->psc.reg_fwctrl_lps == 2)
+		rtlpriv->psc.fwctrl_psmode = FW_PS_MAX_MODE;
+	else if (rtlpriv->psc.reg_fwctrl_lps == 3)
+		rtlpriv->psc.fwctrl_psmode = FW_PS_DTIM_MODE;
+
+	/* for firmware buf */
+	rtlpriv->rtlhal.pfirmware = vzalloc(0x8000);
+	if (!rtlpriv->rtlhal.pfirmware) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "Can't alloc buffer for fw.\n");
+		return 1;
+	}
+
+	rtlpriv->max_fw_size = 0x8000;
+	pr_info("Using firmware %s\n", rtlpriv->cfg->fw_name);
+	err = request_firmware_nowait(THIS_MODULE, 1, rtlpriv->cfg->fw_name,
+				      rtlpriv->io.dev, GFP_KERNEL, hw,
+				      rtl_fw_cb);
+	if (err) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "Failed to request firmware!\n");
+		return 1;
+	}
+	return 0;
+}
+
+void rtl8723be_deinit_sw_vars(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (rtlpriv->cfg->ops->get_btc_status())
+		rtlpriv->btcoexist.btc_ops->btc_halt_notify();
+	if (rtlpriv->rtlhal.pfirmware) {
+		vfree(rtlpriv->rtlhal.pfirmware);
+		rtlpriv->rtlhal.pfirmware = NULL;
+	}
+}
+
+/* get bt coexist status */
+bool rtl8723be_get_btc_status(void)
+{
+	return true;
+}
+
+static bool is_fw_header(struct rtl92c_firmware_header *hdr)
+{
+	return (hdr->signature & 0xfff0) == 0x5300;
+}
+
+static struct rtl_hal_ops rtl8723be_hal_ops = {
+	.init_sw_vars = rtl8723be_init_sw_vars,
+	.deinit_sw_vars = rtl8723be_deinit_sw_vars,
+	.read_eeprom_info = rtl8723be_read_eeprom_info,
+	.interrupt_recognized = rtl8723be_interrupt_recognized,
+	.hw_init = rtl8723be_hw_init,
+	.hw_disable = rtl8723be_card_disable,
+	.hw_suspend = rtl8723be_suspend,
+	.hw_resume = rtl8723be_resume,
+	.enable_interrupt = rtl8723be_enable_interrupt,
+	.disable_interrupt = rtl8723be_disable_interrupt,
+	.set_network_type = rtl8723be_set_network_type,
+	.set_chk_bssid = rtl8723be_set_check_bssid,
+	.set_qos = rtl8723be_set_qos,
+	.set_bcn_reg = rtl8723be_set_beacon_related_registers,
+	.set_bcn_intv = rtl8723be_set_beacon_interval,
+	.update_interrupt_mask = rtl8723be_update_interrupt_mask,
+	.get_hw_reg = rtl8723be_get_hw_reg,
+	.set_hw_reg = rtl8723be_set_hw_reg,
+	.update_rate_tbl = rtl8723be_update_hal_rate_tbl,
+	.fill_tx_desc = rtl8723be_tx_fill_desc,
+	.fill_tx_cmddesc = rtl8723be_tx_fill_cmddesc,
+	.query_rx_desc = rtl8723be_rx_query_desc,
+	.set_channel_access = rtl8723be_update_channel_access_setting,
+	.radio_onoff_checking = rtl8723be_gpio_radio_on_off_checking,
+	.set_bw_mode = rtl8723be_phy_set_bw_mode,
+	.switch_channel = rtl8723be_phy_sw_chnl,
+	.dm_watchdog = rtl8723be_dm_watchdog,
+	.scan_operation_backup = rtl8723be_phy_scan_operation_backup,
+	.set_rf_power_state = rtl8723be_phy_set_rf_power_state,
+	.led_control = rtl8723be_led_control,
+	.set_desc = rtl8723be_set_desc,
+	.get_desc = rtl8723be_get_desc,
+	.is_tx_desc_closed = rtl8723be_is_tx_desc_closed,
+	.tx_polling = rtl8723be_tx_polling,
+	.enable_hw_sec = rtl8723be_enable_hw_security_config,
+	.set_key = rtl8723be_set_key,
+	.init_sw_leds = rtl8723be_init_sw_leds,
+	.get_bbreg = rtl8723_phy_query_bb_reg,
+	.set_bbreg = rtl8723_phy_set_bb_reg,
+	.get_rfreg = rtl8723be_phy_query_rf_reg,
+	.set_rfreg = rtl8723be_phy_set_rf_reg,
+	.fill_h2c_cmd = rtl8723be_fill_h2c_cmd,
+	.get_btc_status = rtl8723be_get_btc_status,
+	.is_fw_header = is_fw_header,
+};
+
+static struct rtl_mod_params rtl8723be_mod_params = {
+	.sw_crypto = false,
+	.inactiveps = true,
+	.swctrl_lps = false,
+	.fwctrl_lps = true,
+	.debug = DBG_EMERG,
+};
+
+static struct rtl_hal_cfg rtl8723be_hal_cfg = {
+	.bar_id = 2,
+	.write_readback = true,
+	.name = "rtl8723be_pci",
+	.fw_name = "rtlwifi/rtl8723befw.bin",
+	.ops = &rtl8723be_hal_ops,
+	.mod_params = &rtl8723be_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[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_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_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 DEFINE_PCI_DEVICE_TABLE(rtl8723be_pci_id) = {
+	{RTL_PCI_DEVICE(PCI_VENDOR_ID_REALTEK, 0xb723, rtl8723be_hal_cfg)},
+	{},
+};
+
+MODULE_DEVICE_TABLE(pci, rtl8723be_pci_id);
+
+MODULE_AUTHOR("PageHe	<page_he@realsil.com.cn>");
+MODULE_AUTHOR("Realtek WlanFAE	<wlanfae@realtek.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Realtek 8723BE 802.11n PCI wireless");
+MODULE_FIRMWARE("rtlwifi/rtl8723befw.bin");
+
+module_param_named(swenc, rtl8723be_mod_params.sw_crypto, bool, 0444);
+module_param_named(debug, rtl8723be_mod_params.debug, int, 0444);
+module_param_named(ips, rtl8723be_mod_params.inactiveps, bool, 0444);
+module_param_named(swlps, rtl8723be_mod_params.swctrl_lps, bool, 0444);
+module_param_named(fwlps, rtl8723be_mod_params.fwctrl_lps, bool, 0444);
+MODULE_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, rtl_pci_suspend, rtl_pci_resume);
+
+static struct pci_driver rtl8723be_driver = {
+	.name = KBUILD_MODNAME,
+	.id_table = rtl8723be_pci_id,
+	.probe = rtl_pci_probe,
+	.remove = rtl_pci_disconnect,
+
+	.driver.pm = &rtlwifi_pm_ops,
+};
+
+module_pci_driver(rtl8723be_driver);
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/sw.h b/drivers/net/wireless/rtlwifi/rtl8723be/sw.h
new file mode 100644
index 000000000000..a7b25e769950
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/sw.h
@@ -0,0 +1,35 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL8723BE_SW_H__
+#define __RTL8723BE_SW_H__
+
+int rtl8723be_init_sw_vars(struct ieee80211_hw *hw);
+void rtl8723be_deinit_sw_vars(struct ieee80211_hw *hw);
+void rtl8723be_init_var_map(struct ieee80211_hw *hw);
+bool rtl8723be_get_btc_status(void);
+
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/table.c b/drivers/net/wireless/rtlwifi/rtl8723be/table.c
new file mode 100644
index 000000000000..4b283cde042e
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/table.c
@@ -0,0 +1,572 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Created on  2010/ 5/18,  1:41
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "table.h"
+u32 RTL8723BEPHY_REG_1TARRAY[] = {
+	0x800, 0x80040000,
+	0x804, 0x00000003,
+	0x808, 0x0000FC00,
+	0x80C, 0x0000000A,
+	0x810, 0x10001331,
+	0x814, 0x020C3D10,
+	0x818, 0x02200385,
+	0x81C, 0x00000000,
+	0x820, 0x01000100,
+	0x824, 0x00390204,
+	0x828, 0x00000000,
+	0x82C, 0x00000000,
+	0x830, 0x00000000,
+	0x834, 0x00000000,
+	0x838, 0x00000000,
+	0x83C, 0x00000000,
+	0x840, 0x00010000,
+	0x844, 0x00000000,
+	0x848, 0x00000000,
+	0x84C, 0x00000000,
+	0x850, 0x00000000,
+	0x854, 0x00000000,
+	0x858, 0x569A11A9,
+	0x85C, 0x01000014,
+	0x860, 0x66F60110,
+	0x864, 0x061F0649,
+	0x868, 0x00000000,
+	0x86C, 0x27272700,
+	0x870, 0x07000760,
+	0x874, 0x25004000,
+	0x878, 0x00000808,
+	0x87C, 0x00000000,
+	0x880, 0xB0000C1C,
+	0x884, 0x00000001,
+	0x888, 0x00000000,
+	0x88C, 0xCCC000C0,
+	0x890, 0x00000800,
+	0x894, 0xFFFFFFFE,
+	0x898, 0x40302010,
+	0x89C, 0x00706050,
+	0x900, 0x00000000,
+	0x904, 0x00000023,
+	0x908, 0x00000000,
+	0x90C, 0x81121111,
+	0x910, 0x00000002,
+	0x914, 0x00000201,
+	0x948, 0x00000000,
+	0xA00, 0x00D047C8,
+	0xA04, 0x80FF000C,
+	0xA08, 0x8C838300,
+	0xA0C, 0x2E7F120F,
+	0xA10, 0x9500BB78,
+	0xA14, 0x1114D028,
+	0xA18, 0x00881117,
+	0xA1C, 0x89140F00,
+	0xA20, 0x1A1B0000,
+	0xA24, 0x090E1317,
+	0xA28, 0x00000204,
+	0xA2C, 0x00D30000,
+	0xA70, 0x101FBF00,
+	0xA74, 0x00000007,
+	0xA78, 0x00000900,
+	0xA7C, 0x225B0606,
+	0xA80, 0x21806490,
+	0xB2C, 0x00000000,
+	0xC00, 0x48071D40,
+	0xC04, 0x03A05611,
+	0xC08, 0x000000E4,
+	0xC0C, 0x6C6C6C6C,
+	0xC10, 0x08800000,
+	0xC14, 0x40000100,
+	0xC18, 0x08800000,
+	0xC1C, 0x40000100,
+	0xC20, 0x00000000,
+	0xC24, 0x00000000,
+	0xC28, 0x00000000,
+	0xC2C, 0x00000000,
+	0xC30, 0x69E9AC44,
+	0xC34, 0x469652AF,
+	0xC38, 0x49795994,
+	0xC3C, 0x0A97971C,
+	0xC40, 0x1F7C403F,
+	0xC44, 0x000100B7,
+	0xC48, 0xEC020107,
+	0xC4C, 0x007F037F,
+	0xC50, 0x69553420,
+	0xC54, 0x43BC0094,
+	0xC58, 0x00023169,
+	0xC5C, 0x00250492,
+	0xC60, 0x00000000,
+	0xC64, 0x7112848B,
+	0xC68, 0x47C00BFF,
+	0xC6C, 0x00000036,
+	0xC70, 0x2C7F000D,
+	0xC74, 0x020610DB,
+	0xC78, 0x0000001F,
+	0xC7C, 0x00B91612,
+	0xC80, 0x390000E4,
+	0xC84, 0x20F60000,
+	0xC88, 0x40000100,
+	0xC8C, 0x20200000,
+	0xC90, 0x00020E1A,
+	0xC94, 0x00000000,
+	0xC98, 0x00020E1A,
+	0xC9C, 0x00007F7F,
+	0xCA0, 0x00000000,
+	0xCA4, 0x000300A0,
+	0xCA8, 0x00000000,
+	0xCAC, 0x00000000,
+	0xCB0, 0x00000000,
+	0xCB4, 0x00000000,
+	0xCB8, 0x00000000,
+	0xCBC, 0x28000000,
+	0xCC0, 0x00000000,
+	0xCC4, 0x00000000,
+	0xCC8, 0x00000000,
+	0xCCC, 0x00000000,
+	0xCD0, 0x00000000,
+	0xCD4, 0x00000000,
+	0xCD8, 0x64B22427,
+	0xCDC, 0x00766932,
+	0xCE0, 0x00222222,
+	0xCE4, 0x00000000,
+	0xCE8, 0x37644302,
+	0xCEC, 0x2F97D40C,
+	0xD00, 0x00000740,
+	0xD04, 0x40020401,
+	0xD08, 0x0000907F,
+	0xD0C, 0x20010201,
+	0xD10, 0xA0633333,
+	0xD14, 0x3333BC53,
+	0xD18, 0x7A8F5B6F,
+	0xD2C, 0xCC979975,
+	0xD30, 0x00000000,
+	0xD34, 0x80608000,
+	0xD38, 0x00000000,
+	0xD3C, 0x00127353,
+	0xD40, 0x00000000,
+	0xD44, 0x00000000,
+	0xD48, 0x00000000,
+	0xD4C, 0x00000000,
+	0xD50, 0x6437140A,
+	0xD54, 0x00000000,
+	0xD58, 0x00000282,
+	0xD5C, 0x30032064,
+	0xD60, 0x4653DE68,
+	0xD64, 0x04518A3C,
+	0xD68, 0x00002101,
+	0xD6C, 0x2A201C16,
+	0xD70, 0x1812362E,
+	0xD74, 0x322C2220,
+	0xD78, 0x000E3C24,
+	0xE00, 0x2D2D2D2D,
+	0xE04, 0x2D2D2D2D,
+	0xE08, 0x0390272D,
+	0xE10, 0x2D2D2D2D,
+	0xE14, 0x2D2D2D2D,
+	0xE18, 0x2D2D2D2D,
+	0xE1C, 0x2D2D2D2D,
+	0xE28, 0x00000000,
+	0xE30, 0x1000DC1F,
+	0xE34, 0x10008C1F,
+	0xE38, 0x02140102,
+	0xE3C, 0x681604C2,
+	0xE40, 0x01007C00,
+	0xE44, 0x01004800,
+	0xE48, 0xFB000000,
+	0xE4C, 0x000028D1,
+	0xE50, 0x1000DC1F,
+	0xE54, 0x10008C1F,
+	0xE58, 0x02140102,
+	0xE5C, 0x28160D05,
+	0xE60, 0x00000008,
+	0xE68, 0x001B2556,
+	0xE6C, 0x00C00096,
+	0xE70, 0x00C00096,
+	0xE74, 0x01000056,
+	0xE78, 0x01000014,
+	0xE7C, 0x01000056,
+	0xE80, 0x01000014,
+	0xE84, 0x00C00096,
+	0xE88, 0x01000056,
+	0xE8C, 0x00C00096,
+	0xED0, 0x00C00096,
+	0xED4, 0x00C00096,
+	0xED8, 0x00C00096,
+	0xEDC, 0x000000D6,
+	0xEE0, 0x000000D6,
+	0xEEC, 0x01C00016,
+	0xF14, 0x00000003,
+	0xF4C, 0x00000000,
+	0xF00, 0x00000300,
+	0x820, 0x01000100,
+	0x800, 0x83040000,
+};
+
+u32 RTL8723BEPHY_REG_ARRAY_PG[] = {
+	0, 0, 0, 0x00000e08, 0x0000ff00, 0x00004000,
+	0, 0, 0, 0x0000086c, 0xffffff00, 0x34363800,
+	0, 0, 0, 0x00000e00, 0xffffffff, 0x42444646,
+	0, 0, 0, 0x00000e04, 0xffffffff, 0x30343840,
+	0, 0, 0, 0x00000e10, 0xffffffff, 0x38404244,
+	0, 0, 0, 0x00000e14, 0xffffffff, 0x26303436
+};
+
+u32 RTL8723BE_RADIOA_1TARRAY[] = {
+	0x000, 0x00010000,
+	0x0B0, 0x000DFFE0,
+	0x0FE, 0x00000000,
+	0x0FE, 0x00000000,
+	0x0FE, 0x00000000,
+	0x0B1, 0x00000018,
+	0x0FE, 0x00000000,
+	0x0FE, 0x00000000,
+	0x0FE, 0x00000000,
+	0x0B2, 0x00084C00,
+	0x0B5, 0x0000D2CC,
+	0x0B6, 0x000925AA,
+	0x0B7, 0x00000010,
+	0x0B8, 0x0000907F,
+	0x05C, 0x00000002,
+	0x07C, 0x00000002,
+	0x07E, 0x00000005,
+	0x08B, 0x0006FC00,
+	0x0B0, 0x000FF9F0,
+	0x01C, 0x000739D2,
+	0x01E, 0x00000000,
+	0x0DF, 0x00000780,
+	0x050, 0x00067435,
+	0x051, 0x0006B04E,
+	0x052, 0x000007D2,
+	0x053, 0x00000000,
+	0x054, 0x00050400,
+	0x055, 0x0004026E,
+	0x0DD, 0x0000004C,
+	0x070, 0x00067435,
+	0x071, 0x0006B04E,
+	0x072, 0x000007D2,
+	0x073, 0x00000000,
+	0x074, 0x00050400,
+	0x075, 0x0004026E,
+	0x0EF, 0x00000100,
+	0x034, 0x0000ADD7,
+	0x035, 0x00005C00,
+	0x034, 0x00009DD4,
+	0x035, 0x00005000,
+	0x034, 0x00008DD1,
+	0x035, 0x00004400,
+	0x034, 0x00007DCE,
+	0x035, 0x00003800,
+	0x034, 0x00006CD1,
+	0x035, 0x00004400,
+	0x034, 0x00005CCE,
+	0x035, 0x00003800,
+	0x034, 0x000048CE,
+	0x035, 0x00004400,
+	0x034, 0x000034CE,
+	0x035, 0x00003800,
+	0x034, 0x00002451,
+	0x035, 0x00004400,
+	0x034, 0x0000144E,
+	0x035, 0x00003800,
+	0x034, 0x00000051,
+	0x035, 0x00004400,
+	0x0EF, 0x00000000,
+	0x0EF, 0x00000100,
+	0x0ED, 0x00000010,
+	0x044, 0x0000ADD7,
+	0x044, 0x00009DD4,
+	0x044, 0x00008DD1,
+	0x044, 0x00007DCE,
+	0x044, 0x00006CC1,
+	0x044, 0x00005CCE,
+	0x044, 0x000044D1,
+	0x044, 0x000034CE,
+	0x044, 0x00002451,
+	0x044, 0x0000144E,
+	0x044, 0x00000051,
+	0x0EF, 0x00000000,
+	0x0ED, 0x00000000,
+	0x0EF, 0x00002000,
+	0x03B, 0x000380EF,
+	0x03B, 0x000302FE,
+	0x03B, 0x00028CE6,
+	0x03B, 0x000200BC,
+	0x03B, 0x000188A5,
+	0x03B, 0x00010FBC,
+	0x03B, 0x00008F71,
+	0x03B, 0x00000900,
+	0x0EF, 0x00000000,
+	0x0ED, 0x00000001,
+	0x040, 0x000380EF,
+	0x040, 0x000302FE,
+	0x040, 0x00028CE6,
+	0x040, 0x000200BC,
+	0x040, 0x000188A5,
+	0x040, 0x00010FBC,
+	0x040, 0x00008F71,
+	0x040, 0x00000900,
+	0x0ED, 0x00000000,
+	0x082, 0x00080000,
+	0x083, 0x00008000,
+	0x084, 0x00048D80,
+	0x085, 0x00068000,
+	0x0A2, 0x00080000,
+	0x0A3, 0x00008000,
+	0x0A4, 0x00048D80,
+	0x0A5, 0x00068000,
+	0x000, 0x00033D80,
+};
+
+u32 RTL8723BEMAC_1T_ARRAY[] = {
+	0x02F, 0x00000030,
+	0x035, 0x00000000,
+	0x428, 0x0000000A,
+	0x429, 0x00000010,
+	0x430, 0x00000000,
+	0x431, 0x00000000,
+	0x432, 0x00000000,
+	0x433, 0x00000001,
+	0x434, 0x00000004,
+	0x435, 0x00000005,
+	0x436, 0x00000007,
+	0x437, 0x00000008,
+	0x43C, 0x00000004,
+	0x43D, 0x00000005,
+	0x43E, 0x00000007,
+	0x43F, 0x00000008,
+	0x440, 0x0000005D,
+	0x441, 0x00000001,
+	0x442, 0x00000000,
+	0x444, 0x00000010,
+	0x445, 0x00000000,
+	0x446, 0x00000000,
+	0x447, 0x00000000,
+	0x448, 0x00000000,
+	0x449, 0x000000F0,
+	0x44A, 0x0000000F,
+	0x44B, 0x0000003E,
+	0x44C, 0x00000010,
+	0x44D, 0x00000000,
+	0x44E, 0x00000000,
+	0x44F, 0x00000000,
+	0x450, 0x00000000,
+	0x451, 0x000000F0,
+	0x452, 0x0000000F,
+	0x453, 0x00000000,
+	0x456, 0x0000005E,
+	0x460, 0x00000066,
+	0x461, 0x00000066,
+	0x4C8, 0x000000FF,
+	0x4C9, 0x00000008,
+	0x4CC, 0x000000FF,
+	0x4CD, 0x000000FF,
+	0x4CE, 0x00000001,
+	0x500, 0x00000026,
+	0x501, 0x000000A2,
+	0x502, 0x0000002F,
+	0x503, 0x00000000,
+	0x504, 0x00000028,
+	0x505, 0x000000A3,
+	0x506, 0x0000005E,
+	0x507, 0x00000000,
+	0x508, 0x0000002B,
+	0x509, 0x000000A4,
+	0x50A, 0x0000005E,
+	0x50B, 0x00000000,
+	0x50C, 0x0000004F,
+	0x50D, 0x000000A4,
+	0x50E, 0x00000000,
+	0x50F, 0x00000000,
+	0x512, 0x0000001C,
+	0x514, 0x0000000A,
+	0x516, 0x0000000A,
+	0x525, 0x0000004F,
+	0x550, 0x00000010,
+	0x551, 0x00000010,
+	0x559, 0x00000002,
+	0x55C, 0x00000050,
+	0x55D, 0x000000FF,
+	0x605, 0x00000030,
+	0x608, 0x0000000E,
+	0x609, 0x0000002A,
+	0x620, 0x000000FF,
+	0x621, 0x000000FF,
+	0x622, 0x000000FF,
+	0x623, 0x000000FF,
+	0x624, 0x000000FF,
+	0x625, 0x000000FF,
+	0x626, 0x000000FF,
+	0x627, 0x000000FF,
+	0x638, 0x00000050,
+	0x63C, 0x0000000A,
+	0x63D, 0x0000000A,
+	0x63E, 0x0000000E,
+	0x63F, 0x0000000E,
+	0x640, 0x00000040,
+	0x642, 0x00000040,
+	0x643, 0x00000000,
+	0x652, 0x000000C8,
+	0x66E, 0x00000005,
+	0x700, 0x00000021,
+	0x701, 0x00000043,
+	0x702, 0x00000065,
+	0x703, 0x00000087,
+	0x708, 0x00000021,
+	0x709, 0x00000043,
+	0x70A, 0x00000065,
+	0x70B, 0x00000087,
+};
+
+u32 RTL8723BEAGCTAB_1TARRAY[] = {
+	0xC78, 0xFD000001,
+	0xC78, 0xFC010001,
+	0xC78, 0xFB020001,
+	0xC78, 0xFA030001,
+	0xC78, 0xF9040001,
+	0xC78, 0xF8050001,
+	0xC78, 0xF7060001,
+	0xC78, 0xF6070001,
+	0xC78, 0xF5080001,
+	0xC78, 0xF4090001,
+	0xC78, 0xF30A0001,
+	0xC78, 0xF20B0001,
+	0xC78, 0xF10C0001,
+	0xC78, 0xF00D0001,
+	0xC78, 0xEF0E0001,
+	0xC78, 0xEE0F0001,
+	0xC78, 0xED100001,
+	0xC78, 0xEC110001,
+	0xC78, 0xEB120001,
+	0xC78, 0xEA130001,
+	0xC78, 0xE9140001,
+	0xC78, 0xE8150001,
+	0xC78, 0xE7160001,
+	0xC78, 0xAA170001,
+	0xC78, 0xA9180001,
+	0xC78, 0xA8190001,
+	0xC78, 0xA71A0001,
+	0xC78, 0xA61B0001,
+	0xC78, 0xA51C0001,
+	0xC78, 0xA41D0001,
+	0xC78, 0xA31E0001,
+	0xC78, 0x671F0001,
+	0xC78, 0x66200001,
+	0xC78, 0x65210001,
+	0xC78, 0x64220001,
+	0xC78, 0x63230001,
+	0xC78, 0x62240001,
+	0xC78, 0x61250001,
+	0xC78, 0x47260001,
+	0xC78, 0x46270001,
+	0xC78, 0x45280001,
+	0xC78, 0x44290001,
+	0xC78, 0x432A0001,
+	0xC78, 0x422B0001,
+	0xC78, 0x292C0001,
+	0xC78, 0x282D0001,
+	0xC78, 0x272E0001,
+	0xC78, 0x262F0001,
+	0xC78, 0x25300001,
+	0xC78, 0x24310001,
+	0xC78, 0x09320001,
+	0xC78, 0x08330001,
+	0xC78, 0x07340001,
+	0xC78, 0x06350001,
+	0xC78, 0x05360001,
+	0xC78, 0x04370001,
+	0xC78, 0x03380001,
+	0xC78, 0x02390001,
+	0xC78, 0x013A0001,
+	0xC78, 0x003B0001,
+	0xC78, 0x003C0001,
+	0xC78, 0x003D0001,
+	0xC78, 0x003E0001,
+	0xC78, 0x003F0001,
+	0xC78, 0xFC400001,
+	0xC78, 0xFB410001,
+	0xC78, 0xFA420001,
+	0xC78, 0xF9430001,
+	0xC78, 0xF8440001,
+	0xC78, 0xF7450001,
+	0xC78, 0xF6460001,
+	0xC78, 0xF5470001,
+	0xC78, 0xF4480001,
+	0xC78, 0xF3490001,
+	0xC78, 0xF24A0001,
+	0xC78, 0xF14B0001,
+	0xC78, 0xF04C0001,
+	0xC78, 0xEF4D0001,
+	0xC78, 0xEE4E0001,
+	0xC78, 0xED4F0001,
+	0xC78, 0xEC500001,
+	0xC78, 0xEB510001,
+	0xC78, 0xEA520001,
+	0xC78, 0xE9530001,
+	0xC78, 0xE8540001,
+	0xC78, 0xE7550001,
+	0xC78, 0xE6560001,
+	0xC78, 0xE5570001,
+	0xC78, 0xAA580001,
+	0xC78, 0xA9590001,
+	0xC78, 0xA85A0001,
+	0xC78, 0xA75B0001,
+	0xC78, 0xA65C0001,
+	0xC78, 0xA55D0001,
+	0xC78, 0xA45E0001,
+	0xC78, 0x675F0001,
+	0xC78, 0x66600001,
+	0xC78, 0x65610001,
+	0xC78, 0x64620001,
+	0xC78, 0x63630001,
+	0xC78, 0x62640001,
+	0xC78, 0x61650001,
+	0xC78, 0x47660001,
+	0xC78, 0x46670001,
+	0xC78, 0x45680001,
+	0xC78, 0x44690001,
+	0xC78, 0x436A0001,
+	0xC78, 0x426B0001,
+	0xC78, 0x296C0001,
+	0xC78, 0x286D0001,
+	0xC78, 0x276E0001,
+	0xC78, 0x266F0001,
+	0xC78, 0x25700001,
+	0xC78, 0x24710001,
+	0xC78, 0x09720001,
+	0xC78, 0x08730001,
+	0xC78, 0x07740001,
+	0xC78, 0x06750001,
+	0xC78, 0x05760001,
+	0xC78, 0x04770001,
+	0xC78, 0x03780001,
+	0xC78, 0x02790001,
+	0xC78, 0x017A0001,
+	0xC78, 0x007B0001,
+	0xC78, 0x007C0001,
+	0xC78, 0x007D0001,
+	0xC78, 0x007E0001,
+	0xC78, 0x007F0001,
+	0xC50, 0x69553422,
+	0xC50, 0x69553420,
+};
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/table.h b/drivers/net/wireless/rtlwifi/rtl8723be/table.h
new file mode 100644
index 000000000000..932760a84827
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/table.h
@@ -0,0 +1,43 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Created on  2010/ 5/18,  1:41
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL8723BE_TABLE__H_
+#define __RTL8723BE_TABLE__H_
+
+#include <linux/types.h>
+#define  RTL8723BEPHY_REG_1TARRAYLEN	388
+extern u32 RTL8723BEPHY_REG_1TARRAY[];
+#define RTL8723BEPHY_REG_ARRAY_PGLEN	36
+extern u32 RTL8723BEPHY_REG_ARRAY_PG[];
+#define	RTL8723BE_RADIOA_1TARRAYLEN	206
+extern u32 RTL8723BE_RADIOA_1TARRAY[];
+#define RTL8723BEMAC_1T_ARRAYLEN	194
+extern u32 RTL8723BEMAC_1T_ARRAY[];
+#define RTL8723BEAGCTAB_1TARRAYLEN	260
+extern u32 RTL8723BEAGCTAB_1TARRAY[];
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/trx.c b/drivers/net/wireless/rtlwifi/rtl8723be/trx.c
new file mode 100644
index 000000000000..74a75dceab08
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/trx.c
@@ -0,0 +1,960 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../pci.h"
+#include "../base.h"
+#include "../stats.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "trx.h"
+#include "led.h"
+#include "dm.h"
+#include "phy.h"
+
+static u8 _rtl8723be_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 _rtl8723be_rate_mapping(struct ieee80211_hw *hw,
+				   bool isht, u8 desc_rate)
+{
+	int rate_idx;
+
+	if (!isht) {
+		if (IEEE80211_BAND_2GHZ == hw->conf.chandef.chan->band) {
+			switch (desc_rate) {
+			case DESC92C_RATE1M:
+				rate_idx = 0;
+				break;
+			case DESC92C_RATE2M:
+				rate_idx = 1;
+				break;
+			case DESC92C_RATE5_5M:
+				rate_idx = 2;
+				break;
+			case DESC92C_RATE11M:
+				rate_idx = 3;
+				break;
+			case DESC92C_RATE6M:
+				rate_idx = 4;
+				break;
+			case DESC92C_RATE9M:
+				rate_idx = 5;
+				break;
+			case DESC92C_RATE12M:
+				rate_idx = 6;
+				break;
+			case DESC92C_RATE18M:
+				rate_idx = 7;
+				break;
+			case DESC92C_RATE24M:
+				rate_idx = 8;
+				break;
+			case DESC92C_RATE36M:
+				rate_idx = 9;
+				break;
+			case DESC92C_RATE48M:
+				rate_idx = 10;
+				break;
+			case DESC92C_RATE54M:
+				rate_idx = 11;
+				break;
+			default:
+				rate_idx = 0;
+				break;
+			}
+		} else {
+			switch (desc_rate) {
+			case DESC92C_RATE6M:
+				rate_idx = 0;
+				break;
+			case DESC92C_RATE9M:
+				rate_idx = 1;
+				break;
+			case DESC92C_RATE12M:
+				rate_idx = 2;
+				break;
+			case DESC92C_RATE18M:
+				rate_idx = 3;
+				break;
+			case DESC92C_RATE24M:
+				rate_idx = 4;
+				break;
+			case DESC92C_RATE36M:
+				rate_idx = 5;
+				break;
+			case DESC92C_RATE48M:
+				rate_idx = 6;
+				break;
+			case DESC92C_RATE54M:
+				rate_idx = 7;
+				break;
+			default:
+				rate_idx = 0;
+				break;
+			}
+		}
+	} else {
+		switch (desc_rate) {
+		case DESC92C_RATEMCS0:
+			rate_idx = 0;
+			break;
+		case DESC92C_RATEMCS1:
+			rate_idx = 1;
+			break;
+		case DESC92C_RATEMCS2:
+			rate_idx = 2;
+			break;
+		case DESC92C_RATEMCS3:
+			rate_idx = 3;
+			break;
+		case DESC92C_RATEMCS4:
+			rate_idx = 4;
+			break;
+		case DESC92C_RATEMCS5:
+			rate_idx = 5;
+			break;
+		case DESC92C_RATEMCS6:
+			rate_idx = 6;
+			break;
+		case DESC92C_RATEMCS7:
+			rate_idx = 7;
+			break;
+		case DESC92C_RATEMCS8:
+			rate_idx = 8;
+			break;
+		case DESC92C_RATEMCS9:
+			rate_idx = 9;
+			break;
+		case DESC92C_RATEMCS10:
+			rate_idx = 10;
+			break;
+		case DESC92C_RATEMCS11:
+			rate_idx = 11;
+			break;
+		case DESC92C_RATEMCS12:
+			rate_idx = 12;
+			break;
+		case DESC92C_RATEMCS13:
+			rate_idx = 13;
+			break;
+		case DESC92C_RATEMCS14:
+			rate_idx = 14;
+			break;
+		case DESC92C_RATEMCS15:
+			rate_idx = 15;
+			break;
+		default:
+			rate_idx = 0;
+			break;
+		}
+	}
+	return rate_idx;
+}
+
+static void _rtl8723be_query_rxphystatus(struct ieee80211_hw *hw,
+					 struct rtl_stats *pstatus, u8 *pdesc,
+					 struct rx_fwinfo_8723be *p_drvinfo,
+					 bool packet_match_bssid,
+					 bool packet_toself,
+					 bool packet_beacon)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtlpriv);
+	struct phy_sts_cck_8723e_t *cck_buf;
+	struct phy_status_rpt *p_phystrpt = (struct phy_status_rpt *)p_drvinfo;
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	char rx_pwr_all = 0, rx_pwr[4];
+	u8 rf_rx_num = 0, evm, pwdb_all;
+	u8 i, max_spatial_stream;
+	u32 rssi, total_rssi = 0;
+	bool is_cck = pstatus->is_cck;
+	u8 lan_idx, vga_idx;
+
+	/* Record it for next packet processing */
+	pstatus->packet_matchbssid = packet_match_bssid;
+	pstatus->packet_toself = packet_toself;
+	pstatus->packet_beacon = packet_beacon;
+	pstatus->rx_mimo_sig_qual[0] = -1;
+	pstatus->rx_mimo_sig_qual[1] = -1;
+
+	if (is_cck) {
+		u8 cck_highpwr;
+		u8 cck_agc_rpt;
+		/* CCK Driver info Structure is not the same as OFDM packet. */
+		cck_buf = (struct phy_sts_cck_8723e_t *)p_drvinfo;
+		cck_agc_rpt = cck_buf->cck_agc_rpt;
+
+		/* (1)Hardware does not provide RSSI for CCK
+		 * (2)PWDB, Average PWDB cacluated by
+		 * hardware (for rate adaptive)
+		 */
+		if (ppsc->rfpwr_state == ERFON)
+			cck_highpwr = (u8) rtl_get_bbreg(hw,
+						       RFPGA0_XA_HSSIPARAMETER2,
+						       BIT(9));
+		else
+			cck_highpwr = false;
+
+		lan_idx = ((cck_agc_rpt & 0xE0) >> 5);
+		vga_idx = (cck_agc_rpt & 0x1f);
+		switch (lan_idx) {
+		case 7:
+			if (vga_idx <= 27)/*VGA_idx = 27~2*/
+				rx_pwr_all = -100 + 2 * (27 - vga_idx);
+			else
+				rx_pwr_all = -100;
+			break;
+		case 6:/*VGA_idx = 2~0*/
+			rx_pwr_all = -48 + 2 * (2 - vga_idx);
+			break;
+		case 5:/*VGA_idx = 7~5*/
+			rx_pwr_all = -42 + 2 * (7 - vga_idx);
+			break;
+		case 4:/*VGA_idx = 7~4*/
+			rx_pwr_all = -36 + 2 * (7 - vga_idx);
+			break;
+		case 3:/*VGA_idx = 7~0*/
+			rx_pwr_all = -24 + 2 * (7 - vga_idx);
+			break;
+		case 2:
+			if (cck_highpwr)/*VGA_idx = 5~0*/
+				rx_pwr_all = -12 + 2 * (5 - vga_idx);
+			else
+				rx_pwr_all = -6 + 2 * (5 - vga_idx);
+			break;
+		case 1:
+			rx_pwr_all = 8 - 2 * vga_idx;
+			break;
+		case 0:
+			rx_pwr_all = 14 - 2 * vga_idx;
+			break;
+		default:
+			break;
+		}
+		rx_pwr_all += 6;
+		pwdb_all = rtl_query_rxpwrpercentage(rx_pwr_all);
+		/* CCK gain is smaller than OFDM/MCS gain,  */
+		/* so we add gain diff by experiences,
+		 * the val is 6
+		 */
+		pwdb_all += 6;
+		if (pwdb_all > 100)
+			pwdb_all = 100;
+		/* modify the offset to make the same gain index with OFDM. */
+		if (pwdb_all > 34 && pwdb_all <= 42)
+			pwdb_all -= 2;
+		else if (pwdb_all > 26 && pwdb_all <= 34)
+			pwdb_all -= 6;
+		else if (pwdb_all > 14 && pwdb_all <= 26)
+			pwdb_all -= 8;
+		else if (pwdb_all > 4 && pwdb_all <= 14)
+			pwdb_all -= 4;
+		if (!cck_highpwr) {
+			if (pwdb_all >= 80)
+				pwdb_all = ((pwdb_all - 80) << 1) +
+					   ((pwdb_all - 80) >> 1) + 80;
+			else if ((pwdb_all <= 78) && (pwdb_all >= 20))
+				pwdb_all += 3;
+			if (pwdb_all > 100)
+				pwdb_all = 100;
+		}
+
+		pstatus->rx_pwdb_all = pwdb_all;
+		pstatus->recvsignalpower = rx_pwr_all;
+
+		/* (3) Get Signal Quality (EVM) */
+		if (packet_match_bssid) {
+			u8 sq;
+
+			if (pstatus->rx_pwdb_all > 40) {
+				sq = 100;
+			} else {
+				sq = cck_buf->sq_rpt;
+				if (sq > 64)
+					sq = 0;
+				else if (sq < 20)
+					sq = 100;
+				else
+					sq = ((64 - sq) * 100) / 44;
+			}
+
+			pstatus->signalquality = sq;
+			pstatus->rx_mimo_sig_qual[0] = sq;
+			pstatus->rx_mimo_sig_qual[1] = -1;
+		}
+	} else {
+		rtlpriv->dm.rfpath_rxenable[0] = true;
+		rtlpriv->dm.rfpath_rxenable[1] = true;
+
+		/* (1)Get RSSI for HT rate */
+		for (i = RF90_PATH_A; i < RF6052_MAX_PATH; i++) {
+			/* we will judge RF RX path now. */
+			if (rtlpriv->dm.rfpath_rxenable[i])
+				rf_rx_num++;
+
+			rx_pwr[i] = ((p_drvinfo->gain_trsw[i] & 0x3f)*2) - 110;
+
+			/* Translate DBM to percentage. */
+			rssi = rtl_query_rxpwrpercentage(rx_pwr[i]);
+			total_rssi += rssi;
+
+			/* Get Rx snr value in DB */
+			rtlpriv->stats.rx_snr_db[i] =
+					(long)(p_drvinfo->rxsnr[i] / 2);
+
+			/* Record Signal Strength for next packet */
+			if (packet_match_bssid)
+				pstatus->rx_mimo_signalstrength[i] = (u8) rssi;
+		}
+
+		/* (2)PWDB, Avg cacluated by hardware (for rate adaptive) */
+		rx_pwr_all = ((p_drvinfo->pwdb_all >> 1) & 0x7f) - 110;
+
+		pwdb_all = rtl_query_rxpwrpercentage(rx_pwr_all);
+		pstatus->rx_pwdb_all = pwdb_all;
+		pstatus->rxpower = rx_pwr_all;
+		pstatus->recvsignalpower = rx_pwr_all;
+
+		/* (3)EVM of HT rate */
+		if (pstatus->is_ht && pstatus->rate >= DESC92C_RATEMCS8 &&
+		    pstatus->rate <= DESC92C_RATEMCS15)
+			max_spatial_stream = 2;
+		else
+			max_spatial_stream = 1;
+
+		for (i = 0; i < max_spatial_stream; i++) {
+			evm = rtl_evm_db_to_percentage(p_drvinfo->rxevm[i]);
+
+			if (packet_match_bssid) {
+				/* Fill value in RFD, Get the first
+				 * spatial stream only
+				 */
+				if (i == 0)
+					pstatus->signalquality =
+							(u8) (evm & 0xff);
+				pstatus->rx_mimo_sig_qual[i] =
+							(u8) (evm & 0xff);
+			}
+		}
+		if (packet_match_bssid) {
+			for (i = RF90_PATH_A; i <= RF90_PATH_B; i++)
+				rtl_priv(hw)->dm.cfo_tail[i] =
+					(char)p_phystrpt->path_cfotail[i];
+
+			rtl_priv(hw)->dm.packet_count++;
+			if (rtl_priv(hw)->dm.packet_count == 0xffffffff)
+				rtl_priv(hw)->dm.packet_count = 0;
+		}
+	}
+
+	/* UI BSS List signal strength(in percentage),
+	 * make it good looking, from 0~100.
+	 */
+	if (is_cck)
+		pstatus->signalstrength = (u8)(rtl_signal_scale_mapping(hw,
+								pwdb_all));
+	else if (rf_rx_num != 0)
+		pstatus->signalstrength = (u8)(rtl_signal_scale_mapping(hw,
+						total_rssi /= rf_rx_num));
+	/*HW antenna diversity*/
+	rtldm->fat_table.antsel_rx_keep_0 = p_phystrpt->ant_sel;
+	rtldm->fat_table.antsel_rx_keep_1 = p_phystrpt->ant_sel_b;
+	rtldm->fat_table.antsel_rx_keep_2 = p_phystrpt->antsel_rx_keep_2;
+}
+
+static void _rtl8723be_translate_rx_signal_stuff(struct ieee80211_hw *hw,
+					struct sk_buff *skb,
+					struct rtl_stats *pstatus,
+					u8 *pdesc,
+					struct rx_fwinfo_8723be *p_drvinfo)
+{
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct ieee80211_hdr *hdr;
+	u8 *tmp_buf;
+	u8 *praddr;
+	u8 *psaddr;
+	u16 fc, type;
+	bool packet_matchbssid, packet_toself, packet_beacon;
+
+	tmp_buf = skb->data + pstatus->rx_drvinfo_size + pstatus->rx_bufshift;
+
+	hdr = (struct ieee80211_hdr *)tmp_buf;
+	fc = le16_to_cpu(hdr->frame_control);
+	type = WLAN_FC_GET_TYPE(hdr->frame_control);
+	praddr = hdr->addr1;
+	psaddr = ieee80211_get_SA(hdr);
+	memcpy(pstatus->psaddr, psaddr, ETH_ALEN);
+
+	packet_matchbssid = ((IEEE80211_FTYPE_CTL != type) &&
+	     (!ether_addr_equal(mac->bssid, (fc & IEEE80211_FCTL_TODS) ?
+				hdr->addr1 : (fc & IEEE80211_FCTL_FROMDS) ?
+				hdr->addr2 : hdr->addr3)) &&
+				(!pstatus->hwerror) &&
+				(!pstatus->crc) && (!pstatus->icv));
+
+	packet_toself = packet_matchbssid &&
+	    (!ether_addr_equal(praddr, rtlefuse->dev_addr));
+
+	/* YP: packet_beacon is not initialized,
+	 * this assignment is neccesary,
+	 * otherwise it counld be true in this case
+	 * the situation is much worse in Kernel 3.10
+	 */
+	if (ieee80211_is_beacon(hdr->frame_control))
+		packet_beacon = true;
+	else
+		packet_beacon = false;
+
+	if (packet_beacon && packet_matchbssid)
+		rtl_priv(hw)->dm.dbginfo.num_qry_beacon_pkt++;
+
+	_rtl8723be_query_rxphystatus(hw, pstatus, pdesc, p_drvinfo,
+				     packet_matchbssid,
+				     packet_toself,
+				     packet_beacon);
+
+	rtl_process_phyinfo(hw, tmp_buf, pstatus);
+}
+
+static void _rtl8723be_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 rtl8723be_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_8723be *p_drvinfo;
+	struct ieee80211_hdr *hdr;
+
+	u32 phystatus = GET_RX_DESC_PHYST(pdesc);
+	status->packet_report_type = (u8)GET_RX_STATUS_DESC_RPT_SEL(pdesc);
+	if (status->packet_report_type == TX_REPORT2)
+		status->length = (u16) GET_RX_RPT2_DESC_PKT_LEN(pdesc);
+	else
+		status->length = (u16) GET_RX_DESC_PKT_LEN(pdesc);
+	status->rx_drvinfo_size = (u8) GET_RX_DESC_DRV_INFO_SIZE(pdesc) *
+				  RX_DRV_INFO_SIZE_UNIT;
+	status->rx_bufshift = (u8) (GET_RX_DESC_SHIFT(pdesc) & 0x03);
+	status->icv = (u16) GET_RX_DESC_ICV(pdesc);
+	status->crc = (u16) GET_RX_DESC_CRC32(pdesc);
+	status->hwerror = (status->crc | status->icv);
+	status->decrypted = !GET_RX_DESC_SWDEC(pdesc);
+	status->rate = (u8) GET_RX_DESC_RXMCS(pdesc);
+	status->shortpreamble = (u16) GET_RX_DESC_SPLCP(pdesc);
+	status->isampdu = (bool) (GET_RX_DESC_PAGGR(pdesc) == 1);
+	status->isfirst_ampdu = (bool) (GET_RX_DESC_PAGGR(pdesc) == 1);
+	if (status->packet_report_type == NORMAL_RX)
+		status->timestamp_low = GET_RX_DESC_TSFL(pdesc);
+	status->rx_is40Mhzpacket = (bool) GET_RX_DESC_BW(pdesc);
+	status->is_ht = (bool)GET_RX_DESC_RXHT(pdesc);
+
+	status->is_cck = RTL8723E_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(rtlpriv, COMP_RXDESC, DBG_LOUD,
+			 "GGGGGGGGGGGGGet Wakeup Packet!! WakeMatch=%d\n",
+			 status->wake_match);
+	rx_status->freq = hw->conf.chandef.chan->center_freq;
+	rx_status->band = hw->conf.chandef.chan->band;
+
+
+	hdr = (struct ieee80211_hdr *)(skb->data + status->rx_drvinfo_size +
+				       status->rx_bufshift);
+
+	if (status->crc)
+		rx_status->flag |= RX_FLAG_FAILED_FCS_CRC;
+
+	if (status->rx_is40Mhzpacket)
+		rx_status->flag |= RX_FLAG_40MHZ;
+
+	if (status->is_ht)
+		rx_status->flag |= RX_FLAG_HT;
+
+	rx_status->flag |= RX_FLAG_MACTIME_START;
+
+	/* hw will set status->decrypted true, if it finds the
+	 * frame is open data frame or mgmt frame.
+	 * So hw will not decryption robust managment frame
+	 * for IEEE80211w but still set status->decrypted
+	 * true, so here we should set it back to undecrypted
+	 * for IEEE80211w frame, and mac80211 sw will help
+	 * to decrypt it
+	 */
+	if (status->decrypted) {
+		if (!hdr) {
+			WARN_ON_ONCE(true);
+			pr_err("decrypted is true but hdr NULL in skb %p\n",
+			       rtl_get_hdr(skb));
+			return false;
+		}
+
+		if ((_ieee80211_is_robust_mgmt_frame(hdr)) &&
+		    (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 = _rtl8723be_rate_mapping(hw, status->is_ht,
+						      status->rate);
+
+	rx_status->mactime = status->timestamp_low;
+	if (phystatus) {
+		p_drvinfo = (struct rx_fwinfo_8723be *)(skb->data +
+							status->rx_bufshift);
+
+		_rtl8723be_translate_rx_signal_stuff(hw, skb, status,
+						     pdesc, p_drvinfo);
+	}
+
+	/*rx_status->qual = status->signal; */
+	rx_status->signal = status->recvsignalpower + 10;
+	if (status->packet_report_type == TX_REPORT2) {
+		status->macid_valid_entry[0] =
+			 GET_RX_RPT2_DESC_MACID_VALID_1(pdesc);
+		status->macid_valid_entry[1] =
+			 GET_RX_RPT2_DESC_MACID_VALID_2(pdesc);
+	}
+	return true;
+}
+
+void rtl8723be_tx_fill_desc(struct ieee80211_hw *hw,
+			    struct ieee80211_hdr *hdr, u8 *pdesc_tx,
+			    u8 *pbd_desc_tx, struct ieee80211_tx_info *info,
+			    struct ieee80211_sta *sta, struct sk_buff *skb,
+			    u8 hw_queue, struct rtl_tcb_desc *ptcb_desc)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	u8 *pdesc = (u8 *)pdesc_tx;
+	u16 seq_number;
+	__le16 fc = hdr->frame_control;
+	unsigned int buf_len = 0;
+	unsigned int skb_len = skb->len;
+	u8 fw_qsel = _rtl8723be_map_hwqueue_to_fwqueue(skb, hw_queue);
+	bool firstseg = ((hdr->seq_ctrl &
+			  cpu_to_le16(IEEE80211_SCTL_FRAG)) == 0);
+	bool lastseg = ((hdr->frame_control &
+			 cpu_to_le16(IEEE80211_FCTL_MOREFRAGS)) == 0);
+	dma_addr_t mapping;
+	u8 bw_40 = 0;
+	u8 short_gi = 0;
+
+	if (mac->opmode == NL80211_IFTYPE_STATION) {
+		bw_40 = mac->bw_40;
+	} else if (mac->opmode == NL80211_IFTYPE_AP ||
+		mac->opmode == NL80211_IFTYPE_ADHOC) {
+		if (sta)
+			bw_40 = sta->ht_cap.cap &
+				IEEE80211_HT_CAP_SUP_WIDTH_20_40;
+	}
+	seq_number = (le16_to_cpu(hdr->seq_ctrl) & IEEE80211_SCTL_SEQ) >> 4;
+	rtl_get_tcb_desc(hw, info, sta, skb, ptcb_desc);
+	/* reserve 8 byte for AMPDU early mode */
+	if (rtlhal->earlymode_enable) {
+		skb_push(skb, EM_HDR_LEN);
+		memset(skb->data, 0, EM_HDR_LEN);
+	}
+	buf_len = skb->len;
+	mapping = pci_map_single(rtlpci->pdev, skb->data, skb->len,
+				 PCI_DMA_TODEVICE);
+	if (pci_dma_mapping_error(rtlpci->pdev, mapping)) {
+		RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE, "DMA mapping error");
+		return;
+	}
+	CLEAR_PCI_TX_DESC_CONTENT(pdesc, sizeof(struct tx_desc_8723be));
+	if (ieee80211_is_nullfunc(fc) || ieee80211_is_ctl(fc)) {
+		firstseg = true;
+		lastseg = true;
+	}
+	if (firstseg) {
+		if (rtlhal->earlymode_enable) {
+			SET_TX_DESC_PKT_OFFSET(pdesc, 1);
+			SET_TX_DESC_OFFSET(pdesc, USB_HWDESC_HEADER_LEN +
+					   EM_HDR_LEN);
+			if (ptcb_desc->empkt_num) {
+				RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
+					 "Insert 8 byte.pTcb->EMPktNum:%d\n",
+					  ptcb_desc->empkt_num);
+				_rtl8723be_insert_emcontent(ptcb_desc,
+							    (u8 *)(skb->data));
+			}
+		} else {
+			SET_TX_DESC_OFFSET(pdesc, USB_HWDESC_HEADER_LEN);
+		}
+
+		/* ptcb_desc->use_driver_rate = true; */
+		SET_TX_DESC_TX_RATE(pdesc, ptcb_desc->hw_rate);
+		if (ptcb_desc->hw_rate > DESC92C_RATEMCS0)
+			short_gi = (ptcb_desc->use_shortgi) ? 1 : 0;
+		else
+			short_gi = (ptcb_desc->use_shortpreamble) ? 1 : 0;
+
+		SET_TX_DESC_DATA_SHORTGI(pdesc, short_gi);
+
+		if (info->flags & IEEE80211_TX_CTL_AMPDU) {
+			SET_TX_DESC_AGG_ENABLE(pdesc, 1);
+			SET_TX_DESC_MAX_AGG_NUM(pdesc, 0x14);
+		}
+		SET_TX_DESC_SEQ(pdesc, seq_number);
+		SET_TX_DESC_RTS_ENABLE(pdesc, ((ptcb_desc->rts_enable &&
+						!ptcb_desc->cts_enable) ?
+						1 : 0));
+		SET_TX_DESC_HW_RTS_ENABLE(pdesc, 0);
+		SET_TX_DESC_CTS2SELF(pdesc, ((ptcb_desc->cts_enable) ?
+					      1 : 0));
+
+		SET_TX_DESC_RTS_RATE(pdesc, ptcb_desc->rts_rate);
+
+		SET_TX_DESC_RTS_SC(pdesc, ptcb_desc->rts_sc);
+		SET_TX_DESC_RTS_SHORT(pdesc,
+			((ptcb_desc->rts_rate <= DESC92C_RATE54M) ?
+			 (ptcb_desc->rts_use_shortpreamble ? 1 : 0) :
+			 (ptcb_desc->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) {
+				SET_TX_DESC_DATA_BW(pdesc, 1);
+				SET_TX_DESC_TX_SUB_CARRIER(pdesc, 3);
+			} else {
+				SET_TX_DESC_DATA_BW(pdesc, 0);
+				SET_TX_DESC_TX_SUB_CARRIER(pdesc, mac->cur_40_prime_sc);
+			}
+		} else {
+			SET_TX_DESC_DATA_BW(pdesc, 0);
+			SET_TX_DESC_TX_SUB_CARRIER(pdesc, 0);
+		}
+
+		SET_TX_DESC_LINIP(pdesc, 0);
+		SET_TX_DESC_PKT_SIZE(pdesc, (u16) skb_len);
+		if (sta) {
+			u8 ampdu_density = sta->ht_cap.ampdu_density;
+			SET_TX_DESC_AMPDU_DENSITY(pdesc, ampdu_density);
+		}
+		if (info->control.hw_key) {
+			struct ieee80211_key_conf *keyconf =
+						info->control.hw_key;
+			switch (keyconf->cipher) {
+			case WLAN_CIPHER_SUITE_WEP40:
+			case WLAN_CIPHER_SUITE_WEP104:
+			case WLAN_CIPHER_SUITE_TKIP:
+				SET_TX_DESC_SEC_TYPE(pdesc, 0x1);
+				break;
+			case WLAN_CIPHER_SUITE_CCMP:
+				SET_TX_DESC_SEC_TYPE(pdesc, 0x3);
+				break;
+			default:
+				SET_TX_DESC_SEC_TYPE(pdesc, 0x0);
+				break;
+			}
+		}
+
+		SET_TX_DESC_QUEUE_SEL(pdesc, fw_qsel);
+		SET_TX_DESC_DATA_RATE_FB_LIMIT(pdesc, 0x1F);
+		SET_TX_DESC_RTS_RATE_FB_LIMIT(pdesc, 0xF);
+		SET_TX_DESC_DISABLE_FB(pdesc, ptcb_desc->disable_ratefallback ?
+				       1 : 0);
+		SET_TX_DESC_USE_RATE(pdesc, ptcb_desc->use_driver_rate ? 1 : 0);
+
+		if (ieee80211_is_data_qos(fc)) {
+			if (mac->rdg_en) {
+				RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
+					 "Enable RDG function.\n");
+				SET_TX_DESC_RDG_ENABLE(pdesc, 1);
+				SET_TX_DESC_HTC(pdesc, 1);
+			}
+		}
+	}
+
+	SET_TX_DESC_FIRST_SEG(pdesc, (firstseg ? 1 : 0));
+	SET_TX_DESC_LAST_SEG(pdesc, (lastseg ? 1 : 0));
+	SET_TX_DESC_TX_BUFFER_SIZE(pdesc, (u16) buf_len);
+	SET_TX_DESC_TX_BUFFER_ADDRESS(pdesc, mapping);
+	SET_TX_DESC_RATE_ID(pdesc, ptcb_desc->ratr_index);
+	SET_TX_DESC_MACID(pdesc, ptcb_desc->mac_id);
+
+	if (!ieee80211_is_data_qos(fc))  {
+		SET_TX_DESC_HWSEQ_EN(pdesc, 1);
+		SET_TX_DESC_HWSEQ_SEL(pdesc, 0);
+	}
+	SET_TX_DESC_MORE_FRAG(pdesc, (lastseg ? 0 : 1));
+	if (is_multicast_ether_addr(ieee80211_get_DA(hdr)) ||
+	    is_broadcast_ether_addr(ieee80211_get_DA(hdr))) {
+		SET_TX_DESC_BMC(pdesc, 1);
+	}
+	RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE, "\n");
+}
+
+void rtl8723be_tx_fill_cmddesc(struct ieee80211_hw *hw, u8 *pdesc,
+			       bool b_firstseg, bool b_lastseg,
+			       struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u8 fw_queue = QSLT_BEACON;
+
+	dma_addr_t mapping = pci_map_single(rtlpci->pdev,
+					    skb->data, skb->len,
+					    PCI_DMA_TODEVICE);
+
+	if (pci_dma_mapping_error(rtlpci->pdev, mapping)) {
+		RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
+			 "DMA mapping error");
+		return;
+	}
+	CLEAR_PCI_TX_DESC_CONTENT(pdesc, TX_DESC_SIZE);
+
+	SET_TX_DESC_OFFSET(pdesc, USB_HWDESC_HEADER_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, 0);
+	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_USE_RATE(pdesc, 1);
+}
+
+void rtl8723be_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
+			u8 desc_name, u8 *val)
+{
+	if (istx) {
+		switch (desc_name) {
+		case HW_DESC_OWN:
+			SET_TX_DESC_OWN(pdesc, 1);
+			break;
+		case HW_DESC_TX_NEXTDESC_ADDR:
+			SET_TX_DESC_NEXT_DESC_ADDRESS(pdesc, *(u32 *)val);
+			break;
+		default:
+			RT_ASSERT(false, "ERR txdesc :%d not process\n",
+				  desc_name);
+			break;
+		}
+	} else {
+		switch (desc_name) {
+		case HW_DESC_RXOWN:
+			SET_RX_DESC_OWN(pdesc, 1);
+			break;
+		case HW_DESC_RXBUFF_ADDR:
+			SET_RX_DESC_BUFF_ADDR(pdesc, *(u32 *)val);
+			break;
+		case HW_DESC_RXPKT_LEN:
+			SET_RX_DESC_PKT_LEN(pdesc, *(u32 *)val);
+			break;
+		case HW_DESC_RXERO:
+			SET_RX_DESC_EOR(pdesc, 1);
+			break;
+		default:
+			RT_ASSERT(false, "ERR rxdesc :%d not process\n",
+				  desc_name);
+			break;
+		}
+	}
+}
+
+u32 rtl8723be_get_desc(u8 *pdesc, bool istx, u8 desc_name)
+{
+	u32 ret = 0;
+
+	if (istx) {
+		switch (desc_name) {
+		case HW_DESC_OWN:
+			ret = GET_TX_DESC_OWN(pdesc);
+			break;
+		case HW_DESC_TXBUFF_ADDR:
+			ret = GET_TX_DESC_TX_BUFFER_ADDRESS(pdesc);
+			break;
+		default:
+			RT_ASSERT(false, "ERR txdesc :%d not process\n",
+				  desc_name);
+			break;
+		}
+	} else {
+		switch (desc_name) {
+		case HW_DESC_OWN:
+			ret = GET_RX_DESC_OWN(pdesc);
+			break;
+		case HW_DESC_RXPKT_LEN:
+			ret = GET_RX_DESC_PKT_LEN(pdesc);
+			break;
+		default:
+			RT_ASSERT(false, "ERR rxdesc :%d not process\n",
+				  desc_name);
+			break;
+		}
+	}
+	return ret;
+}
+
+bool rtl8723be_is_tx_desc_closed(struct ieee80211_hw *hw,
+				 u8 hw_queue, u16 index)
+{
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[hw_queue];
+	u8 *entry = (u8 *)(&ring->desc[ring->idx]);
+	u8 own = (u8) rtl8723be_get_desc(entry, true, HW_DESC_OWN);
+
+	/*beacon packet will only use the first
+	 *descriptor by default, and the own may not
+	 *be cleared by the hardware
+	 */
+	if (own)
+		return false;
+	else
+		return true;
+}
+
+void rtl8723be_tx_polling(struct ieee80211_hw *hw, u8 hw_queue)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	if (hw_queue == BEACON_QUEUE) {
+		rtl_write_word(rtlpriv, REG_PCIE_CTRL_REG, BIT(4));
+	} else {
+		rtl_write_word(rtlpriv, REG_PCIE_CTRL_REG,
+			       BIT(0) << (hw_queue));
+	}
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/trx.h b/drivers/net/wireless/rtlwifi/rtl8723be/trx.h
new file mode 100644
index 000000000000..102f33dcc988
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/trx.h
@@ -0,0 +1,617 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL8723BE_TRX_H__
+#define __RTL8723BE_TRX_H__
+
+#define TX_DESC_SIZE				40
+#define TX_DESC_AGGR_SUBFRAME_SIZE		32
+
+#define RX_DESC_SIZE				32
+#define RX_DRV_INFO_SIZE_UNIT			8
+
+#define	TX_DESC_NEXT_DESC_OFFSET		40
+#define USB_HWDESC_HEADER_LEN			40
+#define CRCLENGTH				4
+
+#define SET_TX_DESC_PKT_SIZE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 0, 16, __val)
+#define SET_TX_DESC_OFFSET(__pdesc, __val)		\
+	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_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_BAR_RTY_TH(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 14, 2, __val)
+#define SET_TX_DESC_AGG_BREAK(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 16, 1, __val)
+#define SET_TX_DESC_MORE_FRAG(__pdesc, __val)		\
+	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_INT(__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_MODE(__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_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)
+
+
+#define SET_TX_DESC_TX_RATE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 0, 7, __val)
+#define SET_TX_DESC_DATA_RATE_FB_LIMIT(__pdesc, __val)	\
+	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_TX_SUB_CARRIER(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 0, 4, __val)
+#define SET_TX_DESC_DATA_SHORTGI(__pdesc, __val)	\
+	SET_BITS_TO_LE_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_CTROL_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_BUFFER_SIZE(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+28, 0, 16, __val)
+
+#define GET_TX_DESC_TX_BUFFER_SIZE(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+28, 0, 16)
+
+#define SET_TX_DESC_HWSEQ_EN(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+32, 15, 1, __val)
+
+#define SET_TX_DESC_SEQ(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+36, 12, 12, __val)
+
+#define SET_TX_DESC_TX_BUFFER_ADDRESS(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+40, 0, 32, __val)
+
+#define GET_TX_DESC_TX_BUFFER_ADDRESS(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+40, 0, 32)
+
+
+#define SET_TX_DESC_NEXT_DESC_ADDRESS(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+48, 0, 32, __val)
+
+#define GET_TX_DESC_NEXT_DESC_ADDRESS(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+48, 0, 32)
+
+#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_AMSDU(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 13, 1)
+#define GET_RX_STATUS_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_CHKERR(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 20, 1)
+#define GET_RX_DESC_IPVER(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 21, 1)
+#define GET_RX_STATUS_DESC_IS_TCPUDP(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+4, 22, 1)
+#define GET_RX_STATUS_DESC_CHK_VLD(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+4, 23, 1)
+#define GET_RX_DESC_PAM(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 24, 1)
+#define GET_RX_DESC_PWR(__pdesc)			\
+	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_STATUS_DESC_RX_IS_QOS(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+8, 16, 1)
+#define GET_RX_STATUS_DESC_WLANHD_IV_LEN(__pdesc)	\
+	LE_BITS_TO_4BYTE(__pdesc+8, 18, 6)
+#define GET_RX_STATUS_DESC_RPT_SEL(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+8, 28, 1)
+
+
+#define GET_RX_DESC_RXMCS(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+12, 0, 7)
+#define GET_RX_DESC_RXHT(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+12, 6, 1)
+#define GET_RX_STATUS_DESC_RX_GF(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+12, 7, 1)
+#define GET_RX_DESC_HTC(__pdesc)			\
+	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_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_SPLCP(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+16, 0, 1)
+#define GET_RX_STATUS_DESC_LDPC(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+16, 1, 1)
+#define GET_RX_STATUS_DESC_STBC(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+16, 2, 1)
+#define GET_RX_DESC_BW(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+16, 4, 2)
+
+#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(__rxstatusdesc)	\
+	LE_BITS_TO_4BYTE(__rxstatusdesc, 0, 9)
+#define GET_RX_RPT2_DESC_MACID_VALID_1(__rxstatusdesc)	\
+	LE_BITS_TO_4BYTE(__rxstatusdesc+16, 0, 32)
+#define GET_RX_RPT2_DESC_MACID_VALID_2(__rxstatusdesc)	\
+	LE_BITS_TO_4BYTE(__rxstatusdesc+20, 0, 32)
+
+#define SET_EARLYMODE_PKTNUM(__paddr, __value)		\
+	SET_BITS_TO_LE_4BYTE(__paddr, 0, 4, __value)
+#define SET_EARLYMODE_LEN0(__paddr, __value)		\
+	SET_BITS_TO_LE_4BYTE(__paddr, 4, 12, __value)
+#define SET_EARLYMODE_LEN1(__paddr, __value)		\
+	SET_BITS_TO_LE_4BYTE(__paddr, 16, 12, __value)
+#define SET_EARLYMODE_LEN2_1(__paddr, __value)		\
+	SET_BITS_TO_LE_4BYTE(__paddr, 28, 4, __value)
+#define SET_EARLYMODE_LEN2_2(__paddr, __value)		\
+	SET_BITS_TO_LE_4BYTE(__paddr+4, 0, 8, __value)
+#define SET_EARLYMODE_LEN3(__paddr, __value)		\
+	SET_BITS_TO_LE_4BYTE(__paddr+4, 8, 12, __value)
+#define SET_EARLYMODE_LEN4(__paddr, __value)		\
+	SET_BITS_TO_LE_4BYTE(__paddr+4, 20, 12, __value)
+
+#define CLEAR_PCI_TX_DESC_CONTENT(__pdesc, _size)		\
+do {								\
+	if (_size > TX_DESC_NEXT_DESC_OFFSET)			\
+		memset(__pdesc, 0, TX_DESC_NEXT_DESC_OFFSET);	\
+	else							\
+		memset(__pdesc, 0, _size);			\
+} while (0)
+
+struct phy_rx_agc_info_t {
+	#ifdef __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;/* ch_corr_msb; */
+	u8 noise_power_db_msb;
+	char path_cfotail[2];
+	u8 pcts_mask[2];
+	char 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;
+#ifdef __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_8723be {
+	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_8723be {
+	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_8723be {
+	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 rtl8723be_tx_fill_desc(struct ieee80211_hw *hw,
+			    struct ieee80211_hdr *hdr, u8 *pdesc,
+			    u8 *pbd_desc_tx, struct ieee80211_tx_info *info,
+			    struct ieee80211_sta *sta, struct sk_buff *skb,
+			    u8 hw_queue, struct rtl_tcb_desc *ptcb_desc);
+bool rtl8723be_rx_query_desc(struct ieee80211_hw *hw,
+			     struct rtl_stats *status,
+			     struct ieee80211_rx_status *rx_status,
+			     u8 *pdesc, struct sk_buff *skb);
+void rtl8723be_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
+			u8 desc_name, u8 *val);
+u32 rtl8723be_get_desc(u8 *pdesc, bool istx, u8 desc_name);
+bool rtl8723be_is_tx_desc_closed(struct ieee80211_hw *hw,
+				 u8 hw_queue, u16 index);
+void rtl8723be_tx_polling(struct ieee80211_hw *hw, u8 hw_queue);
+void rtl8723be_tx_fill_cmddesc(struct ieee80211_hw *hw, u8 *pdesc,
+			       bool b_firstseg, bool b_lastseg,
+			       struct sk_buff *skb);
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723com/Makefile b/drivers/net/wireless/rtlwifi/rtl8723com/Makefile
new file mode 100644
index 000000000000..345a68adcf38
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723com/Makefile
@@ -0,0 +1,9 @@
+rtl8723-common-objs :=		\
+		main.o		\
+		dm_common.o	\
+		fw_common.o	\
+		phy_common.o
+
+obj-$(CONFIG_RTL8723_COMMON) += rtl8723-common.o
+
+ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/net/wireless/rtlwifi/rtl8723com/dm_common.c b/drivers/net/wireless/rtlwifi/rtl8723com/dm_common.c
new file mode 100644
index 000000000000..4e254b72bf45
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723com/dm_common.c
@@ -0,0 +1,65 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "dm_common.h"
+#include "../rtl8723ae/dm.h"
+#include <linux/module.h>
+
+/* These routines are common to RTL8723AE and RTL8723bE */
+
+void rtl8723_dm_init_dynamic_txpower(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->dm.dynamic_txpower_enable = false;
+
+	rtlpriv->dm.last_dtp_lvl = TXHIGHPWRLEVEL_NORMAL;
+	rtlpriv->dm.dynamic_txhighpower_lvl = TXHIGHPWRLEVEL_NORMAL;
+}
+EXPORT_SYMBOL_GPL(rtl8723_dm_init_dynamic_txpower);
+
+void rtl8723_dm_init_edca_turbo(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->dm.current_turbo_edca = false;
+	rtlpriv->dm.is_any_nonbepkts = false;
+	rtlpriv->dm.is_cur_rdlstate = false;
+}
+EXPORT_SYMBOL_GPL(rtl8723_dm_init_edca_turbo);
+
+void rtl8723_dm_init_dynamic_bb_powersaving(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->dm_pstable.pre_ccastate = CCA_MAX;
+	rtlpriv->dm_pstable.cur_ccasate = CCA_MAX;
+	rtlpriv->dm_pstable.pre_rfstate = RF_MAX;
+	rtlpriv->dm_pstable.cur_rfstate = RF_MAX;
+	rtlpriv->dm_pstable.rssi_val_min = 0;
+	rtlpriv->dm_pstable.initialize = 0;
+}
+EXPORT_SYMBOL_GPL(rtl8723_dm_init_dynamic_bb_powersaving);
diff --git a/drivers/net/wireless/rtlwifi/rtl8723com/dm_common.h b/drivers/net/wireless/rtlwifi/rtl8723com/dm_common.h
new file mode 100644
index 000000000000..5c1b94ce2f86
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723com/dm_common.h
@@ -0,0 +1,33 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __DM_COMMON_H__
+#define __DM_COMMON_H__
+
+void rtl8723_dm_init_dynamic_txpower(struct ieee80211_hw *hw);
+void rtl8723_dm_init_edca_turbo(struct ieee80211_hw *hw);
+void rtl8723_dm_init_dynamic_bb_powersaving(struct ieee80211_hw *hw);
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723com/fw_common.c b/drivers/net/wireless/rtlwifi/rtl8723com/fw_common.c
new file mode 100644
index 000000000000..c12da552b7f7
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723com/fw_common.c
@@ -0,0 +1,329 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../pci.h"
+#include "../base.h"
+#include "fw_common.h"
+#include <linux/module.h>
+
+void rtl8723_enable_fw_download(struct ieee80211_hw *hw, bool enable)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmp;
+
+	if (enable) {
+		tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
+		rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN + 1, tmp | 0x04);
+
+		tmp = rtl_read_byte(rtlpriv, REG_MCUFWDL);
+		rtl_write_byte(rtlpriv, REG_MCUFWDL, tmp | 0x01);
+
+		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);
+
+		rtl_write_byte(rtlpriv, REG_MCUFWDL + 1, 0x00);
+	}
+}
+EXPORT_SYMBOL_GPL(rtl8723_enable_fw_download);
+
+void rtl8723_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));
+		}
+	}
+}
+EXPORT_SYMBOL_GPL(rtl8723_fw_block_write);
+
+void rtl8723_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);
+	rtl8723_fw_block_write(hw, buffer, size);
+}
+EXPORT_SYMBOL_GPL(rtl8723_fw_page_write);
+
+static void rtl8723_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;
+}
+
+void rtl8723_write_fw(struct ieee80211_hw *hw,
+		      enum version_8723e version,
+		      u8 *buffer, u32 size)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 *bufferptr = (u8 *)buffer;
+	u32 pagenums, remainsize;
+	u32 page, offset;
+
+	RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "FW size is %d bytes,\n", size);
+
+	rtl8723_fill_dummy(bufferptr, &size);
+
+	pagenums = size / FW_8192C_PAGE_SIZE;
+	remainsize = size % FW_8192C_PAGE_SIZE;
+
+	if (pagenums > 8) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "Page numbers should not greater then 8\n");
+	}
+	for (page = 0; page < pagenums; page++) {
+		offset = page * FW_8192C_PAGE_SIZE;
+		rtl8723_fw_page_write(hw, page, (bufferptr + offset),
+				      FW_8192C_PAGE_SIZE);
+	}
+	if (remainsize) {
+		offset = pagenums * FW_8192C_PAGE_SIZE;
+		page = pagenums;
+		rtl8723_fw_page_write(hw, page, (bufferptr + offset),
+				      remainsize);
+	}
+}
+EXPORT_SYMBOL_GPL(rtl8723_write_fw);
+
+void rtl8723ae_firmware_selfreset(struct ieee80211_hw *hw)
+{
+	u8 u1tmp;
+	u8 delay = 100;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtl_write_byte(rtlpriv, REG_HMETFR + 3, 0x20);
+	u1tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
+
+	while (u1tmp & BIT(2)) {
+		delay--;
+		if (delay == 0)
+			break;
+		udelay(50);
+		u1tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
+	}
+	if (delay == 0) {
+		u1tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
+		rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN + 1, u1tmp&(~BIT(2)));
+	}
+}
+EXPORT_SYMBOL_GPL(rtl8723ae_firmware_selfreset);
+
+void rtl8723be_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(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "  _8051Reset8723be(): 8051 reset success .\n");
+}
+EXPORT_SYMBOL_GPL(rtl8723be_firmware_selfreset);
+
+int rtl8723_fw_free_to_go(struct ieee80211_hw *hw, bool is_8723be)
+{
+	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(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "chksum report fail ! REG_MCUFWDL:0x%08x .\n",
+			 value32);
+		goto exit;
+	}
+	RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
+		 "Checksum report OK ! REG_MCUFWDL:0x%08x .\n", value32);
+
+	value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL) | MCUFWDL_RDY;
+	value32 &= ~WINTINI_RDY;
+	rtl_write_dword(rtlpriv, REG_MCUFWDL, value32);
+
+	if (is_8723be)
+		rtl8723be_firmware_selfreset(hw);
+	counter = 0;
+
+	do {
+		value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
+		if (value32 & WINTINI_RDY) {
+			RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
+				 "Polling FW ready success!! "
+				 "REG_MCUFWDL:0x%08x .\n",
+				 value32);
+			err = 0;
+			goto exit;
+		}
+		udelay(FW_8192C_POLLING_DELAY);
+
+	} while (counter++ < FW_8192C_POLLING_TIMEOUT_COUNT);
+
+	RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+		 "Polling FW ready fail!! REG_MCUFWDL:0x%08x .\n",
+		 value32);
+
+exit:
+	return err;
+}
+EXPORT_SYMBOL_GPL(rtl8723_fw_free_to_go);
+
+int rtl8723_download_fw(struct ieee80211_hw *hw,
+			bool is_8723be)
+{
+	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_8723e version = rtlhal->version;
+
+	if (!rtlhal->pfirmware)
+		return 1;
+
+	pfwheader = (struct rtl92c_firmware_header *)rtlhal->pfirmware;
+	pfwdata = (u8 *)rtlhal->pfirmware;
+	fwsize = rtlhal->fwsize;
+	RT_TRACE(rtlpriv, COMP_FW, DBG_DMESG,
+		 "normal Firmware SIZE %d\n", fwsize);
+
+	if (rtlpriv->cfg->ops->is_fw_header(pfwheader)) {
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
+			 "Firmware Version(%d), Signature(%#x), Size(%d)\n",
+			 pfwheader->version, pfwheader->signature,
+			 (int)sizeof(struct rtl92c_firmware_header));
+
+		pfwdata = pfwdata + sizeof(struct rtl92c_firmware_header);
+		fwsize = fwsize - sizeof(struct rtl92c_firmware_header);
+	}
+	if (rtl_read_byte(rtlpriv, REG_MCUFWDL) & BIT(7)) {
+		rtl_write_byte(rtlpriv, REG_MCUFWDL, 0);
+		if (is_8723be)
+			rtl8723be_firmware_selfreset(hw);
+		else
+			rtl8723ae_firmware_selfreset(hw);
+	}
+	rtl8723_enable_fw_download(hw, true);
+	rtl8723_write_fw(hw, version, pfwdata, fwsize);
+	rtl8723_enable_fw_download(hw, false);
+
+	err = rtl8723_fw_free_to_go(hw, is_8723be);
+	if (err) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "Firmware is not ready to run!\n");
+	} else {
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
+			 "Firmware is ready to run!\n");
+	}
+	return 0;
+}
+EXPORT_SYMBOL_GPL(rtl8723_download_fw);
+
+bool rtl8723_cmd_send_packet(struct ieee80211_hw *hw,
+			     struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl8192_tx_ring *ring;
+	struct rtl_tx_desc *pdesc;
+	struct sk_buff *pskb = NULL;
+	u8 own;
+	unsigned long flags;
+
+	ring = &rtlpci->tx_ring[BEACON_QUEUE];
+
+	pskb = __skb_dequeue(&ring->queue);
+	if (pskb)
+		kfree_skb(pskb);
+
+	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
+
+	pdesc = &ring->desc[0];
+	own = (u8) rtlpriv->cfg->ops->get_desc((u8 *)pdesc, true, HW_DESC_OWN);
+
+	rtlpriv->cfg->ops->fill_tx_cmddesc(hw, (u8 *)pdesc, 1, 1, skb);
+
+	__skb_queue_tail(&ring->queue, skb);
+
+	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
+
+	rtlpriv->cfg->ops->tx_polling(hw, BEACON_QUEUE);
+
+	return true;
+}
+EXPORT_SYMBOL_GPL(rtl8723_cmd_send_packet);
diff --git a/drivers/net/wireless/rtlwifi/rtl8723com/fw_common.h b/drivers/net/wireless/rtlwifi/rtl8723com/fw_common.h
new file mode 100644
index 000000000000..cf1cc5804d06
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723com/fw_common.h
@@ -0,0 +1,126 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __FW_COMMON_H__
+#define __FW_COMMON_H__
+
+#define REG_SYS_FUNC_EN				0x0002
+#define REG_MCUFWDL				0x0080
+#define FW_8192C_START_ADDRESS			0x1000
+#define FW_8192C_PAGE_SIZE			4096
+#define FW_8192C_POLLING_TIMEOUT_COUNT		6000
+#define FW_8192C_POLLING_DELAY			5
+
+#define MCUFWDL_RDY				BIT(1)
+#define FWDL_CHKSUM_RPT				BIT(2)
+#define WINTINI_RDY				BIT(6)
+
+#define REG_RSV_CTRL				0x001C
+#define REG_HMETFR				0x01CC
+
+enum version_8723e {
+	VERSION_TEST_UMC_CHIP_8723 = 0x0081,
+	VERSION_NORMAL_UMC_CHIP_8723_1T1R_A_CUT = 0x0089,
+	VERSION_NORMAL_UMC_CHIP_8723_1T1R_B_CUT = 0x1089,
+	VERSION_TEST_CHIP_1T1R_8723B = 0x0106,
+	VERSION_NORMAL_SMIC_CHIP_1T1R_8723B = 0x010E,
+	VERSION_UNKNOWN = 0xFF,
+};
+
+enum rtl8723ae_h2c_cmd {
+	H2C_AP_OFFLOAD = 0,
+	H2C_SETPWRMODE = 1,
+	H2C_JOINBSSRPT = 2,
+	H2C_RSVDPAGE = 3,
+	H2C_RSSI_REPORT = 4,
+	H2C_P2P_PS_CTW_CMD = 5,
+	H2C_P2P_PS_OFFLOAD = 6,
+	H2C_RA_MASK = 7,
+	MAX_H2CCMD
+};
+
+enum rtl8723be_cmd {
+	H2C_8723BE_RSVDPAGE = 0,
+	H2C_8723BE_JOINBSSRPT = 1,
+	H2C_8723BE_SCAN = 2,
+	H2C_8723BE_KEEP_ALIVE_CTRL = 3,
+	H2C_8723BE_DISCONNECT_DECISION = 4,
+	H2C_8723BE_INIT_OFFLOAD = 6,
+	H2C_8723BE_AP_OFFLOAD = 8,
+	H2C_8723BE_BCN_RSVDPAGE = 9,
+	H2C_8723BE_PROBERSP_RSVDPAGE = 10,
+
+	H2C_8723BE_SETPWRMODE = 0x20,
+	H2C_8723BE_PS_TUNING_PARA = 0x21,
+	H2C_8723BE_PS_TUNING_PARA2 = 0x22,
+	H2C_8723BE_PS_LPS_PARA = 0x23,
+	H2C_8723BE_P2P_PS_OFFLOAD = 0x24,
+
+	H2C_8723BE_WO_WLAN = 0x80,
+	H2C_8723BE_REMOTE_WAKE_CTRL = 0x81,
+	H2C_8723BE_AOAC_GLOBAL_INFO = 0x82,
+	H2C_8723BE_AOAC_RSVDPAGE = 0x83,
+	H2C_8723BE_RSSI_REPORT = 0x42,
+	H2C_8723BE_RA_MASK = 0x40,
+	H2C_8723BE_SELECTIVE_SUSPEND_ROF_CMD,
+	H2C_8723BE_P2P_PS_MODE,
+	H2C_8723BE_PSD_RESULT,
+	/*Not defined CTW CMD for P2P yet*/
+	H2C_8723BE_P2P_PS_CTW_CMD,
+	MAX_8723BE_H2CCMD
+};
+
+struct rtl92c_firmware_header {
+	u16 signature;
+	u8 category;
+	u8 function;
+	u16 version;
+	u8 subversion;
+	u8 rsvd1;
+	u8 month;
+	u8 date;
+	u8 hour;
+	u8 minute;
+	u16 ramcodesize;
+	u16 rsvd2;
+	u32 svnindex;
+	u32 rsvd3;
+	u32 rsvd4;
+	u32 rsvd5;
+};
+
+void rtl8723ae_firmware_selfreset(struct ieee80211_hw *hw);
+void rtl8723be_firmware_selfreset(struct ieee80211_hw *hw);
+void rtl8723_enable_fw_download(struct ieee80211_hw *hw, bool enable);
+void rtl8723_fw_block_write(struct ieee80211_hw *hw,
+			    const u8 *buffer, u32 size);
+void rtl8723_fw_page_write(struct ieee80211_hw *hw,
+			   u32 page, const u8 *buffer, u32 size);
+void rtl8723_write_fw(struct ieee80211_hw *hw,
+		      enum version_8723e version,
+		      u8 *buffer, u32 size);
+int rtl8723_fw_free_to_go(struct ieee80211_hw *hw, bool is_8723be);
+int rtl8723_download_fw(struct ieee80211_hw *hw, bool is_8723be);
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723com/main.c b/drivers/net/wireless/rtlwifi/rtl8723com/main.c
new file mode 100644
index 000000000000..9014a94fac6a
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723com/main.c
@@ -0,0 +1,33 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include <linux/module.h>
+
+
+MODULE_AUTHOR("Realtek WlanFAE	<wlanfae@realtek.com>");
+MODULE_AUTHOR("Larry Finger	<Larry.Finger@lwfinger.net>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Realtek RTL8723AE/RTL8723BE 802.11n PCI wireless common routines");
diff --git a/drivers/net/wireless/rtlwifi/rtl8723com/phy_common.c b/drivers/net/wireless/rtlwifi/rtl8723com/phy_common.c
new file mode 100644
index 000000000000..d73b659bd2b5
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723com/phy_common.c
@@ -0,0 +1,434 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "phy_common.h"
+#include "../rtl8723ae/reg.h"
+#include <linux/module.h>
+
+/* These routines are common to RTL8723AE and RTL8723bE */
+
+u32 rtl8723_phy_query_bb_reg(struct ieee80211_hw *hw,
+			     u32 regaddr, u32 bitmask)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 returnvalue, originalvalue, bitshift;
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "regaddr(%#x), bitmask(%#x)\n", regaddr, bitmask);
+	originalvalue = rtl_read_dword(rtlpriv, regaddr);
+	bitshift = rtl8723_phy_calculate_bit_shift(bitmask);
+	returnvalue = (originalvalue & bitmask) >> bitshift;
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "BBR MASK = 0x%x Addr[0x%x]= 0x%x\n",
+		  bitmask, regaddr, originalvalue);
+
+	return returnvalue;
+}
+EXPORT_SYMBOL_GPL(rtl8723_phy_query_bb_reg);
+
+void rtl8723_phy_set_bb_reg(struct ieee80211_hw *hw, u32 regaddr,
+			      u32 bitmask, u32 data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 originalvalue, bitshift;
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "regaddr(%#x), bitmask(%#x), data(%#x)\n",
+		  regaddr, bitmask, data);
+
+	if (bitmask != MASKDWORD) {
+		originalvalue = rtl_read_dword(rtlpriv, regaddr);
+		bitshift = rtl8723_phy_calculate_bit_shift(bitmask);
+		data = ((originalvalue & (~bitmask)) | (data << bitshift));
+	}
+
+	rtl_write_dword(rtlpriv, regaddr, data);
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "regaddr(%#x), bitmask(%#x), data(%#x)\n",
+		  regaddr, bitmask, data);
+}
+EXPORT_SYMBOL_GPL(rtl8723_phy_set_bb_reg);
+
+u32 rtl8723_phy_calculate_bit_shift(u32 bitmask)
+{
+	u32 i;
+
+	for (i = 0; i <= 31; i++) {
+		if (((bitmask >> i) & 0x1) == 1)
+			break;
+	}
+	return i;
+}
+EXPORT_SYMBOL_GPL(rtl8723_phy_calculate_bit_shift);
+
+u32 rtl8723_phy_rf_serial_read(struct ieee80211_hw *hw,
+			       enum radio_path rfpath, u32 offset)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct bb_reg_def *pphyreg = &rtlphy->phyreg_def[rfpath];
+	u32 newoffset;
+	u32 tmplong, tmplong2;
+	u8 rfpi_enable = 0;
+	u32 retvalue;
+
+	offset &= 0xff;
+	newoffset = offset;
+	if (RT_CANNOT_IO(hw)) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "return all one\n");
+		return 0xFFFFFFFF;
+	}
+	tmplong = rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD);
+	if (rfpath == RF90_PATH_A)
+		tmplong2 = tmplong;
+	else
+		tmplong2 = rtl_get_bbreg(hw, pphyreg->rfhssi_para2, MASKDWORD);
+	tmplong2 = (tmplong2 & (~BLSSIREADADDRESS)) |
+		   (newoffset << 23) | BLSSIREADEDGE;
+	rtl_set_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD,
+		      tmplong & (~BLSSIREADEDGE));
+	mdelay(1);
+	rtl_set_bbreg(hw, pphyreg->rfhssi_para2, MASKDWORD, tmplong2);
+	mdelay(2);
+	if (rfpath == RF90_PATH_A)
+		rfpi_enable = (u8) rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER1,
+						 BIT(8));
+	else if (rfpath == RF90_PATH_B)
+		rfpi_enable = (u8) rtl_get_bbreg(hw, RFPGA0_XB_HSSIPARAMETER1,
+						 BIT(8));
+	if (rfpi_enable)
+		retvalue = rtl_get_bbreg(hw, pphyreg->rf_rbpi,
+					 BLSSIREADBACKDATA);
+	else
+		retvalue = rtl_get_bbreg(hw, pphyreg->rf_rb,
+					 BLSSIREADBACKDATA);
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "RFR-%d Addr[0x%x]= 0x%x\n",
+		  rfpath, pphyreg->rf_rb, retvalue);
+	return retvalue;
+}
+EXPORT_SYMBOL_GPL(rtl8723_phy_rf_serial_read);
+
+void rtl8723_phy_rf_serial_write(struct ieee80211_hw *hw,
+				 enum radio_path rfpath,
+				 u32 offset, u32 data)
+{
+	u32 data_and_addr;
+	u32 newoffset;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct bb_reg_def *pphyreg = &rtlphy->phyreg_def[rfpath];
+
+	if (RT_CANNOT_IO(hw)) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "stop\n");
+		return;
+	}
+	offset &= 0xff;
+	newoffset = offset;
+	data_and_addr = ((newoffset << 20) | (data & 0x000fffff)) & 0x0fffffff;
+	rtl_set_bbreg(hw, pphyreg->rf3wire_offset, MASKDWORD, data_and_addr);
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "RFW-%d Addr[0x%x]= 0x%x\n", rfpath,
+		   pphyreg->rf3wire_offset, data_and_addr);
+}
+EXPORT_SYMBOL_GPL(rtl8723_phy_rf_serial_write);
+
+long rtl8723_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:
+	default:
+		offset = -8;
+		break;
+	}
+	pwrout_dbm = txpwridx / 2 + offset;
+	return pwrout_dbm;
+}
+EXPORT_SYMBOL_GPL(rtl8723_phy_txpwr_idx_to_dbm);
+
+void rtl8723_phy_init_bb_rf_reg_def(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfintfs = RFPGA0_XAB_RFINTERFACESW;
+	rtlphy->phyreg_def[RF90_PATH_B].rfintfs = RFPGA0_XAB_RFINTERFACESW;
+	rtlphy->phyreg_def[RF90_PATH_C].rfintfs = RFPGA0_XCD_RFINTERFACESW;
+	rtlphy->phyreg_def[RF90_PATH_D].rfintfs = RFPGA0_XCD_RFINTERFACESW;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfintfi = RFPGA0_XAB_RFINTERFACERB;
+	rtlphy->phyreg_def[RF90_PATH_B].rfintfi = RFPGA0_XAB_RFINTERFACERB;
+	rtlphy->phyreg_def[RF90_PATH_C].rfintfi = RFPGA0_XCD_RFINTERFACERB;
+	rtlphy->phyreg_def[RF90_PATH_D].rfintfi = RFPGA0_XCD_RFINTERFACERB;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfintfo = RFPGA0_XA_RFINTERFACEOE;
+	rtlphy->phyreg_def[RF90_PATH_B].rfintfo = RFPGA0_XB_RFINTERFACEOE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfintfe = RFPGA0_XA_RFINTERFACEOE;
+	rtlphy->phyreg_def[RF90_PATH_B].rfintfe = RFPGA0_XB_RFINTERFACEOE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rf3wire_offset =
+			    RFPGA0_XA_LSSIPARAMETER;
+	rtlphy->phyreg_def[RF90_PATH_B].rf3wire_offset =
+			    RFPGA0_XB_LSSIPARAMETER;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rflssi_select = rFPGA0_XAB_RFPARAMETER;
+	rtlphy->phyreg_def[RF90_PATH_B].rflssi_select = rFPGA0_XAB_RFPARAMETER;
+	rtlphy->phyreg_def[RF90_PATH_C].rflssi_select = rFPGA0_XCD_RFPARAMETER;
+	rtlphy->phyreg_def[RF90_PATH_D].rflssi_select = rFPGA0_XCD_RFPARAMETER;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rftxgain_stage = RFPGA0_TXGAINSTAGE;
+	rtlphy->phyreg_def[RF90_PATH_B].rftxgain_stage = RFPGA0_TXGAINSTAGE;
+	rtlphy->phyreg_def[RF90_PATH_C].rftxgain_stage = RFPGA0_TXGAINSTAGE;
+	rtlphy->phyreg_def[RF90_PATH_D].rftxgain_stage = RFPGA0_TXGAINSTAGE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfhssi_para1 = RFPGA0_XA_HSSIPARAMETER1;
+	rtlphy->phyreg_def[RF90_PATH_B].rfhssi_para1 = RFPGA0_XB_HSSIPARAMETER1;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfhssi_para2 = RFPGA0_XA_HSSIPARAMETER2;
+	rtlphy->phyreg_def[RF90_PATH_B].rfhssi_para2 = RFPGA0_XB_HSSIPARAMETER2;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfsw_ctrl = RFPGA0_XAB_SWITCHCONTROL;
+	rtlphy->phyreg_def[RF90_PATH_B].rfsw_ctrl = RFPGA0_XAB_SWITCHCONTROL;
+	rtlphy->phyreg_def[RF90_PATH_C].rfsw_ctrl = RFPGA0_XCD_SWITCHCONTROL;
+	rtlphy->phyreg_def[RF90_PATH_D].rfsw_ctrl = RFPGA0_XCD_SWITCHCONTROL;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfagc_control1 = ROFDM0_XAAGCCORE1;
+	rtlphy->phyreg_def[RF90_PATH_B].rfagc_control1 = ROFDM0_XBAGCCORE1;
+	rtlphy->phyreg_def[RF90_PATH_C].rfagc_control1 = ROFDM0_XCAGCCORE1;
+	rtlphy->phyreg_def[RF90_PATH_D].rfagc_control1 = ROFDM0_XDAGCCORE1;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfagc_control2 = ROFDM0_XAAGCCORE2;
+	rtlphy->phyreg_def[RF90_PATH_B].rfagc_control2 = ROFDM0_XBAGCCORE2;
+	rtlphy->phyreg_def[RF90_PATH_C].rfagc_control2 = ROFDM0_XCAGCCORE2;
+	rtlphy->phyreg_def[RF90_PATH_D].rfagc_control2 = ROFDM0_XDAGCCORE2;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfrxiq_imbal = ROFDM0_XARXIQIMBALANCE;
+	rtlphy->phyreg_def[RF90_PATH_B].rfrxiq_imbal = ROFDM0_XBRXIQIMBALANCE;
+	rtlphy->phyreg_def[RF90_PATH_C].rfrxiq_imbal = ROFDM0_XCRXIQIMBANLANCE;
+	rtlphy->phyreg_def[RF90_PATH_D].rfrxiq_imbal = ROFDM0_XDRXIQIMBALANCE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfrx_afe = ROFDM0_XARXAFE;
+	rtlphy->phyreg_def[RF90_PATH_B].rfrx_afe = ROFDM0_XBRXAFE;
+	rtlphy->phyreg_def[RF90_PATH_C].rfrx_afe = ROFDM0_XCRXAFE;
+	rtlphy->phyreg_def[RF90_PATH_D].rfrx_afe = ROFDM0_XDRXAFE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rftxiq_imbal = ROFDM0_XATXIQIMBALANCE;
+	rtlphy->phyreg_def[RF90_PATH_B].rftxiq_imbal = ROFDM0_XBTXIQIMBALANCE;
+	rtlphy->phyreg_def[RF90_PATH_C].rftxiq_imbal = ROFDM0_XCTXIQIMBALANCE;
+	rtlphy->phyreg_def[RF90_PATH_D].rftxiq_imbal = ROFDM0_XDTXIQIMBALANCE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rftx_afe = ROFDM0_XATXAFE;
+	rtlphy->phyreg_def[RF90_PATH_B].rftx_afe = ROFDM0_XBTXAFE;
+	rtlphy->phyreg_def[RF90_PATH_C].rftx_afe = ROFDM0_XCTXAFE;
+	rtlphy->phyreg_def[RF90_PATH_D].rftx_afe = ROFDM0_XDTXAFE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rf_rb = RFPGA0_XA_LSSIREADBACK;
+	rtlphy->phyreg_def[RF90_PATH_B].rf_rb = RFPGA0_XB_LSSIREADBACK;
+	rtlphy->phyreg_def[RF90_PATH_C].rf_rb = RFPGA0_XC_LSSIREADBACK;
+	rtlphy->phyreg_def[RF90_PATH_D].rf_rb = RFPGA0_XD_LSSIREADBACK;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rf_rbpi = TRANSCEIVEA_HSPI_READBACK;
+	rtlphy->phyreg_def[RF90_PATH_B].rf_rbpi = TRANSCEIVEB_HSPI_READBACK;
+}
+EXPORT_SYMBOL_GPL(rtl8723_phy_init_bb_rf_reg_def);
+
+bool rtl8723_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;
+}
+EXPORT_SYMBOL_GPL(rtl8723_phy_set_sw_chnl_cmdarray);
+
+void rtl8723_phy_path_a_fill_iqk_matrix(struct ieee80211_hw *hw,
+					bool iqk_ok,
+					long result[][8],
+					u8 final_candidate,
+					bool btxonly)
+{
+	u32 oldval_0, x, tx0_a, reg;
+	long y, tx0_c;
+
+	if (final_candidate == 0xFF) {
+		return;
+	} else if (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, 0xca0, 0xF0000000, reg);
+	}
+}
+EXPORT_SYMBOL_GPL(rtl8723_phy_path_a_fill_iqk_matrix);
+
+void rtl8723_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);
+}
+EXPORT_SYMBOL_GPL(rtl8723_save_adda_registers);
+
+void rtl8723_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]);
+}
+EXPORT_SYMBOL_GPL(rtl8723_phy_save_mac_registers);
+
+void rtl8723_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]);
+}
+EXPORT_SYMBOL_GPL(rtl8723_phy_reload_adda_registers);
+
+void rtl8723_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]);
+}
+EXPORT_SYMBOL_GPL(rtl8723_phy_reload_mac_registers);
+
+void rtl8723_phy_path_adda_on(struct ieee80211_hw *hw, u32 *addareg,
+			      bool is_patha_on, bool is2t)
+{
+	u32 pathon;
+	u32 i;
+
+	pathon = is_patha_on ? 0x04db25a4 : 0x0b1b25a4;
+	if (!is2t) {
+		pathon = 0x0bdb25a0;
+		rtl_set_bbreg(hw, addareg[0], MASKDWORD, 0x0b1b25a0);
+	} else {
+		rtl_set_bbreg(hw, addareg[0], MASKDWORD, pathon);
+	}
+
+	for (i = 1; i < IQK_ADDA_REG_NUM; i++)
+		rtl_set_bbreg(hw, addareg[i], MASKDWORD, pathon);
+}
+EXPORT_SYMBOL_GPL(rtl8723_phy_path_adda_on);
+
+void rtl8723_phy_mac_setting_calibration(struct ieee80211_hw *hw,
+					 u32 *macreg, u32 *macbackup)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i = 0;
+
+	rtl_write_byte(rtlpriv, macreg[i], 0x3F);
+
+	for (i = 1; i < (IQK_MAC_REG_NUM - 1); i++)
+		rtl_write_byte(rtlpriv, macreg[i],
+			       (u8) (macbackup[i] & (~BIT(3))));
+	rtl_write_byte(rtlpriv, macreg[i], (u8) (macbackup[i] & (~BIT(5))));
+}
+EXPORT_SYMBOL_GPL(rtl8723_phy_mac_setting_calibration);
+
+void rtl8723_phy_path_a_standby(struct ieee80211_hw *hw)
+{
+	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x0);
+	rtl_set_bbreg(hw, 0x840, MASKDWORD, 0x00010000);
+	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x80800000);
+}
+EXPORT_SYMBOL_GPL(rtl8723_phy_path_a_standby);
+
+void rtl8723_phy_pi_mode_switch(struct ieee80211_hw *hw, bool pi_mode)
+{
+	u32 mode;
+
+	mode = pi_mode ? 0x01000100 : 0x01000000;
+	rtl_set_bbreg(hw, 0x820, MASKDWORD, mode);
+	rtl_set_bbreg(hw, 0x828, MASKDWORD, mode);
+}
+EXPORT_SYMBOL_GPL(rtl8723_phy_pi_mode_switch);
diff --git a/drivers/net/wireless/rtlwifi/rtl8723com/phy_common.h b/drivers/net/wireless/rtlwifi/rtl8723com/phy_common.h
new file mode 100644
index 000000000000..83b891a9adb8
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8723com/phy_common.h
@@ -0,0 +1,89 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2014  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __PHY_COMMON__
+#define __PHY_COMMON__
+
+#define RT_CANNOT_IO(hw)			false
+
+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;
+};
+
+u32 rtl8723_phy_query_bb_reg(struct ieee80211_hw *hw,
+			     u32 regaddr, u32 bitmask);
+void rtl8723_phy_set_bb_reg(struct ieee80211_hw *hw, u32 regaddr,
+			      u32 bitmask, u32 data);
+u32 rtl8723_phy_calculate_bit_shift(u32 bitmask);
+u32 rtl8723_phy_rf_serial_read(struct ieee80211_hw *hw,
+			       enum radio_path rfpath, u32 offset);
+void rtl8723_phy_rf_serial_write(struct ieee80211_hw *hw,
+				 enum radio_path rfpath,
+				 u32 offset, u32 data);
+long rtl8723_phy_txpwr_idx_to_dbm(struct ieee80211_hw *hw,
+				  enum wireless_mode wirelessmode,
+				  u8 txpwridx);
+void rtl8723_phy_init_bb_rf_reg_def(struct ieee80211_hw *hw);
+bool rtl8723_phy_set_sw_chnl_cmdarray(struct swchnlcmd *cmdtable,
+				      u32 cmdtableidx,
+				      u32 cmdtablesz,
+				      enum swchnlcmd_id cmdid,
+				      u32 para1, u32 para2,
+				      u32 msdelay);
+void rtl8723_phy_path_a_fill_iqk_matrix(struct ieee80211_hw *hw,
+					bool iqk_ok,
+					long result[][8],
+					u8 final_candidate,
+					bool btxonly);
+void rtl8723_save_adda_registers(struct ieee80211_hw *hw, u32 *addareg,
+				 u32 *addabackup, u32 registernum);
+void rtl8723_phy_save_mac_registers(struct ieee80211_hw *hw,
+				    u32 *macreg, u32 *macbackup);
+void rtl8723_phy_reload_adda_registers(struct ieee80211_hw *hw,
+				       u32 *addareg, u32 *addabackup,
+				       u32 regiesternum);
+void rtl8723_phy_reload_mac_registers(struct ieee80211_hw *hw,
+				      u32 *macreg, u32 *macbackup);
+void rtl8723_phy_path_adda_on(struct ieee80211_hw *hw, u32 *addareg,
+			      bool is_patha_on, bool is2t);
+void rtl8723_phy_mac_setting_calibration(struct ieee80211_hw *hw,
+					 u32 *macreg, u32 *macbackup);
+void rtl8723_phy_path_a_standby(struct ieee80211_hw *hw);
+void rtl8723_phy_pi_mode_switch(struct ieee80211_hw *hw, bool pi_mode);
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/usb.c b/drivers/net/wireless/rtlwifi/usb.c
index 4933f02ce1d5..0398d3ea15b0 100644
--- a/drivers/net/wireless/rtlwifi/usb.c
+++ b/drivers/net/wireless/rtlwifi/usb.c
@@ -410,7 +410,7 @@ static void rtl_usb_init_sw(struct ieee80211_hw *hw)
 	mac->current_ampdu_factor = 3;
 
 	/* QOS */
-	rtlusb->acm_method = eAcmWay2_SW;
+	rtlusb->acm_method = EACMWAY2_SW;
 
 	/* IRQ */
 	/* HIMR - turn all on */
@@ -994,7 +994,7 @@ static void _rtl_usb_tx_preprocess(struct ieee80211_hw *hw,
 		seq_number += 1;
 		seq_number <<= 4;
 	}
-	rtlpriv->cfg->ops->fill_tx_desc(hw, hdr, (u8 *)pdesc, info, sta, skb,
+	rtlpriv->cfg->ops->fill_tx_desc(hw, hdr, (u8 *)pdesc, NULL, info, sta, skb,
 					hw_queue, &tcb_desc);
 	if (!ieee80211_has_morefrags(hdr->frame_control)) {
 		if (qc)
diff --git a/drivers/net/wireless/rtlwifi/wifi.h b/drivers/net/wireless/rtlwifi/wifi.h
index 8c647391bedf..6965afdf572a 100644
--- a/drivers/net/wireless/rtlwifi/wifi.h
+++ b/drivers/net/wireless/rtlwifi/wifi.h
@@ -41,6 +41,38 @@
 #include <linux/completion.h>
 #include "debug.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
+
+#define	MASK4BITS				0x0f
+#define	MASK20BITS				0xfffff
+#define RFREG_OFFSET_MASK			0xfffff
+
+#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 RF_CHANGE_BY_INIT			0
 #define RF_CHANGE_BY_IPS			BIT(28)
 #define RF_CHANGE_BY_PS				BIT(29)
@@ -49,6 +81,7 @@
 
 #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
@@ -86,7 +119,18 @@
 #define MAC80211_4ADDR_LEN			30
 
 #define CHANNEL_MAX_NUMBER	(14 + 24 + 21)	/* 14 is the max channel no */
+#define CHANNEL_MAX_NUMBER_2G		14
+#define CHANNEL_MAX_NUMBER_5G		54 /* Please refer to
+					    *"phy_GetChnlGroup8812A" and
+					    * "Hal_ReadTxPowerInfo8812A"
+					    */
+#define CHANNEL_MAX_NUMBER_5G_80M	7
 #define CHANNEL_GROUP_MAX	(3 + 9)	/*  ch1~3, 4~9, 10~14 = three groups */
+#define CHANNEL_MAX_NUMBER_5G		54 /* Please refer to
+					    *"phy_GetChnlGroup8812A" and
+					    * "Hal_ReadTxPowerInfo8812A"
+					    */
+#define CHANNEL_MAX_NUMBER_5G_80M	7
 #define MAX_PG_GROUP			13
 #define	CHANNEL_GROUP_MAX_2G		3
 #define	CHANNEL_GROUP_IDX_5GL		3
@@ -96,6 +140,7 @@
 #define CHANNEL_MAX_NUMBER_2G		14
 #define AVG_THERMAL_NUM			8
 #define AVG_THERMAL_NUM_88E		4
+#define AVG_THERMAL_NUM_8723BE		4
 #define MAX_TID_COUNT			9
 
 /* for early mode */
@@ -107,6 +152,24 @@
 #define	MAX_CHNL_GROUP_24G		6
 #define	MAX_CHNL_GROUP_5G		14
 
+#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 RTL8192EE_SEG_NUM		1 /* 0:2 seg, 1: 4 seg, 2: 8 seg */
+
+#define DEL_SW_IDX_SZ		30
+#define BAND_NUM			3
+
+enum rf_tx_num {
+	RF_1TX = 0,
+	RF_2TX,
+	RF_MAX_TX_NUM,
+	RF_TX_NUM_NONIMPLEMENT,
+};
+
 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];
@@ -115,6 +178,8 @@ struct txpower_info_2g {
 	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];
 };
 
 struct txpower_info_5g {
@@ -123,6 +188,17 @@ struct txpower_info_5g {
 	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];
+};
+
+enum rate_section {
+	CCK = 0,
+	OFDM,
+	HT_MCS0_MCS7,
+	HT_MCS8_MCS15,
+	VHT_1SSMCS0_1SSMCS9,
+	VHT_2SSMCS0_2SSMCS9,
 };
 
 enum intf_type {
@@ -158,7 +234,10 @@ enum hardware_type {
 	HARDWARE_TYPE_RTL8192DU,
 	HARDWARE_TYPE_RTL8723AE,
 	HARDWARE_TYPE_RTL8723U,
+	HARDWARE_TYPE_RTL8723BE,
 	HARDWARE_TYPE_RTL8188EE,
+	HARDWARE_TYPE_RTL8821AE,
+	HARDWARE_TYPE_RTL8812AE,
 
 	/* keep it last */
 	HARDWARE_TYPE_NUM
@@ -195,8 +274,16 @@ enum hardware_type {
 	 _pdesc->rxmcs == DESC92_RATE5_5M ||		\
 	 _pdesc->rxmcs == DESC92_RATE11M)
 
+#define RTL8723E_RX_HAL_IS_CCK_RATE(rxmcs)		\
+	((rxmcs) == DESC92_RATE1M ||			\
+	 (rxmcs) == DESC92_RATE2M ||			\
+	 (rxmcs) == DESC92_RATE5_5M ||			\
+	 (rxmcs) == DESC92_RATE11M)
+
 enum scan_operation_backup_opt {
 	SCAN_OPT_BACKUP = 0,
+	SCAN_OPT_BACKUP_BAND0 = 0,
+	SCAN_OPT_BACKUP_BAND1,
 	SCAN_OPT_RESTORE,
 	SCAN_OPT_MAX
 };
@@ -231,7 +318,9 @@ struct bb_reg_def {
 
 enum io_type {
 	IO_CMD_PAUSE_DM_BY_SCAN = 0,
-	IO_CMD_RESUME_DM_BY_SCAN = 1,
+	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 {
@@ -298,6 +387,7 @@ enum hw_variables {
 	HW_VAR_SET_RPWM,
 	HW_VAR_H2C_FW_PWRMODE,
 	HW_VAR_H2C_FW_JOINBSSRPT,
+	HW_VAR_H2C_FW_MEDIASTATUSRPT,
 	HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
 	HW_VAR_FW_PSMODE_STATUS,
 	HW_VAR_RESUME_CLK_ON,
@@ -330,6 +420,8 @@ enum hw_variables {
 
 	HAL_DEF_WOWLAN,
 	HW_VAR_MRC,
+	HW_VAR_KEEP_ALIVE,
+	HW_VAR_NAV_UPPER,
 
 	HW_VAR_MGT_FILTER,
 	HW_VAR_CTRL_FILTER,
@@ -348,34 +440,34 @@ enum rt_oem_id {
 	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_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_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_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_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,
@@ -389,6 +481,7 @@ enum hw_descs {
 	HW_DESC_RXBUFF_ADDR,
 	HW_DESC_RXPKT_LEN,
 	HW_DESC_RXERO,
+	HW_DESC_RX_PREPARE,
 };
 
 enum prime_sc {
@@ -407,6 +500,7 @@ enum rf_type {
 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
@@ -471,6 +565,9 @@ enum rtl_var_map {
 	MAC_RCR_ACRC32,
 	MAC_RCR_ACF,
 	MAC_RCR_AAP,
+	MAC_HIMR,
+	MAC_HIMRE,
+	MAC_HSISR,
 
 	/*efuse map */
 	EFUSE_TEST,
@@ -608,7 +705,7 @@ enum rtl_led_pin {
 enum acm_method {
 	eAcmWay0_SwAndHw = 0,
 	eAcmWay1_HW = 1,
-	eAcmWay2_SW = 2,
+	EACMWAY2_SW = 2,
 };
 
 enum macphy_mode {
@@ -645,7 +742,9 @@ enum wireless_mode {
 	WIRELESS_MODE_G = 0x04,
 	WIRELESS_MODE_AUTO = 0x08,
 	WIRELESS_MODE_N_24G = 0x10,
-	WIRELESS_MODE_N_5G = 0x20
+	WIRELESS_MODE_N_5G = 0x20,
+	WIRELESS_MODE_AC_5G = 0x40,
+	WIRELESS_MODE_AC_24G  = 0x80
 };
 
 #define IS_WIRELESS_MODE_A(wirelessmode)	\
@@ -669,6 +768,8 @@ enum ratr_table_mode {
 	RATR_INX_WIRELESS_B = 6,
 	RATR_INX_WIRELESS_MC = 7,
 	RATR_INX_WIRELESS_A = 8,
+	RATR_INX_WIRELESS_AC_5N = 8,
+	RATR_INX_WIRELESS_AC_24N = 9,
 };
 
 enum rtl_link_state {
@@ -803,8 +904,12 @@ struct wireless_stats {
 	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;
 };
@@ -817,9 +922,9 @@ struct rate_adaptive {
 	u32 high_rssi_thresh_for_ra;
 	u32 high2low_rssi_thresh_for_ra;
 	u8 low2high_rssi_thresh_for_ra40m;
-	u32 low_rssi_thresh_for_ra40M;
+	u32 low_rssi_thresh_for_ra40m;
 	u8 low2high_rssi_thresh_for_ra20m;
-	u32 low_rssi_thresh_for_ra20M;
+	u32 low_rssi_thresh_for_ra20m;
 	u32 upper_rssi_threshold_ratr;
 	u32 middleupper_rssi_threshold_ratr;
 	u32 middle_rssi_threshold_ratr;
@@ -833,6 +938,10 @@ struct rate_adaptive {
 	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 {
@@ -841,6 +950,16 @@ struct regd_pair_mapping {
 	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;
@@ -976,16 +1095,29 @@ struct rtl_phy {
 	u32 iqk_bb_backup[10];
 	bool iqk_initialized;
 
+	bool rfpath_rx_enable[MAX_RF_PATH];
+	u8 reg_837;
 	/* Dual mac */
 	bool need_iqk;
 	struct iqk_matrix_regs iqk_matrix[IQK_MATRIX_SETTINGS_NUM];
 
 	bool rfpi_enable;
+	bool iqk_in_progress;
 
 	u8 pwrgroup_cnt;
 	u8 cck_high_power;
 	/* MAX_PG_GROUP groups of pwr diff by rates */
 	u32 mcs_offset[MAX_PG_GROUP][16];
+	u32 tx_power_by_rate_offset[TX_PWR_BY_RATE_NUM_BAND]
+				   [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 */
@@ -998,6 +1130,7 @@ struct rtl_phy {
 	bool apk_done;
 	u32 reg_rf3c[2];	/* pathA / pathB  */
 
+	u32 backup_rf_0x1a;/*92ee*/
 	/* bfsync */
 	u8 framesync;
 	u32 framesync_c34;
@@ -1006,6 +1139,7 @@ struct rtl_phy {
 	struct phy_parameters hwparam_tables[MAX_TAB];
 	u16 rf_pathmap;
 
+	u8 hw_rof_enable; /*Enable GPIO[9] as WL RF HW PDn source*/
 	enum rt_polarity_ctl polarity_ctl;
 };
 
@@ -1133,6 +1267,7 @@ struct rtl_mac {
 	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;
@@ -1213,6 +1348,7 @@ struct rtl_hal {
 	bool being_init_adapter;
 	bool bbrf_ready;
 	bool mac_func_enable;
+	bool pre_edcca_enable;
 	struct bt_coexist_8723 hal_coex_8723;
 
 	enum intf_type interface;
@@ -1234,6 +1370,7 @@ struct rtl_hal {
 	/*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;
@@ -1273,6 +1410,9 @@ struct rtl_hal {
 	bool disable_amsdu_8k;
 	bool master_of_dmsp;
 	bool slave_of_dmsp;
+
+	u16 rx_tag;/*for 92ee*/
+	u8 rts_en;
 };
 
 struct rtl_security {
@@ -1321,6 +1461,16 @@ struct fast_ant_training {
 	bool	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 Dynamic Management */
 	long entry_min_undec_sm_pwdb;
@@ -1360,29 +1510,84 @@ struct rtl_dm {
 	u8 txpower_track_control;
 	bool interrupt_migration;
 	bool disable_tx_int;
-	char ofdm_index[2];
+	char ofdm_index[MAX_RF_PATH];
+	u8 default_ofdm_index;
+	u8 default_cck_index;
 	char cck_index;
-	char delta_power_index;
-	char delta_power_index_last;
-	char power_index_offset;
+	char delta_power_index[MAX_RF_PATH];
+	char delta_power_index_last[MAX_RF_PATH];
+	char power_index_offset[MAX_RF_PATH];
+	char absolute_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 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	swing_idx_ofdm[2];
+	u8	swing_idx_ofdm[MAX_RF_PATH];
 	u8	swing_idx_ofdm_cur;
-	u8	swing_idx_ofdm_base;
+	u8	swing_idx_ofdm_base[MAX_RF_PATH];
 	bool	swing_flag_ofdm;
 	u8	swing_idx_cck;
 	u8	swing_idx_cck_cur;
 	u8	swing_idx_cck_base;
 	bool	swing_flag_cck;
 
+	char	swing_diff_2g;
+	char	swing_diff_5g;
+
+	u8 delta_swing_table_idx_24gccka_p[DEL_SW_IDX_SZ];
+	u8 delta_swing_table_idx_24gccka_n[DEL_SW_IDX_SZ];
+	u8 delta_swing_table_idx_24gcckb_p[DEL_SW_IDX_SZ];
+	u8 delta_swing_table_idx_24gcckb_n[DEL_SW_IDX_SZ];
+	u8 delta_swing_table_idx_24ga_p[DEL_SW_IDX_SZ];
+	u8 delta_swing_table_idx_24ga_n[DEL_SW_IDX_SZ];
+	u8 delta_swing_table_idx_24gb_p[DEL_SW_IDX_SZ];
+	u8 delta_swing_table_idx_24gb_n[DEL_SW_IDX_SZ];
+	u8 delta_swing_table_idx_5ga_p[BAND_NUM][DEL_SW_IDX_SZ];
+	u8 delta_swing_table_idx_5ga_n[BAND_NUM][DEL_SW_IDX_SZ];
+	u8 delta_swing_table_idx_5gb_p[BAND_NUM][DEL_SW_IDX_SZ];
+	u8 delta_swing_table_idx_5gb_n[BAND_NUM][DEL_SW_IDX_SZ];
+	u8 delta_swing_table_idx_24ga_p_8188e[DEL_SW_IDX_SZ];
+	u8 delta_swing_table_idx_24ga_n_8188e[DEL_SW_IDX_SZ];
+
 	/* DMSP */
 	bool supp_phymode_switch;
 
+	/* DulMac */
 	struct fast_ant_training fat_table;
+
+	u8	resp_tx_path;
+	u8	path_sel;
+	u32	patha_sum;
+	u32	pathb_sum;
+	u32	patha_cnt;
+	u32	pathb_cnt;
+
+	u8 pre_channel;
+	u8 *p_channel;
+	u8 linked_interval;
+
+	u64 last_tx_ok_cnt;
+	u64 last_rx_ok_cnt;
 };
 
-#define	EFUSE_MAX_LOGICAL_SIZE			256
+#define	EFUSE_MAX_LOGICAL_SIZE			512
 
 struct rtl_efuse {
 	bool autoLoad_ok;
@@ -1422,12 +1627,9 @@ struct rtl_efuse {
 	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 eprom_chnl_txpwr_ht40_2sdf[2][CHANNEL_GROUP_MAX];
-	u8 txpwrlevel_cck[2][CHANNEL_MAX_NUMBER_2G];
-	u8 txpwrlevel_ht40_1s[2][CHANNEL_MAX_NUMBER];	/*For HT 40MHZ pwr */
-	u8 txpwrlevel_ht40_2s[2][CHANNEL_MAX_NUMBER];	/*For HT 40MHZ pwr */
+	u8 eeprom_chnlarea_txpwr_cck[MAX_RF_PATH][CHANNEL_GROUP_MAX_2G];
+	u8 eeprom_chnlarea_txpwr_ht40_1s[MAX_RF_PATH][CHANNEL_GROUP_MAX];
+	u8 eprom_chnl_txpwr_ht40_2sdf[MAX_RF_PATH][CHANNEL_GROUP_MAX];
 
 	u8 internal_pa_5g[2];	/* pathA / pathB */
 	u8 eeprom_c9;
@@ -1438,9 +1640,38 @@ struct rtl_efuse {
 	u8 pwrgroup_ht20[2][CHANNEL_MAX_NUMBER];
 	u8 pwrgroup_ht40[2][CHANNEL_MAX_NUMBER];
 
-	char txpwr_ht20diff[2][CHANNEL_MAX_NUMBER]; /*HT 20<->40 Pwr diff */
-	/*For HT<->legacy pwr diff*/
-	u8 txpwr_legacyhtdiff[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 */
@@ -1571,7 +1802,9 @@ struct rtl_stats {
 	bool rx_is40Mhzpacket;
 	u32 rx_pwdb_all;
 	u8 rx_mimo_signalstrength[4];	/*in 0~100 index */
-	s8 rx_mimo_sig_qual[2];
+	s8 rx_mimo_sig_qual[4];
+	u8 rx_pwr[4]; /* per-path's pwdb */
+	u8 rx_snr[4]; /* per-path's SNR */
 	bool packet_matchbssid;
 	bool is_cck;
 	bool is_ht;
@@ -1644,6 +1877,8 @@ struct rtl_tcb_desc {
 	bool btx_enable_sw_calc_duration;
 };
 
+struct rtl92c_firmware_header;
+
 struct rtl_hal_ops {
 	int (*init_sw_vars) (struct ieee80211_hw *hw);
 	void (*deinit_sw_vars) (struct ieee80211_hw *hw);
@@ -1673,9 +1908,17 @@ struct rtl_hal_ops {
 	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);
 	void (*update_rate_mask) (struct ieee80211_hw *hw, u8 rssi_level);
+	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_tx,
 			      struct ieee80211_tx_info *info,
 			      struct ieee80211_sta *sta,
 			      struct sk_buff *skb, u8 hw_queue,
@@ -1698,8 +1941,11 @@ struct rtl_hal_ops {
 				    enum rf_pwrstate rfpwr_state);
 	void (*led_control) (struct ieee80211_hw *hw,
 			     enum led_ctl_mode ledaction);
-	void (*set_desc) (u8 *pdesc, bool istx, u8 desc_name, u8 *val);
+	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,
@@ -1738,6 +1984,10 @@ struct rtl_hal_ops {
 	void (*bt_coex_off_before_lps) (struct ieee80211_hw *hw);
 	void (*fill_h2c_cmd) (struct ieee80211_hw *hw, u8 element_id,
 			      u32 cmd_len, u8 *p_cmdbuffer);
+	bool (*get_btc_status) (void);
+	bool (*is_fw_header) (struct rtl92c_firmware_header *hdr);
+	u32 (*rx_command_packet)(struct ieee80211_hw *hw,
+				 struct rtl_stats status, struct sk_buff *skb);
 };
 
 struct rtl_intf_ops {
@@ -1847,6 +2097,8 @@ struct rtl_locks {
 
 	/*Easy concurrent*/
 	spinlock_t check_sendpkt_lock;
+
+	spinlock_t iqk_lock;
 };
 
 struct rtl_works {
@@ -1915,6 +2167,7 @@ struct ps_t {
 	u8 cur_ccasate;
 	u8 pre_rfstate;
 	u8 cur_rfstate;
+	u8 initialize;
 	long rssi_val_min;
 };
 
@@ -1939,6 +2192,7 @@ struct dig_t {
 	u8 cursta_cstate;
 	u8 presta_cstate;
 	u8 curmultista_cstate;
+	u8 stop_dig;
 	char back_val;
 	char back_range_max;
 	char back_range_min;
@@ -1956,6 +2210,7 @@ struct dig_t {
 	u8 cur_ccasate;
 	u8 large_fa_hit;
 	u8 dig_dynamic_min;
+	u8 dig_dynamic_min_1;
 	u8 forbidden_igi;
 	u8 dig_state;
 	u8 dig_highpwrstate;
@@ -1972,6 +2227,7 @@ struct dig_t {
 	char backoffval_range_min;
 	u8 dig_min_0;
 	u8 dig_min_1;
+	u8 bt30_cur_igi;
 	bool media_connect_0;
 	bool media_connect_1;
 
@@ -1986,6 +2242,96 @@ struct rtl_global_var {
 	spinlock_t glb_list_lock;
 };
 
+struct rtl_btc_info {
+	u8 bt_type;
+	u8 btcoexist;
+	u8 ant_num;
+};
+
+struct bt_coexist_info {
+	struct rtl_btc_ops *btc_ops;
+	struct rtl_btc_info btc_info;
+	/* EEPROM BT info. */
+	u8 eeprom_bt_coexist;
+	u8 eeprom_bt_type;
+	u8 eeprom_bt_ant_num;
+	u8 eeprom_bt_ant_isol;
+	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 init_set;
+	bool bt_busy_traffic;
+	bool bt_traffic_mode_set;
+	bool bt_non_traffic_mode_set;
+
+	bool fw_coexist_all_off;
+	bool sw_coexist_all_off;
+	bool hw_coexist_all_off;
+	u32 cstate;
+	u32 previous_state;
+	u32 cstate_h;
+	u32 previous_state_h;
+
+	u8 bt_pre_rssi_state;
+	u8 bt_pre_rssi_state1;
+
+	u8 reg_bt_iso;
+	u8 reg_bt_sco;
+	bool balance_on;
+	u8 bt_active_zero_cnt;
+	bool cur_bt_disabled;
+	bool pre_bt_disabled;
+
+	u8 bt_profile_case;
+	u8 bt_profile_action;
+	bool bt_busy;
+	bool hold_for_bt_operation;
+	u8 lps_counter;
+};
+
+struct rtl_btc_ops {
+	void (*btc_init_variables) (struct rtl_priv *rtlpriv);
+	void (*btc_init_hal_vars) (struct rtl_priv *rtlpriv);
+	void (*btc_init_hw_config) (struct rtl_priv *rtlpriv);
+	void (*btc_ips_notify) (struct rtl_priv *rtlpriv, u8 type);
+	void (*btc_scan_notify) (struct rtl_priv *rtlpriv, u8 scantype);
+	void (*btc_connect_notify) (struct rtl_priv *rtlpriv, u8 action);
+	void (*btc_mediastatus_notify) (struct rtl_priv *rtlpriv,
+					enum _RT_MEDIA_STATUS mstatus);
+	void (*btc_periodical) (struct rtl_priv *rtlpriv);
+	void (*btc_halt_notify) (void);
+	void (*btc_btinfo_notify) (struct rtl_priv *rtlpriv,
+				   u8 *tmp_buf, u8 length);
+	bool (*btc_is_limited_dig) (struct rtl_priv *rtlpriv);
+	bool (*btc_is_disable_edca_turbo) (struct rtl_priv *rtlpriv);
+	bool (*btc_is_bt_disabled) (struct rtl_priv *rtlpriv);
+};
+
+struct 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_priv {
 	struct ieee80211_hw *hw;
 	struct completion firmware_loading_complete;
@@ -2008,6 +2354,7 @@ struct rtl_priv {
 
 	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;
@@ -2048,6 +2395,20 @@ struct rtl_priv {
 	bool enter_ps;	/* true when entering PS */
 	u8 rate_mask[5];
 
+	/* 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 bt_coexist_info btcoexist;
+
+	/* separate 92ee from other ICs,
+	 * 92ee use new trx flow.
+	 */
+	bool use_new_trx_flow;
+
 	/*This must be the last item so
 	   that it points to the data allocated
 	   beyond  this structure like:
@@ -2079,6 +2440,15 @@ enum bt_co_type {
 	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 {
@@ -2104,62 +2474,6 @@ enum bt_radio_shared {
 	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_isol;
-	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 init_set;
-	bool bt_busy_traffic;
-	bool bt_traffic_mode_set;
-	bool bt_non_traffic_mode_set;
-
-	bool fw_coexist_all_off;
-	bool sw_coexist_all_off;
-	bool hw_coexist_all_off;
-	u32 cstate;
-	u32 previous_state;
-	u32 cstate_h;
-	u32 previous_state_h;
-
-	u8 bt_pre_rssi_state;
-	u8 bt_pre_rssi_state1;
-
-	u8 reg_bt_iso;
-	u8 reg_bt_sco;
-	bool balance_on;
-	u8 bt_active_zero_cnt;
-	bool cur_bt_disabled;
-	bool pre_bt_disabled;
-
-	u8 bt_profile_case;
-	u8 bt_profile_action;
-	bool bt_busy;
-	bool hold_for_bt_operation;
-	u8 lps_counter;
-};
-
 
 /****************************************
 	mem access macro define start
diff --git a/drivers/net/wireless/ti/wilink_platform_data.c b/drivers/net/wireless/ti/wilink_platform_data.c
index 998e95895f9d..a92bd3e89796 100644
--- a/drivers/net/wireless/ti/wilink_platform_data.c
+++ b/drivers/net/wireless/ti/wilink_platform_data.c
@@ -23,17 +23,17 @@
 #include <linux/err.h>
 #include <linux/wl12xx.h>
 
-static struct wl12xx_platform_data *platform_data;
+static struct wl12xx_platform_data *wl12xx_platform_data;
 
 int __init wl12xx_set_platform_data(const struct wl12xx_platform_data *data)
 {
-	if (platform_data)
+	if (wl12xx_platform_data)
 		return -EBUSY;
 	if (!data)
 		return -EINVAL;
 
-	platform_data = kmemdup(data, sizeof(*data), GFP_KERNEL);
-	if (!platform_data)
+	wl12xx_platform_data = kmemdup(data, sizeof(*data), GFP_KERNEL);
+	if (!wl12xx_platform_data)
 		return -ENOMEM;
 
 	return 0;
@@ -41,9 +41,34 @@ int __init wl12xx_set_platform_data(const struct wl12xx_platform_data *data)
 
 struct wl12xx_platform_data *wl12xx_get_platform_data(void)
 {
-	if (!platform_data)
+	if (!wl12xx_platform_data)
 		return ERR_PTR(-ENODEV);
 
-	return platform_data;
+	return wl12xx_platform_data;
 }
 EXPORT_SYMBOL(wl12xx_get_platform_data);
+
+static struct wl1251_platform_data *wl1251_platform_data;
+
+int __init wl1251_set_platform_data(const struct wl1251_platform_data *data)
+{
+	if (wl1251_platform_data)
+		return -EBUSY;
+	if (!data)
+		return -EINVAL;
+
+	wl1251_platform_data = kmemdup(data, sizeof(*data), GFP_KERNEL);
+	if (!wl1251_platform_data)
+		return -ENOMEM;
+
+	return 0;
+}
+
+struct wl1251_platform_data *wl1251_get_platform_data(void)
+{
+	if (!wl1251_platform_data)
+		return ERR_PTR(-ENODEV);
+
+	return wl1251_platform_data;
+}
+EXPORT_SYMBOL(wl1251_get_platform_data);
diff --git a/drivers/net/wireless/ti/wl1251/cmd.c b/drivers/net/wireless/ti/wl1251/cmd.c
index 223649bcaa5a..bf1fa18b9786 100644
--- a/drivers/net/wireless/ti/wl1251/cmd.c
+++ b/drivers/net/wireless/ti/wl1251/cmd.c
@@ -448,7 +448,7 @@ int wl1251_cmd_scan(struct wl1251 *wl, u8 *ssid, size_t ssid_len,
 	 * Note: This bug may be caused by the fw's DTIM handling.
 	 */
 	if (is_zero_ether_addr(wl->bssid))
-		cmd->params.scan_options |= WL1251_SCAN_OPT_PRIORITY_HIGH;
+		cmd->params.scan_options |= cpu_to_le16(WL1251_SCAN_OPT_PRIORITY_HIGH);
 	cmd->params.num_channels = n_channels;
 	cmd->params.num_probe_requests = n_probes;
 	cmd->params.tx_rate = cpu_to_le16(1 << 1); /* 2 Mbps */
diff --git a/drivers/net/wireless/ti/wl1251/rx.c b/drivers/net/wireless/ti/wl1251/rx.c
index 123c4bb50e0a..cde0eaf99714 100644
--- a/drivers/net/wireless/ti/wl1251/rx.c
+++ b/drivers/net/wireless/ti/wl1251/rx.c
@@ -180,7 +180,7 @@ static void wl1251_rx_body(struct wl1251 *wl,
 	wl1251_mem_read(wl, rx_packet_ring_addr, rx_buffer, length);
 
 	/* The actual length doesn't include the target's alignment */
-	skb->len = desc->length  - PLCP_HEADER_LENGTH;
+	skb_trim(skb, desc->length - PLCP_HEADER_LENGTH);
 
 	fc = (u16 *)skb->data;
 
diff --git a/drivers/net/wireless/ti/wl1251/sdio.c b/drivers/net/wireless/ti/wl1251/sdio.c
index e2b3d9c541e8..b661f896e9fe 100644
--- a/drivers/net/wireless/ti/wl1251/sdio.c
+++ b/drivers/net/wireless/ti/wl1251/sdio.c
@@ -28,6 +28,7 @@
 #include <linux/wl12xx.h>
 #include <linux/irq.h>
 #include <linux/pm_runtime.h>
+#include <linux/gpio.h>
 
 #include "wl1251.h"
 
@@ -182,8 +183,9 @@ static int wl1251_sdio_set_power(struct wl1251 *wl, bool enable)
 		 * callback in case it wants to do any additional setup,
 		 * for example enabling clock buffer for the module.
 		 */
-		if (wl->set_power)
-			wl->set_power(true);
+		if (gpio_is_valid(wl->power_gpio))
+			gpio_set_value(wl->power_gpio, true);
+
 
 		ret = pm_runtime_get_sync(&func->dev);
 		if (ret < 0) {
@@ -203,8 +205,8 @@ static int wl1251_sdio_set_power(struct wl1251 *wl, bool enable)
 		if (ret < 0)
 			goto out;
 
-		if (wl->set_power)
-			wl->set_power(false);
+		if (gpio_is_valid(wl->power_gpio))
+			gpio_set_value(wl->power_gpio, false);
 	}
 
 out:
@@ -227,7 +229,7 @@ static int wl1251_sdio_probe(struct sdio_func *func,
 	struct wl1251 *wl;
 	struct ieee80211_hw *hw;
 	struct wl1251_sdio *wl_sdio;
-	const struct wl12xx_platform_data *wl12xx_board_data;
+	const struct wl1251_platform_data *wl1251_board_data;
 
 	hw = wl1251_alloc_hw();
 	if (IS_ERR(hw))
@@ -254,11 +256,20 @@ static int wl1251_sdio_probe(struct sdio_func *func,
 	wl->if_priv = wl_sdio;
 	wl->if_ops = &wl1251_sdio_ops;
 
-	wl12xx_board_data = wl12xx_get_platform_data();
-	if (!IS_ERR(wl12xx_board_data)) {
-		wl->set_power = wl12xx_board_data->set_power;
-		wl->irq = wl12xx_board_data->irq;
-		wl->use_eeprom = wl12xx_board_data->use_eeprom;
+	wl1251_board_data = wl1251_get_platform_data();
+	if (!IS_ERR(wl1251_board_data)) {
+		wl->power_gpio = wl1251_board_data->power_gpio;
+		wl->irq = wl1251_board_data->irq;
+		wl->use_eeprom = wl1251_board_data->use_eeprom;
+	}
+
+	if (gpio_is_valid(wl->power_gpio)) {
+		ret = devm_gpio_request(&func->dev, wl->power_gpio,
+								"wl1251 power");
+		if (ret) {
+			wl1251_error("Failed to request gpio: %d\n", ret);
+			goto disable;
+		}
 	}
 
 	if (wl->irq) {
diff --git a/drivers/net/wireless/ti/wl1251/spi.c b/drivers/net/wireless/ti/wl1251/spi.c
index 1342f81e683d..b06d36d99362 100644
--- a/drivers/net/wireless/ti/wl1251/spi.c
+++ b/drivers/net/wireless/ti/wl1251/spi.c
@@ -26,6 +26,10 @@
 #include <linux/crc7.h>
 #include <linux/spi/spi.h>
 #include <linux/wl12xx.h>
+#include <linux/gpio.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
+#include <linux/regulator/consumer.h>
 
 #include "wl1251.h"
 #include "reg.h"
@@ -221,8 +225,8 @@ static void wl1251_spi_disable_irq(struct wl1251 *wl)
 
 static int wl1251_spi_set_power(struct wl1251 *wl, bool enable)
 {
-	if (wl->set_power)
-		wl->set_power(enable);
+	if (gpio_is_valid(wl->power_gpio))
+		gpio_set_value(wl->power_gpio, enable);
 
 	return 0;
 }
@@ -238,13 +242,13 @@ static const struct wl1251_if_operations wl1251_spi_ops = {
 
 static int wl1251_spi_probe(struct spi_device *spi)
 {
-	struct wl12xx_platform_data *pdata;
+	struct wl1251_platform_data *pdata = dev_get_platdata(&spi->dev);
+	struct device_node *np = spi->dev.of_node;
 	struct ieee80211_hw *hw;
 	struct wl1251 *wl;
 	int ret;
 
-	pdata = dev_get_platdata(&spi->dev);
-	if (!pdata) {
+	if (!np && !pdata) {
 		wl1251_error("no platform data");
 		return -ENODEV;
 	}
@@ -271,22 +275,42 @@ static int wl1251_spi_probe(struct spi_device *spi)
 		goto out_free;
 	}
 
-	wl->set_power = pdata->set_power;
-	if (!wl->set_power) {
-		wl1251_error("set power function missing in platform data");
-		return -ENODEV;
+	if (np) {
+		wl->use_eeprom = of_property_read_bool(np, "ti,wl1251-has-eeprom");
+		wl->power_gpio = of_get_named_gpio(np, "ti,power-gpio", 0);
+	} else if (pdata) {
+		wl->power_gpio = pdata->power_gpio;
+		wl->use_eeprom = pdata->use_eeprom;
+	}
+
+	if (wl->power_gpio == -EPROBE_DEFER) {
+		ret = -EPROBE_DEFER;
+		goto out_free;
+	}
+
+	if (gpio_is_valid(wl->power_gpio)) {
+		ret = devm_gpio_request_one(&spi->dev, wl->power_gpio,
+					GPIOF_OUT_INIT_LOW, "wl1251 power");
+		if (ret) {
+			wl1251_error("Failed to request gpio: %d\n", ret);
+			goto out_free;
+		}
+	} else {
+		wl1251_error("set power gpio missing in platform data");
+		ret = -ENODEV;
+		goto out_free;
 	}
 
 	wl->irq = spi->irq;
 	if (wl->irq < 0) {
 		wl1251_error("irq missing in platform data");
-		return -ENODEV;
+		ret = -ENODEV;
+		goto out_free;
 	}
 
-	wl->use_eeprom = pdata->use_eeprom;
-
 	irq_set_status_flags(wl->irq, IRQ_NOAUTOEN);
-	ret = request_irq(wl->irq, wl1251_irq, 0, DRIVER_NAME, wl);
+	ret = devm_request_irq(&spi->dev, wl->irq, wl1251_irq, 0,
+							DRIVER_NAME, wl);
 	if (ret < 0) {
 		wl1251_error("request_irq() failed: %d", ret);
 		goto out_free;
@@ -294,16 +318,26 @@ static int wl1251_spi_probe(struct spi_device *spi)
 
 	irq_set_irq_type(wl->irq, IRQ_TYPE_EDGE_RISING);
 
+	wl->vio = devm_regulator_get(&spi->dev, "vio");
+	if (IS_ERR(wl->vio)) {
+		ret = PTR_ERR(wl->vio);
+		wl1251_error("vio regulator missing: %d", ret);
+		goto out_free;
+	}
+
+	ret = regulator_enable(wl->vio);
+	if (ret)
+		goto out_free;
+
 	ret = wl1251_init_ieee80211(wl);
 	if (ret)
-		goto out_irq;
+		goto disable_regulator;
 
 	return 0;
 
- out_irq:
-	free_irq(wl->irq, wl);
-
- out_free:
+disable_regulator:
+	regulator_disable(wl->vio);
+out_free:
 	ieee80211_free_hw(hw);
 
 	return ret;
@@ -315,6 +349,7 @@ static int wl1251_spi_remove(struct spi_device *spi)
 
 	free_irq(wl->irq, wl);
 	wl1251_free_hw(wl);
+	regulator_disable(wl->vio);
 
 	return 0;
 }
diff --git a/drivers/net/wireless/ti/wl1251/wl1251.h b/drivers/net/wireless/ti/wl1251/wl1251.h
index 235617a7716d..16dae5269175 100644
--- a/drivers/net/wireless/ti/wl1251/wl1251.h
+++ b/drivers/net/wireless/ti/wl1251/wl1251.h
@@ -276,10 +276,12 @@ struct wl1251 {
 	void *if_priv;
 	const struct wl1251_if_operations *if_ops;
 
-	void (*set_power)(bool enable);
+	int power_gpio;
 	int irq;
 	bool use_eeprom;
 
+	struct regulator *vio;
+
 	spinlock_t wl_lock;
 
 	enum wl1251_state state;
diff --git a/drivers/net/wireless/ti/wl12xx/main.c b/drivers/net/wireless/ti/wl12xx/main.c
index be7129ba16ad..d50dfac91631 100644
--- a/drivers/net/wireless/ti/wl12xx/main.c
+++ b/drivers/net/wireless/ti/wl12xx/main.c
@@ -1378,7 +1378,7 @@ static u32 wl12xx_get_rx_packet_len(struct wl1271 *wl, void *rx_data,
 
 static int wl12xx_tx_delayed_compl(struct wl1271 *wl)
 {
-	if (wl->fw_status_1->tx_results_counter ==
+	if (wl->fw_status->tx_results_counter ==
 	    (wl->tx_results_count & 0xff))
 		return 0;
 
@@ -1438,6 +1438,37 @@ out:
 	return ret;
 }
 
+static void wl12xx_convert_fw_status(struct wl1271 *wl, void *raw_fw_status,
+				     struct wl_fw_status *fw_status)
+{
+	struct wl12xx_fw_status *int_fw_status = raw_fw_status;
+
+	fw_status->intr = le32_to_cpu(int_fw_status->intr);
+	fw_status->fw_rx_counter = int_fw_status->fw_rx_counter;
+	fw_status->drv_rx_counter = int_fw_status->drv_rx_counter;
+	fw_status->tx_results_counter = int_fw_status->tx_results_counter;
+	fw_status->rx_pkt_descs = int_fw_status->rx_pkt_descs;
+
+	fw_status->fw_localtime = le32_to_cpu(int_fw_status->fw_localtime);
+	fw_status->link_ps_bitmap = le32_to_cpu(int_fw_status->link_ps_bitmap);
+	fw_status->link_fast_bitmap =
+			le32_to_cpu(int_fw_status->link_fast_bitmap);
+	fw_status->total_released_blks =
+			le32_to_cpu(int_fw_status->total_released_blks);
+	fw_status->tx_total = le32_to_cpu(int_fw_status->tx_total);
+
+	fw_status->counters.tx_released_pkts =
+			int_fw_status->counters.tx_released_pkts;
+	fw_status->counters.tx_lnk_free_pkts =
+			int_fw_status->counters.tx_lnk_free_pkts;
+	fw_status->counters.tx_voice_released_blks =
+			int_fw_status->counters.tx_voice_released_blks;
+	fw_status->counters.tx_last_rate =
+			int_fw_status->counters.tx_last_rate;
+
+	fw_status->log_start_addr = le32_to_cpu(int_fw_status->log_start_addr);
+}
+
 static u32 wl12xx_sta_get_ap_rate_mask(struct wl1271 *wl,
 				       struct wl12xx_vif *wlvif)
 {
@@ -1677,6 +1708,7 @@ static struct wlcore_ops wl12xx_ops = {
 	.tx_delayed_compl	= wl12xx_tx_delayed_compl,
 	.hw_init		= wl12xx_hw_init,
 	.init_vif		= NULL,
+	.convert_fw_status	= wl12xx_convert_fw_status,
 	.sta_get_ap_rate_mask	= wl12xx_sta_get_ap_rate_mask,
 	.get_pg_ver		= wl12xx_get_pg_ver,
 	.get_mac		= wl12xx_get_mac,
@@ -1711,22 +1743,53 @@ static struct ieee80211_sta_ht_cap wl12xx_ht_cap = {
 		},
 };
 
+static const struct ieee80211_iface_limit wl12xx_iface_limits[] = {
+	{
+		.max = 3,
+		.types = BIT(NL80211_IFTYPE_STATION),
+	},
+	{
+		.max = 1,
+		.types = BIT(NL80211_IFTYPE_AP) |
+			 BIT(NL80211_IFTYPE_P2P_GO) |
+			 BIT(NL80211_IFTYPE_P2P_CLIENT),
+	},
+};
+
+static const struct ieee80211_iface_combination
+wl12xx_iface_combinations[] = {
+	{
+		.max_interfaces = 3,
+		.limits = wl12xx_iface_limits,
+		.n_limits = ARRAY_SIZE(wl12xx_iface_limits),
+		.num_different_channels = 1,
+	},
+};
+
 static int wl12xx_setup(struct wl1271 *wl)
 {
 	struct wl12xx_priv *priv = wl->priv;
 	struct wlcore_platdev_data *pdev_data = dev_get_platdata(&wl->pdev->dev);
 	struct wl12xx_platform_data *pdata = pdev_data->pdata;
 
+	BUILD_BUG_ON(WL12XX_MAX_LINKS > WLCORE_MAX_LINKS);
+	BUILD_BUG_ON(WL12XX_MAX_AP_STATIONS > WL12XX_MAX_LINKS);
+
 	wl->rtable = wl12xx_rtable;
 	wl->num_tx_desc = WL12XX_NUM_TX_DESCRIPTORS;
 	wl->num_rx_desc = WL12XX_NUM_RX_DESCRIPTORS;
-	wl->num_channels = 1;
+	wl->num_links = WL12XX_MAX_LINKS;
+	wl->max_ap_stations = WL12XX_MAX_AP_STATIONS;
+	wl->iface_combinations = wl12xx_iface_combinations;
+	wl->n_iface_combinations = ARRAY_SIZE(wl12xx_iface_combinations);
 	wl->num_mac_addr = WL12XX_NUM_MAC_ADDRESSES;
 	wl->band_rate_to_idx = wl12xx_band_rate_to_idx;
 	wl->hw_tx_rate_tbl_size = WL12XX_CONF_HW_RXTX_RATE_MAX;
 	wl->hw_min_ht_rate = WL12XX_CONF_HW_RXTX_RATE_MCS0;
+	wl->fw_status_len = sizeof(struct wl12xx_fw_status);
 	wl->fw_status_priv_len = 0;
 	wl->stats.fw_stats_len = sizeof(struct wl12xx_acx_statistics);
+	wl->ofdm_only_ap = true;
 	wlcore_set_ht_cap(wl, IEEE80211_BAND_2GHZ, &wl12xx_ht_cap);
 	wlcore_set_ht_cap(wl, IEEE80211_BAND_5GHZ, &wl12xx_ht_cap);
 	wl12xx_conf_init(wl);
diff --git a/drivers/net/wireless/ti/wl12xx/wl12xx.h b/drivers/net/wireless/ti/wl12xx/wl12xx.h
index 9e5484a73667..75c92658bfea 100644
--- a/drivers/net/wireless/ti/wl12xx/wl12xx.h
+++ b/drivers/net/wireless/ti/wl12xx/wl12xx.h
@@ -65,6 +65,9 @@
 
 #define WL12XX_RX_BA_MAX_SESSIONS 3
 
+#define WL12XX_MAX_AP_STATIONS 8
+#define WL12XX_MAX_LINKS 12
+
 struct wl127x_rx_mem_pool_addr {
 	u32 addr;
 	u32 addr_extra;
@@ -79,4 +82,54 @@ struct wl12xx_priv {
 	struct wl127x_rx_mem_pool_addr *rx_mem_addr;
 };
 
+struct wl12xx_fw_packet_counters {
+	/* Cumulative counter of released packets per AC */
+	u8 tx_released_pkts[NUM_TX_QUEUES];
+
+	/* Cumulative counter of freed packets per HLID */
+	u8 tx_lnk_free_pkts[WL12XX_MAX_LINKS];
+
+	/* Cumulative counter of released Voice memory blocks */
+	u8 tx_voice_released_blks;
+
+	/* Tx rate of the last transmitted packet */
+	u8 tx_last_rate;
+
+	u8 padding[2];
+} __packed;
+
+/* FW status registers */
+struct wl12xx_fw_status {
+	__le32 intr;
+	u8  fw_rx_counter;
+	u8  drv_rx_counter;
+	u8  reserved;
+	u8  tx_results_counter;
+	__le32 rx_pkt_descs[WL12XX_NUM_RX_DESCRIPTORS];
+
+	__le32 fw_localtime;
+
+	/*
+	 * A bitmap (where each bit represents a single HLID)
+	 * to indicate if the station is in PS mode.
+	 */
+	__le32 link_ps_bitmap;
+
+	/*
+	 * A bitmap (where each bit represents a single HLID) to indicate
+	 * if the station is in Fast mode
+	 */
+	__le32 link_fast_bitmap;
+
+	/* Cumulative counter of total released mem blocks since FW-reset */
+	__le32 total_released_blks;
+
+	/* Size (in Memory Blocks) of TX pool */
+	__le32 tx_total;
+
+	struct wl12xx_fw_packet_counters counters;
+
+	__le32 log_start_addr;
+} __packed;
+
 #endif /* __WL12XX_PRIV_H__ */
diff --git a/drivers/net/wireless/ti/wl18xx/main.c b/drivers/net/wireless/ti/wl18xx/main.c
index ec37b16585df..de5b4fa5d166 100644
--- a/drivers/net/wireless/ti/wl18xx/main.c
+++ b/drivers/net/wireless/ti/wl18xx/main.c
@@ -648,7 +648,7 @@ static const struct wl18xx_clk_cfg wl18xx_clk_table[NUM_CLOCK_CONFIGS] = {
 };
 
 /* TODO: maybe move to a new header file? */
-#define WL18XX_FW_NAME "ti-connectivity/wl18xx-fw-2.bin"
+#define WL18XX_FW_NAME "ti-connectivity/wl18xx-fw-3.bin"
 
 static int wl18xx_identify_chip(struct wl1271 *wl)
 {
@@ -1133,6 +1133,39 @@ static int wl18xx_hw_init(struct wl1271 *wl)
 	return ret;
 }
 
+static void wl18xx_convert_fw_status(struct wl1271 *wl, void *raw_fw_status,
+				     struct wl_fw_status *fw_status)
+{
+	struct wl18xx_fw_status *int_fw_status = raw_fw_status;
+
+	fw_status->intr = le32_to_cpu(int_fw_status->intr);
+	fw_status->fw_rx_counter = int_fw_status->fw_rx_counter;
+	fw_status->drv_rx_counter = int_fw_status->drv_rx_counter;
+	fw_status->tx_results_counter = int_fw_status->tx_results_counter;
+	fw_status->rx_pkt_descs = int_fw_status->rx_pkt_descs;
+
+	fw_status->fw_localtime = le32_to_cpu(int_fw_status->fw_localtime);
+	fw_status->link_ps_bitmap = le32_to_cpu(int_fw_status->link_ps_bitmap);
+	fw_status->link_fast_bitmap =
+			le32_to_cpu(int_fw_status->link_fast_bitmap);
+	fw_status->total_released_blks =
+			le32_to_cpu(int_fw_status->total_released_blks);
+	fw_status->tx_total = le32_to_cpu(int_fw_status->tx_total);
+
+	fw_status->counters.tx_released_pkts =
+			int_fw_status->counters.tx_released_pkts;
+	fw_status->counters.tx_lnk_free_pkts =
+			int_fw_status->counters.tx_lnk_free_pkts;
+	fw_status->counters.tx_voice_released_blks =
+			int_fw_status->counters.tx_voice_released_blks;
+	fw_status->counters.tx_last_rate =
+			int_fw_status->counters.tx_last_rate;
+
+	fw_status->log_start_addr = le32_to_cpu(int_fw_status->log_start_addr);
+
+	fw_status->priv = &int_fw_status->priv;
+}
+
 static void wl18xx_set_tx_desc_csum(struct wl1271 *wl,
 				    struct wl1271_tx_hw_descr *desc,
 				    struct sk_buff *skb)
@@ -1572,7 +1605,7 @@ static bool wl18xx_lnk_high_prio(struct wl1271 *wl, u8 hlid,
 {
 	u8 thold;
 	struct wl18xx_fw_status_priv *status_priv =
-		(struct wl18xx_fw_status_priv *)wl->fw_status_2->priv;
+		(struct wl18xx_fw_status_priv *)wl->fw_status->priv;
 	u32 suspend_bitmap = le32_to_cpu(status_priv->link_suspend_bitmap);
 
 	/* suspended links are never high priority */
@@ -1594,7 +1627,7 @@ static bool wl18xx_lnk_low_prio(struct wl1271 *wl, u8 hlid,
 {
 	u8 thold;
 	struct wl18xx_fw_status_priv *status_priv =
-		(struct wl18xx_fw_status_priv *)wl->fw_status_2->priv;
+		(struct wl18xx_fw_status_priv *)wl->fw_status->priv;
 	u32 suspend_bitmap = le32_to_cpu(status_priv->link_suspend_bitmap);
 
 	if (test_bit(hlid, (unsigned long *)&suspend_bitmap))
@@ -1632,6 +1665,7 @@ static struct wlcore_ops wl18xx_ops = {
 	.tx_immediate_compl = wl18xx_tx_immediate_completion,
 	.tx_delayed_compl = NULL,
 	.hw_init	= wl18xx_hw_init,
+	.convert_fw_status = wl18xx_convert_fw_status,
 	.set_tx_desc_csum = wl18xx_set_tx_desc_csum,
 	.get_pg_ver	= wl18xx_get_pg_ver,
 	.set_rx_csum = wl18xx_set_rx_csum,
@@ -1713,19 +1747,62 @@ static struct ieee80211_sta_ht_cap wl18xx_mimo_ht_cap_2ghz = {
 		},
 };
 
+static const struct ieee80211_iface_limit wl18xx_iface_limits[] = {
+	{
+		.max = 3,
+		.types = BIT(NL80211_IFTYPE_STATION),
+	},
+	{
+		.max = 1,
+		.types = BIT(NL80211_IFTYPE_AP) |
+			 BIT(NL80211_IFTYPE_P2P_GO) |
+			 BIT(NL80211_IFTYPE_P2P_CLIENT),
+	},
+};
+
+static const struct ieee80211_iface_limit wl18xx_iface_ap_limits[] = {
+	{
+		.max = 2,
+		.types = BIT(NL80211_IFTYPE_AP),
+	},
+};
+
+static const struct ieee80211_iface_combination
+wl18xx_iface_combinations[] = {
+	{
+		.max_interfaces = 3,
+		.limits = wl18xx_iface_limits,
+		.n_limits = ARRAY_SIZE(wl18xx_iface_limits),
+		.num_different_channels = 2,
+	},
+	{
+		.max_interfaces = 2,
+		.limits = wl18xx_iface_ap_limits,
+		.n_limits = ARRAY_SIZE(wl18xx_iface_ap_limits),
+		.num_different_channels = 1,
+	}
+};
+
 static int wl18xx_setup(struct wl1271 *wl)
 {
 	struct wl18xx_priv *priv = wl->priv;
 	int ret;
 
+	BUILD_BUG_ON(WL18XX_MAX_LINKS > WLCORE_MAX_LINKS);
+	BUILD_BUG_ON(WL18XX_MAX_AP_STATIONS > WL18XX_MAX_LINKS);
+
 	wl->rtable = wl18xx_rtable;
 	wl->num_tx_desc = WL18XX_NUM_TX_DESCRIPTORS;
 	wl->num_rx_desc = WL18XX_NUM_RX_DESCRIPTORS;
-	wl->num_channels = 2;
+	wl->num_links = WL18XX_MAX_LINKS;
+	wl->max_ap_stations = WL18XX_MAX_AP_STATIONS;
+	wl->iface_combinations = wl18xx_iface_combinations;
+	wl->n_iface_combinations = ARRAY_SIZE(wl18xx_iface_combinations);
 	wl->num_mac_addr = WL18XX_NUM_MAC_ADDRESSES;
 	wl->band_rate_to_idx = wl18xx_band_rate_to_idx;
 	wl->hw_tx_rate_tbl_size = WL18XX_CONF_HW_RXTX_RATE_MAX;
 	wl->hw_min_ht_rate = WL18XX_CONF_HW_RXTX_RATE_MCS0;
+	wl->fw_status_len = sizeof(struct wl18xx_fw_status);
 	wl->fw_status_priv_len = sizeof(struct wl18xx_fw_status_priv);
 	wl->stats.fw_stats_len = sizeof(struct wl18xx_acx_statistics);
 	wl->static_data_priv_len = sizeof(struct wl18xx_static_data_priv);
diff --git a/drivers/net/wireless/ti/wl18xx/tx.c b/drivers/net/wireless/ti/wl18xx/tx.c
index 57c694396647..be1ebd55ac88 100644
--- a/drivers/net/wireless/ti/wl18xx/tx.c
+++ b/drivers/net/wireless/ti/wl18xx/tx.c
@@ -32,7 +32,7 @@ static
 void wl18xx_get_last_tx_rate(struct wl1271 *wl, struct ieee80211_vif *vif,
 			     struct ieee80211_tx_rate *rate)
 {
-	u8 fw_rate = wl->fw_status_2->counters.tx_last_rate;
+	u8 fw_rate = wl->fw_status->counters.tx_last_rate;
 
 	if (fw_rate > CONF_HW_RATE_INDEX_MAX) {
 		wl1271_error("last Tx rate invalid: %d", fw_rate);
@@ -139,7 +139,7 @@ static void wl18xx_tx_complete_packet(struct wl1271 *wl, u8 tx_stat_byte)
 void wl18xx_tx_immediate_complete(struct wl1271 *wl)
 {
 	struct wl18xx_fw_status_priv *status_priv =
-		(struct wl18xx_fw_status_priv *)wl->fw_status_2->priv;
+		(struct wl18xx_fw_status_priv *)wl->fw_status->priv;
 	struct wl18xx_priv *priv = wl->priv;
 	u8 i;
 
diff --git a/drivers/net/wireless/ti/wl18xx/wl18xx.h b/drivers/net/wireless/ti/wl18xx/wl18xx.h
index 9204e07ee432..eb7cfe817010 100644
--- a/drivers/net/wireless/ti/wl18xx/wl18xx.h
+++ b/drivers/net/wireless/ti/wl18xx/wl18xx.h
@@ -26,10 +26,10 @@
 
 /* minimum FW required for driver */
 #define WL18XX_CHIP_VER		8
-#define WL18XX_IFTYPE_VER	5
+#define WL18XX_IFTYPE_VER	8
 #define WL18XX_MAJOR_VER	WLCORE_FW_VER_IGNORE
 #define WL18XX_SUBTYPE_VER	WLCORE_FW_VER_IGNORE
-#define WL18XX_MINOR_VER	39
+#define WL18XX_MINOR_VER	13
 
 #define WL18XX_CMD_MAX_SIZE          740
 
@@ -40,7 +40,10 @@
 
 #define WL18XX_NUM_MAC_ADDRESSES 3
 
-#define WL18XX_RX_BA_MAX_SESSIONS 5
+#define WL18XX_RX_BA_MAX_SESSIONS 13
+
+#define WL18XX_MAX_AP_STATIONS 10
+#define WL18XX_MAX_LINKS 16
 
 struct wl18xx_priv {
 	/* buffer for sending commands to FW */
@@ -109,6 +112,59 @@ struct wl18xx_fw_status_priv {
 	u8 padding[3];
 };
 
+struct wl18xx_fw_packet_counters {
+	/* Cumulative counter of released packets per AC */
+	u8 tx_released_pkts[NUM_TX_QUEUES];
+
+	/* Cumulative counter of freed packets per HLID */
+	u8 tx_lnk_free_pkts[WL18XX_MAX_LINKS];
+
+	/* Cumulative counter of released Voice memory blocks */
+	u8 tx_voice_released_blks;
+
+	/* Tx rate of the last transmitted packet */
+	u8 tx_last_rate;
+
+	u8 padding[2];
+} __packed;
+
+/* FW status registers */
+struct wl18xx_fw_status {
+	__le32 intr;
+	u8  fw_rx_counter;
+	u8  drv_rx_counter;
+	u8  reserved;
+	u8  tx_results_counter;
+	__le32 rx_pkt_descs[WL18XX_NUM_RX_DESCRIPTORS];
+
+	__le32 fw_localtime;
+
+	/*
+	 * A bitmap (where each bit represents a single HLID)
+	 * to indicate if the station is in PS mode.
+	 */
+	__le32 link_ps_bitmap;
+
+	/*
+	 * A bitmap (where each bit represents a single HLID) to indicate
+	 * if the station is in Fast mode
+	 */
+	__le32 link_fast_bitmap;
+
+	/* Cumulative counter of total released mem blocks since FW-reset */
+	__le32 total_released_blks;
+
+	/* Size (in Memory Blocks) of TX pool */
+	__le32 tx_total;
+
+	struct wl18xx_fw_packet_counters counters;
+
+	__le32 log_start_addr;
+
+	/* Private status to be used by the lower drivers */
+	struct wl18xx_fw_status_priv priv;
+} __packed;
+
 #define WL18XX_PHY_VERSION_MAX_LEN 20
 
 struct wl18xx_static_data_priv {
diff --git a/drivers/net/wireless/ti/wlcore/acx.c b/drivers/net/wireless/ti/wlcore/acx.c
index ec83675a2446..b924ceadc02c 100644
--- a/drivers/net/wireless/ti/wlcore/acx.c
+++ b/drivers/net/wireless/ti/wlcore/acx.c
@@ -358,7 +358,8 @@ int wl1271_acx_beacon_filter_opt(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 	struct acx_beacon_filter_option *beacon_filter = NULL;
 	int ret = 0;
 
-	wl1271_debug(DEBUG_ACX, "acx beacon filter opt");
+	wl1271_debug(DEBUG_ACX, "acx beacon filter opt enable=%d",
+		     enable_filter);
 
 	if (enable_filter &&
 	    wl->conf.conn.bcn_filt_mode == CONF_BCN_FILT_MODE_DISABLED)
@@ -1591,7 +1592,8 @@ out:
 	return ret;
 }
 
-int wl1271_acx_set_inconnection_sta(struct wl1271 *wl, u8 *addr)
+int wl1271_acx_set_inconnection_sta(struct wl1271 *wl,
+				    struct wl12xx_vif *wlvif, u8 *addr)
 {
 	struct wl1271_acx_inconnection_sta *acx = NULL;
 	int ret;
@@ -1603,6 +1605,7 @@ int wl1271_acx_set_inconnection_sta(struct wl1271 *wl, u8 *addr)
 		return -ENOMEM;
 
 	memcpy(acx->addr, addr, ETH_ALEN);
+	acx->role_id = wlvif->role_id;
 
 	ret = wl1271_cmd_configure(wl, ACX_UPDATE_INCONNECTION_STA_LIST,
 				   acx, sizeof(*acx));
diff --git a/drivers/net/wireless/ti/wlcore/acx.h b/drivers/net/wireless/ti/wlcore/acx.h
index 6dcfad9b0472..954d57ec98f4 100644
--- a/drivers/net/wireless/ti/wlcore/acx.h
+++ b/drivers/net/wireless/ti/wlcore/acx.h
@@ -824,7 +824,8 @@ struct wl1271_acx_inconnection_sta {
 	struct acx_header header;
 
 	u8 addr[ETH_ALEN];
-	u8 padding1[2];
+	u8 role_id;
+	u8 padding;
 } __packed;
 
 /*
@@ -1118,7 +1119,8 @@ int wl1271_acx_ps_rx_streaming(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 			       bool enable);
 int wl1271_acx_ap_max_tx_retry(struct wl1271 *wl, struct wl12xx_vif *wlvif);
 int wl12xx_acx_config_ps(struct wl1271 *wl, struct wl12xx_vif *wlvif);
-int wl1271_acx_set_inconnection_sta(struct wl1271 *wl, u8 *addr);
+int wl1271_acx_set_inconnection_sta(struct wl1271 *wl,
+				    struct wl12xx_vif *wlvif, u8 *addr);
 int wl1271_acx_fm_coex(struct wl1271 *wl);
 int wl12xx_acx_set_rate_mgmt_params(struct wl1271 *wl);
 int wl12xx_acx_config_hangover(struct wl1271 *wl);
diff --git a/drivers/net/wireless/ti/wlcore/cmd.c b/drivers/net/wireless/ti/wlcore/cmd.c
index 9b2ecf52449f..40dc30f4faaa 100644
--- a/drivers/net/wireless/ti/wlcore/cmd.c
+++ b/drivers/net/wireless/ti/wlcore/cmd.c
@@ -60,8 +60,8 @@ static int __wlcore_cmd_send(struct wl1271 *wl, u16 id, void *buf,
 	u16 status;
 	u16 poll_count = 0;
 
-	if (WARN_ON(wl->state == WLCORE_STATE_RESTARTING &&
-		    id != CMD_STOP_FWLOGGER))
+	if (unlikely(wl->state == WLCORE_STATE_RESTARTING &&
+		     id != CMD_STOP_FWLOGGER))
 		return -EIO;
 
 	cmd = buf;
@@ -312,8 +312,8 @@ static int wlcore_get_new_session_id(struct wl1271 *wl, u8 hlid)
 int wl12xx_allocate_link(struct wl1271 *wl, struct wl12xx_vif *wlvif, u8 *hlid)
 {
 	unsigned long flags;
-	u8 link = find_first_zero_bit(wl->links_map, WL12XX_MAX_LINKS);
-	if (link >= WL12XX_MAX_LINKS)
+	u8 link = find_first_zero_bit(wl->links_map, wl->num_links);
+	if (link >= wl->num_links)
 		return -EBUSY;
 
 	wl->session_ids[link] = wlcore_get_new_session_id(wl, link);
@@ -324,9 +324,14 @@ int wl12xx_allocate_link(struct wl1271 *wl, struct wl12xx_vif *wlvif, u8 *hlid)
 	__set_bit(link, wlvif->links_map);
 	spin_unlock_irqrestore(&wl->wl_lock, flags);
 
-	/* take the last "freed packets" value from the current FW status */
-	wl->links[link].prev_freed_pkts =
-			wl->fw_status_2->counters.tx_lnk_free_pkts[link];
+	/*
+	 * take the last "freed packets" value from the current FW status.
+	 * on recovery, we might not have fw_status yet, and
+	 * tx_lnk_free_pkts will be NULL. check for it.
+	 */
+	if (wl->fw_status->counters.tx_lnk_free_pkts)
+		wl->links[link].prev_freed_pkts =
+			wl->fw_status->counters.tx_lnk_free_pkts[link];
 	wl->links[link].wlvif = wlvif;
 
 	/*
@@ -1527,6 +1532,7 @@ int wl12xx_cmd_add_peer(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 	cmd->sp_len = sta->max_sp;
 	cmd->wmm = sta->wme ? 1 : 0;
 	cmd->session_id = wl->session_ids[hlid];
+	cmd->role_id = wlvif->role_id;
 
 	for (i = 0; i < NUM_ACCESS_CATEGORIES_COPY; i++)
 		if (sta->wme && (sta->uapsd_queues & BIT(i)))
@@ -1563,7 +1569,8 @@ out:
 	return ret;
 }
 
-int wl12xx_cmd_remove_peer(struct wl1271 *wl, u8 hlid)
+int wl12xx_cmd_remove_peer(struct wl1271 *wl, struct wl12xx_vif *wlvif,
+			   u8 hlid)
 {
 	struct wl12xx_cmd_remove_peer *cmd;
 	int ret;
@@ -1581,6 +1588,7 @@ int wl12xx_cmd_remove_peer(struct wl1271 *wl, u8 hlid)
 	/* We never send a deauth, mac80211 is in charge of this */
 	cmd->reason_opcode = 0;
 	cmd->send_deauth_flag = 0;
+	cmd->role_id = wlvif->role_id;
 
 	ret = wl1271_cmd_send(wl, CMD_REMOVE_PEER, cmd, sizeof(*cmd), 0);
 	if (ret < 0) {
diff --git a/drivers/net/wireless/ti/wlcore/cmd.h b/drivers/net/wireless/ti/wlcore/cmd.h
index 323d4a856e4b..b084830a61cf 100644
--- a/drivers/net/wireless/ti/wlcore/cmd.h
+++ b/drivers/net/wireless/ti/wlcore/cmd.h
@@ -88,7 +88,8 @@ int wl12xx_roc(struct wl1271 *wl, struct wl12xx_vif *wlvif, u8 role_id,
 int wl12xx_croc(struct wl1271 *wl, u8 role_id);
 int wl12xx_cmd_add_peer(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 			struct ieee80211_sta *sta, u8 hlid);
-int wl12xx_cmd_remove_peer(struct wl1271 *wl, u8 hlid);
+int wl12xx_cmd_remove_peer(struct wl1271 *wl, struct wl12xx_vif *wlvif,
+			   u8 hlid);
 void wlcore_set_pending_regdomain_ch(struct wl1271 *wl, u16 channel,
 				     enum ieee80211_band band);
 int wlcore_cmd_regdomain_config_locked(struct wl1271 *wl);
@@ -206,7 +207,7 @@ enum cmd_templ {
 #define WL1271_COMMAND_TIMEOUT     2000
 #define WL1271_CMD_TEMPL_DFLT_SIZE 252
 #define WL1271_CMD_TEMPL_MAX_SIZE  512
-#define WL1271_EVENT_TIMEOUT       1500
+#define WL1271_EVENT_TIMEOUT       5000
 
 struct wl1271_cmd_header {
 	__le16 id;
@@ -594,6 +595,8 @@ struct wl12xx_cmd_add_peer {
 	u8 sp_len;
 	u8 wmm;
 	u8 session_id;
+	u8 role_id;
+	u8 padding[3];
 } __packed;
 
 struct wl12xx_cmd_remove_peer {
@@ -602,7 +605,7 @@ struct wl12xx_cmd_remove_peer {
 	u8 hlid;
 	u8 reason_opcode;
 	u8 send_deauth_flag;
-	u8 padding1;
+	u8 role_id;
 } __packed;
 
 /*
diff --git a/drivers/net/wireless/ti/wlcore/event.c b/drivers/net/wireless/ti/wlcore/event.c
index 8d3b34965db3..1f9a36031b06 100644
--- a/drivers/net/wireless/ti/wlcore/event.c
+++ b/drivers/net/wireless/ti/wlcore/event.c
@@ -67,7 +67,7 @@ static void wl1271_stop_ba_event(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 		u8 hlid;
 		struct wl1271_link *lnk;
 		for_each_set_bit(hlid, wlvif->ap.sta_hlid_map,
-				 WL12XX_MAX_LINKS) {
+				 wl->num_links) {
 			lnk = &wl->links[hlid];
 			if (!lnk->ba_bitmap)
 				continue;
@@ -172,7 +172,7 @@ static void wlcore_disconnect_sta(struct wl1271 *wl, unsigned long sta_bitmap)
 	const u8 *addr;
 	int h;
 
-	for_each_set_bit(h, &sta_bitmap, WL12XX_MAX_LINKS) {
+	for_each_set_bit(h, &sta_bitmap, wl->num_links) {
 		bool found = false;
 		/* find the ap vif connected to this sta */
 		wl12xx_for_each_wlvif_ap(wl, wlvif) {
diff --git a/drivers/net/wireless/ti/wlcore/hw_ops.h b/drivers/net/wireless/ti/wlcore/hw_ops.h
index 51f8d634d32f..1555ff970050 100644
--- a/drivers/net/wireless/ti/wlcore/hw_ops.h
+++ b/drivers/net/wireless/ti/wlcore/hw_ops.h
@@ -106,6 +106,15 @@ wlcore_hw_init_vif(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 	return 0;
 }
 
+static inline void
+wlcore_hw_convert_fw_status(struct wl1271 *wl, void *raw_fw_status,
+			    struct wl_fw_status *fw_status)
+{
+	BUG_ON(!wl->ops->convert_fw_status);
+
+	wl->ops->convert_fw_status(wl, raw_fw_status, fw_status);
+}
+
 static inline u32
 wlcore_hw_sta_get_ap_rate_mask(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 {
diff --git a/drivers/net/wireless/ti/wlcore/init.c b/drivers/net/wireless/ti/wlcore/init.c
index 7699f9d07e26..199e94120864 100644
--- a/drivers/net/wireless/ti/wlcore/init.c
+++ b/drivers/net/wireless/ti/wlcore/init.c
@@ -287,8 +287,8 @@ static int wl1271_init_sta_beacon_filter(struct wl1271 *wl,
 	if (ret < 0)
 		return ret;
 
-	/* enable beacon filtering */
-	ret = wl1271_acx_beacon_filter_opt(wl, wlvif, true);
+	/* disable beacon filtering until we get the first beacon */
+	ret = wl1271_acx_beacon_filter_opt(wl, wlvif, false);
 	if (ret < 0)
 		return ret;
 
@@ -462,7 +462,7 @@ int wl1271_init_ap_rates(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 	 * If the basic rates contain OFDM rates, use OFDM only
 	 * rates for unicast TX as well. Else use all supported rates.
 	 */
-	if ((wlvif->basic_rate_set & CONF_TX_OFDM_RATES))
+	if (wl->ofdm_only_ap && (wlvif->basic_rate_set & CONF_TX_OFDM_RATES))
 		supported_rates = CONF_TX_OFDM_RATES;
 	else
 		supported_rates = CONF_TX_ENABLED_RATES;
diff --git a/drivers/net/wireless/ti/wlcore/io.h b/drivers/net/wireless/ti/wlcore/io.h
index 07e3d6a049ad..0305729d0986 100644
--- a/drivers/net/wireless/ti/wlcore/io.h
+++ b/drivers/net/wireless/ti/wlcore/io.h
@@ -60,7 +60,9 @@ static inline int __must_check wlcore_raw_write(struct wl1271 *wl, int addr,
 {
 	int ret;
 
-	if (test_bit(WL1271_FLAG_IO_FAILED, &wl->flags))
+	if (test_bit(WL1271_FLAG_IO_FAILED, &wl->flags) ||
+	    WARN_ON((test_bit(WL1271_FLAG_IN_ELP, &wl->flags) &&
+		     addr != HW_ACCESS_ELP_CTRL_REG)))
 		return -EIO;
 
 	ret = wl->if_ops->write(wl->dev, addr, buf, len, fixed);
@@ -76,7 +78,9 @@ static inline int __must_check wlcore_raw_read(struct wl1271 *wl, int addr,
 {
 	int ret;
 
-	if (test_bit(WL1271_FLAG_IO_FAILED, &wl->flags))
+	if (test_bit(WL1271_FLAG_IO_FAILED, &wl->flags) ||
+	    WARN_ON((test_bit(WL1271_FLAG_IN_ELP, &wl->flags) &&
+		     addr != HW_ACCESS_ELP_CTRL_REG)))
 		return -EIO;
 
 	ret = wl->if_ops->read(wl->dev, addr, buf, len, fixed);
diff --git a/drivers/net/wireless/ti/wlcore/main.c b/drivers/net/wireless/ti/wlcore/main.c
index b46b3116cc55..ed88d3913483 100644
--- a/drivers/net/wireless/ti/wlcore/main.c
+++ b/drivers/net/wireless/ti/wlcore/main.c
@@ -345,24 +345,24 @@ static void wl12xx_irq_ps_regulate_link(struct wl1271 *wl,
 	 * Start high-level PS if the STA is asleep with enough blocks in FW.
 	 * Make an exception if this is the only connected link. In this
 	 * case FW-memory congestion is less of a problem.
-	 * Note that a single connected STA means 3 active links, since we must
-	 * account for the global and broadcast AP links. The "fw_ps" check
-	 * assures us the third link is a STA connected to the AP. Otherwise
-	 * the FW would not set the PSM bit.
+	 * Note that a single connected STA means 2*ap_count + 1 active links,
+	 * since we must account for the global and broadcast AP links
+	 * for each AP. The "fw_ps" check assures us the other link is a STA
+	 * connected to the AP. Otherwise the FW would not set the PSM bit.
 	 */
-	else if (wl->active_link_count > 3 && fw_ps &&
+	else if (wl->active_link_count > (wl->ap_count*2 + 1) && fw_ps &&
 		 tx_pkts >= WL1271_PS_STA_MAX_PACKETS)
 		wl12xx_ps_link_start(wl, wlvif, hlid, true);
 }
 
 static void wl12xx_irq_update_links_status(struct wl1271 *wl,
 					   struct wl12xx_vif *wlvif,
-					   struct wl_fw_status_2 *status)
+					   struct wl_fw_status *status)
 {
 	u32 cur_fw_ps_map;
 	u8 hlid;
 
-	cur_fw_ps_map = le32_to_cpu(status->link_ps_bitmap);
+	cur_fw_ps_map = status->link_ps_bitmap;
 	if (wl->ap_fw_ps_map != cur_fw_ps_map) {
 		wl1271_debug(DEBUG_PSM,
 			     "link ps prev 0x%x cur 0x%x changed 0x%x",
@@ -372,77 +372,73 @@ static void wl12xx_irq_update_links_status(struct wl1271 *wl,
 		wl->ap_fw_ps_map = cur_fw_ps_map;
 	}
 
-	for_each_set_bit(hlid, wlvif->ap.sta_hlid_map, WL12XX_MAX_LINKS)
+	for_each_set_bit(hlid, wlvif->ap.sta_hlid_map, wl->num_links)
 		wl12xx_irq_ps_regulate_link(wl, wlvif, hlid,
 					    wl->links[hlid].allocated_pkts);
 }
 
-static int wlcore_fw_status(struct wl1271 *wl,
-			    struct wl_fw_status_1 *status_1,
-			    struct wl_fw_status_2 *status_2)
+static int wlcore_fw_status(struct wl1271 *wl, struct wl_fw_status *status)
 {
 	struct wl12xx_vif *wlvif;
 	struct timespec ts;
 	u32 old_tx_blk_count = wl->tx_blocks_available;
 	int avail, freed_blocks;
 	int i;
-	size_t status_len;
 	int ret;
 	struct wl1271_link *lnk;
 
-	status_len = WLCORE_FW_STATUS_1_LEN(wl->num_rx_desc) +
-		sizeof(*status_2) + wl->fw_status_priv_len;
-
-	ret = wlcore_raw_read_data(wl, REG_RAW_FW_STATUS_ADDR, status_1,
-				   status_len, false);
+	ret = wlcore_raw_read_data(wl, REG_RAW_FW_STATUS_ADDR,
+				   wl->raw_fw_status,
+				   wl->fw_status_len, false);
 	if (ret < 0)
 		return ret;
 
+	wlcore_hw_convert_fw_status(wl, wl->raw_fw_status, wl->fw_status);
+
 	wl1271_debug(DEBUG_IRQ, "intr: 0x%x (fw_rx_counter = %d, "
 		     "drv_rx_counter = %d, tx_results_counter = %d)",
-		     status_1->intr,
-		     status_1->fw_rx_counter,
-		     status_1->drv_rx_counter,
-		     status_1->tx_results_counter);
+		     status->intr,
+		     status->fw_rx_counter,
+		     status->drv_rx_counter,
+		     status->tx_results_counter);
 
 	for (i = 0; i < NUM_TX_QUEUES; i++) {
 		/* prevent wrap-around in freed-packets counter */
 		wl->tx_allocated_pkts[i] -=
-				(status_2->counters.tx_released_pkts[i] -
+				(status->counters.tx_released_pkts[i] -
 				wl->tx_pkts_freed[i]) & 0xff;
 
-		wl->tx_pkts_freed[i] = status_2->counters.tx_released_pkts[i];
+		wl->tx_pkts_freed[i] = status->counters.tx_released_pkts[i];
 	}
 
 
-	for_each_set_bit(i, wl->links_map, WL12XX_MAX_LINKS) {
+	for_each_set_bit(i, wl->links_map, wl->num_links) {
 		u8 diff;
 		lnk = &wl->links[i];
 
 		/* prevent wrap-around in freed-packets counter */
-		diff = (status_2->counters.tx_lnk_free_pkts[i] -
+		diff = (status->counters.tx_lnk_free_pkts[i] -
 		       lnk->prev_freed_pkts) & 0xff;
 
 		if (diff == 0)
 			continue;
 
 		lnk->allocated_pkts -= diff;
-		lnk->prev_freed_pkts = status_2->counters.tx_lnk_free_pkts[i];
+		lnk->prev_freed_pkts = status->counters.tx_lnk_free_pkts[i];
 
 		/* accumulate the prev_freed_pkts counter */
 		lnk->total_freed_pkts += diff;
 	}
 
 	/* prevent wrap-around in total blocks counter */
-	if (likely(wl->tx_blocks_freed <=
-		   le32_to_cpu(status_2->total_released_blks)))
-		freed_blocks = le32_to_cpu(status_2->total_released_blks) -
+	if (likely(wl->tx_blocks_freed <= status->total_released_blks))
+		freed_blocks = status->total_released_blks -
 			       wl->tx_blocks_freed;
 	else
 		freed_blocks = 0x100000000LL - wl->tx_blocks_freed +
-			       le32_to_cpu(status_2->total_released_blks);
+			       status->total_released_blks;
 
-	wl->tx_blocks_freed = le32_to_cpu(status_2->total_released_blks);
+	wl->tx_blocks_freed = status->total_released_blks;
 
 	wl->tx_allocated_blocks -= freed_blocks;
 
@@ -458,7 +454,7 @@ static int wlcore_fw_status(struct wl1271 *wl,
 			cancel_delayed_work(&wl->tx_watchdog_work);
 	}
 
-	avail = le32_to_cpu(status_2->tx_total) - wl->tx_allocated_blocks;
+	avail = status->tx_total - wl->tx_allocated_blocks;
 
 	/*
 	 * The FW might change the total number of TX memblocks before
@@ -477,15 +473,15 @@ static int wlcore_fw_status(struct wl1271 *wl,
 
 	/* for AP update num of allocated TX blocks per link and ps status */
 	wl12xx_for_each_wlvif_ap(wl, wlvif) {
-		wl12xx_irq_update_links_status(wl, wlvif, status_2);
+		wl12xx_irq_update_links_status(wl, wlvif, status);
 	}
 
 	/* update the host-chipset time offset */
 	getnstimeofday(&ts);
 	wl->time_offset = (timespec_to_ns(&ts) >> 10) -
-		(s64)le32_to_cpu(status_2->fw_localtime);
+		(s64)(status->fw_localtime);
 
-	wl->fw_fast_lnk_map = le32_to_cpu(status_2->link_fast_bitmap);
+	wl->fw_fast_lnk_map = status->link_fast_bitmap;
 
 	return 0;
 }
@@ -549,13 +545,13 @@ static int wlcore_irq_locked(struct wl1271 *wl)
 		clear_bit(WL1271_FLAG_IRQ_RUNNING, &wl->flags);
 		smp_mb__after_clear_bit();
 
-		ret = wlcore_fw_status(wl, wl->fw_status_1, wl->fw_status_2);
+		ret = wlcore_fw_status(wl, wl->fw_status);
 		if (ret < 0)
 			goto out;
 
 		wlcore_hw_tx_immediate_compl(wl);
 
-		intr = le32_to_cpu(wl->fw_status_1->intr);
+		intr = wl->fw_status->intr;
 		intr &= WLCORE_ALL_INTR_MASK;
 		if (!intr) {
 			done = true;
@@ -584,7 +580,7 @@ static int wlcore_irq_locked(struct wl1271 *wl)
 		if (likely(intr & WL1271_ACX_INTR_DATA)) {
 			wl1271_debug(DEBUG_IRQ, "WL1271_ACX_INTR_DATA");
 
-			ret = wlcore_rx(wl, wl->fw_status_1);
+			ret = wlcore_rx(wl, wl->fw_status);
 			if (ret < 0)
 				goto out;
 
@@ -786,10 +782,11 @@ out:
 
 void wl12xx_queue_recovery_work(struct wl1271 *wl)
 {
-	WARN_ON(!test_bit(WL1271_FLAG_INTENDED_FW_RECOVERY, &wl->flags));
-
 	/* Avoid a recursive recovery */
 	if (wl->state == WLCORE_STATE_ON) {
+		WARN_ON(!test_bit(WL1271_FLAG_INTENDED_FW_RECOVERY,
+				  &wl->flags));
+
 		wl->state = WLCORE_STATE_RESTARTING;
 		set_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS, &wl->flags);
 		wl1271_ps_elp_wakeup(wl);
@@ -803,7 +800,7 @@ size_t wl12xx_copy_fwlog(struct wl1271 *wl, u8 *memblock, size_t maxlen)
 	size_t len;
 
 	/* Make sure we have enough room */
-	len = min(maxlen, (size_t)(PAGE_SIZE - wl->fwlog_size));
+	len = min_t(size_t, maxlen, PAGE_SIZE - wl->fwlog_size);
 
 	/* Fill the FW log file, consumed by the sysfs fwlog entry */
 	memcpy(wl->fwlog + wl->fwlog_size, memblock, len);
@@ -843,11 +840,11 @@ static void wl12xx_read_fwlog_panic(struct wl1271 *wl)
 		wl12xx_cmd_stop_fwlog(wl);
 
 	/* Read the first memory block address */
-	ret = wlcore_fw_status(wl, wl->fw_status_1, wl->fw_status_2);
+	ret = wlcore_fw_status(wl, wl->fw_status);
 	if (ret < 0)
 		goto out;
 
-	addr = le32_to_cpu(wl->fw_status_2->log_start_addr);
+	addr = wl->fw_status->log_start_addr;
 	if (!addr)
 		goto out;
 
@@ -990,23 +987,23 @@ static int wlcore_fw_wakeup(struct wl1271 *wl)
 
 static int wl1271_setup(struct wl1271 *wl)
 {
-	wl->fw_status_1 = kzalloc(WLCORE_FW_STATUS_1_LEN(wl->num_rx_desc) +
-				  sizeof(*wl->fw_status_2) +
-				  wl->fw_status_priv_len, GFP_KERNEL);
-	if (!wl->fw_status_1)
-		return -ENOMEM;
+	wl->raw_fw_status = kzalloc(wl->fw_status_len, GFP_KERNEL);
+	if (!wl->raw_fw_status)
+		goto err;
 
-	wl->fw_status_2 = (struct wl_fw_status_2 *)
-				(((u8 *) wl->fw_status_1) +
-				WLCORE_FW_STATUS_1_LEN(wl->num_rx_desc));
+	wl->fw_status = kzalloc(sizeof(*wl->fw_status), GFP_KERNEL);
+	if (!wl->fw_status)
+		goto err;
 
 	wl->tx_res_if = kzalloc(sizeof(*wl->tx_res_if), GFP_KERNEL);
-	if (!wl->tx_res_if) {
-		kfree(wl->fw_status_1);
-		return -ENOMEM;
-	}
+	if (!wl->tx_res_if)
+		goto err;
 
 	return 0;
+err:
+	kfree(wl->fw_status);
+	kfree(wl->raw_fw_status);
+	return -ENOMEM;
 }
 
 static int wl12xx_set_power_on(struct wl1271 *wl)
@@ -1767,6 +1764,12 @@ static int wl1271_op_suspend(struct ieee80211_hw *hw,
 	flush_work(&wl->tx_work);
 	flush_delayed_work(&wl->elp_work);
 
+	/*
+	 * Cancel the watchdog even if above tx_flush failed. We will detect
+	 * it on resume anyway.
+	 */
+	cancel_delayed_work(&wl->tx_watchdog_work);
+
 	return 0;
 }
 
@@ -1824,6 +1827,13 @@ static int wl1271_op_resume(struct ieee80211_hw *hw)
 
 out:
 	wl->wow_enabled = false;
+
+	/*
+	 * Set a flag to re-init the watchdog on the first Tx after resume.
+	 * That way we avoid possible conditions where Tx-complete interrupts
+	 * fail to arrive and we perform a spurious recovery.
+	 */
+	set_bit(WL1271_FLAG_REINIT_TX_WDOG, &wl->flags);
 	mutex_unlock(&wl->mutex);
 
 	return 0;
@@ -1914,6 +1924,7 @@ static void wlcore_op_stop_locked(struct wl1271 *wl)
 	memset(wl->links_map, 0, sizeof(wl->links_map));
 	memset(wl->roc_map, 0, sizeof(wl->roc_map));
 	memset(wl->session_ids, 0, sizeof(wl->session_ids));
+	memset(wl->rx_filter_enabled, 0, sizeof(wl->rx_filter_enabled));
 	wl->active_sta_count = 0;
 	wl->active_link_count = 0;
 
@@ -1938,9 +1949,10 @@ static void wlcore_op_stop_locked(struct wl1271 *wl)
 
 	wl1271_debugfs_reset(wl);
 
-	kfree(wl->fw_status_1);
-	wl->fw_status_1 = NULL;
-	wl->fw_status_2 = NULL;
+	kfree(wl->raw_fw_status);
+	wl->raw_fw_status = NULL;
+	kfree(wl->fw_status);
+	wl->fw_status = NULL;
 	kfree(wl->tx_res_if);
 	wl->tx_res_if = NULL;
 	kfree(wl->target_mem_map);
@@ -2571,10 +2583,8 @@ static void __wl1271_op_remove_interface(struct wl1271 *wl,
 		ieee80211_scan_completed(wl->hw, true);
 	}
 
-	if (wl->sched_vif == wlvif) {
-		ieee80211_sched_scan_stopped(wl->hw);
+	if (wl->sched_vif == wlvif)
 		wl->sched_vif = NULL;
-	}
 
 	if (wl->roc_vif == vif) {
 		wl->roc_vif = NULL;
@@ -2931,6 +2941,11 @@ static int wlcore_unset_assoc(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 		ret = wl1271_acx_keep_alive_mode(wl, wlvif, false);
 		if (ret < 0)
 			return ret;
+
+		/* disable beacon filtering */
+		ret = wl1271_acx_beacon_filter_opt(wl, wlvif, false);
+		if (ret < 0)
+			return ret;
 	}
 
 	if (test_and_clear_bit(WLVIF_FLAG_CS_PROGRESS, &wlvif->flags)) {
@@ -3463,6 +3478,10 @@ static void wl1271_op_set_default_key_idx(struct ieee80211_hw *hw,
 	wl1271_debug(DEBUG_MAC80211, "mac80211 set default key idx %d",
 		     key_idx);
 
+	/* we don't handle unsetting of default key */
+	if (key_idx == -1)
+		return;
+
 	mutex_lock(&wl->mutex);
 
 	if (unlikely(wl->state != WLCORE_STATE_ON)) {
@@ -3649,8 +3668,8 @@ out:
 	return ret;
 }
 
-static void wl1271_op_sched_scan_stop(struct ieee80211_hw *hw,
-				      struct ieee80211_vif *vif)
+static int wl1271_op_sched_scan_stop(struct ieee80211_hw *hw,
+				     struct ieee80211_vif *vif)
 {
 	struct wl1271 *wl = hw->priv;
 	struct wl12xx_vif *wlvif = wl12xx_vif_to_data(vif);
@@ -3672,6 +3691,8 @@ static void wl1271_op_sched_scan_stop(struct ieee80211_hw *hw,
 	wl1271_ps_elp_sleep(wl);
 out:
 	mutex_unlock(&wl->mutex);
+
+	return 0;
 }
 
 static int wl1271_op_set_frag_threshold(struct ieee80211_hw *hw, u32 value)
@@ -4298,6 +4319,13 @@ static void wl1271_bss_info_changed_sta(struct wl1271 *wl,
 		}
 	}
 
+	if ((changed & BSS_CHANGED_BEACON_INFO) && bss_conf->dtim_period) {
+		/* enable beacon filtering */
+		ret = wl1271_acx_beacon_filter_opt(wl, wlvif, true);
+		if (ret < 0)
+			goto out;
+	}
+
 	ret = wl1271_bss_erp_info_changed(wl, vif, bss_conf, changed);
 	if (ret < 0)
 		goto out;
@@ -4651,7 +4679,7 @@ static int wl1271_allocate_sta(struct wl1271 *wl,
 	int ret;
 
 
-	if (wl->active_sta_count >= AP_MAX_STATIONS) {
+	if (wl->active_sta_count >= wl->max_ap_stations) {
 		wl1271_warning("could not allocate HLID - too much stations");
 		return -EBUSY;
 	}
@@ -4754,7 +4782,7 @@ static int wl12xx_sta_remove(struct wl1271 *wl,
 	if (WARN_ON(!test_bit(id, wlvif->ap.sta_hlid_map)))
 		return -EINVAL;
 
-	ret = wl12xx_cmd_remove_peer(wl, wl_sta->hlid);
+	ret = wl12xx_cmd_remove_peer(wl, wlvif, wl_sta->hlid);
 	if (ret < 0)
 		return ret;
 
@@ -5679,28 +5707,6 @@ static void wl1271_unregister_hw(struct wl1271 *wl)
 
 }
 
-static const struct ieee80211_iface_limit wlcore_iface_limits[] = {
-	{
-		.max = 3,
-		.types = BIT(NL80211_IFTYPE_STATION),
-	},
-	{
-		.max = 1,
-		.types = BIT(NL80211_IFTYPE_AP) |
-			 BIT(NL80211_IFTYPE_P2P_GO) |
-			 BIT(NL80211_IFTYPE_P2P_CLIENT),
-	},
-};
-
-static struct ieee80211_iface_combination
-wlcore_iface_combinations[] = {
-	{
-	  .max_interfaces = 3,
-	  .limits = wlcore_iface_limits,
-	  .n_limits = ARRAY_SIZE(wlcore_iface_limits),
-	},
-};
-
 static int wl1271_init_ieee80211(struct wl1271 *wl)
 {
 	int i;
@@ -5733,7 +5739,8 @@ static int wl1271_init_ieee80211(struct wl1271 *wl)
 		IEEE80211_HW_AP_LINK_PS |
 		IEEE80211_HW_AMPDU_AGGREGATION |
 		IEEE80211_HW_TX_AMPDU_SETUP_IN_HW |
-		IEEE80211_HW_QUEUE_CONTROL;
+		IEEE80211_HW_QUEUE_CONTROL |
+		IEEE80211_HW_CHANCTX_STA_CSA;
 
 	wl->hw->wiphy->cipher_suites = cipher_suites;
 	wl->hw->wiphy->n_cipher_suites = ARRAY_SIZE(cipher_suites);
@@ -5821,10 +5828,8 @@ static int wl1271_init_ieee80211(struct wl1271 *wl)
 		NL80211_PROBE_RESP_OFFLOAD_SUPPORT_P2P;
 
 	/* allowed interface combinations */
-	wlcore_iface_combinations[0].num_different_channels = wl->num_channels;
-	wl->hw->wiphy->iface_combinations = wlcore_iface_combinations;
-	wl->hw->wiphy->n_iface_combinations =
-		ARRAY_SIZE(wlcore_iface_combinations);
+	wl->hw->wiphy->iface_combinations = wl->iface_combinations;
+	wl->hw->wiphy->n_iface_combinations = wl->n_iface_combinations;
 
 	SET_IEEE80211_DEV(wl->hw, wl->dev);
 
@@ -5844,8 +5849,6 @@ struct ieee80211_hw *wlcore_alloc_hw(size_t priv_size, u32 aggr_buf_size,
 	int i, j, ret;
 	unsigned int order;
 
-	BUILD_BUG_ON(AP_MAX_STATIONS > WL12XX_MAX_LINKS);
-
 	hw = ieee80211_alloc_hw(sizeof(*wl), &wl1271_ops);
 	if (!hw) {
 		wl1271_error("could not alloc ieee80211_hw");
@@ -5867,8 +5870,12 @@ struct ieee80211_hw *wlcore_alloc_hw(size_t priv_size, u32 aggr_buf_size,
 
 	wl->hw = hw;
 
+	/*
+	 * wl->num_links is not configured yet, so just use WLCORE_MAX_LINKS.
+	 * we don't allocate any additional resource here, so that's fine.
+	 */
 	for (i = 0; i < NUM_TX_QUEUES; i++)
-		for (j = 0; j < WL12XX_MAX_LINKS; j++)
+		for (j = 0; j < WLCORE_MAX_LINKS; j++)
 			skb_queue_head_init(&wl->links[j].tx_queue[i]);
 
 	skb_queue_head_init(&wl->deferred_rx_queue);
@@ -6011,7 +6018,8 @@ int wlcore_free_hw(struct wl1271 *wl)
 	kfree(wl->nvs);
 	wl->nvs = NULL;
 
-	kfree(wl->fw_status_1);
+	kfree(wl->raw_fw_status);
+	kfree(wl->fw_status);
 	kfree(wl->tx_res_if);
 	destroy_workqueue(wl->freezable_wq);
 
diff --git a/drivers/net/wireless/ti/wlcore/ps.c b/drivers/net/wireless/ti/wlcore/ps.c
index 26bfc365ba70..b52516eed7b2 100644
--- a/drivers/net/wireless/ti/wlcore/ps.c
+++ b/drivers/net/wireless/ti/wlcore/ps.c
@@ -280,7 +280,11 @@ void wl12xx_ps_link_start(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 	struct ieee80211_sta *sta;
 	struct ieee80211_vif *vif = wl12xx_wlvif_to_vif(wlvif);
 
-	if (test_bit(hlid, &wl->ap_ps_map))
+	if (WARN_ON_ONCE(wlvif->bss_type != BSS_TYPE_AP_BSS))
+		return;
+
+	if (!test_bit(hlid, wlvif->ap.sta_hlid_map) ||
+	    test_bit(hlid, &wl->ap_ps_map))
 		return;
 
 	wl1271_debug(DEBUG_PSM, "start mac80211 PSM on hlid %d pkts %d "
diff --git a/drivers/net/wireless/ti/wlcore/rx.c b/drivers/net/wireless/ti/wlcore/rx.c
index 6791a1a6afba..e125974285cc 100644
--- a/drivers/net/wireless/ti/wlcore/rx.c
+++ b/drivers/net/wireless/ti/wlcore/rx.c
@@ -203,9 +203,9 @@ static int wl1271_rx_handle_data(struct wl1271 *wl, u8 *data, u32 length,
 	return is_data;
 }
 
-int wlcore_rx(struct wl1271 *wl, struct wl_fw_status_1 *status)
+int wlcore_rx(struct wl1271 *wl, struct wl_fw_status *status)
 {
-	unsigned long active_hlids[BITS_TO_LONGS(WL12XX_MAX_LINKS)] = {0};
+	unsigned long active_hlids[BITS_TO_LONGS(WLCORE_MAX_LINKS)] = {0};
 	u32 buf_size;
 	u32 fw_rx_counter = status->fw_rx_counter % wl->num_rx_desc;
 	u32 drv_rx_counter = wl->rx_counter % wl->num_rx_desc;
@@ -263,12 +263,12 @@ int wlcore_rx(struct wl1271 *wl, struct wl_fw_status_1 *status)
 						  wl->aggr_buf + pkt_offset,
 						  pkt_len, rx_align,
 						  &hlid) == 1) {
-				if (hlid < WL12XX_MAX_LINKS)
+				if (hlid < wl->num_links)
 					__set_bit(hlid, active_hlids);
 				else
 					WARN(1,
-					     "hlid exceeded WL12XX_MAX_LINKS "
-					     "(%d)\n", hlid);
+					     "hlid (%d) exceeded MAX_LINKS\n",
+					     hlid);
 			}
 
 			wl->rx_counter++;
@@ -302,7 +302,7 @@ int wl1271_rx_filter_enable(struct wl1271 *wl,
 {
 	int ret;
 
-	if (wl->rx_filter_enabled[index] == enable) {
+	if (!!test_bit(index, wl->rx_filter_enabled) == enable) {
 		wl1271_warning("Request to enable an already "
 			     "enabled rx filter %d", index);
 		return 0;
@@ -316,7 +316,10 @@ int wl1271_rx_filter_enable(struct wl1271 *wl,
 		return ret;
 	}
 
-	wl->rx_filter_enabled[index] = enable;
+	if (enable)
+		__set_bit(index, wl->rx_filter_enabled);
+	else
+		__clear_bit(index, wl->rx_filter_enabled);
 
 	return 0;
 }
@@ -326,7 +329,7 @@ int wl1271_rx_filter_clear_all(struct wl1271 *wl)
 	int i, ret = 0;
 
 	for (i = 0; i < WL1271_MAX_RX_FILTERS; i++) {
-		if (!wl->rx_filter_enabled[i])
+		if (!test_bit(i, wl->rx_filter_enabled))
 			continue;
 		ret = wl1271_rx_filter_enable(wl, i, 0, NULL);
 		if (ret)
diff --git a/drivers/net/wireless/ti/wlcore/rx.h b/drivers/net/wireless/ti/wlcore/rx.h
index 3363f60fb7da..a3b1618db27c 100644
--- a/drivers/net/wireless/ti/wlcore/rx.h
+++ b/drivers/net/wireless/ti/wlcore/rx.h
@@ -142,7 +142,7 @@ struct wl1271_rx_descriptor {
 	u8  reserved;
 } __packed;
 
-int wlcore_rx(struct wl1271 *wl, struct wl_fw_status_1 *status);
+int wlcore_rx(struct wl1271 *wl, struct wl_fw_status *status);
 u8 wl1271_rate_to_idx(int rate, enum ieee80211_band band);
 int wl1271_rx_filter_enable(struct wl1271 *wl,
 			    int index, bool enable,
diff --git a/drivers/net/wireless/ti/wlcore/spi.c b/drivers/net/wireless/ti/wlcore/spi.c
index b2c018dccf18..dbe826dd7c23 100644
--- a/drivers/net/wireless/ti/wlcore/spi.c
+++ b/drivers/net/wireless/ti/wlcore/spi.c
@@ -211,7 +211,7 @@ static int __must_check wl12xx_spi_raw_read(struct device *child, int addr,
 	u32 chunk_len;
 
 	while (len > 0) {
-		chunk_len = min((size_t)WSPI_MAX_CHUNK_SIZE, len);
+		chunk_len = min_t(size_t, WSPI_MAX_CHUNK_SIZE, len);
 
 		cmd = &wl->buffer_cmd;
 		busy_buf = wl->buffer_busyword;
@@ -285,7 +285,7 @@ static int __must_check wl12xx_spi_raw_write(struct device *child, int addr,
 	cmd = &commands[0];
 	i = 0;
 	while (len > 0) {
-		chunk_len = min((size_t)WSPI_MAX_CHUNK_SIZE, len);
+		chunk_len = min_t(size_t, WSPI_MAX_CHUNK_SIZE, len);
 
 		*cmd = 0;
 		*cmd |= WSPI_CMD_WRITE;
diff --git a/drivers/net/wireless/ti/wlcore/sysfs.c b/drivers/net/wireless/ti/wlcore/sysfs.c
index 8e583497940d..24dd288d6809 100644
--- a/drivers/net/wireless/ti/wlcore/sysfs.c
+++ b/drivers/net/wireless/ti/wlcore/sysfs.c
@@ -152,7 +152,7 @@ static ssize_t wl1271_sysfs_read_fwlog(struct file *filp, struct kobject *kobj,
 	}
 
 	/* Seeking is not supported - old logs are not kept. Disregard pos. */
-	len = min(count, (size_t)wl->fwlog_size);
+	len = min_t(size_t, count, wl->fwlog_size);
 	wl->fwlog_size -= len;
 	memcpy(buffer, wl->fwlog, len);
 
diff --git a/drivers/net/wireless/ti/wlcore/tx.c b/drivers/net/wireless/ti/wlcore/tx.c
index 87cd707affa2..40b43115f835 100644
--- a/drivers/net/wireless/ti/wlcore/tx.c
+++ b/drivers/net/wireless/ti/wlcore/tx.c
@@ -101,7 +101,7 @@ static void wl1271_tx_ap_update_inconnection_sta(struct wl1271 *wl,
 	 * authentication response. this way it won't get de-authed by FW
 	 * when transmitting too soon.
 	 */
-	wl1271_acx_set_inconnection_sta(wl, hdr->addr1);
+	wl1271_acx_set_inconnection_sta(wl, wlvif, hdr->addr1);
 
 	/*
 	 * ROC for 1 second on the AP channel for completing the connection.
@@ -134,12 +134,12 @@ static void wl1271_tx_regulate_link(struct wl1271 *wl,
 	 * into high-level PS and clean out its TX queues.
 	 * Make an exception if this is the only connected link. In this
 	 * case FW-memory congestion is less of a problem.
-	 * Note that a single connected STA means 3 active links, since we must
-	 * account for the global and broadcast AP links. The "fw_ps" check
-	 * assures us the third link is a STA connected to the AP. Otherwise
-	 * the FW would not set the PSM bit.
+	 * Note that a single connected STA means 2*ap_count + 1 active links,
+	 * since we must account for the global and broadcast AP links
+	 * for each AP. The "fw_ps" check assures us the other link is a STA
+	 * connected to the AP. Otherwise the FW would not set the PSM bit.
 	 */
-	if (wl->active_link_count > 3 && fw_ps &&
+	if (wl->active_link_count > (wl->ap_count*2 + 1) && fw_ps &&
 	    tx_pkts >= WL1271_PS_STA_MAX_PACKETS)
 		wl12xx_ps_link_start(wl, wlvif, hlid, true);
 }
@@ -234,8 +234,13 @@ static int wl1271_tx_allocate(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 		wl->tx_blocks_available -= total_blocks;
 		wl->tx_allocated_blocks += total_blocks;
 
-		/* If the FW was empty before, arm the Tx watchdog */
-		if (wl->tx_allocated_blocks == total_blocks)
+		/*
+		 * If the FW was empty before, arm the Tx watchdog. Also do
+		 * this on the first Tx after resume, as we always cancel the
+		 * watchdog on suspend.
+		 */
+		if (wl->tx_allocated_blocks == total_blocks ||
+		    test_and_clear_bit(WL1271_FLAG_REINIT_TX_WDOG, &wl->flags))
 			wl12xx_rearm_tx_watchdog_locked(wl);
 
 		ac = wl1271_tx_get_queue(skb_get_queue_mapping(skb));
@@ -357,6 +362,10 @@ static void wl1271_tx_fill_hdr(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 	    ieee80211_has_protected(frame_control))
 		tx_attr |= TX_HW_ATTR_HOST_ENCRYPT;
 
+	/* send EAPOL frames as voice */
+	if (control->control.flags & IEEE80211_TX_CTRL_PORT_CTRL_PROTO)
+		tx_attr |= TX_HW_ATTR_EAPOL_FRAME;
+
 	desc->tx_attr = cpu_to_le16(tx_attr);
 
 	wlcore_hw_set_tx_desc_csum(wl, desc, skb);
@@ -560,11 +569,11 @@ static struct sk_buff *wlcore_vif_dequeue_high_prio(struct wl1271 *wl,
 	int i, h, start_hlid;
 
 	/* start from the link after the last one */
-	start_hlid = (wlvif->last_tx_hlid + 1) % WL12XX_MAX_LINKS;
+	start_hlid = (wlvif->last_tx_hlid + 1) % wl->num_links;
 
 	/* dequeue according to AC, round robin on each link */
-	for (i = 0; i < WL12XX_MAX_LINKS; i++) {
-		h = (start_hlid + i) % WL12XX_MAX_LINKS;
+	for (i = 0; i < wl->num_links; i++) {
+		h = (start_hlid + i) % wl->num_links;
 
 		/* only consider connected stations */
 		if (!test_bit(h, wlvif->links_map))
@@ -688,8 +697,8 @@ static void wl1271_skb_queue_head(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 		skb_queue_head(&wl->links[hlid].tx_queue[q], skb);
 
 		/* make sure we dequeue the same packet next time */
-		wlvif->last_tx_hlid = (hlid + WL12XX_MAX_LINKS - 1) %
-				      WL12XX_MAX_LINKS;
+		wlvif->last_tx_hlid = (hlid + wl->num_links - 1) %
+				      wl->num_links;
 	}
 
 	spin_lock_irqsave(&wl->wl_lock, flags);
@@ -722,7 +731,7 @@ void wl12xx_rearm_rx_streaming(struct wl1271 *wl, unsigned long *active_hlids)
 	timeout = wl->conf.rx_streaming.duration;
 	wl12xx_for_each_wlvif_sta(wl, wlvif) {
 		bool found = false;
-		for_each_set_bit(hlid, active_hlids, WL12XX_MAX_LINKS) {
+		for_each_set_bit(hlid, active_hlids, wl->num_links) {
 			if (test_bit(hlid, wlvif->links_map)) {
 				found  = true;
 				break;
@@ -759,7 +768,7 @@ int wlcore_tx_work_locked(struct wl1271 *wl)
 	struct wl1271_tx_hw_descr *desc;
 	u32 buf_offset = 0, last_len = 0;
 	bool sent_packets = false;
-	unsigned long active_hlids[BITS_TO_LONGS(WL12XX_MAX_LINKS)] = {0};
+	unsigned long active_hlids[BITS_TO_LONGS(WLCORE_MAX_LINKS)] = {0};
 	int ret = 0;
 	int bus_ret = 0;
 	u8 hlid;
@@ -1061,7 +1070,7 @@ void wl12xx_tx_reset_wlvif(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 	int i;
 
 	/* TX failure */
-	for_each_set_bit(i, wlvif->links_map, WL12XX_MAX_LINKS) {
+	for_each_set_bit(i, wlvif->links_map, wl->num_links) {
 		if (wlvif->bss_type == BSS_TYPE_AP_BSS &&
 		    i != wlvif->ap.bcast_hlid && i != wlvif->ap.global_hlid) {
 			/* this calls wl12xx_free_link */
@@ -1085,7 +1094,7 @@ void wl12xx_tx_reset(struct wl1271 *wl)
 
 	/* only reset the queues if something bad happened */
 	if (wl1271_tx_total_queue_count(wl) != 0) {
-		for (i = 0; i < WL12XX_MAX_LINKS; i++)
+		for (i = 0; i < wl->num_links; i++)
 			wl1271_tx_reset_link_queues(wl, i);
 
 		for (i = 0; i < NUM_TX_QUEUES; i++)
@@ -1178,7 +1187,7 @@ void wl1271_tx_flush(struct wl1271 *wl)
 		       WL1271_TX_FLUSH_TIMEOUT / 1000);
 
 	/* forcibly flush all Tx buffers on our queues */
-	for (i = 0; i < WL12XX_MAX_LINKS; i++)
+	for (i = 0; i < wl->num_links; i++)
 		wl1271_tx_reset_link_queues(wl, i);
 
 out_wake:
diff --git a/drivers/net/wireless/ti/wlcore/tx.h b/drivers/net/wireless/ti/wlcore/tx.h
index 35489c300da1..79cb3ff8b71f 100644
--- a/drivers/net/wireless/ti/wlcore/tx.h
+++ b/drivers/net/wireless/ti/wlcore/tx.h
@@ -37,6 +37,7 @@
 #define TX_HW_ATTR_TX_CMPLT_REQ          BIT(12)
 #define TX_HW_ATTR_TX_DUMMY_REQ          BIT(13)
 #define TX_HW_ATTR_HOST_ENCRYPT          BIT(14)
+#define TX_HW_ATTR_EAPOL_FRAME           BIT(15)
 
 #define TX_HW_ATTR_OFST_SAVE_RETRIES     0
 #define TX_HW_ATTR_OFST_HEADER_PAD       1
diff --git a/drivers/net/wireless/ti/wlcore/wlcore.h b/drivers/net/wireless/ti/wlcore/wlcore.h
index 06efc12a39e5..95a54504f0cc 100644
--- a/drivers/net/wireless/ti/wlcore/wlcore.h
+++ b/drivers/net/wireless/ti/wlcore/wlcore.h
@@ -73,6 +73,8 @@ struct wlcore_ops {
 	void (*tx_immediate_compl)(struct wl1271 *wl);
 	int (*hw_init)(struct wl1271 *wl);
 	int (*init_vif)(struct wl1271 *wl, struct wl12xx_vif *wlvif);
+	void (*convert_fw_status)(struct wl1271 *wl, void *raw_fw_status,
+				  struct wl_fw_status *fw_status);
 	u32 (*sta_get_ap_rate_mask)(struct wl1271 *wl,
 				    struct wl12xx_vif *wlvif);
 	int (*get_pg_ver)(struct wl1271 *wl, s8 *ver);
@@ -220,7 +222,7 @@ struct wl1271 {
 	int channel;
 	u8 system_hlid;
 
-	unsigned long links_map[BITS_TO_LONGS(WL12XX_MAX_LINKS)];
+	unsigned long links_map[BITS_TO_LONGS(WLCORE_MAX_LINKS)];
 	unsigned long roles_map[BITS_TO_LONGS(WL12XX_MAX_ROLES)];
 	unsigned long roc_map[BITS_TO_LONGS(WL12XX_MAX_ROLES)];
 	unsigned long rate_policies_map[
@@ -228,7 +230,7 @@ struct wl1271 {
 	unsigned long klv_templates_map[
 			BITS_TO_LONGS(WLCORE_MAX_KLV_TEMPLATES)];
 
-	u8 session_ids[WL12XX_MAX_LINKS];
+	u8 session_ids[WLCORE_MAX_LINKS];
 
 	struct list_head wlvif_list;
 
@@ -346,8 +348,8 @@ struct wl1271 {
 	u32 buffer_cmd;
 	u32 buffer_busyword[WL1271_BUSY_WORD_CNT];
 
-	struct wl_fw_status_1 *fw_status_1;
-	struct wl_fw_status_2 *fw_status_2;
+	void *raw_fw_status;
+	struct wl_fw_status *fw_status;
 	struct wl1271_tx_hw_res_if *tx_res_if;
 
 	/* Current chipset configuration */
@@ -376,7 +378,7 @@ struct wl1271 {
 	 * AP-mode - links indexed by HLID. The global and broadcast links
 	 * are always active.
 	 */
-	struct wl1271_link links[WL12XX_MAX_LINKS];
+	struct wl1271_link links[WLCORE_MAX_LINKS];
 
 	/* number of currently active links */
 	int active_link_count;
@@ -405,6 +407,9 @@ struct wl1271 {
 	/* AP-mode - number of currently connected stations */
 	int active_sta_count;
 
+	/* Flag determining whether AP should broadcast OFDM-only rates */
+	bool ofdm_only_ap;
+
 	/* last wlvif we transmitted from */
 	struct wl12xx_vif *last_wlvif;
 
@@ -434,6 +439,10 @@ struct wl1271 {
 	u32 num_tx_desc;
 	/* number of RX descriptors the HW supports. */
 	u32 num_rx_desc;
+	/* number of links the HW supports */
+	u8 num_links;
+	/* max stations a single AP can support */
+	u8 max_ap_stations;
 
 	/* translate HW Tx rates to standard rate-indices */
 	const u8 **band_rate_to_idx;
@@ -448,10 +457,11 @@ struct wl1271 {
 	struct ieee80211_sta_ht_cap ht_cap[WLCORE_NUM_BANDS];
 
 	/* size of the private FW status data */
+	size_t fw_status_len;
 	size_t fw_status_priv_len;
 
 	/* RX Data filter rule state - enabled/disabled */
-	bool rx_filter_enabled[WL1271_MAX_RX_FILTERS];
+	unsigned long rx_filter_enabled[BITS_TO_LONGS(WL1271_MAX_RX_FILTERS)];
 
 	/* size of the private static data */
 	size_t static_data_priv_len;
@@ -476,8 +486,9 @@ struct wl1271 {
 
 	struct completion nvs_loading_complete;
 
-	/* number of concurrent channels the HW supports */
-	u32 num_channels;
+	/* interface combinations supported by the hw */
+	const struct ieee80211_iface_combination *iface_combinations;
+	u8 n_iface_combinations;
 };
 
 int wlcore_probe(struct wl1271 *wl, struct platform_device *pdev);
diff --git a/drivers/net/wireless/ti/wlcore/wlcore_i.h b/drivers/net/wireless/ti/wlcore/wlcore_i.h
index ce7261ce8b59..756e890bc5ee 100644
--- a/drivers/net/wireless/ti/wlcore/wlcore_i.h
+++ b/drivers/net/wireless/ti/wlcore/wlcore_i.h
@@ -58,10 +58,15 @@
 #define WL1271_DEFAULT_DTIM_PERIOD 1
 
 #define WL12XX_MAX_ROLES           4
-#define WL12XX_MAX_LINKS           12
 #define WL12XX_INVALID_ROLE_ID     0xff
 #define WL12XX_INVALID_LINK_ID     0xff
 
+/*
+ * max number of links allowed by all HWs.
+ * this is NOT the actual max links supported by the current hw.
+ */
+#define WLCORE_MAX_LINKS 16
+
 /* the driver supports the 2.4Ghz and 5Ghz bands */
 #define WLCORE_NUM_BANDS           2
 
@@ -118,72 +123,58 @@ struct wl1271_chip {
 
 #define NUM_TX_QUEUES              4
 
-#define AP_MAX_STATIONS            8
-
-struct wl_fw_packet_counters {
-	/* Cumulative counter of released packets per AC */
-	u8 tx_released_pkts[NUM_TX_QUEUES];
-
-	/* Cumulative counter of freed packets per HLID */
-	u8 tx_lnk_free_pkts[WL12XX_MAX_LINKS];
-
-	/* Cumulative counter of released Voice memory blocks */
-	u8 tx_voice_released_blks;
-
-	/* Tx rate of the last transmitted packet */
-	u8 tx_last_rate;
-
-	u8 padding[2];
-} __packed;
-
-/* FW status registers */
-struct wl_fw_status_1 {
-	__le32 intr;
+struct wl_fw_status {
+	u32 intr;
 	u8  fw_rx_counter;
 	u8  drv_rx_counter;
-	u8  reserved;
 	u8  tx_results_counter;
-	__le32 rx_pkt_descs[0];
-} __packed;
-
-/*
- * Each HW arch has a different number of Rx descriptors.
- * The length of the status depends on it, since it holds an array
- * of descriptors.
- */
-#define WLCORE_FW_STATUS_1_LEN(num_rx_desc) \
-		(sizeof(struct wl_fw_status_1) + \
-		(sizeof(((struct wl_fw_status_1 *)0)->rx_pkt_descs[0])) * \
-		num_rx_desc)
+	__le32 *rx_pkt_descs;
 
-struct wl_fw_status_2 {
-	__le32 fw_localtime;
+	u32 fw_localtime;
 
 	/*
 	 * A bitmap (where each bit represents a single HLID)
 	 * to indicate if the station is in PS mode.
 	 */
-	__le32 link_ps_bitmap;
+	u32 link_ps_bitmap;
 
 	/*
 	 * A bitmap (where each bit represents a single HLID) to indicate
 	 * if the station is in Fast mode
 	 */
-	__le32 link_fast_bitmap;
+	u32 link_fast_bitmap;
 
 	/* Cumulative counter of total released mem blocks since FW-reset */
-	__le32 total_released_blks;
+	u32 total_released_blks;
 
 	/* Size (in Memory Blocks) of TX pool */
-	__le32 tx_total;
+	u32 tx_total;
+
+	struct {
+		/*
+		 * Cumulative counter of released packets per AC
+		 * (length of the array is NUM_TX_QUEUES)
+		 */
+		u8 *tx_released_pkts;
 
-	struct wl_fw_packet_counters counters;
+		/*
+		 * Cumulative counter of freed packets per HLID
+		 * (length of the array is wl->num_links)
+		 */
+		u8 *tx_lnk_free_pkts;
+
+		/* Cumulative counter of released Voice memory blocks */
+		u8 tx_voice_released_blks;
 
-	__le32 log_start_addr;
+		/* Tx rate of the last transmitted packet */
+		u8 tx_last_rate;
+	} counters;
+
+	u32 log_start_addr;
 
 	/* Private status to be used by the lower drivers */
-	u8 priv[0];
-} __packed;
+	void *priv;
+};
 
 #define WL1271_MAX_CHANNELS 64
 struct wl1271_scan {
@@ -240,6 +231,7 @@ enum wl12xx_flags {
 	WL1271_FLAG_VIF_CHANGE_IN_PROGRESS,
 	WL1271_FLAG_INTENDED_FW_RECOVERY,
 	WL1271_FLAG_IO_FAILED,
+	WL1271_FLAG_REINIT_TX_WDOG,
 };
 
 enum wl12xx_vif_flags {
@@ -368,7 +360,7 @@ struct wl12xx_vif {
 
 			/* HLIDs bitmap of associated stations */
 			unsigned long sta_hlid_map[BITS_TO_LONGS(
-							WL12XX_MAX_LINKS)];
+							WLCORE_MAX_LINKS)];
 
 			/* recoreded keys - set here before AP startup */
 			struct wl1271_ap_key *recorded_keys[MAX_NUM_KEYS];
@@ -385,7 +377,7 @@ struct wl12xx_vif {
 	/* counters of packets per AC, across all links in the vif */
 	int tx_queue_count[NUM_TX_QUEUES];
 
-	unsigned long links_map[BITS_TO_LONGS(WL12XX_MAX_LINKS)];
+	unsigned long links_map[BITS_TO_LONGS(WLCORE_MAX_LINKS)];
 
 	u8 ssid[IEEE80211_MAX_SSID_LEN + 1];
 	u8 ssid_len;
diff --git a/drivers/net/wireless/wl3501_cs.c b/drivers/net/wireless/wl3501_cs.c
index d24d4a958c67..d5c371d77ddf 100644
--- a/drivers/net/wireless/wl3501_cs.c
+++ b/drivers/net/wireless/wl3501_cs.c
@@ -42,8 +42,7 @@
 #include <linux/slab.h>
 #include <linux/string.h>
 #include <linux/wireless.h>
-#include <linux/ieee80211.h>
-#include <linux/etherdevice.h>
+#include <net/cfg80211.h>
 
 #include <net/iw_handler.h>
 
@@ -1454,7 +1453,8 @@ static int wl3501_get_freq(struct net_device *dev, struct iw_request_info *info,
 {
 	struct wl3501_card *this = netdev_priv(dev);
 
-	wrqu->freq.m = ieee80211_dsss_chan_to_freq(this->chan) * 100000;
+	wrqu->freq.m = 100000 *
+		ieee80211_channel_to_frequency(this->chan, IEEE80211_BAND_2GHZ);
 	wrqu->freq.e = 1;
 	return 0;
 }
diff --git a/drivers/net/wireless/zd1201.c b/drivers/net/wireless/zd1201.c
index d39c4178c33a..6f5c793a7855 100644
--- a/drivers/net/wireless/zd1201.c
+++ b/drivers/net/wireless/zd1201.c
@@ -18,7 +18,7 @@
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
 #include <linux/wireless.h>
-#include <linux/ieee80211.h>
+#include <net/cfg80211.h>
 #include <net/iw_handler.h>
 #include <linux/string.h>
 #include <linux/if_arp.h>
@@ -914,11 +914,8 @@ static int zd1201_set_freq(struct net_device *dev,
 
 	if (freq->e == 0)
 		channel = freq->m;
-	else {
-		channel = ieee80211_freq_to_dsss_chan(freq->m);
-		if (channel < 0)
-			channel = 0;
-	}
+	else
+		channel = ieee80211_frequency_to_channel(freq->m);
 
 	err = zd1201_setconfig16(zd, ZD1201_RID_CNFOWNCHANNEL, channel);
 	if (err)
diff --git a/drivers/pci/hotplug/acpiphp_glue.c b/drivers/pci/hotplug/acpiphp_glue.c
index cd929aed3613..e2a783fdb98f 100644
--- a/drivers/pci/hotplug/acpiphp_glue.c
+++ b/drivers/pci/hotplug/acpiphp_glue.c
@@ -210,10 +210,29 @@ static void post_dock_fixups(acpi_handle not_used, u32 event, void *data)
 	}
 }
 
+static void dock_event(acpi_handle handle, u32 type, void *data)
+{
+	struct acpiphp_context *context;
+
+	mutex_lock(&acpiphp_context_lock);
+	context = acpiphp_get_context(handle);
+	if (!context || WARN_ON(context->handle != handle)
+	    || context->func.parent->is_going_away) {
+		mutex_unlock(&acpiphp_context_lock);
+		return;
+	}
+	get_bridge(context->func.parent);
+	acpiphp_put_context(context);
+	mutex_unlock(&acpiphp_context_lock);
+
+	hotplug_event(handle, type, data);
+
+	put_bridge(context->func.parent);
+}
 
 static const struct acpi_dock_ops acpiphp_dock_ops = {
 	.fixup = post_dock_fixups,
-	.handler = hotplug_event,
+	.handler = dock_event,
 };
 
 /* Check whether the PCI device is managed by native PCIe hotplug driver */
@@ -441,7 +460,9 @@ static void cleanup_bridge(struct acpiphp_bridge *bridge)
 	list_del(&bridge->list);
 	mutex_unlock(&bridge_mutex);
 
+	mutex_lock(&acpiphp_context_lock);
 	bridge->is_going_away = true;
+	mutex_unlock(&acpiphp_context_lock);
 }
 
 /**
@@ -742,7 +763,7 @@ static void trim_stale_devices(struct pci_dev *dev)
 
 		/* The device is a bridge. so check the bus below it. */
 		pm_runtime_get_sync(&dev->dev);
-		list_for_each_entry_safe(child, tmp, &bus->devices, bus_list)
+		list_for_each_entry_safe_reverse(child, tmp, &bus->devices, bus_list)
 			trim_stale_devices(child);
 
 		pm_runtime_put(&dev->dev);
@@ -773,8 +794,8 @@ static void acpiphp_check_bridge(struct acpiphp_bridge *bridge)
 			; /* do nothing */
 		} else if (get_slot_status(slot) == ACPI_STA_ALL) {
 			/* remove stale devices if any */
-			list_for_each_entry_safe(dev, tmp, &bus->devices,
-						 bus_list)
+			list_for_each_entry_safe_reverse(dev, tmp,
+							 &bus->devices, bus_list)
 				if (PCI_SLOT(dev->devfn) == slot->device)
 					trim_stale_devices(dev);
 
@@ -805,7 +826,7 @@ static void acpiphp_sanitize_bus(struct pci_bus *bus)
 	int i;
 	unsigned long type_mask = IORESOURCE_IO | IORESOURCE_MEM;
 
-	list_for_each_entry_safe(dev, tmp, &bus->devices, bus_list) {
+	list_for_each_entry_safe_reverse(dev, tmp, &bus->devices, bus_list) {
 		for (i=0; i<PCI_BRIDGE_RESOURCES; i++) {
 			struct resource *res = &dev->resource[i];
 			if ((res->flags & type_mask) && !res->start &&
@@ -829,7 +850,11 @@ void acpiphp_check_host_bridge(acpi_handle handle)
 
 	bridge = acpiphp_handle_to_bridge(handle);
 	if (bridge) {
+		pci_lock_rescan_remove();
+
 		acpiphp_check_bridge(bridge);
+
+		pci_unlock_rescan_remove();
 		put_bridge(bridge);
 	}
 }
@@ -852,6 +877,7 @@ static void hotplug_event(acpi_handle handle, u32 type, void *data)
 
 	mutex_unlock(&acpiphp_context_lock);
 
+	pci_lock_rescan_remove();
 	acpi_get_name(handle, ACPI_FULL_PATHNAME, &buffer);
 
 	switch (type) {
@@ -905,6 +931,7 @@ static void hotplug_event(acpi_handle handle, u32 type, void *data)
 		break;
 	}
 
+	pci_unlock_rescan_remove();
 	if (bridge)
 		put_bridge(bridge);
 }
@@ -915,11 +942,9 @@ static void hotplug_event_work(void *data, u32 type)
 	acpi_handle handle = context->handle;
 
 	acpi_scan_lock_acquire();
-	pci_lock_rescan_remove();
 
 	hotplug_event(handle, type, context);
 
-	pci_unlock_rescan_remove();
 	acpi_scan_lock_release();
 	acpi_evaluate_hotplug_ost(handle, type, ACPI_OST_SC_SUCCESS, NULL);
 	put_bridge(context->func.parent);
@@ -937,6 +962,7 @@ static void handle_hotplug_event(acpi_handle handle, u32 type, void *data)
 {
 	struct acpiphp_context *context;
 	u32 ost_code = ACPI_OST_SC_SUCCESS;
+	acpi_status status;
 
 	switch (type) {
 	case ACPI_NOTIFY_BUS_CHECK:
@@ -972,13 +998,20 @@ static void handle_hotplug_event(acpi_handle handle, u32 type, void *data)
 
 	mutex_lock(&acpiphp_context_lock);
 	context = acpiphp_get_context(handle);
-	if (context && !WARN_ON(context->handle != handle)) {
-		get_bridge(context->func.parent);
-		acpiphp_put_context(context);
-		acpi_hotplug_execute(hotplug_event_work, context, type);
+	if (!context || WARN_ON(context->handle != handle)
+	    || context->func.parent->is_going_away)
+		goto err_out;
+
+	get_bridge(context->func.parent);
+	acpiphp_put_context(context);
+	status = acpi_hotplug_execute(hotplug_event_work, context, type);
+	if (ACPI_SUCCESS(status)) {
 		mutex_unlock(&acpiphp_context_lock);
 		return;
 	}
+	put_bridge(context->func.parent);
+
+ err_out:
 	mutex_unlock(&acpiphp_context_lock);
 	ost_code = ACPI_OST_SC_NON_SPECIFIC_FAILURE;
 
diff --git a/drivers/pci/probe.c b/drivers/pci/probe.c
index 04796c056d12..6e34498ec9f0 100644
--- a/drivers/pci/probe.c
+++ b/drivers/pci/probe.c
@@ -1208,18 +1208,6 @@ static void pci_release_capabilities(struct pci_dev *dev)
 	pci_free_cap_save_buffers(dev);
 }
 
-static void pci_free_resources(struct pci_dev *dev)
-{
-	int i;
-
-	pci_cleanup_rom(dev);
-	for (i = 0; i < PCI_NUM_RESOURCES; i++) {
-		struct resource *res = dev->resource + i;
-		if (res->parent)
-			release_resource(res);
-	}
-}
-
 /**
  * pci_release_dev - free a pci device structure when all users of it are finished.
  * @dev: device that's been disconnected
@@ -1229,14 +1217,9 @@ static void pci_free_resources(struct pci_dev *dev)
  */
 static void pci_release_dev(struct device *dev)
 {
-	struct pci_dev *pci_dev = to_pci_dev(dev);
-
-	down_write(&pci_bus_sem);
-	list_del(&pci_dev->bus_list);
-	up_write(&pci_bus_sem);
-
-	pci_free_resources(pci_dev);
+	struct pci_dev *pci_dev;
 
+	pci_dev = to_pci_dev(dev);
 	pci_release_capabilities(pci_dev);
 	pci_release_of_node(pci_dev);
 	pcibios_release_device(pci_dev);
diff --git a/drivers/pci/remove.c b/drivers/pci/remove.c
index 4ff36bfa785e..8bd76c9ba21c 100644
--- a/drivers/pci/remove.c
+++ b/drivers/pci/remove.c
@@ -3,6 +3,18 @@
 #include <linux/pci-aspm.h>
 #include "pci.h"
 
+static void pci_free_resources(struct pci_dev *dev)
+{
+	int i;
+
+	pci_cleanup_rom(dev);
+	for (i = 0; i < PCI_NUM_RESOURCES; i++) {
+		struct resource *res = dev->resource + i;
+		if (res->parent)
+			release_resource(res);
+	}
+}
+
 static void pci_stop_dev(struct pci_dev *dev)
 {
 	pci_pme_active(dev, false);
@@ -25,6 +37,11 @@ static void pci_destroy_dev(struct pci_dev *dev)
 
 	device_del(&dev->dev);
 
+	down_write(&pci_bus_sem);
+	list_del(&dev->bus_list);
+	up_write(&pci_bus_sem);
+
+	pci_free_resources(dev);
 	put_device(&dev->dev);
 }
 
diff --git a/drivers/pinctrl/core.c b/drivers/pinctrl/core.c
index 5ee61a470016..c0fe6091566a 100644
--- a/drivers/pinctrl/core.c
+++ b/drivers/pinctrl/core.c
@@ -851,7 +851,9 @@ static struct pinctrl *create_pinctrl(struct device *dev)
 	kref_init(&p->users);
 
 	/* Add the pinctrl handle to the global list */
+	mutex_lock(&pinctrl_list_mutex);
 	list_add_tail(&p->node, &pinctrl_list);
+	mutex_unlock(&pinctrl_list_mutex);
 
 	return p;
 }
@@ -1642,8 +1644,10 @@ static void pinctrl_init_device_debugfs(struct pinctrl_dev *pctldev)
 			    device_root, pctldev, &pinctrl_groups_ops);
 	debugfs_create_file("gpio-ranges", S_IFREG | S_IRUGO,
 			    device_root, pctldev, &pinctrl_gpioranges_ops);
-	pinmux_init_device_debugfs(device_root, pctldev);
-	pinconf_init_device_debugfs(device_root, pctldev);
+	if (pctldev->desc->pmxops)
+		pinmux_init_device_debugfs(device_root, pctldev);
+	if (pctldev->desc->confops)
+		pinconf_init_device_debugfs(device_root, pctldev);
 }
 
 static void pinctrl_remove_device_debugfs(struct pinctrl_dev *pctldev)
diff --git a/drivers/pinctrl/pinctrl-at91.c b/drivers/pinctrl/pinctrl-at91.c
index 38c6f8b9790e..d990e33d8aa7 100644
--- a/drivers/pinctrl/pinctrl-at91.c
+++ b/drivers/pinctrl/pinctrl-at91.c
@@ -1286,22 +1286,22 @@ static int alt_gpio_irq_type(struct irq_data *d, unsigned type)
 
 	switch (type) {
 	case IRQ_TYPE_EDGE_RISING:
-		irq_set_handler(d->irq, handle_simple_irq);
+		__irq_set_handler_locked(d->irq, handle_simple_irq);
 		writel_relaxed(mask, pio + PIO_ESR);
 		writel_relaxed(mask, pio + PIO_REHLSR);
 		break;
 	case IRQ_TYPE_EDGE_FALLING:
-		irq_set_handler(d->irq, handle_simple_irq);
+		__irq_set_handler_locked(d->irq, handle_simple_irq);
 		writel_relaxed(mask, pio + PIO_ESR);
 		writel_relaxed(mask, pio + PIO_FELLSR);
 		break;
 	case IRQ_TYPE_LEVEL_LOW:
-		irq_set_handler(d->irq, handle_level_irq);
+		__irq_set_handler_locked(d->irq, handle_level_irq);
 		writel_relaxed(mask, pio + PIO_LSR);
 		writel_relaxed(mask, pio + PIO_FELLSR);
 		break;
 	case IRQ_TYPE_LEVEL_HIGH:
-		irq_set_handler(d->irq, handle_level_irq);
+		__irq_set_handler_locked(d->irq, handle_level_irq);
 		writel_relaxed(mask, pio + PIO_LSR);
 		writel_relaxed(mask, pio + PIO_REHLSR);
 		break;
@@ -1310,7 +1310,7 @@ static int alt_gpio_irq_type(struct irq_data *d, unsigned type)
 		 * disable additional interrupt modes:
 		 * fall back to default behavior
 		 */
-		irq_set_handler(d->irq, handle_simple_irq);
+		__irq_set_handler_locked(d->irq, handle_simple_irq);
 		writel_relaxed(mask, pio + PIO_AIMDR);
 		return 0;
 	case IRQ_TYPE_NONE:
diff --git a/drivers/pinctrl/pinctrl-imx1-core.c b/drivers/pinctrl/pinctrl-imx1-core.c
index 17aecde1b51d..815384b377b5 100644
--- a/drivers/pinctrl/pinctrl-imx1-core.c
+++ b/drivers/pinctrl/pinctrl-imx1-core.c
@@ -45,7 +45,7 @@ struct imx1_pinctrl {
 #define MX1_DDIR 0x00
 #define MX1_OCR 0x04
 #define MX1_ICONFA 0x0c
-#define MX1_ICONFB 0x10
+#define MX1_ICONFB 0x14
 #define MX1_GIUS 0x20
 #define MX1_GPR 0x38
 #define MX1_PUEN 0x40
@@ -97,13 +97,13 @@ static void imx1_write_2bit(struct imx1_pinctrl *ipctl, unsigned int pin_id,
 	u32 old_val;
 	u32 new_val;
 
-	dev_dbg(ipctl->dev, "write: register 0x%p offset %d value 0x%x\n",
-			reg, offset, value);
-
 	/* Use the next register if the pin's port pin number is >=16 */
 	if (pin_id % 32 >= 16)
 		reg += 0x04;
 
+	dev_dbg(ipctl->dev, "write: register 0x%p offset %d value 0x%x\n",
+			reg, offset, value);
+
 	/* Get current state of pins */
 	old_val = readl(reg);
 	old_val &= mask;
@@ -139,7 +139,7 @@ static int imx1_read_2bit(struct imx1_pinctrl *ipctl, unsigned int pin_id,
 		u32 reg_offset)
 {
 	void __iomem *reg = imx1_mem(ipctl, pin_id) + reg_offset;
-	int offset = pin_id % 16;
+	int offset = (pin_id % 16) * 2;
 
 	/* Use the next register if the pin's port pin number is >=16 */
 	if (pin_id % 32 >= 16)
diff --git a/drivers/pinctrl/pinctrl-tegra.c b/drivers/pinctrl/pinctrl-tegra.c
index a2e93a2b5ff4..e767355ab0ad 100644
--- a/drivers/pinctrl/pinctrl-tegra.c
+++ b/drivers/pinctrl/pinctrl-tegra.c
@@ -645,7 +645,7 @@ int tegra_pinctrl_probe(struct platform_device *pdev,
 				 GFP_KERNEL);
 	if (!pmx->regs) {
 		dev_err(&pdev->dev, "Can't alloc regs pointer\n");
-		return -ENODEV;
+		return -ENOMEM;
 	}
 
 	for (i = 0; i < pmx->nbanks; i++) {
diff --git a/drivers/pinctrl/sirf/pinctrl-prima2.c b/drivers/pinctrl/sirf/pinctrl-prima2.c
index 37b42651d76a..dde0285544d6 100644
--- a/drivers/pinctrl/sirf/pinctrl-prima2.c
+++ b/drivers/pinctrl/sirf/pinctrl-prima2.c
@@ -413,7 +413,7 @@ static const struct sirfsoc_padmux ac97_padmux = {
 	.funcval = 0,
 };
 
-static const unsigned ac97_pins[] = { 33, 34, 35, 36 };
+static const unsigned ac97_pins[] = { 43, 44, 45, 46 };
 
 static const struct sirfsoc_muxmask spi1_muxmask[] = {
 	{
diff --git a/drivers/pinctrl/vt8500/pinctrl-wmt.c b/drivers/pinctrl/vt8500/pinctrl-wmt.c
index b28d1af9c232..9802b67040cc 100644
--- a/drivers/pinctrl/vt8500/pinctrl-wmt.c
+++ b/drivers/pinctrl/vt8500/pinctrl-wmt.c
@@ -276,7 +276,20 @@ static int wmt_pctl_dt_node_to_map_pull(struct wmt_pinctrl_data *data,
 	if (!configs)
 		return -ENOMEM;
 
-	configs[0] = pull;
+	switch (pull) {
+	case 0:
+		configs[0] = PIN_CONFIG_BIAS_DISABLE;
+		break;
+	case 1:
+		configs[0] = PIN_CONFIG_BIAS_PULL_DOWN;
+		break;
+	case 2:
+		configs[0] = PIN_CONFIG_BIAS_PULL_UP;
+		break;
+	default:
+		configs[0] = PIN_CONFIG_BIAS_DISABLE;
+		dev_err(data->dev, "invalid pull state %d - disabling\n", pull);
+	}
 
 	map->type = PIN_MAP_TYPE_CONFIGS_PIN;
 	map->data.configs.group_or_pin = data->groups[group];
diff --git a/drivers/regulator/ab3100.c b/drivers/regulator/ab3100.c
index 77b46d0b37a6..e10febe9ec34 100644
--- a/drivers/regulator/ab3100.c
+++ b/drivers/regulator/ab3100.c
@@ -498,7 +498,7 @@ static int ab3100_regulator_register(struct platform_device *pdev,
 				     struct ab3100_platform_data *plfdata,
 				     struct regulator_init_data *init_data,
 				     struct device_node *np,
-				     int id)
+				     unsigned long id)
 {
 	struct regulator_desc *desc;
 	struct ab3100_regulator *reg;
@@ -646,7 +646,7 @@ ab3100_regulator_of_probe(struct platform_device *pdev, struct device_node *np)
 		err = ab3100_regulator_register(
 			pdev, NULL, ab3100_regulator_matches[i].init_data,
 			ab3100_regulator_matches[i].of_node,
-			(int) ab3100_regulator_matches[i].driver_data);
+			(unsigned long)ab3100_regulator_matches[i].driver_data);
 		if (err) {
 			ab3100_regulators_remove(pdev);
 			return err;
diff --git a/drivers/regulator/core.c b/drivers/regulator/core.c
index b38a6b669e8c..16a309e5c024 100644
--- a/drivers/regulator/core.c
+++ b/drivers/regulator/core.c
@@ -1272,6 +1272,8 @@ static struct regulator_dev *regulator_dev_lookup(struct device *dev,
 				if (r->dev.parent &&
 					node == r->dev.of_node)
 					return r;
+			*ret = -EPROBE_DEFER;
+			return NULL;
 		} else {
 			/*
 			 * If we couldn't even get the node then it's
@@ -1312,7 +1314,7 @@ static struct regulator *_regulator_get(struct device *dev, const char *id,
 	struct regulator_dev *rdev;
 	struct regulator *regulator = ERR_PTR(-EPROBE_DEFER);
 	const char *devname = NULL;
-	int ret = -EPROBE_DEFER;
+	int ret;
 
 	if (id == NULL) {
 		pr_err("get() with no identifier\n");
@@ -1322,6 +1324,11 @@ static struct regulator *_regulator_get(struct device *dev, const char *id,
 	if (dev)
 		devname = dev_name(dev);
 
+	if (have_full_constraints())
+		ret = -ENODEV;
+	else
+		ret = -EPROBE_DEFER;
+
 	mutex_lock(&regulator_list_mutex);
 
 	rdev = regulator_dev_lookup(dev, id, &ret);
diff --git a/drivers/regulator/s2mps11.c b/drivers/regulator/s2mps11.c
index d9e557990577..cd0b9e35a56d 100644
--- a/drivers/regulator/s2mps11.c
+++ b/drivers/regulator/s2mps11.c
@@ -441,6 +441,7 @@ common_reg:
 	for (i = 0; i < S2MPS11_REGULATOR_MAX; i++) {
 		if (!reg_np) {
 			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;
diff --git a/drivers/s390/block/dasd_diag.c b/drivers/s390/block/dasd_diag.c
index 92bd22ce6760..9cbc567698ce 100644
--- a/drivers/s390/block/dasd_diag.c
+++ b/drivers/s390/block/dasd_diag.c
@@ -504,7 +504,7 @@ static struct dasd_ccw_req *dasd_diag_build_cp(struct dasd_device *memdev,
 	struct dasd_diag_req *dreq;
 	struct dasd_diag_bio *dbio;
 	struct req_iterator iter;
-	struct bio_vec *bv;
+	struct bio_vec bv;
 	char *dst;
 	unsigned int count, datasize;
 	sector_t recid, first_rec, last_rec;
@@ -525,10 +525,10 @@ static struct dasd_ccw_req *dasd_diag_build_cp(struct dasd_device *memdev,
 	/* Check struct bio and count the number of blocks for the request. */
 	count = 0;
 	rq_for_each_segment(bv, req, iter) {
-		if (bv->bv_len & (blksize - 1))
+		if (bv.bv_len & (blksize - 1))
 			/* Fba can only do full blocks. */
 			return ERR_PTR(-EINVAL);
-		count += bv->bv_len >> (block->s2b_shift + 9);
+		count += bv.bv_len >> (block->s2b_shift + 9);
 	}
 	/* Paranoia. */
 	if (count != last_rec - first_rec + 1)
@@ -545,8 +545,8 @@ static struct dasd_ccw_req *dasd_diag_build_cp(struct dasd_device *memdev,
 	dbio = dreq->bio;
 	recid = first_rec;
 	rq_for_each_segment(bv, req, iter) {
-		dst = page_address(bv->bv_page) + bv->bv_offset;
-		for (off = 0; off < bv->bv_len; off += blksize) {
+		dst = page_address(bv.bv_page) + bv.bv_offset;
+		for (off = 0; off < bv.bv_len; off += blksize) {
 			memset(dbio, 0, sizeof (struct dasd_diag_bio));
 			dbio->type = rw_cmd;
 			dbio->block_number = recid + 1;
diff --git a/drivers/s390/block/dasd_eckd.c b/drivers/s390/block/dasd_eckd.c
index 95e45782692f..2e8e0755070b 100644
--- a/drivers/s390/block/dasd_eckd.c
+++ b/drivers/s390/block/dasd_eckd.c
@@ -2551,7 +2551,7 @@ static struct dasd_ccw_req *dasd_eckd_build_cp_cmd_single(
 	struct dasd_ccw_req *cqr;
 	struct ccw1 *ccw;
 	struct req_iterator iter;
-	struct bio_vec *bv;
+	struct bio_vec bv;
 	char *dst;
 	unsigned int off;
 	int count, cidaw, cplength, datasize;
@@ -2573,13 +2573,13 @@ static struct dasd_ccw_req *dasd_eckd_build_cp_cmd_single(
 	count = 0;
 	cidaw = 0;
 	rq_for_each_segment(bv, req, iter) {
-		if (bv->bv_len & (blksize - 1))
+		if (bv.bv_len & (blksize - 1))
 			/* Eckd can only do full blocks. */
 			return ERR_PTR(-EINVAL);
-		count += bv->bv_len >> (block->s2b_shift + 9);
+		count += bv.bv_len >> (block->s2b_shift + 9);
 #if defined(CONFIG_64BIT)
-		if (idal_is_needed (page_address(bv->bv_page), bv->bv_len))
-			cidaw += bv->bv_len >> (block->s2b_shift + 9);
+		if (idal_is_needed (page_address(bv.bv_page), bv.bv_len))
+			cidaw += bv.bv_len >> (block->s2b_shift + 9);
 #endif
 	}
 	/* Paranoia. */
@@ -2650,16 +2650,16 @@ static struct dasd_ccw_req *dasd_eckd_build_cp_cmd_single(
 			      last_rec - recid + 1, cmd, basedev, blksize);
 	}
 	rq_for_each_segment(bv, req, iter) {
-		dst = page_address(bv->bv_page) + bv->bv_offset;
+		dst = page_address(bv.bv_page) + bv.bv_offset;
 		if (dasd_page_cache) {
 			char *copy = kmem_cache_alloc(dasd_page_cache,
 						      GFP_DMA | __GFP_NOWARN);
 			if (copy && rq_data_dir(req) == WRITE)
-				memcpy(copy + bv->bv_offset, dst, bv->bv_len);
+				memcpy(copy + bv.bv_offset, dst, bv.bv_len);
 			if (copy)
-				dst = copy + bv->bv_offset;
+				dst = copy + bv.bv_offset;
 		}
-		for (off = 0; off < bv->bv_len; off += blksize) {
+		for (off = 0; off < bv.bv_len; off += blksize) {
 			sector_t trkid = recid;
 			unsigned int recoffs = sector_div(trkid, blk_per_trk);
 			rcmd = cmd;
@@ -2735,7 +2735,7 @@ static struct dasd_ccw_req *dasd_eckd_build_cp_cmd_track(
 	struct dasd_ccw_req *cqr;
 	struct ccw1 *ccw;
 	struct req_iterator iter;
-	struct bio_vec *bv;
+	struct bio_vec bv;
 	char *dst, *idaw_dst;
 	unsigned int cidaw, cplength, datasize;
 	unsigned int tlf;
@@ -2813,8 +2813,8 @@ static struct dasd_ccw_req *dasd_eckd_build_cp_cmd_track(
 	idaw_dst = NULL;
 	idaw_len = 0;
 	rq_for_each_segment(bv, req, iter) {
-		dst = page_address(bv->bv_page) + bv->bv_offset;
-		seg_len = bv->bv_len;
+		dst = page_address(bv.bv_page) + bv.bv_offset;
+		seg_len = bv.bv_len;
 		while (seg_len) {
 			if (new_track) {
 				trkid = recid;
@@ -3039,7 +3039,7 @@ static struct dasd_ccw_req *dasd_eckd_build_cp_tpm_track(
 {
 	struct dasd_ccw_req *cqr;
 	struct req_iterator iter;
-	struct bio_vec *bv;
+	struct bio_vec bv;
 	char *dst;
 	unsigned int trkcount, ctidaw;
 	unsigned char cmd;
@@ -3125,8 +3125,8 @@ static struct dasd_ccw_req *dasd_eckd_build_cp_tpm_track(
 		new_track = 1;
 		recid = first_rec;
 		rq_for_each_segment(bv, req, iter) {
-			dst = page_address(bv->bv_page) + bv->bv_offset;
-			seg_len = bv->bv_len;
+			dst = page_address(bv.bv_page) + bv.bv_offset;
+			seg_len = bv.bv_len;
 			while (seg_len) {
 				if (new_track) {
 					trkid = recid;
@@ -3158,9 +3158,9 @@ static struct dasd_ccw_req *dasd_eckd_build_cp_tpm_track(
 		}
 	} else {
 		rq_for_each_segment(bv, req, iter) {
-			dst = page_address(bv->bv_page) + bv->bv_offset;
+			dst = page_address(bv.bv_page) + bv.bv_offset;
 			last_tidaw = itcw_add_tidaw(itcw, 0x00,
-						    dst, bv->bv_len);
+						    dst, bv.bv_len);
 			if (IS_ERR(last_tidaw)) {
 				ret = -EINVAL;
 				goto out_error;
@@ -3278,7 +3278,7 @@ static struct dasd_ccw_req *dasd_raw_build_cp(struct dasd_device *startdev,
 	struct dasd_ccw_req *cqr;
 	struct ccw1 *ccw;
 	struct req_iterator iter;
-	struct bio_vec *bv;
+	struct bio_vec bv;
 	char *dst;
 	unsigned char cmd;
 	unsigned int trkcount;
@@ -3378,8 +3378,8 @@ static struct dasd_ccw_req *dasd_raw_build_cp(struct dasd_device *startdev,
 			idaws = idal_create_words(idaws, rawpadpage, PAGE_SIZE);
 	}
 	rq_for_each_segment(bv, req, iter) {
-		dst = page_address(bv->bv_page) + bv->bv_offset;
-		seg_len = bv->bv_len;
+		dst = page_address(bv.bv_page) + bv.bv_offset;
+		seg_len = bv.bv_len;
 		if (cmd == DASD_ECKD_CCW_READ_TRACK)
 			memset(dst, 0, seg_len);
 		if (!len_to_track_end) {
@@ -3424,7 +3424,7 @@ dasd_eckd_free_cp(struct dasd_ccw_req *cqr, struct request *req)
 	struct dasd_eckd_private *private;
 	struct ccw1 *ccw;
 	struct req_iterator iter;
-	struct bio_vec *bv;
+	struct bio_vec bv;
 	char *dst, *cda;
 	unsigned int blksize, blk_per_trk, off;
 	sector_t recid;
@@ -3442,8 +3442,8 @@ dasd_eckd_free_cp(struct dasd_ccw_req *cqr, struct request *req)
 	if (private->uses_cdl == 0 || recid > 2*blk_per_trk)
 		ccw++;
 	rq_for_each_segment(bv, req, iter) {
-		dst = page_address(bv->bv_page) + bv->bv_offset;
-		for (off = 0; off < bv->bv_len; off += blksize) {
+		dst = page_address(bv.bv_page) + bv.bv_offset;
+		for (off = 0; off < bv.bv_len; off += blksize) {
 			/* Skip locate record. */
 			if (private->uses_cdl && recid <= 2*blk_per_trk)
 				ccw++;
@@ -3454,7 +3454,7 @@ dasd_eckd_free_cp(struct dasd_ccw_req *cqr, struct request *req)
 					cda = (char *)((addr_t) ccw->cda);
 				if (dst != cda) {
 					if (rq_data_dir(req) == READ)
-						memcpy(dst, cda, bv->bv_len);
+						memcpy(dst, cda, bv.bv_len);
 					kmem_cache_free(dasd_page_cache,
 					    (void *)((addr_t)cda & PAGE_MASK));
 				}
diff --git a/drivers/s390/block/dasd_fba.c b/drivers/s390/block/dasd_fba.c
index 9cbc8c32ba59..2c8e68bf9a1c 100644
--- a/drivers/s390/block/dasd_fba.c
+++ b/drivers/s390/block/dasd_fba.c
@@ -260,7 +260,7 @@ static struct dasd_ccw_req *dasd_fba_build_cp(struct dasd_device * memdev,
 	struct dasd_ccw_req *cqr;
 	struct ccw1 *ccw;
 	struct req_iterator iter;
-	struct bio_vec *bv;
+	struct bio_vec bv;
 	char *dst;
 	int count, cidaw, cplength, datasize;
 	sector_t recid, first_rec, last_rec;
@@ -283,13 +283,13 @@ static struct dasd_ccw_req *dasd_fba_build_cp(struct dasd_device * memdev,
 	count = 0;
 	cidaw = 0;
 	rq_for_each_segment(bv, req, iter) {
-		if (bv->bv_len & (blksize - 1))
+		if (bv.bv_len & (blksize - 1))
 			/* Fba can only do full blocks. */
 			return ERR_PTR(-EINVAL);
-		count += bv->bv_len >> (block->s2b_shift + 9);
+		count += bv.bv_len >> (block->s2b_shift + 9);
 #if defined(CONFIG_64BIT)
-		if (idal_is_needed (page_address(bv->bv_page), bv->bv_len))
-			cidaw += bv->bv_len / blksize;
+		if (idal_is_needed (page_address(bv.bv_page), bv.bv_len))
+			cidaw += bv.bv_len / blksize;
 #endif
 	}
 	/* Paranoia. */
@@ -326,16 +326,16 @@ static struct dasd_ccw_req *dasd_fba_build_cp(struct dasd_device * memdev,
 	}
 	recid = first_rec;
 	rq_for_each_segment(bv, req, iter) {
-		dst = page_address(bv->bv_page) + bv->bv_offset;
+		dst = page_address(bv.bv_page) + bv.bv_offset;
 		if (dasd_page_cache) {
 			char *copy = kmem_cache_alloc(dasd_page_cache,
 						      GFP_DMA | __GFP_NOWARN);
 			if (copy && rq_data_dir(req) == WRITE)
-				memcpy(copy + bv->bv_offset, dst, bv->bv_len);
+				memcpy(copy + bv.bv_offset, dst, bv.bv_len);
 			if (copy)
-				dst = copy + bv->bv_offset;
+				dst = copy + bv.bv_offset;
 		}
-		for (off = 0; off < bv->bv_len; off += blksize) {
+		for (off = 0; off < bv.bv_len; off += blksize) {
 			/* Locate record for stupid devices. */
 			if (private->rdc_data.mode.bits.data_chain == 0) {
 				ccw[-1].flags |= CCW_FLAG_CC;
@@ -384,7 +384,7 @@ dasd_fba_free_cp(struct dasd_ccw_req *cqr, struct request *req)
 	struct dasd_fba_private *private;
 	struct ccw1 *ccw;
 	struct req_iterator iter;
-	struct bio_vec *bv;
+	struct bio_vec bv;
 	char *dst, *cda;
 	unsigned int blksize, off;
 	int status;
@@ -399,8 +399,8 @@ dasd_fba_free_cp(struct dasd_ccw_req *cqr, struct request *req)
 	if (private->rdc_data.mode.bits.data_chain != 0)
 		ccw++;
 	rq_for_each_segment(bv, req, iter) {
-		dst = page_address(bv->bv_page) + bv->bv_offset;
-		for (off = 0; off < bv->bv_len; off += blksize) {
+		dst = page_address(bv.bv_page) + bv.bv_offset;
+		for (off = 0; off < bv.bv_len; off += blksize) {
 			/* Skip locate record. */
 			if (private->rdc_data.mode.bits.data_chain == 0)
 				ccw++;
@@ -411,7 +411,7 @@ dasd_fba_free_cp(struct dasd_ccw_req *cqr, struct request *req)
 					cda = (char *)((addr_t) ccw->cda);
 				if (dst != cda) {
 					if (rq_data_dir(req) == READ)
-						memcpy(dst, cda, bv->bv_len);
+						memcpy(dst, cda, bv.bv_len);
 					kmem_cache_free(dasd_page_cache,
 					    (void *)((addr_t)cda & PAGE_MASK));
 				}
diff --git a/drivers/s390/block/dcssblk.c b/drivers/s390/block/dcssblk.c
index 6eca019bcf30..ebf41e228e55 100644
--- a/drivers/s390/block/dcssblk.c
+++ b/drivers/s390/block/dcssblk.c
@@ -808,18 +808,19 @@ static void
 dcssblk_make_request(struct request_queue *q, struct bio *bio)
 {
 	struct dcssblk_dev_info *dev_info;
-	struct bio_vec *bvec;
+	struct bio_vec bvec;
+	struct bvec_iter iter;
 	unsigned long index;
 	unsigned long page_addr;
 	unsigned long source_addr;
 	unsigned long bytes_done;
-	int i;
 
 	bytes_done = 0;
 	dev_info = bio->bi_bdev->bd_disk->private_data;
 	if (dev_info == NULL)
 		goto fail;
-	if ((bio->bi_sector & 7) != 0 || (bio->bi_size & 4095) != 0)
+	if ((bio->bi_iter.bi_sector & 7) != 0 ||
+	    (bio->bi_iter.bi_size & 4095) != 0)
 		/* Request is not page-aligned. */
 		goto fail;
 	if (bio_end_sector(bio) > get_capacity(bio->bi_bdev->bd_disk)) {
@@ -842,22 +843,22 @@ dcssblk_make_request(struct request_queue *q, struct bio *bio)
 		}
 	}
 
-	index = (bio->bi_sector >> 3);
-	bio_for_each_segment(bvec, bio, i) {
+	index = (bio->bi_iter.bi_sector >> 3);
+	bio_for_each_segment(bvec, bio, iter) {
 		page_addr = (unsigned long)
-			page_address(bvec->bv_page) + bvec->bv_offset;
+			page_address(bvec.bv_page) + bvec.bv_offset;
 		source_addr = dev_info->start + (index<<12) + bytes_done;
-		if (unlikely((page_addr & 4095) != 0) || (bvec->bv_len & 4095) != 0)
+		if (unlikely((page_addr & 4095) != 0) || (bvec.bv_len & 4095) != 0)
 			// More paranoia.
 			goto fail;
 		if (bio_data_dir(bio) == READ) {
 			memcpy((void*)page_addr, (void*)source_addr,
-				bvec->bv_len);
+				bvec.bv_len);
 		} else {
 			memcpy((void*)source_addr, (void*)page_addr,
-				bvec->bv_len);
+				bvec.bv_len);
 		}
-		bytes_done += bvec->bv_len;
+		bytes_done += bvec.bv_len;
 	}
 	bio_endio(bio, 0);
 	return;
diff --git a/drivers/s390/block/scm_blk.c b/drivers/s390/block/scm_blk.c
index d0ab5019d885..76bed1743db1 100644
--- a/drivers/s390/block/scm_blk.c
+++ b/drivers/s390/block/scm_blk.c
@@ -130,7 +130,7 @@ static void scm_request_prepare(struct scm_request *scmrq)
 	struct aidaw *aidaw = scmrq->aidaw;
 	struct msb *msb = &scmrq->aob->msb[0];
 	struct req_iterator iter;
-	struct bio_vec *bv;
+	struct bio_vec bv;
 
 	msb->bs = MSB_BS_4K;
 	scmrq->aob->request.msb_count = 1;
@@ -142,9 +142,9 @@ static void scm_request_prepare(struct scm_request *scmrq)
 	msb->data_addr = (u64) aidaw;
 
 	rq_for_each_segment(bv, scmrq->request, iter) {
-		WARN_ON(bv->bv_offset);
-		msb->blk_count += bv->bv_len >> 12;
-		aidaw->data_addr = (u64) page_address(bv->bv_page);
+		WARN_ON(bv.bv_offset);
+		msb->blk_count += bv.bv_len >> 12;
+		aidaw->data_addr = (u64) page_address(bv.bv_page);
 		aidaw++;
 	}
 }
diff --git a/drivers/s390/block/scm_blk_cluster.c b/drivers/s390/block/scm_blk_cluster.c
index 27f930cd657f..9aae909d47a5 100644
--- a/drivers/s390/block/scm_blk_cluster.c
+++ b/drivers/s390/block/scm_blk_cluster.c
@@ -122,7 +122,7 @@ static void scm_prepare_cluster_request(struct scm_request *scmrq)
 	struct aidaw *aidaw = scmrq->aidaw;
 	struct msb *msb = &scmrq->aob->msb[0];
 	struct req_iterator iter;
-	struct bio_vec *bv;
+	struct bio_vec bv;
 	int i = 0;
 	u64 addr;
 
@@ -163,7 +163,7 @@ static void scm_prepare_cluster_request(struct scm_request *scmrq)
 			i++;
 		}
 		rq_for_each_segment(bv, req, iter) {
-			aidaw->data_addr = (u64) page_address(bv->bv_page);
+			aidaw->data_addr = (u64) page_address(bv.bv_page);
 			aidaw++;
 			i++;
 		}
diff --git a/drivers/s390/block/xpram.c b/drivers/s390/block/xpram.c
index 58141f0651f2..6969d39f1e2e 100644
--- a/drivers/s390/block/xpram.c
+++ b/drivers/s390/block/xpram.c
@@ -184,25 +184,26 @@ static unsigned long xpram_highest_page_index(void)
 static void xpram_make_request(struct request_queue *q, struct bio *bio)
 {
 	xpram_device_t *xdev = bio->bi_bdev->bd_disk->private_data;
-	struct bio_vec *bvec;
+	struct bio_vec bvec;
+	struct bvec_iter iter;
 	unsigned int index;
 	unsigned long page_addr;
 	unsigned long bytes;
-	int i;
 
-	if ((bio->bi_sector & 7) != 0 || (bio->bi_size & 4095) != 0)
+	if ((bio->bi_iter.bi_sector & 7) != 0 ||
+	    (bio->bi_iter.bi_size & 4095) != 0)
 		/* Request is not page-aligned. */
 		goto fail;
-	if ((bio->bi_size >> 12) > xdev->size)
+	if ((bio->bi_iter.bi_size >> 12) > xdev->size)
 		/* Request size is no page-aligned. */
 		goto fail;
-	if ((bio->bi_sector >> 3) > 0xffffffffU - xdev->offset)
+	if ((bio->bi_iter.bi_sector >> 3) > 0xffffffffU - xdev->offset)
 		goto fail;
-	index = (bio->bi_sector >> 3) + xdev->offset;
-	bio_for_each_segment(bvec, bio, i) {
+	index = (bio->bi_iter.bi_sector >> 3) + xdev->offset;
+	bio_for_each_segment(bvec, bio, iter) {
 		page_addr = (unsigned long)
-			kmap(bvec->bv_page) + bvec->bv_offset;
-		bytes = bvec->bv_len;
+			kmap(bvec.bv_page) + bvec.bv_offset;
+		bytes = bvec.bv_len;
 		if ((page_addr & 4095) != 0 || (bytes & 4095) != 0)
 			/* More paranoia. */
 			goto fail;
diff --git a/drivers/s390/kvm/virtio_ccw.c b/drivers/s390/kvm/virtio_ccw.c
index d6297176ab85..0fc584832001 100644
--- a/drivers/s390/kvm/virtio_ccw.c
+++ b/drivers/s390/kvm/virtio_ccw.c
@@ -642,8 +642,15 @@ static void virtio_ccw_int_handler(struct ccw_device *cdev,
 	     (SCSW_STCTL_ALERT_STATUS | SCSW_STCTL_STATUS_PEND))) {
 		/* OK */
 	}
-	if (irb_is_error(irb))
-		vcdev->err = -EIO; /* XXX - use real error */
+	if (irb_is_error(irb)) {
+		/* Command reject? */
+		if ((scsw_dstat(&irb->scsw) & DEV_STAT_UNIT_CHECK) &&
+		    (irb->ecw[0] & SNS0_CMD_REJECT))
+			vcdev->err = -EOPNOTSUPP;
+		else
+			/* Map everything else to -EIO. */
+			vcdev->err = -EIO;
+	}
 	if (vcdev->curr_io & activity) {
 		switch (activity) {
 		case VIRTIO_CCW_DOING_READ_FEAT:
diff --git a/drivers/scsi/libsas/sas_expander.c b/drivers/scsi/libsas/sas_expander.c
index 446b85110a1f..0cac7d8fd0f7 100644
--- a/drivers/scsi/libsas/sas_expander.c
+++ b/drivers/scsi/libsas/sas_expander.c
@@ -2163,10 +2163,10 @@ int sas_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 	}
 
 	/* do we need to support multiple segments? */
-	if (bio_segments(req->bio) > 1 || bio_segments(rsp->bio) > 1) {
-		printk("%s: multiple segments req %u %u, rsp %u %u\n",
-		       __func__, bio_segments(req->bio), blk_rq_bytes(req),
-		       bio_segments(rsp->bio), blk_rq_bytes(rsp));
+	if (bio_multiple_segments(req->bio) ||
+	    bio_multiple_segments(rsp->bio)) {
+		printk("%s: multiple segments req %u, rsp %u\n",
+		       __func__, blk_rq_bytes(req), blk_rq_bytes(rsp));
 		return -EINVAL;
 	}
 
diff --git a/drivers/scsi/mpt2sas/mpt2sas_transport.c b/drivers/scsi/mpt2sas/mpt2sas_transport.c
index 9d26637308be..410f4a3e8888 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_transport.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_transport.c
@@ -1901,7 +1901,7 @@ _transport_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 	struct MPT2SAS_ADAPTER *ioc = shost_priv(shost);
 	Mpi2SmpPassthroughRequest_t *mpi_request;
 	Mpi2SmpPassthroughReply_t *mpi_reply;
-	int rc, i;
+	int rc;
 	u16 smid;
 	u32 ioc_state;
 	unsigned long timeleft;
@@ -1916,7 +1916,8 @@ _transport_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 	void *pci_addr_out = NULL;
 	u16 wait_state_count;
 	struct request *rsp = req->next_rq;
-	struct bio_vec *bvec = NULL;
+	struct bio_vec bvec;
+	struct bvec_iter iter;
 
 	if (!rsp) {
 		printk(MPT2SAS_ERR_FMT "%s: the smp response space is "
@@ -1942,7 +1943,7 @@ _transport_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 	ioc->transport_cmds.status = MPT2_CMD_PENDING;
 
 	/* Check if the request is split across multiple segments */
-	if (bio_segments(req->bio) > 1) {
+	if (bio_multiple_segments(req->bio)) {
 		u32 offset = 0;
 
 		/* Allocate memory and copy the request */
@@ -1955,11 +1956,11 @@ _transport_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 			goto out;
 		}
 
-		bio_for_each_segment(bvec, req->bio, i) {
+		bio_for_each_segment(bvec, req->bio, iter) {
 			memcpy(pci_addr_out + offset,
-			    page_address(bvec->bv_page) + bvec->bv_offset,
-			    bvec->bv_len);
-			offset += bvec->bv_len;
+			    page_address(bvec.bv_page) + bvec.bv_offset,
+			    bvec.bv_len);
+			offset += bvec.bv_len;
 		}
 	} else {
 		dma_addr_out = pci_map_single(ioc->pdev, bio_data(req->bio),
@@ -1974,7 +1975,7 @@ _transport_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 
 	/* Check if the response needs to be populated across
 	 * multiple segments */
-	if (bio_segments(rsp->bio) > 1) {
+	if (bio_multiple_segments(rsp->bio)) {
 		pci_addr_in = pci_alloc_consistent(ioc->pdev, blk_rq_bytes(rsp),
 		    &pci_dma_in);
 		if (!pci_addr_in) {
@@ -2041,7 +2042,7 @@ _transport_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 	sgl_flags = (MPI2_SGE_FLAGS_SIMPLE_ELEMENT |
 	    MPI2_SGE_FLAGS_END_OF_BUFFER | MPI2_SGE_FLAGS_HOST_TO_IOC);
 	sgl_flags = sgl_flags << MPI2_SGE_FLAGS_SHIFT;
-	if (bio_segments(req->bio) > 1) {
+	if (bio_multiple_segments(req->bio)) {
 		ioc->base_add_sg_single(psge, sgl_flags |
 		    (blk_rq_bytes(req) - 4), pci_dma_out);
 	} else {
@@ -2057,7 +2058,7 @@ _transport_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 	    MPI2_SGE_FLAGS_LAST_ELEMENT | MPI2_SGE_FLAGS_END_OF_BUFFER |
 	    MPI2_SGE_FLAGS_END_OF_LIST);
 	sgl_flags = sgl_flags << MPI2_SGE_FLAGS_SHIFT;
-	if (bio_segments(rsp->bio) > 1) {
+	if (bio_multiple_segments(rsp->bio)) {
 		ioc->base_add_sg_single(psge, sgl_flags |
 		    (blk_rq_bytes(rsp) + 4), pci_dma_in);
 	} else {
@@ -2102,23 +2103,23 @@ _transport_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 		    le16_to_cpu(mpi_reply->ResponseDataLength);
 		/* check if the resp needs to be copied from the allocated
 		 * pci mem */
-		if (bio_segments(rsp->bio) > 1) {
+		if (bio_multiple_segments(rsp->bio)) {
 			u32 offset = 0;
 			u32 bytes_to_copy =
 			    le16_to_cpu(mpi_reply->ResponseDataLength);
-			bio_for_each_segment(bvec, rsp->bio, i) {
-				if (bytes_to_copy <= bvec->bv_len) {
-					memcpy(page_address(bvec->bv_page) +
-					    bvec->bv_offset, pci_addr_in +
+			bio_for_each_segment(bvec, rsp->bio, iter) {
+				if (bytes_to_copy <= bvec.bv_len) {
+					memcpy(page_address(bvec.bv_page) +
+					    bvec.bv_offset, pci_addr_in +
 					    offset, bytes_to_copy);
 					break;
 				} else {
-					memcpy(page_address(bvec->bv_page) +
-					    bvec->bv_offset, pci_addr_in +
-					    offset, bvec->bv_len);
-					bytes_to_copy -= bvec->bv_len;
+					memcpy(page_address(bvec.bv_page) +
+					    bvec.bv_offset, pci_addr_in +
+					    offset, bvec.bv_len);
+					bytes_to_copy -= bvec.bv_len;
 				}
-				offset += bvec->bv_len;
+				offset += bvec.bv_len;
 			}
 		}
 	} else {
diff --git a/drivers/scsi/mpt3sas/mpt3sas_transport.c b/drivers/scsi/mpt3sas/mpt3sas_transport.c
index e771a88c6a74..65170cb1a00f 100644
--- a/drivers/scsi/mpt3sas/mpt3sas_transport.c
+++ b/drivers/scsi/mpt3sas/mpt3sas_transport.c
@@ -1884,7 +1884,7 @@ _transport_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 	struct MPT3SAS_ADAPTER *ioc = shost_priv(shost);
 	Mpi2SmpPassthroughRequest_t *mpi_request;
 	Mpi2SmpPassthroughReply_t *mpi_reply;
-	int rc, i;
+	int rc;
 	u16 smid;
 	u32 ioc_state;
 	unsigned long timeleft;
@@ -1898,7 +1898,8 @@ _transport_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 	void *pci_addr_out = NULL;
 	u16 wait_state_count;
 	struct request *rsp = req->next_rq;
-	struct bio_vec *bvec = NULL;
+	struct bio_vec bvec;
+	struct bvec_iter iter;
 
 	if (!rsp) {
 		pr_err(MPT3SAS_FMT "%s: the smp response space is missing\n",
@@ -1925,7 +1926,7 @@ _transport_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 	ioc->transport_cmds.status = MPT3_CMD_PENDING;
 
 	/* Check if the request is split across multiple segments */
-	if (req->bio->bi_vcnt > 1) {
+	if (bio_multiple_segments(req->bio)) {
 		u32 offset = 0;
 
 		/* Allocate memory and copy the request */
@@ -1938,11 +1939,11 @@ _transport_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 			goto out;
 		}
 
-		bio_for_each_segment(bvec, req->bio, i) {
+		bio_for_each_segment(bvec, req->bio, iter) {
 			memcpy(pci_addr_out + offset,
-			    page_address(bvec->bv_page) + bvec->bv_offset,
-			    bvec->bv_len);
-			offset += bvec->bv_len;
+			    page_address(bvec.bv_page) + bvec.bv_offset,
+			    bvec.bv_len);
+			offset += bvec.bv_len;
 		}
 	} else {
 		dma_addr_out = pci_map_single(ioc->pdev, bio_data(req->bio),
@@ -1957,7 +1958,7 @@ _transport_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 
 	/* Check if the response needs to be populated across
 	 * multiple segments */
-	if (rsp->bio->bi_vcnt > 1) {
+	if (bio_multiple_segments(rsp->bio)) {
 		pci_addr_in = pci_alloc_consistent(ioc->pdev, blk_rq_bytes(rsp),
 		    &pci_dma_in);
 		if (!pci_addr_in) {
@@ -2018,7 +2019,7 @@ _transport_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 	mpi_request->RequestDataLength = cpu_to_le16(blk_rq_bytes(req) - 4);
 	psge = &mpi_request->SGL;
 
-	if (req->bio->bi_vcnt > 1)
+	if (bio_multiple_segments(req->bio))
 		ioc->build_sg(ioc, psge, pci_dma_out, (blk_rq_bytes(req) - 4),
 		    pci_dma_in, (blk_rq_bytes(rsp) + 4));
 	else
@@ -2063,23 +2064,23 @@ _transport_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 
 		/* check if the resp needs to be copied from the allocated
 		 * pci mem */
-		if (rsp->bio->bi_vcnt > 1) {
+		if (bio_multiple_segments(rsp->bio)) {
 			u32 offset = 0;
 			u32 bytes_to_copy =
 			    le16_to_cpu(mpi_reply->ResponseDataLength);
-			bio_for_each_segment(bvec, rsp->bio, i) {
-				if (bytes_to_copy <= bvec->bv_len) {
-					memcpy(page_address(bvec->bv_page) +
-					    bvec->bv_offset, pci_addr_in +
+			bio_for_each_segment(bvec, rsp->bio, iter) {
+				if (bytes_to_copy <= bvec.bv_len) {
+					memcpy(page_address(bvec.bv_page) +
+					    bvec.bv_offset, pci_addr_in +
 					    offset, bytes_to_copy);
 					break;
 				} else {
-					memcpy(page_address(bvec->bv_page) +
-					    bvec->bv_offset, pci_addr_in +
-					    offset, bvec->bv_len);
-					bytes_to_copy -= bvec->bv_len;
+					memcpy(page_address(bvec.bv_page) +
+					    bvec.bv_offset, pci_addr_in +
+					    offset, bvec.bv_len);
+					bytes_to_copy -= bvec.bv_len;
 				}
-				offset += bvec->bv_len;
+				offset += bvec.bv_len;
 			}
 		}
 	} else {
diff --git a/drivers/scsi/osd/osd_initiator.c b/drivers/scsi/osd/osd_initiator.c
index aa66361ed44b..bac04c2335aa 100644
--- a/drivers/scsi/osd/osd_initiator.c
+++ b/drivers/scsi/osd/osd_initiator.c
@@ -731,7 +731,7 @@ static int _osd_req_list_objects(struct osd_request *or,
 
 	bio->bi_rw &= ~REQ_WRITE;
 	or->in.bio = bio;
-	or->in.total_bytes = bio->bi_size;
+	or->in.total_bytes = bio->bi_iter.bi_size;
 	return 0;
 }
 
diff --git a/drivers/scsi/qla2xxx/qla_attr.c b/drivers/scsi/qla2xxx/qla_attr.c
index 570c7fcc0c4d..4a0d7c92181f 100644
--- a/drivers/scsi/qla2xxx/qla_attr.c
+++ b/drivers/scsi/qla2xxx/qla_attr.c
@@ -1990,6 +1990,8 @@ qla24xx_vport_delete(struct fc_vport *fc_vport)
 
 	vha->flags.delete_progress = 1;
 
+	qlt_remove_target(ha, vha);
+
 	fc_remove_host(vha->host);
 
 	scsi_remove_host(vha->host);
diff --git a/drivers/scsi/qla2xxx/qla_def.h b/drivers/scsi/qla2xxx/qla_def.h
index 41d6491d7bd9..e1fe95ef23e1 100644
--- a/drivers/scsi/qla2xxx/qla_def.h
+++ b/drivers/scsi/qla2xxx/qla_def.h
@@ -2750,6 +2750,13 @@ struct qlfc_fw {
 	uint32_t len;
 };
 
+struct scsi_qlt_host {
+	void *target_lport_ptr;
+	struct mutex tgt_mutex;
+	struct mutex tgt_host_action_mutex;
+	struct qla_tgt *qla_tgt;
+};
+
 struct qlt_hw_data {
 	/* Protected by hw lock */
 	uint32_t enable_class_2:1;
@@ -2765,15 +2772,11 @@ struct qlt_hw_data {
 	uint32_t __iomem *atio_q_in;
 	uint32_t __iomem *atio_q_out;
 
-	void *target_lport_ptr;
 	struct qla_tgt_func_tmpl *tgt_ops;
-	struct qla_tgt *qla_tgt;
 	struct qla_tgt_cmd *cmds[DEFAULT_OUTSTANDING_COMMANDS];
 	uint16_t current_handle;
 
 	struct qla_tgt_vp_map *tgt_vp_map;
-	struct mutex tgt_mutex;
-	struct mutex tgt_host_action_mutex;
 
 	int saved_set;
 	uint16_t saved_exchange_count;
@@ -3435,6 +3438,7 @@ typedef struct scsi_qla_host {
 #define VP_ERR_FAB_LOGOUT	4
 #define VP_ERR_ADAP_NORESOURCES	5
 	struct qla_hw_data *hw;
+	struct scsi_qlt_host vha_tgt;
 	struct req_que *req;
 	int		fw_heartbeat_counter;
 	int		seconds_since_last_heartbeat;
diff --git a/drivers/scsi/qla2xxx/qla_target.c b/drivers/scsi/qla2xxx/qla_target.c
index 38a1257e76e1..9e80d61e5a3a 100644
--- a/drivers/scsi/qla2xxx/qla_target.c
+++ b/drivers/scsi/qla2xxx/qla_target.c
@@ -590,7 +590,7 @@ static struct qla_tgt_sess *qlt_create_sess(
 
 	/* Check to avoid double sessions */
 	spin_lock_irqsave(&ha->hardware_lock, flags);
-	list_for_each_entry(sess, &ha->tgt.qla_tgt->sess_list,
+	list_for_each_entry(sess, &vha->vha_tgt.qla_tgt->sess_list,
 				sess_list_entry) {
 		if (!memcmp(sess->port_name, fcport->port_name, WWN_SIZE)) {
 			ql_dbg(ql_dbg_tgt_mgt, vha, 0xf005,
@@ -627,7 +627,7 @@ static struct qla_tgt_sess *qlt_create_sess(
 
 		return NULL;
 	}
-	sess->tgt = ha->tgt.qla_tgt;
+	sess->tgt = vha->vha_tgt.qla_tgt;
 	sess->vha = vha;
 	sess->s_id = fcport->d_id;
 	sess->loop_id = fcport->loop_id;
@@ -635,7 +635,7 @@ static struct qla_tgt_sess *qlt_create_sess(
 
 	ql_dbg(ql_dbg_tgt_mgt, vha, 0xf006,
 	    "Adding sess %p to tgt %p via ->check_initiator_node_acl()\n",
-	    sess, ha->tgt.qla_tgt);
+	    sess, vha->vha_tgt.qla_tgt);
 
 	be_sid[0] = sess->s_id.b.domain;
 	be_sid[1] = sess->s_id.b.area;
@@ -662,8 +662,8 @@ static struct qla_tgt_sess *qlt_create_sess(
 	memcpy(sess->port_name, fcport->port_name, sizeof(sess->port_name));
 
 	spin_lock_irqsave(&ha->hardware_lock, flags);
-	list_add_tail(&sess->sess_list_entry, &ha->tgt.qla_tgt->sess_list);
-	ha->tgt.qla_tgt->sess_count++;
+	list_add_tail(&sess->sess_list_entry, &vha->vha_tgt.qla_tgt->sess_list);
+	vha->vha_tgt.qla_tgt->sess_count++;
 	spin_unlock_irqrestore(&ha->hardware_lock, flags);
 
 	ql_dbg(ql_dbg_tgt_mgt, vha, 0xf04b,
@@ -682,7 +682,7 @@ static struct qla_tgt_sess *qlt_create_sess(
 void qlt_fc_port_added(struct scsi_qla_host *vha, fc_port_t *fcport)
 {
 	struct qla_hw_data *ha = vha->hw;
-	struct qla_tgt *tgt = ha->tgt.qla_tgt;
+	struct qla_tgt *tgt = vha->vha_tgt.qla_tgt;
 	struct qla_tgt_sess *sess;
 	unsigned long flags;
 
@@ -692,6 +692,9 @@ void qlt_fc_port_added(struct scsi_qla_host *vha, fc_port_t *fcport)
 	if (!tgt || (fcport->port_type != FCT_INITIATOR))
 		return;
 
+	if (qla_ini_mode_enabled(vha))
+		return;
+
 	spin_lock_irqsave(&ha->hardware_lock, flags);
 	if (tgt->tgt_stop) {
 		spin_unlock_irqrestore(&ha->hardware_lock, flags);
@@ -701,9 +704,9 @@ void qlt_fc_port_added(struct scsi_qla_host *vha, fc_port_t *fcport)
 	if (!sess) {
 		spin_unlock_irqrestore(&ha->hardware_lock, flags);
 
-		mutex_lock(&ha->tgt.tgt_mutex);
+		mutex_lock(&vha->vha_tgt.tgt_mutex);
 		sess = qlt_create_sess(vha, fcport, false);
-		mutex_unlock(&ha->tgt.tgt_mutex);
+		mutex_unlock(&vha->vha_tgt.tgt_mutex);
 
 		spin_lock_irqsave(&ha->hardware_lock, flags);
 	} else {
@@ -739,7 +742,7 @@ void qlt_fc_port_added(struct scsi_qla_host *vha, fc_port_t *fcport)
 void qlt_fc_port_deleted(struct scsi_qla_host *vha, fc_port_t *fcport)
 {
 	struct qla_hw_data *ha = vha->hw;
-	struct qla_tgt *tgt = ha->tgt.qla_tgt;
+	struct qla_tgt *tgt = vha->vha_tgt.qla_tgt;
 	struct qla_tgt_sess *sess;
 	unsigned long flags;
 
@@ -806,12 +809,12 @@ void qlt_stop_phase1(struct qla_tgt *tgt)
 	 * Mutex needed to sync with qla_tgt_fc_port_[added,deleted].
 	 * Lock is needed, because we still can get an incoming packet.
 	 */
-	mutex_lock(&ha->tgt.tgt_mutex);
+	mutex_lock(&vha->vha_tgt.tgt_mutex);
 	spin_lock_irqsave(&ha->hardware_lock, flags);
 	tgt->tgt_stop = 1;
 	qlt_clear_tgt_db(tgt, true);
 	spin_unlock_irqrestore(&ha->hardware_lock, flags);
-	mutex_unlock(&ha->tgt.tgt_mutex);
+	mutex_unlock(&vha->vha_tgt.tgt_mutex);
 
 	flush_delayed_work(&tgt->sess_del_work);
 
@@ -845,20 +848,21 @@ EXPORT_SYMBOL(qlt_stop_phase1);
 void qlt_stop_phase2(struct qla_tgt *tgt)
 {
 	struct qla_hw_data *ha = tgt->ha;
+	scsi_qla_host_t *vha = pci_get_drvdata(ha->pdev);
 	unsigned long flags;
 
 	if (tgt->tgt_stopped) {
-		ql_dbg(ql_dbg_tgt_mgt, tgt->vha, 0xf04f,
+		ql_dbg(ql_dbg_tgt_mgt, vha, 0xf04f,
 		    "Already in tgt->tgt_stopped state\n");
 		dump_stack();
 		return;
 	}
 
-	ql_dbg(ql_dbg_tgt_mgt, tgt->vha, 0xf00b,
+	ql_dbg(ql_dbg_tgt_mgt, vha, 0xf00b,
 	    "Waiting for %d IRQ commands to complete (tgt %p)",
 	    tgt->irq_cmd_count, tgt);
 
-	mutex_lock(&ha->tgt.tgt_mutex);
+	mutex_lock(&vha->vha_tgt.tgt_mutex);
 	spin_lock_irqsave(&ha->hardware_lock, flags);
 	while (tgt->irq_cmd_count != 0) {
 		spin_unlock_irqrestore(&ha->hardware_lock, flags);
@@ -868,9 +872,9 @@ void qlt_stop_phase2(struct qla_tgt *tgt)
 	tgt->tgt_stop = 0;
 	tgt->tgt_stopped = 1;
 	spin_unlock_irqrestore(&ha->hardware_lock, flags);
-	mutex_unlock(&ha->tgt.tgt_mutex);
+	mutex_unlock(&vha->vha_tgt.tgt_mutex);
 
-	ql_dbg(ql_dbg_tgt_mgt, tgt->vha, 0xf00c, "Stop of tgt %p finished",
+	ql_dbg(ql_dbg_tgt_mgt, vha, 0xf00c, "Stop of tgt %p finished",
 	    tgt);
 }
 EXPORT_SYMBOL(qlt_stop_phase2);
@@ -878,14 +882,14 @@ EXPORT_SYMBOL(qlt_stop_phase2);
 /* Called from qlt_remove_target() -> qla2x00_remove_one() */
 static void qlt_release(struct qla_tgt *tgt)
 {
-	struct qla_hw_data *ha = tgt->ha;
+	scsi_qla_host_t *vha = tgt->vha;
 
-	if ((ha->tgt.qla_tgt != NULL) && !tgt->tgt_stopped)
+	if ((vha->vha_tgt.qla_tgt != NULL) && !tgt->tgt_stopped)
 		qlt_stop_phase2(tgt);
 
-	ha->tgt.qla_tgt = NULL;
+	vha->vha_tgt.qla_tgt = NULL;
 
-	ql_dbg(ql_dbg_tgt_mgt, tgt->vha, 0xf00d,
+	ql_dbg(ql_dbg_tgt_mgt, vha, 0xf00d,
 	    "Release of tgt %p finished\n", tgt);
 
 	kfree(tgt);
@@ -949,8 +953,8 @@ static void qlt_send_notify_ack(struct scsi_qla_host *vha,
 		return;
 	}
 
-	if (ha->tgt.qla_tgt != NULL)
-		ha->tgt.qla_tgt->notify_ack_expected++;
+	if (vha->vha_tgt.qla_tgt != NULL)
+		vha->vha_tgt.qla_tgt->notify_ack_expected++;
 
 	pkt->entry_type = NOTIFY_ACK_TYPE;
 	pkt->entry_count = 1;
@@ -1054,7 +1058,7 @@ static void qlt_24xx_send_abts_resp(struct scsi_qla_host *vha,
 		/* Other bytes are zero */
 	}
 
-	ha->tgt.qla_tgt->abts_resp_expected++;
+	vha->vha_tgt.qla_tgt->abts_resp_expected++;
 
 	qla2x00_start_iocbs(vha, vha->req);
 }
@@ -1206,7 +1210,7 @@ static void qlt_24xx_handle_abts(struct scsi_qla_host *vha,
 		ql_dbg(ql_dbg_tgt_mgt, vha, 0xf012,
 		    "qla_target(%d): task abort for non-existant session\n",
 		    vha->vp_idx);
-		rc = qlt_sched_sess_work(ha->tgt.qla_tgt,
+		rc = qlt_sched_sess_work(vha->vha_tgt.qla_tgt,
 		    QLA_TGT_SESS_WORK_ABORT, abts, sizeof(*abts));
 		if (rc != 0) {
 			qlt_24xx_send_abts_resp(vha, abts, FCP_TMF_REJECTED,
@@ -2157,8 +2161,7 @@ static int qlt_prepare_srr_ctio(struct scsi_qla_host *vha,
 	struct qla_tgt_cmd *cmd, void *ctio)
 {
 	struct qla_tgt_srr_ctio *sc;
-	struct qla_hw_data *ha = vha->hw;
-	struct qla_tgt *tgt = ha->tgt.qla_tgt;
+	struct qla_tgt *tgt = vha->vha_tgt.qla_tgt;
 	struct qla_tgt_srr_imm *imm;
 
 	tgt->ctio_srr_id++;
@@ -2474,7 +2477,7 @@ static void qlt_do_work(struct work_struct *work)
 	struct qla_tgt_cmd *cmd = container_of(work, struct qla_tgt_cmd, work);
 	scsi_qla_host_t *vha = cmd->vha;
 	struct qla_hw_data *ha = vha->hw;
-	struct qla_tgt *tgt = ha->tgt.qla_tgt;
+	struct qla_tgt *tgt = vha->vha_tgt.qla_tgt;
 	struct qla_tgt_sess *sess = NULL;
 	struct atio_from_isp *atio = &cmd->atio;
 	unsigned char *cdb;
@@ -2507,10 +2510,10 @@ static void qlt_do_work(struct work_struct *work)
 			goto out_term;
 		}
 
-		mutex_lock(&ha->tgt.tgt_mutex);
+		mutex_lock(&vha->vha_tgt.tgt_mutex);
 		sess = qlt_make_local_sess(vha, s_id);
 		/* sess has an extra creation ref. */
-		mutex_unlock(&ha->tgt.tgt_mutex);
+		mutex_unlock(&vha->vha_tgt.tgt_mutex);
 
 		if (!sess)
 			goto out_term;
@@ -2576,8 +2579,7 @@ out_term:
 static int qlt_handle_cmd_for_atio(struct scsi_qla_host *vha,
 	struct atio_from_isp *atio)
 {
-	struct qla_hw_data *ha = vha->hw;
-	struct qla_tgt *tgt = ha->tgt.qla_tgt;
+	struct qla_tgt *tgt = vha->vha_tgt.qla_tgt;
 	struct qla_tgt_cmd *cmd;
 
 	if (unlikely(tgt->tgt_stop)) {
@@ -2597,7 +2599,7 @@ static int qlt_handle_cmd_for_atio(struct scsi_qla_host *vha,
 
 	memcpy(&cmd->atio, atio, sizeof(*atio));
 	cmd->state = QLA_TGT_STATE_NEW;
-	cmd->tgt = ha->tgt.qla_tgt;
+	cmd->tgt = vha->vha_tgt.qla_tgt;
 	cmd->vha = vha;
 
 	INIT_WORK(&cmd->work, qlt_do_work);
@@ -2723,7 +2725,7 @@ static int qlt_handle_task_mgmt(struct scsi_qla_host *vha, void *iocb)
 	uint32_t lun, unpacked_lun;
 	int lun_size, fn;
 
-	tgt = ha->tgt.qla_tgt;
+	tgt = vha->vha_tgt.qla_tgt;
 
 	lun = a->u.isp24.fcp_cmnd.lun;
 	lun_size = sizeof(a->u.isp24.fcp_cmnd.lun);
@@ -2797,7 +2799,7 @@ static int qlt_abort_task(struct scsi_qla_host *vha,
 		ql_dbg(ql_dbg_tgt_mgt, vha, 0xf025,
 		    "qla_target(%d): task abort for unexisting "
 		    "session\n", vha->vp_idx);
-		return qlt_sched_sess_work(ha->tgt.qla_tgt,
+		return qlt_sched_sess_work(vha->vha_tgt.qla_tgt,
 		    QLA_TGT_SESS_WORK_ABORT, iocb, sizeof(*iocb));
 	}
 
@@ -2810,7 +2812,6 @@ static int qlt_abort_task(struct scsi_qla_host *vha,
 static int qlt_24xx_handle_els(struct scsi_qla_host *vha,
 	struct imm_ntfy_from_isp *iocb)
 {
-	struct qla_hw_data *ha = vha->hw;
 	int res = 0;
 
 	ql_dbg(ql_dbg_tgt_mgt, vha, 0xf026,
@@ -2828,7 +2829,7 @@ static int qlt_24xx_handle_els(struct scsi_qla_host *vha,
 	case ELS_PDISC:
 	case ELS_ADISC:
 	{
-		struct qla_tgt *tgt = ha->tgt.qla_tgt;
+		struct qla_tgt *tgt = vha->vha_tgt.qla_tgt;
 		if (tgt->link_reinit_iocb_pending) {
 			qlt_send_notify_ack(vha, &tgt->link_reinit_iocb,
 			    0, 0, 0, 0, 0, 0);
@@ -3202,8 +3203,7 @@ static void qlt_prepare_srr_imm(struct scsi_qla_host *vha,
 	struct imm_ntfy_from_isp *iocb)
 {
 	struct qla_tgt_srr_imm *imm;
-	struct qla_hw_data *ha = vha->hw;
-	struct qla_tgt *tgt = ha->tgt.qla_tgt;
+	struct qla_tgt *tgt = vha->vha_tgt.qla_tgt;
 	struct qla_tgt_srr_ctio *sctio;
 
 	tgt->imm_srr_id++;
@@ -3313,7 +3313,7 @@ static void qlt_handle_imm_notify(struct scsi_qla_host *vha,
 
 	case IMM_NTFY_LIP_LINK_REINIT:
 	{
-		struct qla_tgt *tgt = ha->tgt.qla_tgt;
+		struct qla_tgt *tgt = vha->vha_tgt.qla_tgt;
 		ql_dbg(ql_dbg_tgt_mgt, vha, 0xf033,
 		    "qla_target(%d): LINK REINIT (loop %#x, "
 		    "subcode %x)\n", vha->vp_idx,
@@ -3489,7 +3489,7 @@ static void qlt_24xx_atio_pkt(struct scsi_qla_host *vha,
 	struct atio_from_isp *atio)
 {
 	struct qla_hw_data *ha = vha->hw;
-	struct qla_tgt *tgt = ha->tgt.qla_tgt;
+	struct qla_tgt *tgt = vha->vha_tgt.qla_tgt;
 	int rc;
 
 	if (unlikely(tgt == NULL)) {
@@ -3591,7 +3591,7 @@ static void qlt_24xx_atio_pkt(struct scsi_qla_host *vha,
 static void qlt_response_pkt(struct scsi_qla_host *vha, response_t *pkt)
 {
 	struct qla_hw_data *ha = vha->hw;
-	struct qla_tgt *tgt = ha->tgt.qla_tgt;
+	struct qla_tgt *tgt = vha->vha_tgt.qla_tgt;
 
 	if (unlikely(tgt == NULL)) {
 		ql_dbg(ql_dbg_tgt, vha, 0xe05d,
@@ -3794,7 +3794,7 @@ void qlt_async_event(uint16_t code, struct scsi_qla_host *vha,
 	uint16_t *mailbox)
 {
 	struct qla_hw_data *ha = vha->hw;
-	struct qla_tgt *tgt = ha->tgt.qla_tgt;
+	struct qla_tgt *tgt = vha->vha_tgt.qla_tgt;
 	int login_code;
 
 	ql_dbg(ql_dbg_tgt, vha, 0xe039,
@@ -3924,14 +3924,14 @@ static fc_port_t *qlt_get_port_database(struct scsi_qla_host *vha,
 static struct qla_tgt_sess *qlt_make_local_sess(struct scsi_qla_host *vha,
 	uint8_t *s_id)
 {
-	struct qla_hw_data *ha = vha->hw;
 	struct qla_tgt_sess *sess = NULL;
 	fc_port_t *fcport = NULL;
 	int rc, global_resets;
 	uint16_t loop_id = 0;
 
 retry:
-	global_resets = atomic_read(&ha->tgt.qla_tgt->tgt_global_resets_count);
+	global_resets =
+	    atomic_read(&vha->vha_tgt.qla_tgt->tgt_global_resets_count);
 
 	rc = qla24xx_get_loop_id(vha, s_id, &loop_id);
 	if (rc != 0) {
@@ -3958,12 +3958,13 @@ retry:
 		return NULL;
 
 	if (global_resets !=
-	    atomic_read(&ha->tgt.qla_tgt->tgt_global_resets_count)) {
+	    atomic_read(&vha->vha_tgt.qla_tgt->tgt_global_resets_count)) {
 		ql_dbg(ql_dbg_tgt_mgt, vha, 0xf043,
 		    "qla_target(%d): global reset during session discovery "
 		    "(counter was %d, new %d), retrying", vha->vp_idx,
 		    global_resets,
-		    atomic_read(&ha->tgt.qla_tgt->tgt_global_resets_count));
+		    atomic_read(&vha->vha_tgt.
+			qla_tgt->tgt_global_resets_count));
 		goto retry;
 	}
 
@@ -3998,10 +3999,10 @@ static void qlt_abort_work(struct qla_tgt *tgt,
 	if (!sess) {
 		spin_unlock_irqrestore(&ha->hardware_lock, flags);
 
-		mutex_lock(&ha->tgt.tgt_mutex);
+		mutex_lock(&vha->vha_tgt.tgt_mutex);
 		sess = qlt_make_local_sess(vha, s_id);
 		/* sess has got an extra creation ref */
-		mutex_unlock(&ha->tgt.tgt_mutex);
+		mutex_unlock(&vha->vha_tgt.tgt_mutex);
 
 		spin_lock_irqsave(&ha->hardware_lock, flags);
 		if (!sess)
@@ -4052,10 +4053,10 @@ static void qlt_tmr_work(struct qla_tgt *tgt,
 	if (!sess) {
 		spin_unlock_irqrestore(&ha->hardware_lock, flags);
 
-		mutex_lock(&ha->tgt.tgt_mutex);
+		mutex_lock(&vha->vha_tgt.tgt_mutex);
 		sess = qlt_make_local_sess(vha, s_id);
 		/* sess has got an extra creation ref */
-		mutex_unlock(&ha->tgt.tgt_mutex);
+		mutex_unlock(&vha->vha_tgt.tgt_mutex);
 
 		spin_lock_irqsave(&ha->hardware_lock, flags);
 		if (!sess)
@@ -4141,9 +4142,9 @@ int qlt_add_target(struct qla_hw_data *ha, struct scsi_qla_host *base_vha)
 	}
 
 	ql_dbg(ql_dbg_tgt, base_vha, 0xe03b,
-	    "Registering target for host %ld(%p)", base_vha->host_no, ha);
+	    "Registering target for host %ld(%p).\n", base_vha->host_no, ha);
 
-	BUG_ON((ha->tgt.qla_tgt != NULL) || (ha->tgt.tgt_ops != NULL));
+	BUG_ON(base_vha->vha_tgt.qla_tgt != NULL);
 
 	tgt = kzalloc(sizeof(struct qla_tgt), GFP_KERNEL);
 	if (!tgt) {
@@ -4171,7 +4172,7 @@ int qlt_add_target(struct qla_hw_data *ha, struct scsi_qla_host *base_vha)
 	INIT_WORK(&tgt->srr_work, qlt_handle_srr_work);
 	atomic_set(&tgt->tgt_global_resets_count, 0);
 
-	ha->tgt.qla_tgt = tgt;
+	base_vha->vha_tgt.qla_tgt = tgt;
 
 	ql_dbg(ql_dbg_tgt, base_vha, 0xe067,
 		"qla_target(%d): using 64 Bit PCI addressing",
@@ -4192,16 +4193,16 @@ int qlt_add_target(struct qla_hw_data *ha, struct scsi_qla_host *base_vha)
 /* Must be called under tgt_host_action_mutex */
 int qlt_remove_target(struct qla_hw_data *ha, struct scsi_qla_host *vha)
 {
-	if (!ha->tgt.qla_tgt)
+	if (!vha->vha_tgt.qla_tgt)
 		return 0;
 
 	mutex_lock(&qla_tgt_mutex);
-	list_del(&ha->tgt.qla_tgt->tgt_list_entry);
+	list_del(&vha->vha_tgt.qla_tgt->tgt_list_entry);
 	mutex_unlock(&qla_tgt_mutex);
 
 	ql_dbg(ql_dbg_tgt, vha, 0xe03c, "Unregistering target for host %ld(%p)",
 	    vha->host_no, ha);
-	qlt_release(ha->tgt.qla_tgt);
+	qlt_release(vha->vha_tgt.qla_tgt);
 
 	return 0;
 }
@@ -4235,8 +4236,9 @@ static void qlt_lport_dump(struct scsi_qla_host *vha, u64 wwpn,
  * @callback:  lport initialization callback for tcm_qla2xxx code
  * @target_lport_ptr: pointer for tcm_qla2xxx specific lport data
  */
-int qlt_lport_register(struct qla_tgt_func_tmpl *qla_tgt_ops, u64 wwpn,
-	int (*callback)(struct scsi_qla_host *), void *target_lport_ptr)
+int qlt_lport_register(void *target_lport_ptr, u64 phys_wwpn,
+		       u64 npiv_wwpn, u64 npiv_wwnn,
+		       int (*callback)(struct scsi_qla_host *, void *, u64, u64))
 {
 	struct qla_tgt *tgt;
 	struct scsi_qla_host *vha;
@@ -4255,14 +4257,11 @@ int qlt_lport_register(struct qla_tgt_func_tmpl *qla_tgt_ops, u64 wwpn,
 		if (!host)
 			continue;
 
-		if (ha->tgt.tgt_ops != NULL)
-			continue;
-
 		if (!(host->hostt->supported_mode & MODE_TARGET))
 			continue;
 
 		spin_lock_irqsave(&ha->hardware_lock, flags);
-		if (host->active_mode & MODE_TARGET) {
+		if ((!npiv_wwpn || !npiv_wwnn) && host->active_mode & MODE_TARGET) {
 			pr_debug("MODE_TARGET already active on qla2xxx(%d)\n",
 			    host->host_no);
 			spin_unlock_irqrestore(&ha->hardware_lock, flags);
@@ -4276,24 +4275,18 @@ int qlt_lport_register(struct qla_tgt_func_tmpl *qla_tgt_ops, u64 wwpn,
 			    " qla2xxx scsi_host\n");
 			continue;
 		}
-		qlt_lport_dump(vha, wwpn, b);
+		qlt_lport_dump(vha, phys_wwpn, b);
 
 		if (memcmp(vha->port_name, b, WWN_SIZE)) {
 			scsi_host_put(host);
 			continue;
 		}
-		/*
-		 * Setup passed parameters ahead of invoking callback
-		 */
-		ha->tgt.tgt_ops = qla_tgt_ops;
-		ha->tgt.target_lport_ptr = target_lport_ptr;
-		rc = (*callback)(vha);
-		if (rc != 0) {
-			ha->tgt.tgt_ops = NULL;
-			ha->tgt.target_lport_ptr = NULL;
-			scsi_host_put(host);
-		}
 		mutex_unlock(&qla_tgt_mutex);
+
+		rc = (*callback)(vha, target_lport_ptr, npiv_wwpn, npiv_wwnn);
+		if (rc != 0)
+			scsi_host_put(host);
+
 		return rc;
 	}
 	mutex_unlock(&qla_tgt_mutex);
@@ -4314,7 +4307,7 @@ void qlt_lport_deregister(struct scsi_qla_host *vha)
 	/*
 	 * Clear the target_lport_ptr qla_target_template pointer in qla_hw_data
 	 */
-	ha->tgt.target_lport_ptr = NULL;
+	vha->vha_tgt.target_lport_ptr = NULL;
 	ha->tgt.tgt_ops = NULL;
 	/*
 	 * Release the Scsi_Host reference for the underlying qla2xxx host
@@ -4376,8 +4369,9 @@ void
 qlt_enable_vha(struct scsi_qla_host *vha)
 {
 	struct qla_hw_data *ha = vha->hw;
-	struct qla_tgt *tgt = ha->tgt.qla_tgt;
+	struct qla_tgt *tgt = vha->vha_tgt.qla_tgt;
 	unsigned long flags;
+	scsi_qla_host_t *base_vha = pci_get_drvdata(ha->pdev);
 
 	if (!tgt) {
 		ql_dbg(ql_dbg_tgt, vha, 0xe069,
@@ -4392,9 +4386,14 @@ qlt_enable_vha(struct scsi_qla_host *vha)
 	qlt_set_mode(vha);
 	spin_unlock_irqrestore(&ha->hardware_lock, flags);
 
-	set_bit(ISP_ABORT_NEEDED, &vha->dpc_flags);
-	qla2xxx_wake_dpc(vha);
-	qla2x00_wait_for_hba_online(vha);
+	if (vha->vp_idx) {
+		qla24xx_disable_vp(vha);
+		qla24xx_enable_vp(vha);
+	} else {
+		set_bit(ISP_ABORT_NEEDED, &base_vha->dpc_flags);
+		qla2xxx_wake_dpc(base_vha);
+		qla2x00_wait_for_hba_online(base_vha);
+	}
 }
 EXPORT_SYMBOL(qlt_enable_vha);
 
@@ -4407,7 +4406,7 @@ void
 qlt_disable_vha(struct scsi_qla_host *vha)
 {
 	struct qla_hw_data *ha = vha->hw;
-	struct qla_tgt *tgt = ha->tgt.qla_tgt;
+	struct qla_tgt *tgt = vha->vha_tgt.qla_tgt;
 	unsigned long flags;
 
 	if (!tgt) {
@@ -4438,8 +4437,10 @@ qlt_vport_create(struct scsi_qla_host *vha, struct qla_hw_data *ha)
 	if (!qla_tgt_mode_enabled(vha))
 		return;
 
-	mutex_init(&ha->tgt.tgt_mutex);
-	mutex_init(&ha->tgt.tgt_host_action_mutex);
+	vha->vha_tgt.qla_tgt = NULL;
+
+	mutex_init(&vha->vha_tgt.tgt_mutex);
+	mutex_init(&vha->vha_tgt.tgt_host_action_mutex);
 
 	qlt_clear_mode(vha);
 
@@ -4450,6 +4451,8 @@ qlt_vport_create(struct scsi_qla_host *vha, struct qla_hw_data *ha)
 	 * assigning the value appropriately.
 	 */
 	ha->tgt.atio_q_length = ATIO_ENTRY_CNT_24XX;
+
+	qlt_add_target(ha, vha);
 }
 
 void
@@ -4768,8 +4771,8 @@ qlt_probe_one_stage1(struct scsi_qla_host *base_vha, struct qla_hw_data *ha)
 		ISP_ATIO_Q_OUT(base_vha) = &ha->iobase->isp24.atio_q_out;
 	}
 
-	mutex_init(&ha->tgt.tgt_mutex);
-	mutex_init(&ha->tgt.tgt_host_action_mutex);
+	mutex_init(&base_vha->vha_tgt.tgt_mutex);
+	mutex_init(&base_vha->vha_tgt.tgt_host_action_mutex);
 	qlt_clear_mode(base_vha);
 }
 
diff --git a/drivers/scsi/qla2xxx/qla_target.h b/drivers/scsi/qla2xxx/qla_target.h
index b33e411f28a0..1d10eecad499 100644
--- a/drivers/scsi/qla2xxx/qla_target.h
+++ b/drivers/scsi/qla2xxx/qla_target.h
@@ -932,8 +932,8 @@ void qlt_disable_vha(struct scsi_qla_host *);
  */
 extern int qlt_add_target(struct qla_hw_data *, struct scsi_qla_host *);
 extern int qlt_remove_target(struct qla_hw_data *, struct scsi_qla_host *);
-extern int qlt_lport_register(struct qla_tgt_func_tmpl *, u64,
-			int (*callback)(struct scsi_qla_host *), void *);
+extern int qlt_lport_register(void *, u64, u64, u64,
+			int (*callback)(struct scsi_qla_host *, void *, u64, u64));
 extern void qlt_lport_deregister(struct scsi_qla_host *);
 extern void qlt_unreg_sess(struct qla_tgt_sess *);
 extern void qlt_fc_port_added(struct scsi_qla_host *, fc_port_t *);
diff --git a/drivers/scsi/qla2xxx/tcm_qla2xxx.c b/drivers/scsi/qla2xxx/tcm_qla2xxx.c
index 7eb19be35d46..75a141bbe74d 100644
--- a/drivers/scsi/qla2xxx/tcm_qla2xxx.c
+++ b/drivers/scsi/qla2xxx/tcm_qla2xxx.c
@@ -53,16 +53,6 @@
 struct workqueue_struct *tcm_qla2xxx_free_wq;
 struct workqueue_struct *tcm_qla2xxx_cmd_wq;
 
-static int tcm_qla2xxx_check_true(struct se_portal_group *se_tpg)
-{
-	return 1;
-}
-
-static int tcm_qla2xxx_check_false(struct se_portal_group *se_tpg)
-{
-	return 0;
-}
-
 /*
  * Parse WWN.
  * If strict, we require lower-case hex and colon separators to be sure
@@ -174,7 +164,7 @@ static int tcm_qla2xxx_npiv_parse_wwn(
 	*wwnn = 0;
 
 	/* count may include a LF at end of string */
-	if (name[cnt-1] == '\n')
+	if (name[cnt-1] == '\n' || name[cnt-1] == 0)
 		cnt--;
 
 	/* validate we have enough characters for WWPN */
@@ -777,6 +767,9 @@ static void tcm_qla2xxx_put_session(struct se_session *se_sess)
 
 static void tcm_qla2xxx_put_sess(struct qla_tgt_sess *sess)
 {
+	if (!sess)
+		return;
+
 	assert_spin_locked(&sess->vha->hw->hardware_lock);
 	kref_put(&sess->se_sess->sess_kref, tcm_qla2xxx_release_session);
 }
@@ -957,7 +950,6 @@ static ssize_t tcm_qla2xxx_tpg_store_enable(
 	struct tcm_qla2xxx_lport *lport = container_of(se_wwn,
 			struct tcm_qla2xxx_lport, lport_wwn);
 	struct scsi_qla_host *vha = lport->qla_vha;
-	struct qla_hw_data *ha = vha->hw;
 	struct tcm_qla2xxx_tpg *tpg = container_of(se_tpg,
 			struct tcm_qla2xxx_tpg, se_tpg);
 	unsigned long op;
@@ -977,12 +969,12 @@ static ssize_t tcm_qla2xxx_tpg_store_enable(
 		atomic_set(&tpg->lport_tpg_enabled, 1);
 		qlt_enable_vha(vha);
 	} else {
-		if (!ha->tgt.qla_tgt) {
-			pr_err("truct qla_hw_data *ha->tgt.qla_tgt is NULL\n");
+		if (!vha->vha_tgt.qla_tgt) {
+			pr_err("struct qla_hw_data *vha->vha_tgt.qla_tgt is NULL\n");
 			return -ENODEV;
 		}
 		atomic_set(&tpg->lport_tpg_enabled, 0);
-		qlt_stop_phase1(ha->tgt.qla_tgt);
+		qlt_stop_phase1(vha->vha_tgt.qla_tgt);
 	}
 
 	return count;
@@ -1011,7 +1003,7 @@ static struct se_portal_group *tcm_qla2xxx_make_tpg(
 	if (kstrtoul(name + 5, 10, &tpgt) || tpgt > USHRT_MAX)
 		return ERR_PTR(-EINVAL);
 
-	if (!lport->qla_npiv_vp && (tpgt != 1)) {
+	if ((tpgt != 1)) {
 		pr_err("In non NPIV mode, a single TPG=1 is used for HW port mappings\n");
 		return ERR_PTR(-ENOSYS);
 	}
@@ -1038,11 +1030,8 @@ static struct se_portal_group *tcm_qla2xxx_make_tpg(
 		kfree(tpg);
 		return NULL;
 	}
-	/*
-	 * Setup local TPG=1 pointer for non NPIV mode.
-	 */
-	if (lport->qla_npiv_vp == NULL)
-		lport->tpg_1 = tpg;
+
+	lport->tpg_1 = tpg;
 
 	return &tpg->se_tpg;
 }
@@ -1053,19 +1042,17 @@ static void tcm_qla2xxx_drop_tpg(struct se_portal_group *se_tpg)
 			struct tcm_qla2xxx_tpg, se_tpg);
 	struct tcm_qla2xxx_lport *lport = tpg->lport;
 	struct scsi_qla_host *vha = lport->qla_vha;
-	struct qla_hw_data *ha = vha->hw;
 	/*
 	 * Call into qla2x_target.c LLD logic to shutdown the active
 	 * FC Nexuses and disable target mode operation for this qla_hw_data
 	 */
-	if (ha->tgt.qla_tgt && !ha->tgt.qla_tgt->tgt_stop)
-		qlt_stop_phase1(ha->tgt.qla_tgt);
+	if (vha->vha_tgt.qla_tgt && !vha->vha_tgt.qla_tgt->tgt_stop)
+		qlt_stop_phase1(vha->vha_tgt.qla_tgt);
 
 	core_tpg_deregister(se_tpg);
 	/*
 	 * Clear local TPG=1 pointer for non NPIV mode.
 	 */
-	if (lport->qla_npiv_vp == NULL)
 		lport->tpg_1 = NULL;
 
 	kfree(tpg);
@@ -1095,12 +1082,22 @@ static struct se_portal_group *tcm_qla2xxx_npiv_make_tpg(
 	tpg->lport = lport;
 	tpg->lport_tpgt = tpgt;
 
+	/*
+	 * By default allow READ-ONLY TPG demo-mode access w/ cached dynamic
+	 * NodeACLs
+	 */
+	tpg->tpg_attrib.generate_node_acls = 1;
+	tpg->tpg_attrib.demo_mode_write_protect = 1;
+	tpg->tpg_attrib.cache_dynamic_acls = 1;
+	tpg->tpg_attrib.demo_mode_login_only = 1;
+
 	ret = core_tpg_register(&tcm_qla2xxx_npiv_fabric_configfs->tf_ops, wwn,
 				&tpg->se_tpg, tpg, TRANSPORT_TPG_TYPE_NORMAL);
 	if (ret < 0) {
 		kfree(tpg);
 		return NULL;
 	}
+	lport->tpg_1 = tpg;
 	return &tpg->se_tpg;
 }
 
@@ -1111,13 +1108,12 @@ static struct qla_tgt_sess *tcm_qla2xxx_find_sess_by_s_id(
 	scsi_qla_host_t *vha,
 	const uint8_t *s_id)
 {
-	struct qla_hw_data *ha = vha->hw;
 	struct tcm_qla2xxx_lport *lport;
 	struct se_node_acl *se_nacl;
 	struct tcm_qla2xxx_nacl *nacl;
 	u32 key;
 
-	lport = ha->tgt.target_lport_ptr;
+	lport = vha->vha_tgt.target_lport_ptr;
 	if (!lport) {
 		pr_err("Unable to locate struct tcm_qla2xxx_lport\n");
 		dump_stack();
@@ -1221,13 +1217,12 @@ static struct qla_tgt_sess *tcm_qla2xxx_find_sess_by_loop_id(
 	scsi_qla_host_t *vha,
 	const uint16_t loop_id)
 {
-	struct qla_hw_data *ha = vha->hw;
 	struct tcm_qla2xxx_lport *lport;
 	struct se_node_acl *se_nacl;
 	struct tcm_qla2xxx_nacl *nacl;
 	struct tcm_qla2xxx_fc_loopid *fc_loopid;
 
-	lport = ha->tgt.target_lport_ptr;
+	lport = vha->vha_tgt.target_lport_ptr;
 	if (!lport) {
 		pr_err("Unable to locate struct tcm_qla2xxx_lport\n");
 		dump_stack();
@@ -1341,6 +1336,7 @@ static void tcm_qla2xxx_free_session(struct qla_tgt_sess *sess)
 {
 	struct qla_tgt *tgt = sess->tgt;
 	struct qla_hw_data *ha = tgt->ha;
+	scsi_qla_host_t *vha = pci_get_drvdata(ha->pdev);
 	struct se_session *se_sess;
 	struct se_node_acl *se_nacl;
 	struct tcm_qla2xxx_lport *lport;
@@ -1357,7 +1353,7 @@ static void tcm_qla2xxx_free_session(struct qla_tgt_sess *sess)
 	se_nacl = se_sess->se_node_acl;
 	nacl = container_of(se_nacl, struct tcm_qla2xxx_nacl, se_node_acl);
 
-	lport = ha->tgt.target_lport_ptr;
+	lport = vha->vha_tgt.target_lport_ptr;
 	if (!lport) {
 		pr_err("Unable to locate struct tcm_qla2xxx_lport\n");
 		dump_stack();
@@ -1391,7 +1387,7 @@ static int tcm_qla2xxx_check_initiator_node_acl(
 	unsigned char port_name[36];
 	unsigned long flags;
 
-	lport = ha->tgt.target_lport_ptr;
+	lport = vha->vha_tgt.target_lport_ptr;
 	if (!lport) {
 		pr_err("Unable to locate struct tcm_qla2xxx_lport\n");
 		dump_stack();
@@ -1455,7 +1451,8 @@ static void tcm_qla2xxx_update_sess(struct qla_tgt_sess *sess, port_id_t s_id,
 {
 	struct qla_tgt *tgt = sess->tgt;
 	struct qla_hw_data *ha = tgt->ha;
-	struct tcm_qla2xxx_lport *lport = ha->tgt.target_lport_ptr;
+	scsi_qla_host_t *vha = pci_get_drvdata(ha->pdev);
+	struct tcm_qla2xxx_lport *lport = vha->vha_tgt.target_lport_ptr;
 	struct se_node_acl *se_nacl = sess->se_sess->se_node_acl;
 	struct tcm_qla2xxx_nacl *nacl = container_of(se_nacl,
 			struct tcm_qla2xxx_nacl, se_node_acl);
@@ -1562,15 +1559,18 @@ static int tcm_qla2xxx_init_lport(struct tcm_qla2xxx_lport *lport)
 	return 0;
 }
 
-static int tcm_qla2xxx_lport_register_cb(struct scsi_qla_host *vha)
+static int tcm_qla2xxx_lport_register_cb(struct scsi_qla_host *vha,
+					 void *target_lport_ptr,
+					 u64 npiv_wwpn, u64 npiv_wwnn)
 {
 	struct qla_hw_data *ha = vha->hw;
-	struct tcm_qla2xxx_lport *lport;
+	struct tcm_qla2xxx_lport *lport =
+			(struct tcm_qla2xxx_lport *)target_lport_ptr;
 	/*
-	 * Setup local pointer to vha, NPIV VP pointer (if present) and
-	 * vha->tcm_lport pointer
+	 * Setup tgt_ops, local pointer to vha and target_lport_ptr
 	 */
-	lport = (struct tcm_qla2xxx_lport *)ha->tgt.target_lport_ptr;
+	ha->tgt.tgt_ops = &tcm_qla2xxx_template;
+	vha->vha_tgt.target_lport_ptr = target_lport_ptr;
 	lport->qla_vha = vha;
 
 	return 0;
@@ -1602,8 +1602,8 @@ static struct se_wwn *tcm_qla2xxx_make_lport(
 	if (ret != 0)
 		goto out;
 
-	ret = qlt_lport_register(&tcm_qla2xxx_template, wwpn,
-				tcm_qla2xxx_lport_register_cb, lport);
+	ret = qlt_lport_register(lport, wwpn, 0, 0,
+				 tcm_qla2xxx_lport_register_cb);
 	if (ret != 0)
 		goto out_lport;
 
@@ -1621,7 +1621,6 @@ static void tcm_qla2xxx_drop_lport(struct se_wwn *wwn)
 	struct tcm_qla2xxx_lport *lport = container_of(wwn,
 			struct tcm_qla2xxx_lport, lport_wwn);
 	struct scsi_qla_host *vha = lport->qla_vha;
-	struct qla_hw_data *ha = vha->hw;
 	struct se_node_acl *node;
 	u32 key = 0;
 
@@ -1630,8 +1629,8 @@ static void tcm_qla2xxx_drop_lport(struct se_wwn *wwn)
 	 * shutdown of struct qla_tgt after the call to
 	 * qlt_stop_phase1() from tcm_qla2xxx_drop_tpg() above..
 	 */
-	if (ha->tgt.qla_tgt && !ha->tgt.qla_tgt->tgt_stopped)
-		qlt_stop_phase2(ha->tgt.qla_tgt);
+	if (vha->vha_tgt.qla_tgt && !vha->vha_tgt.qla_tgt->tgt_stopped)
+		qlt_stop_phase2(vha->vha_tgt.qla_tgt);
 
 	qlt_lport_deregister(vha);
 
@@ -1642,17 +1641,70 @@ static void tcm_qla2xxx_drop_lport(struct se_wwn *wwn)
 	kfree(lport);
 }
 
+static int tcm_qla2xxx_lport_register_npiv_cb(struct scsi_qla_host *base_vha,
+					      void *target_lport_ptr,
+					      u64 npiv_wwpn, u64 npiv_wwnn)
+{
+	struct fc_vport *vport;
+	struct Scsi_Host *sh = base_vha->host;
+	struct scsi_qla_host *npiv_vha;
+	struct tcm_qla2xxx_lport *lport =
+			(struct tcm_qla2xxx_lport *)target_lport_ptr;
+	struct fc_vport_identifiers vport_id;
+
+	if (!qla_tgt_mode_enabled(base_vha)) {
+		pr_err("qla2xxx base_vha not enabled for target mode\n");
+		return -EPERM;
+	}
+
+	memset(&vport_id, 0, sizeof(vport_id));
+	vport_id.port_name = npiv_wwpn;
+	vport_id.node_name = npiv_wwnn;
+	vport_id.roles = FC_PORT_ROLE_FCP_INITIATOR;
+	vport_id.vport_type = FC_PORTTYPE_NPIV;
+	vport_id.disable = false;
+
+	vport = fc_vport_create(sh, 0, &vport_id);
+	if (!vport) {
+		pr_err("fc_vport_create failed for qla2xxx_npiv\n");
+		return -ENODEV;
+	}
+	/*
+	 * Setup local pointer to NPIV vhba + target_lport_ptr
+	 */
+	npiv_vha = (struct scsi_qla_host *)vport->dd_data;
+	npiv_vha->vha_tgt.target_lport_ptr = target_lport_ptr;
+	lport->qla_vha = npiv_vha;
+
+	scsi_host_get(npiv_vha->host);
+	return 0;
+}
+
+
 static struct se_wwn *tcm_qla2xxx_npiv_make_lport(
 	struct target_fabric_configfs *tf,
 	struct config_group *group,
 	const char *name)
 {
 	struct tcm_qla2xxx_lport *lport;
-	u64 npiv_wwpn, npiv_wwnn;
+	u64 phys_wwpn, npiv_wwpn, npiv_wwnn;
+	char *p, tmp[128];
 	int ret;
 
-	if (tcm_qla2xxx_npiv_parse_wwn(name, strlen(name)+1,
-				&npiv_wwpn, &npiv_wwnn) < 0)
+	snprintf(tmp, 128, "%s", name);
+
+	p = strchr(tmp, '@');
+	if (!p) {
+		pr_err("Unable to locate NPIV '@' seperator\n");
+		return ERR_PTR(-EINVAL);
+	}
+	*p++ = '\0';
+
+	if (tcm_qla2xxx_parse_wwn(tmp, &phys_wwpn, 1) < 0)
+		return ERR_PTR(-EINVAL);
+
+	if (tcm_qla2xxx_npiv_parse_wwn(p, strlen(p)+1,
+				       &npiv_wwpn, &npiv_wwnn) < 0)
 		return ERR_PTR(-EINVAL);
 
 	lport = kzalloc(sizeof(struct tcm_qla2xxx_lport), GFP_KERNEL);
@@ -1666,12 +1718,19 @@ static struct se_wwn *tcm_qla2xxx_npiv_make_lport(
 			TCM_QLA2XXX_NAMELEN, npiv_wwpn, npiv_wwnn);
 	sprintf(lport->lport_naa_name, "naa.%016llx", (unsigned long long) npiv_wwpn);
 
-/* FIXME: tcm_qla2xxx_npiv_make_lport */
-	ret = -ENOSYS;
+	ret = tcm_qla2xxx_init_lport(lport);
 	if (ret != 0)
 		goto out;
 
+	ret = qlt_lport_register(lport, phys_wwpn, npiv_wwpn, npiv_wwnn,
+				 tcm_qla2xxx_lport_register_npiv_cb);
+	if (ret != 0)
+		goto out_lport;
+
 	return &lport->lport_wwn;
+out_lport:
+	vfree(lport->lport_loopid_map);
+	btree_destroy32(&lport->lport_fcport_map);
 out:
 	kfree(lport);
 	return ERR_PTR(ret);
@@ -1681,14 +1740,16 @@ static void tcm_qla2xxx_npiv_drop_lport(struct se_wwn *wwn)
 {
 	struct tcm_qla2xxx_lport *lport = container_of(wwn,
 			struct tcm_qla2xxx_lport, lport_wwn);
-	struct scsi_qla_host *vha = lport->qla_vha;
-	struct Scsi_Host *sh = vha->host;
+	struct scsi_qla_host *npiv_vha = lport->qla_vha;
+	struct qla_hw_data *ha = npiv_vha->hw;
+	scsi_qla_host_t *base_vha = pci_get_drvdata(ha->pdev);
+
+	scsi_host_put(npiv_vha->host);
 	/*
-	 * Notify libfc that we want to release the lport->npiv_vport
+	 * Notify libfc that we want to release the vha->fc_vport
 	 */
-	fc_vport_terminate(lport->npiv_vport);
-
-	scsi_host_put(sh);
+	fc_vport_terminate(npiv_vha->fc_vport);
+	scsi_host_put(base_vha->host);
 	kfree(lport);
 }
 
@@ -1769,14 +1830,16 @@ static struct target_core_fabric_ops tcm_qla2xxx_npiv_ops = {
 	.tpg_get_pr_transport_id	= tcm_qla2xxx_get_pr_transport_id,
 	.tpg_get_pr_transport_id_len	= tcm_qla2xxx_get_pr_transport_id_len,
 	.tpg_parse_pr_out_transport_id	= tcm_qla2xxx_parse_pr_out_transport_id,
-	.tpg_check_demo_mode		= tcm_qla2xxx_check_false,
-	.tpg_check_demo_mode_cache	= tcm_qla2xxx_check_true,
-	.tpg_check_demo_mode_write_protect = tcm_qla2xxx_check_true,
-	.tpg_check_prod_mode_write_protect = tcm_qla2xxx_check_false,
+	.tpg_check_demo_mode		= tcm_qla2xxx_check_demo_mode,
+	.tpg_check_demo_mode_cache	= tcm_qla2xxx_check_demo_mode_cache,
+	.tpg_check_demo_mode_write_protect = tcm_qla2xxx_check_demo_mode,
+	.tpg_check_prod_mode_write_protect =
+	    tcm_qla2xxx_check_prod_write_protect,
 	.tpg_check_demo_mode_login_only	= tcm_qla2xxx_check_demo_mode_login_only,
 	.tpg_alloc_fabric_acl		= tcm_qla2xxx_alloc_fabric_acl,
 	.tpg_release_fabric_acl		= tcm_qla2xxx_release_fabric_acl,
 	.tpg_get_inst_index		= tcm_qla2xxx_tpg_get_inst_index,
+	.check_stop_free                = tcm_qla2xxx_check_stop_free,
 	.release_cmd			= tcm_qla2xxx_release_cmd,
 	.put_session			= tcm_qla2xxx_put_session,
 	.shutdown_session		= tcm_qla2xxx_shutdown_session,
@@ -1871,7 +1934,8 @@ static int tcm_qla2xxx_register_configfs(void)
 	 * Setup default attribute lists for various npiv_fabric->tf_cit_tmpl
 	 */
 	npiv_fabric->tf_cit_tmpl.tfc_wwn_cit.ct_attrs = tcm_qla2xxx_wwn_attrs;
-	npiv_fabric->tf_cit_tmpl.tfc_tpg_base_cit.ct_attrs = NULL;
+	npiv_fabric->tf_cit_tmpl.tfc_tpg_base_cit.ct_attrs =
+	    tcm_qla2xxx_tpg_attrs;
 	npiv_fabric->tf_cit_tmpl.tfc_tpg_attrib_cit.ct_attrs = NULL;
 	npiv_fabric->tf_cit_tmpl.tfc_tpg_param_cit.ct_attrs = NULL;
 	npiv_fabric->tf_cit_tmpl.tfc_tpg_np_base_cit.ct_attrs = NULL;
diff --git a/drivers/scsi/qla2xxx/tcm_qla2xxx.h b/drivers/scsi/qla2xxx/tcm_qla2xxx.h
index 771f7b816443..275d8b9a7a34 100644
--- a/drivers/scsi/qla2xxx/tcm_qla2xxx.h
+++ b/drivers/scsi/qla2xxx/tcm_qla2xxx.h
@@ -70,12 +70,8 @@ struct tcm_qla2xxx_lport {
 	struct tcm_qla2xxx_fc_loopid *lport_loopid_map;
 	/* Pointer to struct scsi_qla_host from qla2xxx LLD */
 	struct scsi_qla_host *qla_vha;
-	/* Pointer to struct scsi_qla_host for NPIV VP from qla2xxx LLD */
-	struct scsi_qla_host *qla_npiv_vp;
 	/* Pointer to struct qla_tgt pointer */
 	struct qla_tgt lport_qla_tgt;
-	/* Pointer to struct fc_vport for NPIV vport from libfc */
-	struct fc_vport *npiv_vport;
 	/* Pointer to TPG=1 for non NPIV mode */
 	struct tcm_qla2xxx_tpg *tpg_1;
 	/* Returned by tcm_qla2xxx_make_lport() */
diff --git a/drivers/scsi/sd.c b/drivers/scsi/sd.c
index 9846c6ab2aaa..470954aba728 100644
--- a/drivers/scsi/sd.c
+++ b/drivers/scsi/sd.c
@@ -801,7 +801,7 @@ static int sd_setup_write_same_cmnd(struct scsi_device *sdp, struct request *rq)
 	if (sdkp->device->no_write_same)
 		return BLKPREP_KILL;
 
-	BUG_ON(bio_offset(bio) || bio_iovec(bio)->bv_len != sdp->sector_size);
+	BUG_ON(bio_offset(bio) || bio_iovec(bio).bv_len != sdp->sector_size);
 
 	sector >>= ilog2(sdp->sector_size) - 9;
 	nr_sectors >>= ilog2(sdp->sector_size) - 9;
diff --git a/drivers/scsi/sd_dif.c b/drivers/scsi/sd_dif.c
index 6174ca4ea275..a7a691d0af7d 100644
--- a/drivers/scsi/sd_dif.c
+++ b/drivers/scsi/sd_dif.c
@@ -365,7 +365,6 @@ void sd_dif_prepare(struct request *rq, sector_t hw_sector,
 	struct bio *bio;
 	struct scsi_disk *sdkp;
 	struct sd_dif_tuple *sdt;
-	unsigned int i, j;
 	u32 phys, virt;
 
 	sdkp = rq->bio->bi_bdev->bd_disk->private_data;
@@ -376,19 +375,21 @@ void sd_dif_prepare(struct request *rq, sector_t hw_sector,
 	phys = hw_sector & 0xffffffff;
 
 	__rq_for_each_bio(bio, rq) {
-		struct bio_vec *iv;
+		struct bio_vec iv;
+		struct bvec_iter iter;
+		unsigned int j;
 
 		/* Already remapped? */
 		if (bio_flagged(bio, BIO_MAPPED_INTEGRITY))
 			break;
 
-		virt = bio->bi_integrity->bip_sector & 0xffffffff;
+		virt = bio->bi_integrity->bip_iter.bi_sector & 0xffffffff;
 
-		bip_for_each_vec(iv, bio->bi_integrity, i) {
-			sdt = kmap_atomic(iv->bv_page)
-				+ iv->bv_offset;
+		bip_for_each_vec(iv, bio->bi_integrity, iter) {
+			sdt = kmap_atomic(iv.bv_page)
+				+ iv.bv_offset;
 
-			for (j = 0 ; j < iv->bv_len ; j += tuple_sz, sdt++) {
+			for (j = 0; j < iv.bv_len; j += tuple_sz, sdt++) {
 
 				if (be32_to_cpu(sdt->ref_tag) == virt)
 					sdt->ref_tag = cpu_to_be32(phys);
@@ -414,7 +415,7 @@ void sd_dif_complete(struct scsi_cmnd *scmd, unsigned int good_bytes)
 	struct scsi_disk *sdkp;
 	struct bio *bio;
 	struct sd_dif_tuple *sdt;
-	unsigned int i, j, sectors, sector_sz;
+	unsigned int j, sectors, sector_sz;
 	u32 phys, virt;
 
 	sdkp = scsi_disk(scmd->request->rq_disk);
@@ -430,15 +431,16 @@ void sd_dif_complete(struct scsi_cmnd *scmd, unsigned int good_bytes)
 		phys >>= 3;
 
 	__rq_for_each_bio(bio, scmd->request) {
-		struct bio_vec *iv;
+		struct bio_vec iv;
+		struct bvec_iter iter;
 
-		virt = bio->bi_integrity->bip_sector & 0xffffffff;
+		virt = bio->bi_integrity->bip_iter.bi_sector & 0xffffffff;
 
-		bip_for_each_vec(iv, bio->bi_integrity, i) {
-			sdt = kmap_atomic(iv->bv_page)
-				+ iv->bv_offset;
+		bip_for_each_vec(iv, bio->bi_integrity, iter) {
+			sdt = kmap_atomic(iv.bv_page)
+				+ iv.bv_offset;
 
-			for (j = 0 ; j < iv->bv_len ; j += tuple_sz, sdt++) {
+			for (j = 0; j < iv.bv_len; j += tuple_sz, sdt++) {
 
 				if (sectors == 0) {
 					kunmap_atomic(sdt);
diff --git a/drivers/spi/spi-mpc512x-psc.c b/drivers/spi/spi-mpc512x-psc.c
index 46d2313f7c6f..5032141eeeec 100644
--- a/drivers/spi/spi-mpc512x-psc.c
+++ b/drivers/spi/spi-mpc512x-psc.c
@@ -40,6 +40,7 @@ struct mpc512x_psc_spi {
 	unsigned int irq;
 	u8 bits_per_word;
 	struct clk *clk_mclk;
+	struct clk *clk_ipg;
 	u32 mclk_rate;
 
 	struct completion txisrdone;
@@ -475,8 +476,6 @@ static int mpc512x_psc_spi_do_probe(struct device *dev, u32 regaddr,
 	struct spi_master *master;
 	int ret;
 	void *tempp;
-	int psc_num;
-	char clk_name[16];
 	struct clk *clk;
 
 	master = spi_alloc_master(dev, sizeof *mps);
@@ -519,9 +518,7 @@ static int mpc512x_psc_spi_do_probe(struct device *dev, u32 regaddr,
 		goto free_master;
 	init_completion(&mps->txisrdone);
 
-	psc_num = master->bus_num;
-	snprintf(clk_name, sizeof(clk_name), "psc%d_mclk", psc_num);
-	clk = devm_clk_get(dev, clk_name);
+	clk = devm_clk_get(dev, "mclk");
 	if (IS_ERR(clk)) {
 		ret = PTR_ERR(clk);
 		goto free_master;
@@ -532,17 +529,29 @@ static int mpc512x_psc_spi_do_probe(struct device *dev, u32 regaddr,
 	mps->clk_mclk = clk;
 	mps->mclk_rate = clk_get_rate(clk);
 
+	clk = devm_clk_get(dev, "ipg");
+	if (IS_ERR(clk)) {
+		ret = PTR_ERR(clk);
+		goto free_mclk_clock;
+	}
+	ret = clk_prepare_enable(clk);
+	if (ret)
+		goto free_mclk_clock;
+	mps->clk_ipg = clk;
+
 	ret = mpc512x_psc_spi_port_config(master, mps);
 	if (ret < 0)
-		goto free_clock;
+		goto free_ipg_clock;
 
 	ret = devm_spi_register_master(dev, master);
 	if (ret < 0)
-		goto free_clock;
+		goto free_ipg_clock;
 
 	return ret;
 
-free_clock:
+free_ipg_clock:
+	clk_disable_unprepare(mps->clk_ipg);
+free_mclk_clock:
 	clk_disable_unprepare(mps->clk_mclk);
 free_master:
 	spi_master_put(master);
@@ -556,6 +565,7 @@ static int mpc512x_psc_spi_do_remove(struct device *dev)
 	struct mpc512x_psc_spi *mps = spi_master_get_devdata(master);
 
 	clk_disable_unprepare(mps->clk_mclk);
+	clk_disable_unprepare(mps->clk_ipg);
 
 	return 0;
 }
diff --git a/drivers/ssb/Kconfig b/drivers/ssb/Kconfig
index 2cd9b0e44a41..75b3603906c1 100644
--- a/drivers/ssb/Kconfig
+++ b/drivers/ssb/Kconfig
@@ -168,6 +168,7 @@ config SSB_DRIVER_GIGE
 config SSB_DRIVER_GPIO
 	bool "SSB GPIO driver"
 	depends on SSB && GPIOLIB
+	select IRQ_DOMAIN if SSB_EMBEDDED
 	help
 	  Driver to provide access to the GPIO pins on the bus.
 
diff --git a/drivers/ssb/driver_gpio.c b/drivers/ssb/driver_gpio.c
index dc109de228c6..ba350d2035c0 100644
--- a/drivers/ssb/driver_gpio.c
+++ b/drivers/ssb/driver_gpio.c
@@ -9,16 +9,40 @@
  */
 
 #include <linux/gpio.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/irqdomain.h>
 #include <linux/export.h>
 #include <linux/ssb/ssb.h>
 
 #include "ssb_private.h"
 
+
+/**************************************************
+ * Shared
+ **************************************************/
+
 static struct ssb_bus *ssb_gpio_get_bus(struct gpio_chip *chip)
 {
 	return container_of(chip, struct ssb_bus, gpio);
 }
 
+#if IS_ENABLED(CONFIG_SSB_EMBEDDED)
+static int ssb_gpio_to_irq(struct gpio_chip *chip, unsigned gpio)
+{
+	struct ssb_bus *bus = ssb_gpio_get_bus(chip);
+
+	if (bus->bustype == SSB_BUSTYPE_SSB)
+		return irq_find_mapping(bus->irq_domain, gpio);
+	else
+		return -EINVAL;
+}
+#endif
+
+/**************************************************
+ * ChipCommon
+ **************************************************/
+
 static int ssb_gpio_chipco_get_value(struct gpio_chip *chip, unsigned gpio)
 {
 	struct ssb_bus *bus = ssb_gpio_get_bus(chip);
@@ -74,19 +98,129 @@ static void ssb_gpio_chipco_free(struct gpio_chip *chip, unsigned gpio)
 	ssb_chipco_gpio_pullup(&bus->chipco, 1 << gpio, 0);
 }
 
-static int ssb_gpio_chipco_to_irq(struct gpio_chip *chip, unsigned gpio)
+#if IS_ENABLED(CONFIG_SSB_EMBEDDED)
+static void ssb_gpio_irq_chipco_mask(struct irq_data *d)
 {
-	struct ssb_bus *bus = ssb_gpio_get_bus(chip);
+	struct ssb_bus *bus = irq_data_get_irq_chip_data(d);
+	int gpio = irqd_to_hwirq(d);
 
-	if (bus->bustype == SSB_BUSTYPE_SSB)
-		return ssb_mips_irq(bus->chipco.dev) + 2;
-	else
-		return -EINVAL;
+	ssb_chipco_gpio_intmask(&bus->chipco, BIT(gpio), 0);
+}
+
+static void ssb_gpio_irq_chipco_unmask(struct irq_data *d)
+{
+	struct ssb_bus *bus = irq_data_get_irq_chip_data(d);
+	int gpio = irqd_to_hwirq(d);
+	u32 val = ssb_chipco_gpio_in(&bus->chipco, BIT(gpio));
+
+	ssb_chipco_gpio_polarity(&bus->chipco, BIT(gpio), val);
+	ssb_chipco_gpio_intmask(&bus->chipco, BIT(gpio), BIT(gpio));
+}
+
+static struct irq_chip ssb_gpio_irq_chipco_chip = {
+	.name		= "SSB-GPIO-CC",
+	.irq_mask	= ssb_gpio_irq_chipco_mask,
+	.irq_unmask	= ssb_gpio_irq_chipco_unmask,
+};
+
+static irqreturn_t ssb_gpio_irq_chipco_handler(int irq, void *dev_id)
+{
+	struct ssb_bus *bus = dev_id;
+	struct ssb_chipcommon *chipco = &bus->chipco;
+	u32 val = chipco_read32(chipco, SSB_CHIPCO_GPIOIN);
+	u32 mask = chipco_read32(chipco, SSB_CHIPCO_GPIOIRQ);
+	u32 pol = chipco_read32(chipco, SSB_CHIPCO_GPIOPOL);
+	unsigned long irqs = (val ^ pol) & mask;
+	int gpio;
+
+	if (!irqs)
+		return IRQ_NONE;
+
+	for_each_set_bit(gpio, &irqs, bus->gpio.ngpio)
+		generic_handle_irq(ssb_gpio_to_irq(&bus->gpio, gpio));
+	ssb_chipco_gpio_polarity(chipco, irqs, val & irqs);
+
+	return IRQ_HANDLED;
+}
+
+static int ssb_gpio_irq_chipco_domain_init(struct ssb_bus *bus)
+{
+	struct ssb_chipcommon *chipco = &bus->chipco;
+	struct gpio_chip *chip = &bus->gpio;
+	int gpio, hwirq, err;
+
+	if (bus->bustype != SSB_BUSTYPE_SSB)
+		return 0;
+
+	bus->irq_domain = irq_domain_add_linear(NULL, chip->ngpio,
+						&irq_domain_simple_ops, chipco);
+	if (!bus->irq_domain) {
+		err = -ENODEV;
+		goto err_irq_domain;
+	}
+	for (gpio = 0; gpio < chip->ngpio; gpio++) {
+		int irq = irq_create_mapping(bus->irq_domain, gpio);
+
+		irq_set_chip_data(irq, bus);
+		irq_set_chip_and_handler(irq, &ssb_gpio_irq_chipco_chip,
+					 handle_simple_irq);
+	}
+
+	hwirq = ssb_mips_irq(bus->chipco.dev) + 2;
+	err = request_irq(hwirq, ssb_gpio_irq_chipco_handler, IRQF_SHARED,
+			  "gpio", bus);
+	if (err)
+		goto err_req_irq;
+
+	ssb_chipco_gpio_intmask(&bus->chipco, ~0, 0);
+	chipco_set32(chipco, SSB_CHIPCO_IRQMASK, SSB_CHIPCO_IRQ_GPIO);
+
+	return 0;
+
+err_req_irq:
+	for (gpio = 0; gpio < chip->ngpio; gpio++) {
+		int irq = irq_find_mapping(bus->irq_domain, gpio);
+
+		irq_dispose_mapping(irq);
+	}
+	irq_domain_remove(bus->irq_domain);
+err_irq_domain:
+	return err;
+}
+
+static void ssb_gpio_irq_chipco_domain_exit(struct ssb_bus *bus)
+{
+	struct ssb_chipcommon *chipco = &bus->chipco;
+	struct gpio_chip *chip = &bus->gpio;
+	int gpio;
+
+	if (bus->bustype != SSB_BUSTYPE_SSB)
+		return;
+
+	chipco_mask32(chipco, SSB_CHIPCO_IRQMASK, ~SSB_CHIPCO_IRQ_GPIO);
+	free_irq(ssb_mips_irq(bus->chipco.dev) + 2, chipco);
+	for (gpio = 0; gpio < chip->ngpio; gpio++) {
+		int irq = irq_find_mapping(bus->irq_domain, gpio);
+
+		irq_dispose_mapping(irq);
+	}
+	irq_domain_remove(bus->irq_domain);
+}
+#else
+static int ssb_gpio_irq_chipco_domain_init(struct ssb_bus *bus)
+{
+	return 0;
 }
 
+static void ssb_gpio_irq_chipco_domain_exit(struct ssb_bus *bus)
+{
+}
+#endif
+
 static int ssb_gpio_chipco_init(struct ssb_bus *bus)
 {
 	struct gpio_chip *chip = &bus->gpio;
+	int err;
 
 	chip->label		= "ssb_chipco_gpio";
 	chip->owner		= THIS_MODULE;
@@ -96,7 +230,9 @@ static int ssb_gpio_chipco_init(struct ssb_bus *bus)
 	chip->set		= ssb_gpio_chipco_set_value;
 	chip->direction_input	= ssb_gpio_chipco_direction_input;
 	chip->direction_output	= ssb_gpio_chipco_direction_output;
-	chip->to_irq		= ssb_gpio_chipco_to_irq;
+#if IS_ENABLED(CONFIG_SSB_EMBEDDED)
+	chip->to_irq		= ssb_gpio_to_irq;
+#endif
 	chip->ngpio		= 16;
 	/* There is just one SoC in one device and its GPIO addresses should be
 	 * deterministic to address them more easily. The other buses could get
@@ -106,9 +242,23 @@ static int ssb_gpio_chipco_init(struct ssb_bus *bus)
 	else
 		chip->base		= -1;
 
-	return gpiochip_add(chip);
+	err = ssb_gpio_irq_chipco_domain_init(bus);
+	if (err)
+		return err;
+
+	err = gpiochip_add(chip);
+	if (err) {
+		ssb_gpio_irq_chipco_domain_exit(bus);
+		return err;
+	}
+
+	return 0;
 }
 
+/**************************************************
+ * EXTIF
+ **************************************************/
+
 #ifdef CONFIG_SSB_DRIVER_EXTIF
 
 static int ssb_gpio_extif_get_value(struct gpio_chip *chip, unsigned gpio)
@@ -145,19 +295,127 @@ static int ssb_gpio_extif_direction_output(struct gpio_chip *chip,
 	return 0;
 }
 
-static int ssb_gpio_extif_to_irq(struct gpio_chip *chip, unsigned gpio)
+#if IS_ENABLED(CONFIG_SSB_EMBEDDED)
+static void ssb_gpio_irq_extif_mask(struct irq_data *d)
 {
-	struct ssb_bus *bus = ssb_gpio_get_bus(chip);
+	struct ssb_bus *bus = irq_data_get_irq_chip_data(d);
+	int gpio = irqd_to_hwirq(d);
 
-	if (bus->bustype == SSB_BUSTYPE_SSB)
-		return ssb_mips_irq(bus->extif.dev) + 2;
-	else
-		return -EINVAL;
+	ssb_extif_gpio_intmask(&bus->extif, BIT(gpio), 0);
+}
+
+static void ssb_gpio_irq_extif_unmask(struct irq_data *d)
+{
+	struct ssb_bus *bus = irq_data_get_irq_chip_data(d);
+	int gpio = irqd_to_hwirq(d);
+	u32 val = ssb_extif_gpio_in(&bus->extif, BIT(gpio));
+
+	ssb_extif_gpio_polarity(&bus->extif, BIT(gpio), val);
+	ssb_extif_gpio_intmask(&bus->extif, BIT(gpio), BIT(gpio));
+}
+
+static struct irq_chip ssb_gpio_irq_extif_chip = {
+	.name		= "SSB-GPIO-EXTIF",
+	.irq_mask	= ssb_gpio_irq_extif_mask,
+	.irq_unmask	= ssb_gpio_irq_extif_unmask,
+};
+
+static irqreturn_t ssb_gpio_irq_extif_handler(int irq, void *dev_id)
+{
+	struct ssb_bus *bus = dev_id;
+	struct ssb_extif *extif = &bus->extif;
+	u32 val = ssb_read32(extif->dev, SSB_EXTIF_GPIO_IN);
+	u32 mask = ssb_read32(extif->dev, SSB_EXTIF_GPIO_INTMASK);
+	u32 pol = ssb_read32(extif->dev, SSB_EXTIF_GPIO_INTPOL);
+	unsigned long irqs = (val ^ pol) & mask;
+	int gpio;
+
+	if (!irqs)
+		return IRQ_NONE;
+
+	for_each_set_bit(gpio, &irqs, bus->gpio.ngpio)
+		generic_handle_irq(ssb_gpio_to_irq(&bus->gpio, gpio));
+	ssb_extif_gpio_polarity(extif, irqs, val & irqs);
+
+	return IRQ_HANDLED;
+}
+
+static int ssb_gpio_irq_extif_domain_init(struct ssb_bus *bus)
+{
+	struct ssb_extif *extif = &bus->extif;
+	struct gpio_chip *chip = &bus->gpio;
+	int gpio, hwirq, err;
+
+	if (bus->bustype != SSB_BUSTYPE_SSB)
+		return 0;
+
+	bus->irq_domain = irq_domain_add_linear(NULL, chip->ngpio,
+						&irq_domain_simple_ops, extif);
+	if (!bus->irq_domain) {
+		err = -ENODEV;
+		goto err_irq_domain;
+	}
+	for (gpio = 0; gpio < chip->ngpio; gpio++) {
+		int irq = irq_create_mapping(bus->irq_domain, gpio);
+
+		irq_set_chip_data(irq, bus);
+		irq_set_chip_and_handler(irq, &ssb_gpio_irq_extif_chip,
+					 handle_simple_irq);
+	}
+
+	hwirq = ssb_mips_irq(bus->extif.dev) + 2;
+	err = request_irq(hwirq, ssb_gpio_irq_extif_handler, IRQF_SHARED,
+			  "gpio", bus);
+	if (err)
+		goto err_req_irq;
+
+	ssb_extif_gpio_intmask(&bus->extif, ~0, 0);
+
+	return 0;
+
+err_req_irq:
+	for (gpio = 0; gpio < chip->ngpio; gpio++) {
+		int irq = irq_find_mapping(bus->irq_domain, gpio);
+
+		irq_dispose_mapping(irq);
+	}
+	irq_domain_remove(bus->irq_domain);
+err_irq_domain:
+	return err;
+}
+
+static void ssb_gpio_irq_extif_domain_exit(struct ssb_bus *bus)
+{
+	struct ssb_extif *extif = &bus->extif;
+	struct gpio_chip *chip = &bus->gpio;
+	int gpio;
+
+	if (bus->bustype != SSB_BUSTYPE_SSB)
+		return;
+
+	free_irq(ssb_mips_irq(bus->extif.dev) + 2, extif);
+	for (gpio = 0; gpio < chip->ngpio; gpio++) {
+		int irq = irq_find_mapping(bus->irq_domain, gpio);
+
+		irq_dispose_mapping(irq);
+	}
+	irq_domain_remove(bus->irq_domain);
 }
+#else
+static int ssb_gpio_irq_extif_domain_init(struct ssb_bus *bus)
+{
+	return 0;
+}
+
+static void ssb_gpio_irq_extif_domain_exit(struct ssb_bus *bus)
+{
+}
+#endif
 
 static int ssb_gpio_extif_init(struct ssb_bus *bus)
 {
 	struct gpio_chip *chip = &bus->gpio;
+	int err;
 
 	chip->label		= "ssb_extif_gpio";
 	chip->owner		= THIS_MODULE;
@@ -165,7 +423,9 @@ static int ssb_gpio_extif_init(struct ssb_bus *bus)
 	chip->set		= ssb_gpio_extif_set_value;
 	chip->direction_input	= ssb_gpio_extif_direction_input;
 	chip->direction_output	= ssb_gpio_extif_direction_output;
-	chip->to_irq		= ssb_gpio_extif_to_irq;
+#if IS_ENABLED(CONFIG_SSB_EMBEDDED)
+	chip->to_irq		= ssb_gpio_to_irq;
+#endif
 	chip->ngpio		= 5;
 	/* There is just one SoC in one device and its GPIO addresses should be
 	 * deterministic to address them more easily. The other buses could get
@@ -175,7 +435,17 @@ static int ssb_gpio_extif_init(struct ssb_bus *bus)
 	else
 		chip->base		= -1;
 
-	return gpiochip_add(chip);
+	err = ssb_gpio_irq_extif_domain_init(bus);
+	if (err)
+		return err;
+
+	err = gpiochip_add(chip);
+	if (err) {
+		ssb_gpio_irq_extif_domain_exit(bus);
+		return err;
+	}
+
+	return 0;
 }
 
 #else
@@ -185,6 +455,10 @@ static int ssb_gpio_extif_init(struct ssb_bus *bus)
 }
 #endif
 
+/**************************************************
+ * Init
+ **************************************************/
+
 int ssb_gpio_init(struct ssb_bus *bus)
 {
 	if (ssb_chipco_available(&bus->chipco))
diff --git a/drivers/ssb/main.c b/drivers/ssb/main.c
index 32a811d11c25..2fead3820849 100644
--- a/drivers/ssb/main.c
+++ b/drivers/ssb/main.c
@@ -593,6 +593,13 @@ static int ssb_attach_queued_buses(void)
 		ssb_pcicore_init(&bus->pcicore);
 		if (bus->bustype == SSB_BUSTYPE_SSB)
 			ssb_watchdog_register(bus);
+
+		err = ssb_gpio_init(bus);
+		if (err == -ENOTSUPP)
+			ssb_dbg("GPIO driver not activated\n");
+		else if (err)
+			ssb_dbg("Error registering GPIO driver: %i\n", err);
+
 		ssb_bus_may_powerdown(bus);
 
 		err = ssb_devices_register(bus);
@@ -830,11 +837,6 @@ static int ssb_bus_register(struct ssb_bus *bus,
 	ssb_chipcommon_init(&bus->chipco);
 	ssb_extif_init(&bus->extif);
 	ssb_mipscore_init(&bus->mipscore);
-	err = ssb_gpio_init(bus);
-	if (err == -ENOTSUPP)
-		ssb_dbg("GPIO driver not activated\n");
-	else if (err)
-		ssb_dbg("Error registering GPIO driver: %i\n", err);
 	err = ssb_fetch_invariants(bus, get_invariants);
 	if (err) {
 		ssb_bus_may_powerdown(bus);
diff --git a/drivers/staging/Kconfig b/drivers/staging/Kconfig
index 4bb6b11166b3..99375f0a9440 100644
--- a/drivers/staging/Kconfig
+++ b/drivers/staging/Kconfig
@@ -52,6 +52,8 @@ source "drivers/staging/rtl8712/Kconfig"
 
 source "drivers/staging/rtl8188eu/Kconfig"
 
+source "drivers/staging/rtl8821ae/Kconfig"
+
 source "drivers/staging/rts5139/Kconfig"
 
 source "drivers/staging/rts5208/Kconfig"
@@ -76,10 +78,6 @@ source "drivers/staging/sep/Kconfig"
 
 source "drivers/staging/iio/Kconfig"
 
-source "drivers/staging/zsmalloc/Kconfig"
-
-source "drivers/staging/zram/Kconfig"
-
 source "drivers/staging/wlags49_h2/Kconfig"
 
 source "drivers/staging/wlags49_h25/Kconfig"
diff --git a/drivers/staging/Makefile b/drivers/staging/Makefile
index 9f07e5e16094..ddc3c4a5d39d 100644
--- a/drivers/staging/Makefile
+++ b/drivers/staging/Makefile
@@ -18,6 +18,7 @@ obj-$(CONFIG_RTL8192U)		+= rtl8192u/
 obj-$(CONFIG_RTL8192E)		+= rtl8192e/
 obj-$(CONFIG_R8712U)		+= rtl8712/
 obj-$(CONFIG_R8188EU)		+= rtl8188eu/
+obj-$(CONFIG_R8821AE)		+= rtl8821ae/
 obj-$(CONFIG_RTS5139)		+= rts5139/
 obj-$(CONFIG_RTS5208)		+= rts5208/
 obj-$(CONFIG_TRANZPORT)		+= frontier/
@@ -32,8 +33,6 @@ obj-$(CONFIG_VT6656)		+= vt6656/
 obj-$(CONFIG_VME_BUS)		+= vme/
 obj-$(CONFIG_DX_SEP)            += sep/
 obj-$(CONFIG_IIO)		+= iio/
-obj-$(CONFIG_ZRAM)		+= zram/
-obj-$(CONFIG_ZSMALLOC)		+= zsmalloc/
 obj-$(CONFIG_WLAGS49_H2)	+= wlags49_h2/
 obj-$(CONFIG_WLAGS49_H25)	+= wlags49_h25/
 obj-$(CONFIG_FB_SM7XX)		+= sm7xxfb/
diff --git a/drivers/staging/lustre/lustre/llite/lloop.c b/drivers/staging/lustre/lustre/llite/lloop.c
index 5338e8d4c50f..0718905adeb2 100644
--- a/drivers/staging/lustre/lustre/llite/lloop.c
+++ b/drivers/staging/lustre/lustre/llite/lloop.c
@@ -194,10 +194,10 @@ static int do_bio_lustrebacked(struct lloop_device *lo, struct bio *head)
 	struct cl_object     *obj = ll_i2info(inode)->lli_clob;
 	pgoff_t	       offset;
 	int		   ret;
-	int		   i;
 	int		   rw;
 	obd_count	     page_count = 0;
-	struct bio_vec       *bvec;
+	struct bio_vec       bvec;
+	struct bvec_iter   iter;
 	struct bio	   *bio;
 	ssize_t	       bytes;
 
@@ -220,15 +220,15 @@ static int do_bio_lustrebacked(struct lloop_device *lo, struct bio *head)
 	for (bio = head; bio != NULL; bio = bio->bi_next) {
 		LASSERT(rw == bio->bi_rw);
 
-		offset = (pgoff_t)(bio->bi_sector << 9) + lo->lo_offset;
-		bio_for_each_segment(bvec, bio, i) {
-			BUG_ON(bvec->bv_offset != 0);
-			BUG_ON(bvec->bv_len != PAGE_CACHE_SIZE);
+		offset = (pgoff_t)(bio->bi_iter.bi_sector << 9) + lo->lo_offset;
+		bio_for_each_segment(bvec, bio, iter) {
+			BUG_ON(bvec.bv_offset != 0);
+			BUG_ON(bvec.bv_len != PAGE_CACHE_SIZE);
 
-			pages[page_count] = bvec->bv_page;
+			pages[page_count] = bvec.bv_page;
 			offsets[page_count] = offset;
 			page_count++;
-			offset += bvec->bv_len;
+			offset += bvec.bv_len;
 		}
 		LASSERT(page_count <= LLOOP_MAX_SEGMENTS);
 	}
@@ -313,7 +313,8 @@ static unsigned int loop_get_bio(struct lloop_device *lo, struct bio **req)
 	bio = &lo->lo_bio;
 	while (*bio && (*bio)->bi_rw == rw) {
 		CDEBUG(D_INFO, "bio sector %llu size %u count %u vcnt%u \n",
-		       (unsigned long long)(*bio)->bi_sector, (*bio)->bi_size,
+		       (unsigned long long)(*bio)->bi_iter.bi_sector,
+		       (*bio)->bi_iter.bi_size,
 		       page_count, (*bio)->bi_vcnt);
 		if (page_count + (*bio)->bi_vcnt > LLOOP_MAX_SEGMENTS)
 			break;
@@ -347,7 +348,8 @@ static void loop_make_request(struct request_queue *q, struct bio *old_bio)
 		goto err;
 
 	CDEBUG(D_INFO, "submit bio sector %llu size %u\n",
-	       (unsigned long long)old_bio->bi_sector, old_bio->bi_size);
+	       (unsigned long long)old_bio->bi_iter.bi_sector,
+	       old_bio->bi_iter.bi_size);
 
 	spin_lock_irq(&lo->lo_lock);
 	inactive = (lo->lo_state != LLOOP_BOUND);
@@ -367,7 +369,7 @@ static void loop_make_request(struct request_queue *q, struct bio *old_bio)
 	loop_add_bio(lo, old_bio);
 	return;
 err:
-	cfs_bio_io_error(old_bio, old_bio->bi_size);
+	cfs_bio_io_error(old_bio, old_bio->bi_iter.bi_size);
 }
 
 
@@ -378,7 +380,7 @@ static inline void loop_handle_bio(struct lloop_device *lo, struct bio *bio)
 	while (bio) {
 		struct bio *tmp = bio->bi_next;
 		bio->bi_next = NULL;
-		cfs_bio_endio(bio, bio->bi_size, ret);
+		cfs_bio_endio(bio, bio->bi_iter.bi_size, ret);
 		bio = tmp;
 	}
 }
diff --git a/drivers/staging/media/Kconfig b/drivers/staging/media/Kconfig
index 46f1e619cbd8..22b0c9d6f046 100644
--- a/drivers/staging/media/Kconfig
+++ b/drivers/staging/media/Kconfig
@@ -21,6 +21,8 @@ if STAGING_MEDIA
 # Please keep them in alphabetic order
 source "drivers/staging/media/as102/Kconfig"
 
+source "drivers/staging/media/bcm2048/Kconfig"
+
 source "drivers/staging/media/cxd2099/Kconfig"
 
 source "drivers/staging/media/davinci_vpfe/Kconfig"
@@ -31,8 +33,14 @@ source "drivers/staging/media/go7007/Kconfig"
 
 source "drivers/staging/media/msi3101/Kconfig"
 
+source "drivers/staging/media/omap24xx/Kconfig"
+
+source "drivers/staging/media/sn9c102/Kconfig"
+
 source "drivers/staging/media/solo6x10/Kconfig"
 
+source "drivers/staging/media/omap4iss/Kconfig"
+
 # Keep LIRC at the end, as it has sub-menus
 source "drivers/staging/media/lirc/Kconfig"
 
diff --git a/drivers/staging/media/Makefile b/drivers/staging/media/Makefile
index eb7f30b1ccd8..bedc62aaede6 100644
--- a/drivers/staging/media/Makefile
+++ b/drivers/staging/media/Makefile
@@ -1,4 +1,5 @@
 obj-$(CONFIG_DVB_AS102)		+= as102/
+obj-$(CONFIG_I2C_BCM2048)	+= bcm2048/
 obj-$(CONFIG_DVB_CXD2099)	+= cxd2099/
 obj-$(CONFIG_LIRC_STAGING)	+= lirc/
 obj-$(CONFIG_SOLO6X10)		+= solo6x10/
@@ -6,3 +7,7 @@ obj-$(CONFIG_VIDEO_DT3155)	+= dt3155v4l/
 obj-$(CONFIG_VIDEO_GO7007)	+= go7007/
 obj-$(CONFIG_USB_MSI3101)	+= msi3101/
 obj-$(CONFIG_VIDEO_DM365_VPFE)	+= davinci_vpfe/
+obj-$(CONFIG_VIDEO_OMAP4)	+= omap4iss/
+obj-$(CONFIG_USB_SN9C102)       += sn9c102/
+obj-$(CONFIG_VIDEO_OMAP2)       += omap24xx/
+obj-$(CONFIG_VIDEO_TCM825X)     += omap24xx/
diff --git a/drivers/staging/media/as102/as102_drv.c b/drivers/staging/media/as102/as102_drv.c
index 8b7bb9547079..09d64cd67502 100644
--- a/drivers/staging/media/as102/as102_drv.c
+++ b/drivers/staging/media/as102/as102_drv.c
@@ -111,8 +111,6 @@ static int as10x_pid_filter(struct as102_dev_t *dev,
 	struct as10x_bus_adapter_t *bus_adap = &dev->bus_adap;
 	int ret = -EFAULT;
 
-	ENTER();
-
 	if (mutex_lock_interruptible(&dev->bus_adap.lock)) {
 		dprintk(debug, "mutex_lock_interruptible(lock) failed !\n");
 		return -EBUSY;
@@ -133,15 +131,14 @@ static int as10x_pid_filter(struct as102_dev_t *dev,
 		filter.pid = pid;
 
 		ret = as10x_cmd_add_PID_filter(bus_adap, &filter);
-		dprintk(debug, "ADD_PID_FILTER([%02d -> %02d], 0x%04x) ret = %d\n",
+		dprintk(debug,
+			"ADD_PID_FILTER([%02d -> %02d], 0x%04x) ret = %d\n",
 			index, filter.idx, filter.pid, ret);
 		break;
 	}
 	}
 
 	mutex_unlock(&dev->bus_adap.lock);
-
-	LEAVE();
 	return ret;
 }
 
@@ -151,8 +148,6 @@ static int as102_dvb_dmx_start_feed(struct dvb_demux_feed *dvbdmxfeed)
 	struct dvb_demux *demux = dvbdmxfeed->demux;
 	struct as102_dev_t *as102_dev = demux->priv;
 
-	ENTER();
-
 	if (mutex_lock_interruptible(&as102_dev->sem))
 		return -ERESTARTSYS;
 
@@ -164,7 +159,6 @@ static int as102_dvb_dmx_start_feed(struct dvb_demux_feed *dvbdmxfeed)
 		ret = as102_start_stream(as102_dev);
 
 	mutex_unlock(&as102_dev->sem);
-	LEAVE();
 	return ret;
 }
 
@@ -173,8 +167,6 @@ static int as102_dvb_dmx_stop_feed(struct dvb_demux_feed *dvbdmxfeed)
 	struct dvb_demux *demux = dvbdmxfeed->demux;
 	struct as102_dev_t *as102_dev = demux->priv;
 
-	ENTER();
-
 	if (mutex_lock_interruptible(&as102_dev->sem))
 		return -ERESTARTSYS;
 
@@ -186,7 +178,6 @@ static int as102_dvb_dmx_stop_feed(struct dvb_demux_feed *dvbdmxfeed)
 				 dvbdmxfeed->pid, 0);
 
 	mutex_unlock(&as102_dev->sem);
-	LEAVE();
 	return 0;
 }
 
diff --git a/drivers/staging/media/as102/as102_drv.h b/drivers/staging/media/as102/as102_drv.h
index b0e5a23bd532..a06837dcc05d 100644
--- a/drivers/staging/media/as102/as102_drv.h
+++ b/drivers/staging/media/as102/as102_drv.h
@@ -38,14 +38,6 @@ extern int elna_enable;
 		printk(args);	\
 	} } while (0)
 
-#ifdef TRACE
-#define ENTER()	pr_debug(">> enter %s\n", __func__)
-#define LEAVE()	pr_debug("<< leave %s\n", __func__)
-#else
-#define ENTER()
-#define LEAVE()
-#endif
-
 #define AS102_DEVICE_MAJOR	192
 
 #define AS102_USB_BUF_SIZE	512
diff --git a/drivers/staging/media/as102/as102_fe.c b/drivers/staging/media/as102/as102_fe.c
index 9ce8c9daa2e7..b686b7617cdc 100644
--- a/drivers/staging/media/as102/as102_fe.c
+++ b/drivers/staging/media/as102/as102_fe.c
@@ -34,8 +34,6 @@ static int as102_fe_set_frontend(struct dvb_frontend *fe)
 	struct as102_dev_t *dev;
 	struct as10x_tune_args tune_args = { 0 };
 
-	ENTER();
-
 	dev = (struct as102_dev_t *) fe->tuner_priv;
 	if (dev == NULL)
 		return -ENODEV;
@@ -52,7 +50,6 @@ static int as102_fe_set_frontend(struct dvb_frontend *fe)
 
 	mutex_unlock(&dev->bus_adap.lock);
 
-	LEAVE();
 	return (ret < 0) ? -EINVAL : 0;
 }
 
@@ -63,8 +60,6 @@ static int as102_fe_get_frontend(struct dvb_frontend *fe)
 	struct as102_dev_t *dev;
 	struct as10x_tps tps = { 0 };
 
-	ENTER();
-
 	dev = (struct as102_dev_t *) fe->tuner_priv;
 	if (dev == NULL)
 		return -EINVAL;
@@ -80,13 +75,11 @@ static int as102_fe_get_frontend(struct dvb_frontend *fe)
 
 	mutex_unlock(&dev->bus_adap.lock);
 
-	LEAVE();
 	return (ret < 0) ? -EINVAL : 0;
 }
 
 static int as102_fe_get_tune_settings(struct dvb_frontend *fe,
 			struct dvb_frontend_tune_settings *settings) {
-	ENTER();
 
 #if 0
 	dprintk(debug, "step_size    = %d\n", settings->step_size);
@@ -97,7 +90,6 @@ static int as102_fe_get_tune_settings(struct dvb_frontend *fe,
 
 	settings->min_delay_ms = 1000;
 
-	LEAVE();
 	return 0;
 }
 
@@ -108,8 +100,6 @@ static int as102_fe_read_status(struct dvb_frontend *fe, fe_status_t *status)
 	struct as102_dev_t *dev;
 	struct as10x_tune_status tstate = { 0 };
 
-	ENTER();
-
 	dev = (struct as102_dev_t *) fe->tuner_priv;
 	if (dev == NULL)
 		return -ENODEV;
@@ -151,8 +141,8 @@ static int as102_fe_read_status(struct dvb_frontend *fe, fe_status_t *status)
 		if (as10x_cmd_get_demod_stats(&dev->bus_adap,
 			(struct as10x_demod_stats *) &dev->demod_stats) < 0) {
 			memset(&dev->demod_stats, 0, sizeof(dev->demod_stats));
-			dprintk(debug, "as10x_cmd_get_demod_stats failed "
-				"(probably not tuned)\n");
+			dprintk(debug,
+				"as10x_cmd_get_demod_stats failed (probably not tuned)\n");
 		} else {
 			dprintk(debug,
 				"demod status: fc: 0x%08x, bad fc: 0x%08x, "
@@ -168,7 +158,6 @@ static int as102_fe_read_status(struct dvb_frontend *fe, fe_status_t *status)
 
 out:
 	mutex_unlock(&dev->bus_adap.lock);
-	LEAVE();
 	return ret;
 }
 
@@ -183,15 +172,12 @@ static int as102_fe_read_snr(struct dvb_frontend *fe, u16 *snr)
 {
 	struct as102_dev_t *dev;
 
-	ENTER();
-
 	dev = (struct as102_dev_t *) fe->tuner_priv;
 	if (dev == NULL)
 		return -ENODEV;
 
 	*snr = dev->demod_stats.mer;
 
-	LEAVE();
 	return 0;
 }
 
@@ -199,15 +185,12 @@ static int as102_fe_read_ber(struct dvb_frontend *fe, u32 *ber)
 {
 	struct as102_dev_t *dev;
 
-	ENTER();
-
 	dev = (struct as102_dev_t *) fe->tuner_priv;
 	if (dev == NULL)
 		return -ENODEV;
 
 	*ber = dev->ber;
 
-	LEAVE();
 	return 0;
 }
 
@@ -216,15 +199,12 @@ static int as102_fe_read_signal_strength(struct dvb_frontend *fe,
 {
 	struct as102_dev_t *dev;
 
-	ENTER();
-
 	dev = (struct as102_dev_t *) fe->tuner_priv;
 	if (dev == NULL)
 		return -ENODEV;
 
 	*strength = (((0xffff * 400) * dev->signal_strength + 41000) * 2);
 
-	LEAVE();
 	return 0;
 }
 
@@ -232,8 +212,6 @@ static int as102_fe_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
 {
 	struct as102_dev_t *dev;
 
-	ENTER();
-
 	dev = (struct as102_dev_t *) fe->tuner_priv;
 	if (dev == NULL)
 		return -ENODEV;
@@ -243,7 +221,6 @@ static int as102_fe_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
 	else
 		*ucblocks = 0;
 
-	LEAVE();
 	return 0;
 }
 
@@ -252,8 +229,6 @@ static int as102_fe_ts_bus_ctrl(struct dvb_frontend *fe, int acquire)
 	struct as102_dev_t *dev;
 	int ret;
 
-	ENTER();
-
 	dev = (struct as102_dev_t *) fe->tuner_priv;
 	if (dev == NULL)
 		return -ENODEV;
@@ -263,7 +238,8 @@ static int as102_fe_ts_bus_ctrl(struct dvb_frontend *fe, int acquire)
 
 	if (acquire) {
 		if (elna_enable)
-			as10x_cmd_set_context(&dev->bus_adap, CONTEXT_LNA, dev->elna_cfg);
+			as10x_cmd_set_context(&dev->bus_adap,
+					      CONTEXT_LNA, dev->elna_cfg);
 
 		ret = as10x_cmd_turn_on(&dev->bus_adap);
 	} else {
@@ -272,7 +248,6 @@ static int as102_fe_ts_bus_ctrl(struct dvb_frontend *fe, int acquire)
 
 	mutex_unlock(&dev->bus_adap.lock);
 
-	LEAVE();
 	return ret;
 }
 
@@ -581,8 +556,8 @@ static void as102_fe_copy_tune_parameters(struct as10x_tune_args *tune_args,
 			   as102_fe_get_code_rate(params->code_rate_LP);
 		}
 
-		dprintk(debug, "\thierarchy: 0x%02x  "
-				"selected: %s  code_rate_%s: 0x%02x\n",
+		dprintk(debug,
+			"\thierarchy: 0x%02x  selected: %s  code_rate_%s: 0x%02x\n",
 			tune_args->hierarchy,
 			tune_args->hier_select == HIER_HIGH_PRIORITY ?
 			"HP" : "LP",
diff --git a/drivers/staging/media/as102/as102_fw.c b/drivers/staging/media/as102/as102_fw.c
index b9670ee41b4e..f33f752c0aad 100644
--- a/drivers/staging/media/as102/as102_fw.c
+++ b/drivers/staging/media/as102/as102_fw.c
@@ -26,10 +26,10 @@
 #include "as102_drv.h"
 #include "as102_fw.h"
 
-char as102_st_fw1[] = "as102_data1_st.hex";
-char as102_st_fw2[] = "as102_data2_st.hex";
-char as102_dt_fw1[] = "as102_data1_dt.hex";
-char as102_dt_fw2[] = "as102_data2_dt.hex";
+static const char as102_st_fw1[] = "as102_data1_st.hex";
+static const char as102_st_fw2[] = "as102_data2_st.hex";
+static const char as102_dt_fw1[] = "as102_data1_dt.hex";
+static const char as102_dt_fw2[] = "as102_data2_dt.hex";
 
 static unsigned char atohx(unsigned char *dst, char *src)
 {
@@ -109,8 +109,6 @@ static int as102_firmware_upload(struct as10x_bus_adapter_t *bus_adap,
 	int total_read_bytes = 0, errno = 0;
 	unsigned char addr_has_changed = 0;
 
-	ENTER();
-
 	for (total_read_bytes = 0; total_read_bytes < firmware->size; ) {
 		int read_bytes = 0, data_len = 0;
 
@@ -158,7 +156,6 @@ static int as102_firmware_upload(struct as10x_bus_adapter_t *bus_adap,
 		}
 	}
 error:
-	LEAVE();
 	return (errno == 0) ? total_read_bytes : errno;
 }
 
@@ -167,11 +164,9 @@ int as102_fw_upload(struct as10x_bus_adapter_t *bus_adap)
 	int errno = -EFAULT;
 	const struct firmware *firmware = NULL;
 	unsigned char *cmd_buf = NULL;
-	char *fw1, *fw2;
+	const char *fw1, *fw2;
 	struct usb_device *dev = bus_adap->usb_dev;
 
-	ENTER();
-
 	/* select fw file to upload */
 	if (dual_tuner) {
 		fw1 = as102_dt_fw1;
@@ -233,6 +228,5 @@ error:
 	kfree(cmd_buf);
 	release_firmware(firmware);
 
-	LEAVE();
 	return errno;
 }
diff --git a/drivers/staging/media/as102/as102_usb_drv.c b/drivers/staging/media/as102/as102_usb_drv.c
index 9f275f020150..e4a69454ebeb 100644
--- a/drivers/staging/media/as102/as102_usb_drv.c
+++ b/drivers/staging/media/as102/as102_usb_drv.c
@@ -92,7 +92,6 @@ static int as102_usb_xfer_cmd(struct as10x_bus_adapter_t *bus_adap,
 			      unsigned char *recv_buf, int recv_buf_len)
 {
 	int ret = 0;
-	ENTER();
 
 	if (send_buf != NULL) {
 		ret = usb_control_msg(bus_adap->usb_dev,
@@ -140,7 +139,6 @@ static int as102_usb_xfer_cmd(struct as10x_bus_adapter_t *bus_adap,
 #endif
 	}
 
-	LEAVE();
 	return ret;
 }
 
@@ -191,7 +189,7 @@ static int as102_read_ep2(struct as10x_bus_adapter_t *bus_adap,
 	return ret ? ret : actual_len;
 }
 
-struct as102_priv_ops_t as102_priv_ops = {
+static struct as102_priv_ops_t as102_priv_ops = {
 	.upload_fw_pkt	= as102_send_ep1,
 	.xfer_cmd	= as102_usb_xfer_cmd,
 	.as102_read_ep2	= as102_read_ep2,
@@ -240,8 +238,6 @@ static void as102_free_usb_stream_buffer(struct as102_dev_t *dev)
 {
 	int i;
 
-	ENTER();
-
 	for (i = 0; i < MAX_STREAM_URB; i++)
 		usb_free_urb(dev->stream_urb[i]);
 
@@ -249,15 +245,12 @@ static void as102_free_usb_stream_buffer(struct as102_dev_t *dev)
 			MAX_STREAM_URB * AS102_USB_BUF_SIZE,
 			dev->stream,
 			dev->dma_addr);
-	LEAVE();
 }
 
 static int as102_alloc_usb_stream_buffer(struct as102_dev_t *dev)
 {
 	int i, ret = 0;
 
-	ENTER();
-
 	dev->stream = usb_alloc_coherent(dev->bus_adap.usb_dev,
 				       MAX_STREAM_URB * AS102_USB_BUF_SIZE,
 				       GFP_KERNEL,
@@ -287,7 +280,6 @@ static int as102_alloc_usb_stream_buffer(struct as102_dev_t *dev)
 
 		dev->stream_urb[i] = urb;
 	}
-	LEAVE();
 	return ret;
 }
 
@@ -318,23 +310,17 @@ static void as102_usb_release(struct kref *kref)
 {
 	struct as102_dev_t *as102_dev;
 
-	ENTER();
-
 	as102_dev = container_of(kref, struct as102_dev_t, kref);
 	if (as102_dev != NULL) {
 		usb_put_dev(as102_dev->bus_adap.usb_dev);
 		kfree(as102_dev);
 	}
-
-	LEAVE();
 }
 
 static void as102_usb_disconnect(struct usb_interface *intf)
 {
 	struct as102_dev_t *as102_dev;
 
-	ENTER();
-
 	/* extract as102_dev_t from usb_device private data */
 	as102_dev = usb_get_intfdata(intf);
 
@@ -353,8 +339,6 @@ static void as102_usb_disconnect(struct usb_interface *intf)
 	kref_put(&as102_dev->kref, as102_usb_release);
 
 	pr_info("%s: device has been disconnected\n", DRIVER_NAME);
-
-	LEAVE();
 }
 
 static int as102_usb_probe(struct usb_interface *intf,
@@ -364,8 +348,6 @@ static int as102_usb_probe(struct usb_interface *intf,
 	struct as102_dev_t *as102_dev;
 	int i;
 
-	ENTER();
-
 	/* This should never actually happen */
 	if (ARRAY_SIZE(as102_usb_id_table) !=
 	    (sizeof(as102_device_names) / sizeof(const char *))) {
@@ -419,15 +401,21 @@ static int as102_usb_probe(struct usb_interface *intf,
 	/* request buffer allocation for streaming */
 	ret = as102_alloc_usb_stream_buffer(as102_dev);
 	if (ret != 0)
-		goto failed;
+		goto failed_stream;
 
 	/* register dvb layer */
 	ret = as102_dvb_register(as102_dev);
+	if (ret != 0)
+		goto failed_dvb;
 
-	LEAVE();
 	return ret;
 
+failed_dvb:
+	as102_free_usb_stream_buffer(as102_dev);
+failed_stream:
+	usb_deregister_dev(intf, &as102_usb_class_driver);
 failed:
+	usb_put_dev(as102_dev->bus_adap.usb_dev);
 	usb_set_intfdata(intf, NULL);
 	kfree(as102_dev);
 	return ret;
@@ -439,8 +427,6 @@ static int as102_open(struct inode *inode, struct file *file)
 	struct usb_interface *intf = NULL;
 	struct as102_dev_t *dev = NULL;
 
-	ENTER();
-
 	/* read minor from inode */
 	minor = iminor(inode);
 
@@ -467,7 +453,6 @@ static int as102_open(struct inode *inode, struct file *file)
 	kref_get(&dev->kref);
 
 exit:
-	LEAVE();
 	return ret;
 }
 
@@ -476,15 +461,12 @@ static int as102_release(struct inode *inode, struct file *file)
 	int ret = 0;
 	struct as102_dev_t *dev = NULL;
 
-	ENTER();
-
 	dev = file->private_data;
 	if (dev != NULL) {
 		/* decrement the count on our device */
 		kref_put(&dev->kref, as102_usb_release);
 	}
 
-	LEAVE();
 	return ret;
 }
 
diff --git a/drivers/staging/media/as102/as10x_cmd.c b/drivers/staging/media/as102/as10x_cmd.c
index a73df10982d0..9e49f15a7c9f 100644
--- a/drivers/staging/media/as102/as10x_cmd.c
+++ b/drivers/staging/media/as102/as10x_cmd.c
@@ -34,8 +34,6 @@ int as10x_cmd_turn_on(struct as10x_bus_adapter_t *adap)
 	int error = AS10X_CMD_ERROR;
 	struct as10x_cmd_t *pcmd, *prsp;
 
-	ENTER();
-
 	pcmd = adap->cmd;
 	prsp = adap->rsp;
 
@@ -63,7 +61,6 @@ int as10x_cmd_turn_on(struct as10x_bus_adapter_t *adap)
 	error = as10x_rsp_parse(prsp, CONTROL_PROC_TURNON_RSP);
 
 out:
-	LEAVE();
 	return error;
 }
 
@@ -78,8 +75,6 @@ int as10x_cmd_turn_off(struct as10x_bus_adapter_t *adap)
 	int error = AS10X_CMD_ERROR;
 	struct as10x_cmd_t *pcmd, *prsp;
 
-	ENTER();
-
 	pcmd = adap->cmd;
 	prsp = adap->rsp;
 
@@ -106,7 +101,6 @@ int as10x_cmd_turn_off(struct as10x_bus_adapter_t *adap)
 	error = as10x_rsp_parse(prsp, CONTROL_PROC_TURNOFF_RSP);
 
 out:
-	LEAVE();
 	return error;
 }
 
@@ -123,8 +117,6 @@ int as10x_cmd_set_tune(struct as10x_bus_adapter_t *adap,
 	int error = AS10X_CMD_ERROR;
 	struct as10x_cmd_t *preq, *prsp;
 
-	ENTER();
-
 	preq = adap->cmd;
 	prsp = adap->rsp;
 
@@ -164,7 +156,6 @@ int as10x_cmd_set_tune(struct as10x_bus_adapter_t *adap,
 	error = as10x_rsp_parse(prsp, CONTROL_PROC_SETTUNE_RSP);
 
 out:
-	LEAVE();
 	return error;
 }
 
@@ -181,8 +172,6 @@ int as10x_cmd_get_tune_status(struct as10x_bus_adapter_t *adap,
 	int error = AS10X_CMD_ERROR;
 	struct as10x_cmd_t  *preq, *prsp;
 
-	ENTER();
-
 	preq = adap->cmd;
 	prsp = adap->rsp;
 
@@ -220,7 +209,6 @@ int as10x_cmd_get_tune_status(struct as10x_bus_adapter_t *adap,
 	pstatus->BER = le16_to_cpu(prsp->body.get_tune_status.rsp.sts.BER);
 
 out:
-	LEAVE();
 	return error;
 }
 
@@ -236,8 +224,6 @@ int as10x_cmd_get_tps(struct as10x_bus_adapter_t *adap, struct as10x_tps *ptps)
 	int error = AS10X_CMD_ERROR;
 	struct as10x_cmd_t *pcmd, *prsp;
 
-	ENTER();
-
 	pcmd = adap->cmd;
 	prsp = adap->rsp;
 
@@ -281,7 +267,6 @@ int as10x_cmd_get_tps(struct as10x_bus_adapter_t *adap, struct as10x_tps *ptps)
 	ptps->cell_ID = le16_to_cpu(prsp->body.get_tps.rsp.tps.cell_ID);
 
 out:
-	LEAVE();
 	return error;
 }
 
@@ -298,8 +283,6 @@ int as10x_cmd_get_demod_stats(struct as10x_bus_adapter_t *adap,
 	int error = AS10X_CMD_ERROR;
 	struct as10x_cmd_t *pcmd, *prsp;
 
-	ENTER();
-
 	pcmd = adap->cmd;
 	prsp = adap->rsp;
 
@@ -343,7 +326,6 @@ int as10x_cmd_get_demod_stats(struct as10x_bus_adapter_t *adap,
 		prsp->body.get_demod_stats.rsp.stats.has_started;
 
 out:
-	LEAVE();
 	return error;
 }
 
@@ -361,8 +343,6 @@ int as10x_cmd_get_impulse_resp(struct as10x_bus_adapter_t *adap,
 	int error = AS10X_CMD_ERROR;
 	struct as10x_cmd_t *pcmd, *prsp;
 
-	ENTER();
-
 	pcmd = adap->cmd;
 	prsp = adap->rsp;
 
@@ -397,7 +377,6 @@ int as10x_cmd_get_impulse_resp(struct as10x_bus_adapter_t *adap,
 	*is_ready = prsp->body.get_impulse_rsp.rsp.is_ready;
 
 out:
-	LEAVE();
 	return error;
 }
 
diff --git a/drivers/staging/media/as102/as10x_cmd_cfg.c b/drivers/staging/media/as102/as10x_cmd_cfg.c
index 4a2bbd766655..b1e300d88753 100644
--- a/drivers/staging/media/as102/as10x_cmd_cfg.c
+++ b/drivers/staging/media/as102/as10x_cmd_cfg.c
@@ -40,8 +40,6 @@ int as10x_cmd_get_context(struct as10x_bus_adapter_t *adap, uint16_t tag,
 	int  error;
 	struct as10x_cmd_t *pcmd, *prsp;
 
-	ENTER();
-
 	pcmd = adap->cmd;
 	prsp = adap->rsp;
 
@@ -81,7 +79,6 @@ int as10x_cmd_get_context(struct as10x_bus_adapter_t *adap, uint16_t tag,
 	}
 
 out:
-	LEAVE();
 	return error;
 }
 
@@ -99,8 +96,6 @@ int as10x_cmd_set_context(struct as10x_bus_adapter_t *adap, uint16_t tag,
 	int error;
 	struct as10x_cmd_t *pcmd, *prsp;
 
-	ENTER();
-
 	pcmd = adap->cmd;
 	prsp = adap->rsp;
 
@@ -136,7 +131,6 @@ int as10x_cmd_set_context(struct as10x_bus_adapter_t *adap, uint16_t tag,
 	error = as10x_context_rsp_parse(prsp, CONTROL_PROC_CONTEXT_RSP);
 
 out:
-	LEAVE();
 	return error;
 }
 
@@ -156,8 +150,6 @@ int as10x_cmd_eLNA_change_mode(struct as10x_bus_adapter_t *adap, uint8_t mode)
 	int error;
 	struct as10x_cmd_t *pcmd, *prsp;
 
-	ENTER();
-
 	pcmd = adap->cmd;
 	prsp = adap->rsp;
 
@@ -188,7 +180,6 @@ int as10x_cmd_eLNA_change_mode(struct as10x_bus_adapter_t *adap, uint8_t mode)
 	error = as10x_rsp_parse(prsp, CONTROL_PROC_ELNA_CHANGE_MODE_RSP);
 
 out:
-	LEAVE();
 	return error;
 }
 
diff --git a/drivers/staging/media/as102/as10x_cmd_stream.c b/drivers/staging/media/as102/as10x_cmd_stream.c
index 6d000f60fb0e..1088ca1fe92f 100644
--- a/drivers/staging/media/as102/as10x_cmd_stream.c
+++ b/drivers/staging/media/as102/as10x_cmd_stream.c
@@ -34,8 +34,6 @@ int as10x_cmd_add_PID_filter(struct as10x_bus_adapter_t *adap,
 	int error;
 	struct as10x_cmd_t *pcmd, *prsp;
 
-	ENTER();
-
 	pcmd = adap->cmd;
 	prsp = adap->rsp;
 
@@ -77,7 +75,6 @@ int as10x_cmd_add_PID_filter(struct as10x_bus_adapter_t *adap,
 	}
 
 out:
-	LEAVE();
 	return error;
 }
 
@@ -94,8 +91,6 @@ int as10x_cmd_del_PID_filter(struct as10x_bus_adapter_t *adap,
 	int error;
 	struct as10x_cmd_t *pcmd, *prsp;
 
-	ENTER();
-
 	pcmd = adap->cmd;
 	prsp = adap->rsp;
 
@@ -126,7 +121,6 @@ int as10x_cmd_del_PID_filter(struct as10x_bus_adapter_t *adap,
 	error = as10x_rsp_parse(prsp, CONTROL_PROC_REMOVEFILTER_RSP);
 
 out:
-	LEAVE();
 	return error;
 }
 
@@ -141,8 +135,6 @@ int as10x_cmd_start_streaming(struct as10x_bus_adapter_t *adap)
 	int error;
 	struct as10x_cmd_t *pcmd, *prsp;
 
-	ENTER();
-
 	pcmd = adap->cmd;
 	prsp = adap->rsp;
 
@@ -172,7 +164,6 @@ int as10x_cmd_start_streaming(struct as10x_bus_adapter_t *adap)
 	error = as10x_rsp_parse(prsp, CONTROL_PROC_START_STREAMING_RSP);
 
 out:
-	LEAVE();
 	return error;
 }
 
@@ -187,8 +178,6 @@ int as10x_cmd_stop_streaming(struct as10x_bus_adapter_t *adap)
 	int8_t error;
 	struct as10x_cmd_t *pcmd, *prsp;
 
-	ENTER();
-
 	pcmd = adap->cmd;
 	prsp = adap->rsp;
 
@@ -218,6 +207,5 @@ int as10x_cmd_stop_streaming(struct as10x_bus_adapter_t *adap)
 	error = as10x_rsp_parse(prsp, CONTROL_PROC_STOP_STREAMING_RSP);
 
 out:
-	LEAVE();
 	return error;
 }
diff --git a/drivers/staging/media/bcm2048/Kconfig b/drivers/staging/media/bcm2048/Kconfig
new file mode 100644
index 000000000000..a9fc6e186494
--- /dev/null
+++ b/drivers/staging/media/bcm2048/Kconfig
@@ -0,0 +1,13 @@
+#
+# Multimedia Video device configuration
+#
+
+config I2C_BCM2048
+	tristate "Broadcom BCM2048 FM Radio Receiver support"
+	depends on I2C && VIDEO_V4L2 && RADIO_ADAPTERS
+	---help---
+	  Say Y here if you want support to BCM2048 FM Radio Receiver.
+	  This device driver supports only i2c bus.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called radio-bcm2048.
diff --git a/drivers/staging/media/bcm2048/Makefile b/drivers/staging/media/bcm2048/Makefile
new file mode 100644
index 000000000000..b4f5663d1408
--- /dev/null
+++ b/drivers/staging/media/bcm2048/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_I2C_BCM2048) += radio-bcm2048.o
diff --git a/drivers/staging/media/bcm2048/TODO b/drivers/staging/media/bcm2048/TODO
new file mode 100644
index 000000000000..051f85dbe89e
--- /dev/null
+++ b/drivers/staging/media/bcm2048/TODO
@@ -0,0 +1,24 @@
+TODO:
+
+From the initial code review:
+
+The main thing you need to do is to implement all the controls using the
+control framework (see Documentation/video4linux/v4l2-controls.txt).
+Most drivers are by now converted to the control framework, so you will
+find many examples of how to do this in drivers/media/radio.
+
+The sysfs stuff should be replaced by controls as well. A lot of the RDS
+support is now available as controls (although there may well be some
+missing features, but that is easy enough to add). Since the RDS data is
+actually read() from the device I am not sure whether the RDS
+properties/controls should be there at all.
+
+Correct Coding Style, as this driver also violates several Style
+rules, and do evil tricks, like returning from a function inside a
+macro.
+
+Finally this driver should probably be split up into two parts: one
+v4l2_subdev-based core driver and one platform driver. See e.g.
+radio-si4713/si4713-i2c.c as a good example. But I would wait with that
+until the rest of the driver is cleaned up. Then I have a better idea of
+whether this is necessary or not.
diff --git a/drivers/staging/media/bcm2048/radio-bcm2048.c b/drivers/staging/media/bcm2048/radio-bcm2048.c
new file mode 100644
index 000000000000..b2cd3a85166d
--- /dev/null
+++ b/drivers/staging/media/bcm2048/radio-bcm2048.c
@@ -0,0 +1,2744 @@
+/*
+ * drivers/staging/media/radio-bcm2048.c
+ *
+ * Driver for I2C Broadcom BCM2048 FM Radio Receiver:
+ *
+ * Copyright (C) Nokia Corporation
+ * Contact: Eero Nurkkala <ext-eero.nurkkala@nokia.com>
+ *
+ * Copyright (C) Nils Faerber <nils.faerber@kernelconcepts.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.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have 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
+ */
+
+/*
+ * History:
+ *		Eero Nurkkala <ext-eero.nurkkala@nokia.com>
+ *		Version 0.0.1
+ *		- Initial implementation
+ * 2010-02-21	Nils Faerber <nils.faerber@kernelconcepts.de>
+ *		Version 0.0.2
+ *		- Add support for interrupt driven rds data reading
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/version.h>
+#include <linux/interrupt.h>
+#include <linux/sysfs.h>
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ioctl.h>
+#include "radio-bcm2048.h"
+
+/* driver definitions */
+#define BCM2048_DRIVER_AUTHOR	"Eero Nurkkala <ext-eero.nurkkala@nokia.com>"
+#define BCM2048_DRIVER_NAME	BCM2048_NAME
+#define BCM2048_DRIVER_VERSION	KERNEL_VERSION(0, 0, 1)
+#define BCM2048_DRIVER_CARD	"Broadcom bcm2048 FM Radio Receiver"
+#define BCM2048_DRIVER_DESC	"I2C driver for BCM2048 FM Radio Receiver"
+
+/* I2C Control Registers */
+#define BCM2048_I2C_FM_RDS_SYSTEM	0x00
+#define BCM2048_I2C_FM_CTRL		0x01
+#define BCM2048_I2C_RDS_CTRL0		0x02
+#define BCM2048_I2C_RDS_CTRL1		0x03
+#define BCM2048_I2C_FM_AUDIO_PAUSE	0x04
+#define BCM2048_I2C_FM_AUDIO_CTRL0	0x05
+#define BCM2048_I2C_FM_AUDIO_CTRL1	0x06
+#define BCM2048_I2C_FM_SEARCH_CTRL0	0x07
+#define BCM2048_I2C_FM_SEARCH_CTRL1	0x08
+#define BCM2048_I2C_FM_SEARCH_TUNE_MODE	0x09
+#define BCM2048_I2C_FM_FREQ0		0x0a
+#define BCM2048_I2C_FM_FREQ1		0x0b
+#define BCM2048_I2C_FM_AF_FREQ0		0x0c
+#define BCM2048_I2C_FM_AF_FREQ1		0x0d
+#define BCM2048_I2C_FM_CARRIER		0x0e
+#define BCM2048_I2C_FM_RSSI		0x0f
+#define BCM2048_I2C_FM_RDS_MASK0	0x10
+#define BCM2048_I2C_FM_RDS_MASK1	0x11
+#define BCM2048_I2C_FM_RDS_FLAG0	0x12
+#define BCM2048_I2C_FM_RDS_FLAG1	0x13
+#define BCM2048_I2C_RDS_WLINE		0x14
+#define BCM2048_I2C_RDS_BLKB_MATCH0	0x16
+#define BCM2048_I2C_RDS_BLKB_MATCH1	0x17
+#define BCM2048_I2C_RDS_BLKB_MASK0	0x18
+#define BCM2048_I2C_RDS_BLKB_MASK1	0x19
+#define BCM2048_I2C_RDS_PI_MATCH0	0x1a
+#define BCM2048_I2C_RDS_PI_MATCH1	0x1b
+#define BCM2048_I2C_RDS_PI_MASK0	0x1c
+#define BCM2048_I2C_RDS_PI_MASK1	0x1d
+#define BCM2048_I2C_SPARE1		0x20
+#define BCM2048_I2C_SPARE2		0x21
+#define BCM2048_I2C_FM_RDS_REV		0x28
+#define BCM2048_I2C_SLAVE_CONFIGURATION	0x29
+#define BCM2048_I2C_RDS_DATA		0x80
+#define BCM2048_I2C_FM_BEST_TUNE_MODE	0x90
+
+/* BCM2048_I2C_FM_RDS_SYSTEM */
+#define BCM2048_FM_ON			0x01
+#define BCM2048_RDS_ON			0x02
+
+/* BCM2048_I2C_FM_CTRL */
+#define BCM2048_BAND_SELECT			0x01
+#define BCM2048_STEREO_MONO_AUTO_SELECT		0x02
+#define BCM2048_STEREO_MONO_MANUAL_SELECT	0x04
+#define BCM2048_STEREO_MONO_BLEND_SWITCH	0x08
+#define BCM2048_HI_LO_INJECTION			0x10
+
+/* BCM2048_I2C_RDS_CTRL0 */
+#define BCM2048_RBDS_RDS_SELECT		0x01
+#define BCM2048_FLUSH_FIFO		0x02
+
+/* BCM2048_I2C_FM_AUDIO_PAUSE */
+#define BCM2048_AUDIO_PAUSE_RSSI_TRESH	0x0f
+#define BCM2048_AUDIO_PAUSE_DURATION	0xf0
+
+/* BCM2048_I2C_FM_AUDIO_CTRL0 */
+#define BCM2048_RF_MUTE			0x01
+#define BCM2048_MANUAL_MUTE		0x02
+#define BCM2048_DAC_OUTPUT_LEFT		0x04
+#define BCM2048_DAC_OUTPUT_RIGHT	0x08
+#define BCM2048_AUDIO_ROUTE_DAC		0x10
+#define BCM2048_AUDIO_ROUTE_I2S		0x20
+#define BCM2048_DE_EMPHASIS_SELECT	0x40
+#define BCM2048_AUDIO_BANDWIDTH_SELECT	0x80
+
+/* BCM2048_I2C_FM_SEARCH_CTRL0 */
+#define BCM2048_SEARCH_RSSI_THRESHOLD	0x7f
+#define BCM2048_SEARCH_DIRECTION	0x80
+
+/* BCM2048_I2C_FM_SEARCH_TUNE_MODE */
+#define BCM2048_FM_AUTO_SEARCH		0x03
+
+/* BCM2048_I2C_FM_RSSI */
+#define BCM2048_RSSI_VALUE		0xff
+
+/* BCM2048_I2C_FM_RDS_MASK0 */
+/* BCM2048_I2C_FM_RDS_MASK1 */
+#define BCM2048_FM_FLAG_SEARCH_TUNE_FINISHED	0x01
+#define BCM2048_FM_FLAG_SEARCH_TUNE_FAIL	0x02
+#define BCM2048_FM_FLAG_RSSI_LOW		0x04
+#define BCM2048_FM_FLAG_CARRIER_ERROR_HIGH	0x08
+#define BCM2048_FM_FLAG_AUDIO_PAUSE_INDICATION	0x10
+#define BCM2048_FLAG_STEREO_DETECTED		0x20
+#define BCM2048_FLAG_STEREO_ACTIVE		0x40
+
+/* BCM2048_I2C_RDS_DATA */
+#define BCM2048_SLAVE_ADDRESS			0x3f
+#define BCM2048_SLAVE_ENABLE			0x80
+
+/* BCM2048_I2C_FM_BEST_TUNE_MODE */
+#define BCM2048_BEST_TUNE_MODE			0x80
+
+#define BCM2048_FM_FLAG_SEARCH_TUNE_FINISHED	0x01
+#define BCM2048_FM_FLAG_SEARCH_TUNE_FAIL	0x02
+#define BCM2048_FM_FLAG_RSSI_LOW		0x04
+#define BCM2048_FM_FLAG_CARRIER_ERROR_HIGH	0x08
+#define BCM2048_FM_FLAG_AUDIO_PAUSE_INDICATION	0x10
+#define BCM2048_FLAG_STEREO_DETECTED		0x20
+#define BCM2048_FLAG_STEREO_ACTIVE		0x40
+
+#define BCM2048_RDS_FLAG_FIFO_WLINE		0x02
+#define BCM2048_RDS_FLAG_B_BLOCK_MATCH		0x08
+#define BCM2048_RDS_FLAG_SYNC_LOST		0x10
+#define BCM2048_RDS_FLAG_PI_MATCH		0x20
+
+#define BCM2048_RDS_MARK_END_BYTE0		0x7C
+#define BCM2048_RDS_MARK_END_BYTEN		0xFF
+
+#define BCM2048_FM_FLAGS_ALL	(FM_FLAG_SEARCH_TUNE_FINISHED | \
+				 FM_FLAG_SEARCH_TUNE_FAIL | \
+				 FM_FLAG_RSSI_LOW | \
+				 FM_FLAG_CARRIER_ERROR_HIGH | \
+				 FM_FLAG_AUDIO_PAUSE_INDICATION | \
+				 FLAG_STEREO_DETECTED | FLAG_STEREO_ACTIVE)
+
+#define BCM2048_RDS_FLAGS_ALL	(RDS_FLAG_FIFO_WLINE | \
+				 RDS_FLAG_B_BLOCK_MATCH | \
+				 RDS_FLAG_SYNC_LOST | RDS_FLAG_PI_MATCH)
+
+#define BCM2048_DEFAULT_TIMEOUT		1500
+#define BCM2048_AUTO_SEARCH_TIMEOUT	3000
+
+
+#define BCM2048_FREQDEV_UNIT		10000
+#define BCM2048_FREQV4L2_MULTI		625
+#define dev_to_v4l2(f)	((f * BCM2048_FREQDEV_UNIT) / BCM2048_FREQV4L2_MULTI)
+#define v4l2_to_dev(f)	((f * BCM2048_FREQV4L2_MULTI) / BCM2048_FREQDEV_UNIT)
+
+#define msb(x)                  ((u8)((u16) x >> 8))
+#define lsb(x)                  ((u8)((u16) x &  0x00FF))
+#define compose_u16(msb, lsb)	(((u16)msb << 8) | lsb)
+
+#define BCM2048_DEFAULT_POWERING_DELAY	20
+#define BCM2048_DEFAULT_REGION		0x02
+#define BCM2048_DEFAULT_MUTE		0x01
+#define BCM2048_DEFAULT_RSSI_THRESHOLD	0x64
+#define BCM2048_DEFAULT_RDS_WLINE	0x7E
+
+#define BCM2048_FM_SEARCH_INACTIVE	0x00
+#define BCM2048_FM_PRE_SET_MODE		0x01
+#define BCM2048_FM_AUTO_SEARCH_MODE	0x02
+#define BCM2048_FM_AF_JUMP_MODE		0x03
+
+#define BCM2048_FREQUENCY_BASE		64000
+
+#define BCM2048_POWER_ON		0x01
+#define BCM2048_POWER_OFF		0x00
+
+#define BCM2048_ITEM_ENABLED		0x01
+#define BCM2048_SEARCH_DIRECTION_UP	0x01
+
+#define BCM2048_DE_EMPHASIS_75us	75
+#define BCM2048_DE_EMPHASIS_50us	50
+
+#define BCM2048_SCAN_FAIL		0x00
+#define BCM2048_SCAN_OK			0x01
+
+#define BCM2048_FREQ_ERROR_FLOOR	-20
+#define BCM2048_FREQ_ERROR_ROOF		20
+
+/* -60 dB is reported as full signal strenght */
+#define BCM2048_RSSI_LEVEL_BASE		-60
+#define BCM2048_RSSI_LEVEL_ROOF		-100
+#define BCM2048_RSSI_LEVEL_ROOF_NEG	100
+#define BCM2048_SIGNAL_MULTIPLIER	(0xFFFF / \
+					 (BCM2048_RSSI_LEVEL_ROOF_NEG + \
+					  BCM2048_RSSI_LEVEL_BASE))
+
+#define BCM2048_RDS_FIFO_DUPLE_SIZE	0x03
+#define BCM2048_RDS_CRC_MASK		0x0F
+#define BCM2048_RDS_CRC_NONE		0x00
+#define BCM2048_RDS_CRC_MAX_2BITS	0x04
+#define BCM2048_RDS_CRC_LEAST_2BITS	0x08
+#define BCM2048_RDS_CRC_UNRECOVARABLE	0x0C
+
+#define BCM2048_RDS_BLOCK_MASK		0xF0
+#define BCM2048_RDS_BLOCK_A		0x00
+#define BCM2048_RDS_BLOCK_B		0x10
+#define BCM2048_RDS_BLOCK_C		0x20
+#define BCM2048_RDS_BLOCK_D		0x30
+#define BCM2048_RDS_BLOCK_C_SCORED	0x40
+#define BCM2048_RDS_BLOCK_E		0x60
+
+#define BCM2048_RDS_RT			0x20
+#define BCM2048_RDS_PS			0x00
+
+#define BCM2048_RDS_GROUP_AB_MASK	0x08
+#define BCM2048_RDS_GROUP_A		0x00
+#define BCM2048_RDS_GROUP_B		0x08
+
+#define BCM2048_RDS_RT_AB_MASK		0x10
+#define BCM2048_RDS_RT_A		0x00
+#define BCM2048_RDS_RT_B		0x10
+#define BCM2048_RDS_RT_INDEX		0x0F
+
+#define BCM2048_RDS_PS_INDEX		0x03
+
+struct rds_info {
+	u16 rds_pi;
+#define BCM2048_MAX_RDS_RT (64 + 1)
+	u8 rds_rt[BCM2048_MAX_RDS_RT];
+	u8 rds_rt_group_b;
+	u8 rds_rt_ab;
+#define BCM2048_MAX_RDS_PS (8 + 1)
+	u8 rds_ps[BCM2048_MAX_RDS_PS];
+	u8 rds_ps_group;
+	u8 rds_ps_group_cnt;
+#define BCM2048_MAX_RDS_RADIO_TEXT 255
+	u8 radio_text[BCM2048_MAX_RDS_RADIO_TEXT + 3];
+	u8 text_len;
+};
+
+struct region_info {
+	u32 bottom_frequency;
+	u32 top_frequency;
+	u8 deemphasis;
+	u8 channel_spacing;
+	u8 region;
+};
+
+struct bcm2048_device {
+	struct i2c_client *client;
+	struct video_device *videodev;
+	struct work_struct work;
+	struct completion compl;
+	struct mutex mutex;
+	struct bcm2048_platform_data *platform_data;
+	struct rds_info rds_info;
+	struct region_info region_info;
+	u16 frequency;
+	u8 cache_fm_rds_system;
+	u8 cache_fm_ctrl;
+	u8 cache_fm_audio_ctrl0;
+	u8 cache_fm_search_ctrl0;
+	u8 power_state;
+	u8 rds_state;
+	u8 fifo_size;
+	u8 scan_state;
+	u8 mute_state;
+
+	/* for rds data device read */
+	wait_queue_head_t read_queue;
+	unsigned int users;
+	unsigned char rds_data_available;
+	unsigned int rd_index;
+};
+
+static int radio_nr = -1;	/* radio device minor (-1 ==> auto assign) */
+module_param(radio_nr, int, 0);
+MODULE_PARM_DESC(radio_nr,
+		 "Minor number for radio device (-1 ==> auto assign)");
+
+static struct region_info region_configs[] = {
+	/* USA */
+	{
+		.channel_spacing	= 20,
+		.bottom_frequency	= 87500,
+		.top_frequency		= 108000,
+		.deemphasis		= 75,
+		.region			= 0,
+	},
+	/* Australia */
+	{
+		.channel_spacing	= 20,
+		.bottom_frequency	= 87500,
+		.top_frequency		= 108000,
+		.deemphasis		= 50,
+		.region			= 1,
+	},
+	/* Europe */
+	{
+		.channel_spacing	= 10,
+		.bottom_frequency	= 87500,
+		.top_frequency		= 108000,
+		.deemphasis		= 50,
+		.region			= 2,
+	},
+	/* Japan */
+	{
+		.channel_spacing	= 10,
+		.bottom_frequency	= 76000,
+		.top_frequency		= 90000,
+		.deemphasis		= 50,
+		.region			= 3,
+	},
+	/* Japan wide band */
+	{
+		.channel_spacing	= 10,
+		.bottom_frequency	= 76000,
+		.top_frequency		= 108000,
+		.deemphasis		= 50,
+		.region			= 4,
+	},
+};
+
+/*
+ *	I2C Interface read / write
+ */
+static int bcm2048_send_command(struct bcm2048_device *bdev, unsigned int reg,
+					unsigned int value)
+{
+	struct i2c_client *client = bdev->client;
+	u8 data[2];
+
+	if (!bdev->power_state) {
+		dev_err(&bdev->client->dev, "bcm2048: chip not powered!\n");
+		return -EIO;
+	}
+
+	data[0] = reg & 0xff;
+	data[1] = value & 0xff;
+
+	if (i2c_master_send(client, data, 2) == 2) {
+		return 0;
+	} else {
+		dev_err(&bdev->client->dev, "BCM I2C error!\n");
+		dev_err(&bdev->client->dev, "Is Bluetooth up and running?\n");
+		return -EIO;
+	}
+}
+
+static int bcm2048_recv_command(struct bcm2048_device *bdev, unsigned int reg,
+			u8 *value)
+{
+	struct i2c_client *client = bdev->client;
+
+	if (!bdev->power_state) {
+		dev_err(&bdev->client->dev, "bcm2048: chip not powered!\n");
+		return -EIO;
+	}
+
+	value[0] = i2c_smbus_read_byte_data(client, reg & 0xff);
+
+	return 0;
+}
+
+static int bcm2048_recv_duples(struct bcm2048_device *bdev, unsigned int reg,
+			u8 *value, u8 duples)
+{
+	struct i2c_client *client = bdev->client;
+	struct i2c_adapter *adap = client->adapter;
+	struct i2c_msg msg[2];
+	u8 buf;
+
+	if (!bdev->power_state) {
+		dev_err(&bdev->client->dev, "bcm2048: chip not powered!\n");
+		return -EIO;
+	}
+
+	buf = reg & 0xff;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = client->flags & I2C_M_TEN;
+	msg[0].len = 1;
+	msg[0].buf = &buf;
+
+	msg[1].addr = client->addr;
+	msg[1].flags = client->flags & I2C_M_TEN;
+	msg[1].flags |= I2C_M_RD;
+	msg[1].len = duples;
+	msg[1].buf = value;
+
+	return i2c_transfer(adap, msg, 2);
+}
+
+/*
+ *	BCM2048 - I2C register programming helpers
+ */
+static int bcm2048_set_power_state(struct bcm2048_device *bdev, u8 power)
+{
+	int err = 0;
+
+	mutex_lock(&bdev->mutex);
+
+	if (power) {
+		bdev->power_state = BCM2048_POWER_ON;
+		bdev->cache_fm_rds_system |= BCM2048_FM_ON;
+	} else {
+		bdev->cache_fm_rds_system &= ~BCM2048_FM_ON;
+	}
+
+	/*
+	 * Warning! FM cannot be turned off because then
+	 * the I2C communications get ruined!
+	 * Comment off the "if (power)" when the chip works!
+	 */
+	if (power)
+		err = bcm2048_send_command(bdev, BCM2048_I2C_FM_RDS_SYSTEM,
+					bdev->cache_fm_rds_system);
+	msleep(BCM2048_DEFAULT_POWERING_DELAY);
+
+	if (!power)
+		bdev->power_state = BCM2048_POWER_OFF;
+
+	mutex_unlock(&bdev->mutex);
+	return err;
+}
+
+static int bcm2048_get_power_state(struct bcm2048_device *bdev)
+{
+	int err;
+	u8 value;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_recv_command(bdev, BCM2048_I2C_FM_RDS_SYSTEM, &value);
+
+	mutex_unlock(&bdev->mutex);
+
+	if (!err && (value & BCM2048_FM_ON))
+		return BCM2048_POWER_ON;
+
+	return err;
+}
+
+static int bcm2048_set_rds_no_lock(struct bcm2048_device *bdev, u8 rds_on)
+{
+	int err;
+	u8 flags;
+
+	bdev->cache_fm_rds_system &= ~BCM2048_RDS_ON;
+
+	if (rds_on) {
+		bdev->cache_fm_rds_system |= BCM2048_RDS_ON;
+		bdev->rds_state = BCM2048_RDS_ON;
+		flags =	BCM2048_RDS_FLAG_FIFO_WLINE;
+		err = bcm2048_send_command(bdev, BCM2048_I2C_FM_RDS_MASK1,
+						flags);
+	} else {
+		flags =	0;
+		bdev->rds_state = 0;
+		err = bcm2048_send_command(bdev, BCM2048_I2C_FM_RDS_MASK1,
+						flags);
+		memset(&bdev->rds_info, 0, sizeof(bdev->rds_info));
+	}
+
+	err = bcm2048_send_command(bdev, BCM2048_I2C_FM_RDS_SYSTEM,
+					bdev->cache_fm_rds_system);
+
+	return err;
+}
+
+static int bcm2048_get_rds_no_lock(struct bcm2048_device *bdev)
+{
+	int err;
+	u8 value;
+
+	err = bcm2048_recv_command(bdev, BCM2048_I2C_FM_RDS_SYSTEM, &value);
+
+	if (!err && (value & BCM2048_RDS_ON))
+		return BCM2048_ITEM_ENABLED;
+
+	return err;
+}
+
+static int bcm2048_set_rds(struct bcm2048_device *bdev, u8 rds_on)
+{
+	int err;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_set_rds_no_lock(bdev, rds_on);
+
+	mutex_unlock(&bdev->mutex);
+	return err;
+}
+
+static int bcm2048_get_rds(struct bcm2048_device *bdev)
+{
+	int err;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_get_rds_no_lock(bdev);
+
+	mutex_unlock(&bdev->mutex);
+	return err;
+}
+
+static int bcm2048_get_rds_pi(struct bcm2048_device *bdev)
+{
+	return bdev->rds_info.rds_pi;
+}
+
+static int bcm2048_set_fm_automatic_stereo_mono(struct bcm2048_device *bdev,
+						u8 enabled)
+{
+	int err;
+
+	mutex_lock(&bdev->mutex);
+
+	bdev->cache_fm_ctrl &= ~BCM2048_STEREO_MONO_AUTO_SELECT;
+
+	if (enabled)
+		bdev->cache_fm_ctrl |= BCM2048_STEREO_MONO_AUTO_SELECT;
+
+	err = bcm2048_send_command(bdev, BCM2048_I2C_FM_CTRL,
+					bdev->cache_fm_ctrl);
+
+	mutex_unlock(&bdev->mutex);
+	return err;
+}
+
+static int bcm2048_set_fm_hi_lo_injection(struct bcm2048_device *bdev,
+						u8 hi_lo)
+{
+	int err;
+
+	mutex_lock(&bdev->mutex);
+
+	bdev->cache_fm_ctrl &= ~BCM2048_HI_LO_INJECTION;
+
+	if (hi_lo)
+		bdev->cache_fm_ctrl |= BCM2048_HI_LO_INJECTION;
+
+	err = bcm2048_send_command(bdev, BCM2048_I2C_FM_CTRL,
+					bdev->cache_fm_ctrl);
+
+	mutex_unlock(&bdev->mutex);
+	return err;
+}
+
+static int bcm2048_get_fm_hi_lo_injection(struct bcm2048_device *bdev)
+{
+	int err;
+	u8 value;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_recv_command(bdev, BCM2048_I2C_FM_CTRL, &value);
+
+	mutex_unlock(&bdev->mutex);
+
+	if (!err && (value & BCM2048_HI_LO_INJECTION))
+		return BCM2048_ITEM_ENABLED;
+
+	return err;
+}
+
+static int bcm2048_set_fm_frequency(struct bcm2048_device *bdev, u32 frequency)
+{
+	int err;
+
+	if (frequency < bdev->region_info.bottom_frequency ||
+		frequency > bdev->region_info.top_frequency)
+		return -EDOM;
+
+	frequency -= BCM2048_FREQUENCY_BASE;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_send_command(bdev, BCM2048_I2C_FM_FREQ0, lsb(frequency));
+	err |= bcm2048_send_command(bdev, BCM2048_I2C_FM_FREQ1,
+					msb(frequency));
+
+	if (!err)
+		bdev->frequency = frequency;
+
+	mutex_unlock(&bdev->mutex);
+	return err;
+}
+
+static int bcm2048_get_fm_frequency(struct bcm2048_device *bdev)
+{
+	int err;
+	u8 lsb, msb;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_recv_command(bdev, BCM2048_I2C_FM_FREQ0, &lsb);
+	err |= bcm2048_recv_command(bdev, BCM2048_I2C_FM_FREQ1, &msb);
+
+	mutex_unlock(&bdev->mutex);
+
+	if (err)
+		return err;
+
+	err = compose_u16(msb, lsb);
+	err += BCM2048_FREQUENCY_BASE;
+
+	return err;
+}
+
+static int bcm2048_set_fm_af_frequency(struct bcm2048_device *bdev,
+						u32 frequency)
+{
+	int err;
+
+	if (frequency < bdev->region_info.bottom_frequency ||
+		frequency > bdev->region_info.top_frequency)
+		return -EDOM;
+
+	frequency -= BCM2048_FREQUENCY_BASE;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_send_command(bdev, BCM2048_I2C_FM_AF_FREQ0,
+					lsb(frequency));
+	err |= bcm2048_send_command(bdev, BCM2048_I2C_FM_AF_FREQ1,
+					msb(frequency));
+	if (!err)
+		bdev->frequency = frequency;
+
+	mutex_unlock(&bdev->mutex);
+	return err;
+}
+
+static int bcm2048_get_fm_af_frequency(struct bcm2048_device *bdev)
+{
+	int err;
+	u8 lsb, msb;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_recv_command(bdev, BCM2048_I2C_FM_AF_FREQ0, &lsb);
+	err |= bcm2048_recv_command(bdev, BCM2048_I2C_FM_AF_FREQ1, &msb);
+
+	mutex_unlock(&bdev->mutex);
+
+	if (err)
+		return err;
+
+	err = compose_u16(msb, lsb);
+	err += BCM2048_FREQUENCY_BASE;
+
+	return err;
+}
+
+static int bcm2048_set_fm_deemphasis(struct bcm2048_device *bdev, int d)
+{
+	int err;
+	u8 deemphasis;
+
+	if (d == BCM2048_DE_EMPHASIS_75us)
+		deemphasis = BCM2048_DE_EMPHASIS_SELECT;
+	else
+		deemphasis = 0;
+
+	mutex_lock(&bdev->mutex);
+
+	bdev->cache_fm_audio_ctrl0 &= ~BCM2048_DE_EMPHASIS_SELECT;
+	bdev->cache_fm_audio_ctrl0 |= deemphasis;
+
+	err = bcm2048_send_command(bdev, BCM2048_I2C_FM_AUDIO_CTRL0,
+		bdev->cache_fm_audio_ctrl0);
+
+	if (!err)
+		bdev->region_info.deemphasis = d;
+
+	mutex_unlock(&bdev->mutex);
+
+	return err;
+}
+
+static int bcm2048_get_fm_deemphasis(struct bcm2048_device *bdev)
+{
+	int err;
+	u8 value;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_recv_command(bdev, BCM2048_I2C_FM_AUDIO_CTRL0, &value);
+
+	mutex_unlock(&bdev->mutex);
+
+	if (!err) {
+		if (value & BCM2048_DE_EMPHASIS_SELECT)
+			return BCM2048_DE_EMPHASIS_75us;
+		else
+			return BCM2048_DE_EMPHASIS_50us;
+	}
+
+	return err;
+}
+
+static int bcm2048_set_region(struct bcm2048_device *bdev, u8 region)
+{
+	int err;
+	u32 new_frequency = 0;
+
+	if (region > ARRAY_SIZE(region_configs))
+		return -EINVAL;
+
+	mutex_lock(&bdev->mutex);
+	bdev->region_info = region_configs[region];
+	mutex_unlock(&bdev->mutex);
+
+	if (bdev->frequency < region_configs[region].bottom_frequency ||
+		bdev->frequency > region_configs[region].top_frequency)
+		new_frequency = region_configs[region].bottom_frequency;
+
+	if (new_frequency > 0) {
+		err = bcm2048_set_fm_frequency(bdev, new_frequency);
+
+		if (err)
+			goto done;
+	}
+
+	err = bcm2048_set_fm_deemphasis(bdev,
+			region_configs[region].deemphasis);
+
+done:
+	return err;
+}
+
+static int bcm2048_get_region(struct bcm2048_device *bdev)
+{
+	int err;
+
+	mutex_lock(&bdev->mutex);
+	err = bdev->region_info.region;
+	mutex_unlock(&bdev->mutex);
+
+	return err;
+}
+
+static int bcm2048_set_mute(struct bcm2048_device *bdev, u16 mute)
+{
+	int err;
+
+	mutex_lock(&bdev->mutex);
+
+	bdev->cache_fm_audio_ctrl0 &= ~(BCM2048_RF_MUTE | BCM2048_MANUAL_MUTE);
+
+	if (mute)
+		bdev->cache_fm_audio_ctrl0 |= (BCM2048_RF_MUTE |
+						BCM2048_MANUAL_MUTE);
+
+	err = bcm2048_send_command(bdev, BCM2048_I2C_FM_AUDIO_CTRL0,
+					bdev->cache_fm_audio_ctrl0);
+
+	if (!err)
+		bdev->mute_state = mute;
+
+	mutex_unlock(&bdev->mutex);
+	return err;
+}
+
+static int bcm2048_get_mute(struct bcm2048_device *bdev)
+{
+	int err;
+	u8 value;
+
+	mutex_lock(&bdev->mutex);
+
+	if (bdev->power_state) {
+		err = bcm2048_recv_command(bdev, BCM2048_I2C_FM_AUDIO_CTRL0,
+						&value);
+		if (!err)
+			err = value & (BCM2048_RF_MUTE | BCM2048_MANUAL_MUTE);
+	} else {
+		err = bdev->mute_state;
+	}
+
+	mutex_unlock(&bdev->mutex);
+	return err;
+}
+
+static int bcm2048_set_audio_route(struct bcm2048_device *bdev, u8 route)
+{
+	int err;
+
+	mutex_lock(&bdev->mutex);
+
+	route &= (BCM2048_AUDIO_ROUTE_DAC | BCM2048_AUDIO_ROUTE_I2S);
+	bdev->cache_fm_audio_ctrl0 &= ~(BCM2048_AUDIO_ROUTE_DAC |
+		BCM2048_AUDIO_ROUTE_I2S);
+	bdev->cache_fm_audio_ctrl0 |= route;
+
+	err = bcm2048_send_command(bdev, BCM2048_I2C_FM_AUDIO_CTRL0,
+					bdev->cache_fm_audio_ctrl0);
+
+	mutex_unlock(&bdev->mutex);
+	return err;
+}
+
+static int bcm2048_get_audio_route(struct bcm2048_device *bdev)
+{
+	int err;
+	u8 value;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_recv_command(bdev, BCM2048_I2C_FM_AUDIO_CTRL0, &value);
+
+	mutex_unlock(&bdev->mutex);
+
+	if (!err)
+		return value & (BCM2048_AUDIO_ROUTE_DAC |
+			BCM2048_AUDIO_ROUTE_I2S);
+
+	return err;
+}
+
+static int bcm2048_set_dac_output(struct bcm2048_device *bdev, u8 channels)
+{
+	int err;
+
+	mutex_lock(&bdev->mutex);
+
+	bdev->cache_fm_audio_ctrl0 &= ~(BCM2048_DAC_OUTPUT_LEFT |
+					BCM2048_DAC_OUTPUT_RIGHT);
+	bdev->cache_fm_audio_ctrl0 |= channels;
+
+	err = bcm2048_send_command(bdev, BCM2048_I2C_FM_AUDIO_CTRL0,
+					bdev->cache_fm_audio_ctrl0);
+
+	mutex_unlock(&bdev->mutex);
+	return err;
+}
+
+static int bcm2048_get_dac_output(struct bcm2048_device *bdev)
+{
+	int err;
+	u8 value;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_recv_command(bdev, BCM2048_I2C_FM_AUDIO_CTRL0, &value);
+
+	mutex_unlock(&bdev->mutex);
+
+	if (!err)
+		return value & (BCM2048_DAC_OUTPUT_LEFT |
+			BCM2048_DAC_OUTPUT_RIGHT);
+
+	return err;
+}
+
+static int bcm2048_set_fm_search_rssi_threshold(struct bcm2048_device *bdev,
+							u8 threshold)
+{
+	int err;
+
+	mutex_lock(&bdev->mutex);
+
+	threshold &= BCM2048_SEARCH_RSSI_THRESHOLD;
+	bdev->cache_fm_search_ctrl0 &= ~BCM2048_SEARCH_RSSI_THRESHOLD;
+	bdev->cache_fm_search_ctrl0 |= threshold;
+
+	err = bcm2048_send_command(bdev, BCM2048_I2C_FM_SEARCH_CTRL0,
+					bdev->cache_fm_search_ctrl0);
+
+	mutex_unlock(&bdev->mutex);
+	return err;
+}
+
+static int bcm2048_get_fm_search_rssi_threshold(struct bcm2048_device *bdev)
+{
+	int err;
+	u8 value;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_recv_command(bdev, BCM2048_I2C_FM_SEARCH_CTRL0, &value);
+
+	mutex_unlock(&bdev->mutex);
+
+	if (!err)
+		return value & BCM2048_SEARCH_RSSI_THRESHOLD;
+
+	return err;
+}
+
+static int bcm2048_set_fm_search_mode_direction(struct bcm2048_device *bdev,
+						u8 direction)
+{
+	int err;
+
+	mutex_lock(&bdev->mutex);
+
+	bdev->cache_fm_search_ctrl0 &= ~BCM2048_SEARCH_DIRECTION;
+
+	if (direction)
+		bdev->cache_fm_search_ctrl0 |= BCM2048_SEARCH_DIRECTION;
+
+	err = bcm2048_send_command(bdev, BCM2048_I2C_FM_SEARCH_CTRL0,
+					bdev->cache_fm_search_ctrl0);
+
+	mutex_unlock(&bdev->mutex);
+	return err;
+}
+
+static int bcm2048_get_fm_search_mode_direction(struct bcm2048_device *bdev)
+{
+	int err;
+	u8 value;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_recv_command(bdev, BCM2048_I2C_FM_SEARCH_CTRL0, &value);
+
+	mutex_unlock(&bdev->mutex);
+
+	if (!err && (value & BCM2048_SEARCH_DIRECTION))
+		return BCM2048_SEARCH_DIRECTION_UP;
+
+	return err;
+}
+
+static int bcm2048_set_fm_search_tune_mode(struct bcm2048_device *bdev,
+						u8 mode)
+{
+	int err, timeout, restart_rds = 0;
+	u8 value, flags;
+
+	value = mode & BCM2048_FM_AUTO_SEARCH;
+
+	flags =	BCM2048_FM_FLAG_SEARCH_TUNE_FINISHED |
+		BCM2048_FM_FLAG_SEARCH_TUNE_FAIL;
+
+	mutex_lock(&bdev->mutex);
+
+	/*
+	 * If RDS is enabled, and frequency is changed, RDS quits working.
+	 * Thus, always restart RDS if it's enabled. Moreover, RDS must
+	 * not be enabled while changing the frequency because it can
+	 * provide a race to the mutex from the workqueue handler if RDS
+	 * IRQ occurs while waiting for frequency changed IRQ.
+	 */
+	if (bcm2048_get_rds_no_lock(bdev)) {
+		err = bcm2048_set_rds_no_lock(bdev, 0);
+		if (err)
+			goto unlock;
+		restart_rds = 1;
+	}
+
+	err = bcm2048_send_command(bdev, BCM2048_I2C_FM_RDS_MASK0, flags);
+
+	if (err)
+		goto unlock;
+
+	bcm2048_send_command(bdev, BCM2048_I2C_FM_SEARCH_TUNE_MODE, value);
+
+	if (mode != BCM2048_FM_AUTO_SEARCH_MODE)
+		timeout = BCM2048_DEFAULT_TIMEOUT;
+	else
+		timeout = BCM2048_AUTO_SEARCH_TIMEOUT;
+
+	if (!wait_for_completion_timeout(&bdev->compl,
+			msecs_to_jiffies(timeout)))
+			dev_err(&bdev->client->dev, "IRQ timeout.\n");
+
+	if (value)
+		if (!bdev->scan_state)
+			err = -EIO;
+
+unlock:
+	if (restart_rds)
+		err |= bcm2048_set_rds_no_lock(bdev, 1);
+
+	mutex_unlock(&bdev->mutex);
+
+	return err;
+}
+
+static int bcm2048_get_fm_search_tune_mode(struct bcm2048_device *bdev)
+{
+	int err;
+	u8 value;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_recv_command(bdev, BCM2048_I2C_FM_SEARCH_TUNE_MODE,
+					&value);
+
+	mutex_unlock(&bdev->mutex);
+
+	if (!err)
+		return value & BCM2048_FM_AUTO_SEARCH;
+
+	return err;
+}
+
+static int bcm2048_set_rds_b_block_mask(struct bcm2048_device *bdev, u16 mask)
+{
+	int err;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_send_command(bdev,
+			BCM2048_I2C_RDS_BLKB_MASK0, lsb(mask));
+	err |= bcm2048_send_command(bdev,
+			BCM2048_I2C_RDS_BLKB_MASK1, msb(mask));
+
+	mutex_unlock(&bdev->mutex);
+	return err;
+}
+
+static int bcm2048_get_rds_b_block_mask(struct bcm2048_device *bdev)
+{
+	int err;
+	u8 lsb, msb;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_recv_command(bdev,
+			BCM2048_I2C_RDS_BLKB_MASK0, &lsb);
+	err |= bcm2048_recv_command(bdev,
+			BCM2048_I2C_RDS_BLKB_MASK1, &msb);
+
+	mutex_unlock(&bdev->mutex);
+
+	if (!err)
+		return compose_u16(msb, lsb);
+
+	return err;
+}
+
+static int bcm2048_set_rds_b_block_match(struct bcm2048_device *bdev,
+						u16 match)
+{
+	int err;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_send_command(bdev,
+			BCM2048_I2C_RDS_BLKB_MATCH0, lsb(match));
+	err |= bcm2048_send_command(bdev,
+			BCM2048_I2C_RDS_BLKB_MATCH1, msb(match));
+
+	mutex_unlock(&bdev->mutex);
+	return err;
+}
+
+static int bcm2048_get_rds_b_block_match(struct bcm2048_device *bdev)
+{
+	int err;
+	u8 lsb, msb;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_recv_command(bdev,
+			BCM2048_I2C_RDS_BLKB_MATCH0, &lsb);
+	err |= bcm2048_recv_command(bdev,
+			BCM2048_I2C_RDS_BLKB_MATCH1, &msb);
+
+	mutex_unlock(&bdev->mutex);
+
+	if (!err)
+		return compose_u16(msb, lsb);
+
+	return err;
+}
+
+static int bcm2048_set_rds_pi_mask(struct bcm2048_device *bdev, u16 mask)
+{
+	int err;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_send_command(bdev,
+			BCM2048_I2C_RDS_PI_MASK0, lsb(mask));
+	err |= bcm2048_send_command(bdev,
+			BCM2048_I2C_RDS_PI_MASK1, msb(mask));
+
+	mutex_unlock(&bdev->mutex);
+	return err;
+}
+
+static int bcm2048_get_rds_pi_mask(struct bcm2048_device *bdev)
+{
+	int err;
+	u8 lsb, msb;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_recv_command(bdev,
+			BCM2048_I2C_RDS_PI_MASK0, &lsb);
+	err |= bcm2048_recv_command(bdev,
+			BCM2048_I2C_RDS_PI_MASK1, &msb);
+
+	mutex_unlock(&bdev->mutex);
+
+	if (!err)
+		return compose_u16(msb, lsb);
+
+	return err;
+}
+
+static int bcm2048_set_rds_pi_match(struct bcm2048_device *bdev, u16 match)
+{
+	int err;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_send_command(bdev,
+			BCM2048_I2C_RDS_PI_MATCH0, lsb(match));
+	err |= bcm2048_send_command(bdev,
+			BCM2048_I2C_RDS_PI_MATCH1, msb(match));
+
+	mutex_unlock(&bdev->mutex);
+	return err;
+}
+
+static int bcm2048_get_rds_pi_match(struct bcm2048_device *bdev)
+{
+	int err;
+	u8 lsb, msb;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_recv_command(bdev,
+			BCM2048_I2C_RDS_PI_MATCH0, &lsb);
+	err |= bcm2048_recv_command(bdev,
+			BCM2048_I2C_RDS_PI_MATCH1, &msb);
+
+	mutex_unlock(&bdev->mutex);
+
+	if (!err)
+		return compose_u16(msb, lsb);
+
+	return err;
+}
+
+static int bcm2048_set_fm_rds_mask(struct bcm2048_device *bdev, u16 mask)
+{
+	int err;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_send_command(bdev,
+			BCM2048_I2C_FM_RDS_MASK0, lsb(mask));
+	err |= bcm2048_send_command(bdev,
+			BCM2048_I2C_FM_RDS_MASK1, msb(mask));
+
+	mutex_unlock(&bdev->mutex);
+	return err;
+}
+
+static int bcm2048_get_fm_rds_mask(struct bcm2048_device *bdev)
+{
+	int err;
+	u8 value0, value1;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_recv_command(bdev, BCM2048_I2C_FM_RDS_MASK0, &value0);
+	err |= bcm2048_recv_command(bdev, BCM2048_I2C_FM_RDS_MASK1, &value1);
+
+	mutex_unlock(&bdev->mutex);
+
+	if (!err)
+		return compose_u16(value1, value0);
+
+	return err;
+}
+
+static int bcm2048_get_fm_rds_flags(struct bcm2048_device *bdev)
+{
+	int err;
+	u8 value0, value1;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_recv_command(bdev, BCM2048_I2C_FM_RDS_FLAG0, &value0);
+	err |= bcm2048_recv_command(bdev, BCM2048_I2C_FM_RDS_FLAG1, &value1);
+
+	mutex_unlock(&bdev->mutex);
+
+	if (!err)
+		return compose_u16(value1, value0);
+
+	return err;
+}
+
+static int bcm2048_get_region_bottom_frequency(struct bcm2048_device *bdev)
+{
+	return bdev->region_info.bottom_frequency;
+}
+
+static int bcm2048_get_region_top_frequency(struct bcm2048_device *bdev)
+{
+	return bdev->region_info.top_frequency;
+}
+
+static int bcm2048_set_fm_best_tune_mode(struct bcm2048_device *bdev, u8 mode)
+{
+	int err;
+	u8 value;
+
+	mutex_lock(&bdev->mutex);
+
+	/* Perform read as the manual indicates */
+	err = bcm2048_recv_command(bdev, BCM2048_I2C_FM_BEST_TUNE_MODE,
+					&value);
+	value &= ~BCM2048_BEST_TUNE_MODE;
+
+	if (mode)
+		value |= BCM2048_BEST_TUNE_MODE;
+	err |= bcm2048_send_command(bdev, BCM2048_I2C_FM_BEST_TUNE_MODE,
+					value);
+
+	mutex_unlock(&bdev->mutex);
+	return err;
+}
+
+static int bcm2048_get_fm_best_tune_mode(struct bcm2048_device *bdev)
+{
+	int err;
+	u8 value;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_recv_command(bdev, BCM2048_I2C_FM_BEST_TUNE_MODE,
+					&value);
+
+	mutex_unlock(&bdev->mutex);
+
+	if (!err && (value & BCM2048_BEST_TUNE_MODE))
+		return BCM2048_ITEM_ENABLED;
+
+	return err;
+}
+
+static int bcm2048_get_fm_carrier_error(struct bcm2048_device *bdev)
+{
+	int err = 0;
+	s8 value;
+
+	mutex_lock(&bdev->mutex);
+	err = bcm2048_recv_command(bdev, BCM2048_I2C_FM_CARRIER, &value);
+	mutex_unlock(&bdev->mutex);
+
+	if (!err)
+		return value;
+
+	return err;
+}
+
+static int bcm2048_get_fm_rssi(struct bcm2048_device *bdev)
+{
+	int err;
+	s8 value;
+
+	mutex_lock(&bdev->mutex);
+	err = bcm2048_recv_command(bdev, BCM2048_I2C_FM_RSSI, &value);
+	mutex_unlock(&bdev->mutex);
+
+	if (!err)
+		return value;
+
+	return err;
+}
+
+static int bcm2048_set_rds_wline(struct bcm2048_device *bdev, u8 wline)
+{
+	int err;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_send_command(bdev, BCM2048_I2C_RDS_WLINE, wline);
+
+	if (!err)
+		bdev->fifo_size = wline;
+
+	mutex_unlock(&bdev->mutex);
+	return err;
+}
+
+static int bcm2048_get_rds_wline(struct bcm2048_device *bdev)
+{
+	int err;
+	u8 value;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_recv_command(bdev, BCM2048_I2C_RDS_WLINE, &value);
+
+	mutex_unlock(&bdev->mutex);
+
+	if (!err) {
+		bdev->fifo_size = value;
+		return value;
+	}
+
+	return err;
+}
+
+static int bcm2048_checkrev(struct bcm2048_device *bdev)
+{
+	int err;
+	u8 version;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_recv_command(bdev, BCM2048_I2C_FM_RDS_REV, &version);
+
+	mutex_unlock(&bdev->mutex);
+
+	if (!err) {
+		dev_info(&bdev->client->dev, "BCM2048 Version 0x%x\n",
+			version);
+		return version;
+	}
+
+	return err;
+}
+
+static int bcm2048_get_rds_rt(struct bcm2048_device *bdev, char *data)
+{
+	int err = 0, i, j = 0, ce = 0, cr = 0;
+	char data_buffer[BCM2048_MAX_RDS_RT+1];
+
+	mutex_lock(&bdev->mutex);
+
+	if (!bdev->rds_info.text_len) {
+		err = -EINVAL;
+		goto unlock;
+	}
+
+	for (i = 0; i < BCM2048_MAX_RDS_RT; i++) {
+		if (bdev->rds_info.rds_rt[i]) {
+			ce = i;
+			/* Skip the carriage return */
+			if (bdev->rds_info.rds_rt[i] != 0x0d) {
+				data_buffer[j++] = bdev->rds_info.rds_rt[i];
+			} else {
+				cr = i;
+				break;
+			}
+		}
+	}
+
+	if (j <= BCM2048_MAX_RDS_RT)
+		data_buffer[j] = 0;
+
+	for (i = 0; i < BCM2048_MAX_RDS_RT; i++) {
+		if (!bdev->rds_info.rds_rt[i]) {
+			if (cr && (i < cr)) {
+				err = -EBUSY;
+				goto unlock;
+			}
+			if (i < ce) {
+				if (cr && (i >= cr))
+					break;
+				err = -EBUSY;
+				goto unlock;
+			}
+		}
+	}
+
+	memcpy(data, data_buffer, sizeof(data_buffer));
+
+unlock:
+	mutex_unlock(&bdev->mutex);
+	return err;
+}
+
+static int bcm2048_get_rds_ps(struct bcm2048_device *bdev, char *data)
+{
+	int err = 0, i, j = 0;
+	char data_buffer[BCM2048_MAX_RDS_PS+1];
+
+	mutex_lock(&bdev->mutex);
+
+	if (!bdev->rds_info.text_len) {
+		err = -EINVAL;
+		goto unlock;
+	}
+
+	for (i = 0; i < BCM2048_MAX_RDS_PS; i++) {
+		if (bdev->rds_info.rds_ps[i]) {
+			data_buffer[j++] = bdev->rds_info.rds_ps[i];
+		} else {
+			if (i < (BCM2048_MAX_RDS_PS - 1)) {
+				err = -EBUSY;
+				goto unlock;
+			}
+		}
+	}
+
+	if (j <= BCM2048_MAX_RDS_PS)
+		data_buffer[j] = 0;
+
+	memcpy(data, data_buffer, sizeof(data_buffer));
+
+unlock:
+	mutex_unlock(&bdev->mutex);
+	return err;
+}
+
+static void bcm2048_parse_rds_pi(struct bcm2048_device *bdev)
+{
+	int i, cnt = 0;
+	u16 pi;
+
+	for (i = 0; i < bdev->fifo_size; i += BCM2048_RDS_FIFO_DUPLE_SIZE) {
+
+		/* Block A match, only data without crc errors taken */
+		if (bdev->rds_info.radio_text[i] == BCM2048_RDS_BLOCK_A) {
+
+			pi = ((bdev->rds_info.radio_text[i+1] << 8) +
+				bdev->rds_info.radio_text[i+2]);
+
+			if (!bdev->rds_info.rds_pi) {
+				bdev->rds_info.rds_pi = pi;
+				return;
+			}
+			if (pi != bdev->rds_info.rds_pi) {
+				cnt++;
+				if (cnt > 3) {
+					bdev->rds_info.rds_pi = pi;
+					cnt = 0;
+				}
+			} else {
+				cnt = 0;
+			}
+		}
+	}
+}
+
+static int bcm2048_rds_block_crc(struct bcm2048_device *bdev, int i)
+{
+	return bdev->rds_info.radio_text[i] & BCM2048_RDS_CRC_MASK;
+}
+
+static void bcm2048_parse_rds_rt_block(struct bcm2048_device *bdev, int i,
+					int index, int crc)
+{
+	/* Good data will overwrite poor data */
+	if (crc) {
+		if (!bdev->rds_info.rds_rt[index])
+			bdev->rds_info.rds_rt[index] =
+				bdev->rds_info.radio_text[i+1];
+		if (!bdev->rds_info.rds_rt[index+1])
+			bdev->rds_info.rds_rt[index+1] =
+				bdev->rds_info.radio_text[i+2];
+	} else {
+		bdev->rds_info.rds_rt[index] = bdev->rds_info.radio_text[i+1];
+		bdev->rds_info.rds_rt[index+1] =
+			bdev->rds_info.radio_text[i+2];
+	}
+}
+
+static int bcm2048_parse_rt_match_b(struct bcm2048_device *bdev, int i)
+{
+	int crc, rt_id, rt_group_b, rt_ab, index = 0;
+
+	crc = bcm2048_rds_block_crc(bdev, i);
+
+	if (crc == BCM2048_RDS_CRC_UNRECOVARABLE)
+		return -EIO;
+
+	if ((bdev->rds_info.radio_text[i] & BCM2048_RDS_BLOCK_MASK) ==
+		BCM2048_RDS_BLOCK_B) {
+
+		rt_id = (bdev->rds_info.radio_text[i+1] &
+			BCM2048_RDS_BLOCK_MASK);
+		rt_group_b = bdev->rds_info.radio_text[i+1] &
+			BCM2048_RDS_GROUP_AB_MASK;
+		rt_ab = bdev->rds_info.radio_text[i+2] &
+				BCM2048_RDS_RT_AB_MASK;
+
+		if (rt_group_b != bdev->rds_info.rds_rt_group_b) {
+			memset(bdev->rds_info.rds_rt, 0,
+				sizeof(bdev->rds_info.rds_rt));
+			bdev->rds_info.rds_rt_group_b = rt_group_b;
+		}
+
+		if (rt_id == BCM2048_RDS_RT) {
+			/* A to B or (vice versa), means: clear screen */
+			if (rt_ab != bdev->rds_info.rds_rt_ab) {
+				memset(bdev->rds_info.rds_rt, 0,
+					sizeof(bdev->rds_info.rds_rt));
+				bdev->rds_info.rds_rt_ab = rt_ab;
+			}
+
+			index = bdev->rds_info.radio_text[i+2] &
+					BCM2048_RDS_RT_INDEX;
+
+			if (bdev->rds_info.rds_rt_group_b)
+				index <<= 1;
+			else
+				index <<= 2;
+
+			return index;
+		}
+	}
+
+	return -EIO;
+}
+
+static int bcm2048_parse_rt_match_c(struct bcm2048_device *bdev, int i,
+					int index)
+{
+	int crc;
+
+	crc = bcm2048_rds_block_crc(bdev, i);
+
+	if (crc == BCM2048_RDS_CRC_UNRECOVARABLE)
+		return 0;
+
+	BUG_ON((index+2) >= BCM2048_MAX_RDS_RT);
+
+	if ((bdev->rds_info.radio_text[i] & BCM2048_RDS_BLOCK_MASK) ==
+		BCM2048_RDS_BLOCK_C) {
+		if (bdev->rds_info.rds_rt_group_b)
+			return 1;
+		bcm2048_parse_rds_rt_block(bdev, i, index, crc);
+		return 1;
+	}
+
+	return 0;
+}
+
+static void bcm2048_parse_rt_match_d(struct bcm2048_device *bdev, int i,
+					int index)
+{
+	int crc;
+
+	crc = bcm2048_rds_block_crc(bdev, i);
+
+	if (crc == BCM2048_RDS_CRC_UNRECOVARABLE)
+		return;
+
+	BUG_ON((index+4) >= BCM2048_MAX_RDS_RT);
+
+	if ((bdev->rds_info.radio_text[i] & BCM2048_RDS_BLOCK_MASK) ==
+		BCM2048_RDS_BLOCK_D)
+		bcm2048_parse_rds_rt_block(bdev, i, index+2, crc);
+}
+
+static int bcm2048_parse_rds_rt(struct bcm2048_device *bdev)
+{
+	int i, index = 0, crc, match_b = 0, match_c = 0, match_d = 0;
+
+	for (i = 0; i < bdev->fifo_size; i += BCM2048_RDS_FIFO_DUPLE_SIZE) {
+
+		if (match_b) {
+			match_b = 0;
+			index = bcm2048_parse_rt_match_b(bdev, i);
+			if (index >= 0 && index <= (BCM2048_MAX_RDS_RT - 5))
+				match_c = 1;
+			continue;
+		} else if (match_c) {
+			match_c = 0;
+			if (bcm2048_parse_rt_match_c(bdev, i, index))
+				match_d = 1;
+			continue;
+		} else if (match_d) {
+			match_d = 0;
+			bcm2048_parse_rt_match_d(bdev, i, index);
+			continue;
+		}
+
+		/* Skip erroneous blocks due to messed up A block altogether */
+		if ((bdev->rds_info.radio_text[i] & BCM2048_RDS_BLOCK_MASK)
+			== BCM2048_RDS_BLOCK_A) {
+			crc = bcm2048_rds_block_crc(bdev, i);
+			if (crc == BCM2048_RDS_CRC_UNRECOVARABLE)
+				continue;
+			/* Syncronize to a good RDS PI */
+			if (((bdev->rds_info.radio_text[i+1] << 8) +
+				bdev->rds_info.radio_text[i+2]) ==
+				bdev->rds_info.rds_pi)
+					match_b = 1;
+		}
+	}
+
+	return 0;
+}
+
+static void bcm2048_parse_rds_ps_block(struct bcm2048_device *bdev, int i,
+					int index, int crc)
+{
+	/* Good data will overwrite poor data */
+	if (crc) {
+		if (!bdev->rds_info.rds_ps[index])
+			bdev->rds_info.rds_ps[index] =
+				bdev->rds_info.radio_text[i+1];
+		if (!bdev->rds_info.rds_ps[index+1])
+			bdev->rds_info.rds_ps[index+1] =
+				bdev->rds_info.radio_text[i+2];
+	} else {
+		bdev->rds_info.rds_ps[index] = bdev->rds_info.radio_text[i+1];
+		bdev->rds_info.rds_ps[index+1] =
+			bdev->rds_info.radio_text[i+2];
+	}
+}
+
+static int bcm2048_parse_ps_match_c(struct bcm2048_device *bdev, int i,
+					int index)
+{
+	int crc;
+
+	crc = bcm2048_rds_block_crc(bdev, i);
+
+	if (crc == BCM2048_RDS_CRC_UNRECOVARABLE)
+		return 0;
+
+	if ((bdev->rds_info.radio_text[i] & BCM2048_RDS_BLOCK_MASK) ==
+		BCM2048_RDS_BLOCK_C)
+		return 1;
+
+	return 0;
+}
+
+static void bcm2048_parse_ps_match_d(struct bcm2048_device *bdev, int i,
+					int index)
+{
+	int crc;
+
+	crc = bcm2048_rds_block_crc(bdev, i);
+
+	if (crc == BCM2048_RDS_CRC_UNRECOVARABLE)
+		return;
+
+	if ((bdev->rds_info.radio_text[i] & BCM2048_RDS_BLOCK_MASK) ==
+		BCM2048_RDS_BLOCK_D)
+		bcm2048_parse_rds_ps_block(bdev, i, index, crc);
+}
+
+static int bcm2048_parse_ps_match_b(struct bcm2048_device *bdev, int i)
+{
+	int crc, index, ps_id, ps_group;
+
+	crc = bcm2048_rds_block_crc(bdev, i);
+
+	if (crc == BCM2048_RDS_CRC_UNRECOVARABLE)
+		return -EIO;
+
+	/* Block B Radio PS match */
+	if ((bdev->rds_info.radio_text[i] & BCM2048_RDS_BLOCK_MASK) ==
+		BCM2048_RDS_BLOCK_B) {
+		ps_id = bdev->rds_info.radio_text[i+1] &
+			BCM2048_RDS_BLOCK_MASK;
+		ps_group = bdev->rds_info.radio_text[i+1] &
+			BCM2048_RDS_GROUP_AB_MASK;
+
+		/*
+		 * Poor RSSI will lead to RDS data corruption
+		 * So using 3 (same) sequential values to justify major changes
+		 */
+		if (ps_group != bdev->rds_info.rds_ps_group) {
+			if (crc == BCM2048_RDS_CRC_NONE) {
+				bdev->rds_info.rds_ps_group_cnt++;
+				if (bdev->rds_info.rds_ps_group_cnt > 2) {
+					bdev->rds_info.rds_ps_group = ps_group;
+					bdev->rds_info.rds_ps_group_cnt	= 0;
+					dev_err(&bdev->client->dev,
+						"RDS PS Group change!\n");
+				} else {
+					return -EIO;
+				}
+			} else {
+				bdev->rds_info.rds_ps_group_cnt = 0;
+			}
+		}
+
+		if (ps_id == BCM2048_RDS_PS) {
+			index = bdev->rds_info.radio_text[i+2] &
+				BCM2048_RDS_PS_INDEX;
+			index <<= 1;
+			return index;
+		}
+	}
+
+	return -EIO;
+}
+
+static void bcm2048_parse_rds_ps(struct bcm2048_device *bdev)
+{
+	int i, index = 0, crc, match_b = 0, match_c = 0, match_d = 0;
+
+	for (i = 0; i < bdev->fifo_size; i += BCM2048_RDS_FIFO_DUPLE_SIZE) {
+
+		if (match_b) {
+			match_b = 0;
+			index = bcm2048_parse_ps_match_b(bdev, i);
+			if (index >= 0 && index < (BCM2048_MAX_RDS_PS - 1))
+				match_c = 1;
+			continue;
+		} else if (match_c) {
+			match_c = 0;
+			if (bcm2048_parse_ps_match_c(bdev, i, index))
+				match_d = 1;
+			continue;
+		} else if (match_d) {
+			match_d = 0;
+			bcm2048_parse_ps_match_d(bdev, i, index);
+			continue;
+		}
+
+		/* Skip erroneous blocks due to messed up A block altogether */
+		if ((bdev->rds_info.radio_text[i] & BCM2048_RDS_BLOCK_MASK)
+			== BCM2048_RDS_BLOCK_A) {
+			crc = bcm2048_rds_block_crc(bdev, i);
+			if (crc == BCM2048_RDS_CRC_UNRECOVARABLE)
+				continue;
+			/* Syncronize to a good RDS PI */
+			if (((bdev->rds_info.radio_text[i+1] << 8) +
+				bdev->rds_info.radio_text[i+2]) ==
+				bdev->rds_info.rds_pi)
+					match_b = 1;
+		}
+	}
+}
+
+static void bcm2048_rds_fifo_receive(struct bcm2048_device *bdev)
+{
+	int err;
+
+	mutex_lock(&bdev->mutex);
+
+	err = bcm2048_recv_duples(bdev, BCM2048_I2C_RDS_DATA,
+				bdev->rds_info.radio_text, bdev->fifo_size);
+	if (err != 2) {
+		dev_err(&bdev->client->dev, "RDS Read problem\n");
+		mutex_unlock(&bdev->mutex);
+		return;
+	}
+
+	bdev->rds_info.text_len = bdev->fifo_size;
+
+	bcm2048_parse_rds_pi(bdev);
+	bcm2048_parse_rds_rt(bdev);
+	bcm2048_parse_rds_ps(bdev);
+
+	mutex_unlock(&bdev->mutex);
+
+	wake_up_interruptible(&bdev->read_queue);
+}
+
+static int bcm2048_get_rds_data(struct bcm2048_device *bdev, char *data)
+{
+	int err = 0, i, p = 0;
+	char *data_buffer;
+
+	mutex_lock(&bdev->mutex);
+
+	if (!bdev->rds_info.text_len) {
+		err = -EINVAL;
+		goto unlock;
+	}
+
+	data_buffer = kzalloc(BCM2048_MAX_RDS_RADIO_TEXT*5, GFP_KERNEL);
+	if (!data_buffer) {
+		err = -ENOMEM;
+		goto unlock;
+	}
+
+	for (i = 0; i < bdev->rds_info.text_len; i++) {
+		p += sprintf(data_buffer+p, "%x ",
+			bdev->rds_info.radio_text[i]);
+	}
+
+	memcpy(data, data_buffer, p);
+	kfree(data_buffer);
+
+unlock:
+	mutex_unlock(&bdev->mutex);
+	return err;
+}
+
+/*
+ *	BCM2048 default initialization sequence
+ */
+static int bcm2048_init(struct bcm2048_device *bdev)
+{
+	int err;
+
+	err = bcm2048_set_power_state(bdev, BCM2048_POWER_ON);
+	if (err < 0)
+		goto exit;
+
+	err = bcm2048_set_audio_route(bdev, BCM2048_AUDIO_ROUTE_DAC);
+	if (err < 0)
+		goto exit;
+
+	err = bcm2048_set_dac_output(bdev, BCM2048_DAC_OUTPUT_LEFT |
+		BCM2048_DAC_OUTPUT_RIGHT);
+
+exit:
+	return err;
+}
+
+/*
+ *	BCM2048 default deinitialization sequence
+ */
+static int bcm2048_deinit(struct bcm2048_device *bdev)
+{
+	int err;
+
+	err = bcm2048_set_audio_route(bdev, 0);
+	if (err < 0)
+		goto exit;
+
+	err = bcm2048_set_dac_output(bdev, 0);
+	if (err < 0)
+		goto exit;
+
+	err = bcm2048_set_power_state(bdev, BCM2048_POWER_OFF);
+	if (err < 0)
+		goto exit;
+
+exit:
+	return err;
+}
+
+/*
+ *	BCM2048 probe sequence
+ */
+static int bcm2048_probe(struct bcm2048_device *bdev)
+{
+	int err;
+
+	err = bcm2048_set_power_state(bdev, BCM2048_POWER_ON);
+	if (err < 0)
+		goto unlock;
+
+	err = bcm2048_checkrev(bdev);
+	if (err < 0)
+		goto unlock;
+
+	err = bcm2048_set_mute(bdev, BCM2048_DEFAULT_MUTE);
+	if (err < 0)
+		goto unlock;
+
+	err = bcm2048_set_region(bdev, BCM2048_DEFAULT_REGION);
+	if (err < 0)
+		goto unlock;
+
+	err = bcm2048_set_fm_search_rssi_threshold(bdev,
+					BCM2048_DEFAULT_RSSI_THRESHOLD);
+	if (err < 0)
+		goto unlock;
+
+	err = bcm2048_set_fm_automatic_stereo_mono(bdev, BCM2048_ITEM_ENABLED);
+	if (err < 0)
+		goto unlock;
+
+	err = bcm2048_get_rds_wline(bdev);
+	if (err < BCM2048_DEFAULT_RDS_WLINE)
+		err = bcm2048_set_rds_wline(bdev, BCM2048_DEFAULT_RDS_WLINE);
+	if (err < 0)
+		goto unlock;
+
+	err = bcm2048_set_power_state(bdev, BCM2048_POWER_OFF);
+
+	init_waitqueue_head(&bdev->read_queue);
+	bdev->rds_data_available = 0;
+	bdev->rd_index = 0;
+	bdev->users = 0;
+
+unlock:
+	return err;
+}
+
+/*
+ *	BCM2048 workqueue handler
+ */
+static void bcm2048_work(struct work_struct *work)
+{
+	struct bcm2048_device *bdev;
+	u8 flag_lsb, flag_msb, flags;
+
+	bdev = container_of(work, struct bcm2048_device, work);
+	bcm2048_recv_command(bdev, BCM2048_I2C_FM_RDS_FLAG0, &flag_lsb);
+	bcm2048_recv_command(bdev, BCM2048_I2C_FM_RDS_FLAG1, &flag_msb);
+
+	if (flag_lsb & (BCM2048_FM_FLAG_SEARCH_TUNE_FINISHED |
+			BCM2048_FM_FLAG_SEARCH_TUNE_FAIL)) {
+
+		if (flag_lsb & BCM2048_FM_FLAG_SEARCH_TUNE_FAIL)
+			bdev->scan_state = BCM2048_SCAN_FAIL;
+		else
+			bdev->scan_state = BCM2048_SCAN_OK;
+
+		complete(&bdev->compl);
+	}
+
+	if (flag_msb & BCM2048_RDS_FLAG_FIFO_WLINE) {
+		bcm2048_rds_fifo_receive(bdev);
+		if (bdev->rds_state) {
+			flags =	BCM2048_RDS_FLAG_FIFO_WLINE;
+			bcm2048_send_command(bdev, BCM2048_I2C_FM_RDS_MASK1,
+						flags);
+		}
+		bdev->rds_data_available = 1;
+		bdev->rd_index = 0; /* new data, new start */
+	}
+}
+
+/*
+ *	BCM2048 interrupt handler
+ */
+static irqreturn_t bcm2048_handler(int irq, void *dev)
+{
+	struct bcm2048_device *bdev = dev;
+
+	dev_dbg(&bdev->client->dev, "IRQ called, queuing work\n");
+	if (bdev->power_state)
+		schedule_work(&bdev->work);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ *	BCM2048 sysfs interface definitions
+ */
+#define property_write(prop, type, mask, check)				\
+static ssize_t bcm2048_##prop##_write(struct device *dev,		\
+					struct device_attribute *attr,	\
+					const char *buf,		\
+					size_t count)			\
+{									\
+	struct bcm2048_device *bdev = dev_get_drvdata(dev);		\
+	type value;							\
+	int err;							\
+									\
+	if (!bdev)							\
+		return -ENODEV;						\
+									\
+	sscanf(buf, mask, &value);					\
+									\
+	if (check)							\
+		return -EDOM;						\
+									\
+	err = bcm2048_set_##prop(bdev, value);				\
+									\
+	return err < 0 ? err : count;					\
+}
+
+#define property_read(prop, size, mask)					\
+static ssize_t bcm2048_##prop##_read(struct device *dev,		\
+					struct device_attribute *attr,	\
+					char *buf)			\
+{									\
+	struct bcm2048_device *bdev = dev_get_drvdata(dev);		\
+	int value;							\
+									\
+	if (!bdev)							\
+		return -ENODEV;						\
+									\
+	value = bcm2048_get_##prop(bdev);				\
+									\
+	if (value >= 0)							\
+		value = sprintf(buf, mask "\n", value);			\
+									\
+	return value;							\
+}
+
+#define property_signed_read(prop, size, mask)				\
+static ssize_t bcm2048_##prop##_read(struct device *dev,		\
+					struct device_attribute *attr,	\
+					char *buf)			\
+{									\
+	struct bcm2048_device *bdev = dev_get_drvdata(dev);		\
+	size value;							\
+									\
+	if (!bdev)							\
+		return -ENODEV;						\
+									\
+	value = bcm2048_get_##prop(bdev);				\
+									\
+	value = sprintf(buf, mask "\n", value);				\
+									\
+	return value;							\
+}
+
+#define DEFINE_SYSFS_PROPERTY(prop, signal, size, mask, check)		\
+property_write(prop, signal size, mask, check)				\
+property_read(prop, size, mask)
+
+#define property_str_read(prop, size)					\
+static ssize_t bcm2048_##prop##_read(struct device *dev,		\
+					struct device_attribute *attr,	\
+					char *buf)			\
+{									\
+	struct bcm2048_device *bdev = dev_get_drvdata(dev);		\
+	int count;							\
+	u8 *out;							\
+									\
+	if (!bdev)							\
+		return -ENODEV;						\
+									\
+	out = kzalloc(size + 1, GFP_KERNEL);				\
+	if (!out)							\
+		return -ENOMEM;						\
+									\
+	bcm2048_get_##prop(bdev, out);					\
+	count = sprintf(buf, "%s\n", out);				\
+									\
+	kfree(out);							\
+									\
+	return count;							\
+}
+
+DEFINE_SYSFS_PROPERTY(power_state, unsigned, int, "%u", 0)
+DEFINE_SYSFS_PROPERTY(mute, unsigned, int, "%u", 0)
+DEFINE_SYSFS_PROPERTY(audio_route, unsigned, int, "%u", 0)
+DEFINE_SYSFS_PROPERTY(dac_output, unsigned, int, "%u", 0)
+
+DEFINE_SYSFS_PROPERTY(fm_hi_lo_injection, unsigned, int, "%u", 0)
+DEFINE_SYSFS_PROPERTY(fm_frequency, unsigned, int, "%u", 0)
+DEFINE_SYSFS_PROPERTY(fm_af_frequency, unsigned, int, "%u", 0)
+DEFINE_SYSFS_PROPERTY(fm_deemphasis, unsigned, int, "%u", 0)
+DEFINE_SYSFS_PROPERTY(fm_rds_mask, unsigned, int, "%u", 0)
+DEFINE_SYSFS_PROPERTY(fm_best_tune_mode, unsigned, int, "%u", 0)
+DEFINE_SYSFS_PROPERTY(fm_search_rssi_threshold, unsigned, int, "%u", 0)
+DEFINE_SYSFS_PROPERTY(fm_search_mode_direction, unsigned, int, "%u", 0)
+DEFINE_SYSFS_PROPERTY(fm_search_tune_mode, unsigned, int, "%u", value > 3)
+
+DEFINE_SYSFS_PROPERTY(rds, unsigned, int, "%u", 0)
+DEFINE_SYSFS_PROPERTY(rds_b_block_mask, unsigned, int, "%u", 0)
+DEFINE_SYSFS_PROPERTY(rds_b_block_match, unsigned, int, "%u", 0)
+DEFINE_SYSFS_PROPERTY(rds_pi_mask, unsigned, int, "%u", 0)
+DEFINE_SYSFS_PROPERTY(rds_pi_match, unsigned, int, "%u", 0)
+DEFINE_SYSFS_PROPERTY(rds_wline, unsigned, int, "%u", 0)
+property_read(rds_pi, unsigned int, "%x")
+property_str_read(rds_rt, (BCM2048_MAX_RDS_RT + 1))
+property_str_read(rds_ps, (BCM2048_MAX_RDS_PS + 1))
+
+property_read(fm_rds_flags, unsigned int, "%u")
+property_str_read(rds_data, BCM2048_MAX_RDS_RADIO_TEXT*5)
+
+property_read(region_bottom_frequency, unsigned int, "%u")
+property_read(region_top_frequency, unsigned int, "%u")
+property_signed_read(fm_carrier_error, int, "%d")
+property_signed_read(fm_rssi, int, "%d")
+DEFINE_SYSFS_PROPERTY(region, unsigned, int, "%u", 0)
+
+static struct device_attribute attrs[] = {
+	__ATTR(power_state, S_IRUGO | S_IWUSR, bcm2048_power_state_read,
+		bcm2048_power_state_write),
+	__ATTR(mute, S_IRUGO | S_IWUSR, bcm2048_mute_read,
+		bcm2048_mute_write),
+	__ATTR(audio_route, S_IRUGO | S_IWUSR, bcm2048_audio_route_read,
+		bcm2048_audio_route_write),
+	__ATTR(dac_output, S_IRUGO | S_IWUSR, bcm2048_dac_output_read,
+		bcm2048_dac_output_write),
+	__ATTR(fm_hi_lo_injection, S_IRUGO | S_IWUSR,
+		bcm2048_fm_hi_lo_injection_read,
+		bcm2048_fm_hi_lo_injection_write),
+	__ATTR(fm_frequency, S_IRUGO | S_IWUSR, bcm2048_fm_frequency_read,
+		bcm2048_fm_frequency_write),
+	__ATTR(fm_af_frequency, S_IRUGO | S_IWUSR,
+		bcm2048_fm_af_frequency_read,
+		bcm2048_fm_af_frequency_write),
+	__ATTR(fm_deemphasis, S_IRUGO | S_IWUSR, bcm2048_fm_deemphasis_read,
+		bcm2048_fm_deemphasis_write),
+	__ATTR(fm_rds_mask, S_IRUGO | S_IWUSR, bcm2048_fm_rds_mask_read,
+		bcm2048_fm_rds_mask_write),
+	__ATTR(fm_best_tune_mode, S_IRUGO | S_IWUSR,
+		bcm2048_fm_best_tune_mode_read,
+		bcm2048_fm_best_tune_mode_write),
+	__ATTR(fm_search_rssi_threshold, S_IRUGO | S_IWUSR,
+		bcm2048_fm_search_rssi_threshold_read,
+		bcm2048_fm_search_rssi_threshold_write),
+	__ATTR(fm_search_mode_direction, S_IRUGO | S_IWUSR,
+		bcm2048_fm_search_mode_direction_read,
+		bcm2048_fm_search_mode_direction_write),
+	__ATTR(fm_search_tune_mode, S_IRUGO | S_IWUSR,
+		bcm2048_fm_search_tune_mode_read,
+		bcm2048_fm_search_tune_mode_write),
+	__ATTR(rds, S_IRUGO | S_IWUSR, bcm2048_rds_read,
+		bcm2048_rds_write),
+	__ATTR(rds_b_block_mask, S_IRUGO | S_IWUSR,
+		bcm2048_rds_b_block_mask_read,
+		bcm2048_rds_b_block_mask_write),
+	__ATTR(rds_b_block_match, S_IRUGO | S_IWUSR,
+		bcm2048_rds_b_block_match_read,
+		bcm2048_rds_b_block_match_write),
+	__ATTR(rds_pi_mask, S_IRUGO | S_IWUSR, bcm2048_rds_pi_mask_read,
+		bcm2048_rds_pi_mask_write),
+	__ATTR(rds_pi_match, S_IRUGO | S_IWUSR, bcm2048_rds_pi_match_read,
+		bcm2048_rds_pi_match_write),
+	__ATTR(rds_wline, S_IRUGO | S_IWUSR, bcm2048_rds_wline_read,
+		bcm2048_rds_wline_write),
+	__ATTR(rds_pi, S_IRUGO, bcm2048_rds_pi_read, NULL),
+	__ATTR(rds_rt, S_IRUGO, bcm2048_rds_rt_read, NULL),
+	__ATTR(rds_ps, S_IRUGO, bcm2048_rds_ps_read, NULL),
+	__ATTR(fm_rds_flags, S_IRUGO, bcm2048_fm_rds_flags_read, NULL),
+	__ATTR(region_bottom_frequency, S_IRUGO,
+		bcm2048_region_bottom_frequency_read, NULL),
+	__ATTR(region_top_frequency, S_IRUGO,
+		bcm2048_region_top_frequency_read, NULL),
+	__ATTR(fm_carrier_error, S_IRUGO,
+		bcm2048_fm_carrier_error_read, NULL),
+	__ATTR(fm_rssi, S_IRUGO,
+		bcm2048_fm_rssi_read, NULL),
+	__ATTR(region, S_IRUGO | S_IWUSR, bcm2048_region_read,
+		bcm2048_region_write),
+	__ATTR(rds_data, S_IRUGO, bcm2048_rds_data_read, NULL),
+};
+
+static int bcm2048_sysfs_unregister_properties(struct bcm2048_device *bdev,
+						int size)
+{
+	int i;
+
+	for (i = 0; i < size; i++)
+		device_remove_file(&bdev->client->dev, &attrs[i]);
+
+	return 0;
+}
+
+static int bcm2048_sysfs_register_properties(struct bcm2048_device *bdev)
+{
+	int err = 0;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(attrs); i++) {
+		if (device_create_file(&bdev->client->dev, &attrs[i]) != 0) {
+			dev_err(&bdev->client->dev,
+					"could not register sysfs entry\n");
+			err = -EBUSY;
+			bcm2048_sysfs_unregister_properties(bdev, i);
+			break;
+		}
+	}
+
+	return err;
+}
+
+
+static int bcm2048_fops_open(struct file *file)
+{
+	struct bcm2048_device *bdev = video_drvdata(file);
+
+	bdev->users++;
+	bdev->rd_index = 0;
+	bdev->rds_data_available = 0;
+
+	return 0;
+}
+
+static int bcm2048_fops_release(struct file *file)
+{
+	struct bcm2048_device *bdev = video_drvdata(file);
+
+	bdev->users--;
+
+	return 0;
+}
+
+static unsigned int bcm2048_fops_poll(struct file *file,
+		struct poll_table_struct *pts)
+{
+	struct bcm2048_device *bdev = video_drvdata(file);
+	int retval = 0;
+
+	poll_wait(file, &bdev->read_queue, pts);
+
+	if (bdev->rds_data_available)
+		retval = POLLIN | POLLRDNORM;
+
+	return retval;
+}
+
+static ssize_t bcm2048_fops_read(struct file *file, char __user *buf,
+	size_t count, loff_t *ppos)
+{
+	struct bcm2048_device *bdev = video_drvdata(file);
+	int i;
+	int retval = 0;
+
+	/* we return at least 3 bytes, one block */
+	count = (count / 3) * 3; /* only multiples of 3 */
+	if (count < 3)
+		return -ENOBUFS;
+
+	while (!bdev->rds_data_available) {
+		if (file->f_flags & O_NONBLOCK) {
+			retval = -EWOULDBLOCK;
+			goto done;
+		}
+		/* interruptible_sleep_on(&bdev->read_queue); */
+		if (wait_event_interruptible(bdev->read_queue,
+			bdev->rds_data_available) < 0) {
+			retval = -EINTR;
+			goto done;
+		}
+	}
+
+	mutex_lock(&bdev->mutex);
+	/* copy data to userspace */
+	i = bdev->fifo_size - bdev->rd_index;
+	if (count > i)
+		count = (i / 3) * 3;
+
+	i = 0;
+	while (i < count) {
+		unsigned char tmpbuf[3];
+		tmpbuf[i] = bdev->rds_info.radio_text[bdev->rd_index+i+2];
+		tmpbuf[i+1] = bdev->rds_info.radio_text[bdev->rd_index+i+1];
+		tmpbuf[i+2] = ((bdev->rds_info.radio_text[bdev->rd_index+i]
+				& 0xf0) >> 4);
+		if ((bdev->rds_info.radio_text[bdev->rd_index+i] &
+			BCM2048_RDS_CRC_MASK) == BCM2048_RDS_CRC_UNRECOVARABLE)
+			tmpbuf[i+2] |= 0x80;
+		if (copy_to_user(buf+i, tmpbuf, 3)) {
+			retval = -EFAULT;
+			break;
+		}
+		i += 3;
+	}
+
+	bdev->rd_index += i;
+	if (bdev->rd_index >= bdev->fifo_size)
+		bdev->rds_data_available = 0;
+
+	mutex_unlock(&bdev->mutex);
+	if (retval == 0)
+		retval = i;
+
+done:
+	return retval;
+}
+
+/*
+ *	bcm2048_fops - file operations interface
+ */
+static const struct v4l2_file_operations bcm2048_fops = {
+	.owner		= THIS_MODULE,
+	.ioctl		= video_ioctl2,
+	/* for RDS read support */
+	.open		= bcm2048_fops_open,
+	.release	= bcm2048_fops_release,
+	.read		= bcm2048_fops_read,
+	.poll		= bcm2048_fops_poll
+};
+
+/*
+ *	Video4Linux Interface
+ */
+static struct v4l2_queryctrl bcm2048_v4l2_queryctrl[] = {
+	{
+		.id		= V4L2_CID_AUDIO_VOLUME,
+		.flags		= V4L2_CTRL_FLAG_DISABLED,
+	},
+	{
+		.id		= V4L2_CID_AUDIO_BALANCE,
+		.flags		= V4L2_CTRL_FLAG_DISABLED,
+	},
+	{
+		.id		= V4L2_CID_AUDIO_BASS,
+		.flags		= V4L2_CTRL_FLAG_DISABLED,
+	},
+	{
+		.id		= V4L2_CID_AUDIO_TREBLE,
+		.flags		= V4L2_CTRL_FLAG_DISABLED,
+	},
+	{
+		.id		= V4L2_CID_AUDIO_MUTE,
+		.type		= V4L2_CTRL_TYPE_BOOLEAN,
+		.name		= "Mute",
+		.minimum	= 0,
+		.maximum	= 1,
+		.step		= 1,
+		.default_value	= 1,
+	},
+	{
+		.id		= V4L2_CID_AUDIO_LOUDNESS,
+		.flags		= V4L2_CTRL_FLAG_DISABLED,
+	},
+};
+
+static int bcm2048_vidioc_querycap(struct file *file, void *priv,
+		struct v4l2_capability *capability)
+{
+	struct bcm2048_device *bdev = video_get_drvdata(video_devdata(file));
+
+	strlcpy(capability->driver, BCM2048_DRIVER_NAME,
+		sizeof(capability->driver));
+	strlcpy(capability->card, BCM2048_DRIVER_CARD,
+		sizeof(capability->card));
+	snprintf(capability->bus_info, 32, "I2C: 0x%X", bdev->client->addr);
+	capability->version = BCM2048_DRIVER_VERSION;
+	capability->capabilities = V4L2_CAP_TUNER | V4L2_CAP_RADIO |
+					V4L2_CAP_HW_FREQ_SEEK;
+
+	return 0;
+}
+
+static int bcm2048_vidioc_g_input(struct file *filp, void *priv,
+		unsigned int *i)
+{
+	*i = 0;
+
+	return 0;
+}
+
+static int bcm2048_vidioc_s_input(struct file *filp, void *priv,
+					unsigned int i)
+{
+	if (i)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int bcm2048_vidioc_queryctrl(struct file *file, void *priv,
+		struct v4l2_queryctrl *qc)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(bcm2048_v4l2_queryctrl); i++) {
+		if (qc->id && qc->id == bcm2048_v4l2_queryctrl[i].id) {
+			*qc = bcm2048_v4l2_queryctrl[i];
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static int bcm2048_vidioc_g_ctrl(struct file *file, void *priv,
+		struct v4l2_control *ctrl)
+{
+	struct bcm2048_device *bdev = video_get_drvdata(video_devdata(file));
+	int err = 0;
+
+	if (!bdev)
+		return -ENODEV;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUDIO_MUTE:
+		err = bcm2048_get_mute(bdev);
+		if (err >= 0)
+			ctrl->value = err;
+		break;
+	}
+
+	return err;
+}
+
+static int bcm2048_vidioc_s_ctrl(struct file *file, void *priv,
+		struct v4l2_control *ctrl)
+{
+	struct bcm2048_device *bdev = video_get_drvdata(video_devdata(file));
+	int err = 0;
+
+	if (!bdev)
+		return -ENODEV;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUDIO_MUTE:
+		if (ctrl->value) {
+			if (bdev->power_state) {
+				err = bcm2048_set_mute(bdev, ctrl->value);
+				err |= bcm2048_deinit(bdev);
+			}
+		} else {
+			if (!bdev->power_state) {
+				err = bcm2048_init(bdev);
+				err |= bcm2048_set_mute(bdev, ctrl->value);
+			}
+		}
+		break;
+	}
+
+	return err;
+}
+
+static int bcm2048_vidioc_g_audio(struct file *file, void *priv,
+		struct v4l2_audio *audio)
+{
+	if (audio->index > 1)
+		return -EINVAL;
+
+	strncpy(audio->name, "Radio", 32);
+	audio->capability = V4L2_AUDCAP_STEREO;
+
+	return 0;
+}
+
+static int bcm2048_vidioc_s_audio(struct file *file, void *priv,
+		const struct v4l2_audio *audio)
+{
+	if (audio->index != 0)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int bcm2048_vidioc_g_tuner(struct file *file, void *priv,
+		struct v4l2_tuner *tuner)
+{
+	struct bcm2048_device *bdev = video_get_drvdata(video_devdata(file));
+	s8 f_error;
+	s8 rssi;
+
+	if (!bdev)
+		return -ENODEV;
+
+	if (tuner->index > 0)
+		return -EINVAL;
+
+	strncpy(tuner->name, "FM Receiver", 32);
+	tuner->type = V4L2_TUNER_RADIO;
+	tuner->rangelow =
+		dev_to_v4l2(bcm2048_get_region_bottom_frequency(bdev));
+	tuner->rangehigh =
+		dev_to_v4l2(bcm2048_get_region_top_frequency(bdev));
+	tuner->rxsubchans = V4L2_TUNER_SUB_STEREO;
+	tuner->capability = V4L2_TUNER_CAP_STEREO | V4L2_TUNER_CAP_LOW;
+	tuner->audmode = V4L2_TUNER_MODE_STEREO;
+	tuner->afc = 0;
+	if (bdev->power_state) {
+		/*
+		 * Report frequencies with high carrier errors to have zero
+		 * signal level
+		 */
+		f_error = bcm2048_get_fm_carrier_error(bdev);
+		if (f_error < BCM2048_FREQ_ERROR_FLOOR ||
+		    f_error > BCM2048_FREQ_ERROR_ROOF) {
+			tuner->signal = 0;
+		} else {
+			/*
+			 * RSSI level -60 dB is defined to report full
+			 * signal strenght
+			 */
+			rssi = bcm2048_get_fm_rssi(bdev);
+			if (rssi >= BCM2048_RSSI_LEVEL_BASE) {
+				tuner->signal = 0xFFFF;
+			} else if (rssi > BCM2048_RSSI_LEVEL_ROOF) {
+				tuner->signal = (rssi +
+						 BCM2048_RSSI_LEVEL_ROOF_NEG)
+						 * BCM2048_SIGNAL_MULTIPLIER;
+			} else {
+				tuner->signal = 0;
+			}
+		}
+	} else {
+		tuner->signal = 0;
+	}
+
+	return 0;
+}
+
+static int bcm2048_vidioc_s_tuner(struct file *file, void *priv,
+		const struct v4l2_tuner *tuner)
+{
+	struct bcm2048_device *bdev = video_get_drvdata(video_devdata(file));
+
+	if (!bdev)
+		return -ENODEV;
+
+	if (tuner->index > 0)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int bcm2048_vidioc_g_frequency(struct file *file, void *priv,
+		struct v4l2_frequency *freq)
+{
+	struct bcm2048_device *bdev = video_get_drvdata(video_devdata(file));
+	int err = 0;
+	int f;
+
+	if (!bdev->power_state)
+		return -ENODEV;
+
+	freq->type = V4L2_TUNER_RADIO;
+	f = bcm2048_get_fm_frequency(bdev);
+
+	if (f < 0)
+		err = f;
+	else
+		freq->frequency = dev_to_v4l2(f);
+
+	return err;
+}
+
+static int bcm2048_vidioc_s_frequency(struct file *file, void *priv,
+		const struct v4l2_frequency *freq)
+{
+	struct bcm2048_device *bdev = video_get_drvdata(video_devdata(file));
+	int err;
+
+	if (freq->type != V4L2_TUNER_RADIO)
+		return -EINVAL;
+
+	if (!bdev->power_state)
+		return -ENODEV;
+
+	err = bcm2048_set_fm_frequency(bdev, v4l2_to_dev(freq->frequency));
+	err |= bcm2048_set_fm_search_tune_mode(bdev, BCM2048_FM_PRE_SET_MODE);
+
+	return err;
+}
+
+static int bcm2048_vidioc_s_hw_freq_seek(struct file *file, void *priv,
+					const struct v4l2_hw_freq_seek *seek)
+{
+	struct bcm2048_device *bdev = video_get_drvdata(video_devdata(file));
+	int err;
+
+	if (!bdev->power_state)
+		return -ENODEV;
+
+	if ((seek->tuner != 0) || (seek->type != V4L2_TUNER_RADIO))
+		return -EINVAL;
+
+	err = bcm2048_set_fm_search_mode_direction(bdev, seek->seek_upward);
+	err |= bcm2048_set_fm_search_tune_mode(bdev,
+			BCM2048_FM_AUTO_SEARCH_MODE);
+
+	return err;
+}
+
+static struct v4l2_ioctl_ops bcm2048_ioctl_ops = {
+	.vidioc_querycap	= bcm2048_vidioc_querycap,
+	.vidioc_g_input		= bcm2048_vidioc_g_input,
+	.vidioc_s_input		= bcm2048_vidioc_s_input,
+	.vidioc_queryctrl	= bcm2048_vidioc_queryctrl,
+	.vidioc_g_ctrl		= bcm2048_vidioc_g_ctrl,
+	.vidioc_s_ctrl		= bcm2048_vidioc_s_ctrl,
+	.vidioc_g_audio		= bcm2048_vidioc_g_audio,
+	.vidioc_s_audio		= bcm2048_vidioc_s_audio,
+	.vidioc_g_tuner		= bcm2048_vidioc_g_tuner,
+	.vidioc_s_tuner		= bcm2048_vidioc_s_tuner,
+	.vidioc_g_frequency	= bcm2048_vidioc_g_frequency,
+	.vidioc_s_frequency	= bcm2048_vidioc_s_frequency,
+	.vidioc_s_hw_freq_seek  = bcm2048_vidioc_s_hw_freq_seek,
+};
+
+/*
+ * bcm2048_viddev_template - video device interface
+ */
+static struct video_device bcm2048_viddev_template = {
+	.fops			= &bcm2048_fops,
+	.name			= BCM2048_DRIVER_NAME,
+	.release		= video_device_release,
+	.ioctl_ops		= &bcm2048_ioctl_ops,
+};
+
+/*
+ *	I2C driver interface
+ */
+static int bcm2048_i2c_driver_probe(struct i2c_client *client,
+					const struct i2c_device_id *id)
+{
+	struct bcm2048_device *bdev;
+	int err, skip_release = 0;
+
+	bdev = kzalloc(sizeof(*bdev), GFP_KERNEL);
+	if (!bdev) {
+		dev_dbg(&client->dev, "Failed to alloc video device.\n");
+		err = -ENOMEM;
+		goto exit;
+	}
+
+	bdev->videodev = video_device_alloc();
+	if (!bdev->videodev) {
+		dev_dbg(&client->dev, "Failed to alloc video device.\n");
+		err = -ENOMEM;
+		goto free_bdev;
+	}
+
+	bdev->client = client;
+	i2c_set_clientdata(client, bdev);
+	mutex_init(&bdev->mutex);
+	init_completion(&bdev->compl);
+	INIT_WORK(&bdev->work, bcm2048_work);
+
+	if (client->irq) {
+		err = request_irq(client->irq,
+			bcm2048_handler, IRQF_TRIGGER_FALLING | IRQF_DISABLED,
+			client->name, bdev);
+		if (err < 0) {
+			dev_err(&client->dev, "Could not request IRQ\n");
+			goto free_vdev;
+		}
+		dev_dbg(&client->dev, "IRQ requested.\n");
+	} else {
+		dev_dbg(&client->dev, "IRQ not configured. Using timeouts.\n");
+	}
+
+	*bdev->videodev = bcm2048_viddev_template;
+	video_set_drvdata(bdev->videodev, bdev);
+	if (video_register_device(bdev->videodev, VFL_TYPE_RADIO, radio_nr)) {
+		dev_dbg(&client->dev, "Could not register video device.\n");
+		err = -EIO;
+		goto free_irq;
+	}
+
+	err = bcm2048_sysfs_register_properties(bdev);
+	if (err < 0) {
+		dev_dbg(&client->dev, "Could not register sysfs interface.\n");
+		goto free_registration;
+	}
+
+	err = bcm2048_probe(bdev);
+	if (err < 0) {
+		dev_dbg(&client->dev, "Failed to probe device information.\n");
+		goto free_sysfs;
+	}
+
+	return 0;
+
+free_sysfs:
+	bcm2048_sysfs_unregister_properties(bdev, ARRAY_SIZE(attrs));
+free_registration:
+	video_unregister_device(bdev->videodev);
+	/* video_unregister_device frees bdev->videodev */
+	bdev->videodev = NULL;
+	skip_release = 1;
+free_irq:
+	if (client->irq)
+		free_irq(client->irq, bdev);
+free_vdev:
+	if (!skip_release)
+		video_device_release(bdev->videodev);
+	i2c_set_clientdata(client, NULL);
+free_bdev:
+	kfree(bdev);
+exit:
+	return err;
+}
+
+static int __exit bcm2048_i2c_driver_remove(struct i2c_client *client)
+{
+	struct bcm2048_device *bdev = i2c_get_clientdata(client);
+	struct video_device *vd;
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	if (bdev) {
+		vd = bdev->videodev;
+
+		bcm2048_sysfs_unregister_properties(bdev, ARRAY_SIZE(attrs));
+
+		if (vd)
+			video_unregister_device(vd);
+
+		if (bdev->power_state)
+			bcm2048_set_power_state(bdev, BCM2048_POWER_OFF);
+
+		if (client->irq > 0)
+			free_irq(client->irq, bdev);
+
+		cancel_work_sync(&bdev->work);
+
+		kfree(bdev);
+	}
+
+	i2c_set_clientdata(client, NULL);
+
+	return 0;
+}
+
+/*
+ *	bcm2048_i2c_driver - i2c driver interface
+ */
+static const struct i2c_device_id bcm2048_id[] = {
+	{ "bcm2048" , 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, bcm2048_id);
+
+static struct i2c_driver bcm2048_i2c_driver = {
+	.driver		= {
+		.name	= BCM2048_DRIVER_NAME,
+	},
+	.probe		= bcm2048_i2c_driver_probe,
+	.remove		= __exit_p(bcm2048_i2c_driver_remove),
+	.id_table	= bcm2048_id,
+};
+
+/*
+ *	Module Interface
+ */
+static int __init bcm2048_module_init(void)
+{
+	pr_info(BCM2048_DRIVER_DESC "\n");
+
+	return i2c_add_driver(&bcm2048_i2c_driver);
+}
+module_init(bcm2048_module_init);
+
+static void __exit bcm2048_module_exit(void)
+{
+	i2c_del_driver(&bcm2048_i2c_driver);
+}
+module_exit(bcm2048_module_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR(BCM2048_DRIVER_AUTHOR);
+MODULE_DESCRIPTION(BCM2048_DRIVER_DESC);
+MODULE_VERSION("0.0.2");
diff --git a/drivers/staging/media/bcm2048/radio-bcm2048.h b/drivers/staging/media/bcm2048/radio-bcm2048.h
new file mode 100644
index 000000000000..4c90a32db795
--- /dev/null
+++ b/drivers/staging/media/bcm2048/radio-bcm2048.h
@@ -0,0 +1,30 @@
+/*
+ * drivers/staging/media/radio-bcm2048.h
+ *
+ * Property and command definitions for bcm2048 radio receiver chip.
+ *
+ * Copyright (C) Nokia Corporation
+ * Contact: Eero Nurkkala <ext-eero.nurkkala@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 BCM2048_H
+#define BCM2048_H
+
+#define BCM2048_NAME		"bcm2048"
+#define BCM2048_I2C_ADDR	0x22
+
+#endif	/* ifndef BCM2048_H */
diff --git a/drivers/staging/media/davinci_vpfe/dm365_isif.c b/drivers/staging/media/davinci_vpfe/dm365_isif.c
index ff48fce94fcb..b942bf73c43f 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_isif.c
+++ b/drivers/staging/media/davinci_vpfe/dm365_isif.c
@@ -19,6 +19,7 @@
  *      Prabhakar Lad <prabhakar.lad@ti.com>
  */
 
+#include <linux/delay.h>
 #include "dm365_isif.h"
 #include "vpfe_mc_capture.h"
 
@@ -918,7 +919,7 @@ isif_config_dfc(struct vpfe_isif_device *isif, struct vpfe_isif_dfc *vdfc)
 		   (0 << ISIF_VDFC_EN_SHIFT), DFCCTL);
 
 	isif_write(isif->isif_cfg.base_addr, 0x6, DFCMEMCTL);
-	for (i = 0 ; i < vdfc->num_vdefects; i++) {
+	for (i = 0; i < vdfc->num_vdefects; i++) {
 		count = DFC_WRITE_WAIT_COUNT;
 		while (count &&
 			(isif_read(isif->isif_cfg.base_addr, DFCMEMCTL) & 0x2))
diff --git a/drivers/staging/media/davinci_vpfe/vpfe_video.c b/drivers/staging/media/davinci_vpfe/vpfe_video.c
index 24d98a6866bb..1f3b0f9a8d10 100644
--- a/drivers/staging/media/davinci_vpfe/vpfe_video.c
+++ b/drivers/staging/media/davinci_vpfe/vpfe_video.c
@@ -346,7 +346,7 @@ static int vpfe_pipeline_disable(struct vpfe_pipeline *pipe)
 	}
 	mutex_unlock(&mdev->graph_mutex);
 
-	return (ret == 0) ? ret : -ETIMEDOUT ;
+	return ret ? -ETIMEDOUT : 0;
 }
 
 /*
@@ -1201,6 +1201,8 @@ static int vpfe_start_streaming(struct vb2_queue *vq, unsigned int count)
 	unsigned long addr;
 	int ret;
 
+	if (count == 0)
+		return -ENOBUFS;
 	ret = mutex_lock_interruptible(&video->lock);
 	if (ret)
 		goto streamoff;
diff --git a/drivers/staging/media/go7007/go7007-loader.c b/drivers/staging/media/go7007/go7007-loader.c
index 10bb41c2fb6d..eecb1f2a5574 100644
--- a/drivers/staging/media/go7007/go7007-loader.c
+++ b/drivers/staging/media/go7007/go7007-loader.c
@@ -59,7 +59,7 @@ static int go7007_loader_probe(struct usb_interface *interface,
 
 	if (usbdev->descriptor.bNumConfigurations != 1) {
 		dev_err(&interface->dev, "can't handle multiple config\n");
-		return -ENODEV;
+		goto failed2;
 	}
 
 	vendor = le16_to_cpu(usbdev->descriptor.idVendor);
@@ -108,6 +108,7 @@ static int go7007_loader_probe(struct usb_interface *interface,
 	return 0;
 
 failed2:
+	usb_put_dev(usbdev);
 	dev_err(&interface->dev, "probe failed\n");
 	return -ENODEV;
 }
@@ -115,6 +116,7 @@ failed2:
 static void go7007_loader_disconnect(struct usb_interface *interface)
 {
 	dev_info(&interface->dev, "disconnect\n");
+	usb_put_dev(interface_to_usbdev(interface));
 	usb_set_intfdata(interface, NULL);
 }
 
diff --git a/drivers/staging/media/lirc/lirc_parallel.c b/drivers/staging/media/lirc/lirc_parallel.c
index 41d110f8bc02..0b589892351a 100644
--- a/drivers/staging/media/lirc/lirc_parallel.c
+++ b/drivers/staging/media/lirc/lirc_parallel.c
@@ -220,7 +220,7 @@ static void rbuf_write(int signal)
 	wptr = nwptr;
 }
 
-static void irq_handler(void *blah)
+static void lirc_lirc_irq_handler(void *blah)
 {
 	struct timeval tv;
 	static struct timeval lasttv;
@@ -659,7 +659,7 @@ static int __init lirc_parallel_init(void)
 		goto exit_device_put;
 	}
 	ppdevice = parport_register_device(pport, LIRC_DRIVER_NAME,
-					   pf, kf, irq_handler, 0, NULL);
+					   pf, kf, lirc_lirc_irq_handler, 0, NULL);
 	parport_put_port(pport);
 	if (ppdevice == NULL) {
 		pr_notice("parport_register_device() failed\n");
diff --git a/drivers/staging/media/lirc/lirc_serial.c b/drivers/staging/media/lirc/lirc_serial.c
index abe0d5caa20b..10c685d5de7c 100644
--- a/drivers/staging/media/lirc/lirc_serial.c
+++ b/drivers/staging/media/lirc/lirc_serial.c
@@ -650,7 +650,7 @@ static void frbwrite(int l)
 	rbwrite(l);
 }
 
-static irqreturn_t irq_handler(int i, void *blah)
+static irqreturn_t lirc_irq_handler(int i, void *blah)
 {
 	struct timeval tv;
 	int counter, dcd;
@@ -852,7 +852,7 @@ static int lirc_serial_probe(struct platform_device *dev)
 		return result;
 #endif
 
-	result = request_irq(irq, irq_handler,
+	result = request_irq(irq, lirc_irq_handler,
 			     (share_irq ? IRQF_SHARED : 0),
 			     LIRC_DRIVER_NAME, (void *)&hardware);
 	if (result < 0) {
diff --git a/drivers/staging/media/omap24xx/Kconfig b/drivers/staging/media/omap24xx/Kconfig
new file mode 100644
index 000000000000..82e569a21c46
--- /dev/null
+++ b/drivers/staging/media/omap24xx/Kconfig
@@ -0,0 +1,35 @@
+config VIDEO_V4L2_INT_DEVICE
+       tristate
+
+config VIDEO_OMAP2
+	tristate "OMAP2 Camera Capture Interface driver (DEPRECATED)"
+	depends on VIDEO_DEV && ARCH_OMAP2
+	select VIDEOBUF_DMA_SG
+	select VIDEO_V4L2_INT_DEVICE
+	---help---
+	  This is a v4l2 driver for the TI OMAP2 camera capture interface
+
+	  It uses the deprecated int-device API. Since this driver is no
+	  longer actively maintained and nobody is interested in converting
+	  it to the subdev API, this driver will be removed soon.
+
+	  If you do want to keep this driver in the kernel, and are willing
+	  to convert it to the subdev API, then please contact the linux-media
+	  mailinglist.
+
+config VIDEO_TCM825X
+	tristate "TCM825x camera sensor support (DEPRECATED)"
+	depends on I2C && VIDEO_V4L2
+	depends on MEDIA_CAMERA_SUPPORT
+	select VIDEO_V4L2_INT_DEVICE
+	---help---
+	  This is a driver for the Toshiba TCM825x VGA camera sensor.
+	  It is used for example in Nokia N800.
+
+	  It uses the deprecated int-device API. Since this driver is no
+	  longer actively maintained and nobody is interested in converting
+	  it to the subdev API, this driver will be removed soon.
+
+	  If you do want to keep this driver in the kernel, and are willing
+	  to convert it to the subdev API, then please contact the linux-media
+	  mailinglist.
diff --git a/drivers/staging/media/omap24xx/Makefile b/drivers/staging/media/omap24xx/Makefile
new file mode 100644
index 000000000000..c2e7175599c2
--- /dev/null
+++ b/drivers/staging/media/omap24xx/Makefile
@@ -0,0 +1,5 @@
+omap2cam-objs	:=	omap24xxcam.o omap24xxcam-dma.o
+
+obj-$(CONFIG_VIDEO_OMAP2)   += omap2cam.o
+obj-$(CONFIG_VIDEO_TCM825X) += tcm825x.o
+obj-$(CONFIG_VIDEO_V4L2_INT_DEVICE) += v4l2-int-device.o
diff --git a/drivers/media/platform/omap24xxcam-dma.c b/drivers/staging/media/omap24xx/omap24xxcam-dma.c
index 9c00776d6583..9c00776d6583 100644
--- a/drivers/media/platform/omap24xxcam-dma.c
+++ b/drivers/staging/media/omap24xx/omap24xxcam-dma.c
diff --git a/drivers/media/platform/omap24xxcam.c b/drivers/staging/media/omap24xx/omap24xxcam.c
index d2b440c842b3..d2b440c842b3 100644
--- a/drivers/media/platform/omap24xxcam.c
+++ b/drivers/staging/media/omap24xx/omap24xxcam.c
diff --git a/drivers/media/platform/omap24xxcam.h b/drivers/staging/media/omap24xx/omap24xxcam.h
index 7f6f79155537..233bb40cfec3 100644
--- a/drivers/media/platform/omap24xxcam.h
+++ b/drivers/staging/media/omap24xx/omap24xxcam.h
@@ -28,8 +28,8 @@
 #define OMAP24XXCAM_H
 
 #include <media/videobuf-dma-sg.h>
-#include <media/v4l2-int-device.h>
 #include <media/v4l2-device.h>
+#include "v4l2-int-device.h"
 
 /*
  *
diff --git a/drivers/media/i2c/tcm825x.c b/drivers/staging/media/omap24xx/tcm825x.c
index 9252529fc5dd..b1ae8e9c7e14 100644
--- a/drivers/media/i2c/tcm825x.c
+++ b/drivers/staging/media/omap24xx/tcm825x.c
@@ -28,7 +28,7 @@
 
 #include <linux/i2c.h>
 #include <linux/module.h>
-#include <media/v4l2-int-device.h>
+#include "v4l2-int-device.h"
 
 #include "tcm825x.h"
 
diff --git a/drivers/media/i2c/tcm825x.h b/drivers/staging/media/omap24xx/tcm825x.h
index 8ebab953963f..e2d1bcd0bcbe 100644
--- a/drivers/media/i2c/tcm825x.h
+++ b/drivers/staging/media/omap24xx/tcm825x.h
@@ -17,7 +17,7 @@
 
 #include <linux/videodev2.h>
 
-#include <media/v4l2-int-device.h>
+#include "v4l2-int-device.h"
 
 #define TCM825X_NAME "tcm825x"
 
diff --git a/drivers/media/v4l2-core/v4l2-int-device.c b/drivers/staging/media/omap24xx/v4l2-int-device.c
index f4473494af7a..427a89033a1d 100644
--- a/drivers/media/v4l2-core/v4l2-int-device.c
+++ b/drivers/staging/media/omap24xx/v4l2-int-device.c
@@ -28,7 +28,7 @@
 #include <linux/string.h>
 #include <linux/module.h>
 
-#include <media/v4l2-int-device.h>
+#include "v4l2-int-device.h"
 
 static DEFINE_MUTEX(mutex);
 static LIST_HEAD(int_list);
diff --git a/drivers/staging/media/omap24xx/v4l2-int-device.h b/drivers/staging/media/omap24xx/v4l2-int-device.h
new file mode 100644
index 000000000000..0286c95814ff
--- /dev/null
+++ b/drivers/staging/media/omap24xx/v4l2-int-device.h
@@ -0,0 +1,305 @@
+/*
+ * include/media/v4l2-int-device.h
+ *
+ * V4L2 internal ioctl interface.
+ *
+ * Copyright (C) 2007 Nokia Corporation.
+ *
+ * Contact: Sakari Ailus <sakari.ailus@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 V4L2_INT_DEVICE_H
+#define V4L2_INT_DEVICE_H
+
+#include <media/v4l2-common.h>
+
+#define V4L2NAMESIZE 32
+
+/*
+ *
+ * The internal V4L2 device interface core.
+ *
+ */
+
+enum v4l2_int_type {
+	v4l2_int_type_master = 1,
+	v4l2_int_type_slave
+};
+
+struct module;
+
+struct v4l2_int_device;
+
+struct v4l2_int_master {
+	int (*attach)(struct v4l2_int_device *slave);
+	void (*detach)(struct v4l2_int_device *slave);
+};
+
+typedef int (v4l2_int_ioctl_func)(struct v4l2_int_device *);
+typedef int (v4l2_int_ioctl_func_0)(struct v4l2_int_device *);
+typedef int (v4l2_int_ioctl_func_1)(struct v4l2_int_device *, void *);
+
+struct v4l2_int_ioctl_desc {
+	int num;
+	v4l2_int_ioctl_func *func;
+};
+
+struct v4l2_int_slave {
+	/* Don't touch master. */
+	struct v4l2_int_device *master;
+
+	char attach_to[V4L2NAMESIZE];
+
+	int num_ioctls;
+	struct v4l2_int_ioctl_desc *ioctls;
+};
+
+struct v4l2_int_device {
+	/* Don't touch head. */
+	struct list_head head;
+
+	struct module *module;
+
+	char name[V4L2NAMESIZE];
+
+	enum v4l2_int_type type;
+	union {
+		struct v4l2_int_master *master;
+		struct v4l2_int_slave *slave;
+	} u;
+
+	void *priv;
+};
+
+void v4l2_int_device_try_attach_all(void);
+
+int v4l2_int_device_register(struct v4l2_int_device *d);
+void v4l2_int_device_unregister(struct v4l2_int_device *d);
+
+int v4l2_int_ioctl_0(struct v4l2_int_device *d, int cmd);
+int v4l2_int_ioctl_1(struct v4l2_int_device *d, int cmd, void *arg);
+
+/*
+ *
+ * Types and definitions for IOCTL commands.
+ *
+ */
+
+enum v4l2_power {
+	V4L2_POWER_OFF = 0,
+	V4L2_POWER_ON,
+	V4L2_POWER_STANDBY,
+};
+
+/* Slave interface type. */
+enum v4l2_if_type {
+	/*
+	 * Parallel 8-, 10- or 12-bit interface, used by for example
+	 * on certain image sensors.
+	 */
+	V4L2_IF_TYPE_BT656,
+};
+
+enum v4l2_if_type_bt656_mode {
+	/*
+	 * Modes without Bt synchronisation codes. Separate
+	 * synchronisation signal lines are used.
+	 */
+	V4L2_IF_TYPE_BT656_MODE_NOBT_8BIT,
+	V4L2_IF_TYPE_BT656_MODE_NOBT_10BIT,
+	V4L2_IF_TYPE_BT656_MODE_NOBT_12BIT,
+	/*
+	 * Use Bt synchronisation codes. The vertical and horizontal
+	 * synchronisation is done based on synchronisation codes.
+	 */
+	V4L2_IF_TYPE_BT656_MODE_BT_8BIT,
+	V4L2_IF_TYPE_BT656_MODE_BT_10BIT,
+};
+
+struct v4l2_if_type_bt656 {
+	/*
+	 * 0: Frame begins when vsync is high.
+	 * 1: Frame begins when vsync changes from low to high.
+	 */
+	unsigned frame_start_on_rising_vs:1;
+	/* Use Bt synchronisation codes for sync correction. */
+	unsigned bt_sync_correct:1;
+	/* Swap every two adjacent image data elements. */
+	unsigned swap:1;
+	/* Inverted latch clock polarity from slave. */
+	unsigned latch_clk_inv:1;
+	/* Hs polarity. 0 is active high, 1 active low. */
+	unsigned nobt_hs_inv:1;
+	/* Vs polarity. 0 is active high, 1 active low. */
+	unsigned nobt_vs_inv:1;
+	enum v4l2_if_type_bt656_mode mode;
+	/* Minimum accepted bus clock for slave (in Hz). */
+	u32 clock_min;
+	/* Maximum accepted bus clock for slave. */
+	u32 clock_max;
+	/*
+	 * Current wish of the slave. May only change in response to
+	 * ioctls that affect image capture.
+	 */
+	u32 clock_curr;
+};
+
+struct v4l2_ifparm {
+	enum v4l2_if_type if_type;
+	union {
+		struct v4l2_if_type_bt656 bt656;
+	} u;
+};
+
+/* IOCTL command numbers. */
+enum v4l2_int_ioctl_num {
+	/*
+	 *
+	 * "Proper" V4L ioctls, as in struct video_device.
+	 *
+	 */
+	vidioc_int_enum_fmt_cap_num = 1,
+	vidioc_int_g_fmt_cap_num,
+	vidioc_int_s_fmt_cap_num,
+	vidioc_int_try_fmt_cap_num,
+	vidioc_int_queryctrl_num,
+	vidioc_int_g_ctrl_num,
+	vidioc_int_s_ctrl_num,
+	vidioc_int_cropcap_num,
+	vidioc_int_g_crop_num,
+	vidioc_int_s_crop_num,
+	vidioc_int_g_parm_num,
+	vidioc_int_s_parm_num,
+	vidioc_int_querystd_num,
+	vidioc_int_s_std_num,
+	vidioc_int_s_video_routing_num,
+
+	/*
+	 *
+	 * Strictly internal ioctls.
+	 *
+	 */
+	/* Initialise the device when slave attaches to the master. */
+	vidioc_int_dev_init_num = 1000,
+	/* Delinitialise the device at slave detach. */
+	vidioc_int_dev_exit_num,
+	/* Set device power state. */
+	vidioc_int_s_power_num,
+	/*
+	* Get slave private data, e.g. platform-specific slave
+	* configuration used by the master.
+	*/
+	vidioc_int_g_priv_num,
+	/* Get slave interface parameters. */
+	vidioc_int_g_ifparm_num,
+	/* Does the slave need to be reset after VIDIOC_DQBUF? */
+	vidioc_int_g_needs_reset_num,
+	vidioc_int_enum_framesizes_num,
+	vidioc_int_enum_frameintervals_num,
+
+	/*
+	 *
+	 * VIDIOC_INT_* ioctls.
+	 *
+	 */
+	/* VIDIOC_INT_RESET */
+	vidioc_int_reset_num,
+	/* VIDIOC_INT_INIT */
+	vidioc_int_init_num,
+
+	/*
+	 *
+	 * Start of private ioctls.
+	 *
+	 */
+	vidioc_int_priv_start_num = 2000,
+};
+
+/*
+ *
+ * IOCTL wrapper functions for better type checking.
+ *
+ */
+
+#define V4L2_INT_WRAPPER_0(name)					\
+	static inline int vidioc_int_##name(struct v4l2_int_device *d)	\
+	{								\
+		return v4l2_int_ioctl_0(d, vidioc_int_##name##_num);	\
+	}								\
+									\
+	static inline struct v4l2_int_ioctl_desc			\
+	vidioc_int_##name##_cb(int (*func)				\
+			       (struct v4l2_int_device *))		\
+	{								\
+		struct v4l2_int_ioctl_desc desc;			\
+									\
+		desc.num = vidioc_int_##name##_num;			\
+		desc.func = (v4l2_int_ioctl_func *)func;		\
+									\
+		return desc;						\
+	}
+
+#define V4L2_INT_WRAPPER_1(name, arg_type, asterisk)			\
+	static inline int vidioc_int_##name(struct v4l2_int_device *d,	\
+					    arg_type asterisk arg)	\
+	{								\
+		return v4l2_int_ioctl_1(d, vidioc_int_##name##_num,	\
+					(void *)(unsigned long)arg);	\
+	}								\
+									\
+	static inline struct v4l2_int_ioctl_desc			\
+	vidioc_int_##name##_cb(int (*func)				\
+			       (struct v4l2_int_device *,		\
+				arg_type asterisk))			\
+	{								\
+		struct v4l2_int_ioctl_desc desc;			\
+									\
+		desc.num = vidioc_int_##name##_num;			\
+		desc.func = (v4l2_int_ioctl_func *)func;		\
+									\
+		return desc;						\
+	}
+
+V4L2_INT_WRAPPER_1(enum_fmt_cap, struct v4l2_fmtdesc, *);
+V4L2_INT_WRAPPER_1(g_fmt_cap, struct v4l2_format, *);
+V4L2_INT_WRAPPER_1(s_fmt_cap, struct v4l2_format, *);
+V4L2_INT_WRAPPER_1(try_fmt_cap, struct v4l2_format, *);
+V4L2_INT_WRAPPER_1(queryctrl, struct v4l2_queryctrl, *);
+V4L2_INT_WRAPPER_1(g_ctrl, struct v4l2_control, *);
+V4L2_INT_WRAPPER_1(s_ctrl, struct v4l2_control, *);
+V4L2_INT_WRAPPER_1(cropcap, struct v4l2_cropcap, *);
+V4L2_INT_WRAPPER_1(g_crop, struct v4l2_crop, *);
+V4L2_INT_WRAPPER_1(s_crop, struct v4l2_crop, *);
+V4L2_INT_WRAPPER_1(g_parm, struct v4l2_streamparm, *);
+V4L2_INT_WRAPPER_1(s_parm, struct v4l2_streamparm, *);
+V4L2_INT_WRAPPER_1(querystd, v4l2_std_id, *);
+V4L2_INT_WRAPPER_1(s_std, v4l2_std_id, *);
+V4L2_INT_WRAPPER_1(s_video_routing, struct v4l2_routing, *);
+
+V4L2_INT_WRAPPER_0(dev_init);
+V4L2_INT_WRAPPER_0(dev_exit);
+V4L2_INT_WRAPPER_1(s_power, enum v4l2_power, );
+V4L2_INT_WRAPPER_1(g_priv, void, *);
+V4L2_INT_WRAPPER_1(g_ifparm, struct v4l2_ifparm, *);
+V4L2_INT_WRAPPER_1(g_needs_reset, void, *);
+V4L2_INT_WRAPPER_1(enum_framesizes, struct v4l2_frmsizeenum, *);
+V4L2_INT_WRAPPER_1(enum_frameintervals, struct v4l2_frmivalenum, *);
+
+V4L2_INT_WRAPPER_0(reset);
+V4L2_INT_WRAPPER_0(init);
+
+#endif
diff --git a/drivers/staging/media/omap4iss/Kconfig b/drivers/staging/media/omap4iss/Kconfig
new file mode 100644
index 000000000000..b9fe753969bd
--- /dev/null
+++ b/drivers/staging/media/omap4iss/Kconfig
@@ -0,0 +1,12 @@
+config VIDEO_OMAP4
+	bool "OMAP 4 Camera support"
+	depends on VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API && I2C && ARCH_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/Makefile b/drivers/staging/media/omap4iss/Makefile
new file mode 100644
index 000000000000..a716ce936cf6
--- /dev/null
+++ b/drivers/staging/media/omap4iss/Makefile
@@ -0,0 +1,6 @@
+# Makefile for OMAP4 ISS driver
+
+omap4-iss-objs += \
+	iss.o iss_csi2.o iss_csiphy.o iss_ipipeif.o iss_ipipe.o iss_resizer.o iss_video.o
+
+obj-$(CONFIG_VIDEO_OMAP4) += omap4-iss.o
diff --git a/drivers/staging/media/omap4iss/TODO b/drivers/staging/media/omap4iss/TODO
new file mode 100644
index 000000000000..fcde88860a2c
--- /dev/null
+++ b/drivers/staging/media/omap4iss/TODO
@@ -0,0 +1,4 @@
+* Make the driver compile as a module
+* Fix FIFO/buffer overflows and underflows
+* Replace dummy resizer code with a real implementation
+* Fix checkpatch errors and warnings
diff --git a/drivers/staging/media/omap4iss/iss.c b/drivers/staging/media/omap4iss/iss.c
new file mode 100644
index 000000000000..61fbfcd13582
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss.c
@@ -0,0 +1,1563 @@
+/*
+ * TI OMAP4 ISS V4L2 Driver
+ *
+ * Copyright (C) 2012, Texas Instruments
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/sched.h>
+#include <linux/vmalloc.h>
+
+#include <media/v4l2-common.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+
+#include "iss.h"
+#include "iss_regs.h"
+
+#define ISS_PRINT_REGISTER(iss, name)\
+	dev_dbg(iss->dev, "###ISS " #name "=0x%08x\n", \
+		iss_reg_read(iss, OMAP4_ISS_MEM_TOP, ISS_##name))
+
+static void iss_print_status(struct iss_device *iss)
+{
+	dev_dbg(iss->dev, "-------------ISS HL Register dump-------------\n");
+
+	ISS_PRINT_REGISTER(iss, HL_REVISION);
+	ISS_PRINT_REGISTER(iss, HL_SYSCONFIG);
+	ISS_PRINT_REGISTER(iss, HL_IRQSTATUS(5));
+	ISS_PRINT_REGISTER(iss, HL_IRQENABLE_SET(5));
+	ISS_PRINT_REGISTER(iss, HL_IRQENABLE_CLR(5));
+	ISS_PRINT_REGISTER(iss, CTRL);
+	ISS_PRINT_REGISTER(iss, CLKCTRL);
+	ISS_PRINT_REGISTER(iss, CLKSTAT);
+
+	dev_dbg(iss->dev, "-----------------------------------------------\n");
+}
+
+/*
+ * omap4iss_flush - Post pending L3 bus writes by doing a register readback
+ * @iss: OMAP4 ISS device
+ *
+ * In order to force posting of pending writes, we need to write and
+ * readback the same register, in this case the revision register.
+ *
+ * See this link for reference:
+ *   http://www.mail-archive.com/linux-omap@vger.kernel.org/msg08149.html
+ */
+void omap4iss_flush(struct iss_device *iss)
+{
+	iss_reg_write(iss, OMAP4_ISS_MEM_TOP, ISS_HL_REVISION, 0);
+	iss_reg_read(iss, OMAP4_ISS_MEM_TOP, ISS_HL_REVISION);
+}
+
+/*
+ * iss_isp_enable_interrupts - Enable ISS ISP interrupts.
+ * @iss: OMAP4 ISS device
+ */
+static void omap4iss_isp_enable_interrupts(struct iss_device *iss)
+{
+	static const u32 isp_irq = ISP5_IRQ_OCP_ERR |
+				   ISP5_IRQ_RSZ_FIFO_IN_BLK_ERR |
+				   ISP5_IRQ_RSZ_FIFO_OVF |
+				   ISP5_IRQ_RSZ_INT_DMA |
+				   ISP5_IRQ_ISIF_INT(0);
+
+	/* Enable ISP interrupts */
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_IRQSTATUS(0), isp_irq);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_IRQENABLE_SET(0),
+		      isp_irq);
+}
+
+/*
+ * iss_isp_disable_interrupts - Disable ISS interrupts.
+ * @iss: OMAP4 ISS device
+ */
+static void omap4iss_isp_disable_interrupts(struct iss_device *iss)
+{
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_IRQENABLE_CLR(0), ~0);
+}
+
+/*
+ * iss_enable_interrupts - Enable ISS interrupts.
+ * @iss: OMAP4 ISS device
+ */
+static void iss_enable_interrupts(struct iss_device *iss)
+{
+	static const u32 hl_irq = ISS_HL_IRQ_CSIA | ISS_HL_IRQ_CSIB
+				| ISS_HL_IRQ_ISP(0);
+
+	/* Enable HL interrupts */
+	iss_reg_write(iss, OMAP4_ISS_MEM_TOP, ISS_HL_IRQSTATUS(5), hl_irq);
+	iss_reg_write(iss, OMAP4_ISS_MEM_TOP, ISS_HL_IRQENABLE_SET(5), hl_irq);
+
+	if (iss->regs[OMAP4_ISS_MEM_ISP_SYS1])
+		omap4iss_isp_enable_interrupts(iss);
+}
+
+/*
+ * iss_disable_interrupts - Disable ISS interrupts.
+ * @iss: OMAP4 ISS device
+ */
+static void iss_disable_interrupts(struct iss_device *iss)
+{
+	if (iss->regs[OMAP4_ISS_MEM_ISP_SYS1])
+		omap4iss_isp_disable_interrupts(iss);
+
+	iss_reg_write(iss, OMAP4_ISS_MEM_TOP, ISS_HL_IRQENABLE_CLR(5), ~0);
+}
+
+int omap4iss_get_external_info(struct iss_pipeline *pipe,
+			       struct media_link *link)
+{
+	struct iss_device *iss =
+		container_of(pipe, struct iss_video, pipe)->iss;
+	struct v4l2_subdev_format fmt;
+	struct v4l2_ctrl *ctrl;
+	int ret;
+
+	if (!pipe->external)
+		return 0;
+
+	if (pipe->external_rate)
+		return 0;
+
+	memset(&fmt, 0, sizeof(fmt));
+
+	fmt.pad = link->source->index;
+	fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	ret = v4l2_subdev_call(media_entity_to_v4l2_subdev(link->sink->entity),
+			       pad, get_fmt, NULL, &fmt);
+	if (ret < 0)
+		return -EPIPE;
+
+	pipe->external_bpp = omap4iss_video_format_info(fmt.format.code)->bpp;
+
+	ctrl = v4l2_ctrl_find(pipe->external->ctrl_handler,
+			      V4L2_CID_PIXEL_RATE);
+	if (ctrl == NULL) {
+		dev_warn(iss->dev, "no pixel rate control in subdev %s\n",
+			 pipe->external->name);
+		return -EPIPE;
+	}
+
+	pipe->external_rate = v4l2_ctrl_g_ctrl_int64(ctrl);
+
+	return 0;
+}
+
+/*
+ * Configure the bridge. Valid inputs are
+ *
+ * IPIPEIF_INPUT_CSI2A: CSI2a receiver
+ * IPIPEIF_INPUT_CSI2B: CSI2b receiver
+ *
+ * The bridge and lane shifter are configured according to the selected input
+ * and the ISP platform data.
+ */
+void omap4iss_configure_bridge(struct iss_device *iss,
+			       enum ipipeif_input_entity input)
+{
+	u32 issctrl_val;
+	u32 isp5ctrl_val;
+
+	issctrl_val = iss_reg_read(iss, OMAP4_ISS_MEM_TOP, ISS_CTRL);
+	issctrl_val &= ~ISS_CTRL_INPUT_SEL_MASK;
+	issctrl_val &= ~ISS_CTRL_CLK_DIV_MASK;
+
+	isp5ctrl_val = iss_reg_read(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_CTRL);
+
+	switch (input) {
+	case IPIPEIF_INPUT_CSI2A:
+		issctrl_val |= ISS_CTRL_INPUT_SEL_CSI2A;
+		break;
+
+	case IPIPEIF_INPUT_CSI2B:
+		issctrl_val |= ISS_CTRL_INPUT_SEL_CSI2B;
+		break;
+
+	default:
+		return;
+	}
+
+	issctrl_val |= ISS_CTRL_SYNC_DETECT_VS_RAISING;
+
+	isp5ctrl_val |= ISP5_CTRL_VD_PULSE_EXT | ISP5_CTRL_PSYNC_CLK_SEL |
+			ISP5_CTRL_SYNC_ENABLE;
+
+	iss_reg_write(iss, OMAP4_ISS_MEM_TOP, ISS_CTRL, issctrl_val);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_CTRL, isp5ctrl_val);
+}
+
+#if defined(DEBUG) && defined(ISS_ISR_DEBUG)
+static void iss_isr_dbg(struct iss_device *iss, u32 irqstatus)
+{
+	static const char * const name[] = {
+		"ISP_0",
+		"ISP_1",
+		"ISP_2",
+		"ISP_3",
+		"CSIA",
+		"CSIB",
+		"CCP2_0",
+		"CCP2_1",
+		"CCP2_2",
+		"CCP2_3",
+		"CBUFF",
+		"BTE",
+		"SIMCOP_0",
+		"SIMCOP_1",
+		"SIMCOP_2",
+		"SIMCOP_3",
+		"CCP2_8",
+		"HS_VS",
+		"18",
+		"19",
+		"20",
+		"21",
+		"22",
+		"23",
+		"24",
+		"25",
+		"26",
+		"27",
+		"28",
+		"29",
+		"30",
+		"31",
+	};
+	unsigned int i;
+
+	dev_dbg(iss->dev, "ISS IRQ: ");
+
+	for (i = 0; i < ARRAY_SIZE(name); i++) {
+		if ((1 << i) & irqstatus)
+			pr_cont("%s ", name[i]);
+	}
+	pr_cont("\n");
+}
+
+static void iss_isp_isr_dbg(struct iss_device *iss, u32 irqstatus)
+{
+	static const char * const name[] = {
+		"ISIF_0",
+		"ISIF_1",
+		"ISIF_2",
+		"ISIF_3",
+		"IPIPEREQ",
+		"IPIPELAST_PIX",
+		"IPIPEDMA",
+		"IPIPEBSC",
+		"IPIPEHST",
+		"IPIPEIF",
+		"AEW",
+		"AF",
+		"H3A",
+		"RSZ_REG",
+		"RSZ_LAST_PIX",
+		"RSZ_DMA",
+		"RSZ_CYC_RZA",
+		"RSZ_CYC_RZB",
+		"RSZ_FIFO_OVF",
+		"RSZ_FIFO_IN_BLK_ERR",
+		"20",
+		"21",
+		"RSZ_EOF0",
+		"RSZ_EOF1",
+		"H3A_EOF",
+		"IPIPE_EOF",
+		"26",
+		"IPIPE_DPC_INI",
+		"IPIPE_DPC_RNEW0",
+		"IPIPE_DPC_RNEW1",
+		"30",
+		"OCP_ERR",
+	};
+	unsigned int i;
+
+	dev_dbg(iss->dev, "ISP IRQ: ");
+
+	for (i = 0; i < ARRAY_SIZE(name); i++) {
+		if ((1 << i) & irqstatus)
+			pr_cont("%s ", name[i]);
+	}
+	pr_cont("\n");
+}
+#endif
+
+/*
+ * iss_isr - Interrupt Service Routine for ISS module.
+ * @irq: Not used currently.
+ * @_iss: Pointer to the OMAP4 ISS device
+ *
+ * Handles the corresponding callback if plugged in.
+ *
+ * Returns IRQ_HANDLED when IRQ was correctly handled, or IRQ_NONE when the
+ * IRQ wasn't handled.
+ */
+static irqreturn_t iss_isr(int irq, void *_iss)
+{
+	static const u32 ipipeif_events = ISP5_IRQ_IPIPEIF_IRQ |
+					  ISP5_IRQ_ISIF_INT(0);
+	static const u32 resizer_events = ISP5_IRQ_RSZ_FIFO_IN_BLK_ERR |
+					  ISP5_IRQ_RSZ_FIFO_OVF |
+					  ISP5_IRQ_RSZ_INT_DMA;
+	struct iss_device *iss = _iss;
+	u32 irqstatus;
+
+	irqstatus = iss_reg_read(iss, OMAP4_ISS_MEM_TOP, ISS_HL_IRQSTATUS(5));
+	iss_reg_write(iss, OMAP4_ISS_MEM_TOP, ISS_HL_IRQSTATUS(5), irqstatus);
+
+	if (irqstatus & ISS_HL_IRQ_CSIA)
+		omap4iss_csi2_isr(&iss->csi2a);
+
+	if (irqstatus & ISS_HL_IRQ_CSIB)
+		omap4iss_csi2_isr(&iss->csi2b);
+
+	if (irqstatus & ISS_HL_IRQ_ISP(0)) {
+		u32 isp_irqstatus = iss_reg_read(iss, OMAP4_ISS_MEM_ISP_SYS1,
+						 ISP5_IRQSTATUS(0));
+		iss_reg_write(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_IRQSTATUS(0),
+			      isp_irqstatus);
+
+		if (isp_irqstatus & ISP5_IRQ_OCP_ERR)
+			dev_dbg(iss->dev, "ISP5 OCP Error!\n");
+
+		if (isp_irqstatus & ipipeif_events) {
+			omap4iss_ipipeif_isr(&iss->ipipeif,
+					     isp_irqstatus & ipipeif_events);
+		}
+
+		if (isp_irqstatus & resizer_events)
+			omap4iss_resizer_isr(&iss->resizer,
+					     isp_irqstatus & resizer_events);
+
+#if defined(DEBUG) && defined(ISS_ISR_DEBUG)
+		iss_isp_isr_dbg(iss, isp_irqstatus);
+#endif
+	}
+
+	omap4iss_flush(iss);
+
+#if defined(DEBUG) && defined(ISS_ISR_DEBUG)
+	iss_isr_dbg(iss, irqstatus);
+#endif
+
+	return IRQ_HANDLED;
+}
+
+/* -----------------------------------------------------------------------------
+ * Pipeline power management
+ *
+ * Entities must be powered up when part of a pipeline that contains at least
+ * one open video device node.
+ *
+ * To achieve this use the entity use_count field to track the number of users.
+ * For entities corresponding to video device nodes the use_count field stores
+ * the users count of the node. For entities corresponding to subdevs the
+ * use_count field stores the total number of users of all video device nodes
+ * in the pipeline.
+ *
+ * The omap4iss_pipeline_pm_use() function must be called in the open() and
+ * close() handlers of video device nodes. It increments or decrements the use
+ * count of all subdev entities in the pipeline.
+ *
+ * To react to link management on powered pipelines, the link setup notification
+ * callback updates the use count of all entities in the source and sink sides
+ * of the link.
+ */
+
+/*
+ * iss_pipeline_pm_use_count - Count the number of users of a pipeline
+ * @entity: The entity
+ *
+ * Return the total number of users of all video device nodes in the pipeline.
+ */
+static int iss_pipeline_pm_use_count(struct media_entity *entity)
+{
+	struct media_entity_graph graph;
+	int use = 0;
+
+	media_entity_graph_walk_start(&graph, entity);
+
+	while ((entity = media_entity_graph_walk_next(&graph))) {
+		if (media_entity_type(entity) == MEDIA_ENT_T_DEVNODE)
+			use += entity->use_count;
+	}
+
+	return use;
+}
+
+/*
+ * iss_pipeline_pm_power_one - Apply power change to an entity
+ * @entity: The entity
+ * @change: Use count change
+ *
+ * Change the entity use count by @change. If the entity is a subdev update its
+ * power state by calling the core::s_power operation when the use count goes
+ * from 0 to != 0 or from != 0 to 0.
+ *
+ * Return 0 on success or a negative error code on failure.
+ */
+static int iss_pipeline_pm_power_one(struct media_entity *entity, int change)
+{
+	struct v4l2_subdev *subdev;
+
+	subdev = media_entity_type(entity) == MEDIA_ENT_T_V4L2_SUBDEV
+	       ? media_entity_to_v4l2_subdev(entity) : NULL;
+
+	if (entity->use_count == 0 && change > 0 && subdev != NULL) {
+		int ret;
+
+		ret = v4l2_subdev_call(subdev, core, s_power, 1);
+		if (ret < 0 && ret != -ENOIOCTLCMD)
+			return ret;
+	}
+
+	entity->use_count += change;
+	WARN_ON(entity->use_count < 0);
+
+	if (entity->use_count == 0 && change < 0 && subdev != NULL)
+		v4l2_subdev_call(subdev, core, s_power, 0);
+
+	return 0;
+}
+
+/*
+ * iss_pipeline_pm_power - Apply power change to all entities in a pipeline
+ * @entity: The entity
+ * @change: Use count change
+ *
+ * Walk the pipeline to update the use count and the power state of all non-node
+ * entities.
+ *
+ * Return 0 on success or a negative error code on failure.
+ */
+static int iss_pipeline_pm_power(struct media_entity *entity, int change)
+{
+	struct media_entity_graph graph;
+	struct media_entity *first = entity;
+	int ret = 0;
+
+	if (!change)
+		return 0;
+
+	media_entity_graph_walk_start(&graph, entity);
+
+	while (!ret && (entity = media_entity_graph_walk_next(&graph)))
+		if (media_entity_type(entity) != MEDIA_ENT_T_DEVNODE)
+			ret = iss_pipeline_pm_power_one(entity, change);
+
+	if (!ret)
+		return 0;
+
+	media_entity_graph_walk_start(&graph, first);
+
+	while ((first = media_entity_graph_walk_next(&graph))
+	       && first != entity)
+		if (media_entity_type(first) != MEDIA_ENT_T_DEVNODE)
+			iss_pipeline_pm_power_one(first, -change);
+
+	return ret;
+}
+
+/*
+ * omap4iss_pipeline_pm_use - Update the use count of an entity
+ * @entity: The entity
+ * @use: Use (1) or stop using (0) the entity
+ *
+ * Update the use count of all entities in the pipeline and power entities on or
+ * off accordingly.
+ *
+ * Return 0 on success or a negative error code on failure. Powering entities
+ * off is assumed to never fail. No failure can occur when the use parameter is
+ * set to 0.
+ */
+int omap4iss_pipeline_pm_use(struct media_entity *entity, int use)
+{
+	int change = use ? 1 : -1;
+	int ret;
+
+	mutex_lock(&entity->parent->graph_mutex);
+
+	/* Apply use count to node. */
+	entity->use_count += change;
+	WARN_ON(entity->use_count < 0);
+
+	/* Apply power change to connected non-nodes. */
+	ret = iss_pipeline_pm_power(entity, change);
+	if (ret < 0)
+		entity->use_count -= change;
+
+	mutex_unlock(&entity->parent->graph_mutex);
+
+	return ret;
+}
+
+/*
+ * iss_pipeline_link_notify - Link management notification callback
+ * @link: The link
+ * @flags: New link flags that will be applied
+ *
+ * React to link management on powered pipelines by updating the use count of
+ * all entities in the source and sink sides of the link. Entities are powered
+ * on or off accordingly.
+ *
+ * Return 0 on success or a negative error code on failure. Powering entities
+ * off is assumed to never fail. This function will not fail for disconnection
+ * events.
+ */
+static int iss_pipeline_link_notify(struct media_link *link, u32 flags,
+				    unsigned int notification)
+{
+	struct media_entity *source = link->source->entity;
+	struct media_entity *sink = link->sink->entity;
+	int source_use = iss_pipeline_pm_use_count(source);
+	int sink_use = iss_pipeline_pm_use_count(sink);
+	int ret;
+
+	if (notification == MEDIA_DEV_NOTIFY_POST_LINK_CH &&
+	    !(link->flags & MEDIA_LNK_FL_ENABLED)) {
+		/* Powering off entities is assumed to never fail. */
+		iss_pipeline_pm_power(source, -sink_use);
+		iss_pipeline_pm_power(sink, -source_use);
+		return 0;
+	}
+
+	if (notification == MEDIA_DEV_NOTIFY_POST_LINK_CH &&
+		(flags & MEDIA_LNK_FL_ENABLED)) {
+		ret = iss_pipeline_pm_power(source, sink_use);
+		if (ret < 0)
+			return ret;
+
+		ret = iss_pipeline_pm_power(sink, source_use);
+		if (ret < 0)
+			iss_pipeline_pm_power(source, -sink_use);
+
+		return ret;
+	}
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * Pipeline stream management
+ */
+
+/*
+ * iss_pipeline_enable - Enable streaming on a pipeline
+ * @pipe: ISS pipeline
+ * @mode: Stream mode (single shot or continuous)
+ *
+ * Walk the entities chain starting at the pipeline output video node and start
+ * all modules in the chain in the given mode.
+ *
+ * Return 0 if successful, or the return value of the failed video::s_stream
+ * operation otherwise.
+ */
+static int iss_pipeline_enable(struct iss_pipeline *pipe,
+			       enum iss_pipeline_stream_state mode)
+{
+	struct iss_device *iss = pipe->output->iss;
+	struct media_entity *entity;
+	struct media_pad *pad;
+	struct v4l2_subdev *subdev;
+	unsigned long flags;
+	int ret;
+
+	/* If one of the entities in the pipeline has crashed it will not work
+	 * properly. Refuse to start streaming in that case. This check must be
+	 * performed before the loop below to avoid starting entities if the
+	 * pipeline won't start anyway (those entities would then likely fail to
+	 * stop, making the problem worse).
+	 */
+	if (pipe->entities & iss->crashed)
+		return -EIO;
+
+	spin_lock_irqsave(&pipe->lock, flags);
+	pipe->state &= ~(ISS_PIPELINE_IDLE_INPUT | ISS_PIPELINE_IDLE_OUTPUT);
+	spin_unlock_irqrestore(&pipe->lock, flags);
+
+	pipe->do_propagation = false;
+
+	entity = &pipe->output->video.entity;
+	while (1) {
+		pad = &entity->pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			break;
+
+		pad = media_entity_remote_pad(pad);
+		if (pad == NULL ||
+		    media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+			break;
+
+		entity = pad->entity;
+		subdev = media_entity_to_v4l2_subdev(entity);
+
+		ret = v4l2_subdev_call(subdev, video, s_stream, mode);
+		if (ret < 0 && ret != -ENOIOCTLCMD)
+			return ret;
+	}
+	iss_print_status(pipe->output->iss);
+	return 0;
+}
+
+/*
+ * iss_pipeline_disable - Disable streaming on a pipeline
+ * @pipe: ISS pipeline
+ *
+ * Walk the entities chain starting at the pipeline output video node and stop
+ * all modules in the chain. Wait synchronously for the modules to be stopped if
+ * necessary.
+ */
+static int iss_pipeline_disable(struct iss_pipeline *pipe)
+{
+	struct iss_device *iss = pipe->output->iss;
+	struct media_entity *entity;
+	struct media_pad *pad;
+	struct v4l2_subdev *subdev;
+	int failure = 0;
+	int ret;
+
+	entity = &pipe->output->video.entity;
+	while (1) {
+		pad = &entity->pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			break;
+
+		pad = media_entity_remote_pad(pad);
+		if (pad == NULL ||
+		    media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+			break;
+
+		entity = pad->entity;
+		subdev = media_entity_to_v4l2_subdev(entity);
+
+		ret = v4l2_subdev_call(subdev, video, s_stream, 0);
+		if (ret < 0) {
+			dev_dbg(iss->dev, "%s: module stop timeout.\n",
+				subdev->name);
+			/* If the entity failed to stopped, assume it has
+			 * crashed. Mark it as such, the ISS will be reset when
+			 * applications will release it.
+			 */
+			iss->crashed |= 1U << subdev->entity.id;
+			failure = -ETIMEDOUT;
+		}
+	}
+
+	return failure;
+}
+
+/*
+ * omap4iss_pipeline_set_stream - Enable/disable streaming on a pipeline
+ * @pipe: ISS pipeline
+ * @state: Stream state (stopped, single shot or continuous)
+ *
+ * Set the pipeline to the given stream state. Pipelines can be started in
+ * single-shot or continuous mode.
+ *
+ * Return 0 if successful, or the return value of the failed video::s_stream
+ * operation otherwise. The pipeline state is not updated when the operation
+ * fails, except when stopping the pipeline.
+ */
+int omap4iss_pipeline_set_stream(struct iss_pipeline *pipe,
+				 enum iss_pipeline_stream_state state)
+{
+	int ret;
+
+	if (state == ISS_PIPELINE_STREAM_STOPPED)
+		ret = iss_pipeline_disable(pipe);
+	else
+		ret = iss_pipeline_enable(pipe, state);
+
+	if (ret == 0 || state == ISS_PIPELINE_STREAM_STOPPED)
+		pipe->stream_state = state;
+
+	return ret;
+}
+
+/*
+ * omap4iss_pipeline_cancel_stream - Cancel stream on a pipeline
+ * @pipe: ISS pipeline
+ *
+ * Cancelling a stream mark all buffers on all video nodes in the pipeline as
+ * erroneous and makes sure no new buffer can be queued. This function is called
+ * when a fatal error that prevents any further operation on the pipeline
+ * occurs.
+ */
+void omap4iss_pipeline_cancel_stream(struct iss_pipeline *pipe)
+{
+	if (pipe->input)
+		omap4iss_video_cancel_stream(pipe->input);
+	if (pipe->output)
+		omap4iss_video_cancel_stream(pipe->output);
+}
+
+/*
+ * iss_pipeline_is_last - Verify if entity has an enabled link to the output
+ *			  video node
+ * @me: ISS module's media entity
+ *
+ * Returns 1 if the entity has an enabled link to the output video node or 0
+ * otherwise. It's true only while pipeline can have no more than one output
+ * node.
+ */
+static int iss_pipeline_is_last(struct media_entity *me)
+{
+	struct iss_pipeline *pipe;
+	struct media_pad *pad;
+
+	if (!me->pipe)
+		return 0;
+	pipe = to_iss_pipeline(me);
+	if (pipe->stream_state == ISS_PIPELINE_STREAM_STOPPED)
+		return 0;
+	pad = media_entity_remote_pad(&pipe->output->pad);
+	return pad->entity == me;
+}
+
+static int iss_reset(struct iss_device *iss)
+{
+	unsigned long timeout = 0;
+
+	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);
+	}
+
+	iss->crashed = 0;
+	return 0;
+}
+
+static int iss_isp_reset(struct iss_device *iss)
+{
+	unsigned long timeout = 0;
+
+	/* Fist, ensure that the ISP is IDLE (no transactions happening) */
+	iss_reg_update(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_SYSCONFIG,
+		       ISP5_SYSCONFIG_STANDBYMODE_MASK,
+		       ISP5_SYSCONFIG_STANDBYMODE_SMART);
+
+	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);
+	}
+
+	/* 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);
+	}
+
+	return 0;
+}
+
+/*
+ * iss_module_sync_idle - Helper to sync module with its idle state
+ * @me: ISS submodule's media entity
+ * @wait: ISS submodule's wait queue for streamoff/interrupt synchronization
+ * @stopping: flag which tells module wants to stop
+ *
+ * This function checks if ISS submodule needs to wait for next interrupt. If
+ * yes, makes the caller to sleep while waiting for such event.
+ */
+int omap4iss_module_sync_idle(struct media_entity *me, wait_queue_head_t *wait,
+			      atomic_t *stopping)
+{
+	struct iss_pipeline *pipe = to_iss_pipeline(me);
+	struct iss_video *video = pipe->output;
+	unsigned long flags;
+
+	if (pipe->stream_state == ISS_PIPELINE_STREAM_STOPPED ||
+	    (pipe->stream_state == ISS_PIPELINE_STREAM_SINGLESHOT &&
+	     !iss_pipeline_ready(pipe)))
+		return 0;
+
+	/*
+	 * atomic_set() doesn't include memory barrier on ARM platform for SMP
+	 * scenario. We'll call it here to avoid race conditions.
+	 */
+	atomic_set(stopping, 1);
+	smp_wmb();
+
+	/*
+	 * If module is the last one, it's writing to memory. In this case,
+	 * it's necessary to check if the module is already paused due to
+	 * DMA queue underrun or if it has to wait for next interrupt to be
+	 * idle.
+	 * If it isn't the last one, the function won't sleep but *stopping
+	 * will still be set to warn next submodule caller's interrupt the
+	 * module wants to be idle.
+	 */
+	if (!iss_pipeline_is_last(me))
+		return 0;
+
+	spin_lock_irqsave(&video->qlock, flags);
+	if (video->dmaqueue_flags & ISS_VIDEO_DMAQUEUE_UNDERRUN) {
+		spin_unlock_irqrestore(&video->qlock, flags);
+		atomic_set(stopping, 0);
+		smp_wmb();
+		return 0;
+	}
+	spin_unlock_irqrestore(&video->qlock, flags);
+	if (!wait_event_timeout(*wait, !atomic_read(stopping),
+				msecs_to_jiffies(1000))) {
+		atomic_set(stopping, 0);
+		smp_wmb();
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+/*
+ * omap4iss_module_sync_is_stopped - Helper to verify if module was stopping
+ * @wait: ISS submodule's wait queue for streamoff/interrupt synchronization
+ * @stopping: flag which tells module wants to stop
+ *
+ * This function checks if ISS submodule was stopping. In case of yes, it
+ * notices the caller by setting stopping to 0 and waking up the wait queue.
+ * Returns 1 if it was stopping or 0 otherwise.
+ */
+int omap4iss_module_sync_is_stopping(wait_queue_head_t *wait,
+				     atomic_t *stopping)
+{
+	if (atomic_cmpxchg(stopping, 1, 0)) {
+		wake_up(wait);
+		return 1;
+	}
+
+	return 0;
+}
+
+/* --------------------------------------------------------------------------
+ * Clock management
+ */
+
+#define ISS_CLKCTRL_MASK	(ISS_CLKCTRL_CSI2_A |\
+				 ISS_CLKCTRL_CSI2_B |\
+				 ISS_CLKCTRL_ISP)
+
+static int __iss_subclk_update(struct iss_device *iss)
+{
+	u32 clk = 0;
+	int ret = 0, timeout = 1000;
+
+	if (iss->subclk_resources & OMAP4_ISS_SUBCLK_CSI2_A)
+		clk |= ISS_CLKCTRL_CSI2_A;
+
+	if (iss->subclk_resources & OMAP4_ISS_SUBCLK_CSI2_B)
+		clk |= ISS_CLKCTRL_CSI2_B;
+
+	if (iss->subclk_resources & OMAP4_ISS_SUBCLK_ISP)
+		clk |= ISS_CLKCTRL_ISP;
+
+	iss_reg_update(iss, OMAP4_ISS_MEM_TOP, ISS_CLKCTRL,
+		       ISS_CLKCTRL_MASK, clk);
+
+	/* Wait for HW assertion */
+	while (--timeout > 0) {
+		udelay(1);
+		if ((iss_reg_read(iss, OMAP4_ISS_MEM_TOP, ISS_CLKSTAT) &
+		    ISS_CLKCTRL_MASK) == clk)
+			break;
+	}
+
+	if (!timeout)
+		ret = -EBUSY;
+
+	return ret;
+}
+
+int omap4iss_subclk_enable(struct iss_device *iss,
+			    enum iss_subclk_resource res)
+{
+	iss->subclk_resources |= res;
+
+	return __iss_subclk_update(iss);
+}
+
+int omap4iss_subclk_disable(struct iss_device *iss,
+			     enum iss_subclk_resource res)
+{
+	iss->subclk_resources &= ~res;
+
+	return __iss_subclk_update(iss);
+}
+
+#define ISS_ISP5_CLKCTRL_MASK	(ISP5_CTRL_BL_CLK_ENABLE |\
+				 ISP5_CTRL_ISIF_CLK_ENABLE |\
+				 ISP5_CTRL_H3A_CLK_ENABLE |\
+				 ISP5_CTRL_RSZ_CLK_ENABLE |\
+				 ISP5_CTRL_IPIPE_CLK_ENABLE |\
+				 ISP5_CTRL_IPIPEIF_CLK_ENABLE)
+
+static void __iss_isp_subclk_update(struct iss_device *iss)
+{
+	u32 clk = 0;
+
+	if (iss->isp_subclk_resources & OMAP4_ISS_ISP_SUBCLK_ISIF)
+		clk |= ISP5_CTRL_ISIF_CLK_ENABLE;
+
+	if (iss->isp_subclk_resources & OMAP4_ISS_ISP_SUBCLK_H3A)
+		clk |= ISP5_CTRL_H3A_CLK_ENABLE;
+
+	if (iss->isp_subclk_resources & OMAP4_ISS_ISP_SUBCLK_RSZ)
+		clk |= ISP5_CTRL_RSZ_CLK_ENABLE;
+
+	if (iss->isp_subclk_resources & OMAP4_ISS_ISP_SUBCLK_IPIPE)
+		clk |= ISP5_CTRL_IPIPE_CLK_ENABLE;
+
+	if (iss->isp_subclk_resources & OMAP4_ISS_ISP_SUBCLK_IPIPEIF)
+		clk |= ISP5_CTRL_IPIPEIF_CLK_ENABLE;
+
+	if (clk)
+		clk |= ISP5_CTRL_BL_CLK_ENABLE;
+
+	iss_reg_update(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_CTRL,
+		       ISS_ISP5_CLKCTRL_MASK, clk);
+}
+
+void omap4iss_isp_subclk_enable(struct iss_device *iss,
+				enum iss_isp_subclk_resource res)
+{
+	iss->isp_subclk_resources |= res;
+
+	__iss_isp_subclk_update(iss);
+}
+
+void omap4iss_isp_subclk_disable(struct iss_device *iss,
+				 enum iss_isp_subclk_resource res)
+{
+	iss->isp_subclk_resources &= ~res;
+
+	__iss_isp_subclk_update(iss);
+}
+
+/*
+ * iss_enable_clocks - Enable ISS clocks
+ * @iss: OMAP4 ISS device
+ *
+ * Return 0 if successful, or clk_enable return value if any of tthem fails.
+ */
+static int iss_enable_clocks(struct iss_device *iss)
+{
+	int ret;
+
+	ret = clk_enable(iss->iss_fck);
+	if (ret) {
+		dev_err(iss->dev, "clk_enable iss_fck failed\n");
+		return ret;
+	}
+
+	ret = clk_enable(iss->iss_ctrlclk);
+	if (ret) {
+		dev_err(iss->dev, "clk_enable iss_ctrlclk failed\n");
+		clk_disable(iss->iss_fck);
+		return ret;
+	}
+
+	return 0;
+}
+
+/*
+ * iss_disable_clocks - Disable ISS clocks
+ * @iss: OMAP4 ISS device
+ */
+static void iss_disable_clocks(struct iss_device *iss)
+{
+	clk_disable(iss->iss_ctrlclk);
+	clk_disable(iss->iss_fck);
+}
+
+static void iss_put_clocks(struct iss_device *iss)
+{
+	if (iss->iss_fck) {
+		clk_put(iss->iss_fck);
+		iss->iss_fck = NULL;
+	}
+
+	if (iss->iss_ctrlclk) {
+		clk_put(iss->iss_ctrlclk);
+		iss->iss_ctrlclk = NULL;
+	}
+}
+
+static int iss_get_clocks(struct iss_device *iss)
+{
+	iss->iss_fck = clk_get(iss->dev, "iss_fck");
+	if (IS_ERR(iss->iss_fck)) {
+		dev_err(iss->dev, "Unable to get iss_fck clock info\n");
+		iss_put_clocks(iss);
+		return PTR_ERR(iss->iss_fck);
+	}
+
+	iss->iss_ctrlclk = clk_get(iss->dev, "iss_ctrlclk");
+	if (IS_ERR(iss->iss_ctrlclk)) {
+		dev_err(iss->dev, "Unable to get iss_ctrlclk clock info\n");
+		iss_put_clocks(iss);
+		return PTR_ERR(iss->iss_fck);
+	}
+
+	return 0;
+}
+
+/*
+ * omap4iss_get - Acquire the ISS resource.
+ *
+ * Initializes the clocks for the first acquire.
+ *
+ * Increment the reference count on the ISS. If the first reference is taken,
+ * enable clocks and power-up all submodules.
+ *
+ * Return a pointer to the ISS device structure, or NULL if an error occurred.
+ */
+struct iss_device *omap4iss_get(struct iss_device *iss)
+{
+	struct iss_device *__iss = iss;
+
+	if (iss == NULL)
+		return NULL;
+
+	mutex_lock(&iss->iss_mutex);
+	if (iss->ref_count > 0)
+		goto out;
+
+	if (iss_enable_clocks(iss) < 0) {
+		__iss = NULL;
+		goto out;
+	}
+
+	iss_enable_interrupts(iss);
+
+out:
+	if (__iss != NULL)
+		iss->ref_count++;
+	mutex_unlock(&iss->iss_mutex);
+
+	return __iss;
+}
+
+/*
+ * omap4iss_put - Release the ISS
+ *
+ * Decrement the reference count on the ISS. If the last reference is released,
+ * power-down all submodules, disable clocks and free temporary buffers.
+ */
+void omap4iss_put(struct iss_device *iss)
+{
+	if (iss == NULL)
+		return;
+
+	mutex_lock(&iss->iss_mutex);
+	BUG_ON(iss->ref_count == 0);
+	if (--iss->ref_count == 0) {
+		iss_disable_interrupts(iss);
+		/* Reset the ISS if an entity has failed to stop. This is the
+		 * only way to recover from such conditions, although it would
+		 * be worth investigating whether resetting the ISP only can't
+		 * fix the problem in some cases.
+		 */
+		if (iss->crashed)
+			iss_reset(iss);
+		iss_disable_clocks(iss);
+	}
+	mutex_unlock(&iss->iss_mutex);
+}
+
+static int iss_map_mem_resource(struct platform_device *pdev,
+				struct iss_device *iss,
+				enum iss_mem_resources res)
+{
+	struct resource *mem;
+
+	/* request the mem region for the camera registers */
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, res);
+	if (!mem) {
+		dev_err(iss->dev, "no mem resource?\n");
+		return -ENODEV;
+	}
+
+	if (!request_mem_region(mem->start, resource_size(mem), pdev->name)) {
+		dev_err(iss->dev,
+			"cannot reserve camera register I/O region\n");
+		return -ENODEV;
+	}
+	iss->res[res] = mem;
+
+	/* map the region */
+	iss->regs[res] = ioremap_nocache(mem->start, resource_size(mem));
+	if (!iss->regs[res]) {
+		dev_err(iss->dev, "cannot map camera register I/O region\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static void iss_unregister_entities(struct iss_device *iss)
+{
+	omap4iss_resizer_unregister_entities(&iss->resizer);
+	omap4iss_ipipe_unregister_entities(&iss->ipipe);
+	omap4iss_ipipeif_unregister_entities(&iss->ipipeif);
+	omap4iss_csi2_unregister_entities(&iss->csi2a);
+	omap4iss_csi2_unregister_entities(&iss->csi2b);
+
+	v4l2_device_unregister(&iss->v4l2_dev);
+	media_device_unregister(&iss->media_dev);
+}
+
+/*
+ * iss_register_subdev_group - Register a group of subdevices
+ * @iss: OMAP4 ISS device
+ * @board_info: I2C subdevs board information array
+ *
+ * Register all I2C subdevices in the board_info array. The array must be
+ * terminated by a NULL entry, and the first entry must be the sensor.
+ *
+ * Return a pointer to the sensor media entity if it has been successfully
+ * registered, or NULL otherwise.
+ */
+static struct v4l2_subdev *
+iss_register_subdev_group(struct iss_device *iss,
+		     struct iss_subdev_i2c_board_info *board_info)
+{
+	struct v4l2_subdev *sensor = NULL;
+	unsigned int first;
+
+	if (board_info->board_info == NULL)
+		return NULL;
+
+	for (first = 1; board_info->board_info; ++board_info, first = 0) {
+		struct v4l2_subdev *subdev;
+		struct i2c_adapter *adapter;
+
+		adapter = i2c_get_adapter(board_info->i2c_adapter_id);
+		if (adapter == NULL) {
+			dev_err(iss->dev,
+				"%s: Unable to get I2C adapter %d for device %s\n",
+				__func__, board_info->i2c_adapter_id,
+				board_info->board_info->type);
+			continue;
+		}
+
+		subdev = v4l2_i2c_new_subdev_board(&iss->v4l2_dev, adapter,
+				board_info->board_info, NULL);
+		if (subdev == NULL) {
+			dev_err(iss->dev, "%s: Unable to register subdev %s\n",
+				__func__, board_info->board_info->type);
+			continue;
+		}
+
+		if (first)
+			sensor = subdev;
+	}
+
+	return sensor;
+}
+
+static int iss_register_entities(struct iss_device *iss)
+{
+	struct iss_platform_data *pdata = iss->pdata;
+	struct iss_v4l2_subdevs_group *subdevs;
+	int ret;
+
+	iss->media_dev.dev = iss->dev;
+	strlcpy(iss->media_dev.model, "TI OMAP4 ISS",
+		sizeof(iss->media_dev.model));
+	iss->media_dev.hw_revision = iss->revision;
+	iss->media_dev.link_notify = iss_pipeline_link_notify;
+	ret = media_device_register(&iss->media_dev);
+	if (ret < 0) {
+		dev_err(iss->dev, "%s: Media device registration failed (%d)\n",
+			__func__, ret);
+		return ret;
+	}
+
+	iss->v4l2_dev.mdev = &iss->media_dev;
+	ret = v4l2_device_register(iss->dev, &iss->v4l2_dev);
+	if (ret < 0) {
+		dev_err(iss->dev, "%s: V4L2 device registration failed (%d)\n",
+			__func__, ret);
+		goto done;
+	}
+
+	/* Register internal entities */
+	ret = omap4iss_csi2_register_entities(&iss->csi2a, &iss->v4l2_dev);
+	if (ret < 0)
+		goto done;
+
+	ret = omap4iss_csi2_register_entities(&iss->csi2b, &iss->v4l2_dev);
+	if (ret < 0)
+		goto done;
+
+	ret = omap4iss_ipipeif_register_entities(&iss->ipipeif, &iss->v4l2_dev);
+	if (ret < 0)
+		goto done;
+
+	ret = omap4iss_ipipe_register_entities(&iss->ipipe, &iss->v4l2_dev);
+	if (ret < 0)
+		goto done;
+
+	ret = omap4iss_resizer_register_entities(&iss->resizer, &iss->v4l2_dev);
+	if (ret < 0)
+		goto done;
+
+	/* Register external entities */
+	for (subdevs = pdata->subdevs; subdevs && subdevs->subdevs; ++subdevs) {
+		struct v4l2_subdev *sensor;
+		struct media_entity *input;
+		unsigned int flags;
+		unsigned int pad;
+
+		sensor = iss_register_subdev_group(iss, subdevs->subdevs);
+		if (sensor == NULL)
+			continue;
+
+		sensor->host_priv = subdevs;
+
+		/* Connect the sensor to the correct interface module.
+		 * CSI2a receiver through CSIPHY1, or
+		 * CSI2b receiver through CSIPHY2
+		 */
+		switch (subdevs->interface) {
+		case ISS_INTERFACE_CSI2A_PHY1:
+			input = &iss->csi2a.subdev.entity;
+			pad = CSI2_PAD_SINK;
+			flags = MEDIA_LNK_FL_IMMUTABLE
+			      | MEDIA_LNK_FL_ENABLED;
+			break;
+
+		case ISS_INTERFACE_CSI2B_PHY2:
+			input = &iss->csi2b.subdev.entity;
+			pad = CSI2_PAD_SINK;
+			flags = MEDIA_LNK_FL_IMMUTABLE
+			      | MEDIA_LNK_FL_ENABLED;
+			break;
+
+		default:
+			dev_err(iss->dev, "%s: invalid interface type %u\n",
+				__func__, subdevs->interface);
+			ret = -EINVAL;
+			goto done;
+		}
+
+		ret = media_entity_create_link(&sensor->entity, 0, input, pad,
+					       flags);
+		if (ret < 0)
+			goto done;
+	}
+
+	ret = v4l2_device_register_subdev_nodes(&iss->v4l2_dev);
+
+done:
+	if (ret < 0)
+		iss_unregister_entities(iss);
+
+	return ret;
+}
+
+static void iss_cleanup_modules(struct iss_device *iss)
+{
+	omap4iss_csi2_cleanup(iss);
+	omap4iss_ipipeif_cleanup(iss);
+	omap4iss_ipipe_cleanup(iss);
+	omap4iss_resizer_cleanup(iss);
+}
+
+static int iss_initialize_modules(struct iss_device *iss)
+{
+	int ret;
+
+	ret = omap4iss_csiphy_init(iss);
+	if (ret < 0) {
+		dev_err(iss->dev, "CSI PHY initialization failed\n");
+		goto error_csiphy;
+	}
+
+	ret = omap4iss_csi2_init(iss);
+	if (ret < 0) {
+		dev_err(iss->dev, "CSI2 initialization failed\n");
+		goto error_csi2;
+	}
+
+	ret = omap4iss_ipipeif_init(iss);
+	if (ret < 0) {
+		dev_err(iss->dev, "ISP IPIPEIF initialization failed\n");
+		goto error_ipipeif;
+	}
+
+	ret = omap4iss_ipipe_init(iss);
+	if (ret < 0) {
+		dev_err(iss->dev, "ISP IPIPE initialization failed\n");
+		goto error_ipipe;
+	}
+
+	ret = omap4iss_resizer_init(iss);
+	if (ret < 0) {
+		dev_err(iss->dev, "ISP RESIZER initialization failed\n");
+		goto error_resizer;
+	}
+
+	/* Connect the submodules. */
+	ret = media_entity_create_link(
+			&iss->csi2a.subdev.entity, CSI2_PAD_SOURCE,
+			&iss->ipipeif.subdev.entity, IPIPEIF_PAD_SINK, 0);
+	if (ret < 0)
+		goto error_link;
+
+	ret = media_entity_create_link(
+			&iss->csi2b.subdev.entity, CSI2_PAD_SOURCE,
+			&iss->ipipeif.subdev.entity, IPIPEIF_PAD_SINK, 0);
+	if (ret < 0)
+		goto error_link;
+
+	ret = media_entity_create_link(
+			&iss->ipipeif.subdev.entity, IPIPEIF_PAD_SOURCE_VP,
+			&iss->resizer.subdev.entity, RESIZER_PAD_SINK, 0);
+	if (ret < 0)
+		goto error_link;
+
+	ret = media_entity_create_link(
+			&iss->ipipeif.subdev.entity, IPIPEIF_PAD_SOURCE_VP,
+			&iss->ipipe.subdev.entity, IPIPE_PAD_SINK, 0);
+	if (ret < 0)
+		goto error_link;
+
+	ret = media_entity_create_link(
+			&iss->ipipe.subdev.entity, IPIPE_PAD_SOURCE_VP,
+			&iss->resizer.subdev.entity, RESIZER_PAD_SINK, 0);
+	if (ret < 0)
+		goto error_link;
+
+	return 0;
+
+error_link:
+	omap4iss_resizer_cleanup(iss);
+error_resizer:
+	omap4iss_ipipe_cleanup(iss);
+error_ipipe:
+	omap4iss_ipipeif_cleanup(iss);
+error_ipipeif:
+	omap4iss_csi2_cleanup(iss);
+error_csi2:
+error_csiphy:
+	return ret;
+}
+
+static int iss_probe(struct platform_device *pdev)
+{
+	struct iss_platform_data *pdata = pdev->dev.platform_data;
+	struct iss_device *iss;
+	unsigned int i;
+	int ret;
+
+	if (pdata == NULL)
+		return -EINVAL;
+
+	iss = kzalloc(sizeof(*iss), GFP_KERNEL);
+	if (!iss) {
+		dev_err(&pdev->dev, "Could not allocate memory\n");
+		return -ENOMEM;
+	}
+
+	mutex_init(&iss->iss_mutex);
+
+	iss->dev = &pdev->dev;
+	iss->pdata = pdata;
+
+	iss->raw_dmamask = DMA_BIT_MASK(32);
+	iss->dev->dma_mask = &iss->raw_dmamask;
+	iss->dev->coherent_dma_mask = DMA_BIT_MASK(32);
+
+	platform_set_drvdata(pdev, iss);
+
+	/* Clocks */
+	ret = iss_map_mem_resource(pdev, iss, OMAP4_ISS_MEM_TOP);
+	if (ret < 0)
+		goto error;
+
+	ret = iss_get_clocks(iss);
+	if (ret < 0)
+		goto error;
+
+	if (omap4iss_get(iss) == NULL)
+		goto error;
+
+	ret = iss_reset(iss);
+	if (ret < 0)
+		goto error_iss;
+
+	iss->revision = iss_reg_read(iss, OMAP4_ISS_MEM_TOP, ISS_HL_REVISION);
+	dev_info(iss->dev, "Revision %08x found\n", iss->revision);
+
+	for (i = 1; i < OMAP4_ISS_MEM_LAST; i++) {
+		ret = iss_map_mem_resource(pdev, iss, i);
+		if (ret)
+			goto error_iss;
+	}
+
+	/* Configure BTE BW_LIMITER field to max recommended value (1 GB) */
+	iss_reg_update(iss, OMAP4_ISS_MEM_BTE, BTE_CTRL,
+		       BTE_CTRL_BW_LIMITER_MASK,
+		       18 << BTE_CTRL_BW_LIMITER_SHIFT);
+
+	/* Perform ISP reset */
+	ret = omap4iss_subclk_enable(iss, OMAP4_ISS_SUBCLK_ISP);
+	if (ret < 0)
+		goto error_iss;
+
+	ret = iss_isp_reset(iss);
+	if (ret < 0)
+		goto error_iss;
+
+	dev_info(iss->dev, "ISP Revision %08x found\n",
+		 iss_reg_read(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_REVISION));
+
+	/* Interrupt */
+	iss->irq_num = platform_get_irq(pdev, 0);
+	if (iss->irq_num <= 0) {
+		dev_err(iss->dev, "No IRQ resource\n");
+		ret = -ENODEV;
+		goto error_iss;
+	}
+
+	if (request_irq(iss->irq_num, iss_isr, IRQF_SHARED, "OMAP4 ISS", iss)) {
+		dev_err(iss->dev, "Unable to request IRQ\n");
+		ret = -EINVAL;
+		goto error_iss;
+	}
+
+	/* Entities */
+	ret = iss_initialize_modules(iss);
+	if (ret < 0)
+		goto error_irq;
+
+	ret = iss_register_entities(iss);
+	if (ret < 0)
+		goto error_modules;
+
+	omap4iss_put(iss);
+
+	return 0;
+
+error_modules:
+	iss_cleanup_modules(iss);
+error_irq:
+	free_irq(iss->irq_num, iss);
+error_iss:
+	omap4iss_put(iss);
+error:
+	iss_put_clocks(iss);
+
+	for (i = 0; i < OMAP4_ISS_MEM_LAST; i++) {
+		if (iss->regs[i]) {
+			iounmap(iss->regs[i]);
+			iss->regs[i] = NULL;
+		}
+
+		if (iss->res[i]) {
+			release_mem_region(iss->res[i]->start,
+					   resource_size(iss->res[i]));
+			iss->res[i] = NULL;
+		}
+	}
+	platform_set_drvdata(pdev, NULL);
+
+	mutex_destroy(&iss->iss_mutex);
+	kfree(iss);
+
+	return ret;
+}
+
+static int iss_remove(struct platform_device *pdev)
+{
+	struct iss_device *iss = platform_get_drvdata(pdev);
+	unsigned int i;
+
+	iss_unregister_entities(iss);
+	iss_cleanup_modules(iss);
+
+	free_irq(iss->irq_num, iss);
+	iss_put_clocks(iss);
+
+	for (i = 0; i < OMAP4_ISS_MEM_LAST; i++) {
+		if (iss->regs[i]) {
+			iounmap(iss->regs[i]);
+			iss->regs[i] = NULL;
+		}
+
+		if (iss->res[i]) {
+			release_mem_region(iss->res[i]->start,
+					   resource_size(iss->res[i]));
+			iss->res[i] = NULL;
+		}
+	}
+
+	kfree(iss);
+
+	return 0;
+}
+
+static struct platform_device_id omap4iss_id_table[] = {
+	{ "omap4iss", 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(platform, omap4iss_id_table);
+
+static struct platform_driver iss_driver = {
+	.probe		= iss_probe,
+	.remove		= iss_remove,
+	.id_table	= omap4iss_id_table,
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "omap4iss",
+	},
+};
+
+module_platform_driver(iss_driver);
+
+MODULE_DESCRIPTION("TI OMAP4 ISS driver");
+MODULE_AUTHOR("Sergio Aguirre <sergio.a.aguirre@gmail.com>");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(ISS_VIDEO_DRIVER_VERSION);
diff --git a/drivers/staging/media/omap4iss/iss.h b/drivers/staging/media/omap4iss/iss.h
new file mode 100644
index 000000000000..346db9233171
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss.h
@@ -0,0 +1,236 @@
+/*
+ * TI OMAP4 ISS V4L2 Driver
+ *
+ * Copyright (C) 2012 Texas Instruments.
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@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.
+ */
+
+#ifndef _OMAP4_ISS_H_
+#define _OMAP4_ISS_H_
+
+#include <media/v4l2-device.h>
+#include <linux/device.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/wait.h>
+
+#include <media/omap4iss.h>
+
+#include "iss_regs.h"
+#include "iss_csiphy.h"
+#include "iss_csi2.h"
+#include "iss_ipipeif.h"
+#include "iss_ipipe.h"
+#include "iss_resizer.h"
+
+#define to_iss_device(ptr_module)				\
+	container_of(ptr_module, struct iss_device, ptr_module)
+#define to_device(ptr_module)						\
+	(to_iss_device(ptr_module)->dev)
+
+enum iss_mem_resources {
+	OMAP4_ISS_MEM_TOP,
+	OMAP4_ISS_MEM_CSI2_A_REGS1,
+	OMAP4_ISS_MEM_CAMERARX_CORE1,
+	OMAP4_ISS_MEM_CSI2_B_REGS1,
+	OMAP4_ISS_MEM_CAMERARX_CORE2,
+	OMAP4_ISS_MEM_BTE,
+	OMAP4_ISS_MEM_ISP_SYS1,
+	OMAP4_ISS_MEM_ISP_RESIZER,
+	OMAP4_ISS_MEM_ISP_IPIPE,
+	OMAP4_ISS_MEM_ISP_ISIF,
+	OMAP4_ISS_MEM_ISP_IPIPEIF,
+	OMAP4_ISS_MEM_LAST,
+};
+
+enum iss_subclk_resource {
+	OMAP4_ISS_SUBCLK_SIMCOP		= (1 << 0),
+	OMAP4_ISS_SUBCLK_ISP		= (1 << 1),
+	OMAP4_ISS_SUBCLK_CSI2_A		= (1 << 2),
+	OMAP4_ISS_SUBCLK_CSI2_B		= (1 << 3),
+	OMAP4_ISS_SUBCLK_CCP2		= (1 << 4),
+};
+
+enum iss_isp_subclk_resource {
+	OMAP4_ISS_ISP_SUBCLK_BL		= (1 << 0),
+	OMAP4_ISS_ISP_SUBCLK_ISIF	= (1 << 1),
+	OMAP4_ISS_ISP_SUBCLK_H3A	= (1 << 2),
+	OMAP4_ISS_ISP_SUBCLK_RSZ	= (1 << 3),
+	OMAP4_ISS_ISP_SUBCLK_IPIPE	= (1 << 4),
+	OMAP4_ISS_ISP_SUBCLK_IPIPEIF	= (1 << 5),
+};
+
+/*
+ * struct iss_reg - Structure for ISS register values.
+ * @reg: 32-bit Register address.
+ * @val: 32-bit Register value.
+ */
+struct iss_reg {
+	enum iss_mem_resources mmio_range;
+	u32 reg;
+	u32 val;
+};
+
+/*
+ * struct iss_device - ISS device structure.
+ * @crashed: Bitmask of crashed entities (indexed by entity ID)
+ */
+struct iss_device {
+	struct v4l2_device v4l2_dev;
+	struct media_device media_dev;
+	struct device *dev;
+	u32 revision;
+
+	/* platform HW resources */
+	struct iss_platform_data *pdata;
+	unsigned int irq_num;
+
+	struct resource *res[OMAP4_ISS_MEM_LAST];
+	void __iomem *regs[OMAP4_ISS_MEM_LAST];
+
+	u64 raw_dmamask;
+
+	struct mutex iss_mutex;	/* For handling ref_count field */
+	bool crashed;
+	int has_context;
+	int ref_count;
+
+	struct clk *iss_fck;
+	struct clk *iss_ctrlclk;
+
+	/* ISS modules */
+	struct iss_csi2_device csi2a;
+	struct iss_csi2_device csi2b;
+	struct iss_csiphy csiphy1;
+	struct iss_csiphy csiphy2;
+	struct iss_ipipeif_device ipipeif;
+	struct iss_ipipe_device ipipe;
+	struct iss_resizer_device resizer;
+
+	unsigned int subclk_resources;
+	unsigned int isp_subclk_resources;
+};
+
+#define v4l2_dev_to_iss_device(dev) \
+	container_of(dev, struct iss_device, v4l2_dev)
+
+int omap4iss_get_external_info(struct iss_pipeline *pipe,
+			       struct media_link *link);
+
+int omap4iss_module_sync_idle(struct media_entity *me, wait_queue_head_t *wait,
+			      atomic_t *stopping);
+
+int omap4iss_module_sync_is_stopping(wait_queue_head_t *wait,
+				     atomic_t *stopping);
+
+int omap4iss_pipeline_set_stream(struct iss_pipeline *pipe,
+				 enum iss_pipeline_stream_state state);
+void omap4iss_pipeline_cancel_stream(struct iss_pipeline *pipe);
+
+void omap4iss_configure_bridge(struct iss_device *iss,
+			       enum ipipeif_input_entity input);
+
+struct iss_device *omap4iss_get(struct iss_device *iss);
+void omap4iss_put(struct iss_device *iss);
+int omap4iss_subclk_enable(struct iss_device *iss,
+			   enum iss_subclk_resource res);
+int omap4iss_subclk_disable(struct iss_device *iss,
+			    enum iss_subclk_resource res);
+void omap4iss_isp_subclk_enable(struct iss_device *iss,
+				enum iss_isp_subclk_resource res);
+void omap4iss_isp_subclk_disable(struct iss_device *iss,
+				 enum iss_isp_subclk_resource res);
+
+int omap4iss_pipeline_pm_use(struct media_entity *entity, int use);
+
+int omap4iss_register_entities(struct platform_device *pdev,
+			       struct v4l2_device *v4l2_dev);
+void omap4iss_unregister_entities(struct platform_device *pdev);
+
+/*
+ * iss_reg_read - Read the value of an OMAP4 ISS register
+ * @iss: the ISS device
+ * @res: memory resource in which the register is located
+ * @offset: register offset in the memory resource
+ *
+ * Return the register value.
+ */
+static inline
+u32 iss_reg_read(struct iss_device *iss, enum iss_mem_resources res,
+		 u32 offset)
+{
+	return readl(iss->regs[res] + offset);
+}
+
+/*
+ * iss_reg_write - Write a value to an OMAP4 ISS register
+ * @iss: the ISS device
+ * @res: memory resource in which the register is located
+ * @offset: register offset in the memory resource
+ * @value: value to be written
+ */
+static inline
+void iss_reg_write(struct iss_device *iss, enum iss_mem_resources res,
+		   u32 offset, u32 value)
+{
+	writel(value, iss->regs[res] + offset);
+}
+
+/*
+ * iss_reg_clr - Clear bits in an OMAP4 ISS register
+ * @iss: the ISS device
+ * @res: memory resource in which the register is located
+ * @offset: register offset in the memory resource
+ * @clr: bit mask to be cleared
+ */
+static inline
+void iss_reg_clr(struct iss_device *iss, enum iss_mem_resources res,
+		 u32 offset, u32 clr)
+{
+	u32 v = iss_reg_read(iss, res, offset);
+
+	iss_reg_write(iss, res, offset, v & ~clr);
+}
+
+/*
+ * iss_reg_set - Set bits in an OMAP4 ISS register
+ * @iss: the ISS device
+ * @res: memory resource in which the register is located
+ * @offset: register offset in the memory resource
+ * @set: bit mask to be set
+ */
+static inline
+void iss_reg_set(struct iss_device *iss, enum iss_mem_resources res,
+		 u32 offset, u32 set)
+{
+	u32 v = iss_reg_read(iss, res, offset);
+
+	iss_reg_write(iss, res, offset, v | set);
+}
+
+/*
+ * iss_reg_update - Clear and set bits in an OMAP4 ISS register
+ * @iss: the ISS device
+ * @res: memory resource in which the register is located
+ * @offset: register offset in the memory resource
+ * @clr: bit mask to be cleared
+ * @set: bit mask to be set
+ *
+ * Clear the clr mask first and then set the set mask.
+ */
+static inline
+void iss_reg_update(struct iss_device *iss, enum iss_mem_resources res,
+		    u32 offset, u32 clr, u32 set)
+{
+	u32 v = iss_reg_read(iss, res, offset);
+
+	iss_reg_write(iss, res, offset, (v & ~clr) | set);
+}
+
+#endif /* _OMAP4_ISS_H_ */
diff --git a/drivers/staging/media/omap4iss/iss_csi2.c b/drivers/staging/media/omap4iss/iss_csi2.c
new file mode 100644
index 000000000000..61fc350eb251
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss_csi2.c
@@ -0,0 +1,1343 @@
+/*
+ * TI OMAP4 ISS V4L2 Driver - CSI PHY module
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/delay.h>
+#include <media/v4l2-common.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/mm.h>
+
+#include "iss.h"
+#include "iss_regs.h"
+#include "iss_csi2.h"
+
+/*
+ * csi2_if_enable - Enable CSI2 Receiver interface.
+ * @enable: enable flag
+ *
+ */
+static void csi2_if_enable(struct iss_csi2_device *csi2, u8 enable)
+{
+	struct iss_csi2_ctrl_cfg *currctrl = &csi2->ctrl;
+
+	iss_reg_update(csi2->iss, csi2->regs1, CSI2_CTRL, CSI2_CTRL_IF_EN,
+		       enable ? CSI2_CTRL_IF_EN : 0);
+
+	currctrl->if_enable = enable;
+}
+
+/*
+ * csi2_recv_config - CSI2 receiver module configuration.
+ * @currctrl: iss_csi2_ctrl_cfg structure
+ *
+ */
+static void csi2_recv_config(struct iss_csi2_device *csi2,
+			     struct iss_csi2_ctrl_cfg *currctrl)
+{
+	u32 reg = 0;
+
+	if (currctrl->frame_mode)
+		reg |= CSI2_CTRL_FRAME;
+	else
+		reg &= ~CSI2_CTRL_FRAME;
+
+	if (currctrl->vp_clk_enable)
+		reg |= CSI2_CTRL_VP_CLK_EN;
+	else
+		reg &= ~CSI2_CTRL_VP_CLK_EN;
+
+	if (currctrl->vp_only_enable)
+		reg |= CSI2_CTRL_VP_ONLY_EN;
+	else
+		reg &= ~CSI2_CTRL_VP_ONLY_EN;
+
+	reg &= ~CSI2_CTRL_VP_OUT_CTRL_MASK;
+	reg |= currctrl->vp_out_ctrl << CSI2_CTRL_VP_OUT_CTRL_SHIFT;
+
+	if (currctrl->ecc_enable)
+		reg |= CSI2_CTRL_ECC_EN;
+	else
+		reg &= ~CSI2_CTRL_ECC_EN;
+
+	/*
+	 * Set MFlag assertion boundaries to:
+	 * Low: 4/8 of FIFO size
+	 * High: 6/8 of FIFO size
+	 */
+	reg &= ~(CSI2_CTRL_MFLAG_LEVH_MASK | CSI2_CTRL_MFLAG_LEVL_MASK);
+	reg |= (2 << CSI2_CTRL_MFLAG_LEVH_SHIFT) |
+	       (4 << CSI2_CTRL_MFLAG_LEVL_SHIFT);
+
+	/* Generation of 16x64-bit bursts (Recommended) */
+	reg |= CSI2_CTRL_BURST_SIZE_EXPAND;
+
+	/* Do Non-Posted writes (Recommended) */
+	reg |= CSI2_CTRL_NON_POSTED_WRITE;
+
+	/*
+	 * Enforce Little endian for all formats, including:
+	 * YUV4:2:2 8-bit and YUV4:2:0 Legacy
+	 */
+	reg |= CSI2_CTRL_ENDIANNESS;
+
+	iss_reg_write(csi2->iss, csi2->regs1, CSI2_CTRL, reg);
+}
+
+static const unsigned int csi2_input_fmts[] = {
+	V4L2_MBUS_FMT_SGRBG10_1X10,
+	V4L2_MBUS_FMT_SGRBG10_DPCM8_1X8,
+	V4L2_MBUS_FMT_SRGGB10_1X10,
+	V4L2_MBUS_FMT_SRGGB10_DPCM8_1X8,
+	V4L2_MBUS_FMT_SBGGR10_1X10,
+	V4L2_MBUS_FMT_SBGGR10_DPCM8_1X8,
+	V4L2_MBUS_FMT_SGBRG10_1X10,
+	V4L2_MBUS_FMT_SGBRG10_DPCM8_1X8,
+	V4L2_MBUS_FMT_SBGGR8_1X8,
+	V4L2_MBUS_FMT_SGBRG8_1X8,
+	V4L2_MBUS_FMT_SGRBG8_1X8,
+	V4L2_MBUS_FMT_SRGGB8_1X8,
+	V4L2_MBUS_FMT_UYVY8_1X16,
+	V4L2_MBUS_FMT_YUYV8_1X16,
+};
+
+/* To set the format on the CSI2 requires a mapping function that takes
+ * the following inputs:
+ * - 3 different formats (at this time)
+ * - 2 destinations (mem, vp+mem) (vp only handled separately)
+ * - 2 decompression options (on, off)
+ * Output should be CSI2 frame format code
+ * Array indices as follows: [format][dest][decompr]
+ * Not all combinations are valid. 0 means invalid.
+ */
+static const u16 __csi2_fmt_map[][2][2] = {
+	/* RAW10 formats */
+	{
+		/* Output to memory */
+		{
+			/* No DPCM decompression */
+			CSI2_PIX_FMT_RAW10_EXP16,
+			/* DPCM decompression */
+			0,
+		},
+		/* Output to both */
+		{
+			/* No DPCM decompression */
+			CSI2_PIX_FMT_RAW10_EXP16_VP,
+			/* DPCM decompression */
+			0,
+		},
+	},
+	/* RAW10 DPCM8 formats */
+	{
+		/* Output to memory */
+		{
+			/* No DPCM decompression */
+			CSI2_USERDEF_8BIT_DATA1,
+			/* DPCM decompression */
+			CSI2_USERDEF_8BIT_DATA1_DPCM10,
+		},
+		/* Output to both */
+		{
+			/* No DPCM decompression */
+			CSI2_PIX_FMT_RAW8_VP,
+			/* DPCM decompression */
+			CSI2_USERDEF_8BIT_DATA1_DPCM10_VP,
+		},
+	},
+	/* RAW8 formats */
+	{
+		/* Output to memory */
+		{
+			/* No DPCM decompression */
+			CSI2_PIX_FMT_RAW8,
+			/* DPCM decompression */
+			0,
+		},
+		/* Output to both */
+		{
+			/* No DPCM decompression */
+			CSI2_PIX_FMT_RAW8_VP,
+			/* DPCM decompression */
+			0,
+		},
+	},
+	/* YUV422 formats */
+	{
+		/* Output to memory */
+		{
+			/* No DPCM decompression */
+			CSI2_PIX_FMT_YUV422_8BIT,
+			/* DPCM decompression */
+			0,
+		},
+		/* Output to both */
+		{
+			/* No DPCM decompression */
+			CSI2_PIX_FMT_YUV422_8BIT_VP16,
+			/* DPCM decompression */
+			0,
+		},
+	},
+};
+
+/*
+ * csi2_ctx_map_format - Map CSI2 sink media bus format to CSI2 format ID
+ * @csi2: ISS CSI2 device
+ *
+ * Returns CSI2 physical format id
+ */
+static u16 csi2_ctx_map_format(struct iss_csi2_device *csi2)
+{
+	const struct v4l2_mbus_framefmt *fmt = &csi2->formats[CSI2_PAD_SINK];
+	int fmtidx, destidx;
+
+	switch (fmt->code) {
+	case V4L2_MBUS_FMT_SGRBG10_1X10:
+	case V4L2_MBUS_FMT_SRGGB10_1X10:
+	case V4L2_MBUS_FMT_SBGGR10_1X10:
+	case V4L2_MBUS_FMT_SGBRG10_1X10:
+		fmtidx = 0;
+		break;
+	case V4L2_MBUS_FMT_SGRBG10_DPCM8_1X8:
+	case V4L2_MBUS_FMT_SRGGB10_DPCM8_1X8:
+	case V4L2_MBUS_FMT_SBGGR10_DPCM8_1X8:
+	case V4L2_MBUS_FMT_SGBRG10_DPCM8_1X8:
+		fmtidx = 1;
+		break;
+	case V4L2_MBUS_FMT_SBGGR8_1X8:
+	case V4L2_MBUS_FMT_SGBRG8_1X8:
+	case V4L2_MBUS_FMT_SGRBG8_1X8:
+	case V4L2_MBUS_FMT_SRGGB8_1X8:
+		fmtidx = 2;
+		break;
+	case V4L2_MBUS_FMT_UYVY8_1X16:
+	case V4L2_MBUS_FMT_YUYV8_1X16:
+		fmtidx = 3;
+		break;
+	default:
+		WARN(1, KERN_ERR "CSI2: pixel format %08x unsupported!\n",
+		     fmt->code);
+		return 0;
+	}
+
+	if (!(csi2->output & CSI2_OUTPUT_IPIPEIF) &&
+	    !(csi2->output & CSI2_OUTPUT_MEMORY)) {
+		/* Neither output enabled is a valid combination */
+		return CSI2_PIX_FMT_OTHERS;
+	}
+
+	/* If we need to skip frames at the beginning of the stream disable the
+	 * video port to avoid sending the skipped frames to the IPIPEIF.
+	 */
+	destidx = csi2->frame_skip ? 0 : !!(csi2->output & CSI2_OUTPUT_IPIPEIF);
+
+	return __csi2_fmt_map[fmtidx][destidx][csi2->dpcm_decompress];
+}
+
+/*
+ * csi2_set_outaddr - Set memory address to save output image
+ * @csi2: Pointer to ISS CSI2a device.
+ * @addr: 32-bit memory address aligned on 32 byte boundary.
+ *
+ * Sets the memory address where the output will be saved.
+ *
+ * Returns 0 if successful, or -EINVAL if the address is not in the 32 byte
+ * boundary.
+ */
+static void csi2_set_outaddr(struct iss_csi2_device *csi2, u32 addr)
+{
+	struct iss_csi2_ctx_cfg *ctx = &csi2->contexts[0];
+
+	ctx->ping_addr = addr;
+	ctx->pong_addr = addr;
+	iss_reg_write(csi2->iss, csi2->regs1, CSI2_CTX_PING_ADDR(ctx->ctxnum),
+		      ctx->ping_addr);
+	iss_reg_write(csi2->iss, csi2->regs1, CSI2_CTX_PONG_ADDR(ctx->ctxnum),
+		      ctx->pong_addr);
+}
+
+/*
+ * is_usr_def_mapping - Checks whether USER_DEF_MAPPING should
+ *			be enabled by CSI2.
+ * @format_id: mapped format id
+ *
+ */
+static inline int is_usr_def_mapping(u32 format_id)
+{
+	return (format_id & 0xf0) == 0x40 ? 1 : 0;
+}
+
+/*
+ * csi2_ctx_enable - Enable specified CSI2 context
+ * @ctxnum: Context number, valid between 0 and 7 values.
+ * @enable: enable
+ *
+ */
+static void csi2_ctx_enable(struct iss_csi2_device *csi2, u8 ctxnum, u8 enable)
+{
+	struct iss_csi2_ctx_cfg *ctx = &csi2->contexts[ctxnum];
+	u32 reg;
+
+	reg = iss_reg_read(csi2->iss, csi2->regs1, CSI2_CTX_CTRL1(ctxnum));
+
+	if (enable) {
+		unsigned int skip = 0;
+
+		if (csi2->frame_skip)
+			skip = csi2->frame_skip;
+		else if (csi2->output & CSI2_OUTPUT_MEMORY)
+			skip = 1;
+
+		reg &= ~CSI2_CTX_CTRL1_COUNT_MASK;
+		reg |= CSI2_CTX_CTRL1_COUNT_UNLOCK
+		    |  (skip << CSI2_CTX_CTRL1_COUNT_SHIFT)
+		    |  CSI2_CTX_CTRL1_CTX_EN;
+	} else {
+		reg &= ~CSI2_CTX_CTRL1_CTX_EN;
+	}
+
+	iss_reg_write(csi2->iss, csi2->regs1, CSI2_CTX_CTRL1(ctxnum), reg);
+	ctx->enabled = enable;
+}
+
+/*
+ * csi2_ctx_config - CSI2 context configuration.
+ * @ctx: context configuration
+ *
+ */
+static void csi2_ctx_config(struct iss_csi2_device *csi2,
+			    struct iss_csi2_ctx_cfg *ctx)
+{
+	u32 reg;
+
+	/* Set up CSI2_CTx_CTRL1 */
+	if (ctx->eof_enabled)
+		reg = CSI2_CTX_CTRL1_EOF_EN;
+
+	if (ctx->eol_enabled)
+		reg |= CSI2_CTX_CTRL1_EOL_EN;
+
+	if (ctx->checksum_enabled)
+		reg |= CSI2_CTX_CTRL1_CS_EN;
+
+	iss_reg_write(csi2->iss, csi2->regs1, CSI2_CTX_CTRL1(ctx->ctxnum), reg);
+
+	/* Set up CSI2_CTx_CTRL2 */
+	reg = ctx->virtual_id << CSI2_CTX_CTRL2_VIRTUAL_ID_SHIFT;
+	reg |= ctx->format_id << CSI2_CTX_CTRL2_FORMAT_SHIFT;
+
+	if (ctx->dpcm_decompress && ctx->dpcm_predictor)
+		reg |= CSI2_CTX_CTRL2_DPCM_PRED;
+
+	if (is_usr_def_mapping(ctx->format_id))
+		reg |= 2 << CSI2_CTX_CTRL2_USER_DEF_MAP_SHIFT;
+
+	iss_reg_write(csi2->iss, csi2->regs1, CSI2_CTX_CTRL2(ctx->ctxnum), reg);
+
+	/* Set up CSI2_CTx_CTRL3 */
+	iss_reg_write(csi2->iss, csi2->regs1, CSI2_CTX_CTRL3(ctx->ctxnum),
+		      ctx->alpha << CSI2_CTX_CTRL3_ALPHA_SHIFT);
+
+	/* Set up CSI2_CTx_DAT_OFST */
+	iss_reg_update(csi2->iss, csi2->regs1, CSI2_CTX_DAT_OFST(ctx->ctxnum),
+		       CSI2_CTX_DAT_OFST_MASK, ctx->data_offset);
+
+	iss_reg_write(csi2->iss, csi2->regs1, CSI2_CTX_PING_ADDR(ctx->ctxnum),
+		      ctx->ping_addr);
+	iss_reg_write(csi2->iss, csi2->regs1, CSI2_CTX_PONG_ADDR(ctx->ctxnum),
+		      ctx->pong_addr);
+}
+
+/*
+ * csi2_timing_config - CSI2 timing configuration.
+ * @timing: csi2_timing_cfg structure
+ */
+static void csi2_timing_config(struct iss_csi2_device *csi2,
+			       struct iss_csi2_timing_cfg *timing)
+{
+	u32 reg;
+
+	reg = iss_reg_read(csi2->iss, csi2->regs1, CSI2_TIMING);
+
+	if (timing->force_rx_mode)
+		reg |= CSI2_TIMING_FORCE_RX_MODE_IO1;
+	else
+		reg &= ~CSI2_TIMING_FORCE_RX_MODE_IO1;
+
+	if (timing->stop_state_16x)
+		reg |= CSI2_TIMING_STOP_STATE_X16_IO1;
+	else
+		reg &= ~CSI2_TIMING_STOP_STATE_X16_IO1;
+
+	if (timing->stop_state_4x)
+		reg |= CSI2_TIMING_STOP_STATE_X4_IO1;
+	else
+		reg &= ~CSI2_TIMING_STOP_STATE_X4_IO1;
+
+	reg &= ~CSI2_TIMING_STOP_STATE_COUNTER_IO1_MASK;
+	reg |= timing->stop_state_counter <<
+	       CSI2_TIMING_STOP_STATE_COUNTER_IO1_SHIFT;
+
+	iss_reg_write(csi2->iss, csi2->regs1, CSI2_TIMING, reg);
+}
+
+/*
+ * csi2_irq_ctx_set - Enables CSI2 Context IRQs.
+ * @enable: Enable/disable CSI2 Context interrupts
+ */
+static void csi2_irq_ctx_set(struct iss_csi2_device *csi2, int enable)
+{
+	u32 reg = CSI2_CTX_IRQ_FE;
+	int i;
+
+	if (csi2->use_fs_irq)
+		reg |= CSI2_CTX_IRQ_FS;
+
+	for (i = 0; i < 8; i++) {
+		iss_reg_write(csi2->iss, csi2->regs1, CSI2_CTX_IRQSTATUS(i),
+			      reg);
+		if (enable)
+			iss_reg_set(csi2->iss, csi2->regs1,
+				    CSI2_CTX_IRQENABLE(i), reg);
+		else
+			iss_reg_clr(csi2->iss, csi2->regs1,
+				    CSI2_CTX_IRQENABLE(i), reg);
+	}
+}
+
+/*
+ * csi2_irq_complexio1_set - Enables CSI2 ComplexIO IRQs.
+ * @enable: Enable/disable CSI2 ComplexIO #1 interrupts
+ */
+static void csi2_irq_complexio1_set(struct iss_csi2_device *csi2, int enable)
+{
+	u32 reg;
+	reg = CSI2_COMPLEXIO_IRQ_STATEALLULPMEXIT |
+		CSI2_COMPLEXIO_IRQ_STATEALLULPMENTER |
+		CSI2_COMPLEXIO_IRQ_STATEULPM5 |
+		CSI2_COMPLEXIO_IRQ_ERRCONTROL5 |
+		CSI2_COMPLEXIO_IRQ_ERRESC5 |
+		CSI2_COMPLEXIO_IRQ_ERRSOTSYNCHS5 |
+		CSI2_COMPLEXIO_IRQ_ERRSOTHS5 |
+		CSI2_COMPLEXIO_IRQ_STATEULPM4 |
+		CSI2_COMPLEXIO_IRQ_ERRCONTROL4 |
+		CSI2_COMPLEXIO_IRQ_ERRESC4 |
+		CSI2_COMPLEXIO_IRQ_ERRSOTSYNCHS4 |
+		CSI2_COMPLEXIO_IRQ_ERRSOTHS4 |
+		CSI2_COMPLEXIO_IRQ_STATEULPM3 |
+		CSI2_COMPLEXIO_IRQ_ERRCONTROL3 |
+		CSI2_COMPLEXIO_IRQ_ERRESC3 |
+		CSI2_COMPLEXIO_IRQ_ERRSOTSYNCHS3 |
+		CSI2_COMPLEXIO_IRQ_ERRSOTHS3 |
+		CSI2_COMPLEXIO_IRQ_STATEULPM2 |
+		CSI2_COMPLEXIO_IRQ_ERRCONTROL2 |
+		CSI2_COMPLEXIO_IRQ_ERRESC2 |
+		CSI2_COMPLEXIO_IRQ_ERRSOTSYNCHS2 |
+		CSI2_COMPLEXIO_IRQ_ERRSOTHS2 |
+		CSI2_COMPLEXIO_IRQ_STATEULPM1 |
+		CSI2_COMPLEXIO_IRQ_ERRCONTROL1 |
+		CSI2_COMPLEXIO_IRQ_ERRESC1 |
+		CSI2_COMPLEXIO_IRQ_ERRSOTSYNCHS1 |
+		CSI2_COMPLEXIO_IRQ_ERRSOTHS1;
+	iss_reg_write(csi2->iss, csi2->regs1, CSI2_COMPLEXIO_IRQSTATUS, reg);
+	if (enable)
+		iss_reg_set(csi2->iss, csi2->regs1, CSI2_COMPLEXIO_IRQENABLE,
+			    reg);
+	else
+		iss_reg_write(csi2->iss, csi2->regs1, CSI2_COMPLEXIO_IRQENABLE,
+			      0);
+}
+
+/*
+ * csi2_irq_status_set - Enables CSI2 Status IRQs.
+ * @enable: Enable/disable CSI2 Status interrupts
+ */
+static void csi2_irq_status_set(struct iss_csi2_device *csi2, int enable)
+{
+	u32 reg;
+	reg = CSI2_IRQ_OCP_ERR |
+		CSI2_IRQ_SHORT_PACKET |
+		CSI2_IRQ_ECC_CORRECTION |
+		CSI2_IRQ_ECC_NO_CORRECTION |
+		CSI2_IRQ_COMPLEXIO_ERR |
+		CSI2_IRQ_FIFO_OVF |
+		CSI2_IRQ_CONTEXT0;
+	iss_reg_write(csi2->iss, csi2->regs1, CSI2_IRQSTATUS, reg);
+	if (enable)
+		iss_reg_set(csi2->iss, csi2->regs1, CSI2_IRQENABLE, reg);
+	else
+		iss_reg_write(csi2->iss, csi2->regs1, CSI2_IRQENABLE, 0);
+}
+
+/*
+ * omap4iss_csi2_reset - Resets the CSI2 module.
+ *
+ * Must be called with the phy lock held.
+ *
+ * Returns 0 if successful, or -EBUSY if power command didn't respond.
+ */
+int omap4iss_csi2_reset(struct iss_csi2_device *csi2)
+{
+	u8 soft_reset_retries = 0;
+	u32 reg;
+	int i;
+
+	if (!csi2->available)
+		return -ENODEV;
+
+	if (csi2->phy->phy_in_use)
+		return -EBUSY;
+
+	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");
+		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");
+		return -EBUSY;
+	}
+
+	iss_reg_update(csi2->iss, csi2->regs1, CSI2_SYSCONFIG,
+		       CSI2_SYSCONFIG_MSTANDBY_MODE_MASK |
+		       CSI2_SYSCONFIG_AUTO_IDLE,
+		       CSI2_SYSCONFIG_MSTANDBY_MODE_NO);
+
+	return 0;
+}
+
+static int csi2_configure(struct iss_csi2_device *csi2)
+{
+	const struct iss_v4l2_subdevs_group *pdata;
+	struct iss_csi2_timing_cfg *timing = &csi2->timing[0];
+	struct v4l2_subdev *sensor;
+	struct media_pad *pad;
+
+	/*
+	 * CSI2 fields that can be updated while the context has
+	 * been enabled or the interface has been enabled are not
+	 * updated dynamically currently. So we do not allow to
+	 * reconfigure if either has been enabled
+	 */
+	if (csi2->contexts[0].enabled || csi2->ctrl.if_enable)
+		return -EBUSY;
+
+	pad = media_entity_remote_pad(&csi2->pads[CSI2_PAD_SINK]);
+	sensor = media_entity_to_v4l2_subdev(pad->entity);
+	pdata = sensor->host_priv;
+
+	csi2->frame_skip = 0;
+	v4l2_subdev_call(sensor, sensor, g_skip_frames, &csi2->frame_skip);
+
+	csi2->ctrl.vp_out_ctrl = pdata->bus.csi2.vpclk_div;
+	csi2->ctrl.frame_mode = ISS_CSI2_FRAME_IMMEDIATE;
+	csi2->ctrl.ecc_enable = pdata->bus.csi2.crc;
+
+	timing->force_rx_mode = 1;
+	timing->stop_state_16x = 1;
+	timing->stop_state_4x = 1;
+	timing->stop_state_counter = 0x1ff;
+
+	/*
+	 * The CSI2 receiver can't do any format conversion except DPCM
+	 * decompression, so every set_format call configures both pads
+	 * and enables DPCM decompression as a special case:
+	 */
+	if (csi2->formats[CSI2_PAD_SINK].code !=
+	    csi2->formats[CSI2_PAD_SOURCE].code)
+		csi2->dpcm_decompress = true;
+	else
+		csi2->dpcm_decompress = false;
+
+	csi2->contexts[0].format_id = csi2_ctx_map_format(csi2);
+
+	if (csi2->video_out.bpl_padding == 0)
+		csi2->contexts[0].data_offset = 0;
+	else
+		csi2->contexts[0].data_offset = csi2->video_out.bpl_value;
+
+	/*
+	 * Enable end of frame and end of line signals generation for
+	 * context 0. These signals are generated from CSI2 receiver to
+	 * qualify the last pixel of a frame and the last pixel of a line.
+	 * Without enabling the signals CSI2 receiver writes data to memory
+	 * beyond buffer size and/or data line offset is not handled correctly.
+	 */
+	csi2->contexts[0].eof_enabled = 1;
+	csi2->contexts[0].eol_enabled = 1;
+
+	csi2_irq_complexio1_set(csi2, 1);
+	csi2_irq_ctx_set(csi2, 1);
+	csi2_irq_status_set(csi2, 1);
+
+	/* Set configuration (timings, format and links) */
+	csi2_timing_config(csi2, timing);
+	csi2_recv_config(csi2, &csi2->ctrl);
+	csi2_ctx_config(csi2, &csi2->contexts[0]);
+
+	return 0;
+}
+
+/*
+ * csi2_print_status - Prints CSI2 debug information.
+ */
+#define CSI2_PRINT_REGISTER(iss, regs, name)\
+	dev_dbg(iss->dev, "###CSI2 " #name "=0x%08x\n", \
+		iss_reg_read(iss, regs, CSI2_##name))
+
+static void csi2_print_status(struct iss_csi2_device *csi2)
+{
+	struct iss_device *iss = csi2->iss;
+
+	if (!csi2->available)
+		return;
+
+	dev_dbg(iss->dev, "-------------CSI2 Register dump-------------\n");
+
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, SYSCONFIG);
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, SYSSTATUS);
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, IRQENABLE);
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, IRQSTATUS);
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, CTRL);
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, DBG_H);
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, COMPLEXIO_CFG);
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, COMPLEXIO_IRQSTATUS);
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, SHORT_PACKET);
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, COMPLEXIO_IRQENABLE);
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, DBG_P);
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, TIMING);
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, CTX_CTRL1(0));
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, CTX_CTRL2(0));
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, CTX_DAT_OFST(0));
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, CTX_PING_ADDR(0));
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, CTX_PONG_ADDR(0));
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, CTX_IRQENABLE(0));
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, CTX_IRQSTATUS(0));
+	CSI2_PRINT_REGISTER(iss, csi2->regs1, CTX_CTRL3(0));
+
+	dev_dbg(iss->dev, "--------------------------------------------\n");
+}
+
+/* -----------------------------------------------------------------------------
+ * Interrupt handling
+ */
+
+/*
+ * csi2_isr_buffer - Does buffer handling at end-of-frame
+ * when writing to memory.
+ */
+static void csi2_isr_buffer(struct iss_csi2_device *csi2)
+{
+	struct iss_buffer *buffer;
+
+	csi2_ctx_enable(csi2, 0, 0);
+
+	buffer = omap4iss_video_buffer_next(&csi2->video_out);
+
+	/*
+	 * Let video queue operation restart engine if there is an underrun
+	 * condition.
+	 */
+	if (buffer == NULL)
+		return;
+
+	csi2_set_outaddr(csi2, buffer->iss_addr);
+	csi2_ctx_enable(csi2, 0, 1);
+}
+
+static void csi2_isr_ctx(struct iss_csi2_device *csi2,
+			 struct iss_csi2_ctx_cfg *ctx)
+{
+	unsigned int n = ctx->ctxnum;
+	u32 status;
+
+	status = iss_reg_read(csi2->iss, csi2->regs1, CSI2_CTX_IRQSTATUS(n));
+	iss_reg_write(csi2->iss, csi2->regs1, CSI2_CTX_IRQSTATUS(n), status);
+
+	/* Propagate frame number */
+	if (status & CSI2_CTX_IRQ_FS) {
+		struct iss_pipeline *pipe =
+				     to_iss_pipeline(&csi2->subdev.entity);
+		if (pipe->do_propagation)
+			atomic_inc(&pipe->frame_number);
+	}
+
+	if (!(status & CSI2_CTX_IRQ_FE))
+		return;
+
+	/* Skip interrupts until we reach the frame skip count. The CSI2 will be
+	 * automatically disabled, as the frame skip count has been programmed
+	 * in the CSI2_CTx_CTRL1::COUNT field, so reenable it.
+	 *
+	 * It would have been nice to rely on the FRAME_NUMBER interrupt instead
+	 * but it turned out that the interrupt is only generated when the CSI2
+	 * writes to memory (the CSI2_CTx_CTRL1::COUNT field is decreased
+	 * correctly and reaches 0 when data is forwarded to the video port only
+	 * but no interrupt arrives). Maybe a CSI2 hardware bug.
+	 */
+	if (csi2->frame_skip) {
+		csi2->frame_skip--;
+		if (csi2->frame_skip == 0) {
+			ctx->format_id = csi2_ctx_map_format(csi2);
+			csi2_ctx_config(csi2, ctx);
+			csi2_ctx_enable(csi2, n, 1);
+		}
+		return;
+	}
+
+	if (csi2->output & CSI2_OUTPUT_MEMORY)
+		csi2_isr_buffer(csi2);
+}
+
+/*
+ * omap4iss_csi2_isr - CSI2 interrupt handling.
+ */
+void omap4iss_csi2_isr(struct iss_csi2_device *csi2)
+{
+	struct iss_pipeline *pipe = to_iss_pipeline(&csi2->subdev.entity);
+	u32 csi2_irqstatus, cpxio1_irqstatus;
+	struct iss_device *iss = csi2->iss;
+
+	if (!csi2->available)
+		return;
+
+	csi2_irqstatus = iss_reg_read(csi2->iss, csi2->regs1, CSI2_IRQSTATUS);
+	iss_reg_write(csi2->iss, csi2->regs1, CSI2_IRQSTATUS, csi2_irqstatus);
+
+	/* Failure Cases */
+	if (csi2_irqstatus & CSI2_IRQ_COMPLEXIO_ERR) {
+		cpxio1_irqstatus = iss_reg_read(csi2->iss, csi2->regs1,
+						CSI2_COMPLEXIO_IRQSTATUS);
+		iss_reg_write(csi2->iss, csi2->regs1, CSI2_COMPLEXIO_IRQSTATUS,
+			      cpxio1_irqstatus);
+		dev_dbg(iss->dev, "CSI2: ComplexIO Error IRQ %x\n",
+			cpxio1_irqstatus);
+		pipe->error = true;
+	}
+
+	if (csi2_irqstatus & (CSI2_IRQ_OCP_ERR |
+			      CSI2_IRQ_SHORT_PACKET |
+			      CSI2_IRQ_ECC_NO_CORRECTION |
+			      CSI2_IRQ_COMPLEXIO_ERR |
+			      CSI2_IRQ_FIFO_OVF)) {
+		dev_dbg(iss->dev,
+			"CSI2 Err: OCP:%d SHORT:%d ECC:%d CPXIO:%d OVF:%d\n",
+			csi2_irqstatus & CSI2_IRQ_OCP_ERR ? 1 : 0,
+			csi2_irqstatus & CSI2_IRQ_SHORT_PACKET ? 1 : 0,
+			csi2_irqstatus & CSI2_IRQ_ECC_NO_CORRECTION ? 1 : 0,
+			csi2_irqstatus & CSI2_IRQ_COMPLEXIO_ERR ? 1 : 0,
+			csi2_irqstatus & CSI2_IRQ_FIFO_OVF ? 1 : 0);
+		pipe->error = true;
+	}
+
+	if (omap4iss_module_sync_is_stopping(&csi2->wait, &csi2->stopping))
+		return;
+
+	/* Successful cases */
+	if (csi2_irqstatus & CSI2_IRQ_CONTEXT0)
+		csi2_isr_ctx(csi2, &csi2->contexts[0]);
+
+	if (csi2_irqstatus & CSI2_IRQ_ECC_CORRECTION)
+		dev_dbg(iss->dev, "CSI2: ECC correction done\n");
+}
+
+/* -----------------------------------------------------------------------------
+ * ISS video operations
+ */
+
+/*
+ * csi2_queue - Queues the first buffer when using memory output
+ * @video: The video node
+ * @buffer: buffer to queue
+ */
+static int csi2_queue(struct iss_video *video, struct iss_buffer *buffer)
+{
+	struct iss_csi2_device *csi2 = container_of(video,
+				struct iss_csi2_device, video_out);
+
+	csi2_set_outaddr(csi2, buffer->iss_addr);
+
+	/*
+	 * If streaming was enabled before there was a buffer queued
+	 * or underrun happened in the ISR, the hardware was not enabled
+	 * and DMA queue flag ISS_VIDEO_DMAQUEUE_UNDERRUN is still set.
+	 * Enable it now.
+	 */
+	if (csi2->video_out.dmaqueue_flags & ISS_VIDEO_DMAQUEUE_UNDERRUN) {
+		/* Enable / disable context 0 and IRQs */
+		csi2_if_enable(csi2, 1);
+		csi2_ctx_enable(csi2, 0, 1);
+		iss_video_dmaqueue_flags_clr(&csi2->video_out);
+	}
+
+	return 0;
+}
+
+static const struct iss_video_operations csi2_issvideo_ops = {
+	.queue = csi2_queue,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev operations
+ */
+
+static struct v4l2_mbus_framefmt *
+__csi2_get_format(struct iss_csi2_device *csi2, struct v4l2_subdev_fh *fh,
+		  unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(fh, pad);
+	else
+		return &csi2->formats[pad];
+}
+
+static void
+csi2_try_format(struct iss_csi2_device *csi2, struct v4l2_subdev_fh *fh,
+		unsigned int pad, struct v4l2_mbus_framefmt *fmt,
+		enum v4l2_subdev_format_whence which)
+{
+	enum v4l2_mbus_pixelcode pixelcode;
+	struct v4l2_mbus_framefmt *format;
+	const struct iss_format_info *info;
+	unsigned int i;
+
+	switch (pad) {
+	case CSI2_PAD_SINK:
+		/* Clamp the width and height to valid range (1-8191). */
+		for (i = 0; i < ARRAY_SIZE(csi2_input_fmts); i++) {
+			if (fmt->code == csi2_input_fmts[i])
+				break;
+		}
+
+		/* If not found, use SGRBG10 as default */
+		if (i >= ARRAY_SIZE(csi2_input_fmts))
+			fmt->code = V4L2_MBUS_FMT_SGRBG10_1X10;
+
+		fmt->width = clamp_t(u32, fmt->width, 1, 8191);
+		fmt->height = clamp_t(u32, fmt->height, 1, 8191);
+		break;
+
+	case CSI2_PAD_SOURCE:
+		/* Source format same as sink format, except for DPCM
+		 * compression.
+		 */
+		pixelcode = fmt->code;
+		format = __csi2_get_format(csi2, fh, CSI2_PAD_SINK, which);
+		memcpy(fmt, format, sizeof(*fmt));
+
+		/*
+		 * Only Allow DPCM decompression, and check that the
+		 * pattern is preserved
+		 */
+		info = omap4iss_video_format_info(fmt->code);
+		if (info->uncompressed == pixelcode)
+			fmt->code = pixelcode;
+		break;
+	}
+
+	/* RGB, non-interlaced */
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+	fmt->field = V4L2_FIELD_NONE;
+}
+
+/*
+ * csi2_enum_mbus_code - Handle pixel format enumeration
+ * @sd     : pointer to v4l2 subdev structure
+ * @fh     : V4L2 subdev file handle
+ * @code   : pointer to v4l2_subdev_mbus_code_enum structure
+ * return -EINVAL or zero on success
+ */
+static int csi2_enum_mbus_code(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_fh *fh,
+			       struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct iss_csi2_device *csi2 = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+	const struct iss_format_info *info;
+
+	if (code->pad == CSI2_PAD_SINK) {
+		if (code->index >= ARRAY_SIZE(csi2_input_fmts))
+			return -EINVAL;
+
+		code->code = csi2_input_fmts[code->index];
+	} else {
+		format = __csi2_get_format(csi2, fh, CSI2_PAD_SINK,
+					   V4L2_SUBDEV_FORMAT_TRY);
+		switch (code->index) {
+		case 0:
+			/* Passthrough sink pad code */
+			code->code = format->code;
+			break;
+		case 1:
+			/* Uncompressed code */
+			info = omap4iss_video_format_info(format->code);
+			if (info->uncompressed == format->code)
+				return -EINVAL;
+
+			code->code = info->uncompressed;
+			break;
+		default:
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static int csi2_enum_frame_size(struct v4l2_subdev *sd,
+				struct v4l2_subdev_fh *fh,
+				struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct iss_csi2_device *csi2 = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt format;
+
+	if (fse->index != 0)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = 1;
+	format.height = 1;
+	csi2_try_format(csi2, fh, fse->pad, &format, V4L2_SUBDEV_FORMAT_TRY);
+	fse->min_width = format.width;
+	fse->min_height = format.height;
+
+	if (format.code != fse->code)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = -1;
+	format.height = -1;
+	csi2_try_format(csi2, fh, fse->pad, &format, V4L2_SUBDEV_FORMAT_TRY);
+	fse->max_width = format.width;
+	fse->max_height = format.height;
+
+	return 0;
+}
+
+/*
+ * csi2_get_format - Handle get format by pads subdev method
+ * @sd : pointer to v4l2 subdev structure
+ * @fh : V4L2 subdev file handle
+ * @fmt: pointer to v4l2 subdev format structure
+ * return -EINVAL or zero on success
+ */
+static int csi2_get_format(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct iss_csi2_device *csi2 = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __csi2_get_format(csi2, fh, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	fmt->format = *format;
+	return 0;
+}
+
+/*
+ * csi2_set_format - Handle set format by pads subdev method
+ * @sd : pointer to v4l2 subdev structure
+ * @fh : V4L2 subdev file handle
+ * @fmt: pointer to v4l2 subdev format structure
+ * return -EINVAL or zero on success
+ */
+static int csi2_set_format(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct iss_csi2_device *csi2 = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __csi2_get_format(csi2, fh, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	csi2_try_format(csi2, fh, fmt->pad, &fmt->format, fmt->which);
+	*format = fmt->format;
+
+	/* Propagate the format from sink to source */
+	if (fmt->pad == CSI2_PAD_SINK) {
+		format = __csi2_get_format(csi2, fh, CSI2_PAD_SOURCE,
+					   fmt->which);
+		*format = fmt->format;
+		csi2_try_format(csi2, fh, CSI2_PAD_SOURCE, format, fmt->which);
+	}
+
+	return 0;
+}
+
+static int csi2_link_validate(struct v4l2_subdev *sd, struct media_link *link,
+			      struct v4l2_subdev_format *source_fmt,
+			      struct v4l2_subdev_format *sink_fmt)
+{
+	struct iss_csi2_device *csi2 = v4l2_get_subdevdata(sd);
+	struct iss_pipeline *pipe = to_iss_pipeline(&csi2->subdev.entity);
+	int rval;
+
+	pipe->external = media_entity_to_v4l2_subdev(link->source->entity);
+	rval = omap4iss_get_external_info(pipe, link);
+	if (rval < 0)
+		return rval;
+
+	return v4l2_subdev_link_validate_default(sd, link, source_fmt,
+						 sink_fmt);
+}
+
+/*
+ * csi2_init_formats - Initialize formats on all pads
+ * @sd: ISS CSI2 V4L2 subdevice
+ * @fh: V4L2 subdev file handle
+ *
+ * Initialize all pad formats with default values. If fh is not NULL, try
+ * formats are initialized on the file handle. Otherwise active formats are
+ * initialized on the device.
+ */
+static int csi2_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_subdev_format format;
+
+	memset(&format, 0, sizeof(format));
+	format.pad = CSI2_PAD_SINK;
+	format.which = fh ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+	format.format.code = V4L2_MBUS_FMT_SGRBG10_1X10;
+	format.format.width = 4096;
+	format.format.height = 4096;
+	csi2_set_format(sd, fh, &format);
+
+	return 0;
+}
+
+/*
+ * csi2_set_stream - Enable/Disable streaming on the CSI2 module
+ * @sd: ISS CSI2 V4L2 subdevice
+ * @enable: ISS pipeline stream state
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+static int csi2_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct iss_csi2_device *csi2 = v4l2_get_subdevdata(sd);
+	struct iss_device *iss = csi2->iss;
+	struct iss_pipeline *pipe = to_iss_pipeline(&csi2->subdev.entity);
+	struct iss_video *video_out = &csi2->video_out;
+	int ret = 0;
+
+	if (csi2->state == ISS_PIPELINE_STREAM_STOPPED) {
+		if (enable == ISS_PIPELINE_STREAM_STOPPED)
+			return 0;
+
+		omap4iss_subclk_enable(iss, csi2->subclk);
+	}
+
+	switch (enable) {
+	case ISS_PIPELINE_STREAM_CONTINUOUS: {
+		ret = omap4iss_csiphy_config(iss, sd);
+		if (ret < 0)
+			return ret;
+
+		if (omap4iss_csiphy_acquire(csi2->phy) < 0)
+			return -ENODEV;
+		csi2->use_fs_irq = pipe->do_propagation;
+		csi2_configure(csi2);
+		csi2_print_status(csi2);
+
+		/*
+		 * When outputting to memory with no buffer available, let the
+		 * buffer queue handler start the hardware. A DMA queue flag
+		 * ISS_VIDEO_DMAQUEUE_QUEUED will be set as soon as there is
+		 * a buffer available.
+		 */
+		if (csi2->output & CSI2_OUTPUT_MEMORY &&
+		    !(video_out->dmaqueue_flags & ISS_VIDEO_DMAQUEUE_QUEUED))
+			break;
+		/* Enable context 0 and IRQs */
+		atomic_set(&csi2->stopping, 0);
+		csi2_ctx_enable(csi2, 0, 1);
+		csi2_if_enable(csi2, 1);
+		iss_video_dmaqueue_flags_clr(video_out);
+		break;
+	}
+	case ISS_PIPELINE_STREAM_STOPPED:
+		if (csi2->state == ISS_PIPELINE_STREAM_STOPPED)
+			return 0;
+		if (omap4iss_module_sync_idle(&sd->entity, &csi2->wait,
+					      &csi2->stopping))
+			ret = -ETIMEDOUT;
+		csi2_ctx_enable(csi2, 0, 0);
+		csi2_if_enable(csi2, 0);
+		csi2_irq_ctx_set(csi2, 0);
+		omap4iss_csiphy_release(csi2->phy);
+		omap4iss_subclk_disable(iss, csi2->subclk);
+		iss_video_dmaqueue_flags_clr(video_out);
+		break;
+	}
+
+	csi2->state = enable;
+	return ret;
+}
+
+/* subdev video operations */
+static const struct v4l2_subdev_video_ops csi2_video_ops = {
+	.s_stream = csi2_set_stream,
+};
+
+/* subdev pad operations */
+static const struct v4l2_subdev_pad_ops csi2_pad_ops = {
+	.enum_mbus_code = csi2_enum_mbus_code,
+	.enum_frame_size = csi2_enum_frame_size,
+	.get_fmt = csi2_get_format,
+	.set_fmt = csi2_set_format,
+	.link_validate = csi2_link_validate,
+};
+
+/* subdev operations */
+static const struct v4l2_subdev_ops csi2_ops = {
+	.video = &csi2_video_ops,
+	.pad = &csi2_pad_ops,
+};
+
+/* subdev internal operations */
+static const struct v4l2_subdev_internal_ops csi2_internal_ops = {
+	.open = csi2_init_formats,
+};
+
+/* -----------------------------------------------------------------------------
+ * Media entity operations
+ */
+
+/*
+ * csi2_link_setup - Setup CSI2 connections.
+ * @entity : Pointer to media entity structure
+ * @local  : Pointer to local pad array
+ * @remote : Pointer to remote pad array
+ * @flags  : Link flags
+ * return -EINVAL or zero on success
+ */
+static int csi2_link_setup(struct media_entity *entity,
+			   const struct media_pad *local,
+			   const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct iss_csi2_device *csi2 = v4l2_get_subdevdata(sd);
+	struct iss_csi2_ctrl_cfg *ctrl = &csi2->ctrl;
+
+	/*
+	 * The ISS core doesn't support pipelines with multiple video outputs.
+	 * Revisit this when it will be implemented, and return -EBUSY for now.
+	 */
+
+	switch (local->index | media_entity_type(remote->entity)) {
+	case CSI2_PAD_SOURCE | MEDIA_ENT_T_DEVNODE:
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (csi2->output & ~CSI2_OUTPUT_MEMORY)
+				return -EBUSY;
+			csi2->output |= CSI2_OUTPUT_MEMORY;
+		} else {
+			csi2->output &= ~CSI2_OUTPUT_MEMORY;
+		}
+		break;
+
+	case CSI2_PAD_SOURCE | MEDIA_ENT_T_V4L2_SUBDEV:
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (csi2->output & ~CSI2_OUTPUT_IPIPEIF)
+				return -EBUSY;
+			csi2->output |= CSI2_OUTPUT_IPIPEIF;
+		} else {
+			csi2->output &= ~CSI2_OUTPUT_IPIPEIF;
+		}
+		break;
+
+	default:
+		/* Link from camera to CSI2 is fixed... */
+		return -EINVAL;
+	}
+
+	ctrl->vp_only_enable = csi2->output & CSI2_OUTPUT_MEMORY ? false : true;
+	ctrl->vp_clk_enable = !!(csi2->output & CSI2_OUTPUT_IPIPEIF);
+
+	return 0;
+}
+
+/* media operations */
+static const struct media_entity_operations csi2_media_ops = {
+	.link_setup = csi2_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+void omap4iss_csi2_unregister_entities(struct iss_csi2_device *csi2)
+{
+	v4l2_device_unregister_subdev(&csi2->subdev);
+	omap4iss_video_unregister(&csi2->video_out);
+}
+
+int omap4iss_csi2_register_entities(struct iss_csi2_device *csi2,
+				    struct v4l2_device *vdev)
+{
+	int ret;
+
+	/* Register the subdev and video nodes. */
+	ret = v4l2_device_register_subdev(vdev, &csi2->subdev);
+	if (ret < 0)
+		goto error;
+
+	ret = omap4iss_video_register(&csi2->video_out, vdev);
+	if (ret < 0)
+		goto error;
+
+	return 0;
+
+error:
+	omap4iss_csi2_unregister_entities(csi2);
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * ISS CSI2 initialisation and cleanup
+ */
+
+/*
+ * csi2_init_entities - Initialize subdev and media entity.
+ * @csi2: Pointer to csi2 structure.
+ * return -ENOMEM or zero on success
+ */
+static int csi2_init_entities(struct iss_csi2_device *csi2, const char *subname)
+{
+	struct v4l2_subdev *sd = &csi2->subdev;
+	struct media_pad *pads = csi2->pads;
+	struct media_entity *me = &sd->entity;
+	int ret;
+	char name[V4L2_SUBDEV_NAME_SIZE];
+
+	v4l2_subdev_init(sd, &csi2_ops);
+	sd->internal_ops = &csi2_internal_ops;
+	sprintf(name, "CSI2%s", subname);
+	snprintf(sd->name, sizeof(sd->name), "OMAP4 ISS %s", name);
+
+	sd->grp_id = 1 << 16;	/* group ID for iss subdevs */
+	v4l2_set_subdevdata(sd, csi2);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	pads[CSI2_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
+	pads[CSI2_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+
+	me->ops = &csi2_media_ops;
+	ret = media_entity_init(me, CSI2_PADS_NUM, pads, 0);
+	if (ret < 0)
+		return ret;
+
+	csi2_init_formats(sd, NULL);
+
+	/* Video device node */
+	csi2->video_out.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	csi2->video_out.ops = &csi2_issvideo_ops;
+	csi2->video_out.bpl_alignment = 32;
+	csi2->video_out.bpl_zero_padding = 1;
+	csi2->video_out.bpl_max = 0x1ffe0;
+	csi2->video_out.iss = csi2->iss;
+	csi2->video_out.capture_mem = PAGE_ALIGN(4096 * 4096) * 3;
+
+	ret = omap4iss_video_init(&csi2->video_out, name);
+	if (ret < 0)
+		goto error_video;
+
+	/* Connect the CSI2 subdev to the video node. */
+	ret = media_entity_create_link(&csi2->subdev.entity, CSI2_PAD_SOURCE,
+				       &csi2->video_out.video.entity, 0, 0);
+	if (ret < 0)
+		goto error_link;
+
+	return 0;
+
+error_link:
+	omap4iss_video_cleanup(&csi2->video_out);
+error_video:
+	media_entity_cleanup(&csi2->subdev.entity);
+	return ret;
+}
+
+/*
+ * omap4iss_csi2_init - Routine for module driver init
+ */
+int omap4iss_csi2_init(struct iss_device *iss)
+{
+	struct iss_csi2_device *csi2a = &iss->csi2a;
+	struct iss_csi2_device *csi2b = &iss->csi2b;
+	int ret;
+
+	csi2a->iss = iss;
+	csi2a->available = 1;
+	csi2a->regs1 = OMAP4_ISS_MEM_CSI2_A_REGS1;
+	csi2a->phy = &iss->csiphy1;
+	csi2a->subclk = OMAP4_ISS_SUBCLK_CSI2_A;
+	csi2a->state = ISS_PIPELINE_STREAM_STOPPED;
+	init_waitqueue_head(&csi2a->wait);
+
+	ret = csi2_init_entities(csi2a, "a");
+	if (ret < 0)
+		return ret;
+
+	csi2b->iss = iss;
+	csi2b->available = 1;
+	csi2b->regs1 = OMAP4_ISS_MEM_CSI2_B_REGS1;
+	csi2b->phy = &iss->csiphy2;
+	csi2b->subclk = OMAP4_ISS_SUBCLK_CSI2_B;
+	csi2b->state = ISS_PIPELINE_STREAM_STOPPED;
+	init_waitqueue_head(&csi2b->wait);
+
+	ret = csi2_init_entities(csi2b, "b");
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+/*
+ * omap4iss_csi2_cleanup - Routine for module driver cleanup
+ */
+void omap4iss_csi2_cleanup(struct iss_device *iss)
+{
+	struct iss_csi2_device *csi2a = &iss->csi2a;
+	struct iss_csi2_device *csi2b = &iss->csi2b;
+
+	omap4iss_video_cleanup(&csi2a->video_out);
+	media_entity_cleanup(&csi2a->subdev.entity);
+
+	omap4iss_video_cleanup(&csi2b->video_out);
+	media_entity_cleanup(&csi2b->subdev.entity);
+}
diff --git a/drivers/staging/media/omap4iss/iss_csi2.h b/drivers/staging/media/omap4iss/iss_csi2.h
new file mode 100644
index 000000000000..971aa7b08013
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss_csi2.h
@@ -0,0 +1,158 @@
+/*
+ * TI OMAP4 ISS V4L2 Driver - CSI2 module
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@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.
+ */
+
+#ifndef OMAP4_ISS_CSI2_H
+#define OMAP4_ISS_CSI2_H
+
+#include <linux/types.h>
+#include <linux/videodev2.h>
+
+#include "iss_video.h"
+
+struct iss_csiphy;
+
+/* This is not an exhaustive list */
+enum iss_csi2_pix_formats {
+	CSI2_PIX_FMT_OTHERS = 0,
+	CSI2_PIX_FMT_YUV422_8BIT = 0x1e,
+	CSI2_PIX_FMT_YUV422_8BIT_VP = 0x9e,
+	CSI2_PIX_FMT_YUV422_8BIT_VP16 = 0xde,
+	CSI2_PIX_FMT_RAW10_EXP16 = 0xab,
+	CSI2_PIX_FMT_RAW10_EXP16_VP = 0x12f,
+	CSI2_PIX_FMT_RAW8 = 0x2a,
+	CSI2_PIX_FMT_RAW8_DPCM10_EXP16 = 0x2aa,
+	CSI2_PIX_FMT_RAW8_DPCM10_VP = 0x32a,
+	CSI2_PIX_FMT_RAW8_VP = 0x12a,
+	CSI2_USERDEF_8BIT_DATA1_DPCM10_VP = 0x340,
+	CSI2_USERDEF_8BIT_DATA1_DPCM10 = 0x2c0,
+	CSI2_USERDEF_8BIT_DATA1 = 0x40,
+};
+
+enum iss_csi2_irqevents {
+	OCP_ERR_IRQ = 0x4000,
+	SHORT_PACKET_IRQ = 0x2000,
+	ECC_CORRECTION_IRQ = 0x1000,
+	ECC_NO_CORRECTION_IRQ = 0x800,
+	COMPLEXIO2_ERR_IRQ = 0x400,
+	COMPLEXIO1_ERR_IRQ = 0x200,
+	FIFO_OVF_IRQ = 0x100,
+	CONTEXT7 = 0x80,
+	CONTEXT6 = 0x40,
+	CONTEXT5 = 0x20,
+	CONTEXT4 = 0x10,
+	CONTEXT3 = 0x8,
+	CONTEXT2 = 0x4,
+	CONTEXT1 = 0x2,
+	CONTEXT0 = 0x1,
+};
+
+enum iss_csi2_ctx_irqevents {
+	CTX_ECC_CORRECTION = 0x100,
+	CTX_LINE_NUMBER = 0x80,
+	CTX_FRAME_NUMBER = 0x40,
+	CTX_CS = 0x20,
+	CTX_LE = 0x8,
+	CTX_LS = 0x4,
+	CTX_FE = 0x2,
+	CTX_FS = 0x1,
+};
+
+enum iss_csi2_frame_mode {
+	ISS_CSI2_FRAME_IMMEDIATE,
+	ISS_CSI2_FRAME_AFTERFEC,
+};
+
+#define ISS_CSI2_MAX_CTX_NUM	7
+
+struct iss_csi2_ctx_cfg {
+	u8 ctxnum;		/* context number 0 - 7 */
+	u8 dpcm_decompress;
+
+	/* Fields in CSI2_CTx_CTRL2 - locked by CSI2_CTx_CTRL1.CTX_EN */
+	u8 virtual_id;
+	u16 format_id;		/* as in CSI2_CTx_CTRL2[9:0] */
+	u8 dpcm_predictor;	/* 1: simple, 0: advanced */
+
+	/* Fields in CSI2_CTx_CTRL1/3 - Shadowed */
+	u16 alpha;
+	u16 data_offset;
+	u32 ping_addr;
+	u32 pong_addr;
+	u8 eof_enabled;
+	u8 eol_enabled;
+	u8 checksum_enabled;
+	u8 enabled;
+};
+
+struct iss_csi2_timing_cfg {
+	u8 ionum;			/* IO1 or IO2 as in CSI2_TIMING */
+	unsigned force_rx_mode:1;
+	unsigned stop_state_16x:1;
+	unsigned stop_state_4x:1;
+	u16 stop_state_counter;
+};
+
+struct iss_csi2_ctrl_cfg {
+	bool vp_clk_enable;
+	bool vp_only_enable;
+	u8 vp_out_ctrl;
+	enum iss_csi2_frame_mode frame_mode;
+	bool ecc_enable;
+	bool if_enable;
+};
+
+#define CSI2_PAD_SINK		0
+#define CSI2_PAD_SOURCE		1
+#define CSI2_PADS_NUM		2
+
+#define CSI2_OUTPUT_IPIPEIF	(1 << 0)
+#define CSI2_OUTPUT_MEMORY	(1 << 1)
+
+struct iss_csi2_device {
+	struct v4l2_subdev subdev;
+	struct media_pad pads[CSI2_PADS_NUM];
+	struct v4l2_mbus_framefmt formats[CSI2_PADS_NUM];
+
+	struct iss_video video_out;
+	struct iss_device *iss;
+
+	u8 available;		/* Is the IP present on the silicon? */
+
+	/* memory resources, as defined in enum iss_mem_resources */
+	unsigned int regs1;
+	unsigned int regs2;
+	/* ISP subclock, as defined in enum iss_isp_subclk_resource */
+	unsigned int subclk;
+
+	u32 output; /* output to IPIPEIF, memory or both? */
+	bool dpcm_decompress;
+	unsigned int frame_skip;
+	bool use_fs_irq;
+
+	struct iss_csiphy *phy;
+	struct iss_csi2_ctx_cfg contexts[ISS_CSI2_MAX_CTX_NUM + 1];
+	struct iss_csi2_timing_cfg timing[2];
+	struct iss_csi2_ctrl_cfg ctrl;
+	enum iss_pipeline_stream_state state;
+	wait_queue_head_t wait;
+	atomic_t stopping;
+};
+
+void omap4iss_csi2_isr(struct iss_csi2_device *csi2);
+int omap4iss_csi2_reset(struct iss_csi2_device *csi2);
+int omap4iss_csi2_init(struct iss_device *iss);
+void omap4iss_csi2_cleanup(struct iss_device *iss);
+void omap4iss_csi2_unregister_entities(struct iss_csi2_device *csi2);
+int omap4iss_csi2_register_entities(struct iss_csi2_device *csi2,
+				    struct v4l2_device *vdev);
+#endif	/* OMAP4_ISS_CSI2_H */
diff --git a/drivers/staging/media/omap4iss/iss_csiphy.c b/drivers/staging/media/omap4iss/iss_csiphy.c
new file mode 100644
index 000000000000..7c3d55d811ef
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss_csiphy.c
@@ -0,0 +1,279 @@
+/*
+ * TI OMAP4 ISS V4L2 Driver - CSI PHY module
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/delay.h>
+#include <linux/device.h>
+
+#include "../../../../arch/arm/mach-omap2/control.h"
+
+#include "iss.h"
+#include "iss_regs.h"
+#include "iss_csiphy.h"
+
+/*
+ * csiphy_lanes_config - Configuration of CSIPHY lanes.
+ *
+ * Updates HW configuration.
+ * Called with phy->mutex taken.
+ */
+static void csiphy_lanes_config(struct iss_csiphy *phy)
+{
+	unsigned int i;
+	u32 reg;
+
+	reg = iss_reg_read(phy->iss, phy->cfg_regs, CSI2_COMPLEXIO_CFG);
+
+	for (i = 0; i < phy->max_data_lanes; i++) {
+		reg &= ~(CSI2_COMPLEXIO_CFG_DATA_POL(i + 1) |
+			 CSI2_COMPLEXIO_CFG_DATA_POSITION_MASK(i + 1));
+		reg |= (phy->lanes.data[i].pol ?
+			CSI2_COMPLEXIO_CFG_DATA_POL(i + 1) : 0);
+		reg |= (phy->lanes.data[i].pos <<
+			CSI2_COMPLEXIO_CFG_DATA_POSITION_SHIFT(i + 1));
+	}
+
+	reg &= ~(CSI2_COMPLEXIO_CFG_CLOCK_POL |
+		 CSI2_COMPLEXIO_CFG_CLOCK_POSITION_MASK);
+	reg |= phy->lanes.clk.pol ? CSI2_COMPLEXIO_CFG_CLOCK_POL : 0;
+	reg |= phy->lanes.clk.pos << CSI2_COMPLEXIO_CFG_CLOCK_POSITION_SHIFT;
+
+	iss_reg_write(phy->iss, phy->cfg_regs, CSI2_COMPLEXIO_CFG, reg);
+}
+
+/*
+ * csiphy_set_power
+ * @power: Power state to be set.
+ *
+ * Returns 0 if successful, or -EBUSY if the retry count is exceeded.
+ */
+static int csiphy_set_power(struct iss_csiphy *phy, u32 power)
+{
+	u32 reg;
+	u8 retry_count;
+
+	iss_reg_update(phy->iss, phy->cfg_regs, CSI2_COMPLEXIO_CFG,
+		       CSI2_COMPLEXIO_CFG_PWD_CMD_MASK,
+		       power | CSI2_COMPLEXIO_CFG_PWR_AUTO);
+
+	retry_count = 0;
+	do {
+		udelay(1);
+		reg = iss_reg_read(phy->iss, phy->cfg_regs, CSI2_COMPLEXIO_CFG)
+		    & CSI2_COMPLEXIO_CFG_PWD_STATUS_MASK;
+
+		if (reg != power >> 2)
+			retry_count++;
+
+	} while ((reg != power >> 2) && (retry_count < 250));
+
+	if (retry_count == 250) {
+		dev_err(phy->iss->dev, "CSI2 CIO set power failed!\n");
+		return -EBUSY;
+	}
+
+	return 0;
+}
+
+/*
+ * csiphy_dphy_config - Configure CSI2 D-PHY parameters.
+ *
+ * Called with phy->mutex taken.
+ */
+static void csiphy_dphy_config(struct iss_csiphy *phy)
+{
+	u32 reg;
+
+	/* Set up REGISTER0 */
+	reg = phy->dphy.ths_term << REGISTER0_THS_TERM_SHIFT;
+	reg |= phy->dphy.ths_settle << REGISTER0_THS_SETTLE_SHIFT;
+
+	iss_reg_write(phy->iss, phy->phy_regs, REGISTER0, reg);
+
+	/* Set up REGISTER1 */
+	reg = phy->dphy.tclk_term << REGISTER1_TCLK_TERM_SHIFT;
+	reg |= phy->dphy.tclk_miss << REGISTER1_CTRLCLK_DIV_FACTOR_SHIFT;
+	reg |= phy->dphy.tclk_settle << REGISTER1_TCLK_SETTLE_SHIFT;
+	reg |= 0xb8 << REGISTER1_DPHY_HS_SYNC_PATTERN_SHIFT;
+
+	iss_reg_write(phy->iss, phy->phy_regs, REGISTER1, reg);
+}
+
+/*
+ * TCLK values are OK at their reset values
+ */
+#define TCLK_TERM	0
+#define TCLK_MISS	1
+#define TCLK_SETTLE	14
+
+int omap4iss_csiphy_config(struct iss_device *iss,
+			   struct v4l2_subdev *csi2_subdev)
+{
+	struct iss_csi2_device *csi2 = v4l2_get_subdevdata(csi2_subdev);
+	struct iss_pipeline *pipe = to_iss_pipeline(&csi2_subdev->entity);
+	struct iss_v4l2_subdevs_group *subdevs = pipe->external->host_priv;
+	struct iss_csiphy_dphy_cfg csi2phy;
+	int csi2_ddrclk_khz;
+	struct iss_csiphy_lanes_cfg *lanes;
+	unsigned int used_lanes = 0;
+	u32 cam_rx_ctrl;
+	unsigned int i;
+
+	lanes = &subdevs->bus.csi2.lanecfg;
+
+	/*
+	 * SCM.CONTROL_CAMERA_RX
+	 * - bit [31] : CSIPHY2 lane 2 enable (4460+ only)
+	 * - bit [30:29] : CSIPHY2 per-lane enable (1 to 0)
+	 * - bit [28:24] : CSIPHY1 per-lane enable (4 to 0)
+	 * - bit [21] : CSIPHY2 CTRLCLK enable
+	 * - bit [20:19] : CSIPHY2 config: 00 d-phy, 01/10 ccp2
+	 * - bit [18] : CSIPHY1 CTRLCLK enable
+	 * - bit [17:16] : CSIPHY1 config: 00 d-phy, 01/10 ccp2
+	 */
+	cam_rx_ctrl = omap4_ctrl_pad_readl(
+			OMAP4_CTRL_MODULE_PAD_CORE_CONTROL_CAMERA_RX);
+
+
+	if (subdevs->interface == ISS_INTERFACE_CSI2A_PHY1) {
+		cam_rx_ctrl &= ~(OMAP4_CAMERARX_CSI21_LANEENABLE_MASK |
+				OMAP4_CAMERARX_CSI21_CAMMODE_MASK);
+		/* NOTE: Leave CSIPHY1 config to 0x0: D-PHY mode */
+		/* Enable all lanes for now */
+		cam_rx_ctrl |=
+			0x1f << OMAP4_CAMERARX_CSI21_LANEENABLE_SHIFT;
+		/* Enable CTRLCLK */
+		cam_rx_ctrl |= OMAP4_CAMERARX_CSI21_CTRLCLKEN_MASK;
+	}
+
+	if (subdevs->interface == ISS_INTERFACE_CSI2B_PHY2) {
+		cam_rx_ctrl &= ~(OMAP4_CAMERARX_CSI22_LANEENABLE_MASK |
+				OMAP4_CAMERARX_CSI22_CAMMODE_MASK);
+		/* NOTE: Leave CSIPHY2 config to 0x0: D-PHY mode */
+		/* Enable all lanes for now */
+		cam_rx_ctrl |=
+			0x3 << OMAP4_CAMERARX_CSI22_LANEENABLE_SHIFT;
+		/* Enable CTRLCLK */
+		cam_rx_ctrl |= OMAP4_CAMERARX_CSI22_CTRLCLKEN_MASK;
+	}
+
+	omap4_ctrl_pad_writel(cam_rx_ctrl,
+		 OMAP4_CTRL_MODULE_PAD_CORE_CONTROL_CAMERA_RX);
+
+	/* Reset used lane count */
+	csi2->phy->used_data_lanes = 0;
+
+	/* Clock and data lanes verification */
+	for (i = 0; i < csi2->phy->max_data_lanes; i++) {
+		if (lanes->data[i].pos == 0)
+			continue;
+
+		if (lanes->data[i].pol > 1 ||
+		    lanes->data[i].pos > (csi2->phy->max_data_lanes + 1))
+			return -EINVAL;
+
+		if (used_lanes & (1 << lanes->data[i].pos))
+			return -EINVAL;
+
+		used_lanes |= 1 << lanes->data[i].pos;
+		csi2->phy->used_data_lanes++;
+	}
+
+	if (lanes->clk.pol > 1 ||
+	    lanes->clk.pos > (csi2->phy->max_data_lanes + 1))
+		return -EINVAL;
+
+	if (lanes->clk.pos == 0 || used_lanes & (1 << lanes->clk.pos))
+		return -EINVAL;
+
+	csi2_ddrclk_khz = pipe->external_rate / 1000
+		/ (2 * csi2->phy->used_data_lanes)
+		* pipe->external_bpp;
+
+	/*
+	 * THS_TERM: Programmed value = ceil(12.5 ns/DDRClk period) - 1.
+	 * THS_SETTLE: Programmed value = ceil(90 ns/DDRClk period) + 3.
+	 */
+	csi2phy.ths_term = DIV_ROUND_UP(25 * csi2_ddrclk_khz, 2000000) - 1;
+	csi2phy.ths_settle = DIV_ROUND_UP(90 * csi2_ddrclk_khz, 1000000) + 3;
+	csi2phy.tclk_term = TCLK_TERM;
+	csi2phy.tclk_miss = TCLK_MISS;
+	csi2phy.tclk_settle = TCLK_SETTLE;
+
+	mutex_lock(&csi2->phy->mutex);
+	csi2->phy->dphy = csi2phy;
+	csi2->phy->lanes = *lanes;
+	mutex_unlock(&csi2->phy->mutex);
+
+	return 0;
+}
+
+int omap4iss_csiphy_acquire(struct iss_csiphy *phy)
+{
+	int rval;
+
+	mutex_lock(&phy->mutex);
+
+	rval = omap4iss_csi2_reset(phy->csi2);
+	if (rval)
+		goto done;
+
+	csiphy_dphy_config(phy);
+	csiphy_lanes_config(phy);
+
+	rval = csiphy_set_power(phy, CSI2_COMPLEXIO_CFG_PWD_CMD_ON);
+	if (rval)
+		goto done;
+
+	phy->phy_in_use = 1;
+
+done:
+	mutex_unlock(&phy->mutex);
+	return rval;
+}
+
+void omap4iss_csiphy_release(struct iss_csiphy *phy)
+{
+	mutex_lock(&phy->mutex);
+	if (phy->phy_in_use) {
+		csiphy_set_power(phy, CSI2_COMPLEXIO_CFG_PWD_CMD_OFF);
+		phy->phy_in_use = 0;
+	}
+	mutex_unlock(&phy->mutex);
+}
+
+/*
+ * omap4iss_csiphy_init - Initialize the CSI PHY frontends
+ */
+int omap4iss_csiphy_init(struct iss_device *iss)
+{
+	struct iss_csiphy *phy1 = &iss->csiphy1;
+	struct iss_csiphy *phy2 = &iss->csiphy2;
+
+	phy1->iss = iss;
+	phy1->csi2 = &iss->csi2a;
+	phy1->max_data_lanes = ISS_CSIPHY1_NUM_DATA_LANES;
+	phy1->used_data_lanes = 0;
+	phy1->cfg_regs = OMAP4_ISS_MEM_CSI2_A_REGS1;
+	phy1->phy_regs = OMAP4_ISS_MEM_CAMERARX_CORE1;
+	mutex_init(&phy1->mutex);
+
+	phy2->iss = iss;
+	phy2->csi2 = &iss->csi2b;
+	phy2->max_data_lanes = ISS_CSIPHY2_NUM_DATA_LANES;
+	phy2->used_data_lanes = 0;
+	phy2->cfg_regs = OMAP4_ISS_MEM_CSI2_B_REGS1;
+	phy2->phy_regs = OMAP4_ISS_MEM_CAMERARX_CORE2;
+	mutex_init(&phy2->mutex);
+
+	return 0;
+}
diff --git a/drivers/staging/media/omap4iss/iss_csiphy.h b/drivers/staging/media/omap4iss/iss_csiphy.h
new file mode 100644
index 000000000000..e9ca43955654
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss_csiphy.h
@@ -0,0 +1,51 @@
+/*
+ * TI OMAP4 ISS V4L2 Driver - CSI PHY module
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@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.
+ */
+
+#ifndef OMAP4_ISS_CSI_PHY_H
+#define OMAP4_ISS_CSI_PHY_H
+
+#include <media/omap4iss.h>
+
+struct iss_csi2_device;
+
+struct iss_csiphy_dphy_cfg {
+	u8 ths_term;
+	u8 ths_settle;
+	u8 tclk_term;
+	unsigned tclk_miss:1;
+	u8 tclk_settle;
+};
+
+struct iss_csiphy {
+	struct iss_device *iss;
+	struct mutex mutex;	/* serialize csiphy configuration */
+	u8 phy_in_use;
+	struct iss_csi2_device *csi2;
+
+	/* memory resources, as defined in enum iss_mem_resources */
+	unsigned int cfg_regs;
+	unsigned int phy_regs;
+
+	u8 max_data_lanes;	/* number of CSI2 Data Lanes supported */
+	u8 used_data_lanes;	/* number of CSI2 Data Lanes used */
+	struct iss_csiphy_lanes_cfg lanes;
+	struct iss_csiphy_dphy_cfg dphy;
+};
+
+int omap4iss_csiphy_config(struct iss_device *iss,
+			   struct v4l2_subdev *csi2_subdev);
+int omap4iss_csiphy_acquire(struct iss_csiphy *phy);
+void omap4iss_csiphy_release(struct iss_csiphy *phy);
+int omap4iss_csiphy_init(struct iss_device *iss);
+
+#endif	/* OMAP4_ISS_CSI_PHY_H */
diff --git a/drivers/staging/media/omap4iss/iss_ipipe.c b/drivers/staging/media/omap4iss/iss_ipipe.c
new file mode 100644
index 000000000000..6eaafc5e2eea
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss_ipipe.c
@@ -0,0 +1,570 @@
+/*
+ * TI OMAP4 ISS V4L2 Driver - ISP IPIPE module
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/uaccess.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+
+#include "iss.h"
+#include "iss_regs.h"
+#include "iss_ipipe.h"
+
+static struct v4l2_mbus_framefmt *
+__ipipe_get_format(struct iss_ipipe_device *ipipe, struct v4l2_subdev_fh *fh,
+		  unsigned int pad, enum v4l2_subdev_format_whence which);
+
+static const unsigned int ipipe_fmts[] = {
+	V4L2_MBUS_FMT_SGRBG10_1X10,
+	V4L2_MBUS_FMT_SRGGB10_1X10,
+	V4L2_MBUS_FMT_SBGGR10_1X10,
+	V4L2_MBUS_FMT_SGBRG10_1X10,
+};
+
+/*
+ * ipipe_print_status - Print current IPIPE Module register values.
+ * @ipipe: Pointer to ISS ISP IPIPE device.
+ *
+ * Also prints other debug information stored in the IPIPE module.
+ */
+#define IPIPE_PRINT_REGISTER(iss, name)\
+	dev_dbg(iss->dev, "###IPIPE " #name "=0x%08x\n", \
+		iss_reg_read(iss, OMAP4_ISS_MEM_ISP_IPIPE, IPIPE_##name))
+
+static void ipipe_print_status(struct iss_ipipe_device *ipipe)
+{
+	struct iss_device *iss = to_iss_device(ipipe);
+
+	dev_dbg(iss->dev, "-------------IPIPE Register dump-------------\n");
+
+	IPIPE_PRINT_REGISTER(iss, SRC_EN);
+	IPIPE_PRINT_REGISTER(iss, SRC_MODE);
+	IPIPE_PRINT_REGISTER(iss, SRC_FMT);
+	IPIPE_PRINT_REGISTER(iss, SRC_COL);
+	IPIPE_PRINT_REGISTER(iss, SRC_VPS);
+	IPIPE_PRINT_REGISTER(iss, SRC_VSZ);
+	IPIPE_PRINT_REGISTER(iss, SRC_HPS);
+	IPIPE_PRINT_REGISTER(iss, SRC_HSZ);
+	IPIPE_PRINT_REGISTER(iss, GCK_MMR);
+	IPIPE_PRINT_REGISTER(iss, YUV_PHS);
+
+	dev_dbg(iss->dev, "-----------------------------------------------\n");
+}
+
+/*
+ * ipipe_enable - Enable/Disable IPIPE.
+ * @enable: enable flag
+ *
+ */
+static void ipipe_enable(struct iss_ipipe_device *ipipe, u8 enable)
+{
+	struct iss_device *iss = to_iss_device(ipipe);
+
+	iss_reg_update(iss, OMAP4_ISS_MEM_ISP_IPIPE, IPIPE_SRC_EN,
+		       IPIPE_SRC_EN_EN, enable ? IPIPE_SRC_EN_EN : 0);
+}
+
+/* -----------------------------------------------------------------------------
+ * Format- and pipeline-related configuration helpers
+ */
+
+static void ipipe_configure(struct iss_ipipe_device *ipipe)
+{
+	struct iss_device *iss = to_iss_device(ipipe);
+	struct v4l2_mbus_framefmt *format;
+
+	/* IPIPE_PAD_SINK */
+	format = &ipipe->formats[IPIPE_PAD_SINK];
+
+	/* NOTE: Currently just supporting pipeline IN: RGB, OUT: YUV422 */
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_IPIPE, IPIPE_SRC_FMT,
+		      IPIPE_SRC_FMT_RAW2YUV);
+
+	/* Enable YUV444 -> YUV422 conversion */
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_IPIPE, IPIPE_YUV_PHS,
+		      IPIPE_YUV_PHS_LPF);
+
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_IPIPE, IPIPE_SRC_VPS, 0);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_IPIPE, IPIPE_SRC_HPS, 0);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_IPIPE, IPIPE_SRC_VSZ,
+		      (format->height - 2) & IPIPE_SRC_VSZ_MASK);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_IPIPE, IPIPE_SRC_HSZ,
+		      (format->width - 1) & IPIPE_SRC_HSZ_MASK);
+
+	/* Ignore ipipeif_wrt signal, and operate on-the-fly.  */
+	iss_reg_clr(iss, OMAP4_ISS_MEM_ISP_IPIPE, IPIPE_SRC_MODE,
+		    IPIPE_SRC_MODE_WRT | IPIPE_SRC_MODE_OST);
+
+	/* HACK: Values tuned for Ducati SW (OV) */
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_IPIPE, IPIPE_SRC_COL,
+		      IPIPE_SRC_COL_EE_B | IPIPE_SRC_COL_EO_GB |
+		      IPIPE_SRC_COL_OE_GR | IPIPE_SRC_COL_OO_R);
+
+	/* IPIPE_PAD_SOURCE_VP */
+	format = &ipipe->formats[IPIPE_PAD_SOURCE_VP];
+	/* Do nothing? */
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev operations
+ */
+
+/*
+ * ipipe_set_stream - Enable/Disable streaming on the IPIPE module
+ * @sd: ISP IPIPE V4L2 subdevice
+ * @enable: Enable/disable stream
+ */
+static int ipipe_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct iss_ipipe_device *ipipe = v4l2_get_subdevdata(sd);
+	struct iss_device *iss = to_iss_device(ipipe);
+	int ret = 0;
+
+	if (ipipe->state == ISS_PIPELINE_STREAM_STOPPED) {
+		if (enable == ISS_PIPELINE_STREAM_STOPPED)
+			return 0;
+
+		omap4iss_isp_subclk_enable(iss, OMAP4_ISS_ISP_SUBCLK_IPIPE);
+
+		/* Enable clk_arm_g0 */
+		iss_reg_write(iss, OMAP4_ISS_MEM_ISP_IPIPE, IPIPE_GCK_MMR,
+			      IPIPE_GCK_MMR_REG);
+
+		/* Enable clk_pix_g[3:0] */
+		iss_reg_write(iss, OMAP4_ISS_MEM_ISP_IPIPE, IPIPE_GCK_PIX,
+			      IPIPE_GCK_PIX_G3 | IPIPE_GCK_PIX_G2 |
+			      IPIPE_GCK_PIX_G1 | IPIPE_GCK_PIX_G0);
+	}
+
+	switch (enable) {
+	case ISS_PIPELINE_STREAM_CONTINUOUS:
+
+		ipipe_configure(ipipe);
+		ipipe_print_status(ipipe);
+
+		atomic_set(&ipipe->stopping, 0);
+		ipipe_enable(ipipe, 1);
+		break;
+
+	case ISS_PIPELINE_STREAM_STOPPED:
+		if (ipipe->state == ISS_PIPELINE_STREAM_STOPPED)
+			return 0;
+		if (omap4iss_module_sync_idle(&sd->entity, &ipipe->wait,
+					      &ipipe->stopping))
+			ret = -ETIMEDOUT;
+
+		ipipe_enable(ipipe, 0);
+		omap4iss_isp_subclk_disable(iss, OMAP4_ISS_ISP_SUBCLK_IPIPE);
+		break;
+	}
+
+	ipipe->state = enable;
+	return ret;
+}
+
+static struct v4l2_mbus_framefmt *
+__ipipe_get_format(struct iss_ipipe_device *ipipe, struct v4l2_subdev_fh *fh,
+		  unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(fh, pad);
+	else
+		return &ipipe->formats[pad];
+}
+
+/*
+ * ipipe_try_format - Try video format on a pad
+ * @ipipe: ISS IPIPE device
+ * @fh : V4L2 subdev file handle
+ * @pad: Pad number
+ * @fmt: Format
+ */
+static void
+ipipe_try_format(struct iss_ipipe_device *ipipe, struct v4l2_subdev_fh *fh,
+		unsigned int pad, struct v4l2_mbus_framefmt *fmt,
+		enum v4l2_subdev_format_whence which)
+{
+	struct v4l2_mbus_framefmt *format;
+	unsigned int width = fmt->width;
+	unsigned int height = fmt->height;
+	unsigned int i;
+
+	switch (pad) {
+	case IPIPE_PAD_SINK:
+		for (i = 0; i < ARRAY_SIZE(ipipe_fmts); i++) {
+			if (fmt->code == ipipe_fmts[i])
+				break;
+		}
+
+		/* If not found, use SGRBG10 as default */
+		if (i >= ARRAY_SIZE(ipipe_fmts))
+			fmt->code = V4L2_MBUS_FMT_SGRBG10_1X10;
+
+		/* Clamp the input size. */
+		fmt->width = clamp_t(u32, width, 1, 8192);
+		fmt->height = clamp_t(u32, height, 1, 8192);
+		fmt->colorspace = V4L2_COLORSPACE_SRGB;
+		break;
+
+	case IPIPE_PAD_SOURCE_VP:
+		format = __ipipe_get_format(ipipe, fh, IPIPE_PAD_SINK, which);
+		memcpy(fmt, format, sizeof(*fmt));
+
+		fmt->code = V4L2_MBUS_FMT_UYVY8_1X16;
+		fmt->width = clamp_t(u32, width, 32, fmt->width);
+		fmt->height = clamp_t(u32, height, 32, fmt->height);
+		fmt->colorspace = V4L2_COLORSPACE_JPEG;
+		break;
+	}
+
+	fmt->field = V4L2_FIELD_NONE;
+}
+
+/*
+ * ipipe_enum_mbus_code - Handle pixel format enumeration
+ * @sd     : pointer to v4l2 subdev structure
+ * @fh : V4L2 subdev file handle
+ * @code   : pointer to v4l2_subdev_mbus_code_enum structure
+ * return -EINVAL or zero on success
+ */
+static int ipipe_enum_mbus_code(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_fh *fh,
+			       struct v4l2_subdev_mbus_code_enum *code)
+{
+	switch (code->pad) {
+	case IPIPE_PAD_SINK:
+		if (code->index >= ARRAY_SIZE(ipipe_fmts))
+			return -EINVAL;
+
+		code->code = ipipe_fmts[code->index];
+		break;
+
+	case IPIPE_PAD_SOURCE_VP:
+		/* FIXME: Forced format conversion inside IPIPE ? */
+		if (code->index != 0)
+			return -EINVAL;
+
+		code->code = V4L2_MBUS_FMT_UYVY8_1X16;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int ipipe_enum_frame_size(struct v4l2_subdev *sd,
+				struct v4l2_subdev_fh *fh,
+				struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct iss_ipipe_device *ipipe = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt format;
+
+	if (fse->index != 0)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = 1;
+	format.height = 1;
+	ipipe_try_format(ipipe, fh, fse->pad, &format, V4L2_SUBDEV_FORMAT_TRY);
+	fse->min_width = format.width;
+	fse->min_height = format.height;
+
+	if (format.code != fse->code)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = -1;
+	format.height = -1;
+	ipipe_try_format(ipipe, fh, fse->pad, &format, V4L2_SUBDEV_FORMAT_TRY);
+	fse->max_width = format.width;
+	fse->max_height = format.height;
+
+	return 0;
+}
+
+/*
+ * ipipe_get_format - Retrieve the video format on a pad
+ * @sd : ISP IPIPE V4L2 subdevice
+ * @fh : V4L2 subdev file handle
+ * @fmt: Format
+ *
+ * Return 0 on success or -EINVAL if the pad is invalid or doesn't correspond
+ * to the format type.
+ */
+static int ipipe_get_format(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct iss_ipipe_device *ipipe = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __ipipe_get_format(ipipe, fh, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	fmt->format = *format;
+	return 0;
+}
+
+/*
+ * ipipe_set_format - Set the video format on a pad
+ * @sd : ISP IPIPE V4L2 subdevice
+ * @fh : V4L2 subdev file handle
+ * @fmt: Format
+ *
+ * Return 0 on success or -EINVAL if the pad is invalid or doesn't correspond
+ * to the format type.
+ */
+static int ipipe_set_format(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct iss_ipipe_device *ipipe = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __ipipe_get_format(ipipe, fh, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	ipipe_try_format(ipipe, fh, fmt->pad, &fmt->format, fmt->which);
+	*format = fmt->format;
+
+	/* Propagate the format from sink to source */
+	if (fmt->pad == IPIPE_PAD_SINK) {
+		format = __ipipe_get_format(ipipe, fh, IPIPE_PAD_SOURCE_VP,
+					   fmt->which);
+		*format = fmt->format;
+		ipipe_try_format(ipipe, fh, IPIPE_PAD_SOURCE_VP, format,
+				fmt->which);
+	}
+
+	return 0;
+}
+
+static int ipipe_link_validate(struct v4l2_subdev *sd, struct media_link *link,
+				 struct v4l2_subdev_format *source_fmt,
+				 struct v4l2_subdev_format *sink_fmt)
+{
+	/* Check if the two ends match */
+	if (source_fmt->format.width != sink_fmt->format.width ||
+	    source_fmt->format.height != sink_fmt->format.height)
+		return -EPIPE;
+
+	if (source_fmt->format.code != sink_fmt->format.code)
+		return -EPIPE;
+
+	return 0;
+}
+
+/*
+ * ipipe_init_formats - Initialize formats on all pads
+ * @sd: ISP IPIPE V4L2 subdevice
+ * @fh: V4L2 subdev file handle
+ *
+ * Initialize all pad formats with default values. If fh is not NULL, try
+ * formats are initialized on the file handle. Otherwise active formats are
+ * initialized on the device.
+ */
+static int ipipe_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_subdev_format format;
+
+	memset(&format, 0, sizeof(format));
+	format.pad = IPIPE_PAD_SINK;
+	format.which = fh ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+	format.format.code = V4L2_MBUS_FMT_SGRBG10_1X10;
+	format.format.width = 4096;
+	format.format.height = 4096;
+	ipipe_set_format(sd, fh, &format);
+
+	return 0;
+}
+
+/* V4L2 subdev video operations */
+static const struct v4l2_subdev_video_ops ipipe_v4l2_video_ops = {
+	.s_stream = ipipe_set_stream,
+};
+
+/* V4L2 subdev pad operations */
+static const struct v4l2_subdev_pad_ops ipipe_v4l2_pad_ops = {
+	.enum_mbus_code = ipipe_enum_mbus_code,
+	.enum_frame_size = ipipe_enum_frame_size,
+	.get_fmt = ipipe_get_format,
+	.set_fmt = ipipe_set_format,
+	.link_validate = ipipe_link_validate,
+};
+
+/* V4L2 subdev operations */
+static const struct v4l2_subdev_ops ipipe_v4l2_ops = {
+	.video = &ipipe_v4l2_video_ops,
+	.pad = &ipipe_v4l2_pad_ops,
+};
+
+/* V4L2 subdev internal operations */
+static const struct v4l2_subdev_internal_ops ipipe_v4l2_internal_ops = {
+	.open = ipipe_init_formats,
+};
+
+/* -----------------------------------------------------------------------------
+ * Media entity operations
+ */
+
+/*
+ * ipipe_link_setup - Setup IPIPE connections
+ * @entity: IPIPE media entity
+ * @local: Pad at the local end of the link
+ * @remote: Pad at the remote end of the link
+ * @flags: Link flags
+ *
+ * return -EINVAL or zero on success
+ */
+static int ipipe_link_setup(struct media_entity *entity,
+			   const struct media_pad *local,
+			   const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct iss_ipipe_device *ipipe = v4l2_get_subdevdata(sd);
+	struct iss_device *iss = to_iss_device(ipipe);
+
+	switch (local->index | media_entity_type(remote->entity)) {
+	case IPIPE_PAD_SINK | MEDIA_ENT_T_V4L2_SUBDEV:
+		/* Read from IPIPEIF. */
+		if (!(flags & MEDIA_LNK_FL_ENABLED)) {
+			ipipe->input = IPIPE_INPUT_NONE;
+			break;
+		}
+
+		if (ipipe->input != IPIPE_INPUT_NONE)
+			return -EBUSY;
+
+		if (remote->entity == &iss->ipipeif.subdev.entity)
+			ipipe->input = IPIPE_INPUT_IPIPEIF;
+
+		break;
+
+	case IPIPE_PAD_SOURCE_VP | MEDIA_ENT_T_V4L2_SUBDEV:
+		/* Send to RESIZER */
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (ipipe->output & ~IPIPE_OUTPUT_VP)
+				return -EBUSY;
+			ipipe->output |= IPIPE_OUTPUT_VP;
+		} else {
+			ipipe->output &= ~IPIPE_OUTPUT_VP;
+		}
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* media operations */
+static const struct media_entity_operations ipipe_media_ops = {
+	.link_setup = ipipe_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+/*
+ * ipipe_init_entities - Initialize V4L2 subdev and media entity
+ * @ipipe: ISS ISP IPIPE module
+ *
+ * Return 0 on success and a negative error code on failure.
+ */
+static int ipipe_init_entities(struct iss_ipipe_device *ipipe)
+{
+	struct v4l2_subdev *sd = &ipipe->subdev;
+	struct media_pad *pads = ipipe->pads;
+	struct media_entity *me = &sd->entity;
+	int ret;
+
+	ipipe->input = IPIPE_INPUT_NONE;
+
+	v4l2_subdev_init(sd, &ipipe_v4l2_ops);
+	sd->internal_ops = &ipipe_v4l2_internal_ops;
+	strlcpy(sd->name, "OMAP4 ISS ISP IPIPE", sizeof(sd->name));
+	sd->grp_id = 1 << 16;	/* group ID for iss subdevs */
+	v4l2_set_subdevdata(sd, ipipe);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	pads[IPIPE_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	pads[IPIPE_PAD_SOURCE_VP].flags = MEDIA_PAD_FL_SOURCE;
+
+	me->ops = &ipipe_media_ops;
+	ret = media_entity_init(me, IPIPE_PADS_NUM, pads, 0);
+	if (ret < 0)
+		return ret;
+
+	ipipe_init_formats(sd, NULL);
+
+	return 0;
+}
+
+void omap4iss_ipipe_unregister_entities(struct iss_ipipe_device *ipipe)
+{
+	media_entity_cleanup(&ipipe->subdev.entity);
+
+	v4l2_device_unregister_subdev(&ipipe->subdev);
+}
+
+int omap4iss_ipipe_register_entities(struct iss_ipipe_device *ipipe,
+	struct v4l2_device *vdev)
+{
+	int ret;
+
+	/* Register the subdev and video node. */
+	ret = v4l2_device_register_subdev(vdev, &ipipe->subdev);
+	if (ret < 0)
+		goto error;
+
+	return 0;
+
+error:
+	omap4iss_ipipe_unregister_entities(ipipe);
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * ISP IPIPE initialisation and cleanup
+ */
+
+/*
+ * omap4iss_ipipe_init - IPIPE module initialization.
+ * @iss: Device pointer specific to the OMAP4 ISS.
+ *
+ * TODO: Get the initialisation values from platform data.
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+int omap4iss_ipipe_init(struct iss_device *iss)
+{
+	struct iss_ipipe_device *ipipe = &iss->ipipe;
+
+	ipipe->state = ISS_PIPELINE_STREAM_STOPPED;
+	init_waitqueue_head(&ipipe->wait);
+
+	return ipipe_init_entities(ipipe);
+}
+
+/*
+ * omap4iss_ipipe_cleanup - IPIPE module cleanup.
+ * @iss: Device pointer specific to the OMAP4 ISS.
+ */
+void omap4iss_ipipe_cleanup(struct iss_device *iss)
+{
+	/* FIXME: are you sure there's nothing to do? */
+}
diff --git a/drivers/staging/media/omap4iss/iss_ipipe.h b/drivers/staging/media/omap4iss/iss_ipipe.h
new file mode 100644
index 000000000000..c22d9041f2a5
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss_ipipe.h
@@ -0,0 +1,67 @@
+/*
+ * TI OMAP4 ISS V4L2 Driver - ISP IPIPE module
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@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.
+ */
+
+#ifndef OMAP4_ISS_IPIPE_H
+#define OMAP4_ISS_IPIPE_H
+
+#include "iss_video.h"
+
+enum ipipe_input_entity {
+	IPIPE_INPUT_NONE,
+	IPIPE_INPUT_IPIPEIF,
+};
+
+#define IPIPE_OUTPUT_VP		(1 << 0)
+
+/* Sink and source IPIPE pads */
+#define IPIPE_PAD_SINK				0
+#define IPIPE_PAD_SOURCE_VP			1
+#define IPIPE_PADS_NUM				2
+
+/*
+ * struct iss_ipipe_device - Structure for the IPIPE module to store its own
+ *			    information
+ * @subdev: V4L2 subdevice
+ * @pads: Sink and source media entity pads
+ * @formats: Active video formats
+ * @input: Active input
+ * @output: Active outputs
+ * @error: A hardware error occurred during capture
+ * @state: Streaming state
+ * @wait: Wait queue used to stop the module
+ * @stopping: Stopping state
+ */
+struct iss_ipipe_device {
+	struct v4l2_subdev subdev;
+	struct media_pad pads[IPIPE_PADS_NUM];
+	struct v4l2_mbus_framefmt formats[IPIPE_PADS_NUM];
+
+	enum ipipe_input_entity input;
+	unsigned int output;
+	unsigned int error;
+
+	enum iss_pipeline_stream_state state;
+	wait_queue_head_t wait;
+	atomic_t stopping;
+};
+
+struct iss_device;
+
+int omap4iss_ipipe_register_entities(struct iss_ipipe_device *ipipe,
+	struct v4l2_device *vdev);
+void omap4iss_ipipe_unregister_entities(struct iss_ipipe_device *ipipe);
+
+int omap4iss_ipipe_init(struct iss_device *iss);
+void omap4iss_ipipe_cleanup(struct iss_device *iss);
+
+#endif	/* OMAP4_ISS_IPIPE_H */
diff --git a/drivers/staging/media/omap4iss/iss_ipipeif.c b/drivers/staging/media/omap4iss/iss_ipipeif.c
new file mode 100644
index 000000000000..7bc145762499
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss_ipipeif.c
@@ -0,0 +1,849 @@
+/*
+ * TI OMAP4 ISS V4L2 Driver - ISP IPIPEIF module
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/uaccess.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+
+#include "iss.h"
+#include "iss_regs.h"
+#include "iss_ipipeif.h"
+
+static const unsigned int ipipeif_fmts[] = {
+	V4L2_MBUS_FMT_SGRBG10_1X10,
+	V4L2_MBUS_FMT_SRGGB10_1X10,
+	V4L2_MBUS_FMT_SBGGR10_1X10,
+	V4L2_MBUS_FMT_SGBRG10_1X10,
+	V4L2_MBUS_FMT_UYVY8_1X16,
+	V4L2_MBUS_FMT_YUYV8_1X16,
+};
+
+/*
+ * ipipeif_print_status - Print current IPIPEIF Module register values.
+ * @ipipeif: Pointer to ISS ISP IPIPEIF device.
+ *
+ * Also prints other debug information stored in the IPIPEIF module.
+ */
+#define IPIPEIF_PRINT_REGISTER(iss, name)\
+	dev_dbg(iss->dev, "###IPIPEIF " #name "=0x%08x\n", \
+		iss_reg_read(iss, OMAP4_ISS_MEM_ISP_IPIPEIF, IPIPEIF_##name))
+
+#define ISIF_PRINT_REGISTER(iss, name)\
+	dev_dbg(iss->dev, "###ISIF " #name "=0x%08x\n", \
+		iss_reg_read(iss, OMAP4_ISS_MEM_ISP_ISIF, ISIF_##name))
+
+#define ISP5_PRINT_REGISTER(iss, name)\
+	dev_dbg(iss->dev, "###ISP5 " #name "=0x%08x\n", \
+		iss_reg_read(iss, OMAP4_ISS_MEM_ISP_SYS1, ISP5_##name))
+
+static void ipipeif_print_status(struct iss_ipipeif_device *ipipeif)
+{
+	struct iss_device *iss = to_iss_device(ipipeif);
+
+	dev_dbg(iss->dev, "-------------IPIPEIF Register dump-------------\n");
+
+	IPIPEIF_PRINT_REGISTER(iss, CFG1);
+	IPIPEIF_PRINT_REGISTER(iss, CFG2);
+
+	ISIF_PRINT_REGISTER(iss, SYNCEN);
+	ISIF_PRINT_REGISTER(iss, CADU);
+	ISIF_PRINT_REGISTER(iss, CADL);
+	ISIF_PRINT_REGISTER(iss, MODESET);
+	ISIF_PRINT_REGISTER(iss, CCOLP);
+	ISIF_PRINT_REGISTER(iss, SPH);
+	ISIF_PRINT_REGISTER(iss, LNH);
+	ISIF_PRINT_REGISTER(iss, LNV);
+	ISIF_PRINT_REGISTER(iss, VDINT(0));
+	ISIF_PRINT_REGISTER(iss, HSIZE);
+
+	ISP5_PRINT_REGISTER(iss, SYSCONFIG);
+	ISP5_PRINT_REGISTER(iss, CTRL);
+	ISP5_PRINT_REGISTER(iss, IRQSTATUS(0));
+	ISP5_PRINT_REGISTER(iss, IRQENABLE_SET(0));
+	ISP5_PRINT_REGISTER(iss, IRQENABLE_CLR(0));
+
+	dev_dbg(iss->dev, "-----------------------------------------------\n");
+}
+
+static void ipipeif_write_enable(struct iss_ipipeif_device *ipipeif, u8 enable)
+{
+	struct iss_device *iss = to_iss_device(ipipeif);
+
+	iss_reg_update(iss, OMAP4_ISS_MEM_ISP_ISIF, ISIF_SYNCEN,
+		       ISIF_SYNCEN_DWEN, enable ? ISIF_SYNCEN_DWEN : 0);
+}
+
+/*
+ * ipipeif_enable - Enable/Disable IPIPEIF.
+ * @enable: enable flag
+ *
+ */
+static void ipipeif_enable(struct iss_ipipeif_device *ipipeif, u8 enable)
+{
+	struct iss_device *iss = to_iss_device(ipipeif);
+
+	iss_reg_update(iss, OMAP4_ISS_MEM_ISP_ISIF, ISIF_SYNCEN,
+		       ISIF_SYNCEN_SYEN, enable ? ISIF_SYNCEN_SYEN : 0);
+}
+
+/* -----------------------------------------------------------------------------
+ * Format- and pipeline-related configuration helpers
+ */
+
+/*
+ * ipipeif_set_outaddr - Set memory address to save output image
+ * @ipipeif: Pointer to ISP IPIPEIF device.
+ * @addr: 32-bit memory address aligned on 32 byte boundary.
+ *
+ * Sets the memory address where the output will be saved.
+ */
+static void ipipeif_set_outaddr(struct iss_ipipeif_device *ipipeif, u32 addr)
+{
+	struct iss_device *iss = to_iss_device(ipipeif);
+
+	/* Save address splitted in Base Address H & L */
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_ISIF, ISIF_CADU,
+		      (addr >> (16 + 5)) & ISIF_CADU_MASK);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_ISIF, ISIF_CADL,
+		      (addr >> 5) & ISIF_CADL_MASK);
+}
+
+static void ipipeif_configure(struct iss_ipipeif_device *ipipeif)
+{
+	struct iss_device *iss = to_iss_device(ipipeif);
+	const struct iss_format_info *info;
+	struct v4l2_mbus_framefmt *format;
+	u32 isif_ccolp = 0;
+
+	omap4iss_configure_bridge(iss, ipipeif->input);
+
+	/* IPIPEIF_PAD_SINK */
+	format = &ipipeif->formats[IPIPEIF_PAD_SINK];
+
+	/* IPIPEIF with YUV422 input from ISIF */
+	iss_reg_clr(iss, OMAP4_ISS_MEM_ISP_IPIPEIF, IPIPEIF_CFG1,
+		    IPIPEIF_CFG1_INPSRC1_MASK | IPIPEIF_CFG1_INPSRC2_MASK);
+
+	/* Select ISIF/IPIPEIF input format */
+	switch (format->code) {
+	case V4L2_MBUS_FMT_UYVY8_1X16:
+	case V4L2_MBUS_FMT_YUYV8_1X16:
+		iss_reg_update(iss, OMAP4_ISS_MEM_ISP_ISIF, ISIF_MODESET,
+			       ISIF_MODESET_CCDMD | ISIF_MODESET_INPMOD_MASK |
+			       ISIF_MODESET_CCDW_MASK,
+			       ISIF_MODESET_INPMOD_YCBCR16);
+
+		iss_reg_update(iss, OMAP4_ISS_MEM_ISP_IPIPEIF, IPIPEIF_CFG2,
+			       IPIPEIF_CFG2_YUV8, IPIPEIF_CFG2_YUV16);
+
+		break;
+	case V4L2_MBUS_FMT_SGRBG10_1X10:
+		isif_ccolp = ISIF_CCOLP_CP0_F0_GR |
+			ISIF_CCOLP_CP1_F0_R |
+			ISIF_CCOLP_CP2_F0_B |
+			ISIF_CCOLP_CP3_F0_GB;
+		goto cont_raw;
+	case V4L2_MBUS_FMT_SRGGB10_1X10:
+		isif_ccolp = ISIF_CCOLP_CP0_F0_R |
+			ISIF_CCOLP_CP1_F0_GR |
+			ISIF_CCOLP_CP2_F0_GB |
+			ISIF_CCOLP_CP3_F0_B;
+		goto cont_raw;
+	case V4L2_MBUS_FMT_SBGGR10_1X10:
+		isif_ccolp = ISIF_CCOLP_CP0_F0_B |
+			ISIF_CCOLP_CP1_F0_GB |
+			ISIF_CCOLP_CP2_F0_GR |
+			ISIF_CCOLP_CP3_F0_R;
+		goto cont_raw;
+	case V4L2_MBUS_FMT_SGBRG10_1X10:
+		isif_ccolp = ISIF_CCOLP_CP0_F0_GB |
+			ISIF_CCOLP_CP1_F0_B |
+			ISIF_CCOLP_CP2_F0_R |
+			ISIF_CCOLP_CP3_F0_GR;
+cont_raw:
+		iss_reg_clr(iss, OMAP4_ISS_MEM_ISP_IPIPEIF, IPIPEIF_CFG2,
+			    IPIPEIF_CFG2_YUV16);
+
+		iss_reg_update(iss, OMAP4_ISS_MEM_ISP_ISIF, ISIF_MODESET,
+			       ISIF_MODESET_CCDMD | ISIF_MODESET_INPMOD_MASK |
+			       ISIF_MODESET_CCDW_MASK, ISIF_MODESET_INPMOD_RAW |
+			       ISIF_MODESET_CCDW_2BIT);
+
+		info = omap4iss_video_format_info(format->code);
+		iss_reg_update(iss, OMAP4_ISS_MEM_ISP_ISIF, ISIF_CGAMMAWD,
+			       ISIF_CGAMMAWD_GWDI_MASK,
+			       ISIF_CGAMMAWD_GWDI(info->bpp));
+
+		/* Set RAW Bayer pattern */
+		iss_reg_write(iss, OMAP4_ISS_MEM_ISP_ISIF, ISIF_CCOLP,
+			      isif_ccolp);
+		break;
+	}
+
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_ISIF, ISIF_SPH, 0 & ISIF_SPH_MASK);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_ISIF, ISIF_LNH,
+		      (format->width - 1) & ISIF_LNH_MASK);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_ISIF, ISIF_LNV,
+		      (format->height - 1) & ISIF_LNV_MASK);
+
+	/* Generate ISIF0 on the last line of the image */
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_ISIF, ISIF_VDINT(0),
+		      format->height - 1);
+
+	/* IPIPEIF_PAD_SOURCE_ISIF_SF */
+	format = &ipipeif->formats[IPIPEIF_PAD_SOURCE_ISIF_SF];
+
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_ISIF, ISIF_HSIZE,
+		      (ipipeif->video_out.bpl_value >> 5) &
+		      ISIF_HSIZE_HSIZE_MASK);
+
+	/* IPIPEIF_PAD_SOURCE_VP */
+	/* Do nothing? */
+}
+
+/* -----------------------------------------------------------------------------
+ * Interrupt handling
+ */
+
+static void ipipeif_isr_buffer(struct iss_ipipeif_device *ipipeif)
+{
+	struct iss_buffer *buffer;
+
+	/* The ISIF generates VD0 interrupts even when writes are disabled.
+	 * deal with it anyway). Disabling the ISIF when no buffer is available
+	 * is thus not be enough, we need to handle the situation explicitly.
+	 */
+	if (list_empty(&ipipeif->video_out.dmaqueue))
+		return;
+
+	ipipeif_write_enable(ipipeif, 0);
+
+	buffer = omap4iss_video_buffer_next(&ipipeif->video_out);
+	if (buffer == NULL)
+		return;
+
+	ipipeif_set_outaddr(ipipeif, buffer->iss_addr);
+
+	ipipeif_write_enable(ipipeif, 1);
+}
+
+/*
+ * ipipeif_isif0_isr - Handle ISIF0 event
+ * @ipipeif: Pointer to ISP IPIPEIF device.
+ *
+ * Executes LSC deferred enablement before next frame starts.
+ */
+static void ipipeif_isif0_isr(struct iss_ipipeif_device *ipipeif)
+{
+	struct iss_pipeline *pipe =
+			     to_iss_pipeline(&ipipeif->subdev.entity);
+	if (pipe->do_propagation)
+		atomic_inc(&pipe->frame_number);
+
+	if (ipipeif->output & IPIPEIF_OUTPUT_MEMORY)
+		ipipeif_isr_buffer(ipipeif);
+}
+
+/*
+ * omap4iss_ipipeif_isr - Configure ipipeif during interframe time.
+ * @ipipeif: Pointer to ISP IPIPEIF device.
+ * @events: IPIPEIF events
+ */
+void omap4iss_ipipeif_isr(struct iss_ipipeif_device *ipipeif, u32 events)
+{
+	if (omap4iss_module_sync_is_stopping(&ipipeif->wait,
+					     &ipipeif->stopping))
+		return;
+
+	if (events & ISP5_IRQ_ISIF_INT(0))
+		ipipeif_isif0_isr(ipipeif);
+}
+
+/* -----------------------------------------------------------------------------
+ * ISP video operations
+ */
+
+static int ipipeif_video_queue(struct iss_video *video,
+			       struct iss_buffer *buffer)
+{
+	struct iss_ipipeif_device *ipipeif = container_of(video,
+				struct iss_ipipeif_device, video_out);
+
+	if (!(ipipeif->output & IPIPEIF_OUTPUT_MEMORY))
+		return -ENODEV;
+
+	ipipeif_set_outaddr(ipipeif, buffer->iss_addr);
+
+	/*
+	 * If streaming was enabled before there was a buffer queued
+	 * or underrun happened in the ISR, the hardware was not enabled
+	 * and DMA queue flag ISS_VIDEO_DMAQUEUE_UNDERRUN is still set.
+	 * Enable it now.
+	 */
+	if (video->dmaqueue_flags & ISS_VIDEO_DMAQUEUE_UNDERRUN) {
+		if (ipipeif->output & IPIPEIF_OUTPUT_MEMORY)
+			ipipeif_write_enable(ipipeif, 1);
+		ipipeif_enable(ipipeif, 1);
+		iss_video_dmaqueue_flags_clr(video);
+	}
+
+	return 0;
+}
+
+static const struct iss_video_operations ipipeif_video_ops = {
+	.queue = ipipeif_video_queue,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev operations
+ */
+
+#define IPIPEIF_DRV_SUBCLK_MASK	(OMAP4_ISS_ISP_SUBCLK_IPIPEIF |\
+				 OMAP4_ISS_ISP_SUBCLK_ISIF)
+/*
+ * ipipeif_set_stream - Enable/Disable streaming on the IPIPEIF module
+ * @sd: ISP IPIPEIF V4L2 subdevice
+ * @enable: Enable/disable stream
+ */
+static int ipipeif_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct iss_ipipeif_device *ipipeif = v4l2_get_subdevdata(sd);
+	struct iss_device *iss = to_iss_device(ipipeif);
+	struct iss_video *video_out = &ipipeif->video_out;
+	int ret = 0;
+
+	if (ipipeif->state == ISS_PIPELINE_STREAM_STOPPED) {
+		if (enable == ISS_PIPELINE_STREAM_STOPPED)
+			return 0;
+
+		omap4iss_isp_subclk_enable(iss, IPIPEIF_DRV_SUBCLK_MASK);
+	}
+
+	switch (enable) {
+	case ISS_PIPELINE_STREAM_CONTINUOUS:
+
+		ipipeif_configure(ipipeif);
+		ipipeif_print_status(ipipeif);
+
+		/*
+		 * When outputting to memory with no buffer available, let the
+		 * buffer queue handler start the hardware. A DMA queue flag
+		 * ISS_VIDEO_DMAQUEUE_QUEUED will be set as soon as there is
+		 * a buffer available.
+		 */
+		if (ipipeif->output & IPIPEIF_OUTPUT_MEMORY &&
+		    !(video_out->dmaqueue_flags & ISS_VIDEO_DMAQUEUE_QUEUED))
+			break;
+
+		atomic_set(&ipipeif->stopping, 0);
+		if (ipipeif->output & IPIPEIF_OUTPUT_MEMORY)
+			ipipeif_write_enable(ipipeif, 1);
+		ipipeif_enable(ipipeif, 1);
+		iss_video_dmaqueue_flags_clr(video_out);
+		break;
+
+	case ISS_PIPELINE_STREAM_STOPPED:
+		if (ipipeif->state == ISS_PIPELINE_STREAM_STOPPED)
+			return 0;
+		if (omap4iss_module_sync_idle(&sd->entity, &ipipeif->wait,
+					      &ipipeif->stopping))
+			ret = -ETIMEDOUT;
+
+		if (ipipeif->output & IPIPEIF_OUTPUT_MEMORY)
+			ipipeif_write_enable(ipipeif, 0);
+		ipipeif_enable(ipipeif, 0);
+		omap4iss_isp_subclk_disable(iss, IPIPEIF_DRV_SUBCLK_MASK);
+		iss_video_dmaqueue_flags_clr(video_out);
+		break;
+	}
+
+	ipipeif->state = enable;
+	return ret;
+}
+
+static struct v4l2_mbus_framefmt *
+__ipipeif_get_format(struct iss_ipipeif_device *ipipeif,
+		     struct v4l2_subdev_fh *fh, unsigned int pad,
+		     enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(fh, pad);
+	else
+		return &ipipeif->formats[pad];
+}
+
+/*
+ * ipipeif_try_format - Try video format on a pad
+ * @ipipeif: ISS IPIPEIF device
+ * @fh : V4L2 subdev file handle
+ * @pad: Pad number
+ * @fmt: Format
+ */
+static void
+ipipeif_try_format(struct iss_ipipeif_device *ipipeif,
+		   struct v4l2_subdev_fh *fh, unsigned int pad,
+		   struct v4l2_mbus_framefmt *fmt,
+		   enum v4l2_subdev_format_whence which)
+{
+	struct v4l2_mbus_framefmt *format;
+	unsigned int width = fmt->width;
+	unsigned int height = fmt->height;
+	unsigned int i;
+
+	switch (pad) {
+	case IPIPEIF_PAD_SINK:
+		/* TODO: If the IPIPEIF output formatter pad is connected
+		 * directly to the resizer, only YUV formats can be used.
+		 */
+		for (i = 0; i < ARRAY_SIZE(ipipeif_fmts); i++) {
+			if (fmt->code == ipipeif_fmts[i])
+				break;
+		}
+
+		/* If not found, use SGRBG10 as default */
+		if (i >= ARRAY_SIZE(ipipeif_fmts))
+			fmt->code = V4L2_MBUS_FMT_SGRBG10_1X10;
+
+		/* Clamp the input size. */
+		fmt->width = clamp_t(u32, width, 1, 8192);
+		fmt->height = clamp_t(u32, height, 1, 8192);
+		break;
+
+	case IPIPEIF_PAD_SOURCE_ISIF_SF:
+		format = __ipipeif_get_format(ipipeif, fh, IPIPEIF_PAD_SINK,
+					      which);
+		memcpy(fmt, format, sizeof(*fmt));
+
+		/* The data formatter truncates the number of horizontal output
+		 * pixels to a multiple of 16. To avoid clipping data, allow
+		 * callers to request an output size bigger than the input size
+		 * up to the nearest multiple of 16.
+		 */
+		fmt->width = clamp_t(u32, width, 32, (fmt->width + 15) & ~15);
+		fmt->width &= ~15;
+		fmt->height = clamp_t(u32, height, 32, fmt->height);
+		break;
+
+	case IPIPEIF_PAD_SOURCE_VP:
+		format = __ipipeif_get_format(ipipeif, fh, IPIPEIF_PAD_SINK,
+					      which);
+		memcpy(fmt, format, sizeof(*fmt));
+
+		fmt->width = clamp_t(u32, width, 32, fmt->width);
+		fmt->height = clamp_t(u32, height, 32, fmt->height);
+		break;
+	}
+
+	/* Data is written to memory unpacked, each 10-bit or 12-bit pixel is
+	 * stored on 2 bytes.
+	 */
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+	fmt->field = V4L2_FIELD_NONE;
+}
+
+/*
+ * ipipeif_enum_mbus_code - Handle pixel format enumeration
+ * @sd     : pointer to v4l2 subdev structure
+ * @fh : V4L2 subdev file handle
+ * @code   : pointer to v4l2_subdev_mbus_code_enum structure
+ * return -EINVAL or zero on success
+ */
+static int ipipeif_enum_mbus_code(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_fh *fh,
+			       struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct iss_ipipeif_device *ipipeif = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	switch (code->pad) {
+	case IPIPEIF_PAD_SINK:
+		if (code->index >= ARRAY_SIZE(ipipeif_fmts))
+			return -EINVAL;
+
+		code->code = ipipeif_fmts[code->index];
+		break;
+
+	case IPIPEIF_PAD_SOURCE_ISIF_SF:
+	case IPIPEIF_PAD_SOURCE_VP:
+		/* No format conversion inside IPIPEIF */
+		if (code->index != 0)
+			return -EINVAL;
+
+		format = __ipipeif_get_format(ipipeif, fh, IPIPEIF_PAD_SINK,
+					      V4L2_SUBDEV_FORMAT_TRY);
+
+		code->code = format->code;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int ipipeif_enum_frame_size(struct v4l2_subdev *sd,
+				struct v4l2_subdev_fh *fh,
+				struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct iss_ipipeif_device *ipipeif = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt format;
+
+	if (fse->index != 0)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = 1;
+	format.height = 1;
+	ipipeif_try_format(ipipeif, fh, fse->pad, &format,
+			   V4L2_SUBDEV_FORMAT_TRY);
+	fse->min_width = format.width;
+	fse->min_height = format.height;
+
+	if (format.code != fse->code)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = -1;
+	format.height = -1;
+	ipipeif_try_format(ipipeif, fh, fse->pad, &format,
+			   V4L2_SUBDEV_FORMAT_TRY);
+	fse->max_width = format.width;
+	fse->max_height = format.height;
+
+	return 0;
+}
+
+/*
+ * ipipeif_get_format - Retrieve the video format on a pad
+ * @sd : ISP IPIPEIF V4L2 subdevice
+ * @fh : V4L2 subdev file handle
+ * @fmt: Format
+ *
+ * Return 0 on success or -EINVAL if the pad is invalid or doesn't correspond
+ * to the format type.
+ */
+static int ipipeif_get_format(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct iss_ipipeif_device *ipipeif = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __ipipeif_get_format(ipipeif, fh, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	fmt->format = *format;
+	return 0;
+}
+
+/*
+ * ipipeif_set_format - Set the video format on a pad
+ * @sd : ISP IPIPEIF V4L2 subdevice
+ * @fh : V4L2 subdev file handle
+ * @fmt: Format
+ *
+ * Return 0 on success or -EINVAL if the pad is invalid or doesn't correspond
+ * to the format type.
+ */
+static int ipipeif_set_format(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct iss_ipipeif_device *ipipeif = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __ipipeif_get_format(ipipeif, fh, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	ipipeif_try_format(ipipeif, fh, fmt->pad, &fmt->format, fmt->which);
+	*format = fmt->format;
+
+	/* Propagate the format from sink to source */
+	if (fmt->pad == IPIPEIF_PAD_SINK) {
+		format = __ipipeif_get_format(ipipeif, fh,
+					      IPIPEIF_PAD_SOURCE_ISIF_SF,
+					      fmt->which);
+		*format = fmt->format;
+		ipipeif_try_format(ipipeif, fh, IPIPEIF_PAD_SOURCE_ISIF_SF,
+				   format, fmt->which);
+
+		format = __ipipeif_get_format(ipipeif, fh,
+					      IPIPEIF_PAD_SOURCE_VP,
+					      fmt->which);
+		*format = fmt->format;
+		ipipeif_try_format(ipipeif, fh, IPIPEIF_PAD_SOURCE_VP, format,
+				fmt->which);
+	}
+
+	return 0;
+}
+
+static int ipipeif_link_validate(struct v4l2_subdev *sd,
+				 struct media_link *link,
+				 struct v4l2_subdev_format *source_fmt,
+				 struct v4l2_subdev_format *sink_fmt)
+{
+	/* Check if the two ends match */
+	if (source_fmt->format.width != sink_fmt->format.width ||
+	    source_fmt->format.height != sink_fmt->format.height)
+		return -EPIPE;
+
+	if (source_fmt->format.code != sink_fmt->format.code)
+		return -EPIPE;
+
+	return 0;
+}
+
+/*
+ * ipipeif_init_formats - Initialize formats on all pads
+ * @sd: ISP IPIPEIF V4L2 subdevice
+ * @fh: V4L2 subdev file handle
+ *
+ * Initialize all pad formats with default values. If fh is not NULL, try
+ * formats are initialized on the file handle. Otherwise active formats are
+ * initialized on the device.
+ */
+static int ipipeif_init_formats(struct v4l2_subdev *sd,
+				struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_subdev_format format;
+
+	memset(&format, 0, sizeof(format));
+	format.pad = IPIPEIF_PAD_SINK;
+	format.which = fh ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+	format.format.code = V4L2_MBUS_FMT_SGRBG10_1X10;
+	format.format.width = 4096;
+	format.format.height = 4096;
+	ipipeif_set_format(sd, fh, &format);
+
+	return 0;
+}
+
+/* V4L2 subdev video operations */
+static const struct v4l2_subdev_video_ops ipipeif_v4l2_video_ops = {
+	.s_stream = ipipeif_set_stream,
+};
+
+/* V4L2 subdev pad operations */
+static const struct v4l2_subdev_pad_ops ipipeif_v4l2_pad_ops = {
+	.enum_mbus_code = ipipeif_enum_mbus_code,
+	.enum_frame_size = ipipeif_enum_frame_size,
+	.get_fmt = ipipeif_get_format,
+	.set_fmt = ipipeif_set_format,
+	.link_validate = ipipeif_link_validate,
+};
+
+/* V4L2 subdev operations */
+static const struct v4l2_subdev_ops ipipeif_v4l2_ops = {
+	.video = &ipipeif_v4l2_video_ops,
+	.pad = &ipipeif_v4l2_pad_ops,
+};
+
+/* V4L2 subdev internal operations */
+static const struct v4l2_subdev_internal_ops ipipeif_v4l2_internal_ops = {
+	.open = ipipeif_init_formats,
+};
+
+/* -----------------------------------------------------------------------------
+ * Media entity operations
+ */
+
+/*
+ * ipipeif_link_setup - Setup IPIPEIF connections
+ * @entity: IPIPEIF media entity
+ * @local: Pad at the local end of the link
+ * @remote: Pad at the remote end of the link
+ * @flags: Link flags
+ *
+ * return -EINVAL or zero on success
+ */
+static int ipipeif_link_setup(struct media_entity *entity,
+			   const struct media_pad *local,
+			   const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct iss_ipipeif_device *ipipeif = v4l2_get_subdevdata(sd);
+	struct iss_device *iss = to_iss_device(ipipeif);
+
+	switch (local->index | media_entity_type(remote->entity)) {
+	case IPIPEIF_PAD_SINK | MEDIA_ENT_T_V4L2_SUBDEV:
+		/* Read from the sensor CSI2a or CSI2b. */
+		if (!(flags & MEDIA_LNK_FL_ENABLED)) {
+			ipipeif->input = IPIPEIF_INPUT_NONE;
+			break;
+		}
+
+		if (ipipeif->input != IPIPEIF_INPUT_NONE)
+			return -EBUSY;
+
+		if (remote->entity == &iss->csi2a.subdev.entity)
+			ipipeif->input = IPIPEIF_INPUT_CSI2A;
+		else if (remote->entity == &iss->csi2b.subdev.entity)
+			ipipeif->input = IPIPEIF_INPUT_CSI2B;
+
+		break;
+
+	case IPIPEIF_PAD_SOURCE_ISIF_SF | MEDIA_ENT_T_DEVNODE:
+		/* Write to memory */
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (ipipeif->output & ~IPIPEIF_OUTPUT_MEMORY)
+				return -EBUSY;
+			ipipeif->output |= IPIPEIF_OUTPUT_MEMORY;
+		} else {
+			ipipeif->output &= ~IPIPEIF_OUTPUT_MEMORY;
+		}
+		break;
+
+	case IPIPEIF_PAD_SOURCE_VP | MEDIA_ENT_T_V4L2_SUBDEV:
+		/* Send to IPIPE/RESIZER */
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (ipipeif->output & ~IPIPEIF_OUTPUT_VP)
+				return -EBUSY;
+			ipipeif->output |= IPIPEIF_OUTPUT_VP;
+		} else {
+			ipipeif->output &= ~IPIPEIF_OUTPUT_VP;
+		}
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* media operations */
+static const struct media_entity_operations ipipeif_media_ops = {
+	.link_setup = ipipeif_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+/*
+ * ipipeif_init_entities - Initialize V4L2 subdev and media entity
+ * @ipipeif: ISS ISP IPIPEIF module
+ *
+ * Return 0 on success and a negative error code on failure.
+ */
+static int ipipeif_init_entities(struct iss_ipipeif_device *ipipeif)
+{
+	struct v4l2_subdev *sd = &ipipeif->subdev;
+	struct media_pad *pads = ipipeif->pads;
+	struct media_entity *me = &sd->entity;
+	int ret;
+
+	ipipeif->input = IPIPEIF_INPUT_NONE;
+
+	v4l2_subdev_init(sd, &ipipeif_v4l2_ops);
+	sd->internal_ops = &ipipeif_v4l2_internal_ops;
+	strlcpy(sd->name, "OMAP4 ISS ISP IPIPEIF", sizeof(sd->name));
+	sd->grp_id = 1 << 16;	/* group ID for iss subdevs */
+	v4l2_set_subdevdata(sd, ipipeif);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	pads[IPIPEIF_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	pads[IPIPEIF_PAD_SOURCE_ISIF_SF].flags = MEDIA_PAD_FL_SOURCE;
+	pads[IPIPEIF_PAD_SOURCE_VP].flags = MEDIA_PAD_FL_SOURCE;
+
+	me->ops = &ipipeif_media_ops;
+	ret = media_entity_init(me, IPIPEIF_PADS_NUM, pads, 0);
+	if (ret < 0)
+		return ret;
+
+	ipipeif_init_formats(sd, NULL);
+
+	ipipeif->video_out.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	ipipeif->video_out.ops = &ipipeif_video_ops;
+	ipipeif->video_out.iss = to_iss_device(ipipeif);
+	ipipeif->video_out.capture_mem = PAGE_ALIGN(4096 * 4096) * 3;
+	ipipeif->video_out.bpl_alignment = 32;
+	ipipeif->video_out.bpl_zero_padding = 1;
+	ipipeif->video_out.bpl_max = 0x1ffe0;
+
+	ret = omap4iss_video_init(&ipipeif->video_out, "ISP IPIPEIF");
+	if (ret < 0)
+		return ret;
+
+	/* Connect the IPIPEIF subdev to the video node. */
+	ret = media_entity_create_link(&ipipeif->subdev.entity,
+				       IPIPEIF_PAD_SOURCE_ISIF_SF,
+				       &ipipeif->video_out.video.entity, 0, 0);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+void omap4iss_ipipeif_unregister_entities(struct iss_ipipeif_device *ipipeif)
+{
+	media_entity_cleanup(&ipipeif->subdev.entity);
+
+	v4l2_device_unregister_subdev(&ipipeif->subdev);
+	omap4iss_video_unregister(&ipipeif->video_out);
+}
+
+int omap4iss_ipipeif_register_entities(struct iss_ipipeif_device *ipipeif,
+	struct v4l2_device *vdev)
+{
+	int ret;
+
+	/* Register the subdev and video node. */
+	ret = v4l2_device_register_subdev(vdev, &ipipeif->subdev);
+	if (ret < 0)
+		goto error;
+
+	ret = omap4iss_video_register(&ipipeif->video_out, vdev);
+	if (ret < 0)
+		goto error;
+
+	return 0;
+
+error:
+	omap4iss_ipipeif_unregister_entities(ipipeif);
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * ISP IPIPEIF initialisation and cleanup
+ */
+
+/*
+ * omap4iss_ipipeif_init - IPIPEIF module initialization.
+ * @iss: Device pointer specific to the OMAP4 ISS.
+ *
+ * TODO: Get the initialisation values from platform data.
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+int omap4iss_ipipeif_init(struct iss_device *iss)
+{
+	struct iss_ipipeif_device *ipipeif = &iss->ipipeif;
+
+	ipipeif->state = ISS_PIPELINE_STREAM_STOPPED;
+	init_waitqueue_head(&ipipeif->wait);
+
+	return ipipeif_init_entities(ipipeif);
+}
+
+/*
+ * omap4iss_ipipeif_cleanup - IPIPEIF module cleanup.
+ * @iss: Device pointer specific to the OMAP4 ISS.
+ */
+void omap4iss_ipipeif_cleanup(struct iss_device *iss)
+{
+	/* FIXME: are you sure there's nothing to do? */
+}
diff --git a/drivers/staging/media/omap4iss/iss_ipipeif.h b/drivers/staging/media/omap4iss/iss_ipipeif.h
new file mode 100644
index 000000000000..cbdccb982eee
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss_ipipeif.h
@@ -0,0 +1,92 @@
+/*
+ * TI OMAP4 ISS V4L2 Driver - ISP IPIPEIF module
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@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.
+ */
+
+#ifndef OMAP4_ISS_IPIPEIF_H
+#define OMAP4_ISS_IPIPEIF_H
+
+#include "iss_video.h"
+
+enum ipipeif_input_entity {
+	IPIPEIF_INPUT_NONE,
+	IPIPEIF_INPUT_CSI2A,
+	IPIPEIF_INPUT_CSI2B
+};
+
+#define IPIPEIF_OUTPUT_MEMORY		(1 << 0)
+#define IPIPEIF_OUTPUT_VP		(1 << 1)
+
+/* Sink and source IPIPEIF pads */
+#define IPIPEIF_PAD_SINK			0
+#define IPIPEIF_PAD_SOURCE_ISIF_SF		1
+#define IPIPEIF_PAD_SOURCE_VP			2
+#define IPIPEIF_PADS_NUM			3
+
+/*
+ * struct iss_ipipeif_device - Structure for the IPIPEIF module to store its own
+ *			    information
+ * @subdev: V4L2 subdevice
+ * @pads: Sink and source media entity pads
+ * @formats: Active video formats
+ * @input: Active input
+ * @output: Active outputs
+ * @video_out: Output video node
+ * @error: A hardware error occurred during capture
+ * @alaw: A-law compression enabled (1) or disabled (0)
+ * @lpf: Low pass filter enabled (1) or disabled (0)
+ * @obclamp: Optical-black clamp enabled (1) or disabled (0)
+ * @fpc_en: Faulty pixels correction enabled (1) or disabled (0)
+ * @blcomp: Black level compensation configuration
+ * @clamp: Optical-black or digital clamp configuration
+ * @fpc: Faulty pixels correction configuration
+ * @lsc: Lens shading compensation configuration
+ * @update: Bitmask of controls to update during the next interrupt
+ * @shadow_update: Controls update in progress by userspace
+ * @syncif: Interface synchronization configuration
+ * @vpcfg: Video port configuration
+ * @underrun: A buffer underrun occurred and a new buffer has been queued
+ * @state: Streaming state
+ * @lock: Serializes shadow_update with interrupt handler
+ * @wait: Wait queue used to stop the module
+ * @stopping: Stopping state
+ * @ioctl_lock: Serializes ioctl calls and LSC requests freeing
+ */
+struct iss_ipipeif_device {
+	struct v4l2_subdev subdev;
+	struct media_pad pads[IPIPEIF_PADS_NUM];
+	struct v4l2_mbus_framefmt formats[IPIPEIF_PADS_NUM];
+
+	enum ipipeif_input_entity input;
+	unsigned int output;
+	struct iss_video video_out;
+	unsigned int error;
+
+	enum iss_pipeline_stream_state state;
+	wait_queue_head_t wait;
+	atomic_t stopping;
+};
+
+struct iss_device;
+
+int omap4iss_ipipeif_init(struct iss_device *iss);
+void omap4iss_ipipeif_cleanup(struct iss_device *iss);
+int omap4iss_ipipeif_register_entities(struct iss_ipipeif_device *ipipeif,
+	struct v4l2_device *vdev);
+void omap4iss_ipipeif_unregister_entities(struct iss_ipipeif_device *ipipeif);
+
+int omap4iss_ipipeif_busy(struct iss_ipipeif_device *ipipeif);
+void omap4iss_ipipeif_isr(struct iss_ipipeif_device *ipipeif, u32 events);
+void omap4iss_ipipeif_restore_context(struct iss_device *iss);
+void omap4iss_ipipeif_max_rate(struct iss_ipipeif_device *ipipeif,
+	unsigned int *max_rate);
+
+#endif	/* OMAP4_ISS_IPIPEIF_H */
diff --git a/drivers/staging/media/omap4iss/iss_regs.h b/drivers/staging/media/omap4iss/iss_regs.h
new file mode 100644
index 000000000000..efd0291a86f7
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss_regs.h
@@ -0,0 +1,901 @@
+/*
+ * TI OMAP4 ISS V4L2 Driver - Register defines
+ *
+ * Copyright (C) 2012 Texas Instruments.
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@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.
+ */
+
+#ifndef _OMAP4_ISS_REGS_H_
+#define _OMAP4_ISS_REGS_H_
+
+/* ISS */
+#define ISS_HL_REVISION					0x0
+
+#define ISS_HL_SYSCONFIG				0x10
+#define ISS_HL_SYSCONFIG_IDLEMODE_SHIFT			2
+#define ISS_HL_SYSCONFIG_IDLEMODE_FORCEIDLE		0x0
+#define ISS_HL_SYSCONFIG_IDLEMODE_NOIDLE		0x1
+#define ISS_HL_SYSCONFIG_IDLEMODE_SMARTIDLE		0x2
+#define ISS_HL_SYSCONFIG_SOFTRESET			(1 << 0)
+
+#define ISS_HL_IRQSTATUS_RAW(i)				(0x20 + (0x10 * (i)))
+#define ISS_HL_IRQSTATUS(i)				(0x24 + (0x10 * (i)))
+#define ISS_HL_IRQENABLE_SET(i)				(0x28 + (0x10 * (i)))
+#define ISS_HL_IRQENABLE_CLR(i)				(0x2c + (0x10 * (i)))
+
+#define ISS_HL_IRQ_HS_VS				(1 << 17)
+#define ISS_HL_IRQ_SIMCOP(i)				(1 << (12 + (i)))
+#define ISS_HL_IRQ_BTE					(1 << 11)
+#define ISS_HL_IRQ_CBUFF				(1 << 10)
+#define ISS_HL_IRQ_CCP2(i)				(1 << ((i) > 3 ? 16 : 14 + (i)))
+#define ISS_HL_IRQ_CSIB					(1 << 5)
+#define ISS_HL_IRQ_CSIA					(1 << 4)
+#define ISS_HL_IRQ_ISP(i)				(1 << (i))
+
+#define ISS_CTRL					0x80
+#define ISS_CTRL_CLK_DIV_MASK				(3 << 4)
+#define ISS_CTRL_INPUT_SEL_MASK				(3 << 2)
+#define ISS_CTRL_INPUT_SEL_CSI2A			(0 << 2)
+#define ISS_CTRL_INPUT_SEL_CSI2B			(1 << 2)
+#define ISS_CTRL_SYNC_DETECT_VS_RAISING			(3 << 0)
+
+#define ISS_CLKCTRL					0x84
+#define ISS_CLKCTRL_VPORT2_CLK				(1 << 30)
+#define ISS_CLKCTRL_VPORT1_CLK				(1 << 29)
+#define ISS_CLKCTRL_VPORT0_CLK				(1 << 28)
+#define ISS_CLKCTRL_CCP2				(1 << 4)
+#define ISS_CLKCTRL_CSI2_B				(1 << 3)
+#define ISS_CLKCTRL_CSI2_A				(1 << 2)
+#define ISS_CLKCTRL_ISP					(1 << 1)
+#define ISS_CLKCTRL_SIMCOP				(1 << 0)
+
+#define ISS_CLKSTAT					0x88
+#define ISS_CLKSTAT_VPORT2_CLK				(1 << 30)
+#define ISS_CLKSTAT_VPORT1_CLK				(1 << 29)
+#define ISS_CLKSTAT_VPORT0_CLK				(1 << 28)
+#define ISS_CLKSTAT_CCP2				(1 << 4)
+#define ISS_CLKSTAT_CSI2_B				(1 << 3)
+#define ISS_CLKSTAT_CSI2_A				(1 << 2)
+#define ISS_CLKSTAT_ISP					(1 << 1)
+#define ISS_CLKSTAT_SIMCOP				(1 << 0)
+
+#define ISS_PM_STATUS					0x8c
+#define ISS_PM_STATUS_CBUFF_PM_MASK			(3 << 12)
+#define ISS_PM_STATUS_BTE_PM_MASK			(3 << 10)
+#define ISS_PM_STATUS_SIMCOP_PM_MASK			(3 << 8)
+#define ISS_PM_STATUS_ISP_PM_MASK			(3 << 6)
+#define ISS_PM_STATUS_CCP2_PM_MASK			(3 << 4)
+#define ISS_PM_STATUS_CSI2_B_PM_MASK			(3 << 2)
+#define ISS_PM_STATUS_CSI2_A_PM_MASK			(3 << 0)
+
+#define REGISTER0					0x0
+#define REGISTER0_HSCLOCKCONFIG				(1 << 24)
+#define REGISTER0_THS_TERM_MASK				(0xff << 8)
+#define REGISTER0_THS_TERM_SHIFT			8
+#define REGISTER0_THS_SETTLE_MASK			(0xff << 0)
+#define REGISTER0_THS_SETTLE_SHIFT			0
+
+#define REGISTER1					0x4
+#define REGISTER1_RESET_DONE_CTRLCLK			(1 << 29)
+#define REGISTER1_CLOCK_MISS_DETECTOR_STATUS		(1 << 25)
+#define REGISTER1_TCLK_TERM_MASK			(0x3f << 18)
+#define REGISTER1_TCLK_TERM_SHIFT			18
+#define REGISTER1_DPHY_HS_SYNC_PATTERN_SHIFT		10
+#define REGISTER1_CTRLCLK_DIV_FACTOR_MASK		(0x3 << 8)
+#define REGISTER1_CTRLCLK_DIV_FACTOR_SHIFT		8
+#define REGISTER1_TCLK_SETTLE_MASK			(0xff << 0)
+#define REGISTER1_TCLK_SETTLE_SHIFT			0
+
+#define REGISTER2					0x8
+
+#define CSI2_SYSCONFIG					0x10
+#define CSI2_SYSCONFIG_MSTANDBY_MODE_MASK		(3 << 12)
+#define CSI2_SYSCONFIG_MSTANDBY_MODE_FORCE		(0 << 12)
+#define CSI2_SYSCONFIG_MSTANDBY_MODE_NO			(1 << 12)
+#define CSI2_SYSCONFIG_MSTANDBY_MODE_SMART		(2 << 12)
+#define CSI2_SYSCONFIG_SOFT_RESET			(1 << 1)
+#define CSI2_SYSCONFIG_AUTO_IDLE			(1 << 0)
+
+#define CSI2_SYSSTATUS					0x14
+#define CSI2_SYSSTATUS_RESET_DONE			(1 << 0)
+
+#define CSI2_IRQSTATUS					0x18
+#define CSI2_IRQENABLE					0x1c
+
+/* Shared bits across CSI2_IRQENABLE and IRQSTATUS */
+
+#define CSI2_IRQ_OCP_ERR				(1 << 14)
+#define CSI2_IRQ_SHORT_PACKET				(1 << 13)
+#define CSI2_IRQ_ECC_CORRECTION				(1 << 12)
+#define CSI2_IRQ_ECC_NO_CORRECTION			(1 << 11)
+#define CSI2_IRQ_COMPLEXIO_ERR				(1 << 9)
+#define CSI2_IRQ_FIFO_OVF				(1 << 8)
+#define CSI2_IRQ_CONTEXT0				(1 << 0)
+
+#define CSI2_CTRL					0x40
+#define CSI2_CTRL_MFLAG_LEVH_MASK			(7 << 20)
+#define CSI2_CTRL_MFLAG_LEVH_SHIFT			20
+#define CSI2_CTRL_MFLAG_LEVL_MASK			(7 << 17)
+#define CSI2_CTRL_MFLAG_LEVL_SHIFT			17
+#define CSI2_CTRL_BURST_SIZE_EXPAND			(1 << 16)
+#define CSI2_CTRL_VP_CLK_EN				(1 << 15)
+#define CSI2_CTRL_NON_POSTED_WRITE			(1 << 13)
+#define CSI2_CTRL_VP_ONLY_EN				(1 << 11)
+#define CSI2_CTRL_VP_OUT_CTRL_MASK			(3 << 8)
+#define CSI2_CTRL_VP_OUT_CTRL_SHIFT			8
+#define CSI2_CTRL_DBG_EN				(1 << 7)
+#define CSI2_CTRL_BURST_SIZE_MASK			(3 << 5)
+#define CSI2_CTRL_ENDIANNESS				(1 << 4)
+#define CSI2_CTRL_FRAME					(1 << 3)
+#define CSI2_CTRL_ECC_EN				(1 << 2)
+#define CSI2_CTRL_IF_EN					(1 << 0)
+
+#define CSI2_DBG_H					0x44
+
+#define CSI2_COMPLEXIO_CFG				0x50
+#define CSI2_COMPLEXIO_CFG_RESET_CTRL			(1 << 30)
+#define CSI2_COMPLEXIO_CFG_RESET_DONE			(1 << 29)
+#define CSI2_COMPLEXIO_CFG_PWD_CMD_MASK			(3 << 27)
+#define CSI2_COMPLEXIO_CFG_PWD_CMD_OFF			(0 << 27)
+#define CSI2_COMPLEXIO_CFG_PWD_CMD_ON			(1 << 27)
+#define CSI2_COMPLEXIO_CFG_PWD_CMD_ULP			(2 << 27)
+#define CSI2_COMPLEXIO_CFG_PWD_STATUS_MASK		(3 << 25)
+#define CSI2_COMPLEXIO_CFG_PWD_STATUS_OFF		(0 << 25)
+#define CSI2_COMPLEXIO_CFG_PWD_STATUS_ON		(1 << 25)
+#define CSI2_COMPLEXIO_CFG_PWD_STATUS_ULP		(2 << 25)
+#define CSI2_COMPLEXIO_CFG_PWR_AUTO			(1 << 24)
+#define CSI2_COMPLEXIO_CFG_DATA_POL(i)			(1 << (((i) * 4) + 3))
+#define CSI2_COMPLEXIO_CFG_DATA_POSITION_MASK(i)	(7 << ((i) * 4))
+#define CSI2_COMPLEXIO_CFG_DATA_POSITION_SHIFT(i)	((i) * 4)
+#define CSI2_COMPLEXIO_CFG_CLOCK_POL			(1 << 3)
+#define CSI2_COMPLEXIO_CFG_CLOCK_POSITION_MASK		(7 << 0)
+#define CSI2_COMPLEXIO_CFG_CLOCK_POSITION_SHIFT		0
+
+#define CSI2_COMPLEXIO_IRQSTATUS			0x54
+
+#define CSI2_SHORT_PACKET				0x5c
+
+#define CSI2_COMPLEXIO_IRQENABLE			0x60
+
+/* Shared bits across CSI2_COMPLEXIO_IRQENABLE and IRQSTATUS */
+#define CSI2_COMPLEXIO_IRQ_STATEALLULPMEXIT		(1 << 26)
+#define CSI2_COMPLEXIO_IRQ_STATEALLULPMENTER		(1 << 25)
+#define CSI2_COMPLEXIO_IRQ_STATEULPM5			(1 << 24)
+#define CSI2_COMPLEXIO_IRQ_STATEULPM4			(1 << 23)
+#define CSI2_COMPLEXIO_IRQ_STATEULPM3			(1 << 22)
+#define CSI2_COMPLEXIO_IRQ_STATEULPM2			(1 << 21)
+#define CSI2_COMPLEXIO_IRQ_STATEULPM1			(1 << 20)
+#define CSI2_COMPLEXIO_IRQ_ERRCONTROL5			(1 << 19)
+#define CSI2_COMPLEXIO_IRQ_ERRCONTROL4			(1 << 18)
+#define CSI2_COMPLEXIO_IRQ_ERRCONTROL3			(1 << 17)
+#define CSI2_COMPLEXIO_IRQ_ERRCONTROL2			(1 << 16)
+#define CSI2_COMPLEXIO_IRQ_ERRCONTROL1			(1 << 15)
+#define CSI2_COMPLEXIO_IRQ_ERRESC5			(1 << 14)
+#define CSI2_COMPLEXIO_IRQ_ERRESC4			(1 << 13)
+#define CSI2_COMPLEXIO_IRQ_ERRESC3			(1 << 12)
+#define CSI2_COMPLEXIO_IRQ_ERRESC2			(1 << 11)
+#define CSI2_COMPLEXIO_IRQ_ERRESC1			(1 << 10)
+#define CSI2_COMPLEXIO_IRQ_ERRSOTSYNCHS5		(1 << 9)
+#define CSI2_COMPLEXIO_IRQ_ERRSOTSYNCHS4		(1 << 8)
+#define CSI2_COMPLEXIO_IRQ_ERRSOTSYNCHS3		(1 << 7)
+#define CSI2_COMPLEXIO_IRQ_ERRSOTSYNCHS2		(1 << 6)
+#define CSI2_COMPLEXIO_IRQ_ERRSOTSYNCHS1		(1 << 5)
+#define CSI2_COMPLEXIO_IRQ_ERRSOTHS5			(1 << 4)
+#define CSI2_COMPLEXIO_IRQ_ERRSOTHS4			(1 << 3)
+#define CSI2_COMPLEXIO_IRQ_ERRSOTHS3			(1 << 2)
+#define CSI2_COMPLEXIO_IRQ_ERRSOTHS2			(1 << 1)
+#define CSI2_COMPLEXIO_IRQ_ERRSOTHS1			(1 << 0)
+
+#define CSI2_DBG_P					0x68
+
+#define CSI2_TIMING					0x6c
+#define CSI2_TIMING_FORCE_RX_MODE_IO1			(1 << 15)
+#define CSI2_TIMING_STOP_STATE_X16_IO1			(1 << 14)
+#define CSI2_TIMING_STOP_STATE_X4_IO1			(1 << 13)
+#define CSI2_TIMING_STOP_STATE_COUNTER_IO1_MASK		(0x1fff << 0)
+#define CSI2_TIMING_STOP_STATE_COUNTER_IO1_SHIFT	0
+
+#define CSI2_CTX_CTRL1(i)				(0x70 + (0x20 * i))
+#define CSI2_CTX_CTRL1_GENERIC				(1 << 30)
+#define CSI2_CTX_CTRL1_TRANSCODE			(0xf << 24)
+#define CSI2_CTX_CTRL1_FEC_NUMBER_MASK			(0xff << 16)
+#define CSI2_CTX_CTRL1_COUNT_MASK			(0xff << 8)
+#define CSI2_CTX_CTRL1_COUNT_SHIFT			8
+#define CSI2_CTX_CTRL1_EOF_EN				(1 << 7)
+#define CSI2_CTX_CTRL1_EOL_EN				(1 << 6)
+#define CSI2_CTX_CTRL1_CS_EN				(1 << 5)
+#define CSI2_CTX_CTRL1_COUNT_UNLOCK			(1 << 4)
+#define CSI2_CTX_CTRL1_PING_PONG			(1 << 3)
+#define CSI2_CTX_CTRL1_CTX_EN				(1 << 0)
+
+#define CSI2_CTX_CTRL2(i)				(0x74 + (0x20 * i))
+#define CSI2_CTX_CTRL2_USER_DEF_MAP_SHIFT		13
+#define CSI2_CTX_CTRL2_USER_DEF_MAP_MASK		\
+		(0x3 << CSI2_CTX_CTRL2_USER_DEF_MAP_SHIFT)
+#define CSI2_CTX_CTRL2_VIRTUAL_ID_MASK			(3 << 11)
+#define CSI2_CTX_CTRL2_VIRTUAL_ID_SHIFT			11
+#define CSI2_CTX_CTRL2_DPCM_PRED			(1 << 10)
+#define CSI2_CTX_CTRL2_FORMAT_MASK			(0x3ff << 0)
+#define CSI2_CTX_CTRL2_FORMAT_SHIFT			0
+
+#define CSI2_CTX_DAT_OFST(i)				(0x78 + (0x20 * i))
+#define CSI2_CTX_DAT_OFST_MASK				(0xfff << 5)
+
+#define CSI2_CTX_PING_ADDR(i)				(0x7c + (0x20 * i))
+#define CSI2_CTX_PING_ADDR_MASK				0xffffffe0
+
+#define CSI2_CTX_PONG_ADDR(i)				(0x80 + (0x20 * i))
+#define CSI2_CTX_PONG_ADDR_MASK				CSI2_CTX_PING_ADDR_MASK
+
+#define CSI2_CTX_IRQENABLE(i)				(0x84 + (0x20 * i))
+#define CSI2_CTX_IRQSTATUS(i)				(0x88 + (0x20 * i))
+
+#define CSI2_CTX_CTRL3(i)				(0x8c + (0x20 * i))
+#define CSI2_CTX_CTRL3_ALPHA_SHIFT			5
+#define CSI2_CTX_CTRL3_ALPHA_MASK			\
+		(0x3fff << CSI2_CTX_CTRL3_ALPHA_SHIFT)
+
+/* Shared bits across CSI2_CTX_IRQENABLE and IRQSTATUS */
+#define CSI2_CTX_IRQ_ECC_CORRECTION			(1 << 8)
+#define CSI2_CTX_IRQ_LINE_NUMBER			(1 << 7)
+#define CSI2_CTX_IRQ_FRAME_NUMBER			(1 << 6)
+#define CSI2_CTX_IRQ_CS					(1 << 5)
+#define CSI2_CTX_IRQ_LE					(1 << 3)
+#define CSI2_CTX_IRQ_LS					(1 << 2)
+#define CSI2_CTX_IRQ_FE					(1 << 1)
+#define CSI2_CTX_IRQ_FS					(1 << 0)
+
+/* ISS BTE */
+#define BTE_CTRL					(0x0030)
+#define BTE_CTRL_BW_LIMITER_MASK			(0x3ff << 22)
+#define BTE_CTRL_BW_LIMITER_SHIFT			22
+
+/* ISS ISP_SYS1 */
+#define ISP5_REVISION					(0x0000)
+#define ISP5_SYSCONFIG					(0x0010)
+#define ISP5_SYSCONFIG_STANDBYMODE_MASK			(3 << 4)
+#define ISP5_SYSCONFIG_STANDBYMODE_FORCE		(0 << 4)
+#define ISP5_SYSCONFIG_STANDBYMODE_NO			(1 << 4)
+#define ISP5_SYSCONFIG_STANDBYMODE_SMART		(2 << 4)
+#define ISP5_SYSCONFIG_SOFTRESET			(1 << 1)
+
+#define ISP5_IRQSTATUS(i)				(0x0028 + (0x10 * (i)))
+#define ISP5_IRQENABLE_SET(i)				(0x002c + (0x10 * (i)))
+#define ISP5_IRQENABLE_CLR(i)				(0x0030 + (0x10 * (i)))
+
+/* Bits shared for ISP5_IRQ* registers */
+#define ISP5_IRQ_OCP_ERR				(1 << 31)
+#define ISP5_IRQ_IPIPE_INT_DPC_RNEW1			(1 << 29)
+#define ISP5_IRQ_IPIPE_INT_DPC_RNEW0			(1 << 28)
+#define ISP5_IRQ_IPIPE_INT_DPC_INIT			(1 << 27)
+#define ISP5_IRQ_IPIPE_INT_EOF				(1 << 25)
+#define ISP5_IRQ_H3A_INT_EOF				(1 << 24)
+#define ISP5_IRQ_RSZ_INT_EOF1				(1 << 23)
+#define ISP5_IRQ_RSZ_INT_EOF0				(1 << 22)
+#define ISP5_IRQ_RSZ_FIFO_IN_BLK_ERR			(1 << 19)
+#define ISP5_IRQ_RSZ_FIFO_OVF				(1 << 18)
+#define ISP5_IRQ_RSZ_INT_CYC_RSZB			(1 << 17)
+#define ISP5_IRQ_RSZ_INT_CYC_RSZA			(1 << 16)
+#define ISP5_IRQ_RSZ_INT_DMA				(1 << 15)
+#define ISP5_IRQ_RSZ_INT_LAST_PIX			(1 << 14)
+#define ISP5_IRQ_RSZ_INT_REG				(1 << 13)
+#define ISP5_IRQ_H3A_INT				(1 << 12)
+#define ISP5_IRQ_AF_INT					(1 << 11)
+#define ISP5_IRQ_AEW_INT				(1 << 10)
+#define ISP5_IRQ_IPIPEIF_IRQ				(1 << 9)
+#define ISP5_IRQ_IPIPE_INT_HST				(1 << 8)
+#define ISP5_IRQ_IPIPE_INT_BSC				(1 << 7)
+#define ISP5_IRQ_IPIPE_INT_DMA				(1 << 6)
+#define ISP5_IRQ_IPIPE_INT_LAST_PIX			(1 << 5)
+#define ISP5_IRQ_IPIPE_INT_REG				(1 << 4)
+#define ISP5_IRQ_ISIF_INT(i)				(1 << (i))
+
+#define ISP5_CTRL					(0x006c)
+#define ISP5_CTRL_MSTANDBY				(1 << 24)
+#define ISP5_CTRL_VD_PULSE_EXT				(1 << 23)
+#define ISP5_CTRL_MSTANDBY_WAIT				(1 << 20)
+#define ISP5_CTRL_BL_CLK_ENABLE				(1 << 15)
+#define ISP5_CTRL_ISIF_CLK_ENABLE			(1 << 14)
+#define ISP5_CTRL_H3A_CLK_ENABLE			(1 << 13)
+#define ISP5_CTRL_RSZ_CLK_ENABLE			(1 << 12)
+#define ISP5_CTRL_IPIPE_CLK_ENABLE			(1 << 11)
+#define ISP5_CTRL_IPIPEIF_CLK_ENABLE			(1 << 10)
+#define ISP5_CTRL_SYNC_ENABLE				(1 << 9)
+#define ISP5_CTRL_PSYNC_CLK_SEL				(1 << 8)
+
+/* ISS ISP ISIF register offsets */
+#define ISIF_SYNCEN					(0x0000)
+#define ISIF_SYNCEN_DWEN				(1 << 1)
+#define ISIF_SYNCEN_SYEN				(1 << 0)
+
+#define ISIF_MODESET					(0x0004)
+#define ISIF_MODESET_INPMOD_MASK			(3 << 12)
+#define ISIF_MODESET_INPMOD_RAW				(0 << 12)
+#define ISIF_MODESET_INPMOD_YCBCR16			(1 << 12)
+#define ISIF_MODESET_INPMOD_YCBCR8			(2 << 12)
+#define ISIF_MODESET_CCDW_MASK				(7 << 8)
+#define ISIF_MODESET_CCDW_2BIT				(2 << 8)
+#define ISIF_MODESET_CCDMD				(1 << 7)
+#define ISIF_MODESET_SWEN				(1 << 5)
+#define ISIF_MODESET_HDPOL				(1 << 3)
+#define ISIF_MODESET_VDPOL				(1 << 2)
+
+#define ISIF_SPH					(0x0018)
+#define ISIF_SPH_MASK					(0x7fff)
+
+#define ISIF_LNH					(0x001c)
+#define ISIF_LNH_MASK					(0x7fff)
+
+#define ISIF_LNV					(0x0028)
+#define ISIF_LNV_MASK					(0x7fff)
+
+#define ISIF_HSIZE					(0x0034)
+#define ISIF_HSIZE_ADCR					(1 << 12)
+#define ISIF_HSIZE_HSIZE_MASK				(0xfff)
+
+#define ISIF_CADU					(0x003c)
+#define ISIF_CADU_MASK					(0x7ff)
+
+#define ISIF_CADL					(0x0040)
+#define ISIF_CADL_MASK					(0xffff)
+
+#define ISIF_CCOLP					(0x004c)
+#define ISIF_CCOLP_CP0_F0_R				(0 << 6)
+#define ISIF_CCOLP_CP0_F0_GR				(1 << 6)
+#define ISIF_CCOLP_CP0_F0_B				(3 << 6)
+#define ISIF_CCOLP_CP0_F0_GB				(2 << 6)
+#define ISIF_CCOLP_CP1_F0_R				(0 << 4)
+#define ISIF_CCOLP_CP1_F0_GR				(1 << 4)
+#define ISIF_CCOLP_CP1_F0_B				(3 << 4)
+#define ISIF_CCOLP_CP1_F0_GB				(2 << 4)
+#define ISIF_CCOLP_CP2_F0_R				(0 << 2)
+#define ISIF_CCOLP_CP2_F0_GR				(1 << 2)
+#define ISIF_CCOLP_CP2_F0_B				(3 << 2)
+#define ISIF_CCOLP_CP2_F0_GB				(2 << 2)
+#define ISIF_CCOLP_CP3_F0_R				(0 << 0)
+#define ISIF_CCOLP_CP3_F0_GR				(1 << 0)
+#define ISIF_CCOLP_CP3_F0_B				(3 << 0)
+#define ISIF_CCOLP_CP3_F0_GB				(2 << 0)
+
+#define ISIF_VDINT(i)					(0x0070 + (i) * 4)
+#define ISIF_VDINT_MASK					(0x7fff)
+
+#define ISIF_CGAMMAWD					(0x0080)
+#define ISIF_CGAMMAWD_GWDI_MASK				(0xf << 1)
+#define ISIF_CGAMMAWD_GWDI(bpp)				((16 - (bpp)) << 1)
+
+#define ISIF_CCDCFG					(0x0088)
+#define ISIF_CCDCFG_Y8POS				(1 << 11)
+
+/* ISS ISP IPIPEIF register offsets */
+#define IPIPEIF_ENABLE					(0x0000)
+
+#define IPIPEIF_CFG1					(0x0004)
+#define IPIPEIF_CFG1_INPSRC1_MASK			(3 << 14)
+#define IPIPEIF_CFG1_INPSRC1_VPORT_RAW			(0 << 14)
+#define IPIPEIF_CFG1_INPSRC1_SDRAM_RAW			(1 << 14)
+#define IPIPEIF_CFG1_INPSRC1_ISIF_DARKFM		(2 << 14)
+#define IPIPEIF_CFG1_INPSRC1_SDRAM_YUV			(3 << 14)
+#define IPIPEIF_CFG1_INPSRC2_MASK			(3 << 2)
+#define IPIPEIF_CFG1_INPSRC2_ISIF			(0 << 2)
+#define IPIPEIF_CFG1_INPSRC2_SDRAM_RAW			(1 << 2)
+#define IPIPEIF_CFG1_INPSRC2_ISIF_DARKFM		(2 << 2)
+#define IPIPEIF_CFG1_INPSRC2_SDRAM_YUV			(3 << 2)
+
+#define IPIPEIF_CFG2					(0x0030)
+#define IPIPEIF_CFG2_YUV8P				(1 << 7)
+#define IPIPEIF_CFG2_YUV8				(1 << 6)
+#define IPIPEIF_CFG2_YUV16				(1 << 3)
+#define IPIPEIF_CFG2_VDPOL				(1 << 2)
+#define IPIPEIF_CFG2_HDPOL				(1 << 1)
+#define IPIPEIF_CFG2_INTSW				(1 << 0)
+
+#define IPIPEIF_CLKDIV					(0x0040)
+
+/* ISS ISP IPIPE register offsets */
+#define IPIPE_SRC_EN					(0x0000)
+#define IPIPE_SRC_EN_EN					(1 << 0)
+
+#define IPIPE_SRC_MODE					(0x0004)
+#define IPIPE_SRC_MODE_WRT				(1 << 1)
+#define IPIPE_SRC_MODE_OST				(1 << 0)
+
+#define IPIPE_SRC_FMT					(0x0008)
+#define IPIPE_SRC_FMT_RAW2YUV				(0 << 0)
+#define IPIPE_SRC_FMT_RAW2RAW				(1 << 0)
+#define IPIPE_SRC_FMT_RAW2STATS				(2 << 0)
+#define IPIPE_SRC_FMT_YUV2YUV				(3 << 0)
+
+#define IPIPE_SRC_COL					(0x000c)
+#define IPIPE_SRC_COL_OO_R				(0 << 6)
+#define IPIPE_SRC_COL_OO_GR				(1 << 6)
+#define IPIPE_SRC_COL_OO_B				(3 << 6)
+#define IPIPE_SRC_COL_OO_GB				(2 << 6)
+#define IPIPE_SRC_COL_OE_R				(0 << 4)
+#define IPIPE_SRC_COL_OE_GR				(1 << 4)
+#define IPIPE_SRC_COL_OE_B				(3 << 4)
+#define IPIPE_SRC_COL_OE_GB				(2 << 4)
+#define IPIPE_SRC_COL_EO_R				(0 << 2)
+#define IPIPE_SRC_COL_EO_GR				(1 << 2)
+#define IPIPE_SRC_COL_EO_B				(3 << 2)
+#define IPIPE_SRC_COL_EO_GB				(2 << 2)
+#define IPIPE_SRC_COL_EE_R				(0 << 0)
+#define IPIPE_SRC_COL_EE_GR				(1 << 0)
+#define IPIPE_SRC_COL_EE_B				(3 << 0)
+#define IPIPE_SRC_COL_EE_GB				(2 << 0)
+
+#define IPIPE_SRC_VPS					(0x0010)
+#define IPIPE_SRC_VPS_MASK				(0xffff)
+
+#define IPIPE_SRC_VSZ					(0x0014)
+#define IPIPE_SRC_VSZ_MASK				(0x1fff)
+
+#define IPIPE_SRC_HPS					(0x0018)
+#define IPIPE_SRC_HPS_MASK				(0xffff)
+
+#define IPIPE_SRC_HSZ					(0x001c)
+#define IPIPE_SRC_HSZ_MASK				(0x1ffe)
+
+#define IPIPE_SEL_SBU					(0x0020)
+
+#define IPIPE_SRC_STA					(0x0024)
+
+#define IPIPE_GCK_MMR					(0x0028)
+#define IPIPE_GCK_MMR_REG				(1 << 0)
+
+#define IPIPE_GCK_PIX					(0x002c)
+#define IPIPE_GCK_PIX_G3				(1 << 3)
+#define IPIPE_GCK_PIX_G2				(1 << 2)
+#define IPIPE_GCK_PIX_G1				(1 << 1)
+#define IPIPE_GCK_PIX_G0				(1 << 0)
+
+#define IPIPE_DPC_LUT_EN				(0x0034)
+#define IPIPE_DPC_LUT_SEL				(0x0038)
+#define IPIPE_DPC_LUT_ADR				(0x003c)
+#define IPIPE_DPC_LUT_SIZ				(0x0040)
+
+#define IPIPE_DPC_OTF_EN				(0x0044)
+#define IPIPE_DPC_OTF_TYP				(0x0048)
+#define IPIPE_DPC_OTF_2_D_THR_R				(0x004c)
+#define IPIPE_DPC_OTF_2_D_THR_GR			(0x0050)
+#define IPIPE_DPC_OTF_2_D_THR_GB			(0x0054)
+#define IPIPE_DPC_OTF_2_D_THR_B				(0x0058)
+#define IPIPE_DPC_OTF_2_C_THR_R				(0x005c)
+#define IPIPE_DPC_OTF_2_C_THR_GR			(0x0060)
+#define IPIPE_DPC_OTF_2_C_THR_GB			(0x0064)
+#define IPIPE_DPC_OTF_2_C_THR_B				(0x0068)
+#define IPIPE_DPC_OTF_3_SHF				(0x006c)
+#define IPIPE_DPC_OTF_3_D_THR				(0x0070)
+#define IPIPE_DPC_OTF_3_D_SPL				(0x0074)
+#define IPIPE_DPC_OTF_3_D_MIN				(0x0078)
+#define IPIPE_DPC_OTF_3_D_MAX				(0x007c)
+#define IPIPE_DPC_OTF_3_C_THR				(0x0080)
+#define IPIPE_DPC_OTF_3_C_SLP				(0x0084)
+#define IPIPE_DPC_OTF_3_C_MIN				(0x0088)
+#define IPIPE_DPC_OTF_3_C_MAX				(0x008c)
+
+#define IPIPE_LSC_VOFT					(0x0090)
+#define IPIPE_LSC_VA2					(0x0094)
+#define IPIPE_LSC_VA1					(0x0098)
+#define IPIPE_LSC_VS					(0x009c)
+#define IPIPE_LSC_HOFT					(0x00a0)
+#define IPIPE_LSC_HA2					(0x00a4)
+#define IPIPE_LSC_HA1					(0x00a8)
+#define IPIPE_LSC_HS					(0x00ac)
+#define IPIPE_LSC_GAN_R					(0x00b0)
+#define IPIPE_LSC_GAN_GR				(0x00b4)
+#define IPIPE_LSC_GAN_GB				(0x00b8)
+#define IPIPE_LSC_GAN_B					(0x00bc)
+#define IPIPE_LSC_OFT_R					(0x00c0)
+#define IPIPE_LSC_OFT_GR				(0x00c4)
+#define IPIPE_LSC_OFT_GB				(0x00c8)
+#define IPIPE_LSC_OFT_B					(0x00cc)
+#define IPIPE_LSC_SHF					(0x00d0)
+#define IPIPE_LSC_MAX					(0x00d4)
+
+#define IPIPE_D2F_1ST_EN				(0x00d8)
+#define IPIPE_D2F_1ST_TYP				(0x00dc)
+#define IPIPE_D2F_1ST_THR_00				(0x00e0)
+#define IPIPE_D2F_1ST_THR_01				(0x00e4)
+#define IPIPE_D2F_1ST_THR_02				(0x00e8)
+#define IPIPE_D2F_1ST_THR_03				(0x00ec)
+#define IPIPE_D2F_1ST_THR_04				(0x00f0)
+#define IPIPE_D2F_1ST_THR_05				(0x00f4)
+#define IPIPE_D2F_1ST_THR_06				(0x00f8)
+#define IPIPE_D2F_1ST_THR_07				(0x00fc)
+#define IPIPE_D2F_1ST_STR_00				(0x0100)
+#define IPIPE_D2F_1ST_STR_01				(0x0104)
+#define IPIPE_D2F_1ST_STR_02				(0x0108)
+#define IPIPE_D2F_1ST_STR_03				(0x010c)
+#define IPIPE_D2F_1ST_STR_04				(0x0110)
+#define IPIPE_D2F_1ST_STR_05				(0x0114)
+#define IPIPE_D2F_1ST_STR_06				(0x0118)
+#define IPIPE_D2F_1ST_STR_07				(0x011c)
+#define IPIPE_D2F_1ST_SPR_00				(0x0120)
+#define IPIPE_D2F_1ST_SPR_01				(0x0124)
+#define IPIPE_D2F_1ST_SPR_02				(0x0128)
+#define IPIPE_D2F_1ST_SPR_03				(0x012c)
+#define IPIPE_D2F_1ST_SPR_04				(0x0130)
+#define IPIPE_D2F_1ST_SPR_05				(0x0134)
+#define IPIPE_D2F_1ST_SPR_06				(0x0138)
+#define IPIPE_D2F_1ST_SPR_07				(0x013c)
+#define IPIPE_D2F_1ST_EDG_MIN				(0x0140)
+#define IPIPE_D2F_1ST_EDG_MAX				(0x0144)
+#define IPIPE_D2F_2ND_EN				(0x0148)
+#define IPIPE_D2F_2ND_TYP				(0x014c)
+#define IPIPE_D2F_2ND_THR00				(0x0150)
+#define IPIPE_D2F_2ND_THR01				(0x0154)
+#define IPIPE_D2F_2ND_THR02				(0x0158)
+#define IPIPE_D2F_2ND_THR03				(0x015c)
+#define IPIPE_D2F_2ND_THR04				(0x0160)
+#define IPIPE_D2F_2ND_THR05				(0x0164)
+#define IPIPE_D2F_2ND_THR06				(0x0168)
+#define IPIPE_D2F_2ND_THR07				(0x016c)
+#define IPIPE_D2F_2ND_STR_00				(0x0170)
+#define IPIPE_D2F_2ND_STR_01				(0x0174)
+#define IPIPE_D2F_2ND_STR_02				(0x0178)
+#define IPIPE_D2F_2ND_STR_03				(0x017c)
+#define IPIPE_D2F_2ND_STR_04				(0x0180)
+#define IPIPE_D2F_2ND_STR_05				(0x0184)
+#define IPIPE_D2F_2ND_STR_06				(0x0188)
+#define IPIPE_D2F_2ND_STR_07				(0x018c)
+#define IPIPE_D2F_2ND_SPR_00				(0x0190)
+#define IPIPE_D2F_2ND_SPR_01				(0x0194)
+#define IPIPE_D2F_2ND_SPR_02				(0x0198)
+#define IPIPE_D2F_2ND_SPR_03				(0x019c)
+#define IPIPE_D2F_2ND_SPR_04				(0x01a0)
+#define IPIPE_D2F_2ND_SPR_05				(0x01a4)
+#define IPIPE_D2F_2ND_SPR_06				(0x01a8)
+#define IPIPE_D2F_2ND_SPR_07				(0x01ac)
+#define IPIPE_D2F_2ND_EDG_MIN				(0x01b0)
+#define IPIPE_D2F_2ND_EDG_MAX				(0x01b4)
+
+#define IPIPE_GIC_EN					(0x01b8)
+#define IPIPE_GIC_TYP					(0x01bc)
+#define IPIPE_GIC_GAN					(0x01c0)
+#define IPIPE_GIC_NFGAIN				(0x01c4)
+#define IPIPE_GIC_THR					(0x01c8)
+#define IPIPE_GIC_SLP					(0x01cc)
+
+#define IPIPE_WB2_OFT_R					(0x01d0)
+#define IPIPE_WB2_OFT_GR				(0x01d4)
+#define IPIPE_WB2_OFT_GB				(0x01d8)
+#define IPIPE_WB2_OFT_B					(0x01dc)
+
+#define IPIPE_WB2_WGN_R					(0x01e0)
+#define IPIPE_WB2_WGN_GR				(0x01e4)
+#define IPIPE_WB2_WGN_GB				(0x01e8)
+#define IPIPE_WB2_WGN_B					(0x01ec)
+
+#define IPIPE_CFA_MODE					(0x01f0)
+#define IPIPE_CFA_2DIR_HPF_THR				(0x01f4)
+#define IPIPE_CFA_2DIR_HPF_SLP				(0x01f8)
+#define IPIPE_CFA_2DIR_MIX_THR				(0x01fc)
+#define IPIPE_CFA_2DIR_MIX_SLP				(0x0200)
+#define IPIPE_CFA_2DIR_DIR_TRH				(0x0204)
+#define IPIPE_CFA_2DIR_DIR_SLP				(0x0208)
+#define IPIPE_CFA_2DIR_NDWT				(0x020c)
+#define IPIPE_CFA_MONO_HUE_FRA				(0x0210)
+#define IPIPE_CFA_MONO_EDG_THR				(0x0214)
+#define IPIPE_CFA_MONO_THR_MIN				(0x0218)
+
+#define IPIPE_CFA_MONO_THR_SLP				(0x021c)
+#define IPIPE_CFA_MONO_SLP_MIN				(0x0220)
+#define IPIPE_CFA_MONO_SLP_SLP				(0x0224)
+#define IPIPE_CFA_MONO_LPWT				(0x0228)
+
+#define IPIPE_RGB1_MUL_RR				(0x022c)
+#define IPIPE_RGB1_MUL_GR				(0x0230)
+#define IPIPE_RGB1_MUL_BR				(0x0234)
+#define IPIPE_RGB1_MUL_RG				(0x0238)
+#define IPIPE_RGB1_MUL_GG				(0x023c)
+#define IPIPE_RGB1_MUL_BG				(0x0240)
+#define IPIPE_RGB1_MUL_RB				(0x0244)
+#define IPIPE_RGB1_MUL_GB				(0x0248)
+#define IPIPE_RGB1_MUL_BB				(0x024c)
+#define IPIPE_RGB1_OFT_OR				(0x0250)
+#define IPIPE_RGB1_OFT_OG				(0x0254)
+#define IPIPE_RGB1_OFT_OB				(0x0258)
+#define IPIPE_GMM_CFG					(0x025c)
+#define IPIPE_RGB2_MUL_RR				(0x0260)
+#define IPIPE_RGB2_MUL_GR				(0x0264)
+#define IPIPE_RGB2_MUL_BR				(0x0268)
+#define IPIPE_RGB2_MUL_RG				(0x026c)
+#define IPIPE_RGB2_MUL_GG				(0x0270)
+#define IPIPE_RGB2_MUL_BG				(0x0274)
+#define IPIPE_RGB2_MUL_RB				(0x0278)
+#define IPIPE_RGB2_MUL_GB				(0x027c)
+#define IPIPE_RGB2_MUL_BB				(0x0280)
+#define IPIPE_RGB2_OFT_OR				(0x0284)
+#define IPIPE_RGB2_OFT_OG				(0x0288)
+#define IPIPE_RGB2_OFT_OB				(0x028c)
+
+#define IPIPE_YUV_ADJ					(0x0294)
+#define IPIPE_YUV_MUL_RY				(0x0298)
+#define IPIPE_YUV_MUL_GY				(0x029c)
+#define IPIPE_YUV_MUL_BY				(0x02a0)
+#define IPIPE_YUV_MUL_RCB				(0x02a4)
+#define IPIPE_YUV_MUL_GCB				(0x02a8)
+#define IPIPE_YUV_MUL_BCB				(0x02ac)
+#define IPIPE_YUV_MUL_RCR				(0x02b0)
+#define IPIPE_YUV_MUL_GCR				(0x02b4)
+#define IPIPE_YUV_MUL_BCR				(0x02b8)
+#define IPIPE_YUV_OFT_Y					(0x02bc)
+#define IPIPE_YUV_OFT_CB				(0x02c0)
+#define IPIPE_YUV_OFT_CR				(0x02c4)
+
+#define IPIPE_YUV_PHS					(0x02c8)
+#define IPIPE_YUV_PHS_LPF				(1 << 1)
+#define IPIPE_YUV_PHS_POS				(1 << 0)
+
+#define IPIPE_YEE_EN					(0x02d4)
+#define IPIPE_YEE_TYP					(0x02d8)
+#define IPIPE_YEE_SHF					(0x02dc)
+#define IPIPE_YEE_MUL_00				(0x02e0)
+#define IPIPE_YEE_MUL_01				(0x02e4)
+#define IPIPE_YEE_MUL_02				(0x02e8)
+#define IPIPE_YEE_MUL_10				(0x02ec)
+#define IPIPE_YEE_MUL_11				(0x02f0)
+#define IPIPE_YEE_MUL_12				(0x02f4)
+#define IPIPE_YEE_MUL_20				(0x02f8)
+#define IPIPE_YEE_MUL_21				(0x02fc)
+#define IPIPE_YEE_MUL_22				(0x0300)
+#define IPIPE_YEE_THR					(0x0304)
+#define IPIPE_YEE_E_GAN					(0x0308)
+#define IPIPE_YEE_E_THR_1				(0x030c)
+#define IPIPE_YEE_E_THR_2				(0x0310)
+#define IPIPE_YEE_G_GAN					(0x0314)
+#define IPIPE_YEE_G_OFT					(0x0318)
+
+#define IPIPE_CAR_EN					(0x031c)
+#define IPIPE_CAR_TYP					(0x0320)
+#define IPIPE_CAR_SW					(0x0324)
+#define IPIPE_CAR_HPF_TYP				(0x0328)
+#define IPIPE_CAR_HPF_SHF				(0x032c)
+#define IPIPE_CAR_HPF_THR				(0x0330)
+#define IPIPE_CAR_GN1_GAN				(0x0334)
+#define IPIPE_CAR_GN1_SHF				(0x0338)
+#define IPIPE_CAR_GN1_MIN				(0x033c)
+#define IPIPE_CAR_GN2_GAN				(0x0340)
+#define IPIPE_CAR_GN2_SHF				(0x0344)
+#define IPIPE_CAR_GN2_MIN				(0x0348)
+#define IPIPE_CGS_EN					(0x034c)
+#define IPIPE_CGS_GN1_L_THR				(0x0350)
+#define IPIPE_CGS_GN1_L_GAIN				(0x0354)
+#define IPIPE_CGS_GN1_L_SHF				(0x0358)
+#define IPIPE_CGS_GN1_L_MIN				(0x035c)
+#define IPIPE_CGS_GN1_H_THR				(0x0360)
+#define IPIPE_CGS_GN1_H_GAIN				(0x0364)
+#define IPIPE_CGS_GN1_H_SHF				(0x0368)
+#define IPIPE_CGS_GN1_H_MIN				(0x036c)
+#define IPIPE_CGS_GN2_L_THR				(0x0370)
+#define IPIPE_CGS_GN2_L_GAIN				(0x0374)
+#define IPIPE_CGS_GN2_L_SHF				(0x0378)
+#define IPIPE_CGS_GN2_L_MIN				(0x037c)
+
+#define IPIPE_BOX_EN					(0x0380)
+#define IPIPE_BOX_MODE					(0x0384)
+#define IPIPE_BOX_TYP					(0x0388)
+#define IPIPE_BOX_SHF					(0x038c)
+#define IPIPE_BOX_SDR_SAD_H				(0x0390)
+#define IPIPE_BOX_SDR_SAD_L				(0x0394)
+
+#define IPIPE_HST_EN					(0x039c)
+#define IPIPE_HST_MODE					(0x03a0)
+#define IPIPE_HST_SEL					(0x03a4)
+#define IPIPE_HST_PARA					(0x03a8)
+#define IPIPE_HST_0_VPS					(0x03ac)
+#define IPIPE_HST_0_VSZ					(0x03b0)
+#define IPIPE_HST_0_HPS					(0x03b4)
+#define IPIPE_HST_0_HSZ					(0x03b8)
+#define IPIPE_HST_1_VPS					(0x03bc)
+#define IPIPE_HST_1_VSZ					(0x03c0)
+#define IPIPE_HST_1_HPS					(0x03c4)
+#define IPIPE_HST_1_HSZ					(0x03c8)
+#define IPIPE_HST_2_VPS					(0x03cc)
+#define IPIPE_HST_2_VSZ					(0x03d0)
+#define IPIPE_HST_2_HPS					(0x03d4)
+#define IPIPE_HST_2_HSZ					(0x03d8)
+#define IPIPE_HST_3_VPS					(0x03dc)
+#define IPIPE_HST_3_VSZ					(0x03e0)
+#define IPIPE_HST_3_HPS					(0x03e4)
+#define IPIPE_HST_3_HSZ					(0x03e8)
+#define IPIPE_HST_TBL					(0x03ec)
+#define IPIPE_HST_MUL_R					(0x03f0)
+#define IPIPE_HST_MUL_GR				(0x03f4)
+#define IPIPE_HST_MUL_GB				(0x03f8)
+#define IPIPE_HST_MUL_B					(0x03fc)
+
+#define IPIPE_BSC_EN					(0x0400)
+#define IPIPE_BSC_MODE					(0x0404)
+#define IPIPE_BSC_TYP					(0x0408)
+#define IPIPE_BSC_ROW_VCT				(0x040c)
+#define IPIPE_BSC_ROW_SHF				(0x0410)
+#define IPIPE_BSC_ROW_VPO				(0x0414)
+#define IPIPE_BSC_ROW_VNU				(0x0418)
+#define IPIPE_BSC_ROW_VSKIP				(0x041c)
+#define IPIPE_BSC_ROW_HPO				(0x0420)
+#define IPIPE_BSC_ROW_HNU				(0x0424)
+#define IPIPE_BSC_ROW_HSKIP				(0x0428)
+#define IPIPE_BSC_COL_VCT				(0x042c)
+#define IPIPE_BSC_COL_SHF				(0x0430)
+#define IPIPE_BSC_COL_VPO				(0x0434)
+#define IPIPE_BSC_COL_VNU				(0x0438)
+#define IPIPE_BSC_COL_VSKIP				(0x043c)
+#define IPIPE_BSC_COL_HPO				(0x0440)
+#define IPIPE_BSC_COL_HNU				(0x0444)
+#define IPIPE_BSC_COL_HSKIP				(0x0448)
+
+#define IPIPE_BSC_EN					(0x0400)
+
+/* ISS ISP Resizer register offsets */
+#define RSZ_REVISION					(0x0000)
+#define RSZ_SYSCONFIG					(0x0004)
+#define RSZ_SYSCONFIG_RSZB_CLK_EN			(1 << 9)
+#define RSZ_SYSCONFIG_RSZA_CLK_EN			(1 << 8)
+
+#define RSZ_IN_FIFO_CTRL				(0x000c)
+#define RSZ_IN_FIFO_CTRL_THRLD_LOW_MASK			(0x1ff << 16)
+#define RSZ_IN_FIFO_CTRL_THRLD_LOW_SHIFT		16
+#define RSZ_IN_FIFO_CTRL_THRLD_HIGH_MASK		(0x1ff << 0)
+#define RSZ_IN_FIFO_CTRL_THRLD_HIGH_SHIFT		0
+
+#define RSZ_FRACDIV					(0x0008)
+#define RSZ_FRACDIV_MASK				(0xffff)
+
+#define RSZ_SRC_EN					(0x0020)
+#define RSZ_SRC_EN_SRC_EN				(1 << 0)
+
+#define RSZ_SRC_MODE					(0x0024)
+#define RSZ_SRC_MODE_OST				(1 << 0)
+#define RSZ_SRC_MODE_WRT				(1 << 1)
+
+#define RSZ_SRC_FMT0					(0x0028)
+#define RSZ_SRC_FMT0_BYPASS				(1 << 1)
+#define RSZ_SRC_FMT0_SEL				(1 << 0)
+
+#define RSZ_SRC_FMT1					(0x002c)
+#define RSZ_SRC_FMT1_IN420				(1 << 1)
+
+#define RSZ_SRC_VPS					(0x0030)
+#define RSZ_SRC_VSZ					(0x0034)
+#define RSZ_SRC_HPS					(0x0038)
+#define RSZ_SRC_HSZ					(0x003c)
+#define RSZ_DMA_RZA					(0x0040)
+#define RSZ_DMA_RZB					(0x0044)
+#define RSZ_DMA_STA					(0x0048)
+#define RSZ_GCK_MMR					(0x004c)
+#define RSZ_GCK_MMR_MMR					(1 << 0)
+
+#define RSZ_GCK_SDR					(0x0054)
+#define RSZ_GCK_SDR_CORE				(1 << 0)
+
+#define RSZ_IRQ_RZA					(0x0058)
+#define RSZ_IRQ_RZA_MASK				(0x1fff)
+
+#define RSZ_IRQ_RZB					(0x005c)
+#define RSZ_IRQ_RZB_MASK				(0x1fff)
+
+#define RSZ_YUV_Y_MIN					(0x0060)
+#define RSZ_YUV_Y_MAX					(0x0064)
+#define RSZ_YUV_C_MIN					(0x0068)
+#define RSZ_YUV_C_MAX					(0x006c)
+
+#define RSZ_SEQ						(0x0074)
+#define RSZ_SEQ_HRVB					(1 << 2)
+#define RSZ_SEQ_HRVA					(1 << 0)
+
+#define RZA_EN						(0x0078)
+#define RZA_MODE					(0x007c)
+#define RZA_MODE_ONE_SHOT				(1 << 0)
+
+#define RZA_420						(0x0080)
+#define RZA_I_VPS					(0x0084)
+#define RZA_I_HPS					(0x0088)
+#define RZA_O_VSZ					(0x008c)
+#define RZA_O_HSZ					(0x0090)
+#define RZA_V_PHS_Y					(0x0094)
+#define RZA_V_PHS_C					(0x0098)
+#define RZA_V_DIF					(0x009c)
+#define RZA_V_TYP					(0x00a0)
+#define RZA_V_LPF					(0x00a4)
+#define RZA_H_PHS					(0x00a8)
+#define RZA_H_DIF					(0x00b0)
+#define RZA_H_TYP					(0x00b4)
+#define RZA_H_LPF					(0x00b8)
+#define RZA_DWN_EN					(0x00bc)
+#define RZA_SDR_Y_BAD_H					(0x00d0)
+#define RZA_SDR_Y_BAD_L					(0x00d4)
+#define RZA_SDR_Y_SAD_H					(0x00d8)
+#define RZA_SDR_Y_SAD_L					(0x00dc)
+#define RZA_SDR_Y_OFT					(0x00e0)
+#define RZA_SDR_Y_PTR_S					(0x00e4)
+#define RZA_SDR_Y_PTR_E					(0x00e8)
+#define RZA_SDR_C_BAD_H					(0x00ec)
+#define RZA_SDR_C_BAD_L					(0x00f0)
+#define RZA_SDR_C_SAD_H					(0x00f4)
+#define RZA_SDR_C_SAD_L					(0x00f8)
+#define RZA_SDR_C_OFT					(0x00fc)
+#define RZA_SDR_C_PTR_S					(0x0100)
+#define RZA_SDR_C_PTR_E					(0x0104)
+
+#define RZB_EN						(0x0108)
+#define RZB_MODE					(0x010c)
+#define RZB_420						(0x0110)
+#define RZB_I_VPS					(0x0114)
+#define RZB_I_HPS					(0x0118)
+#define RZB_O_VSZ					(0x011c)
+#define RZB_O_HSZ					(0x0120)
+
+#define RZB_V_DIF					(0x012c)
+#define RZB_V_TYP					(0x0130)
+#define RZB_V_LPF					(0x0134)
+
+#define RZB_H_DIF					(0x0140)
+#define RZB_H_TYP					(0x0144)
+#define RZB_H_LPF					(0x0148)
+
+#define RZB_SDR_Y_BAD_H					(0x0160)
+#define RZB_SDR_Y_BAD_L					(0x0164)
+#define RZB_SDR_Y_SAD_H					(0x0168)
+#define RZB_SDR_Y_SAD_L					(0x016c)
+#define RZB_SDR_Y_OFT					(0x0170)
+#define RZB_SDR_Y_PTR_S					(0x0174)
+#define RZB_SDR_Y_PTR_E					(0x0178)
+#define RZB_SDR_C_BAD_H					(0x017c)
+#define RZB_SDR_C_BAD_L					(0x0180)
+#define RZB_SDR_C_SAD_H					(0x0184)
+#define RZB_SDR_C_SAD_L					(0x0188)
+
+#define RZB_SDR_C_PTR_S					(0x0190)
+#define RZB_SDR_C_PTR_E					(0x0194)
+
+/* Shared Bitmasks between RZA & RZB */
+#define RSZ_EN_EN					(1 << 0)
+
+#define RSZ_420_CEN					(1 << 1)
+#define RSZ_420_YEN					(1 << 0)
+
+#define RSZ_I_VPS_MASK					(0x1fff)
+
+#define RSZ_I_HPS_MASK					(0x1fff)
+
+#define RSZ_O_VSZ_MASK					(0x1fff)
+
+#define RSZ_O_HSZ_MASK					(0x1ffe)
+
+#define RSZ_V_PHS_Y_MASK				(0x3fff)
+
+#define RSZ_V_PHS_C_MASK				(0x3fff)
+
+#define RSZ_V_DIF_MASK					(0x3fff)
+
+#define RSZ_V_TYP_C					(1 << 1)
+#define RSZ_V_TYP_Y					(1 << 0)
+
+#define RSZ_V_LPF_C_MASK				(0x3f << 6)
+#define RSZ_V_LPF_C_SHIFT				6
+#define RSZ_V_LPF_Y_MASK				(0x3f << 0)
+#define RSZ_V_LPF_Y_SHIFT				0
+
+#define RSZ_H_PHS_MASK					(0x3fff)
+
+#define RSZ_H_DIF_MASK					(0x3fff)
+
+#define RSZ_H_TYP_C					(1 << 1)
+#define RSZ_H_TYP_Y					(1 << 0)
+
+#define RSZ_H_LPF_C_MASK				(0x3f << 6)
+#define RSZ_H_LPF_C_SHIFT				6
+#define RSZ_H_LPF_Y_MASK				(0x3f << 0)
+#define RSZ_H_LPF_Y_SHIFT				0
+
+#define RSZ_DWN_EN_DWN_EN				(1 << 0)
+
+#endif /* _OMAP4_ISS_REGS_H_ */
diff --git a/drivers/staging/media/omap4iss/iss_resizer.c b/drivers/staging/media/omap4iss/iss_resizer.c
new file mode 100644
index 000000000000..ae831b8985c9
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss_resizer.c
@@ -0,0 +1,893 @@
+/*
+ * TI OMAP4 ISS V4L2 Driver - ISP RESIZER module
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/uaccess.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+
+#include "iss.h"
+#include "iss_regs.h"
+#include "iss_resizer.h"
+
+static const unsigned int resizer_fmts[] = {
+	V4L2_MBUS_FMT_UYVY8_1X16,
+	V4L2_MBUS_FMT_YUYV8_1X16,
+};
+
+/*
+ * resizer_print_status - Print current RESIZER Module register values.
+ * @resizer: Pointer to ISS ISP RESIZER device.
+ *
+ * Also prints other debug information stored in the RESIZER module.
+ */
+#define RSZ_PRINT_REGISTER(iss, name)\
+	dev_dbg(iss->dev, "###RSZ " #name "=0x%08x\n", \
+		iss_reg_read(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_##name))
+
+#define RZA_PRINT_REGISTER(iss, name)\
+	dev_dbg(iss->dev, "###RZA " #name "=0x%08x\n", \
+		iss_reg_read(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_##name))
+
+static void resizer_print_status(struct iss_resizer_device *resizer)
+{
+	struct iss_device *iss = to_iss_device(resizer);
+
+	dev_dbg(iss->dev, "-------------RESIZER Register dump-------------\n");
+
+	RSZ_PRINT_REGISTER(iss, SYSCONFIG);
+	RSZ_PRINT_REGISTER(iss, IN_FIFO_CTRL);
+	RSZ_PRINT_REGISTER(iss, FRACDIV);
+	RSZ_PRINT_REGISTER(iss, SRC_EN);
+	RSZ_PRINT_REGISTER(iss, SRC_MODE);
+	RSZ_PRINT_REGISTER(iss, SRC_FMT0);
+	RSZ_PRINT_REGISTER(iss, SRC_FMT1);
+	RSZ_PRINT_REGISTER(iss, SRC_VPS);
+	RSZ_PRINT_REGISTER(iss, SRC_VSZ);
+	RSZ_PRINT_REGISTER(iss, SRC_HPS);
+	RSZ_PRINT_REGISTER(iss, SRC_HSZ);
+	RSZ_PRINT_REGISTER(iss, DMA_RZA);
+	RSZ_PRINT_REGISTER(iss, DMA_RZB);
+	RSZ_PRINT_REGISTER(iss, DMA_STA);
+	RSZ_PRINT_REGISTER(iss, GCK_MMR);
+	RSZ_PRINT_REGISTER(iss, GCK_SDR);
+	RSZ_PRINT_REGISTER(iss, IRQ_RZA);
+	RSZ_PRINT_REGISTER(iss, IRQ_RZB);
+	RSZ_PRINT_REGISTER(iss, YUV_Y_MIN);
+	RSZ_PRINT_REGISTER(iss, YUV_Y_MAX);
+	RSZ_PRINT_REGISTER(iss, YUV_C_MIN);
+	RSZ_PRINT_REGISTER(iss, YUV_C_MAX);
+	RSZ_PRINT_REGISTER(iss, SEQ);
+
+	RZA_PRINT_REGISTER(iss, EN);
+	RZA_PRINT_REGISTER(iss, MODE);
+	RZA_PRINT_REGISTER(iss, 420);
+	RZA_PRINT_REGISTER(iss, I_VPS);
+	RZA_PRINT_REGISTER(iss, I_HPS);
+	RZA_PRINT_REGISTER(iss, O_VSZ);
+	RZA_PRINT_REGISTER(iss, O_HSZ);
+	RZA_PRINT_REGISTER(iss, V_PHS_Y);
+	RZA_PRINT_REGISTER(iss, V_PHS_C);
+	RZA_PRINT_REGISTER(iss, V_DIF);
+	RZA_PRINT_REGISTER(iss, V_TYP);
+	RZA_PRINT_REGISTER(iss, V_LPF);
+	RZA_PRINT_REGISTER(iss, H_PHS);
+	RZA_PRINT_REGISTER(iss, H_DIF);
+	RZA_PRINT_REGISTER(iss, H_TYP);
+	RZA_PRINT_REGISTER(iss, H_LPF);
+	RZA_PRINT_REGISTER(iss, DWN_EN);
+	RZA_PRINT_REGISTER(iss, SDR_Y_BAD_H);
+	RZA_PRINT_REGISTER(iss, SDR_Y_BAD_L);
+	RZA_PRINT_REGISTER(iss, SDR_Y_SAD_H);
+	RZA_PRINT_REGISTER(iss, SDR_Y_SAD_L);
+	RZA_PRINT_REGISTER(iss, SDR_Y_OFT);
+	RZA_PRINT_REGISTER(iss, SDR_Y_PTR_S);
+	RZA_PRINT_REGISTER(iss, SDR_Y_PTR_E);
+	RZA_PRINT_REGISTER(iss, SDR_C_BAD_H);
+	RZA_PRINT_REGISTER(iss, SDR_C_BAD_L);
+	RZA_PRINT_REGISTER(iss, SDR_C_SAD_H);
+	RZA_PRINT_REGISTER(iss, SDR_C_SAD_L);
+	RZA_PRINT_REGISTER(iss, SDR_C_OFT);
+	RZA_PRINT_REGISTER(iss, SDR_C_PTR_S);
+	RZA_PRINT_REGISTER(iss, SDR_C_PTR_E);
+
+	dev_dbg(iss->dev, "-----------------------------------------------\n");
+}
+
+/*
+ * resizer_enable - Enable/Disable RESIZER.
+ * @enable: enable flag
+ *
+ */
+static void resizer_enable(struct iss_resizer_device *resizer, u8 enable)
+{
+	struct iss_device *iss = to_iss_device(resizer);
+
+	iss_reg_update(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_SRC_EN,
+		       RSZ_SRC_EN_SRC_EN, enable ? RSZ_SRC_EN_SRC_EN : 0);
+
+	/* TODO: Enable RSZB */
+	iss_reg_update(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_EN, RSZ_EN_EN,
+		       enable ? RSZ_EN_EN : 0);
+}
+
+/* -----------------------------------------------------------------------------
+ * Format- and pipeline-related configuration helpers
+ */
+
+/*
+ * resizer_set_outaddr - Set memory address to save output image
+ * @resizer: Pointer to ISP RESIZER device.
+ * @addr: 32-bit memory address aligned on 32 byte boundary.
+ *
+ * Sets the memory address where the output will be saved.
+ */
+static void resizer_set_outaddr(struct iss_resizer_device *resizer, u32 addr)
+{
+	struct iss_device *iss = to_iss_device(resizer);
+	struct v4l2_mbus_framefmt *informat, *outformat;
+
+	informat = &resizer->formats[RESIZER_PAD_SINK];
+	outformat = &resizer->formats[RESIZER_PAD_SOURCE_MEM];
+
+	/* Save address splitted in Base Address H & L */
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_SDR_Y_BAD_H,
+		      (addr >> 16) & 0xffff);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_SDR_Y_BAD_L,
+		      addr & 0xffff);
+
+	/* SAD = BAD */
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_SDR_Y_SAD_H,
+		      (addr >> 16) & 0xffff);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_SDR_Y_SAD_L,
+		      addr & 0xffff);
+
+	/* Program UV buffer address... Hardcoded to be contiguous! */
+	if ((informat->code == V4L2_MBUS_FMT_UYVY8_1X16) &&
+	    (outformat->code == V4L2_MBUS_FMT_YUYV8_1_5X8)) {
+		u32 c_addr = addr + (resizer->video_out.bpl_value *
+				     (outformat->height - 1));
+
+		/* Ensure Y_BAD_L[6:0] = C_BAD_L[6:0]*/
+		if ((c_addr ^ addr) & 0x7f) {
+			c_addr &= ~0x7f;
+			c_addr += 0x80;
+			c_addr |= addr & 0x7f;
+		}
+
+		/* Save address splitted in Base Address H & L */
+		iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_SDR_C_BAD_H,
+			      (c_addr >> 16) & 0xffff);
+		iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_SDR_C_BAD_L,
+			      c_addr & 0xffff);
+
+		/* SAD = BAD */
+		iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_SDR_C_SAD_H,
+			      (c_addr >> 16) & 0xffff);
+		iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_SDR_C_SAD_L,
+			      c_addr & 0xffff);
+	}
+}
+
+static void resizer_configure(struct iss_resizer_device *resizer)
+{
+	struct iss_device *iss = to_iss_device(resizer);
+	struct v4l2_mbus_framefmt *informat, *outformat;
+
+	informat = &resizer->formats[RESIZER_PAD_SINK];
+	outformat = &resizer->formats[RESIZER_PAD_SOURCE_MEM];
+
+	/* Disable pass-through more. Despite its name, the BYPASS bit controls
+	 * pass-through mode, not bypass mode.
+	 */
+	iss_reg_clr(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_SRC_FMT0,
+		    RSZ_SRC_FMT0_BYPASS);
+
+	/* Select RSZ input */
+	iss_reg_update(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_SRC_FMT0,
+		       RSZ_SRC_FMT0_SEL,
+		       resizer->input == RESIZER_INPUT_IPIPEIF ?
+		       RSZ_SRC_FMT0_SEL : 0);
+
+	/* RSZ ignores WEN signal from IPIPE/IPIPEIF */
+	iss_reg_clr(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_SRC_MODE,
+		    RSZ_SRC_MODE_WRT);
+
+	/* Set Resizer in free-running mode */
+	iss_reg_clr(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_SRC_MODE,
+		    RSZ_SRC_MODE_OST);
+
+	/* Init Resizer A */
+	iss_reg_clr(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_MODE,
+		    RZA_MODE_ONE_SHOT);
+
+	/* Set size related things now */
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_SRC_VPS, 0);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_SRC_HPS, 0);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_SRC_VSZ,
+		      informat->height - 2);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_SRC_HSZ,
+		      informat->width - 1);
+
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_I_VPS, 0);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_I_HPS, 0);
+
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_O_VSZ,
+		      outformat->height - 2);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_O_HSZ,
+		      outformat->width - 1);
+
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_V_DIF, 0x100);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_H_DIF, 0x100);
+
+	/* Buffer output settings */
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_SDR_Y_PTR_S, 0);
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_SDR_Y_PTR_E,
+		      outformat->height - 1);
+
+	iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_SDR_Y_OFT,
+		      resizer->video_out.bpl_value);
+
+	/* UYVY -> NV12 conversion */
+	if ((informat->code == V4L2_MBUS_FMT_UYVY8_1X16) &&
+	    (outformat->code == V4L2_MBUS_FMT_YUYV8_1_5X8)) {
+		iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_420,
+			      RSZ_420_CEN | RSZ_420_YEN);
+
+		/* UV Buffer output settings */
+		iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_SDR_C_PTR_S,
+			      0);
+		iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_SDR_C_PTR_E,
+			      outformat->height - 1);
+
+		iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_SDR_C_OFT,
+			      resizer->video_out.bpl_value);
+	} else {
+		iss_reg_write(iss, OMAP4_ISS_MEM_ISP_RESIZER, RZA_420, 0);
+	}
+}
+
+/* -----------------------------------------------------------------------------
+ * Interrupt handling
+ */
+
+static void resizer_isr_buffer(struct iss_resizer_device *resizer)
+{
+	struct iss_buffer *buffer;
+
+	/* The whole resizer needs to be stopped. Disabling RZA only produces
+	 * input FIFO overflows, most probably when the next frame is received.
+	 */
+	resizer_enable(resizer, 0);
+
+	buffer = omap4iss_video_buffer_next(&resizer->video_out);
+	if (buffer == NULL)
+		return;
+
+	resizer_set_outaddr(resizer, buffer->iss_addr);
+
+	resizer_enable(resizer, 1);
+}
+
+/*
+ * resizer_isif0_isr - Handle ISIF0 event
+ * @resizer: Pointer to ISP RESIZER device.
+ *
+ * Executes LSC deferred enablement before next frame starts.
+ */
+static void resizer_int_dma_isr(struct iss_resizer_device *resizer)
+{
+	struct iss_pipeline *pipe =
+			     to_iss_pipeline(&resizer->subdev.entity);
+	if (pipe->do_propagation)
+		atomic_inc(&pipe->frame_number);
+
+	resizer_isr_buffer(resizer);
+}
+
+/*
+ * omap4iss_resizer_isr - Configure resizer during interframe time.
+ * @resizer: Pointer to ISP RESIZER device.
+ * @events: RESIZER events
+ */
+void omap4iss_resizer_isr(struct iss_resizer_device *resizer, u32 events)
+{
+	struct iss_device *iss = to_iss_device(resizer);
+	struct iss_pipeline *pipe =
+			     to_iss_pipeline(&resizer->subdev.entity);
+
+	if (events & (ISP5_IRQ_RSZ_FIFO_IN_BLK_ERR |
+		      ISP5_IRQ_RSZ_FIFO_OVF)) {
+		dev_dbg(iss->dev, "RSZ Err: FIFO_IN_BLK:%d, FIFO_OVF:%d\n",
+			events & ISP5_IRQ_RSZ_FIFO_IN_BLK_ERR ? 1 : 0,
+			events & ISP5_IRQ_RSZ_FIFO_OVF ? 1 : 0);
+		omap4iss_pipeline_cancel_stream(pipe);
+	}
+
+	if (omap4iss_module_sync_is_stopping(&resizer->wait,
+					     &resizer->stopping))
+		return;
+
+	if (events & ISP5_IRQ_RSZ_INT_DMA)
+		resizer_int_dma_isr(resizer);
+}
+
+/* -----------------------------------------------------------------------------
+ * ISS video operations
+ */
+
+static int resizer_video_queue(struct iss_video *video,
+			       struct iss_buffer *buffer)
+{
+	struct iss_resizer_device *resizer = container_of(video,
+				struct iss_resizer_device, video_out);
+
+	if (!(resizer->output & RESIZER_OUTPUT_MEMORY))
+		return -ENODEV;
+
+	resizer_set_outaddr(resizer, buffer->iss_addr);
+
+	/*
+	 * If streaming was enabled before there was a buffer queued
+	 * or underrun happened in the ISR, the hardware was not enabled
+	 * and DMA queue flag ISS_VIDEO_DMAQUEUE_UNDERRUN is still set.
+	 * Enable it now.
+	 */
+	if (video->dmaqueue_flags & ISS_VIDEO_DMAQUEUE_UNDERRUN) {
+		resizer_enable(resizer, 1);
+		iss_video_dmaqueue_flags_clr(video);
+	}
+
+	return 0;
+}
+
+static const struct iss_video_operations resizer_video_ops = {
+	.queue = resizer_video_queue,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev operations
+ */
+
+/*
+ * resizer_set_stream - Enable/Disable streaming on the RESIZER module
+ * @sd: ISP RESIZER V4L2 subdevice
+ * @enable: Enable/disable stream
+ */
+static int resizer_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct iss_resizer_device *resizer = v4l2_get_subdevdata(sd);
+	struct iss_device *iss = to_iss_device(resizer);
+	struct iss_video *video_out = &resizer->video_out;
+	int ret = 0;
+
+	if (resizer->state == ISS_PIPELINE_STREAM_STOPPED) {
+		if (enable == ISS_PIPELINE_STREAM_STOPPED)
+			return 0;
+
+		omap4iss_isp_subclk_enable(iss, OMAP4_ISS_ISP_SUBCLK_RSZ);
+
+		iss_reg_set(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_GCK_MMR,
+			    RSZ_GCK_MMR_MMR);
+		iss_reg_set(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_GCK_SDR,
+			    RSZ_GCK_SDR_CORE);
+
+		/* FIXME: Enable RSZB also */
+		iss_reg_set(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_SYSCONFIG,
+			    RSZ_SYSCONFIG_RSZA_CLK_EN);
+	}
+
+	switch (enable) {
+	case ISS_PIPELINE_STREAM_CONTINUOUS:
+
+		resizer_configure(resizer);
+		resizer_print_status(resizer);
+
+		/*
+		 * When outputting to memory with no buffer available, let the
+		 * buffer queue handler start the hardware. A DMA queue flag
+		 * ISS_VIDEO_DMAQUEUE_QUEUED will be set as soon as there is
+		 * a buffer available.
+		 */
+		if (resizer->output & RESIZER_OUTPUT_MEMORY &&
+		    !(video_out->dmaqueue_flags & ISS_VIDEO_DMAQUEUE_QUEUED))
+			break;
+
+		atomic_set(&resizer->stopping, 0);
+		resizer_enable(resizer, 1);
+		iss_video_dmaqueue_flags_clr(video_out);
+		break;
+
+	case ISS_PIPELINE_STREAM_STOPPED:
+		if (resizer->state == ISS_PIPELINE_STREAM_STOPPED)
+			return 0;
+		if (omap4iss_module_sync_idle(&sd->entity, &resizer->wait,
+					      &resizer->stopping))
+			ret = -ETIMEDOUT;
+
+		resizer_enable(resizer, 0);
+		iss_reg_clr(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_SYSCONFIG,
+			    RSZ_SYSCONFIG_RSZA_CLK_EN);
+		iss_reg_clr(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_GCK_SDR,
+			    RSZ_GCK_SDR_CORE);
+		iss_reg_clr(iss, OMAP4_ISS_MEM_ISP_RESIZER, RSZ_GCK_MMR,
+			    RSZ_GCK_MMR_MMR);
+		omap4iss_isp_subclk_disable(iss, OMAP4_ISS_ISP_SUBCLK_RSZ);
+		iss_video_dmaqueue_flags_clr(video_out);
+		break;
+	}
+
+	resizer->state = enable;
+	return ret;
+}
+
+static struct v4l2_mbus_framefmt *
+__resizer_get_format(struct iss_resizer_device *resizer,
+		     struct v4l2_subdev_fh *fh, unsigned int pad,
+		     enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(fh, pad);
+	else
+		return &resizer->formats[pad];
+}
+
+/*
+ * resizer_try_format - Try video format on a pad
+ * @resizer: ISS RESIZER device
+ * @fh : V4L2 subdev file handle
+ * @pad: Pad number
+ * @fmt: Format
+ */
+static void
+resizer_try_format(struct iss_resizer_device *resizer,
+		   struct v4l2_subdev_fh *fh, unsigned int pad,
+		   struct v4l2_mbus_framefmt *fmt,
+		   enum v4l2_subdev_format_whence which)
+{
+	enum v4l2_mbus_pixelcode pixelcode;
+	struct v4l2_mbus_framefmt *format;
+	unsigned int width = fmt->width;
+	unsigned int height = fmt->height;
+	unsigned int i;
+
+	switch (pad) {
+	case RESIZER_PAD_SINK:
+		for (i = 0; i < ARRAY_SIZE(resizer_fmts); i++) {
+			if (fmt->code == resizer_fmts[i])
+				break;
+		}
+
+		/* If not found, use UYVY as default */
+		if (i >= ARRAY_SIZE(resizer_fmts))
+			fmt->code = V4L2_MBUS_FMT_UYVY8_1X16;
+
+		/* Clamp the input size. */
+		fmt->width = clamp_t(u32, width, 1, 8192);
+		fmt->height = clamp_t(u32, height, 1, 8192);
+		break;
+
+	case RESIZER_PAD_SOURCE_MEM:
+		pixelcode = fmt->code;
+		format = __resizer_get_format(resizer, fh, RESIZER_PAD_SINK,
+					      which);
+		memcpy(fmt, format, sizeof(*fmt));
+
+		if ((pixelcode == V4L2_MBUS_FMT_YUYV8_1_5X8) &&
+		    (fmt->code == V4L2_MBUS_FMT_UYVY8_1X16))
+			fmt->code = pixelcode;
+
+		/* The data formatter truncates the number of horizontal output
+		 * pixels to a multiple of 16. To avoid clipping data, allow
+		 * callers to request an output size bigger than the input size
+		 * up to the nearest multiple of 16.
+		 */
+		fmt->width = clamp_t(u32, width, 32, (fmt->width + 15) & ~15);
+		fmt->width &= ~15;
+		fmt->height = clamp_t(u32, height, 32, fmt->height);
+		break;
+
+	}
+
+	fmt->colorspace = V4L2_COLORSPACE_JPEG;
+	fmt->field = V4L2_FIELD_NONE;
+}
+
+/*
+ * resizer_enum_mbus_code - Handle pixel format enumeration
+ * @sd     : pointer to v4l2 subdev structure
+ * @fh : V4L2 subdev file handle
+ * @code   : pointer to v4l2_subdev_mbus_code_enum structure
+ * return -EINVAL or zero on success
+ */
+static int resizer_enum_mbus_code(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_fh *fh,
+			       struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct iss_resizer_device *resizer = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	switch (code->pad) {
+	case RESIZER_PAD_SINK:
+		if (code->index >= ARRAY_SIZE(resizer_fmts))
+			return -EINVAL;
+
+		code->code = resizer_fmts[code->index];
+		break;
+
+	case RESIZER_PAD_SOURCE_MEM:
+		format = __resizer_get_format(resizer, fh, RESIZER_PAD_SINK,
+					      V4L2_SUBDEV_FORMAT_TRY);
+
+		if (code->index == 0) {
+			code->code = format->code;
+			break;
+		}
+
+		switch (format->code) {
+		case V4L2_MBUS_FMT_UYVY8_1X16:
+			if (code->index == 1)
+				code->code = V4L2_MBUS_FMT_YUYV8_1_5X8;
+			else
+				return -EINVAL;
+			break;
+		default:
+			if (code->index != 0)
+				return -EINVAL;
+		}
+
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int resizer_enum_frame_size(struct v4l2_subdev *sd,
+				struct v4l2_subdev_fh *fh,
+				struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct iss_resizer_device *resizer = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt format;
+
+	if (fse->index != 0)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = 1;
+	format.height = 1;
+	resizer_try_format(resizer, fh, fse->pad, &format,
+			   V4L2_SUBDEV_FORMAT_TRY);
+	fse->min_width = format.width;
+	fse->min_height = format.height;
+
+	if (format.code != fse->code)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = -1;
+	format.height = -1;
+	resizer_try_format(resizer, fh, fse->pad, &format,
+			   V4L2_SUBDEV_FORMAT_TRY);
+	fse->max_width = format.width;
+	fse->max_height = format.height;
+
+	return 0;
+}
+
+/*
+ * resizer_get_format - Retrieve the video format on a pad
+ * @sd : ISP RESIZER V4L2 subdevice
+ * @fh : V4L2 subdev file handle
+ * @fmt: Format
+ *
+ * Return 0 on success or -EINVAL if the pad is invalid or doesn't correspond
+ * to the format type.
+ */
+static int resizer_get_format(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct iss_resizer_device *resizer = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __resizer_get_format(resizer, fh, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	fmt->format = *format;
+	return 0;
+}
+
+/*
+ * resizer_set_format - Set the video format on a pad
+ * @sd : ISP RESIZER V4L2 subdevice
+ * @fh : V4L2 subdev file handle
+ * @fmt: Format
+ *
+ * Return 0 on success or -EINVAL if the pad is invalid or doesn't correspond
+ * to the format type.
+ */
+static int resizer_set_format(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
+			      struct v4l2_subdev_format *fmt)
+{
+	struct iss_resizer_device *resizer = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __resizer_get_format(resizer, fh, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	resizer_try_format(resizer, fh, fmt->pad, &fmt->format, fmt->which);
+	*format = fmt->format;
+
+	/* Propagate the format from sink to source */
+	if (fmt->pad == RESIZER_PAD_SINK) {
+		format = __resizer_get_format(resizer, fh,
+					      RESIZER_PAD_SOURCE_MEM,
+					      fmt->which);
+		*format = fmt->format;
+		resizer_try_format(resizer, fh, RESIZER_PAD_SOURCE_MEM, format,
+				fmt->which);
+	}
+
+	return 0;
+}
+
+static int resizer_link_validate(struct v4l2_subdev *sd,
+				 struct media_link *link,
+				 struct v4l2_subdev_format *source_fmt,
+				 struct v4l2_subdev_format *sink_fmt)
+{
+	/* Check if the two ends match */
+	if (source_fmt->format.width != sink_fmt->format.width ||
+	    source_fmt->format.height != sink_fmt->format.height)
+		return -EPIPE;
+
+	if (source_fmt->format.code != sink_fmt->format.code)
+		return -EPIPE;
+
+	return 0;
+}
+
+/*
+ * resizer_init_formats - Initialize formats on all pads
+ * @sd: ISP RESIZER V4L2 subdevice
+ * @fh: V4L2 subdev file handle
+ *
+ * Initialize all pad formats with default values. If fh is not NULL, try
+ * formats are initialized on the file handle. Otherwise active formats are
+ * initialized on the device.
+ */
+static int resizer_init_formats(struct v4l2_subdev *sd,
+				struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_subdev_format format;
+
+	memset(&format, 0, sizeof(format));
+	format.pad = RESIZER_PAD_SINK;
+	format.which = fh ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+	format.format.code = V4L2_MBUS_FMT_UYVY8_1X16;
+	format.format.width = 4096;
+	format.format.height = 4096;
+	resizer_set_format(sd, fh, &format);
+
+	return 0;
+}
+
+/* V4L2 subdev video operations */
+static const struct v4l2_subdev_video_ops resizer_v4l2_video_ops = {
+	.s_stream = resizer_set_stream,
+};
+
+/* V4L2 subdev pad operations */
+static const struct v4l2_subdev_pad_ops resizer_v4l2_pad_ops = {
+	.enum_mbus_code = resizer_enum_mbus_code,
+	.enum_frame_size = resizer_enum_frame_size,
+	.get_fmt = resizer_get_format,
+	.set_fmt = resizer_set_format,
+	.link_validate = resizer_link_validate,
+};
+
+/* V4L2 subdev operations */
+static const struct v4l2_subdev_ops resizer_v4l2_ops = {
+	.video = &resizer_v4l2_video_ops,
+	.pad = &resizer_v4l2_pad_ops,
+};
+
+/* V4L2 subdev internal operations */
+static const struct v4l2_subdev_internal_ops resizer_v4l2_internal_ops = {
+	.open = resizer_init_formats,
+};
+
+/* -----------------------------------------------------------------------------
+ * Media entity operations
+ */
+
+/*
+ * resizer_link_setup - Setup RESIZER connections
+ * @entity: RESIZER media entity
+ * @local: Pad at the local end of the link
+ * @remote: Pad at the remote end of the link
+ * @flags: Link flags
+ *
+ * return -EINVAL or zero on success
+ */
+static int resizer_link_setup(struct media_entity *entity,
+			   const struct media_pad *local,
+			   const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct iss_resizer_device *resizer = v4l2_get_subdevdata(sd);
+	struct iss_device *iss = to_iss_device(resizer);
+
+	switch (local->index | media_entity_type(remote->entity)) {
+	case RESIZER_PAD_SINK | MEDIA_ENT_T_V4L2_SUBDEV:
+		/* Read from IPIPE or IPIPEIF. */
+		if (!(flags & MEDIA_LNK_FL_ENABLED)) {
+			resizer->input = RESIZER_INPUT_NONE;
+			break;
+		}
+
+		if (resizer->input != RESIZER_INPUT_NONE)
+			return -EBUSY;
+
+		if (remote->entity == &iss->ipipeif.subdev.entity)
+			resizer->input = RESIZER_INPUT_IPIPEIF;
+		else if (remote->entity == &iss->ipipe.subdev.entity)
+			resizer->input = RESIZER_INPUT_IPIPE;
+
+
+		break;
+
+	case RESIZER_PAD_SOURCE_MEM | MEDIA_ENT_T_DEVNODE:
+		/* Write to memory */
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (resizer->output & ~RESIZER_OUTPUT_MEMORY)
+				return -EBUSY;
+			resizer->output |= RESIZER_OUTPUT_MEMORY;
+		} else {
+			resizer->output &= ~RESIZER_OUTPUT_MEMORY;
+		}
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* media operations */
+static const struct media_entity_operations resizer_media_ops = {
+	.link_setup = resizer_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+/*
+ * resizer_init_entities - Initialize V4L2 subdev and media entity
+ * @resizer: ISS ISP RESIZER module
+ *
+ * Return 0 on success and a negative error code on failure.
+ */
+static int resizer_init_entities(struct iss_resizer_device *resizer)
+{
+	struct v4l2_subdev *sd = &resizer->subdev;
+	struct media_pad *pads = resizer->pads;
+	struct media_entity *me = &sd->entity;
+	int ret;
+
+	resizer->input = RESIZER_INPUT_NONE;
+
+	v4l2_subdev_init(sd, &resizer_v4l2_ops);
+	sd->internal_ops = &resizer_v4l2_internal_ops;
+	strlcpy(sd->name, "OMAP4 ISS ISP resizer", sizeof(sd->name));
+	sd->grp_id = 1 << 16;	/* group ID for iss subdevs */
+	v4l2_set_subdevdata(sd, resizer);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	pads[RESIZER_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	pads[RESIZER_PAD_SOURCE_MEM].flags = MEDIA_PAD_FL_SOURCE;
+
+	me->ops = &resizer_media_ops;
+	ret = media_entity_init(me, RESIZER_PADS_NUM, pads, 0);
+	if (ret < 0)
+		return ret;
+
+	resizer_init_formats(sd, NULL);
+
+	resizer->video_out.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	resizer->video_out.ops = &resizer_video_ops;
+	resizer->video_out.iss = to_iss_device(resizer);
+	resizer->video_out.capture_mem = PAGE_ALIGN(4096 * 4096) * 3;
+	resizer->video_out.bpl_alignment = 32;
+	resizer->video_out.bpl_zero_padding = 1;
+	resizer->video_out.bpl_max = 0x1ffe0;
+
+	ret = omap4iss_video_init(&resizer->video_out, "ISP resizer a");
+	if (ret < 0)
+		return ret;
+
+	/* Connect the RESIZER subdev to the video node. */
+	ret = media_entity_create_link(&resizer->subdev.entity,
+				       RESIZER_PAD_SOURCE_MEM,
+				       &resizer->video_out.video.entity, 0, 0);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+void omap4iss_resizer_unregister_entities(struct iss_resizer_device *resizer)
+{
+	media_entity_cleanup(&resizer->subdev.entity);
+
+	v4l2_device_unregister_subdev(&resizer->subdev);
+	omap4iss_video_unregister(&resizer->video_out);
+}
+
+int omap4iss_resizer_register_entities(struct iss_resizer_device *resizer,
+	struct v4l2_device *vdev)
+{
+	int ret;
+
+	/* Register the subdev and video node. */
+	ret = v4l2_device_register_subdev(vdev, &resizer->subdev);
+	if (ret < 0)
+		goto error;
+
+	ret = omap4iss_video_register(&resizer->video_out, vdev);
+	if (ret < 0)
+		goto error;
+
+	return 0;
+
+error:
+	omap4iss_resizer_unregister_entities(resizer);
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * ISP RESIZER initialisation and cleanup
+ */
+
+/*
+ * omap4iss_resizer_init - RESIZER module initialization.
+ * @iss: Device pointer specific to the OMAP4 ISS.
+ *
+ * TODO: Get the initialisation values from platform data.
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+int omap4iss_resizer_init(struct iss_device *iss)
+{
+	struct iss_resizer_device *resizer = &iss->resizer;
+
+	resizer->state = ISS_PIPELINE_STREAM_STOPPED;
+	init_waitqueue_head(&resizer->wait);
+
+	return resizer_init_entities(resizer);
+}
+
+/*
+ * omap4iss_resizer_cleanup - RESIZER module cleanup.
+ * @iss: Device pointer specific to the OMAP4 ISS.
+ */
+void omap4iss_resizer_cleanup(struct iss_device *iss)
+{
+	/* FIXME: are you sure there's nothing to do? */
+}
diff --git a/drivers/staging/media/omap4iss/iss_resizer.h b/drivers/staging/media/omap4iss/iss_resizer.h
new file mode 100644
index 000000000000..3727498b06a3
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss_resizer.h
@@ -0,0 +1,75 @@
+/*
+ * TI OMAP4 ISS V4L2 Driver - ISP RESIZER module
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@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.
+ */
+
+#ifndef OMAP4_ISS_RESIZER_H
+#define OMAP4_ISS_RESIZER_H
+
+#include "iss_video.h"
+
+enum resizer_input_entity {
+	RESIZER_INPUT_NONE,
+	RESIZER_INPUT_IPIPE,
+	RESIZER_INPUT_IPIPEIF
+};
+
+#define RESIZER_OUTPUT_MEMORY		(1 << 0)
+
+/* Sink and source RESIZER pads */
+#define RESIZER_PAD_SINK			0
+#define RESIZER_PAD_SOURCE_MEM			1
+#define RESIZER_PADS_NUM			2
+
+/*
+ * struct iss_resizer_device - Structure for the RESIZER module to store its own
+ *			    information
+ * @subdev: V4L2 subdevice
+ * @pads: Sink and source media entity pads
+ * @formats: Active video formats
+ * @input: Active input
+ * @output: Active outputs
+ * @video_out: Output video node
+ * @error: A hardware error occurred during capture
+ * @state: Streaming state
+ * @wait: Wait queue used to stop the module
+ * @stopping: Stopping state
+ */
+struct iss_resizer_device {
+	struct v4l2_subdev subdev;
+	struct media_pad pads[RESIZER_PADS_NUM];
+	struct v4l2_mbus_framefmt formats[RESIZER_PADS_NUM];
+
+	enum resizer_input_entity input;
+	unsigned int output;
+	struct iss_video video_out;
+	unsigned int error;
+
+	enum iss_pipeline_stream_state state;
+	wait_queue_head_t wait;
+	atomic_t stopping;
+};
+
+struct iss_device;
+
+int omap4iss_resizer_init(struct iss_device *iss);
+void omap4iss_resizer_cleanup(struct iss_device *iss);
+int omap4iss_resizer_register_entities(struct iss_resizer_device *resizer,
+	struct v4l2_device *vdev);
+void omap4iss_resizer_unregister_entities(struct iss_resizer_device *resizer);
+
+int omap4iss_resizer_busy(struct iss_resizer_device *resizer);
+void omap4iss_resizer_isr(struct iss_resizer_device *resizer, u32 events);
+void omap4iss_resizer_restore_context(struct iss_device *iss);
+void omap4iss_resizer_max_rate(struct iss_resizer_device *resizer,
+	unsigned int *max_rate);
+
+#endif	/* OMAP4_ISS_RESIZER_H */
diff --git a/drivers/staging/media/omap4iss/iss_video.c b/drivers/staging/media/omap4iss/iss_video.c
new file mode 100644
index 000000000000..8c7f35029cd5
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss_video.c
@@ -0,0 +1,1226 @@
+/*
+ * TI OMAP4 ISS V4L2 Driver - Generic video node
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <asm/cacheflush.h>
+#include <linux/clk.h>
+#include <linux/mm.h>
+#include <linux/pagemap.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+#include <linux/module.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-ioctl.h>
+
+#include "iss_video.h"
+#include "iss.h"
+
+
+/* -----------------------------------------------------------------------------
+ * Helper functions
+ */
+
+static struct iss_format_info formats[] = {
+	{ V4L2_MBUS_FMT_Y8_1X8, V4L2_MBUS_FMT_Y8_1X8,
+	  V4L2_MBUS_FMT_Y8_1X8, V4L2_MBUS_FMT_Y8_1X8,
+	  V4L2_PIX_FMT_GREY, 8, "Greyscale 8 bpp", },
+	{ V4L2_MBUS_FMT_Y10_1X10, V4L2_MBUS_FMT_Y10_1X10,
+	  V4L2_MBUS_FMT_Y10_1X10, V4L2_MBUS_FMT_Y8_1X8,
+	  V4L2_PIX_FMT_Y10, 10, "Greyscale 10 bpp", },
+	{ V4L2_MBUS_FMT_Y12_1X12, V4L2_MBUS_FMT_Y10_1X10,
+	  V4L2_MBUS_FMT_Y12_1X12, V4L2_MBUS_FMT_Y8_1X8,
+	  V4L2_PIX_FMT_Y12, 12, "Greyscale 12 bpp", },
+	{ V4L2_MBUS_FMT_SBGGR8_1X8, V4L2_MBUS_FMT_SBGGR8_1X8,
+	  V4L2_MBUS_FMT_SBGGR8_1X8, V4L2_MBUS_FMT_SBGGR8_1X8,
+	  V4L2_PIX_FMT_SBGGR8, 8, "BGGR Bayer 8 bpp", },
+	{ V4L2_MBUS_FMT_SGBRG8_1X8, V4L2_MBUS_FMT_SGBRG8_1X8,
+	  V4L2_MBUS_FMT_SGBRG8_1X8, V4L2_MBUS_FMT_SGBRG8_1X8,
+	  V4L2_PIX_FMT_SGBRG8, 8, "GBRG Bayer 8 bpp", },
+	{ V4L2_MBUS_FMT_SGRBG8_1X8, V4L2_MBUS_FMT_SGRBG8_1X8,
+	  V4L2_MBUS_FMT_SGRBG8_1X8, V4L2_MBUS_FMT_SGRBG8_1X8,
+	  V4L2_PIX_FMT_SGRBG8, 8, "GRBG Bayer 8 bpp", },
+	{ V4L2_MBUS_FMT_SRGGB8_1X8, V4L2_MBUS_FMT_SRGGB8_1X8,
+	  V4L2_MBUS_FMT_SRGGB8_1X8, V4L2_MBUS_FMT_SRGGB8_1X8,
+	  V4L2_PIX_FMT_SRGGB8, 8, "RGGB Bayer 8 bpp", },
+	{ V4L2_MBUS_FMT_SGRBG10_DPCM8_1X8, V4L2_MBUS_FMT_SGRBG10_DPCM8_1X8,
+	  V4L2_MBUS_FMT_SGRBG10_1X10, 0,
+	  V4L2_PIX_FMT_SGRBG10DPCM8, 8, "GRBG Bayer 10 bpp DPCM8",  },
+	{ V4L2_MBUS_FMT_SBGGR10_1X10, V4L2_MBUS_FMT_SBGGR10_1X10,
+	  V4L2_MBUS_FMT_SBGGR10_1X10, V4L2_MBUS_FMT_SBGGR8_1X8,
+	  V4L2_PIX_FMT_SBGGR10, 10, "BGGR Bayer 10 bpp", },
+	{ V4L2_MBUS_FMT_SGBRG10_1X10, V4L2_MBUS_FMT_SGBRG10_1X10,
+	  V4L2_MBUS_FMT_SGBRG10_1X10, V4L2_MBUS_FMT_SGBRG8_1X8,
+	  V4L2_PIX_FMT_SGBRG10, 10, "GBRG Bayer 10 bpp", },
+	{ V4L2_MBUS_FMT_SGRBG10_1X10, V4L2_MBUS_FMT_SGRBG10_1X10,
+	  V4L2_MBUS_FMT_SGRBG10_1X10, V4L2_MBUS_FMT_SGRBG8_1X8,
+	  V4L2_PIX_FMT_SGRBG10, 10, "GRBG Bayer 10 bpp", },
+	{ V4L2_MBUS_FMT_SRGGB10_1X10, V4L2_MBUS_FMT_SRGGB10_1X10,
+	  V4L2_MBUS_FMT_SRGGB10_1X10, V4L2_MBUS_FMT_SRGGB8_1X8,
+	  V4L2_PIX_FMT_SRGGB10, 10, "RGGB Bayer 10 bpp", },
+	{ V4L2_MBUS_FMT_SBGGR12_1X12, V4L2_MBUS_FMT_SBGGR10_1X10,
+	  V4L2_MBUS_FMT_SBGGR12_1X12, V4L2_MBUS_FMT_SBGGR8_1X8,
+	  V4L2_PIX_FMT_SBGGR12, 12, "BGGR Bayer 12 bpp", },
+	{ V4L2_MBUS_FMT_SGBRG12_1X12, V4L2_MBUS_FMT_SGBRG10_1X10,
+	  V4L2_MBUS_FMT_SGBRG12_1X12, V4L2_MBUS_FMT_SGBRG8_1X8,
+	  V4L2_PIX_FMT_SGBRG12, 12, "GBRG Bayer 12 bpp", },
+	{ V4L2_MBUS_FMT_SGRBG12_1X12, V4L2_MBUS_FMT_SGRBG10_1X10,
+	  V4L2_MBUS_FMT_SGRBG12_1X12, V4L2_MBUS_FMT_SGRBG8_1X8,
+	  V4L2_PIX_FMT_SGRBG12, 12, "GRBG Bayer 12 bpp", },
+	{ V4L2_MBUS_FMT_SRGGB12_1X12, V4L2_MBUS_FMT_SRGGB10_1X10,
+	  V4L2_MBUS_FMT_SRGGB12_1X12, V4L2_MBUS_FMT_SRGGB8_1X8,
+	  V4L2_PIX_FMT_SRGGB12, 12, "RGGB Bayer 12 bpp", },
+	{ V4L2_MBUS_FMT_UYVY8_1X16, V4L2_MBUS_FMT_UYVY8_1X16,
+	  V4L2_MBUS_FMT_UYVY8_1X16, 0,
+	  V4L2_PIX_FMT_UYVY, 16, "YUV 4:2:2 (UYVY)", },
+	{ V4L2_MBUS_FMT_YUYV8_1X16, V4L2_MBUS_FMT_YUYV8_1X16,
+	  V4L2_MBUS_FMT_YUYV8_1X16, 0,
+	  V4L2_PIX_FMT_YUYV, 16, "YUV 4:2:2 (YUYV)", },
+	{ V4L2_MBUS_FMT_YUYV8_1_5X8, V4L2_MBUS_FMT_YUYV8_1_5X8,
+	  V4L2_MBUS_FMT_YUYV8_1_5X8, 0,
+	  V4L2_PIX_FMT_NV12, 8, "YUV 4:2:0 (NV12)", },
+};
+
+const struct iss_format_info *
+omap4iss_video_format_info(enum v4l2_mbus_pixelcode code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(formats); ++i) {
+		if (formats[i].code == code)
+			return &formats[i];
+	}
+
+	return NULL;
+}
+
+/*
+ * iss_video_mbus_to_pix - Convert v4l2_mbus_framefmt to v4l2_pix_format
+ * @video: ISS video instance
+ * @mbus: v4l2_mbus_framefmt format (input)
+ * @pix: v4l2_pix_format format (output)
+ *
+ * Fill the output pix structure with information from the input mbus format.
+ * The bytesperline and sizeimage fields are computed from the requested bytes
+ * per line value in the pix format and information from the video instance.
+ *
+ * Return the number of padding bytes at end of line.
+ */
+static unsigned int iss_video_mbus_to_pix(const struct iss_video *video,
+					  const struct v4l2_mbus_framefmt *mbus,
+					  struct v4l2_pix_format *pix)
+{
+	unsigned int bpl = pix->bytesperline;
+	unsigned int min_bpl;
+	unsigned int i;
+
+	memset(pix, 0, sizeof(*pix));
+	pix->width = mbus->width;
+	pix->height = mbus->height;
+
+	/* Skip the last format in the loop so that it will be selected if no
+	 * match is found.
+	 */
+	for (i = 0; i < ARRAY_SIZE(formats) - 1; ++i) {
+		if (formats[i].code == mbus->code)
+			break;
+	}
+
+	min_bpl = pix->width * ALIGN(formats[i].bpp, 8) / 8;
+
+	/* Clamp the requested bytes per line value. If the maximum bytes per
+	 * line value is zero, the module doesn't support user configurable line
+	 * sizes. Override the requested value with the minimum in that case.
+	 */
+	if (video->bpl_max)
+		bpl = clamp(bpl, min_bpl, video->bpl_max);
+	else
+		bpl = min_bpl;
+
+	if (!video->bpl_zero_padding || bpl != min_bpl)
+		bpl = ALIGN(bpl, video->bpl_alignment);
+
+	pix->pixelformat = formats[i].pixelformat;
+	pix->bytesperline = bpl;
+	pix->sizeimage = pix->bytesperline * pix->height;
+	pix->colorspace = mbus->colorspace;
+	pix->field = mbus->field;
+
+	/* FIXME: Special case for NV12! We should make this nicer... */
+	if (pix->pixelformat == V4L2_PIX_FMT_NV12)
+		pix->sizeimage += (pix->bytesperline * pix->height) / 2;
+
+	return bpl - min_bpl;
+}
+
+static void iss_video_pix_to_mbus(const struct v4l2_pix_format *pix,
+				  struct v4l2_mbus_framefmt *mbus)
+{
+	unsigned int i;
+
+	memset(mbus, 0, sizeof(*mbus));
+	mbus->width = pix->width;
+	mbus->height = pix->height;
+
+	for (i = 0; i < ARRAY_SIZE(formats); ++i) {
+		if (formats[i].pixelformat == pix->pixelformat)
+			break;
+	}
+
+	if (WARN_ON(i == ARRAY_SIZE(formats)))
+		return;
+
+	mbus->code = formats[i].code;
+	mbus->colorspace = pix->colorspace;
+	mbus->field = pix->field;
+}
+
+static struct v4l2_subdev *
+iss_video_remote_subdev(struct iss_video *video, u32 *pad)
+{
+	struct media_pad *remote;
+
+	remote = media_entity_remote_pad(&video->pad);
+
+	if (remote == NULL ||
+	    media_entity_type(remote->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+		return NULL;
+
+	if (pad)
+		*pad = remote->index;
+
+	return media_entity_to_v4l2_subdev(remote->entity);
+}
+
+/* Return a pointer to the ISS video instance at the far end of the pipeline. */
+static struct iss_video *
+iss_video_far_end(struct iss_video *video)
+{
+	struct media_entity_graph graph;
+	struct media_entity *entity = &video->video.entity;
+	struct media_device *mdev = entity->parent;
+	struct iss_video *far_end = NULL;
+
+	mutex_lock(&mdev->graph_mutex);
+	media_entity_graph_walk_start(&graph, entity);
+
+	while ((entity = media_entity_graph_walk_next(&graph))) {
+		if (entity == &video->video.entity)
+			continue;
+
+		if (media_entity_type(entity) != MEDIA_ENT_T_DEVNODE)
+			continue;
+
+		far_end = to_iss_video(media_entity_to_video_device(entity));
+		if (far_end->type != video->type)
+			break;
+
+		far_end = NULL;
+	}
+
+	mutex_unlock(&mdev->graph_mutex);
+	return far_end;
+}
+
+static int
+__iss_video_get_format(struct iss_video *video,
+		       struct v4l2_mbus_framefmt *format)
+{
+	struct v4l2_subdev_format fmt;
+	struct v4l2_subdev *subdev;
+	u32 pad;
+	int ret;
+
+	subdev = iss_video_remote_subdev(video, &pad);
+	if (subdev == NULL)
+		return -EINVAL;
+
+	memset(&fmt, 0, sizeof(fmt));
+	fmt.pad = pad;
+	fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+
+	mutex_lock(&video->mutex);
+	ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
+	mutex_unlock(&video->mutex);
+
+	if (ret)
+		return ret;
+
+	*format = fmt.format;
+	return 0;
+}
+
+static int
+iss_video_check_format(struct iss_video *video, struct iss_video_fh *vfh)
+{
+	struct v4l2_mbus_framefmt format;
+	struct v4l2_pix_format pixfmt;
+	int ret;
+
+	ret = __iss_video_get_format(video, &format);
+	if (ret < 0)
+		return ret;
+
+	pixfmt.bytesperline = 0;
+	ret = iss_video_mbus_to_pix(video, &format, &pixfmt);
+
+	if (vfh->format.fmt.pix.pixelformat != pixfmt.pixelformat ||
+	    vfh->format.fmt.pix.height != pixfmt.height ||
+	    vfh->format.fmt.pix.width != pixfmt.width ||
+	    vfh->format.fmt.pix.bytesperline != pixfmt.bytesperline ||
+	    vfh->format.fmt.pix.sizeimage != pixfmt.sizeimage)
+		return -EINVAL;
+
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * Video queue operations
+ */
+
+static int iss_video_queue_setup(struct vb2_queue *vq,
+				 const struct v4l2_format *fmt,
+				 unsigned int *count, unsigned int *num_planes,
+				 unsigned int sizes[], void *alloc_ctxs[])
+{
+	struct iss_video_fh *vfh = vb2_get_drv_priv(vq);
+	struct iss_video *video = vfh->video;
+
+	/* Revisit multi-planar support for NV12 */
+	*num_planes = 1;
+
+	sizes[0] = vfh->format.fmt.pix.sizeimage;
+	if (sizes[0] == 0)
+		return -EINVAL;
+
+	alloc_ctxs[0] = video->alloc_ctx;
+
+	*count = min(*count, video->capture_mem / PAGE_ALIGN(sizes[0]));
+
+	return 0;
+}
+
+static void iss_video_buf_cleanup(struct vb2_buffer *vb)
+{
+	struct iss_buffer *buffer = container_of(vb, struct iss_buffer, vb);
+
+	if (buffer->iss_addr)
+		buffer->iss_addr = 0;
+}
+
+static int iss_video_buf_prepare(struct vb2_buffer *vb)
+{
+	struct iss_video_fh *vfh = vb2_get_drv_priv(vb->vb2_queue);
+	struct iss_buffer *buffer = container_of(vb, struct iss_buffer, vb);
+	struct iss_video *video = vfh->video;
+	unsigned long size = vfh->format.fmt.pix.sizeimage;
+	dma_addr_t addr;
+
+	if (vb2_plane_size(vb, 0) < size)
+		return -ENOBUFS;
+
+	/* Refuse to prepare the buffer is the video node has registered an
+	 * error. We don't need to take any lock here as the operation is
+	 * inherently racy. The authoritative check will be performed in the
+	 * queue handler, which can't return an error, this check is just a best
+	 * effort to notify userspace as early as possible.
+	 */
+	if (unlikely(video->error))
+		return -EIO;
+
+	addr = vb2_dma_contig_plane_dma_addr(vb, 0);
+	if (!IS_ALIGNED(addr, 32)) {
+		dev_dbg(video->iss->dev,
+			"Buffer address must be aligned to 32 bytes boundary.\n");
+		return -EINVAL;
+	}
+
+	vb2_set_plane_payload(vb, 0, size);
+	buffer->iss_addr = addr;
+	return 0;
+}
+
+static void iss_video_buf_queue(struct vb2_buffer *vb)
+{
+	struct iss_video_fh *vfh = vb2_get_drv_priv(vb->vb2_queue);
+	struct iss_video *video = vfh->video;
+	struct iss_buffer *buffer = container_of(vb, struct iss_buffer, vb);
+	struct iss_pipeline *pipe = to_iss_pipeline(&video->video.entity);
+	unsigned long flags;
+	bool empty;
+
+	spin_lock_irqsave(&video->qlock, flags);
+
+	if (unlikely(video->error)) {
+		vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
+		spin_unlock_irqrestore(&video->qlock, flags);
+		return;
+	}
+
+	empty = list_empty(&video->dmaqueue);
+	list_add_tail(&buffer->list, &video->dmaqueue);
+
+	spin_unlock_irqrestore(&video->qlock, flags);
+
+	if (empty) {
+		enum iss_pipeline_state state;
+		unsigned int start;
+
+		if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+			state = ISS_PIPELINE_QUEUE_OUTPUT;
+		else
+			state = ISS_PIPELINE_QUEUE_INPUT;
+
+		spin_lock_irqsave(&pipe->lock, flags);
+		pipe->state |= state;
+		video->ops->queue(video, buffer);
+		video->dmaqueue_flags |= ISS_VIDEO_DMAQUEUE_QUEUED;
+
+		start = iss_pipeline_ready(pipe);
+		if (start)
+			pipe->state |= ISS_PIPELINE_STREAM;
+		spin_unlock_irqrestore(&pipe->lock, flags);
+
+		if (start)
+			omap4iss_pipeline_set_stream(pipe,
+						ISS_PIPELINE_STREAM_SINGLESHOT);
+	}
+}
+
+static struct vb2_ops iss_video_vb2ops = {
+	.queue_setup	= iss_video_queue_setup,
+	.buf_prepare	= iss_video_buf_prepare,
+	.buf_queue	= iss_video_buf_queue,
+	.buf_cleanup	= iss_video_buf_cleanup,
+};
+
+/*
+ * omap4iss_video_buffer_next - Complete the current buffer and return the next
+ * @video: ISS video object
+ *
+ * Remove the current video buffer from the DMA queue and fill its timestamp,
+ * field count and state fields before waking up its completion handler.
+ *
+ * For capture video nodes, the buffer state is set to VB2_BUF_STATE_DONE if no
+ * error has been flagged in the pipeline, or to VB2_BUF_STATE_ERROR otherwise.
+ *
+ * The DMA queue is expected to contain at least one buffer.
+ *
+ * Return a pointer to the next buffer in the DMA queue, or NULL if the queue is
+ * empty.
+ */
+struct iss_buffer *omap4iss_video_buffer_next(struct iss_video *video)
+{
+	struct iss_pipeline *pipe = to_iss_pipeline(&video->video.entity);
+	enum iss_pipeline_state state;
+	struct iss_buffer *buf;
+	unsigned long flags;
+	struct timespec ts;
+
+	spin_lock_irqsave(&video->qlock, flags);
+	if (WARN_ON(list_empty(&video->dmaqueue))) {
+		spin_unlock_irqrestore(&video->qlock, flags);
+		return NULL;
+	}
+
+	buf = list_first_entry(&video->dmaqueue, struct iss_buffer,
+			       list);
+	list_del(&buf->list);
+	spin_unlock_irqrestore(&video->qlock, flags);
+
+	ktime_get_ts(&ts);
+	buf->vb.v4l2_buf.timestamp.tv_sec = ts.tv_sec;
+	buf->vb.v4l2_buf.timestamp.tv_usec = ts.tv_nsec / NSEC_PER_USEC;
+
+	/* Do frame number propagation only if this is the output video node.
+	 * Frame number either comes from the CSI receivers or it gets
+	 * incremented here if H3A is not active.
+	 * Note: There is no guarantee that the output buffer will finish
+	 * first, so the input number might lag behind by 1 in some cases.
+	 */
+	if (video == pipe->output && !pipe->do_propagation)
+		buf->vb.v4l2_buf.sequence =
+			atomic_inc_return(&pipe->frame_number);
+	else
+		buf->vb.v4l2_buf.sequence = atomic_read(&pipe->frame_number);
+
+	vb2_buffer_done(&buf->vb, pipe->error ?
+			VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+	pipe->error = false;
+
+	spin_lock_irqsave(&video->qlock, flags);
+	if (list_empty(&video->dmaqueue)) {
+		spin_unlock_irqrestore(&video->qlock, flags);
+		if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+			state = ISS_PIPELINE_QUEUE_OUTPUT
+			      | ISS_PIPELINE_STREAM;
+		else
+			state = ISS_PIPELINE_QUEUE_INPUT
+			      | ISS_PIPELINE_STREAM;
+
+		spin_lock_irqsave(&pipe->lock, flags);
+		pipe->state &= ~state;
+		if (video->pipe.stream_state == ISS_PIPELINE_STREAM_CONTINUOUS)
+			video->dmaqueue_flags |= ISS_VIDEO_DMAQUEUE_UNDERRUN;
+		spin_unlock_irqrestore(&pipe->lock, flags);
+		return NULL;
+	}
+
+	if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE && pipe->input != NULL) {
+		spin_lock(&pipe->lock);
+		pipe->state &= ~ISS_PIPELINE_STREAM;
+		spin_unlock(&pipe->lock);
+	}
+
+	buf = list_first_entry(&video->dmaqueue, struct iss_buffer,
+			       list);
+	spin_unlock_irqrestore(&video->qlock, flags);
+	buf->vb.state = VB2_BUF_STATE_ACTIVE;
+	return buf;
+}
+
+/*
+ * omap4iss_video_cancel_stream - Cancel stream on a video node
+ * @video: ISS video object
+ *
+ * Cancelling a stream mark all buffers on the video node as erroneous and makes
+ * sure no new buffer can be queued.
+ */
+void omap4iss_video_cancel_stream(struct iss_video *video)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&video->qlock, flags);
+
+	while (!list_empty(&video->dmaqueue)) {
+		struct iss_buffer *buf;
+
+		buf = list_first_entry(&video->dmaqueue, struct iss_buffer,
+				       list);
+		list_del(&buf->list);
+		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+	}
+
+	video->error = true;
+
+	spin_unlock_irqrestore(&video->qlock, flags);
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 ioctls
+ */
+
+static int
+iss_video_querycap(struct file *file, void *fh, struct v4l2_capability *cap)
+{
+	struct iss_video *video = video_drvdata(file);
+
+	strlcpy(cap->driver, ISS_VIDEO_DRIVER_NAME, sizeof(cap->driver));
+	strlcpy(cap->card, video->video.name, sizeof(cap->card));
+	strlcpy(cap->bus_info, "media", sizeof(cap->bus_info));
+
+	if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	else
+		cap->device_caps = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
+
+	cap->capabilities = V4L2_CAP_DEVICE_CAPS | V4L2_CAP_STREAMING
+			  | V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OUTPUT;
+
+	return 0;
+}
+
+static int
+iss_video_enum_format(struct file *file, void *fh, struct v4l2_fmtdesc *f)
+{
+	struct iss_video *video = video_drvdata(file);
+	struct v4l2_mbus_framefmt format;
+	unsigned int index = f->index;
+	unsigned int i;
+	int ret;
+
+	if (f->type != video->type)
+		return -EINVAL;
+
+	ret = __iss_video_get_format(video, &format);
+	if (ret < 0)
+		return ret;
+
+	for (i = 0; i < ARRAY_SIZE(formats); ++i) {
+		const struct iss_format_info *info = &formats[i];
+
+		if (format.code != info->code)
+			continue;
+
+		if (index == 0) {
+			f->pixelformat = info->pixelformat;
+			strlcpy(f->description, info->description,
+				sizeof(f->description));
+			return 0;
+		}
+
+		index--;
+	}
+
+	return -EINVAL;
+}
+
+static int
+iss_video_get_format(struct file *file, void *fh, struct v4l2_format *format)
+{
+	struct iss_video_fh *vfh = to_iss_video_fh(fh);
+	struct iss_video *video = video_drvdata(file);
+
+	if (format->type != video->type)
+		return -EINVAL;
+
+	mutex_lock(&video->mutex);
+	*format = vfh->format;
+	mutex_unlock(&video->mutex);
+
+	return 0;
+}
+
+static int
+iss_video_set_format(struct file *file, void *fh, struct v4l2_format *format)
+{
+	struct iss_video_fh *vfh = to_iss_video_fh(fh);
+	struct iss_video *video = video_drvdata(file);
+	struct v4l2_mbus_framefmt fmt;
+
+	if (format->type != video->type)
+		return -EINVAL;
+
+	mutex_lock(&video->mutex);
+
+	/* Fill the bytesperline and sizeimage fields by converting to media bus
+	 * format and back to pixel format.
+	 */
+	iss_video_pix_to_mbus(&format->fmt.pix, &fmt);
+	iss_video_mbus_to_pix(video, &fmt, &format->fmt.pix);
+
+	vfh->format = *format;
+
+	mutex_unlock(&video->mutex);
+	return 0;
+}
+
+static int
+iss_video_try_format(struct file *file, void *fh, struct v4l2_format *format)
+{
+	struct iss_video *video = video_drvdata(file);
+	struct v4l2_subdev_format fmt;
+	struct v4l2_subdev *subdev;
+	u32 pad;
+	int ret;
+
+	if (format->type != video->type)
+		return -EINVAL;
+
+	subdev = iss_video_remote_subdev(video, &pad);
+	if (subdev == NULL)
+		return -EINVAL;
+
+	iss_video_pix_to_mbus(&format->fmt.pix, &fmt.format);
+
+	fmt.pad = pad;
+	fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
+	if (ret)
+		return ret;
+
+	iss_video_mbus_to_pix(video, &fmt.format, &format->fmt.pix);
+	return 0;
+}
+
+static int
+iss_video_cropcap(struct file *file, void *fh, struct v4l2_cropcap *cropcap)
+{
+	struct iss_video *video = video_drvdata(file);
+	struct v4l2_subdev *subdev;
+	int ret;
+
+	subdev = iss_video_remote_subdev(video, NULL);
+	if (subdev == NULL)
+		return -EINVAL;
+
+	mutex_lock(&video->mutex);
+	ret = v4l2_subdev_call(subdev, video, cropcap, cropcap);
+	mutex_unlock(&video->mutex);
+
+	return ret == -ENOIOCTLCMD ? -ENOTTY : ret;
+}
+
+static int
+iss_video_get_crop(struct file *file, void *fh, struct v4l2_crop *crop)
+{
+	struct iss_video *video = video_drvdata(file);
+	struct v4l2_subdev_format format;
+	struct v4l2_subdev *subdev;
+	u32 pad;
+	int ret;
+
+	subdev = iss_video_remote_subdev(video, &pad);
+	if (subdev == NULL)
+		return -EINVAL;
+
+	/* Try the get crop operation first and fallback to get format if not
+	 * implemented.
+	 */
+	ret = v4l2_subdev_call(subdev, video, g_crop, crop);
+	if (ret != -ENOIOCTLCMD)
+		return ret;
+
+	format.pad = pad;
+	format.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &format);
+	if (ret < 0)
+		return ret == -ENOIOCTLCMD ? -ENOTTY : ret;
+
+	crop->c.left = 0;
+	crop->c.top = 0;
+	crop->c.width = format.format.width;
+	crop->c.height = format.format.height;
+
+	return 0;
+}
+
+static int
+iss_video_set_crop(struct file *file, void *fh, const struct v4l2_crop *crop)
+{
+	struct iss_video *video = video_drvdata(file);
+	struct v4l2_subdev *subdev;
+	int ret;
+
+	subdev = iss_video_remote_subdev(video, NULL);
+	if (subdev == NULL)
+		return -EINVAL;
+
+	mutex_lock(&video->mutex);
+	ret = v4l2_subdev_call(subdev, video, s_crop, crop);
+	mutex_unlock(&video->mutex);
+
+	return ret == -ENOIOCTLCMD ? -ENOTTY : ret;
+}
+
+static int
+iss_video_get_param(struct file *file, void *fh, struct v4l2_streamparm *a)
+{
+	struct iss_video_fh *vfh = to_iss_video_fh(fh);
+	struct iss_video *video = video_drvdata(file);
+
+	if (video->type != V4L2_BUF_TYPE_VIDEO_OUTPUT ||
+	    video->type != a->type)
+		return -EINVAL;
+
+	memset(a, 0, sizeof(*a));
+	a->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	a->parm.output.capability = V4L2_CAP_TIMEPERFRAME;
+	a->parm.output.timeperframe = vfh->timeperframe;
+
+	return 0;
+}
+
+static int
+iss_video_set_param(struct file *file, void *fh, struct v4l2_streamparm *a)
+{
+	struct iss_video_fh *vfh = to_iss_video_fh(fh);
+	struct iss_video *video = video_drvdata(file);
+
+	if (video->type != V4L2_BUF_TYPE_VIDEO_OUTPUT ||
+	    video->type != a->type)
+		return -EINVAL;
+
+	if (a->parm.output.timeperframe.denominator == 0)
+		a->parm.output.timeperframe.denominator = 1;
+
+	vfh->timeperframe = a->parm.output.timeperframe;
+
+	return 0;
+}
+
+static int
+iss_video_reqbufs(struct file *file, void *fh, struct v4l2_requestbuffers *rb)
+{
+	struct iss_video_fh *vfh = to_iss_video_fh(fh);
+
+	return vb2_reqbufs(&vfh->queue, rb);
+}
+
+static int
+iss_video_querybuf(struct file *file, void *fh, struct v4l2_buffer *b)
+{
+	struct iss_video_fh *vfh = to_iss_video_fh(fh);
+
+	return vb2_querybuf(&vfh->queue, b);
+}
+
+static int
+iss_video_qbuf(struct file *file, void *fh, struct v4l2_buffer *b)
+{
+	struct iss_video_fh *vfh = to_iss_video_fh(fh);
+
+	return vb2_qbuf(&vfh->queue, b);
+}
+
+static int
+iss_video_dqbuf(struct file *file, void *fh, struct v4l2_buffer *b)
+{
+	struct iss_video_fh *vfh = to_iss_video_fh(fh);
+
+	return vb2_dqbuf(&vfh->queue, b, file->f_flags & O_NONBLOCK);
+}
+
+/*
+ * Stream management
+ *
+ * Every ISS pipeline has a single input and a single output. The input can be
+ * either a sensor or a video node. The output is always a video node.
+ *
+ * As every pipeline has an output video node, the ISS video objects at the
+ * pipeline output stores the pipeline state. It tracks the streaming state of
+ * both the input and output, as well as the availability of buffers.
+ *
+ * In sensor-to-memory mode, frames are always available at the pipeline input.
+ * Starting the sensor usually requires I2C transfers and must be done in
+ * interruptible context. The pipeline is started and stopped synchronously
+ * to the stream on/off commands. All modules in the pipeline will get their
+ * subdev set stream handler called. The module at the end of the pipeline must
+ * delay starting the hardware until buffers are available at its output.
+ *
+ * In memory-to-memory mode, starting/stopping the stream requires
+ * synchronization between the input and output. ISS modules can't be stopped
+ * in the middle of a frame, and at least some of the modules seem to become
+ * busy as soon as they're started, even if they don't receive a frame start
+ * event. For that reason frames need to be processed in single-shot mode. The
+ * driver needs to wait until a frame is completely processed and written to
+ * memory before restarting the pipeline for the next frame. Pipelined
+ * processing might be possible but requires more testing.
+ *
+ * Stream start must be delayed until buffers are available at both the input
+ * and output. The pipeline must be started in the videobuf queue callback with
+ * the buffers queue spinlock held. The modules subdev set stream operation must
+ * not sleep.
+ */
+static int
+iss_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
+{
+	struct iss_video_fh *vfh = to_iss_video_fh(fh);
+	struct iss_video *video = video_drvdata(file);
+	struct media_entity_graph graph;
+	struct media_entity *entity;
+	enum iss_pipeline_state state;
+	struct iss_pipeline *pipe;
+	struct iss_video *far_end;
+	unsigned long flags;
+	int ret;
+
+	if (type != video->type)
+		return -EINVAL;
+
+	mutex_lock(&video->stream_lock);
+
+	/* Start streaming on the pipeline. No link touching an entity in the
+	 * pipeline can be activated or deactivated once streaming is started.
+	 */
+	pipe = video->video.entity.pipe
+	     ? to_iss_pipeline(&video->video.entity) : &video->pipe;
+	pipe->external = NULL;
+	pipe->external_rate = 0;
+	pipe->external_bpp = 0;
+	pipe->entities = 0;
+
+	if (video->iss->pdata->set_constraints)
+		video->iss->pdata->set_constraints(video->iss, true);
+
+	ret = media_entity_pipeline_start(&video->video.entity, &pipe->pipe);
+	if (ret < 0)
+		goto err_media_entity_pipeline_start;
+
+	entity = &video->video.entity;
+	media_entity_graph_walk_start(&graph, entity);
+	while ((entity = media_entity_graph_walk_next(&graph)))
+		pipe->entities |= 1 << entity->id;
+
+	/* Verify that the currently configured format matches the output of
+	 * the connected subdev.
+	 */
+	ret = iss_video_check_format(video, vfh);
+	if (ret < 0)
+		goto err_iss_video_check_format;
+
+	video->bpl_padding = ret;
+	video->bpl_value = vfh->format.fmt.pix.bytesperline;
+
+	/* Find the ISS video node connected at the far end of the pipeline and
+	 * update the pipeline.
+	 */
+	far_end = iss_video_far_end(video);
+
+	if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		state = ISS_PIPELINE_STREAM_OUTPUT | ISS_PIPELINE_IDLE_OUTPUT;
+		pipe->input = far_end;
+		pipe->output = video;
+	} else {
+		if (far_end == NULL) {
+			ret = -EPIPE;
+			goto err_iss_video_check_format;
+		}
+
+		state = ISS_PIPELINE_STREAM_INPUT | ISS_PIPELINE_IDLE_INPUT;
+		pipe->input = video;
+		pipe->output = far_end;
+	}
+
+	spin_lock_irqsave(&pipe->lock, flags);
+	pipe->state &= ~ISS_PIPELINE_STREAM;
+	pipe->state |= state;
+	spin_unlock_irqrestore(&pipe->lock, flags);
+
+	/* Set the maximum time per frame as the value requested by userspace.
+	 * This is a soft limit that can be overridden if the hardware doesn't
+	 * support the request limit.
+	 */
+	if (video->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		pipe->max_timeperframe = vfh->timeperframe;
+
+	video->queue = &vfh->queue;
+	INIT_LIST_HEAD(&video->dmaqueue);
+	spin_lock_init(&video->qlock);
+	video->error = false;
+	atomic_set(&pipe->frame_number, -1);
+
+	ret = vb2_streamon(&vfh->queue, type);
+	if (ret < 0)
+		goto err_iss_video_check_format;
+
+	/* In sensor-to-memory mode, the stream can be started synchronously
+	 * to the stream on command. In memory-to-memory mode, it will be
+	 * started when buffers are queued on both the input and output.
+	 */
+	if (pipe->input == NULL) {
+		unsigned long flags;
+		ret = omap4iss_pipeline_set_stream(pipe,
+					      ISS_PIPELINE_STREAM_CONTINUOUS);
+		if (ret < 0)
+			goto err_omap4iss_set_stream;
+		spin_lock_irqsave(&video->qlock, flags);
+		if (list_empty(&video->dmaqueue))
+			video->dmaqueue_flags |= ISS_VIDEO_DMAQUEUE_UNDERRUN;
+		spin_unlock_irqrestore(&video->qlock, flags);
+	}
+
+	mutex_unlock(&video->stream_lock);
+	return 0;
+
+err_omap4iss_set_stream:
+	vb2_streamoff(&vfh->queue, type);
+err_iss_video_check_format:
+	media_entity_pipeline_stop(&video->video.entity);
+err_media_entity_pipeline_start:
+	if (video->iss->pdata->set_constraints)
+		video->iss->pdata->set_constraints(video->iss, false);
+	video->queue = NULL;
+
+	mutex_unlock(&video->stream_lock);
+	return ret;
+}
+
+static int
+iss_video_streamoff(struct file *file, void *fh, enum v4l2_buf_type type)
+{
+	struct iss_video_fh *vfh = to_iss_video_fh(fh);
+	struct iss_video *video = video_drvdata(file);
+	struct iss_pipeline *pipe = to_iss_pipeline(&video->video.entity);
+	enum iss_pipeline_state state;
+	unsigned long flags;
+
+	if (type != video->type)
+		return -EINVAL;
+
+	mutex_lock(&video->stream_lock);
+
+	if (!vb2_is_streaming(&vfh->queue))
+		goto done;
+
+	/* Update the pipeline state. */
+	if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		state = ISS_PIPELINE_STREAM_OUTPUT
+		      | ISS_PIPELINE_QUEUE_OUTPUT;
+	else
+		state = ISS_PIPELINE_STREAM_INPUT
+		      | ISS_PIPELINE_QUEUE_INPUT;
+
+	spin_lock_irqsave(&pipe->lock, flags);
+	pipe->state &= ~state;
+	spin_unlock_irqrestore(&pipe->lock, flags);
+
+	/* Stop the stream. */
+	omap4iss_pipeline_set_stream(pipe, ISS_PIPELINE_STREAM_STOPPED);
+	vb2_streamoff(&vfh->queue, type);
+	video->queue = NULL;
+
+	if (video->iss->pdata->set_constraints)
+		video->iss->pdata->set_constraints(video->iss, false);
+	media_entity_pipeline_stop(&video->video.entity);
+
+done:
+	mutex_unlock(&video->stream_lock);
+	return 0;
+}
+
+static int
+iss_video_enum_input(struct file *file, void *fh, struct v4l2_input *input)
+{
+	if (input->index > 0)
+		return -EINVAL;
+
+	strlcpy(input->name, "camera", sizeof(input->name));
+	input->type = V4L2_INPUT_TYPE_CAMERA;
+
+	return 0;
+}
+
+static int
+iss_video_g_input(struct file *file, void *fh, unsigned int *input)
+{
+	*input = 0;
+
+	return 0;
+}
+
+static int
+iss_video_s_input(struct file *file, void *fh, unsigned int input)
+{
+	return input == 0 ? 0 : -EINVAL;
+}
+
+static const struct v4l2_ioctl_ops iss_video_ioctl_ops = {
+	.vidioc_querycap		= iss_video_querycap,
+	.vidioc_enum_fmt_vid_cap        = iss_video_enum_format,
+	.vidioc_g_fmt_vid_cap		= iss_video_get_format,
+	.vidioc_s_fmt_vid_cap		= iss_video_set_format,
+	.vidioc_try_fmt_vid_cap		= iss_video_try_format,
+	.vidioc_g_fmt_vid_out		= iss_video_get_format,
+	.vidioc_s_fmt_vid_out		= iss_video_set_format,
+	.vidioc_try_fmt_vid_out		= iss_video_try_format,
+	.vidioc_cropcap			= iss_video_cropcap,
+	.vidioc_g_crop			= iss_video_get_crop,
+	.vidioc_s_crop			= iss_video_set_crop,
+	.vidioc_g_parm			= iss_video_get_param,
+	.vidioc_s_parm			= iss_video_set_param,
+	.vidioc_reqbufs			= iss_video_reqbufs,
+	.vidioc_querybuf		= iss_video_querybuf,
+	.vidioc_qbuf			= iss_video_qbuf,
+	.vidioc_dqbuf			= iss_video_dqbuf,
+	.vidioc_streamon		= iss_video_streamon,
+	.vidioc_streamoff		= iss_video_streamoff,
+	.vidioc_enum_input		= iss_video_enum_input,
+	.vidioc_g_input			= iss_video_g_input,
+	.vidioc_s_input			= iss_video_s_input,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 file operations
+ */
+
+static int iss_video_open(struct file *file)
+{
+	struct iss_video *video = video_drvdata(file);
+	struct iss_video_fh *handle;
+	struct vb2_queue *q;
+	int ret = 0;
+
+	handle = kzalloc(sizeof(*handle), GFP_KERNEL);
+	if (handle == NULL)
+		return -ENOMEM;
+
+	v4l2_fh_init(&handle->vfh, &video->video);
+	v4l2_fh_add(&handle->vfh);
+
+	/* If this is the first user, initialise the pipeline. */
+	if (omap4iss_get(video->iss) == NULL) {
+		ret = -EBUSY;
+		goto done;
+	}
+
+	ret = omap4iss_pipeline_pm_use(&video->video.entity, 1);
+	if (ret < 0) {
+		omap4iss_put(video->iss);
+		goto done;
+	}
+
+	video->alloc_ctx = vb2_dma_contig_init_ctx(video->iss->dev);
+	if (IS_ERR(video->alloc_ctx)) {
+		ret = PTR_ERR(video->alloc_ctx);
+		omap4iss_put(video->iss);
+		goto done;
+	}
+
+	q = &handle->queue;
+
+	q->type = video->type;
+	q->io_modes = VB2_MMAP;
+	q->drv_priv = handle;
+	q->ops = &iss_video_vb2ops;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->buf_struct_size = sizeof(struct iss_buffer);
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+
+	ret = vb2_queue_init(q);
+	if (ret) {
+		omap4iss_put(video->iss);
+		goto done;
+	}
+
+	memset(&handle->format, 0, sizeof(handle->format));
+	handle->format.type = video->type;
+	handle->timeperframe.denominator = 1;
+
+	handle->video = video;
+	file->private_data = &handle->vfh;
+
+done:
+	if (ret < 0) {
+		v4l2_fh_del(&handle->vfh);
+		kfree(handle);
+	}
+
+	return ret;
+}
+
+static int iss_video_release(struct file *file)
+{
+	struct iss_video *video = video_drvdata(file);
+	struct v4l2_fh *vfh = file->private_data;
+	struct iss_video_fh *handle = to_iss_video_fh(vfh);
+
+	/* Disable streaming and free the buffers queue resources. */
+	iss_video_streamoff(file, vfh, video->type);
+
+	omap4iss_pipeline_pm_use(&video->video.entity, 0);
+
+	/* Release the videobuf2 queue */
+	vb2_queue_release(&handle->queue);
+
+	/* Release the file handle. */
+	v4l2_fh_del(vfh);
+	kfree(handle);
+	file->private_data = NULL;
+
+	omap4iss_put(video->iss);
+
+	return 0;
+}
+
+static unsigned int iss_video_poll(struct file *file, poll_table *wait)
+{
+	struct iss_video_fh *vfh = to_iss_video_fh(file->private_data);
+
+	return vb2_poll(&vfh->queue, file, wait);
+}
+
+static int iss_video_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct iss_video_fh *vfh = to_iss_video_fh(file->private_data);
+
+	return vb2_mmap(&vfh->queue, vma);
+}
+
+static struct v4l2_file_operations iss_video_fops = {
+	.owner = THIS_MODULE,
+	.unlocked_ioctl = video_ioctl2,
+	.open = iss_video_open,
+	.release = iss_video_release,
+	.poll = iss_video_poll,
+	.mmap = iss_video_mmap,
+};
+
+/* -----------------------------------------------------------------------------
+ * ISS video core
+ */
+
+static const struct iss_video_operations iss_video_dummy_ops = {
+};
+
+int omap4iss_video_init(struct iss_video *video, const char *name)
+{
+	const char *direction;
+	int ret;
+
+	switch (video->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		direction = "output";
+		video->pad.flags = MEDIA_PAD_FL_SINK;
+		break;
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+		direction = "input";
+		video->pad.flags = MEDIA_PAD_FL_SOURCE;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	ret = media_entity_init(&video->video.entity, 1, &video->pad, 0);
+	if (ret < 0)
+		return ret;
+
+	mutex_init(&video->mutex);
+	atomic_set(&video->active, 0);
+
+	spin_lock_init(&video->pipe.lock);
+	mutex_init(&video->stream_lock);
+
+	/* Initialize the video device. */
+	if (video->ops == NULL)
+		video->ops = &iss_video_dummy_ops;
+
+	video->video.fops = &iss_video_fops;
+	snprintf(video->video.name, sizeof(video->video.name),
+		 "OMAP4 ISS %s %s", name, direction);
+	video->video.vfl_type = VFL_TYPE_GRABBER;
+	video->video.release = video_device_release_empty;
+	video->video.ioctl_ops = &iss_video_ioctl_ops;
+	video->pipe.stream_state = ISS_PIPELINE_STREAM_STOPPED;
+
+	video_set_drvdata(&video->video, video);
+
+	return 0;
+}
+
+void omap4iss_video_cleanup(struct iss_video *video)
+{
+	media_entity_cleanup(&video->video.entity);
+	mutex_destroy(&video->stream_lock);
+	mutex_destroy(&video->mutex);
+}
+
+int omap4iss_video_register(struct iss_video *video, struct v4l2_device *vdev)
+{
+	int ret;
+
+	video->video.v4l2_dev = vdev;
+
+	ret = video_register_device(&video->video, VFL_TYPE_GRABBER, -1);
+	if (ret < 0)
+		dev_err(video->iss->dev,
+			"%s: could not register video device (%d)\n",
+			__func__, ret);
+
+	return ret;
+}
+
+void omap4iss_video_unregister(struct iss_video *video)
+{
+	video_unregister_device(&video->video);
+}
diff --git a/drivers/staging/media/omap4iss/iss_video.h b/drivers/staging/media/omap4iss/iss_video.h
new file mode 100644
index 000000000000..878e4a3082e7
--- /dev/null
+++ b/drivers/staging/media/omap4iss/iss_video.h
@@ -0,0 +1,204 @@
+/*
+ * TI OMAP4 ISS V4L2 Driver - Generic video node
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * Author: Sergio Aguirre <sergio.a.aguirre@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.
+ */
+
+#ifndef OMAP4_ISS_VIDEO_H
+#define OMAP4_ISS_VIDEO_H
+
+#include <linux/v4l2-mediabus.h>
+#include <media/media-entity.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-fh.h>
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-dma-contig.h>
+
+#define ISS_VIDEO_DRIVER_NAME		"issvideo"
+#define ISS_VIDEO_DRIVER_VERSION	"0.0.2"
+
+struct iss_device;
+struct iss_video;
+struct v4l2_mbus_framefmt;
+struct v4l2_pix_format;
+
+/*
+ * struct iss_format_info - ISS media bus format information
+ * @code: V4L2 media bus format code
+ * @truncated: V4L2 media bus format code for the same format truncated to 10
+ *	bits. Identical to @code if the format is 10 bits wide or less.
+ * @uncompressed: V4L2 media bus format code for the corresponding uncompressed
+ *	format. Identical to @code if the format is not DPCM compressed.
+ * @flavor: V4L2 media bus format code for the same pixel layout but
+ *	shifted to be 8 bits per pixel. =0 if format is not shiftable.
+ * @pixelformat: V4L2 pixel format FCC identifier
+ * @bpp: Bits per pixel
+ * @description: Human-readable format description
+ */
+struct iss_format_info {
+	enum v4l2_mbus_pixelcode code;
+	enum v4l2_mbus_pixelcode truncated;
+	enum v4l2_mbus_pixelcode uncompressed;
+	enum v4l2_mbus_pixelcode flavor;
+	u32 pixelformat;
+	unsigned int bpp;
+	const char *description;
+};
+
+enum iss_pipeline_stream_state {
+	ISS_PIPELINE_STREAM_STOPPED = 0,
+	ISS_PIPELINE_STREAM_CONTINUOUS = 1,
+	ISS_PIPELINE_STREAM_SINGLESHOT = 2,
+};
+
+enum iss_pipeline_state {
+	/* The stream has been started on the input video node. */
+	ISS_PIPELINE_STREAM_INPUT = 1,
+	/* The stream has been started on the output video node. */
+	ISS_PIPELINE_STREAM_OUTPUT = (1 << 1),
+	/* At least one buffer is queued on the input video node. */
+	ISS_PIPELINE_QUEUE_INPUT = (1 << 2),
+	/* At least one buffer is queued on the output video node. */
+	ISS_PIPELINE_QUEUE_OUTPUT = (1 << 3),
+	/* The input entity is idle, ready to be started. */
+	ISS_PIPELINE_IDLE_INPUT = (1 << 4),
+	/* The output entity is idle, ready to be started. */
+	ISS_PIPELINE_IDLE_OUTPUT = (1 << 5),
+	/* The pipeline is currently streaming. */
+	ISS_PIPELINE_STREAM = (1 << 6),
+};
+
+/*
+ * struct iss_pipeline - An OMAP4 ISS hardware pipeline
+ * @entities: Bitmask of entities in the pipeline (indexed by entity ID)
+ * @error: A hardware error occurred during capture
+ */
+struct iss_pipeline {
+	struct media_pipeline pipe;
+	spinlock_t lock;		/* Pipeline state and queue flags */
+	unsigned int state;
+	enum iss_pipeline_stream_state stream_state;
+	struct iss_video *input;
+	struct iss_video *output;
+	unsigned int entities;
+	atomic_t frame_number;
+	bool do_propagation; /* of frame number */
+	bool error;
+	struct v4l2_fract max_timeperframe;
+	struct v4l2_subdev *external;
+	unsigned int external_rate;
+	int external_bpp;
+};
+
+#define to_iss_pipeline(__e) \
+	container_of((__e)->pipe, struct iss_pipeline, pipe)
+
+static inline int iss_pipeline_ready(struct iss_pipeline *pipe)
+{
+	return pipe->state == (ISS_PIPELINE_STREAM_INPUT |
+			       ISS_PIPELINE_STREAM_OUTPUT |
+			       ISS_PIPELINE_QUEUE_INPUT |
+			       ISS_PIPELINE_QUEUE_OUTPUT |
+			       ISS_PIPELINE_IDLE_INPUT |
+			       ISS_PIPELINE_IDLE_OUTPUT);
+}
+
+/*
+ * struct iss_buffer - ISS buffer
+ * @buffer: ISS video buffer
+ * @iss_addr: Physical address of the buffer.
+ */
+struct iss_buffer {
+	/* common v4l buffer stuff -- must be first */
+	struct vb2_buffer	vb;
+	struct list_head	list;
+	dma_addr_t iss_addr;
+};
+
+#define to_iss_buffer(buf)	container_of(buf, struct iss_buffer, buffer)
+
+enum iss_video_dmaqueue_flags {
+	/* Set if DMA queue becomes empty when ISS_PIPELINE_STREAM_CONTINUOUS */
+	ISS_VIDEO_DMAQUEUE_UNDERRUN = (1 << 0),
+	/* Set when queuing buffer to an empty DMA queue */
+	ISS_VIDEO_DMAQUEUE_QUEUED = (1 << 1),
+};
+
+#define iss_video_dmaqueue_flags_clr(video)	\
+			({ (video)->dmaqueue_flags = 0; })
+
+/*
+ * struct iss_video_operations - ISS video operations
+ * @queue:	Resume streaming when a buffer is queued. Called on VIDIOC_QBUF
+ *		if there was no buffer previously queued.
+ */
+struct iss_video_operations {
+	int(*queue)(struct iss_video *video, struct iss_buffer *buffer);
+};
+
+struct iss_video {
+	struct video_device video;
+	enum v4l2_buf_type type;
+	struct media_pad pad;
+
+	struct mutex mutex;		/* format and crop settings */
+	atomic_t active;
+
+	struct iss_device *iss;
+
+	unsigned int capture_mem;
+	unsigned int bpl_alignment;	/* alignment value */
+	unsigned int bpl_zero_padding;	/* whether the alignment is optional */
+	unsigned int bpl_max;		/* maximum bytes per line value */
+	unsigned int bpl_value;		/* bytes per line value */
+	unsigned int bpl_padding;	/* padding at end of line */
+
+	/* Pipeline state */
+	struct iss_pipeline pipe;
+	struct mutex stream_lock;	/* pipeline and stream states */
+	bool error;
+
+	/* Video buffers queue */
+	struct vb2_queue *queue;
+	spinlock_t qlock;		/* protects dmaqueue and error */
+	struct list_head dmaqueue;
+	enum iss_video_dmaqueue_flags dmaqueue_flags;
+	struct vb2_alloc_ctx *alloc_ctx;
+
+	const struct iss_video_operations *ops;
+};
+
+#define to_iss_video(vdev)	container_of(vdev, struct iss_video, video)
+
+struct iss_video_fh {
+	struct v4l2_fh vfh;
+	struct iss_video *video;
+	struct vb2_queue queue;
+	struct v4l2_format format;
+	struct v4l2_fract timeperframe;
+};
+
+#define to_iss_video_fh(fh)	container_of(fh, struct iss_video_fh, vfh)
+#define iss_video_queue_to_iss_video_fh(q) \
+				container_of(q, struct iss_video_fh, queue)
+
+int omap4iss_video_init(struct iss_video *video, const char *name);
+void omap4iss_video_cleanup(struct iss_video *video);
+int omap4iss_video_register(struct iss_video *video,
+			    struct v4l2_device *vdev);
+void omap4iss_video_unregister(struct iss_video *video);
+struct iss_buffer *omap4iss_video_buffer_next(struct iss_video *video);
+void omap4iss_video_cancel_stream(struct iss_video *video);
+struct media_pad *omap4iss_video_remote_pad(struct iss_video *video);
+
+const struct iss_format_info *
+omap4iss_video_format_info(enum v4l2_mbus_pixelcode code);
+
+#endif /* OMAP4_ISS_VIDEO_H */
diff --git a/drivers/media/usb/sn9c102/Kconfig b/drivers/staging/media/sn9c102/Kconfig
index 6ebaf2940d06..c9aba59258d9 100644
--- a/drivers/media/usb/sn9c102/Kconfig
+++ b/drivers/staging/media/sn9c102/Kconfig
@@ -1,14 +1,17 @@
 config USB_SN9C102
 	tristate "USB SN9C1xx PC Camera Controller support (DEPRECATED)"
-	depends on VIDEO_V4L2
+	depends on VIDEO_V4L2 && MEDIA_USB_SUPPORT
 	---help---
-	  This driver is DEPRECATED please use the gspca sonixb and
+	  This driver is DEPRECATED, please use the gspca sonixb and
 	  sonixj modules instead.
 
 	  Say Y here if you want support for cameras based on SONiX SN9C101,
 	  SN9C102, SN9C103, SN9C105 and SN9C120 PC Camera Controllers.
 
-	  See <file:Documentation/video4linux/sn9c102.txt> for more info.
+	  See <file:drivers/staging/media/sn9c102/sn9c102.txt> for more info.
+
+	  If you have webcams that are only supported by this driver and not by
+	  the gspca driver, then contact the linux-media mailinglist.
 
 	  To compile this driver as a module, choose M here: the
 	  module will be called sn9c102.
diff --git a/drivers/media/usb/sn9c102/Makefile b/drivers/staging/media/sn9c102/Makefile
index 7ecd5a90c7c9..7ecd5a90c7c9 100644
--- a/drivers/media/usb/sn9c102/Makefile
+++ b/drivers/staging/media/sn9c102/Makefile
diff --git a/drivers/media/usb/sn9c102/sn9c102.h b/drivers/staging/media/sn9c102/sn9c102.h
index 8a917f060503..8a917f060503 100644
--- a/drivers/media/usb/sn9c102/sn9c102.h
+++ b/drivers/staging/media/sn9c102/sn9c102.h
diff --git a/drivers/staging/media/sn9c102/sn9c102.txt b/drivers/staging/media/sn9c102/sn9c102.txt
new file mode 100644
index 000000000000..b4f67040403a
--- /dev/null
+++ b/drivers/staging/media/sn9c102/sn9c102.txt
@@ -0,0 +1,592 @@
+
+			 SN9C1xx PC Camera Controllers
+				Driver for Linux
+			 =============================
+
+			       - Documentation -
+
+
+Index
+=====
+1.  Copyright
+2.  Disclaimer
+3.  License
+4.  Overview and features
+5.  Module dependencies
+6.  Module loading
+7.  Module parameters
+8.  Optional device control through "sysfs"
+9.  Supported devices
+10. Notes for V4L2 application developers
+11. Video frame formats
+12. Contact information
+13. Credits
+
+
+1. Copyright
+============
+Copyright (C) 2004-2007 by Luca Risolia <luca.risolia@studio.unibo.it>
+
+
+2. Disclaimer
+=============
+SONiX is a trademark of SONiX Technology Company Limited, inc.
+This software is not sponsored or developed by SONiX.
+
+
+3. License
+==========
+This program is free software; you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation; either version 2 of the License, or
+(at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program; if not, write to the Free Software
+Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+
+4. Overview and features
+========================
+This driver attempts to support the video interface of the devices assembling
+the SONiX SN9C101, SN9C102, SN9C103, SN9C105 and SN9C120 PC Camera Controllers
+("SN9C1xx" from now on).
+
+The driver relies on the Video4Linux2 and USB core modules. It has been
+designed to run properly on SMP systems as well.
+
+The latest version of the SN9C1xx driver can be found at the following URL:
+http://www.linux-projects.org/
+
+Some of the features of the driver are:
+
+- full compliance with the Video4Linux2 API (see also "Notes for V4L2
+  application developers" paragraph);
+- available mmap or read/poll methods for video streaming through isochronous
+  data transfers;
+- automatic detection of image sensor;
+- support for built-in microphone interface;
+- support for any window resolutions and optional panning within the maximum
+  pixel area of image sensor;
+- image downscaling with arbitrary scaling factors from 1, 2 and 4 in both
+  directions (see "Notes for V4L2 application developers" paragraph);
+- two different video formats for uncompressed or compressed data in low or
+  high compression quality (see also "Notes for V4L2 application developers"
+  and "Video frame formats" paragraphs);
+- full support for the capabilities of many of the possible image sensors that
+  can be connected to the SN9C1xx bridges, including, for instance, red, green,
+  blue and global gain adjustments and exposure (see "Supported devices"
+  paragraph for details);
+- use of default color settings for sunlight conditions;
+- dynamic I/O interface for both SN9C1xx and image sensor control and
+  monitoring (see "Optional device control through 'sysfs'" paragraph);
+- dynamic driver control thanks to various module parameters (see "Module
+  parameters" paragraph);
+- up to 64 cameras can be handled at the same time; they can be connected and
+  disconnected from the host many times without turning off the computer, if
+  the system supports hotplugging;
+- no known bugs.
+
+
+5. Module dependencies
+======================
+For it to work properly, the driver needs kernel support for Video4Linux and
+USB.
+
+The following options of the kernel configuration file must be enabled and
+corresponding modules must be compiled:
+
+	# Multimedia devices
+	#
+	CONFIG_VIDEO_DEV=m
+
+To enable advanced debugging functionality on the device through /sysfs:
+
+	# Multimedia devices
+	#
+	CONFIG_VIDEO_ADV_DEBUG=y
+
+	# USB support
+	#
+	CONFIG_USB=m
+
+In addition, depending on the hardware being used, the modules below are
+necessary:
+
+	# USB Host Controller Drivers
+	#
+	CONFIG_USB_EHCI_HCD=m
+	CONFIG_USB_UHCI_HCD=m
+	CONFIG_USB_OHCI_HCD=m
+
+The SN9C103, SN9c105 and SN9C120 controllers also provide a built-in microphone
+interface. It is supported by the USB Audio driver thanks to the ALSA API:
+
+	# Sound
+	#
+	CONFIG_SOUND=y
+
+	# Advanced Linux Sound Architecture
+	#
+	CONFIG_SND=m
+
+	# USB devices
+	#
+	CONFIG_SND_USB_AUDIO=m
+
+And finally:
+
+	# USB Multimedia devices
+	#
+	CONFIG_USB_SN9C102=m
+
+
+6. Module loading
+=================
+To use the driver, it is necessary to load the "sn9c102" module into memory
+after every other module required: "videodev", "v4l2_common", "compat_ioctl32",
+"usbcore" and, depending on the USB host controller you have, "ehci-hcd",
+"uhci-hcd" or "ohci-hcd".
+
+Loading can be done as shown below:
+
+	[root@localhost home]# modprobe sn9c102
+
+Note that the module is called "sn9c102" for historic reasons, although it
+does not just support the SN9C102.
+
+At this point all the devices supported by the driver and connected to the USB
+ports should be recognized. You can invoke "dmesg" to analyze kernel messages
+and verify that the loading process has gone well:
+
+	[user@localhost home]$ dmesg
+
+or, to isolate all the kernel messages generated by the driver:
+
+	[user@localhost home]$ dmesg | grep sn9c102
+
+
+7. Module parameters
+====================
+Module parameters are listed below:
+-------------------------------------------------------------------------------
+Name:           video_nr
+Type:           short array (min = 0, max = 64)
+Syntax:         <-1|n[,...]>
+Description:    Specify V4L2 minor mode number:
+		-1 = use next available
+		 n = use minor number n
+		You can specify up to 64 cameras this way.
+		For example:
+		video_nr=-1,2,-1 would assign minor number 2 to the second
+		recognized camera and use auto for the first one and for every
+		other camera.
+Default:        -1
+-------------------------------------------------------------------------------
+Name:           force_munmap
+Type:           bool array (min = 0, max = 64)
+Syntax:         <0|1[,...]>
+Description:    Force the application to unmap previously mapped buffer memory
+		before calling any VIDIOC_S_CROP or VIDIOC_S_FMT ioctl's. Not
+		all the applications support this feature. This parameter is
+		specific for each detected camera.
+		0 = do not force memory unmapping
+		1 = force memory unmapping (save memory)
+Default:        0
+-------------------------------------------------------------------------------
+Name:           frame_timeout
+Type:           uint array (min = 0, max = 64)
+Syntax:         <0|n[,...]>
+Description:    Timeout for a video frame in seconds before returning an I/O
+		error; 0 for infinity. This parameter is specific for each
+		detected camera and can be changed at runtime thanks to the
+		/sys filesystem interface.
+Default:        2
+-------------------------------------------------------------------------------
+Name:           debug
+Type:           ushort
+Syntax:         <n>
+Description:    Debugging information level, from 0 to 3:
+		0 = none (use carefully)
+		1 = critical errors
+		2 = significant information
+		3 = more verbose messages
+		Level 3 is useful for testing only. It also shows some more
+		information about the hardware being detected.
+		This parameter can be changed at runtime thanks to the /sys
+		filesystem interface.
+Default:        2
+-------------------------------------------------------------------------------
+
+
+8. Optional device control through "sysfs" [1]
+==========================================
+If the kernel has been compiled with the CONFIG_VIDEO_ADV_DEBUG option enabled,
+it is possible to read and write both the SN9C1xx and the image sensor
+registers by using the "sysfs" filesystem interface.
+
+Every time a supported device is recognized, a write-only file named "green" is
+created in the /sys/class/video4linux/videoX directory. You can set the green
+channel's gain by writing the desired value to it. The value may range from 0
+to 15 for the SN9C101 or SN9C102 bridges, from 0 to 127 for the SN9C103,
+SN9C105 and SN9C120 bridges.
+Similarly, only for the SN9C103, SN9C105 and SN9C120 controllers, blue and red
+gain control files are available in the same directory, for which accepted
+values may range from 0 to 127.
+
+There are other four entries in the directory above for each registered camera:
+"reg", "val", "i2c_reg" and "i2c_val". The first two files control the
+SN9C1xx bridge, while the other two control the sensor chip. "reg" and
+"i2c_reg" hold the values of the current register index where the following
+reading/writing operations are addressed at through "val" and "i2c_val". Their
+use is not intended for end-users. Note that "i2c_reg" and "i2c_val" will not
+be created if the sensor does not actually support the standard I2C protocol or
+its registers are not 8-bit long. Also, remember that you must be logged in as
+root before writing to them.
+
+As an example, suppose we were to want to read the value contained in the
+register number 1 of the sensor register table - which is usually the product
+identifier - of the camera registered as "/dev/video0":
+
+	[root@localhost #] cd /sys/class/video4linux/video0
+	[root@localhost #] echo 1 > i2c_reg
+	[root@localhost #] cat i2c_val
+
+Note that "cat" will fail if sensor registers cannot be read.
+
+Now let's set the green gain's register of the SN9C101 or SN9C102 chips to 2:
+
+	[root@localhost #] echo 0x11 > reg
+	[root@localhost #] echo 2 > val
+
+Note that the SN9C1xx always returns 0 when some of its registers are read.
+To avoid race conditions, all the I/O accesses to the above files are
+serialized.
+The sysfs interface also provides the "frame_header" entry, which exports the
+frame header of the most recent requested and captured video frame. The header
+is always 18-bytes long and is appended to every video frame by the SN9C1xx
+controllers. As an example, this additional information can be used by the user
+application for implementing auto-exposure features via software.
+
+The following table describes the frame header exported by the SN9C101 and
+SN9C102:
+
+Byte #  Value or bits Description
+------  ------------- -----------
+0x00    0xFF          Frame synchronisation pattern
+0x01    0xFF          Frame synchronisation pattern
+0x02    0x00          Frame synchronisation pattern
+0x03    0xC4          Frame synchronisation pattern
+0x04    0xC4          Frame synchronisation pattern
+0x05    0x96          Frame synchronisation pattern
+0x06    [3:0]         Read channel gain control = (1+R_GAIN/8)
+	[7:4]         Blue channel gain control = (1+B_GAIN/8)
+0x07    [ 0 ]         Compression mode. 0=No compression, 1=Compression enabled
+	[2:1]         Maximum scale factor for compression
+	[ 3 ]         1 = USB fifo(2K bytes) is full
+	[ 4 ]         1 = Digital gain is finish
+	[ 5 ]         1 = Exposure is finish
+	[7:6]         Frame index
+0x08    [7:0]         Y sum inside Auto-Exposure area (low-byte)
+0x09    [7:0]         Y sum inside Auto-Exposure area (high-byte)
+		      where Y sum = (R/4 + 5G/16 + B/8) / 32
+0x0A    [7:0]         Y sum outside Auto-Exposure area (low-byte)
+0x0B    [7:0]         Y sum outside Auto-Exposure area (high-byte)
+		      where Y sum = (R/4 + 5G/16 + B/8) / 128
+0x0C    0xXX          Not used
+0x0D    0xXX          Not used
+0x0E    0xXX          Not used
+0x0F    0xXX          Not used
+0x10    0xXX          Not used
+0x11    0xXX          Not used
+
+The following table describes the frame header exported by the SN9C103:
+
+Byte #  Value or bits Description
+------  ------------- -----------
+0x00    0xFF          Frame synchronisation pattern
+0x01    0xFF          Frame synchronisation pattern
+0x02    0x00          Frame synchronisation pattern
+0x03    0xC4          Frame synchronisation pattern
+0x04    0xC4          Frame synchronisation pattern
+0x05    0x96          Frame synchronisation pattern
+0x06    [6:0]         Read channel gain control = (1/2+R_GAIN/64)
+0x07    [6:0]         Blue channel gain control = (1/2+B_GAIN/64)
+	[7:4]
+0x08    [ 0 ]         Compression mode. 0=No compression, 1=Compression enabled
+	[2:1]         Maximum scale factor for compression
+	[ 3 ]         1 = USB fifo(2K bytes) is full
+	[ 4 ]         1 = Digital gain is finish
+	[ 5 ]         1 = Exposure is finish
+	[7:6]         Frame index
+0x09    [7:0]         Y sum inside Auto-Exposure area (low-byte)
+0x0A    [7:0]         Y sum inside Auto-Exposure area (high-byte)
+		      where Y sum = (R/4 + 5G/16 + B/8) / 32
+0x0B    [7:0]         Y sum outside Auto-Exposure area (low-byte)
+0x0C    [7:0]         Y sum outside Auto-Exposure area (high-byte)
+		      where Y sum = (R/4 + 5G/16 + B/8) / 128
+0x0D    [1:0]         Audio frame number
+	[ 2 ]         1 = Audio is recording
+0x0E    [7:0]         Audio summation (low-byte)
+0x0F    [7:0]         Audio summation (high-byte)
+0x10    [7:0]         Audio sample count
+0x11    [7:0]         Audio peak data in audio frame
+
+The AE area (sx, sy, ex, ey) in the active window can be set by programming the
+registers 0x1c, 0x1d, 0x1e and 0x1f of the SN9C1xx controllers, where one unit
+corresponds to 32 pixels.
+
+[1] The frame headers exported by the SN9C105 and SN9C120 are not described.
+
+
+9. Supported devices
+====================
+None of the names of the companies as well as their products will be mentioned
+here. They have never collaborated with the author, so no advertising.
+
+From the point of view of a driver, what unambiguously identify a device are
+its vendor and product USB identifiers. Below is a list of known identifiers of
+devices assembling the SN9C1xx PC camera controllers:
+
+Vendor ID  Product ID
+---------  ----------
+0x0458     0x7025
+0x045e     0x00f5
+0x045e     0x00f7
+0x0471     0x0327
+0x0471     0x0328
+0x0c45     0x6001
+0x0c45     0x6005
+0x0c45     0x6007
+0x0c45     0x6009
+0x0c45     0x600d
+0x0c45     0x6011
+0x0c45     0x6019
+0x0c45     0x6024
+0x0c45     0x6025
+0x0c45     0x6028
+0x0c45     0x6029
+0x0c45     0x602a
+0x0c45     0x602b
+0x0c45     0x602c
+0x0c45     0x602d
+0x0c45     0x602e
+0x0c45     0x6030
+0x0c45     0x603f
+0x0c45     0x6080
+0x0c45     0x6082
+0x0c45     0x6083
+0x0c45     0x6088
+0x0c45     0x608a
+0x0c45     0x608b
+0x0c45     0x608c
+0x0c45     0x608e
+0x0c45     0x608f
+0x0c45     0x60a0
+0x0c45     0x60a2
+0x0c45     0x60a3
+0x0c45     0x60a8
+0x0c45     0x60aa
+0x0c45     0x60ab
+0x0c45     0x60ac
+0x0c45     0x60ae
+0x0c45     0x60af
+0x0c45     0x60b0
+0x0c45     0x60b2
+0x0c45     0x60b3
+0x0c45     0x60b8
+0x0c45     0x60ba
+0x0c45     0x60bb
+0x0c45     0x60bc
+0x0c45     0x60be
+0x0c45     0x60c0
+0x0c45     0x60c2
+0x0c45     0x60c8
+0x0c45     0x60cc
+0x0c45     0x60ea
+0x0c45     0x60ec
+0x0c45     0x60ef
+0x0c45     0x60fa
+0x0c45     0x60fb
+0x0c45     0x60fc
+0x0c45     0x60fe
+0x0c45     0x6102
+0x0c45     0x6108
+0x0c45     0x610f
+0x0c45     0x6130
+0x0c45     0x6138
+0x0c45     0x613a
+0x0c45     0x613b
+0x0c45     0x613c
+0x0c45     0x613e
+
+The list above does not imply that all those devices work with this driver: up
+until now only the ones that assemble the following pairs of SN9C1xx bridges
+and image sensors are supported; kernel messages will always tell you whether
+this is the case (see "Module loading" paragraph):
+
+Image sensor / SN9C1xx bridge      | SN9C10[12]  SN9C103  SN9C105  SN9C120
+-------------------------------------------------------------------------------
+HV7131D    Hynix Semiconductor     | Yes         No       No       No
+HV7131R    Hynix Semiconductor     | No          Yes      Yes      Yes
+MI-0343    Micron Technology       | Yes         No       No       No
+MI-0360    Micron Technology       | No          Yes      Yes      Yes
+OV7630     OmniVision Technologies | Yes         Yes      Yes      Yes
+OV7660     OmniVision Technologies | No          No       Yes      Yes
+PAS106B    PixArt Imaging          | Yes         No       No       No
+PAS202B    PixArt Imaging          | Yes         Yes      No       No
+TAS5110C1B Taiwan Advanced Sensor  | Yes         No       No       No
+TAS5110D   Taiwan Advanced Sensor  | Yes         No       No       No
+TAS5130D1B Taiwan Advanced Sensor  | Yes         No       No       No
+
+"Yes" means that the pair is supported by the driver, while "No" means that the
+pair does not exist or is not supported by the driver.
+
+Only some of the available control settings of each image sensor are supported
+through the V4L2 interface.
+
+Donations of new models for further testing and support would be much
+appreciated. Non-available hardware will not be supported by the author of this
+driver.
+
+
+10. Notes for V4L2 application developers
+=========================================
+This driver follows the V4L2 API specifications. In particular, it enforces two
+rules:
+
+- exactly one I/O method, either "mmap" or "read", is associated with each
+file descriptor. Once it is selected, the application must close and reopen the
+device to switch to the other I/O method;
+
+- although it is not mandatory, previously mapped buffer memory should always
+be unmapped before calling any "VIDIOC_S_CROP" or "VIDIOC_S_FMT" ioctl's.
+The same number of buffers as before will be allocated again to match the size
+of the new video frames, so you have to map the buffers again before any I/O
+attempts on them.
+
+Consistently with the hardware limits, this driver also supports image
+downscaling with arbitrary scaling factors from 1, 2 and 4 in both directions.
+However, the V4L2 API specifications don't correctly define how the scaling
+factor can be chosen arbitrarily by the "negotiation" of the "source" and
+"target" rectangles. To work around this flaw, we have added the convention
+that, during the negotiation, whenever the "VIDIOC_S_CROP" ioctl is issued, the
+scaling factor is restored to 1.
+
+This driver supports two different video formats: the first one is the "8-bit
+Sequential Bayer" format and can be used to obtain uncompressed video data
+from the device through the current I/O method, while the second one provides
+either "raw" compressed video data (without frame headers not related to the
+compressed data) or standard JPEG (with frame headers). The compression quality
+may vary from 0 to 1 and can be selected or queried thanks to the
+VIDIOC_S_JPEGCOMP and VIDIOC_G_JPEGCOMP V4L2 ioctl's. For maximum flexibility,
+both the default active video format and the default compression quality
+depend on how the image sensor being used is initialized.
+
+
+11. Video frame formats [1]
+=======================
+The SN9C1xx PC Camera Controllers can send images in two possible video
+formats over the USB: either native "Sequential RGB Bayer" or compressed.
+The compression is used to achieve high frame rates. With regard to the
+SN9C101, SN9C102 and SN9C103, the compression is based on the Huffman encoding
+algorithm described below, while with regard to the SN9C105 and SN9C120 the
+compression is based on the JPEG standard.
+The current video format may be selected or queried from the user application
+by calling the VIDIOC_S_FMT or VIDIOC_G_FMT ioctl's, as described in the V4L2
+API specifications.
+
+The name "Sequential Bayer" indicates the organization of the red, green and
+blue pixels in one video frame. Each pixel is associated with a 8-bit long
+value and is disposed in memory according to the pattern shown below:
+
+B[0]   G[1]    B[2]    G[3]    ...   B[m-2]         G[m-1]
+G[m]   R[m+1]  G[m+2]  R[m+2]  ...   G[2m-2]        R[2m-1]
+...
+...                                  B[(n-1)(m-2)]  G[(n-1)(m-1)]
+...                                  G[n(m-2)]      R[n(m-1)]
+
+The above matrix also represents the sequential or progressive read-out mode of
+the (n, m) Bayer color filter array used in many CCD or CMOS image sensors.
+
+The Huffman compressed video frame consists of a bitstream that encodes for
+every R, G, or B pixel the difference between the value of the pixel itself and
+some reference pixel value. Pixels are organised in the Bayer pattern and the
+Bayer sub-pixels are tracked individually and alternatingly. For example, in
+the first line values for the B and G1 pixels are alternatingly encoded, while
+in the second line values for the G2 and R pixels are alternatingly encoded.
+
+The pixel reference value is calculated as follows:
+- the 4 top left pixels are encoded in raw uncompressed 8-bit format;
+- the value in the top two rows is the value of the pixel left of the current
+  pixel;
+- the value in the left column is the value of the pixel above the current
+  pixel;
+- for all other pixels, the reference value is the average of the value of the
+  pixel on the left and the value of the pixel above the current pixel;
+- there is one code in the bitstream that specifies the value of a pixel
+  directly (in 4-bit resolution);
+- pixel values need to be clamped inside the range [0..255] for proper
+  decoding.
+
+The algorithm purely describes the conversion from compressed Bayer code used
+in the SN9C101, SN9C102 and SN9C103 chips to uncompressed Bayer. Additional
+steps are required to convert this to a color image (i.e. a color interpolation
+algorithm).
+
+The following Huffman codes have been found:
+0: +0 (relative to reference pixel value)
+100: +4
+101: -4?
+1110xxxx: set absolute value to xxxx.0000
+1101: +11
+1111: -11
+11001: +20
+110000: -20
+110001: ??? - these codes are apparently not used
+
+[1] The Huffman compression algorithm has been reverse-engineered and
+    documented by Bertrik Sikken.
+
+
+12. Contact information
+=======================
+The author may be contacted by e-mail at <luca.risolia@studio.unibo.it>.
+
+GPG/PGP encrypted e-mail's are accepted. The GPG key ID of the author is
+'FCE635A4'; the public 1024-bit key should be available at any keyserver;
+the fingerprint is: '88E8 F32F 7244 68BA 3958  5D40 99DA 5D2A FCE6 35A4'.
+
+
+13. Credits
+===========
+Many thanks to following persons for their contribute (listed in alphabetical
+order):
+
+- David Anderson for the donation of a webcam;
+- Luca Capello for the donation of a webcam;
+- Philippe Coval for having helped testing the PAS202BCA image sensor;
+- Joao Rodrigo Fuzaro, Joao Limirio, Claudio Filho and Caio Begotti for the
+  donation of a webcam;
+- Dennis Heitmann for the donation of a webcam;
+- Jon Hollstrom for the donation of a webcam;
+- Nick McGill for the donation of a webcam;
+- Carlos Eduardo Medaglia Dyonisio, who added the support for the PAS202BCB
+  image sensor;
+- Stefano Mozzi, who donated 45 EU;
+- Andrew Pearce for the donation of a webcam;
+- John Pullan for the donation of a webcam;
+- Bertrik Sikken, who reverse-engineered and documented the Huffman compression
+  algorithm used in the SN9C101, SN9C102 and SN9C103 controllers and
+  implemented the first decoder;
+- Ronny Standke for the donation of a webcam;
+- Mizuno Takafumi for the donation of a webcam;
+- an "anonymous" donator (who didn't want his name to be revealed) for the
+  donation of a webcam.
+- an anonymous donator for the donation of four webcams and two boards with ten
+  image sensors.
diff --git a/drivers/media/usb/sn9c102/sn9c102_config.h b/drivers/staging/media/sn9c102/sn9c102_config.h
index 0f4e0378b071..0f4e0378b071 100644
--- a/drivers/media/usb/sn9c102/sn9c102_config.h
+++ b/drivers/staging/media/sn9c102/sn9c102_config.h
diff --git a/drivers/media/usb/sn9c102/sn9c102_core.c b/drivers/staging/media/sn9c102/sn9c102_core.c
index 2cb44de2b92c..2cb44de2b92c 100644
--- a/drivers/media/usb/sn9c102/sn9c102_core.c
+++ b/drivers/staging/media/sn9c102/sn9c102_core.c
diff --git a/drivers/media/usb/sn9c102/sn9c102_devtable.h b/drivers/staging/media/sn9c102/sn9c102_devtable.h
index b3d2cc729657..b3d2cc729657 100644
--- a/drivers/media/usb/sn9c102/sn9c102_devtable.h
+++ b/drivers/staging/media/sn9c102/sn9c102_devtable.h
diff --git a/drivers/media/usb/sn9c102/sn9c102_hv7131d.c b/drivers/staging/media/sn9c102/sn9c102_hv7131d.c
index 2dce5c908c8e..2dce5c908c8e 100644
--- a/drivers/media/usb/sn9c102/sn9c102_hv7131d.c
+++ b/drivers/staging/media/sn9c102/sn9c102_hv7131d.c
diff --git a/drivers/media/usb/sn9c102/sn9c102_hv7131r.c b/drivers/staging/media/sn9c102/sn9c102_hv7131r.c
index 4295887ff609..4295887ff609 100644
--- a/drivers/media/usb/sn9c102/sn9c102_hv7131r.c
+++ b/drivers/staging/media/sn9c102/sn9c102_hv7131r.c
diff --git a/drivers/media/usb/sn9c102/sn9c102_mi0343.c b/drivers/staging/media/sn9c102/sn9c102_mi0343.c
index 1f5b09bec89c..1f5b09bec89c 100644
--- a/drivers/media/usb/sn9c102/sn9c102_mi0343.c
+++ b/drivers/staging/media/sn9c102/sn9c102_mi0343.c
diff --git a/drivers/media/usb/sn9c102/sn9c102_mi0360.c b/drivers/staging/media/sn9c102/sn9c102_mi0360.c
index d973fc1973d9..d973fc1973d9 100644
--- a/drivers/media/usb/sn9c102/sn9c102_mi0360.c
+++ b/drivers/staging/media/sn9c102/sn9c102_mi0360.c
diff --git a/drivers/media/usb/sn9c102/sn9c102_mt9v111.c b/drivers/staging/media/sn9c102/sn9c102_mt9v111.c
index 95986eb492e4..95986eb492e4 100644
--- a/drivers/media/usb/sn9c102/sn9c102_mt9v111.c
+++ b/drivers/staging/media/sn9c102/sn9c102_mt9v111.c
diff --git a/drivers/media/usb/sn9c102/sn9c102_ov7630.c b/drivers/staging/media/sn9c102/sn9c102_ov7630.c
index 803712c29f02..803712c29f02 100644
--- a/drivers/media/usb/sn9c102/sn9c102_ov7630.c
+++ b/drivers/staging/media/sn9c102/sn9c102_ov7630.c
diff --git a/drivers/media/usb/sn9c102/sn9c102_ov7660.c b/drivers/staging/media/sn9c102/sn9c102_ov7660.c
index 7977795d342b..7977795d342b 100644
--- a/drivers/media/usb/sn9c102/sn9c102_ov7660.c
+++ b/drivers/staging/media/sn9c102/sn9c102_ov7660.c
diff --git a/drivers/media/usb/sn9c102/sn9c102_pas106b.c b/drivers/staging/media/sn9c102/sn9c102_pas106b.c
index 81cd969c1b7b..81cd969c1b7b 100644
--- a/drivers/media/usb/sn9c102/sn9c102_pas106b.c
+++ b/drivers/staging/media/sn9c102/sn9c102_pas106b.c
diff --git a/drivers/media/usb/sn9c102/sn9c102_pas202bcb.c b/drivers/staging/media/sn9c102/sn9c102_pas202bcb.c
index 2e86fdc86989..2e86fdc86989 100644
--- a/drivers/media/usb/sn9c102/sn9c102_pas202bcb.c
+++ b/drivers/staging/media/sn9c102/sn9c102_pas202bcb.c
diff --git a/drivers/media/usb/sn9c102/sn9c102_sensor.h b/drivers/staging/media/sn9c102/sn9c102_sensor.h
index 3679970dba2c..3679970dba2c 100644
--- a/drivers/media/usb/sn9c102/sn9c102_sensor.h
+++ b/drivers/staging/media/sn9c102/sn9c102_sensor.h
diff --git a/drivers/media/usb/sn9c102/sn9c102_tas5110c1b.c b/drivers/staging/media/sn9c102/sn9c102_tas5110c1b.c
index 04cdfdde8564..04cdfdde8564 100644
--- a/drivers/media/usb/sn9c102/sn9c102_tas5110c1b.c
+++ b/drivers/staging/media/sn9c102/sn9c102_tas5110c1b.c
diff --git a/drivers/media/usb/sn9c102/sn9c102_tas5110d.c b/drivers/staging/media/sn9c102/sn9c102_tas5110d.c
index 9372e6f9fcff..9372e6f9fcff 100644
--- a/drivers/media/usb/sn9c102/sn9c102_tas5110d.c
+++ b/drivers/staging/media/sn9c102/sn9c102_tas5110d.c
diff --git a/drivers/media/usb/sn9c102/sn9c102_tas5130d1b.c b/drivers/staging/media/sn9c102/sn9c102_tas5130d1b.c
index a30bbc4389f5..a30bbc4389f5 100644
--- a/drivers/media/usb/sn9c102/sn9c102_tas5130d1b.c
+++ b/drivers/staging/media/sn9c102/sn9c102_tas5130d1b.c
diff --git a/drivers/staging/media/solo6x10/solo6x10-v4l2-enc.c b/drivers/staging/media/solo6x10/solo6x10-v4l2-enc.c
index d582c5b84c14..ce9e5aaf7fd4 100644
--- a/drivers/staging/media/solo6x10/solo6x10-v4l2-enc.c
+++ b/drivers/staging/media/solo6x10/solo6x10-v4l2-enc.c
@@ -964,7 +964,7 @@ static int solo_enc_s_std(struct file *file, void *priv, v4l2_std_id std)
 {
 	struct solo_enc_dev *solo_enc = video_drvdata(file);
 
-	return solo_set_video_type(solo_enc->solo_dev, std & V4L2_STD_PAL);
+	return solo_set_video_type(solo_enc->solo_dev, std & V4L2_STD_625_50);
 }
 
 static int solo_enum_framesizes(struct file *file, void *priv,
diff --git a/drivers/staging/media/solo6x10/solo6x10-v4l2.c b/drivers/staging/media/solo6x10/solo6x10-v4l2.c
index 7b26de3488da..47e72dac9b13 100644
--- a/drivers/staging/media/solo6x10/solo6x10-v4l2.c
+++ b/drivers/staging/media/solo6x10/solo6x10-v4l2.c
@@ -527,7 +527,7 @@ static int solo_g_std(struct file *file, void *priv, v4l2_std_id *i)
 	return 0;
 }
 
-int solo_set_video_type(struct solo_dev *solo_dev, bool type)
+int solo_set_video_type(struct solo_dev *solo_dev, bool is_50hz)
 {
 	int i;
 
@@ -537,7 +537,8 @@ int solo_set_video_type(struct solo_dev *solo_dev, bool type)
 	for (i = 0; i < solo_dev->nr_chans; i++)
 		if (vb2_is_busy(&solo_dev->v4l2_enc[i]->vidq))
 			return -EBUSY;
-	solo_dev->video_type = type;
+	solo_dev->video_type = is_50hz ? SOLO_VO_FMT_TYPE_PAL :
+					 SOLO_VO_FMT_TYPE_NTSC;
 	/* Reconfigure for the new standard */
 	solo_disp_init(solo_dev);
 	solo_enc_init(solo_dev);
@@ -551,7 +552,7 @@ static int solo_s_std(struct file *file, void *priv, v4l2_std_id std)
 {
 	struct solo_dev *solo_dev = video_drvdata(file);
 
-	return solo_set_video_type(solo_dev, std & V4L2_STD_PAL);
+	return solo_set_video_type(solo_dev, std & V4L2_STD_625_50);
 }
 
 static int solo_s_ctrl(struct v4l2_ctrl *ctrl)
diff --git a/drivers/staging/media/solo6x10/solo6x10.h b/drivers/staging/media/solo6x10/solo6x10.h
index f1bbb8cb74e6..8964f8be158e 100644
--- a/drivers/staging/media/solo6x10/solo6x10.h
+++ b/drivers/staging/media/solo6x10/solo6x10.h
@@ -398,7 +398,7 @@ int solo_p2m_dma_desc(struct solo_dev *solo_dev,
 		      int desc_cnt);
 
 /* Global s_std ioctl */
-int solo_set_video_type(struct solo_dev *solo_dev, bool type);
+int solo_set_video_type(struct solo_dev *solo_dev, bool is_50hz);
 void solo_update_mode(struct solo_enc_dev *solo_enc);
 
 /* Set the threshold for motion detection */
diff --git a/drivers/staging/rtl8821ae/Kconfig b/drivers/staging/rtl8821ae/Kconfig
new file mode 100644
index 000000000000..2aa5dac2f1df
--- /dev/null
+++ b/drivers/staging/rtl8821ae/Kconfig
@@ -0,0 +1,11 @@
+config R8821AE
+	tristate "RealTek RTL8821AE Wireless LAN NIC driver"
+	depends on PCI && WLAN
+	depends on m
+	select WIRELESS_EXT
+	select WEXT_PRIV
+	select EEPROM_93CX6
+	select CRYPTO
+	default N
+	---help---
+	  If built as a module, it will be called r8821ae.ko.
diff --git a/drivers/staging/rtl8821ae/Makefile b/drivers/staging/rtl8821ae/Makefile
new file mode 100644
index 000000000000..8a23bd7e8842
--- /dev/null
+++ b/drivers/staging/rtl8821ae/Makefile
@@ -0,0 +1,35 @@
+PCI_MAIN_OBJS	:= base.o	\
+		rc.o	\
+		debug.o	\
+		regd.o	\
+		efuse.o	\
+		cam.o	\
+		ps.o	\
+		core.o	\
+		stats.o	\
+		pci.o	\
+
+BT_COEXIST_OBJS:=	btcoexist/halbtc8192e2ant.o\
+			btcoexist/halbtc8723b1ant.o\
+			btcoexist/halbtc8723b2ant.o\
+			btcoexist/halbtcoutsrc.o\
+			btcoexist/rtl_btc.o	\
+
+PCI_8821AE_HAL_OBJS:=	\
+	rtl8821ae/hw.o		\
+	rtl8821ae/table.o		\
+	rtl8821ae/sw.o		\
+	rtl8821ae/trx.o		\
+	rtl8821ae/led.o		\
+	rtl8821ae/fw.o		\
+	rtl8821ae/phy.o		\
+	rtl8821ae/rf.o		\
+	rtl8821ae/dm.o		\
+	rtl8821ae/pwrseq.o	\
+	rtl8821ae/pwrseqcmd.o	\
+	rtl8821ae/hal_btc.o	\
+	rtl8821ae/hal_bt_coexist.o	\
+
+rtl8821ae-objs += $(BT_COEXIST_OBJS) $(PCI_MAIN_OBJS) $(PCI_8821AE_HAL_OBJS)
+
+obj-$(CONFIG_R8821AE) += rtl8821ae.o
diff --git a/drivers/staging/rtl8821ae/TODO b/drivers/staging/rtl8821ae/TODO
new file mode 100644
index 000000000000..3ee7529d4ed5
--- /dev/null
+++ b/drivers/staging/rtl8821ae/TODO
@@ -0,0 +1,10 @@
+Realtek 8821AE PCI wifi driver TODO:
+  - remove built-in btcoexist module when the "real" one gets upstream
+  - remove built-in rtlwifi code by porting driver to use the "real" one
+    in the drivers/net/ directory.
+  - fix up coding style issues
+
+Please send any patches for this driver to:
+	Greg Kroah-Hartman <gregkh@linuxfoundation.org>
+and the <devel@driverdev.osuosl.org> mailing list.
+
diff --git a/drivers/staging/rtl8821ae/base.c b/drivers/staging/rtl8821ae/base.c
new file mode 100644
index 000000000000..18c936fbdf1e
--- /dev/null
+++ b/drivers/staging/rtl8821ae/base.c
@@ -0,0 +1,1873 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include <linux/ip.h>
+#include <linux/module.h>
+#include "wifi.h"
+#include "rc.h"
+#include "base.h"
+#include "efuse.h"
+#include "cam.h"
+#include "ps.h"
+#include "regd.h"
+#include "pci.h"
+
+/*
+ *NOTICE!!!: This file will be very big, we 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 rtl_tid_to_ac(struct ieee80211_hw *hw, u8 tid)
+{
+	return tid_to_ac[tid];
+}
+
+static void _rtl_init_hw_ht_capab(struct ieee80211_hw *hw,
+				  struct ieee80211_sta_ht_cap *ht_cap)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	ht_cap->ht_supported = true;
+	ht_cap->cap = IEEE80211_HT_CAP_SUP_WIDTH_20_40 |
+	    IEEE80211_HT_CAP_SGI_40 |
+	    IEEE80211_HT_CAP_SGI_20 |
+	    IEEE80211_HT_CAP_DSSSCCK40 | IEEE80211_HT_CAP_MAX_AMSDU;
+
+	if (rtlpriv->rtlhal.disable_amsdu_8k)
+		ht_cap->cap &= ~IEEE80211_HT_CAP_MAX_AMSDU;
+
+	/*
+	 *Maximum length of AMPDU that the STA can receive.
+	 *Length = 2 ^ (13 + max_ampdu_length_exp) - 1 (octets)
+	 */
+	ht_cap->ampdu_factor = IEEE80211_HT_MAX_AMPDU_64K;
+
+	/*Minimum MPDU start spacing , */
+	ht_cap->ampdu_density = IEEE80211_HT_MPDU_DENSITY_16;
+
+	ht_cap->mcs.tx_params = IEEE80211_HT_MCS_TX_DEFINED;
+
+	/*
+	 *hw->wiphy->bands[IEEE80211_BAND_2GHZ]
+	 *base on ant_num
+	 *rx_mask: RX mask
+	 *if rx_ant =1 rx_mask[0]=0xff;==>MCS0-MCS7
+	 *if rx_ant =2 rx_mask[1]=0xff;==>MCS8-MCS15
+	 *if rx_ant >=3 rx_mask[2]=0xff;
+	 *if BW_40 rx_mask[4]=0x01;
+	 *highest supported RX rate
+	 */
+	if (rtlpriv->dm.supp_phymode_switch) {
+		RT_TRACE(COMP_INIT, DBG_EMERG, ("Support phy mode switch\n"));
+
+		ht_cap->mcs.rx_mask[0] = 0xFF;
+		ht_cap->mcs.rx_mask[1] = 0xFF;
+		ht_cap->mcs.rx_mask[4] = 0x01;
+
+		ht_cap->mcs.rx_highest = MAX_BIT_RATE_40MHZ_MCS15;
+	} else {
+		if (get_rf_type(rtlphy) == RF_1T2R ||
+				get_rf_type(rtlphy) == RF_2T2R) {
+
+			RT_TRACE(COMP_INIT, DBG_DMESG, ("1T2R or 2T2R\n"));
+
+			ht_cap->mcs.rx_mask[0] = 0xFF;
+			ht_cap->mcs.rx_mask[1] = 0xFF;
+			ht_cap->mcs.rx_mask[4] = 0x01;
+
+			ht_cap->mcs.rx_highest = MAX_BIT_RATE_40MHZ_MCS15;
+		} else if (get_rf_type(rtlphy) == RF_1T1R) {
+
+			RT_TRACE(COMP_INIT, DBG_DMESG, ("1T1R\n"));
+
+			ht_cap->mcs.rx_mask[0] = 0xFF;
+			ht_cap->mcs.rx_mask[1] = 0x00;
+			ht_cap->mcs.rx_mask[4] = 0x01;
+
+			ht_cap->mcs.rx_highest = MAX_BIT_RATE_40MHZ_MCS7;
+		}
+	}
+}
+
+static void _rtl_init_mac80211(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	struct rtl_mac *rtlmac = rtl_mac(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct ieee80211_supported_band *sband;
+
+
+	if (rtlhal->macphymode == SINGLEMAC_SINGLEPHY &&
+	    rtlhal->bandset == BAND_ON_BOTH) {
+		/* 1: 2.4 G bands */
+		/* <1> use  mac->bands as mem for hw->wiphy->bands */
+		sband = &(rtlmac->bands[IEEE80211_BAND_2GHZ]);
+
+		/* <2> set hw->wiphy->bands[IEEE80211_BAND_2GHZ]
+		 * to default value(1T1R) */
+		memcpy(&(rtlmac->bands[IEEE80211_BAND_2GHZ]), &rtl_band_2ghz,
+				sizeof(struct ieee80211_supported_band));
+
+		/* <3> init ht cap base on ant_num */
+		_rtl_init_hw_ht_capab(hw, &sband->ht_cap);
+
+		/* <4> set mac->sband to wiphy->sband */
+		hw->wiphy->bands[IEEE80211_BAND_2GHZ] = sband;
+
+		/* 2: 5 G bands */
+		/* <1> use  mac->bands as mem for hw->wiphy->bands */
+		sband = &(rtlmac->bands[IEEE80211_BAND_5GHZ]);
+
+		/* <2> set hw->wiphy->bands[IEEE80211_BAND_5GHZ]
+		 * to default value(1T1R) */
+		memcpy(&(rtlmac->bands[IEEE80211_BAND_5GHZ]), &rtl_band_5ghz,
+				sizeof(struct ieee80211_supported_band));
+
+		/* <3> init ht cap base on ant_num */
+		_rtl_init_hw_ht_capab(hw, &sband->ht_cap);
+
+		/* <4> set mac->sband to wiphy->sband */
+		hw->wiphy->bands[IEEE80211_BAND_5GHZ] = sband;
+	} else {
+		if (rtlhal->current_bandtype == BAND_ON_2_4G) {
+			/* <1> use  mac->bands as mem for hw->wiphy->bands */
+			sband = &(rtlmac->bands[IEEE80211_BAND_2GHZ]);
+
+			/* <2> set hw->wiphy->bands[IEEE80211_BAND_2GHZ]
+			 * to default value(1T1R) */
+			memcpy(&(rtlmac->bands[IEEE80211_BAND_2GHZ]),
+			       &rtl_band_2ghz,
+			       sizeof(struct ieee80211_supported_band));
+
+			/* <3> init ht cap base on ant_num */
+			_rtl_init_hw_ht_capab(hw, &sband->ht_cap);
+
+			/* <4> set mac->sband to wiphy->sband */
+			hw->wiphy->bands[IEEE80211_BAND_2GHZ] = sband;
+		} else if (rtlhal->current_bandtype == BAND_ON_5G) {
+			/* <1> use  mac->bands as mem for hw->wiphy->bands */
+			sband = &(rtlmac->bands[IEEE80211_BAND_5GHZ]);
+
+			/* <2> set hw->wiphy->bands[IEEE80211_BAND_5GHZ]
+			 * to default value(1T1R) */
+			memcpy(&(rtlmac->bands[IEEE80211_BAND_5GHZ]),
+			       &rtl_band_5ghz,
+			       sizeof(struct ieee80211_supported_band));
+
+			/* <3> init ht cap base on ant_num */
+			_rtl_init_hw_ht_capab(hw, &sband->ht_cap);
+
+			/* <4> set mac->sband to wiphy->sband */
+			hw->wiphy->bands[IEEE80211_BAND_5GHZ] = sband;
+		} else {
+			RT_TRACE(COMP_INIT, DBG_EMERG, ("Err BAND %d\n",
+				 rtlhal->current_bandtype));
+		}
+	}
+	/* <5> set hw caps */
+	hw->flags = IEEE80211_HW_SIGNAL_DBM |
+	    IEEE80211_HW_RX_INCLUDES_FCS |
+#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 |
+	    /* 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>*/
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,37))
+	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);
+#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;
+
+	hw->queues = AC_MAX;
+	hw->extra_tx_headroom = RTL_TX_HEADER_SIZE;
+
+	/* TODO: Correct this value for our hw */
+	/* TODO: define these hard code value */
+	hw->max_listen_interval = 10;
+	hw->max_rate_tries = 4;
+	/* hw->max_rates = 1; */
+	hw->sta_data_size = sizeof(struct rtl_sta_info);
+#ifdef VIF_TODO
+	hw->vif_data_size = sizeof(struct rtl_vif_info);
+#endif
+
+	/* <6> mac address */
+	if (is_valid_ether_addr(rtlefuse->dev_addr)) {
+		SET_IEEE80211_PERM_ADDR(hw, rtlefuse->dev_addr);
+	} else {
+		u8 rtlmac[] = { 0x00, 0xe0, 0x4c, 0x81, 0x92, 0x00 };
+		get_random_bytes((rtlmac + (ETH_ALEN - 1)), 1);
+		SET_IEEE80211_PERM_ADDR(hw, rtlmac);
+	}
+
+}
+
+static 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,
+		    rtl_watch_dog_timer_callback, (unsigned long)hw);
+	init_timer(&rtlpriv->works.dualmac_easyconcurrent_retrytimer);
+	setup_timer(&rtlpriv->works.dualmac_easyconcurrent_retrytimer,
+		    rtl_easy_concurrent_retrytimer_callback, (unsigned long)hw);
+	/* <2> work queue */
+	rtlpriv->works.hw = hw;
+/*<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>*/
+	INIT_DELAYED_WORK(&rtlpriv->works.watchdog_wq,
+			  (void *)rtl_watchdog_wq_callback);
+	INIT_DELAYED_WORK(&rtlpriv->works.ips_nic_off_wq,
+			  (void *)rtl_ips_nic_off_wq_callback);
+	INIT_DELAYED_WORK(&rtlpriv->works.ps_work,
+			  (void *)rtl_swlps_wq_callback);
+	INIT_DELAYED_WORK(&rtlpriv->works.ps_rfon_wq,
+			  (void *)rtl_swlps_rfon_wq_callback);
+	INIT_DELAYED_WORK(&rtlpriv->works.fwevt_wq,
+			  (void *)rtl_fwevt_wq_callback);
+
+}
+
+void rtl_deinit_deferred_work(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	del_timer_sync(&rtlpriv->works.watchdog_timer);
+
+	cancel_delayed_work(&rtlpriv->works.watchdog_wq);
+	cancel_delayed_work(&rtlpriv->works.ips_nic_off_wq);
+	cancel_delayed_work(&rtlpriv->works.ps_work);
+	cancel_delayed_work(&rtlpriv->works.ps_rfon_wq);
+	cancel_delayed_work(&rtlpriv->works.fwevt_wq);
+}
+
+void rtl_init_rfkill(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	bool radio_state;
+	bool blocked;
+	u8 valid = 0;
+
+	/*set init state to on */
+	rtlpriv->rfkill.rfkill_state = 1;
+	wiphy_rfkill_set_hw_state(hw->wiphy, 0);
+
+	radio_state = rtlpriv->cfg->ops->radio_onoff_checking(hw, &valid);
+
+	if (valid) {
+		printk(KERN_INFO "rtlwifi: wireless switch is %s\n",
+		       rtlpriv->rfkill.rfkill_state ? "on" : "off");
+
+		rtlpriv->rfkill.rfkill_state = radio_state;
+
+		blocked = (rtlpriv->rfkill.rfkill_state == 1) ? 0 : 1;
+		wiphy_rfkill_set_hw_state(hw->wiphy, blocked);
+	}
+
+	wiphy_rfkill_start_polling(hw->wiphy);
+}
+
+void rtl_deinit_rfkill(struct ieee80211_hw *hw)
+{
+	wiphy_rfkill_stop_polling(hw->wiphy);
+}
+
+#ifdef VIF_TODO
+static void rtl_init_vif(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	INIT_LIST_HEAD(&rtlpriv->vif_priv.vif_list);
+
+	rtlpriv->vif_priv.vifs = 0;
+}
+#endif
+
+int rtl_init_core(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *rtlmac = rtl_mac(rtl_priv(hw));
+
+	/* <1> init mac80211 */
+	_rtl_init_mac80211(hw);
+	rtlmac->hw = hw;
+	rtlmac->link_state = MAC80211_NOLINK;
+
+	/* <2> rate control register */
+	hw->rate_control_algorithm = "rtl_rc";
+
+	/*
+	 * <3> init CRDA must come after init
+	 * mac80211 hw  in _rtl_init_mac80211.
+	 */
+	if (rtl_regd_init(hw, rtl_reg_notifier)) {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("REGD init failed\n"));
+		return 1;
+	}
+
+	/* <4> locks */
+	mutex_init(&rtlpriv->locks.conf_mutex);
+	spin_lock_init(&rtlpriv->locks.ips_lock);
+	spin_lock_init(&rtlpriv->locks.irq_th_lock);
+	spin_lock_init(&rtlpriv->locks.h2c_lock);
+	spin_lock_init(&rtlpriv->locks.rf_ps_lock);
+	spin_lock_init(&rtlpriv->locks.rf_lock);
+	spin_lock_init(&rtlpriv->locks.lps_lock);
+	spin_lock_init(&rtlpriv->locks.waitq_lock);
+	spin_lock_init(&rtlpriv->locks.entry_list_lock);
+	spin_lock_init(&rtlpriv->locks.cck_and_rw_pagea_lock);
+	spin_lock_init(&rtlpriv->locks.check_sendpkt_lock);
+	spin_lock_init(&rtlpriv->locks.fw_ps_lock);
+	spin_lock_init(&rtlpriv->locks.iqk_lock);
+	/* <5> init list */
+	INIT_LIST_HEAD(&rtlpriv->entry_list);
+
+	/* <6> init deferred work */
+	_rtl_init_deferred_work(hw);
+
+	/* <7> */
+#ifdef VIF_TODO
+	rtl_init_vif(hw);
+#endif
+
+	return 0;
+}
+
+void rtl_deinit_core(struct ieee80211_hw *hw)
+{
+}
+
+void rtl_init_rx_config(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_RCR, (u8 *) (&mac->rx_conf));
+}
+
+/*********************************************************
+ *
+ * tx information functions
+ *
+ *********************************************************/
+static void _rtl_qurey_shortpreamble_mode(struct ieee80211_hw *hw,
+					  struct rtl_tcb_desc *tcb_desc,
+					  struct ieee80211_tx_info *info)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 rate_flag = info->control.rates[0].flags;
+
+	tcb_desc->use_shortpreamble = false;
+
+	/* 1M can only use Long Preamble. 11B spec */
+	if (tcb_desc->hw_rate == rtlpriv->cfg->maps[RTL_RC_CCK_RATE1M])
+		return;
+	else if (rate_flag & IEEE80211_TX_RC_USE_SHORT_PREAMBLE)
+		tcb_desc->use_shortpreamble = true;
+
+	return;
+}
+
+static void _rtl_query_shortgi(struct ieee80211_hw *hw,
+			       struct ieee80211_sta *sta,
+			       struct rtl_tcb_desc *tcb_desc,
+			       struct ieee80211_tx_info *info)
+{
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u8 rate_flag = info->control.rates[0].flags;
+	u8 sgi_40 = 0, sgi_20 = 0, bw_40 = 0;
+	tcb_desc->use_shortgi = false;
+
+	if (sta == NULL)
+		return;
+
+	sgi_40 = sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40;
+	sgi_20 = sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20;
+
+	if (!(sta->ht_cap.ht_supported))
+		return;
+
+	if (!sgi_40 && !sgi_20)
+		return;
+
+	if (mac->opmode == NL80211_IFTYPE_STATION)
+		bw_40 = mac->bw_40;
+	else if (mac->opmode == NL80211_IFTYPE_AP ||
+		 mac->opmode == NL80211_IFTYPE_ADHOC ||
+		 mac->opmode == NL80211_IFTYPE_MESH_POINT)
+		bw_40 = sta->ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40;
+
+	if ((bw_40 == true) && sgi_40)
+		tcb_desc->use_shortgi = true;
+	else if ((bw_40 == false) && sgi_20)
+		tcb_desc->use_shortgi = true;
+
+	if (!(rate_flag & IEEE80211_TX_RC_SHORT_GI))
+		tcb_desc->use_shortgi = false;
+}
+
+static void _rtl_query_protection_mode(struct ieee80211_hw *hw,
+				       struct rtl_tcb_desc *tcb_desc,
+				       struct ieee80211_tx_info *info)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 rate_flag = info->control.rates[0].flags;
+
+	/* Common Settings */
+	tcb_desc->b_rts_stbc = false;
+	tcb_desc->b_cts_enable = false;
+	tcb_desc->rts_sc = 0;
+	tcb_desc->b_rts_bw = false;
+	tcb_desc->b_rts_use_shortpreamble = false;
+	tcb_desc->b_rts_use_shortgi = false;
+
+	if (rate_flag & IEEE80211_TX_RC_USE_CTS_PROTECT) {
+		/* Use CTS-to-SELF in protection mode. */
+		tcb_desc->b_rts_enable = true;
+		tcb_desc->b_cts_enable = true;
+		tcb_desc->rts_rate = rtlpriv->cfg->maps[RTL_RC_OFDM_RATE24M];
+	} else if (rate_flag & IEEE80211_TX_RC_USE_RTS_CTS) {
+		/* Use RTS-CTS in protection mode. */
+		tcb_desc->b_rts_enable = true;
+		tcb_desc->rts_rate = rtlpriv->cfg->maps[RTL_RC_OFDM_RATE24M];
+	}
+}
+
+static void _rtl_txrate_selectmode(struct ieee80211_hw *hw,
+				   struct ieee80211_sta *sta,
+				   struct rtl_tcb_desc *tcb_desc)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_sta_info *sta_entry = NULL;
+	u8 ratr_index = 7;
+
+	if (sta) {
+		sta_entry = (struct rtl_sta_info *) sta->drv_priv;
+		ratr_index = sta_entry->ratr_index;
+	}
+	if (!tcb_desc->disable_ratefallback || !tcb_desc->use_driver_rate) {
+		if (mac->opmode == NL80211_IFTYPE_STATION) {
+			tcb_desc->ratr_index = 0;
+		} else if (mac->opmode == NL80211_IFTYPE_ADHOC ||
+				mac->opmode == NL80211_IFTYPE_MESH_POINT) {
+			if (tcb_desc->b_multicast || tcb_desc->b_broadcast) {
+				tcb_desc->hw_rate =
+				    rtlpriv->cfg->maps[RTL_RC_CCK_RATE2M];
+				tcb_desc->use_driver_rate = 1;
+				tcb_desc->ratr_index = RATR_INX_WIRELESS_MC;
+			} else {
+				tcb_desc->ratr_index = ratr_index;
+			}
+		} else if (mac->opmode == NL80211_IFTYPE_AP) {
+			tcb_desc->ratr_index = ratr_index;
+		}
+	}
+
+	if (rtlpriv->dm.b_useramask) {
+		tcb_desc->ratr_index = ratr_index;
+		/* TODO we will differentiate adhoc and station futrue  */
+		if (mac->opmode == NL80211_IFTYPE_STATION ||
+			mac->opmode == NL80211_IFTYPE_MESH_POINT) {
+			tcb_desc->mac_id = 0;
+
+			if (mac->mode == WIRELESS_MODE_N_24G) {
+				tcb_desc->ratr_index = RATR_INX_WIRELESS_NGB;
+			} else if (mac->mode == WIRELESS_MODE_N_5G) {
+				tcb_desc->ratr_index = RATR_INX_WIRELESS_NG;
+			} else if (mac->mode & WIRELESS_MODE_G) {
+				tcb_desc->ratr_index = RATR_INX_WIRELESS_GB;
+			} else if (mac->mode & WIRELESS_MODE_B) {
+				tcb_desc->ratr_index = RATR_INX_WIRELESS_B;
+			} else if (mac->mode & WIRELESS_MODE_A) {
+				tcb_desc->ratr_index = RATR_INX_WIRELESS_G;
+			}
+		} else if (mac->opmode == NL80211_IFTYPE_AP ||
+			mac->opmode == NL80211_IFTYPE_ADHOC) {
+			if (NULL != sta) {
+				if (sta->aid > 0) {
+					tcb_desc->mac_id = sta->aid + 1;
+				} else {
+					tcb_desc->mac_id = 1;
+				}
+			} else {
+				tcb_desc->mac_id = 0;
+			}
+		}
+	}
+}
+
+static void _rtl_query_bandwidth_mode(struct ieee80211_hw *hw,
+				      struct ieee80211_sta *sta,
+				      struct rtl_tcb_desc *tcb_desc)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+
+	tcb_desc->b_packet_bw = false;
+	if (!sta)
+		return;
+	if (mac->opmode == NL80211_IFTYPE_AP ||
+		mac->opmode == NL80211_IFTYPE_ADHOC ||
+		mac->opmode == NL80211_IFTYPE_MESH_POINT) {
+		if (!(sta->ht_cap.ht_supported) ||
+			!(sta->ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40))
+			return;
+	} else if (mac->opmode == NL80211_IFTYPE_STATION) {
+		if (!mac->bw_40 || !(sta->ht_cap.ht_supported))
+		return;
+	}
+	if (tcb_desc->b_multicast || tcb_desc->b_broadcast)
+		return;
+
+	/*use legency rate, shall use 20MHz */
+	if (tcb_desc->hw_rate <= rtlpriv->cfg->maps[RTL_RC_OFDM_RATE54M])
+		return;
+
+	tcb_desc->b_packet_bw = true;
+}
+
+static u8 _rtl_get_highest_n_rate(struct ieee80211_hw *hw,
+				  struct ieee80211_sta *sta)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 hw_rate;
+
+	if ((get_rf_type(rtlphy) == RF_2T2R) && (sta->ht_cap.mcs.rx_mask[1]!=0))
+		hw_rate = rtlpriv->cfg->maps[RTL_RC_HT_RATEMCS15];
+	else
+		hw_rate = rtlpriv->cfg->maps[RTL_RC_HT_RATEMCS7];
+
+	return hw_rate;
+}
+
+void rtl_get_tcb_desc(struct ieee80211_hw *hw,
+		      struct ieee80211_tx_info *info,
+		      struct ieee80211_sta *sta,
+		      struct sk_buff *skb, struct rtl_tcb_desc *tcb_desc)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *rtlmac = rtl_mac(rtl_priv(hw));
+	struct ieee80211_hdr *hdr = rtl_get_hdr(skb);
+	struct ieee80211_rate *txrate;
+	u16 fc = rtl_get_fc(skb);
+
+	txrate = ieee80211_get_tx_rate(hw, info);
+	if (txrate != NULL)
+		tcb_desc->hw_rate = txrate->hw_value;
+
+	if (ieee80211_is_data(fc)) {
+		/*
+		 *we set data rate INX 0
+		 *in rtl_rc.c   if skb is special data or
+		 *mgt which need low data rate.
+		 */
+
+		/*
+		 *So tcb_desc->hw_rate is just used for
+		 *special data and mgt frames
+		 */
+		if (info->control.rates[0].idx == 0 ||
+				ieee80211_is_nullfunc(fc)) {
+			tcb_desc->use_driver_rate = true;
+			tcb_desc->ratr_index = RATR_INX_WIRELESS_MC;
+
+			tcb_desc->disable_ratefallback = 1;
+		} else {
+			/*
+			 *because hw will 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->ht_cap.ht_supported)) {
+				tcb_desc->hw_rate = _rtl_get_highest_n_rate(hw, sta);
+			} else {
+				if(rtlmac->mode == WIRELESS_MODE_B) {
+					tcb_desc->hw_rate =
+					    rtlpriv->cfg->maps[RTL_RC_CCK_RATE11M];
+				} else {
+					tcb_desc->hw_rate =
+					    rtlpriv->cfg->maps[RTL_RC_OFDM_RATE54M];
+				}
+			}
+		}
+
+		if (is_multicast_ether_addr(ieee80211_get_DA(hdr)))
+			tcb_desc->b_multicast = 1;
+		else if (is_broadcast_ether_addr(ieee80211_get_DA(hdr)))
+			tcb_desc->b_broadcast = 1;
+
+		_rtl_txrate_selectmode(hw, sta, tcb_desc);
+		_rtl_query_bandwidth_mode(hw, sta, tcb_desc);
+		_rtl_qurey_shortpreamble_mode(hw, tcb_desc, info);
+		_rtl_query_shortgi(hw, sta, tcb_desc, info);
+		_rtl_query_protection_mode(hw, tcb_desc, info);
+	} else {
+		tcb_desc->use_driver_rate = true;
+		tcb_desc->ratr_index = RATR_INX_WIRELESS_MC;
+		tcb_desc->disable_ratefallback = 1;
+		tcb_desc->mac_id = 0;
+		tcb_desc->b_packet_bw = false;
+	}
+}
+//EXPORT_SYMBOL(rtl_get_tcb_desc);
+
+bool rtl_tx_mgmt_proc(struct ieee80211_hw *hw, struct sk_buff *skb)
+{
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u16 fc = rtl_get_fc(skb);
+
+	if (rtlpriv->dm.supp_phymode_switch &&
+		mac->link_state < MAC80211_LINKED &&
+		(ieee80211_is_auth(fc) || ieee80211_is_probe_req(fc))) {
+		if (rtlpriv->cfg->ops->check_switch_to_dmdp)
+			rtlpriv->cfg->ops->check_switch_to_dmdp(hw);
+	}
+	if (ieee80211_is_auth(fc)) {
+		RT_TRACE(COMP_SEND, DBG_DMESG, ("MAC80211_LINKING\n"));
+		rtl_ips_nic_on(hw);
+
+		mac->link_state = MAC80211_LINKING;
+		/* Dul mac */
+		rtlpriv->phy.b_need_iqk = true;
+
+	}
+
+	return true;
+}
+
+struct sk_buff *rtl_make_del_ba(struct ieee80211_hw *hw, u8 *sa,
+				u8 *bssid, u16 tid);
+bool rtl_action_proc(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx)
+{
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct ieee80211_hdr *hdr = rtl_get_hdr(skb);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u16 fc = rtl_get_fc(skb);
+	u8 *act = (u8 *) (((u8 *) skb->data + MAC80211_3ADDR_LEN));
+	u8 category;
+
+	if (!ieee80211_is_action(fc))
+		return true;
+
+	category = *act;
+	act++;
+	switch (category) {
+	case ACT_CAT_BA:
+		switch (*act) {
+		case ACT_ADDBAREQ:
+			if (mac->act_scanning)
+				return false;
+
+			RT_TRACE((COMP_SEND | COMP_RECV), DBG_DMESG,
+				 ("%s ACT_ADDBAREQ From :%pM\n",
+				  is_tx ? "Tx" : "Rx", hdr->addr2));
+			RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_DMESG, ("req \n"),
+		      	skb->data, skb->len);
+			if (!is_tx) {
+				struct ieee80211_sta *sta = NULL;
+				struct rtl_sta_info *sta_entry = NULL;
+				struct ieee80211_mgmt *mgmt = (void *)skb->data;
+				u16 capab = 0, tid = 0;
+				struct rtl_tid_data *tid_data;
+				struct sk_buff *skb_delba = NULL;
+				struct ieee80211_rx_status rx_status = { 0 };
+
+				rcu_read_lock();
+				sta = rtl_find_sta(hw, hdr->addr3);
+				if (sta == NULL) {
+					RT_TRACE((COMP_SEND | COMP_RECV),
+						 DBG_EMERG, ("sta is NULL\n"));
+					rcu_read_unlock();
+					return true;
+				}
+
+				sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+				if (!sta_entry) {
+					rcu_read_unlock();
+					return true;
+				}
+				capab = le16_to_cpu(mgmt->u.action.u.addba_req.capab);
+				tid = (capab & IEEE80211_ADDBA_PARAM_TID_MASK) >> 2;
+				tid_data = &sta_entry->tids[tid];
+				if (tid_data->agg.rx_agg_state ==
+				    RTL_RX_AGG_START) {
+					skb_delba = rtl_make_del_ba(hw,
+								    hdr->addr2,
+								    hdr->addr3,
+								    tid);
+					if (skb_delba) {
+#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;
+						rx_status.signal = 50 + 10;
+						memcpy(IEEE80211_SKB_RXCB(skb_delba), &rx_status,
+								sizeof(rx_status));
+						RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_DMESG,
+								("fake del\n"), skb_delba->data,
+								skb_delba->len);
+						ieee80211_rx_irqsafe(hw, skb_delba);
+					}
+				}
+				rcu_read_unlock();
+			}
+			break;
+		case ACT_ADDBARSP:
+			RT_TRACE((COMP_SEND | COMP_RECV), DBG_DMESG,
+				 ("%s ACT_ADDBARSP From :%pM\n",
+				  is_tx ? "Tx" : "Rx", hdr->addr2));
+			break;
+		case ACT_DELBA:
+			RT_TRACE((COMP_SEND | COMP_RECV), DBG_DMESG,
+				 ("ACT_ADDBADEL From :%pM\n", hdr->addr2));
+			break;
+		}
+		break;
+	default:
+		break;
+	}
+
+	return true;
+}
+
+/*should call before software enc*/
+u8 rtl_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	u16 fc = rtl_get_fc(skb);
+	u16 ether_type;
+	u8 mac_hdr_len = ieee80211_get_hdrlen_from_skb(skb);
+	const struct iphdr *ip;
+
+	if (!ieee80211_is_data(fc))
+		goto end;
+
+
+	ip = (struct iphdr *)((u8 *) skb->data + mac_hdr_len +
+			      SNAP_SIZE + PROTOC_TYPE_SIZE);
+	ether_type = *(u16 *) ((u8 *) skb->data + mac_hdr_len + SNAP_SIZE);
+	ether_type = ntohs(ether_type);
+
+	if (ETH_P_IP == ether_type) {
+		if (IPPROTO_UDP == ip->protocol) {
+			struct udphdr *udp = (struct udphdr *)((u8 *) ip +
+							       (ip->ihl << 2));
+			if (((((u8 *) udp)[1] == 68) &&
+			     (((u8 *) udp)[3] == 67)) ||
+			    ((((u8 *) udp)[1] == 67) &&
+			     (((u8 *) udp)[3] == 68))) {
+				/*
+				 * 68 : UDP BOOTP client
+				 * 67 : UDP BOOTP server
+				 */
+				RT_TRACE((COMP_SEND | COMP_RECV),
+					 DBG_DMESG, ("dhcp %s !!\n",
+						     (is_tx) ? "Tx" : "Rx"));
+
+				if (is_tx) {
+					rtlpriv->ra.is_special_data = true;
+					rtl_lps_leave(hw);
+					ppsc->last_delaylps_stamp_jiffies =
+									jiffies;
+				}
+
+				return true;
+			}
+		}
+	} else if (ETH_P_ARP == ether_type) {
+		if (is_tx) {
+			rtlpriv->ra.is_special_data = true;
+			rtl_lps_leave(hw);
+			ppsc->last_delaylps_stamp_jiffies = jiffies;
+		}
+
+		return true;
+	} else if (ETH_P_PAE == ether_type) {
+		RT_TRACE((COMP_SEND | COMP_RECV), DBG_DMESG,
+			 ("802.1X %s EAPOL pkt!!\n", (is_tx) ? "Tx" : "Rx"));
+
+		if (is_tx) {
+			rtlpriv->ra.is_special_data = true;
+			rtl_lps_leave(hw);
+			ppsc->last_delaylps_stamp_jiffies = jiffies;
+		}
+
+		return true;
+	} else if (0x86DD == ether_type) {
+		return true;
+	}
+
+end:
+	rtlpriv->ra.is_special_data = false;
+	return false;
+}
+
+/*********************************************************
+ *
+ * functions called by core.c
+ *
+ *********************************************************/
+int rtl_tx_agg_start(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		     struct ieee80211_sta *sta, u16 tid, u16 *ssn)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_tid_data *tid_data;
+	struct rtl_sta_info *sta_entry = NULL;
+
+	if (sta == NULL)
+		return -EINVAL;
+
+	if (unlikely(tid >= MAX_TID_COUNT))
+		return -EINVAL;
+
+	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+	if (!sta_entry)
+		return -ENXIO;
+	tid_data = &sta_entry->tids[tid];
+
+	RT_TRACE(COMP_SEND, DBG_DMESG,
+		 ("on ra = %pM tid = %d seq:%d\n", sta->addr, tid,
+		  tid_data->seq_number));
+
+	*ssn = tid_data->seq_number;
+	tid_data->agg.agg_state = RTL_AGG_START;
+
+	ieee80211_start_tx_ba_cb_irqsafe(vif, sta->addr, tid);
+	return 0;
+}
+
+int rtl_tx_agg_stop(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		    struct ieee80211_sta *sta, u16 tid)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_tid_data *tid_data;
+	struct rtl_sta_info *sta_entry = NULL;
+
+	if (sta == NULL)
+		return -EINVAL;
+
+	if (!sta->addr) {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("ra = NULL\n"));
+		return -EINVAL;
+	}
+
+	RT_TRACE(COMP_SEND, DBG_DMESG,
+		 ("on ra = %pM tid = %d\n", sta->addr, tid));
+
+	if (unlikely(tid >= MAX_TID_COUNT))
+		return -EINVAL;
+
+	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+	tid_data = &sta_entry->tids[tid];
+	sta_entry->tids[tid].agg.agg_state = RTL_AGG_STOP;
+
+	ieee80211_stop_tx_ba_cb_irqsafe(vif, sta->addr, tid);
+	return 0;
+}
+
+int rtl_rx_agg_start(struct ieee80211_hw *hw,
+		     struct ieee80211_sta *sta, u16 tid)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_tid_data *tid_data;
+	struct rtl_sta_info *sta_entry = NULL;
+
+	if (sta == NULL)
+		return -EINVAL;
+
+	if (unlikely(tid >= MAX_TID_COUNT))
+		return -EINVAL;
+
+	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+	if (!sta_entry)
+		return -ENXIO;
+	tid_data = &sta_entry->tids[tid];
+
+	RT_TRACE(COMP_RECV, DBG_DMESG,
+		 ("on ra = %pM tid = %d seq:%d\n", sta->addr, tid,
+		 tid_data->seq_number));
+
+	tid_data->agg.rx_agg_state = RTL_RX_AGG_START;
+	return 0;
+}
+
+int rtl_rx_agg_stop(struct ieee80211_hw *hw,
+		struct ieee80211_sta *sta, u16 tid)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_tid_data *tid_data;
+	struct rtl_sta_info *sta_entry = NULL;
+
+	if (sta == NULL)
+		return -EINVAL;
+
+	if (!sta->addr) {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("ra = NULL\n"));
+		return -EINVAL;
+	}
+
+	RT_TRACE(COMP_SEND, DBG_DMESG,
+		 ("on ra = %pM tid = %d\n", sta->addr, tid));
+
+	if (unlikely(tid >= MAX_TID_COUNT))
+		return -EINVAL;
+
+	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+	tid_data = &sta_entry->tids[tid];
+	sta_entry->tids[tid].agg.rx_agg_state = RTL_RX_AGG_STOP;
+
+	return 0;
+}
+int rtl_tx_agg_oper(struct ieee80211_hw *hw,
+		struct ieee80211_sta *sta, u16 tid)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_tid_data *tid_data;
+	struct rtl_sta_info *sta_entry = NULL;
+
+	if (sta == NULL)
+		return -EINVAL;
+
+	if (!sta->addr) {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("ra = NULL\n"));
+		return -EINVAL;
+	}
+
+	RT_TRACE(COMP_SEND, DBG_DMESG,
+		 ("on ra = %pM tid = %d\n", sta->addr, tid));
+
+	if (unlikely(tid >= MAX_TID_COUNT))
+		return -EINVAL;
+
+	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+	tid_data = &sta_entry->tids[tid];
+	sta_entry->tids[tid].agg.agg_state = RTL_AGG_OPERATIONAL;
+
+	return 0;
+}
+
+/*********************************************************
+ *
+ * wq & timer callback functions
+ *
+ *********************************************************/
+/* this function is used for roaming */
+void rtl_beacon_statistic(struct ieee80211_hw *hw, struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+
+	if (rtlpriv->mac80211.opmode != NL80211_IFTYPE_STATION)
+		return;
+
+	if (rtlpriv->mac80211.link_state < MAC80211_LINKED)
+		return;
+
+	/* check if this really is a beacon */
+	if (!ieee80211_is_beacon(hdr->frame_control) &&
+		!ieee80211_is_probe_resp(hdr->frame_control))
+		return;
+
+	/* min. beacon length + FCS_LEN */
+	if (skb->len <= 40 + FCS_LEN)
+		return;
+
+	/* and only beacons from the associated BSSID, please */
+	if (ether_addr_equal(hdr->addr3, rtlpriv->mac80211.bssid))
+		return;
+
+	rtlpriv->link_info.bcn_rx_inperiod ++;
+}
+
+void rtl_watchdog_wq_callback(void *data)
+{
+	struct rtl_works *rtlworks = container_of_dwork_rtl(data,
+							    struct rtl_works,
+							    watchdog_wq);
+	struct ieee80211_hw *hw = rtlworks->hw;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	bool b_busytraffic = false;
+	bool b_tx_busy_traffic = false;
+	bool b_rx_busy_traffic = false;
+	bool b_higher_busytraffic = false;
+	bool b_higher_busyrxtraffic = false;
+	u8 idx, tid;
+	u32 rx_cnt_inp4eriod = 0;
+	u32 tx_cnt_inp4eriod = 0;
+	u32 aver_rx_cnt_inperiod = 0;
+	u32 aver_tx_cnt_inperiod = 0;
+	u32 aver_tidtx_inperiod[MAX_TID_COUNT] = {0};
+	u32 tidtx_inp4eriod[MAX_TID_COUNT] = {0};
+	bool benter_ps = false;
+
+	if (is_hal_stop(rtlhal))
+		return;
+
+	/* <1> Determine if action frame is allowed */
+	if (mac->link_state > MAC80211_NOLINK) {
+		if (mac->cnt_after_linked < 20)
+			mac->cnt_after_linked++;
+	} else {
+		mac->cnt_after_linked = 0;
+	}
+
+	/* <2> to check if traffic busy, if
+	 * busytraffic we don't change channel */
+	if (mac->link_state >= MAC80211_LINKED) {
+
+		/* (1) get aver_rx_cnt_inperiod & aver_tx_cnt_inperiod */
+		for (idx = 0; idx <= 2; idx++) {
+			rtlpriv->link_info.num_rx_in4period[idx] =
+			    rtlpriv->link_info.num_rx_in4period[idx + 1];
+			rtlpriv->link_info.num_tx_in4period[idx] =
+			    rtlpriv->link_info.num_tx_in4period[idx + 1];
+		}
+		rtlpriv->link_info.num_rx_in4period[3] =
+		    rtlpriv->link_info.num_rx_inperiod;
+		rtlpriv->link_info.num_tx_in4period[3] =
+		    rtlpriv->link_info.num_tx_inperiod;
+		for (idx = 0; idx <= 3; idx++) {
+			rx_cnt_inp4eriod +=
+			    rtlpriv->link_info.num_rx_in4period[idx];
+			tx_cnt_inp4eriod +=
+			    rtlpriv->link_info.num_tx_in4period[idx];
+		}
+		aver_rx_cnt_inperiod = rx_cnt_inp4eriod / 4;
+		aver_tx_cnt_inperiod = tx_cnt_inp4eriod / 4;
+
+		/* (2) check traffic busy */
+		if (aver_rx_cnt_inperiod > 100 || aver_tx_cnt_inperiod > 100) {
+			b_busytraffic = true;
+			if (aver_rx_cnt_inperiod > aver_tx_cnt_inperiod)
+				b_rx_busy_traffic = true;
+			else
+				b_tx_busy_traffic = false;
+		}
+
+		/* Higher Tx/Rx data. */
+		if (aver_rx_cnt_inperiod > 4000 ||
+			aver_tx_cnt_inperiod > 4000) {
+			b_higher_busytraffic = true;
+
+			/* Extremely high Rx data. */
+			if (aver_rx_cnt_inperiod > 5000)
+				b_higher_busyrxtraffic = true;
+		}
+
+		/* check every tid's tx traffic */
+		for (tid = 0; tid <= 7; tid++) {
+			for (idx = 0; idx <= 2; idx++)
+				rtlpriv->link_info.tidtx_in4period[tid][idx] =
+					rtlpriv->link_info.tidtx_in4period[tid]
+					[idx + 1];
+			rtlpriv->link_info.tidtx_in4period[tid][3] =
+				rtlpriv->link_info.tidtx_inperiod[tid];
+
+			for (idx = 0; idx <= 3; idx++)
+				tidtx_inp4eriod[tid] +=
+				   rtlpriv->link_info.tidtx_in4period[tid][idx];
+			aver_tidtx_inperiod[tid] = tidtx_inp4eriod[tid] / 4;
+			if (aver_tidtx_inperiod[tid] > 5000)
+				rtlpriv->link_info.higher_busytxtraffic[tid] =
+									true;
+			else
+				rtlpriv->link_info.higher_busytxtraffic[tid] =
+									false;
+		}
+
+		if (((rtlpriv->link_info.num_rx_inperiod +
+		      rtlpriv->link_info.num_tx_inperiod) > 8) ||
+		    (rtlpriv->link_info.num_rx_inperiod > 2))
+			benter_ps = false;
+		else
+			benter_ps = true;
+
+		/* LeisurePS only work in infra mode. */
+		if (benter_ps)
+			rtl_lps_enter(hw);
+		else
+			rtl_lps_leave(hw);
+	}
+
+	rtlpriv->link_info.num_rx_inperiod = 0;
+	rtlpriv->link_info.num_tx_inperiod = 0;
+	for (tid = 0; tid <= 7; tid++)
+		rtlpriv->link_info.tidtx_inperiod[tid] = 0;
+
+	rtlpriv->link_info.b_busytraffic = b_busytraffic;
+	rtlpriv->link_info.b_rx_busy_traffic = b_rx_busy_traffic;
+	rtlpriv->link_info.b_tx_busy_traffic = b_tx_busy_traffic;
+	rtlpriv->link_info.b_higher_busytraffic = b_higher_busytraffic;
+	rtlpriv->link_info.b_higher_busyrxtraffic = b_higher_busyrxtraffic;
+
+	/* <3> DM */
+	rtlpriv->cfg->ops->dm_watchdog(hw);
+
+	/* <4> roaming */
+	if (mac->link_state == MAC80211_LINKED &&
+			mac->opmode == NL80211_IFTYPE_STATION) {
+		if ((rtlpriv->link_info.bcn_rx_inperiod +
+			rtlpriv->link_info.num_rx_inperiod) == 0) {
+			rtlpriv->link_info.roam_times++;
+			RT_TRACE(COMP_ERR, DBG_DMESG, ("AP off for %d s\n",
+				(rtlpriv->link_info.roam_times * 2)));
+
+			/* if we can't recv beacon for 10s,
+			 * we should reconnect this AP */
+			if (rtlpriv->link_info.roam_times >= 5) {
+				RT_TRACE(COMP_ERR, DBG_EMERG,
+					 ("AP off, try to reconnect now\n"));
+				rtlpriv->link_info.roam_times = 0;
+				ieee80211_connection_loss(rtlpriv->mac80211.vif);
+			}
+		} else {
+			rtlpriv->link_info.roam_times = 0;
+		}
+	}
+	rtlpriv->link_info.bcn_rx_inperiod = 0;
+}
+
+void rtl_watch_dog_timer_callback(unsigned long data)
+{
+	struct ieee80211_hw *hw = (struct ieee80211_hw *)data;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	queue_delayed_work(rtlpriv->works.rtl_wq,
+			   &rtlpriv->works.watchdog_wq, 0);
+
+	mod_timer(&rtlpriv->works.watchdog_timer,
+		  jiffies + MSECS(RTL_WATCH_DOG_TIME));
+}
+void rtl_fwevt_wq_callback(void *data)
+{
+	struct rtl_works *rtlworks =
+		container_of_dwork_rtl(data, struct rtl_works, fwevt_wq);
+	struct ieee80211_hw *hw = rtlworks->hw;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->cfg->ops->c2h_command_handle(hw);
+}
+void rtl_easy_concurrent_retrytimer_callback(unsigned long data)
+{
+	struct ieee80211_hw *hw = (struct ieee80211_hw *)data;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_priv *buddy_priv = rtlpriv->buddy_priv;
+
+	if(buddy_priv == NULL)
+		return;
+
+	rtlpriv->cfg->ops->dualmac_easy_concurrent(hw);
+}
+/*********************************************************
+ *
+ * frame process functions
+ *
+ *********************************************************/
+u8 *rtl_find_ie(u8 *data, unsigned int len, u8 ie)
+{
+	struct ieee80211_mgmt *mgmt = (void *)data;
+	u8 *pos, *end;
+
+	pos = (u8 *)mgmt->u.beacon.variable;
+	end = data + len;
+	while (pos < end) {
+		if (pos + 2 + pos[1] > end)
+			return NULL;
+
+		if (pos[0] == ie)
+			return pos;
+
+		pos += 2 + pos[1];
+	}
+	return NULL;
+}
+
+/* when we use 2 rx ants we send IEEE80211_SMPS_OFF */
+/* when we use 1 rx ant we send IEEE80211_SMPS_STATIC */
+struct sk_buff *rtl_make_smps_action(struct ieee80211_hw *hw,
+				     enum ieee80211_smps_mode smps,
+				     u8 *da, u8 *bssid)
+{
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct sk_buff *skb;
+	struct ieee80211_mgmt_compat *action_frame;
+
+	/* 27 = header + category + action + smps mode */
+	skb = dev_alloc_skb(27 + hw->extra_tx_headroom);
+	if (!skb)
+		return NULL;
+
+	skb_reserve(skb, hw->extra_tx_headroom);
+	action_frame = (void *)skb_put(skb, 27);
+	memset(action_frame, 0, 27);
+	memcpy(action_frame->da, da, ETH_ALEN);
+	memcpy(action_frame->sa, rtlefuse->dev_addr, ETH_ALEN);
+	memcpy(action_frame->bssid, bssid, ETH_ALEN);
+	action_frame->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+						  IEEE80211_STYPE_ACTION);
+	action_frame->u.action.category = WLAN_CATEGORY_HT;
+	action_frame->u.action.u.ht_smps.action = WLAN_HT_ACTION_SMPS;
+	switch (smps) {
+	case IEEE80211_SMPS_AUTOMATIC:/* 0 */
+	case IEEE80211_SMPS_NUM_MODES:/* 4 */
+		WARN_ON(1);
+	case IEEE80211_SMPS_OFF:/* 1 */ /*MIMO_PS_NOLIMIT*/
+		action_frame->u.action.u.ht_smps.smps_control =
+				WLAN_HT_SMPS_CONTROL_DISABLED;/* 0 */
+		break;
+	case IEEE80211_SMPS_STATIC:/* 2 */ /*MIMO_PS_STATIC*/
+		action_frame->u.action.u.ht_smps.smps_control =
+				WLAN_HT_SMPS_CONTROL_STATIC;/* 1 */
+		break;
+	case IEEE80211_SMPS_DYNAMIC:/* 3 */ /*MIMO_PS_DYNAMIC*/
+		action_frame->u.action.u.ht_smps.smps_control =
+				WLAN_HT_SMPS_CONTROL_DYNAMIC;/* 3 */
+		break;
+	}
+
+	return skb;
+}
+
+int rtl_send_smps_action(struct ieee80211_hw *hw,
+			 struct ieee80211_sta *sta,
+			 enum ieee80211_smps_mode smps)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct sk_buff *skb = NULL;
+	struct rtl_tcb_desc tcb_desc;
+	u8 bssid[ETH_ALEN] = {0};
+
+	memset(&tcb_desc, 0, sizeof(struct rtl_tcb_desc));
+
+	if (rtlpriv->mac80211.act_scanning)
+		goto err_free;
+
+	if (!sta)
+		goto err_free;
+
+	if (unlikely(is_hal_stop(rtlhal) || ppsc->rfpwr_state != ERFON))
+		goto err_free;
+
+	if (!test_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status))
+		goto err_free;
+
+	if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_AP)
+		memcpy(bssid, rtlpriv->efuse.dev_addr, ETH_ALEN);
+	else
+		memcpy(bssid, rtlpriv->mac80211.bssid, ETH_ALEN);
+
+	skb = rtl_make_smps_action(hw, smps, sta->addr, bssid);
+	/* this is a type = mgmt * stype = action frame */
+	if (skb) {
+		struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+		struct rtl_sta_info *sta_entry =
+			(struct rtl_sta_info *) sta->drv_priv;
+		sta_entry->mimo_ps = smps;
+		/* rtlpriv->cfg->ops->update_rate_tbl(hw, sta, 0); */
+
+		info->control.rates[0].idx = 0;
+#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;
+
+err_free:
+	return 0;
+}
+//EXPORT_SYMBOL(rtl_send_smps_action);
+
+/* because mac80211 have issues when can receive del ba
+ * so here we just make a fake del_ba if we receive a ba_req
+ * but rx_agg was opened to let mac80211 release some ba
+ * related resources, so please this del_ba for tx */
+struct sk_buff *rtl_make_del_ba(struct ieee80211_hw *hw,
+				u8 *sa, u8 *bssid, u16 tid)
+{
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct sk_buff *skb;
+	struct ieee80211_mgmt *action_frame;
+	u16 params;
+
+	/* 27 = header + category + action + smps mode */
+	skb = dev_alloc_skb(34 + hw->extra_tx_headroom);
+	if (!skb)
+		return NULL;
+
+	skb_reserve(skb, hw->extra_tx_headroom);
+	action_frame = (void *)skb_put(skb, 34);
+	memset(action_frame, 0, 34);
+	memcpy(action_frame->sa, sa, ETH_ALEN);
+	memcpy(action_frame->da, rtlefuse->dev_addr, ETH_ALEN);
+	memcpy(action_frame->bssid, bssid, ETH_ALEN);
+	action_frame->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+						  IEEE80211_STYPE_ACTION);
+	action_frame->u.action.category = WLAN_CATEGORY_BACK;
+	action_frame->u.action.u.delba.action_code = WLAN_ACTION_DELBA;
+	params = (u16)(1 << 11); 	/* bit 11 initiator */
+	params |= (u16)(tid << 12); 		/* bit 15:12 TID number */
+
+	action_frame->u.action.u.delba.params = cpu_to_le16(params);
+	action_frame->u.action.u.delba.reason_code =
+		cpu_to_le16(WLAN_REASON_QSTA_TIMEOUT);
+
+	return skb;
+}
+
+/*********************************************************
+ *
+ * IOT functions
+ *
+ *********************************************************/
+static bool rtl_chk_vendor_ouisub(struct ieee80211_hw *hw,
+				  struct octet_string vendor_ie)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	bool matched = false;
+	static u8 athcap_1[] = { 0x00, 0x03, 0x7F };
+	static u8 athcap_2[] = { 0x00, 0x13, 0x74 };
+	static u8 broadcap_1[] = { 0x00, 0x10, 0x18 };
+	static u8 broadcap_2[] = { 0x00, 0x0a, 0xf7 };
+	static u8 broadcap_3[] = { 0x00, 0x05, 0xb5 };
+	static u8 racap[] = { 0x00, 0x0c, 0x43 };
+	static u8 ciscocap[] = { 0x00, 0x40, 0x96 };
+	static u8 marvcap[] = { 0x00, 0x50, 0x43 };
+
+	if (memcmp(vendor_ie.octet, athcap_1, 3) == 0 ||
+		memcmp(vendor_ie.octet, athcap_2, 3) == 0) {
+		rtlpriv->mac80211.vendor = PEER_ATH;
+		matched = true;
+	} else if (memcmp(vendor_ie.octet, broadcap_1, 3) == 0 ||
+		memcmp(vendor_ie.octet, broadcap_2, 3) == 0 ||
+		memcmp(vendor_ie.octet, broadcap_3, 3) == 0) {
+		rtlpriv->mac80211.vendor = PEER_BROAD;
+		matched = true;
+	} else if (memcmp(vendor_ie.octet, racap, 3) == 0) {
+		rtlpriv->mac80211.vendor = PEER_RAL;
+		matched = true;
+	} else if (memcmp(vendor_ie.octet, ciscocap, 3) == 0) {
+		rtlpriv->mac80211.vendor = PEER_CISCO;
+		matched = true;
+	} else if (memcmp(vendor_ie.octet, marvcap, 3) == 0) {
+		rtlpriv->mac80211.vendor = PEER_MARV;
+		matched = true;
+	}
+
+	return matched;
+}
+
+bool rtl_find_221_ie(struct ieee80211_hw *hw, u8 *data,
+		unsigned int len)
+{
+	struct ieee80211_mgmt *mgmt = (void *)data;
+	struct octet_string vendor_ie;
+	u8 *pos, *end;
+
+	pos = (u8 *)mgmt->u.beacon.variable;
+	end = data + len;
+	while (pos < end) {
+		if (pos[0] == 221) {
+			vendor_ie.length = pos[1];
+			vendor_ie.octet = &pos[2];
+			if (rtl_chk_vendor_ouisub(hw, vendor_ie))
+				return true;
+		}
+
+		if (pos + 2 + pos[1] > end)
+			return false;
+
+		pos += 2 + pos[1];
+	}
+	return false;
+}
+
+void rtl_recognize_peer(struct ieee80211_hw *hw, u8 *data, unsigned int len)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct ieee80211_hdr *hdr = (void *)data;
+	u32 vendor = PEER_UNKNOWN;
+
+	static u8 ap3_1[3] = { 0x00, 0x14, 0xbf };
+	static u8 ap3_2[3] = { 0x00, 0x1a, 0x70 };
+	static u8 ap3_3[3] = { 0x00, 0x1d, 0x7e };
+	static u8 ap4_1[3] = { 0x00, 0x90, 0xcc };
+	static u8 ap4_2[3] = { 0x00, 0x0e, 0x2e };
+	static u8 ap4_3[3] = { 0x00, 0x18, 0x02 };
+	static u8 ap4_4[3] = { 0x00, 0x17, 0x3f };
+	static u8 ap4_5[3] = { 0x00, 0x1c, 0xdf };
+	static u8 ap5_1[3] = { 0x00, 0x1c, 0xf0 };
+	static u8 ap5_2[3] = { 0x00, 0x21, 0x91 };
+	static u8 ap5_3[3] = { 0x00, 0x24, 0x01 };
+	static u8 ap5_4[3] = { 0x00, 0x15, 0xe9 };
+	static u8 ap5_5[3] = { 0x00, 0x17, 0x9A };
+	static u8 ap5_6[3] = { 0x00, 0x18, 0xE7 };
+	static u8 ap6_1[3] = { 0x00, 0x17, 0x94 };
+	static u8 ap7_1[3] = { 0x00, 0x14, 0xa4 };
+
+	if (mac->opmode != NL80211_IFTYPE_STATION)
+		return;
+
+	if (mac->link_state == MAC80211_NOLINK) {
+		mac->vendor = PEER_UNKNOWN;
+		return;
+	}
+
+	if (mac->cnt_after_linked > 2)
+		return;
+
+	/* check if this really is a beacon */
+	if (!ieee80211_is_beacon(hdr->frame_control))
+		return;
+
+	/* min. beacon length + FCS_LEN */
+	if (len <= 40 + FCS_LEN)
+		return;
+
+	/* and only beacons from the associated BSSID, please */
+	if (ether_addr_equal(hdr->addr3, rtlpriv->mac80211.bssid))
+		return;
+
+	if (rtl_find_221_ie(hw, data, len)) {
+		vendor = mac->vendor;
+	}
+
+	if ((memcmp(mac->bssid, ap5_1, 3) == 0) ||
+		(memcmp(mac->bssid, ap5_2, 3) == 0) ||
+		(memcmp(mac->bssid, ap5_3, 3) == 0) ||
+		(memcmp(mac->bssid, ap5_4, 3) == 0) ||
+		(memcmp(mac->bssid, ap5_5, 3) == 0) ||
+		(memcmp(mac->bssid, ap5_6, 3) == 0) ||
+		vendor == PEER_ATH) {
+		vendor = PEER_ATH;
+		RT_TRACE(COMP_MAC80211, DBG_LOUD, ("=>ath find\n"));
+	} else if ((memcmp(mac->bssid, ap4_4, 3) == 0) ||
+		(memcmp(mac->bssid, ap4_5, 3) == 0) ||
+		(memcmp(mac->bssid, ap4_1, 3) == 0) ||
+		(memcmp(mac->bssid, ap4_2, 3) == 0) ||
+		(memcmp(mac->bssid, ap4_3, 3) == 0) ||
+		vendor == PEER_RAL) {
+		RT_TRACE(COMP_MAC80211, DBG_LOUD, ("=>ral 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
+ *
+ *********************************************************/
+static ssize_t rtl_show_debug_level(struct device *d,
+				    struct device_attribute *attr, char *buf)
+{
+	struct ieee80211_hw *hw = dev_get_drvdata(d);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	return sprintf(buf, "0x%08X\n", rtlpriv->dbg.global_debuglevel);
+}
+
+static ssize_t rtl_store_debug_level(struct device *d,
+				     struct device_attribute *attr,
+				     const char *buf, size_t count)
+{
+	struct ieee80211_hw *hw = dev_get_drvdata(d);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	unsigned long val;
+	int ret;
+
+	ret = strict_strtoul(buf, 0, &val);
+	if (ret) {
+		printk(KERN_DEBUG "%s is not in hex or decimal form.\n", buf);
+	} else {
+		rtlpriv->dbg.global_debuglevel = val;
+		printk(KERN_DEBUG "debuglevel:%x\n",
+			rtlpriv->dbg.global_debuglevel);
+	}
+
+	return strnlen(buf, count);
+}
+
+static DEVICE_ATTR(debug_level, S_IWUSR | S_IRUGO,
+		   rtl_show_debug_level, rtl_store_debug_level);
+
+static struct attribute *rtl_sysfs_entries[] = {
+
+	&dev_attr_debug_level.attr,
+
+	NULL
+};
+
+/*
+ * "name" is folder name witch will be
+ * put in device directory like :
+ * sys/devices/pci0000:00/0000:00:1c.4/
+ * 0000:06:00.0/rtl_sysfs
+ */
+struct attribute_group rtl_attribute_group = {
+	.name = "rtlsysfs",
+	.attrs = rtl_sysfs_entries,
+};
+
+#ifdef VIF_TODO
+/*********************************************************
+ *
+ * vif functions
+ *
+ *********************************************************/
+static inline struct ieee80211_vif *
+rtl_get_vif(struct rtl_vif_info *vif_priv)
+{
+	return container_of((void *)vif_priv, struct ieee80211_vif, drv_priv);
+}
+
+/* Protected by ar->mutex or RCU */
+struct ieee80211_vif *rtl_get_main_vif(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_vif_info *cvif;
+
+	list_for_each_entry_rcu(cvif, &rtlpriv->vif_priv.vif_list, list) {
+		if (cvif->active)
+			return rtl_get_vif(cvif);
+	}
+
+	return NULL;
+}
+
+static inline bool is_main_vif(struct ieee80211_hw *hw,
+			       struct ieee80211_vif *vif)
+{
+	bool ret;
+
+	rcu_read_lock();
+	ret = (rtl_get_main_vif(hw) == vif);
+	rcu_read_unlock();
+	return ret;
+}
+
+bool rtl_set_vif_info(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
+{
+	struct rtl_vif_info *vif_info = (void *) vif->drv_priv;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int vif_id = -1;
+
+	if (rtlpriv->vif_priv.vifs >= MAX_VIRTUAL_MAC) {
+		RT_TRACE(COMP_ERR, DBG_WARNING,
+			 ("vif number can not bigger than %d, now vifs is:%d\n",
+			  MAX_VIRTUAL_MAC, rtlpriv->vif_priv.vifs));
+		return false;
+	}
+
+	rcu_read_lock();
+	vif_id = bitmap_find_free_region(&rtlpriv->vif_priv.vif_bitmap,
+					 MAX_VIRTUAL_MAC, 0);
+	RT_TRACE(COMP_MAC80211, DBG_DMESG,
+		 ("%s vid_id:%d\n", __func__, vif_id));
+
+	if (vif_id < 0) {
+		rcu_read_unlock();
+		return false;
+	}
+
+	BUG_ON(rtlpriv->vif_priv.vif[vif_id].id != vif_id);
+	vif_info->active = true;
+	vif_info->id = vif_id;
+	vif_info->enable_beacon = false;
+	rtlpriv->vif_priv.vifs++;
+	if (rtlpriv->vif_priv.vifs > 1) {
+		rtlpriv->psc.b_inactiveps = false;
+		rtlpriv->psc.b_swctrl_lps = false;
+		rtlpriv->psc.b_fwctrl_lps = false;
+	}
+
+	list_add_tail_rcu(&vif_info->list, &rtlpriv->vif_priv.vif_list);
+	rcu_assign_pointer(rtlpriv->vif_priv.vif[vif_id].vif, vif);
+
+	RT_TRACE(COMP_MAC80211, DBG_DMESG, ("vifaddress:%p %p %p\n",
+		 rtlpriv->vif_priv.vif[vif_id].vif, vif, rtl_get_main_vif(hw)));
+
+	rcu_read_unlock();
+
+	return true;
+}
+#endif
+
+
+#if 0
+MODULE_AUTHOR("lizhaoming	<chaoming_li@realsil.com.cn>");
+MODULE_AUTHOR("Realtek WlanFAE	<wlanfae@realtek.com>");
+MODULE_AUTHOR("Larry Finger	<Larry.FInger@lwfinger.net>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Realtek 802.11n PCI wireless core");
+#endif
+struct rtl_global_var global_var = {};
+
+int rtl_core_module_init(void)
+{
+	if (rtl_rate_control_register())
+		printk(KERN_DEBUG "rtl: Unable to register rtl_rc,"
+			  "use default RC !!\n");
+
+	/* add proc for debug */
+	rtl_proc_add_topdir();
+
+	/* init some global vars */
+	INIT_LIST_HEAD(&global_var.glb_priv_list);
+	spin_lock_init(&global_var.glb_list_lock);
+
+	return 0;
+}
+
+void rtl_core_module_exit(void)
+{
+	/*RC*/
+   	rtl_rate_control_unregister();
+
+	/* add proc for debug */
+	rtl_proc_remove_topdir();
+}
+
+#if 0
+module_init(rtl_core_module_init);
+module_exit(rtl_core_module_exit);
+#endif
diff --git a/drivers/staging/rtl8821ae/base.h b/drivers/staging/rtl8821ae/base.h
new file mode 100644
index 000000000000..629d14f42f0b
--- /dev/null
+++ b/drivers/staging/rtl8821ae/base.h
@@ -0,0 +1,159 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL_BASE_H__
+#define __RTL_BASE_H__
+
+#include "compat.h"
+
+enum ap_peer {
+	PEER_UNKNOWN = 0,
+	PEER_RTL = 1,
+	PEER_RTL_92SE = 2,
+	PEER_BROAD = 3,
+	PEER_RAL = 4,
+	PEER_ATH = 5,
+	PEER_CISCO = 6,
+	PEER_MARV = 7,
+	PEER_AIRGO = 9,
+	PEER_MAX = 10,
+} ;
+
+#define RTL_DUMMY_OFFSET	0
+#define RTL_DUMMY_UNIT		8
+#define RTL_TX_DUMMY_SIZE	(RTL_DUMMY_OFFSET * RTL_DUMMY_UNIT)
+#define RTL_TX_DESC_SIZE	32
+#define RTL_TX_HEADER_SIZE	(RTL_TX_DESC_SIZE + RTL_TX_DUMMY_SIZE)
+
+#define HT_AMSDU_SIZE_4K 	3839
+#define HT_AMSDU_SIZE_8K 	7935
+
+#define MAX_BIT_RATE_40MHZ_MCS15 	300	/* Mbps */
+#define MAX_BIT_RATE_40MHZ_MCS7 	150	/* Mbps */
+
+#define RTL_RATE_COUNT_LEGACY		12
+#define RTL_CHANNEL_COUNT		14
+
+#define FRAME_OFFSET_FRAME_CONTROL	0
+#define FRAME_OFFSET_DURATION		2
+#define FRAME_OFFSET_ADDRESS1		4
+#define FRAME_OFFSET_ADDRESS2		10
+#define FRAME_OFFSET_ADDRESS3		16
+#define FRAME_OFFSET_SEQUENCE		22
+#define FRAME_OFFSET_ADDRESS4		24
+
+#define SET_80211_HDR_FRAME_CONTROL(_hdr, _val)		\
+	WRITEEF2BYTE(_hdr, _val)
+#define SET_80211_HDR_TYPE_AND_SUBTYPE(_hdr, _val)	\
+	WRITEEF1BYTE(_hdr, _val)
+#define SET_80211_HDR_PWR_MGNT(_hdr, _val)		\
+	SET_BITS_TO_LE_2BYTE(_hdr, 12, 1, _val)
+#define SET_80211_HDR_TO_DS(_hdr, _val)			\
+	SET_BITS_TO_LE_2BYTE(_hdr, 8, 1, _val)
+
+#define SET_80211_PS_POLL_AID(_hdr, _val)		\
+	WRITEEF2BYTE(((u8*)(_hdr))+2, _val)
+#define SET_80211_PS_POLL_BSSID(_hdr, _val)		\
+	CP_MACADDR(((u8*)(_hdr))+4, (u8*)(_val))
+#define SET_80211_PS_POLL_TA(_hdr, _val)		\
+	CP_MACADDR(((u8*)(_hdr))+10, (u8*)(_val))
+
+#define SET_80211_HDR_DURATION(_hdr, _val)	\
+	WRITEEF2BYTE((u8*)(_hdr)+FRAME_OFFSET_DURATION, _val)
+#define SET_80211_HDR_ADDRESS1(_hdr, _val)	\
+	CP_MACADDR((u8*)(_hdr)+FRAME_OFFSET_ADDRESS1, (u8*)(_val))
+#define SET_80211_HDR_ADDRESS2(_hdr, _val) 	\
+	CP_MACADDR((u8*)(_hdr)+FRAME_OFFSET_ADDRESS2, (u8*)(_val))
+#define SET_80211_HDR_ADDRESS3(_hdr, _val) 	\
+	CP_MACADDR((u8*)(_hdr)+FRAME_OFFSET_ADDRESS3, (u8*)(_val))
+#define SET_80211_HDR_FRAGMENT_SEQUENCE(_hdr, _val)  \
+	WRITEEF2BYTE((u8*)(_hdr)+FRAME_OFFSET_SEQUENCE, _val)
+
+#define SET_BEACON_PROBE_RSP_TIME_STAMP_LOW(__phdr, __val) 	\
+	WRITEEF4BYTE(((u8*)(__phdr)) + 24, __val)
+#define SET_BEACON_PROBE_RSP_TIME_STAMP_HIGH(__phdr, __val) \
+	WRITEEF4BYTE(((u8*)(__phdr)) + 28, __val)
+#define SET_BEACON_PROBE_RSP_BEACON_INTERVAL(__phdr, __val) \
+	WRITEEF2BYTE(((u8*)(__phdr)) + 32, __val)
+#define GET_BEACON_PROBE_RSP_CAPABILITY_INFO(__phdr) 		\
+	READEF2BYTE(((u8*)(__phdr)) + 34)
+#define SET_BEACON_PROBE_RSP_CAPABILITY_INFO(__phdr, __val) \
+	WRITEEF2BYTE(((u8*)(__phdr)) + 34, __val)
+#define MASK_BEACON_PROBE_RSP_CAPABILITY_INFO(__phdr, __val) \
+	SET_BEACON_PROBE_RSP_CAPABILITY_INFO(__phdr, \
+	(GET_BEACON_PROBE_RSP_CAPABILITY_INFO(__phdr) & (~(__val))))
+
+int rtl_init_core(struct ieee80211_hw *hw);
+void rtl_deinit_core(struct ieee80211_hw *hw);
+void rtl_init_rx_config(struct ieee80211_hw *hw);
+void rtl_init_rfkill(struct ieee80211_hw *hw);
+void rtl_deinit_rfkill(struct ieee80211_hw *hw);
+
+void rtl_watch_dog_timer_callback(unsigned long data);
+void rtl_deinit_deferred_work(struct ieee80211_hw *hw);
+
+bool rtl_action_proc(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx);
+bool rtl_tx_mgmt_proc(struct ieee80211_hw *hw, struct sk_buff *skb);
+u8 rtl_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx);
+
+void rtl_beacon_statistic(struct ieee80211_hw *hw, struct sk_buff *skb);
+void rtl_watch_dog_timer_callback(unsigned long data);
+int rtl_tx_agg_start(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+	struct ieee80211_sta *sta, u16 tid, u16 * ssn);
+int rtl_tx_agg_stop(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+	struct ieee80211_sta *sta, u16 tid);
+int rtl_tx_agg_oper(struct ieee80211_hw *hw,
+		    struct ieee80211_sta *sta, u16 tid);
+int rtl_rx_agg_start(struct ieee80211_hw *hw,
+		     struct ieee80211_sta *sta, u16 tid);
+int rtl_rx_agg_stop(struct ieee80211_hw *hw,
+		    struct ieee80211_sta *sta, u16 tid);
+void rtl_watchdog_wq_callback(void *data);
+void rtl_fwevt_wq_callback(void *data);
+
+void rtl_get_tcb_desc(struct ieee80211_hw *hw,
+		      struct ieee80211_tx_info *info,
+		      struct ieee80211_sta *sta,
+		      struct sk_buff *skb, struct rtl_tcb_desc *tcb_desc);
+
+int rtl_send_smps_action(struct ieee80211_hw *hw,
+		struct ieee80211_sta *sta,
+		enum ieee80211_smps_mode smps);
+u8 *rtl_find_ie(u8 *data, unsigned int len, u8 ie);
+void rtl_recognize_peer(struct ieee80211_hw *hw, u8 *data, unsigned int len);
+u8 rtl_tid_to_ac(struct ieee80211_hw *hw, u8 tid);
+extern struct attribute_group rtl_attribute_group;
+void rtl_easy_concurrent_retrytimer_callback(unsigned long data);
+extern struct rtl_global_var global_var;
+
+#ifdef VIF_TODO
+struct ieee80211_vif *rtl_get_main_vif(struct ieee80211_hw *hw);
+bool rtl_set_vif_info(struct ieee80211_hw *hw, struct ieee80211_vif *vif);
+#endif
+#endif
diff --git a/drivers/staging/rtl8821ae/btcoexist/HalBtc8812a1Ant.c b/drivers/staging/rtl8821ae/btcoexist/HalBtc8812a1Ant.c
new file mode 100644
index 000000000000..b30f17ae0215
--- /dev/null
+++ b/drivers/staging/rtl8821ae/btcoexist/HalBtc8812a1Ant.c
@@ -0,0 +1,3976 @@
+//============================================================
+// Description:
+//
+// This file is for 8812a1ant Co-exist mechanism
+//
+// History
+// 2012/11/15 Cosa first check in.
+//
+//============================================================
+
+//============================================================
+// include files
+//============================================================
+#include "halbt_precomp.h"
+#if 1
+//============================================================
+// Global variables, these are static variables
+//============================================================
+static COEX_DM_8812A_1ANT		GLCoexDm8812a1Ant;
+static PCOEX_DM_8812A_1ANT 	coex_dm=&GLCoexDm8812a1Ant;
+static COEX_STA_8812A_1ANT		GLCoexSta8812a1Ant;
+static PCOEX_STA_8812A_1ANT	coex_sta=&GLCoexSta8812a1Ant;
+
+const char *const GLBtInfoSrc8812a1Ant[]={
+	"BT Info[wifi fw]",
+	"BT Info[bt rsp]",
+	"BT Info[bt auto report]",
+};
+
+//============================================================
+// local function proto type if needed
+//============================================================
+//============================================================
+// local function start with halbtc8812a1ant_
+//============================================================
+#if 0
+void
+halbtc8812a1ant_Reg0x550Bit3(
+	 	PBTC_COEXIST		btcoexist,
+	 	BOOLEAN			bSet
+	)
+{
+	u1Byte	u1tmp=0;
+	
+	u1tmp = btcoexist->btc_read_1byte(btcoexist, 0x550);
+	if(bSet)
+	{
+		u1tmp |= BIT3;
+	}
+	else
+	{
+		u1tmp &= ~BIT3;
+	}
+	btcoexist->btc_write_1byte(btcoexist, 0x550, u1tmp);
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], set 0x550[3]=%d\n", (bSet? 1:0)));
+}
+#endif
+u1Byte
+halbtc8812a1ant_BtRssiState(
+	u1Byte			level_num,
+	u1Byte			rssi_thresh,
+	u1Byte			rssi_thresh1
+	)
+{
+	s4Byte			bt_rssi=0;
+	u1Byte			bt_rssi_state;
+
+	bt_rssi = coex_sta->bt_rssi;
+
+	if(level_num == 2)
+	{			
+		if( (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
+			(coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW))
+		{
+			if(bt_rssi >= (rssi_thresh+BTC_RSSI_COEX_THRESH_TOL_8812A_1ANT))
+			{
+				bt_rssi_state = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to High\n"));
+			}
+			else
+			{
+				bt_rssi_state = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at Low\n"));
+			}
+		}
+		else
+		{
+			if(bt_rssi < rssi_thresh)
+			{
+				bt_rssi_state = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Low\n"));
+			}
+			else
+			{
+				bt_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at High\n"));
+			}
+		}
+	}
+	else if(level_num == 3)
+	{
+		if(rssi_thresh > rssi_thresh1)
+		{
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi thresh error!!\n"));
+			return coex_sta->pre_bt_rssi_state;
+		}
+		
+		if( (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
+			(coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW))
+		{
+			if(bt_rssi >= (rssi_thresh+BTC_RSSI_COEX_THRESH_TOL_8812A_1ANT))
+			{
+				bt_rssi_state = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Medium\n"));
+			}
+			else
+			{
+				bt_rssi_state = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at Low\n"));
+			}
+		}
+		else if( (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_MEDIUM) ||
+			(coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_MEDIUM))
+		{
+			if(bt_rssi >= (rssi_thresh1+BTC_RSSI_COEX_THRESH_TOL_8812A_1ANT))
+			{
+				bt_rssi_state = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to High\n"));
+			}
+			else if(bt_rssi < rssi_thresh)
+			{
+				bt_rssi_state = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Low\n"));
+			}
+			else
+			{
+				bt_rssi_state = BTC_RSSI_STATE_STAY_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at Medium\n"));
+			}
+		}
+		else
+		{
+			if(bt_rssi < rssi_thresh1)
+			{
+				bt_rssi_state = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Medium\n"));
+			}
+			else
+			{
+				bt_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at High\n"));
+			}
+		}
+	}
+		
+	coex_sta->pre_bt_rssi_state = bt_rssi_state;
+
+	return bt_rssi_state;
+}
+
+u1Byte
+halbtc8812a1ant_WifiRssiState(
+	 	PBTC_COEXIST		btcoexist,
+	 	u1Byte			index,
+	 	u1Byte			level_num,
+	 	u1Byte			rssi_thresh,
+	 	u1Byte			rssi_thresh1
+	)
+{
+	s4Byte			wifi_rssi=0;
+	u1Byte			wifi_rssi_state;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
+	
+	if(level_num == 2)
+	{
+		if( (coex_sta->pre_wifi_rssi_state[index] == BTC_RSSI_STATE_LOW) ||
+			(coex_sta->pre_wifi_rssi_state[index] == BTC_RSSI_STATE_STAY_LOW))
+		{
+			if(wifi_rssi >= (rssi_thresh+BTC_RSSI_COEX_THRESH_TOL_8812A_1ANT))
+			{
+				wifi_rssi_state = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to High\n"));
+			}
+			else
+			{
+				wifi_rssi_state = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at Low\n"));
+			}
+		}
+		else
+		{
+			if(wifi_rssi < rssi_thresh)
+			{
+				wifi_rssi_state = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Low\n"));
+			}
+			else
+			{
+				wifi_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at High\n"));
+			}
+		}
+	}
+	else if(level_num == 3)
+	{
+		if(rssi_thresh > rssi_thresh1)
+		{
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI thresh error!!\n"));
+			return coex_sta->pre_wifi_rssi_state[index];
+		}
+		
+		if( (coex_sta->pre_wifi_rssi_state[index] == BTC_RSSI_STATE_LOW) ||
+			(coex_sta->pre_wifi_rssi_state[index] == BTC_RSSI_STATE_STAY_LOW))
+		{
+			if(wifi_rssi >= (rssi_thresh+BTC_RSSI_COEX_THRESH_TOL_8812A_1ANT))
+			{
+				wifi_rssi_state = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Medium\n"));
+			}
+			else
+			{
+				wifi_rssi_state = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at Low\n"));
+			}
+		}
+		else if( (coex_sta->pre_wifi_rssi_state[index] == BTC_RSSI_STATE_MEDIUM) ||
+			(coex_sta->pre_wifi_rssi_state[index] == BTC_RSSI_STATE_STAY_MEDIUM))
+		{
+			if(wifi_rssi >= (rssi_thresh1+BTC_RSSI_COEX_THRESH_TOL_8812A_1ANT))
+			{
+				wifi_rssi_state = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to High\n"));
+			}
+			else if(wifi_rssi < rssi_thresh)
+			{
+				wifi_rssi_state = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Low\n"));
+			}
+			else
+			{
+				wifi_rssi_state = BTC_RSSI_STATE_STAY_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at Medium\n"));
+			}
+		}
+		else
+		{
+			if(wifi_rssi < rssi_thresh1)
+			{
+				wifi_rssi_state = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Medium\n"));
+			}
+			else
+			{
+				wifi_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at High\n"));
+			}
+		}
+	}
+		
+	coex_sta->pre_wifi_rssi_state[index] = wifi_rssi_state;
+
+	return wifi_rssi_state;
+}
+
+void
+halbtc8812a1ant_MonitorBtEnableDisable(
+	  	PBTC_COEXIST		btcoexist
+	)
+{
+	static BOOLEAN	pre_bt_disabled=false;
+	static u4Byte		bt_disable_cnt=0;
+	BOOLEAN			bt_active=true, bt_disable_by68=false, bt_disabled=false;
+	u4Byte			u4_tmp=0;
+
+	// This function check if bt is disabled
+
+	if(	coex_sta->high_priority_tx == 0 &&
+		coex_sta->high_priority_rx == 0 &&
+		coex_sta->low_priority_tx == 0 &&
+		coex_sta->low_priority_rx == 0)
+	{
+		bt_active = false;
+	}
+	if(	coex_sta->high_priority_tx == 0xffff &&
+		coex_sta->high_priority_rx == 0xffff &&
+		coex_sta->low_priority_tx == 0xffff &&
+		coex_sta->low_priority_rx == 0xffff)
+	{
+		bt_active = false;
+	}
+	if(bt_active)
+	{
+		bt_disable_cnt = 0;
+		bt_disabled = false;
+		btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_DISABLE, &bt_disabled);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], BT is enabled !!\n"));
+	}
+	else
+	{
+		bt_disable_cnt++;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], bt all counters=0, %d times!!\n", 
+				bt_disable_cnt));
+		if(bt_disable_cnt >= 2 ||bt_disable_by68)
+		{
+			bt_disabled = true;
+			btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_DISABLE, &bt_disabled);
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], BT is disabled !!\n"));
+		}
+	}
+	if(pre_bt_disabled != bt_disabled)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], BT is from %s to %s!!\n", 
+			(pre_bt_disabled ? "disabled":"enabled"), 
+			(bt_disabled ? "disabled":"enabled")));
+		pre_bt_disabled = bt_disabled;
+		if(!bt_disabled)
+		{
+		}
+		else
+		{
+		}
+	}
+}
+
+void
+halbtc8812a1ant_MonitorBtCtr(
+	 	PBTC_COEXIST		btcoexist
+	)
+{
+	u4Byte 			reg_hp_tx_rx, reg_lp_tx_rx, u4_tmp;
+	u4Byte			reg_hp_tx=0, reg_hp_rx=0, reg_lp_tx=0, reg_lp_rx=0;
+	u1Byte			u1_tmp;
+	
+	reg_hp_tx_rx = 0x770;
+	reg_lp_tx_rx = 0x774;
+
+	u4_tmp = btcoexist->btc_read_4byte(btcoexist, reg_hp_tx_rx);
+	reg_hp_tx = u4_tmp & bMaskLWord;
+	reg_hp_rx = (u4_tmp & bMaskHWord)>>16;
+
+	u4_tmp = btcoexist->btc_read_4byte(btcoexist, reg_lp_tx_rx);
+	reg_lp_tx = u4_tmp & bMaskLWord;
+	reg_lp_rx = (u4_tmp & bMaskHWord)>>16;
+		
+	coex_sta->high_priority_tx = reg_hp_tx;
+	coex_sta->high_priority_rx = reg_hp_rx;
+	coex_sta->low_priority_tx = reg_lp_tx;
+	coex_sta->low_priority_rx = reg_lp_rx;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], High Priority Tx/Rx (reg 0x%x)=%x(%d)/%x(%d)\n", 
+		reg_hp_tx_rx, reg_hp_tx, reg_hp_tx, reg_hp_rx, reg_hp_rx));
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], Low Priority Tx/Rx (reg 0x%x)=%x(%d)/%x(%d)\n", 
+		reg_lp_tx_rx, reg_lp_tx, reg_lp_tx, reg_lp_rx, reg_lp_rx));
+
+	// reset counter
+	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
+}
+
+void
+halbtc8812a1ant_QueryBtInfo(
+	 	PBTC_COEXIST		btcoexist
+	)
+{	
+	u1Byte	dataLen=3;
+	u1Byte	buf[5] = {0};
+	static	u4Byte	btInfoCnt=0;
+
+	if(!btInfoCnt ||
+		(coex_sta->bt_info_c2h_cnt[BT_INFO_SRC_8812A_1ANT_BT_RSP]-btInfoCnt)>2)
+	{
+		buf[0] = dataLen;
+		buf[1] = 0x1;	// polling enable, 1=enable, 0=disable
+		buf[2] = 0x2;	// polling time in seconds
+		buf[3] = 0x1;	// auto report enable, 1=enable, 0=disable
+			
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_CTRL_BT_INFO, (PVOID)&buf[0]);
+	}
+	btInfoCnt = coex_sta->bt_info_c2h_cnt[BT_INFO_SRC_8812A_1ANT_BT_RSP];
+}
+u1Byte
+halbtc8812a1ant_ActionAlgorithm(
+	 	PBTC_COEXIST		btcoexist
+	)
+{
+	PBTC_STACK_INFO		stack_info=&btcoexist->stack_info;
+	BOOLEAN				bt_hs_on=false;
+	u1Byte				algorithm=BT_8812A_1ANT_COEX_ALGO_UNDEFINED;
+	u1Byte				num_of_diff_profile=0;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	
+	if(!stack_info->bt_link_exist)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], No profile exists!!!\n"));
+		return algorithm;
+	}
+
+	if(stack_info->sco_exist)
+		num_of_diff_profile++;
+	if(stack_info->hid_exist)
+		num_of_diff_profile++;
+	if(stack_info->pan_exist)
+		num_of_diff_profile++;
+	if(stack_info->a2dp_exist)
+		num_of_diff_profile++;
+	
+	if(num_of_diff_profile == 1)
+	{
+		if(stack_info->sco_exist)
+		{
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO only\n"));
+			algorithm = BT_8812A_1ANT_COEX_ALGO_SCO;
+		}
+		else
+		{
+			if(stack_info->hid_exist)
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID only\n"));
+				algorithm = BT_8812A_1ANT_COEX_ALGO_HID;
+			}
+			else if(stack_info->a2dp_exist)
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], A2DP only\n"));
+				algorithm = BT_8812A_1ANT_COEX_ALGO_A2DP;
+			}
+			else if(stack_info->pan_exist)
+			{
+				if(bt_hs_on)
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PAN(HS) only\n"));
+					algorithm = BT_8812A_1ANT_COEX_ALGO_PANHS;
+				}
+				else
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PAN(EDR) only\n"));
+					algorithm = BT_8812A_1ANT_COEX_ALGO_PANEDR;
+				}
+			}
+		}
+	}
+	else if(num_of_diff_profile == 2)
+	{
+		if(stack_info->sco_exist)
+		{
+			if(stack_info->hid_exist)
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID\n"));
+				algorithm = BT_8812A_1ANT_COEX_ALGO_HID;
+			}
+			else if(stack_info->a2dp_exist)
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + A2DP ==> SCO\n"));
+				algorithm = BT_8812A_1ANT_COEX_ALGO_SCO;
+			}
+			else if(stack_info->pan_exist)
+			{
+				if(bt_hs_on)
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + PAN(HS)\n"));
+					algorithm = BT_8812A_1ANT_COEX_ALGO_SCO;
+				}
+				else
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + PAN(EDR)\n"));
+					algorithm = BT_8812A_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		}
+		else
+		{
+			if( stack_info->hid_exist &&
+				stack_info->a2dp_exist )
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + A2DP\n"));
+				algorithm = BT_8812A_1ANT_COEX_ALGO_HID_A2DP;
+			}
+			else if( stack_info->hid_exist &&
+				stack_info->pan_exist )
+			{
+				if(bt_hs_on)
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + PAN(HS)\n"));
+					algorithm = BT_8812A_1ANT_COEX_ALGO_HID_A2DP;
+				}
+				else
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + PAN(EDR)\n"));
+					algorithm = BT_8812A_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+			else if( stack_info->pan_exist &&
+				stack_info->a2dp_exist )
+			{
+				if(bt_hs_on)
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], A2DP + PAN(HS)\n"));
+					algorithm = BT_8812A_1ANT_COEX_ALGO_A2DP_PANHS;
+				}
+				else
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], A2DP + PAN(EDR)\n"));
+					algorithm = BT_8812A_1ANT_COEX_ALGO_PANEDR_A2DP;
+				}
+			}
+		}
+	}
+	else if(num_of_diff_profile == 3)
+	{
+		if(stack_info->sco_exist)
+		{
+			if( stack_info->hid_exist &&
+				stack_info->a2dp_exist )
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID + A2DP ==> HID\n"));
+				algorithm = BT_8812A_1ANT_COEX_ALGO_HID;
+			}
+			else if( stack_info->hid_exist &&
+				stack_info->pan_exist )
+			{
+				if(bt_hs_on)
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID + PAN(HS)\n"));
+					algorithm = BT_8812A_1ANT_COEX_ALGO_HID_A2DP;
+				}
+				else
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID + PAN(EDR)\n"));
+					algorithm = BT_8812A_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+			else if( stack_info->pan_exist &&
+				stack_info->a2dp_exist )
+			{
+				if(bt_hs_on)
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + A2DP + PAN(HS)\n"));
+					algorithm = BT_8812A_1ANT_COEX_ALGO_SCO;
+				}
+				else
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + A2DP + PAN(EDR) ==> HID\n"));
+					algorithm = BT_8812A_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		}
+		else
+		{
+			if( stack_info->hid_exist &&
+				stack_info->pan_exist &&
+				stack_info->a2dp_exist )
+			{
+				if(bt_hs_on)
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + A2DP + PAN(HS)\n"));
+					algorithm = BT_8812A_1ANT_COEX_ALGO_HID_A2DP;
+				}
+				else
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + A2DP + PAN(EDR)\n"));
+					algorithm = BT_8812A_1ANT_COEX_ALGO_HID_A2DP_PANEDR;
+				}
+			}
+		}
+	}
+	else if(num_of_diff_profile >= 3)
+	{
+		if(stack_info->sco_exist)
+		{
+			if( stack_info->hid_exist &&
+				stack_info->pan_exist &&
+				stack_info->a2dp_exist )
+			{
+				if(bt_hs_on)
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Error!!! SCO + HID + A2DP + PAN(HS)\n"));
+
+				}
+				else
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID + A2DP + PAN(EDR)==>PAN(EDR)+HID\n"));
+					algorithm = BT_8812A_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		}
+	}
+
+	return algorithm;
+}
+
+BOOLEAN
+halbtc8812a1ant_NeedToDecBtPwr(
+	 	PBTC_COEXIST		btcoexist
+	)
+{
+	BOOLEAN		ret=false;
+	BOOLEAN		bt_hs_on=false, wifi_connected=false;
+	s4Byte		bt_hs_rssi=0;
+
+	if(!btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on))
+		return false;
+	if(!btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED, &wifi_connected))
+		return false;
+	if(!btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi))
+		return false;
+
+	if(wifi_connected)
+	{
+		if(bt_hs_on)
+		{
+			if(bt_hs_rssi > 37)
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], Need to decrease bt power for HS mode!!\n"));
+				ret = true;
+			}
+		}
+		else
+		{
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], Need to decrease bt power for Wifi is connected!!\n"));
+			ret = true;
+		}
+	}
+	
+	return ret;
+}
+
+void
+halbtc8812a1ant_SetFwDacSwingLevel(
+	 	PBTC_COEXIST		btcoexist,
+	 	u1Byte			dac_swing_lvl
+	)
+{
+	u1Byte			h2c_parameter[1] ={0};
+
+	// There are several type of dacswing
+	// 0x18/ 0x10/ 0xc/ 0x8/ 0x4/ 0x6
+	h2c_parameter[0] = dac_swing_lvl;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], Set Dac Swing Level=0x%x\n", dac_swing_lvl));
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], FW write 0x64=0x%x\n", h2c_parameter[0]));
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x64, 1, h2c_parameter);
+}
+
+void
+halbtc8812a1ant_SetFwDecBtPwr(
+	 	PBTC_COEXIST		btcoexist,
+	 	BOOLEAN			dec_bt_pwr
+	)
+{
+	u1Byte	dataLen=3;
+	u1Byte	buf[5] = {0};
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], decrease Bt Power : %s\n", 
+			(dec_bt_pwr? "Yes!!":"No!!")));
+
+	buf[0] = dataLen;
+	buf[1] = 0x3;		// OP_Code
+	buf[2] = 0x1;		// OP_Code_Length
+	if(dec_bt_pwr)
+		buf[3] = 0x1;	// OP_Code_Content
+	else
+		buf[3] = 0x0;
+		
+	btcoexist->btc_set(btcoexist, BTC_SET_ACT_CTRL_BT_COEX, (PVOID)&buf[0]);	
+}
+
+void
+halbtc8812a1ant_DecBtPwr(
+	 	PBTC_COEXIST		btcoexist,
+	 	BOOLEAN			force_exec,
+	 	BOOLEAN			dec_bt_pwr
+	)
+{
+	return;
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s Dec BT power = %s\n",  
+		(force_exec? "force to":""), ((dec_bt_pwr)? "ON":"OFF")));
+	coex_dm->cur_dec_bt_pwr = dec_bt_pwr;
+
+	if(!force_exec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], pre_dec_bt_pwr=%d, cur_dec_bt_pwr=%d\n", 
+			coex_dm->pre_dec_bt_pwr, coex_dm->cur_dec_bt_pwr));
+
+		if(coex_dm->pre_dec_bt_pwr == coex_dm->cur_dec_bt_pwr) 
+			return;
+	}
+	halbtc8812a1ant_SetFwDecBtPwr(btcoexist, coex_dm->cur_dec_bt_pwr);
+
+	coex_dm->pre_dec_bt_pwr = coex_dm->cur_dec_bt_pwr;
+}
+
+void
+halbtc8812a1ant_SetFwBtLnaConstrain(
+	 	PBTC_COEXIST		btcoexist,
+	 	BOOLEAN			bt_lna_cons_on
+	)
+{
+	u1Byte	dataLen=3;
+	u1Byte	buf[5] = {0};
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], set BT LNA Constrain: %s\n", 
+		(bt_lna_cons_on? "ON!!":"OFF!!")));
+
+	buf[0] = dataLen;
+	buf[1] = 0x2;		// OP_Code
+	buf[2] = 0x1;		// OP_Code_Length
+	if(bt_lna_cons_on)
+		buf[3] = 0x1;	// OP_Code_Content
+	else
+		buf[3] = 0x0;
+		
+	btcoexist->btc_set(btcoexist, BTC_SET_ACT_CTRL_BT_COEX, (PVOID)&buf[0]);	
+}
+
+void
+halbtc8812a1ant_SetBtLnaConstrain(
+	 	PBTC_COEXIST		btcoexist,
+	 	BOOLEAN			force_exec,
+	 	BOOLEAN			bt_lna_cons_on
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s BT Constrain = %s\n",  
+		(force_exec? "force":""), ((bt_lna_cons_on)? "ON":"OFF")));
+	coex_dm->bCurBtLnaConstrain = bt_lna_cons_on;
+
+	if(!force_exec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], bPreBtLnaConstrain=%d, bCurBtLnaConstrain=%d\n", 
+			coex_dm->bPreBtLnaConstrain, coex_dm->bCurBtLnaConstrain));
+
+		if(coex_dm->bPreBtLnaConstrain == coex_dm->bCurBtLnaConstrain) 
+			return;
+	}
+	halbtc8812a1ant_SetFwBtLnaConstrain(btcoexist, coex_dm->bCurBtLnaConstrain);
+
+	coex_dm->bPreBtLnaConstrain = coex_dm->bCurBtLnaConstrain;
+}
+
+void
+halbtc8812a1ant_SetFwBtPsdMode(
+	 	PBTC_COEXIST		btcoexist,
+	 	u1Byte			bt_psd_mode
+	)
+{
+	u1Byte	dataLen=3;
+	u1Byte	buf[5] = {0};
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], set BT PSD mode=0x%x\n", 
+		bt_psd_mode));
+
+	buf[0] = dataLen;
+	buf[1] = 0x4;			// OP_Code
+	buf[2] = 0x1;			// OP_Code_Length
+	buf[3] = bt_psd_mode;	// OP_Code_Content
+		
+	btcoexist->btc_set(btcoexist, BTC_SET_ACT_CTRL_BT_COEX, (PVOID)&buf[0]);	
+}
+
+
+void
+halbtc8812a1ant_SetBtPsdMode(
+	 	PBTC_COEXIST		btcoexist,
+	 	BOOLEAN			force_exec,
+	 	u1Byte			bt_psd_mode
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s BT PSD mode = 0x%x\n",  
+		(force_exec? "force":""), bt_psd_mode));
+	coex_dm->bCurBtPsdMode = bt_psd_mode;
+
+	if(!force_exec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], bPreBtPsdMode=0x%x, bCurBtPsdMode=0x%x\n", 
+			coex_dm->bPreBtPsdMode, coex_dm->bCurBtPsdMode));
+
+		if(coex_dm->bPreBtPsdMode == coex_dm->bCurBtPsdMode) 
+			return;
+	}
+	halbtc8812a1ant_SetFwBtPsdMode(btcoexist, coex_dm->bCurBtPsdMode);
+
+	coex_dm->bPreBtPsdMode = coex_dm->bCurBtPsdMode;
+}
+
+
+void
+halbtc8812a1ant_SetBtAutoReport(
+	 	PBTC_COEXIST		btcoexist,
+	 	BOOLEAN			enable_auto_report
+	)
+{
+#if 0
+	u1Byte			h2c_parameter[1] ={0};
+	
+	h2c_parameter[0] = 0;
+
+	if(enable_auto_report)
+	{
+		h2c_parameter[0] |= BIT0;
+	}
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], BT FW auto report : %s, FW write 0x68=0x%x\n", 
+		(enable_auto_report? "Enabled!!":"Disabled!!"), h2c_parameter[0]));
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x68, 1, h2c_parameter);	
+#else
+
+#endif
+}
+
+void
+halbtc8812a1ant_BtAutoReport(
+	 	PBTC_COEXIST		btcoexist,
+	 	BOOLEAN			force_exec,
+	 	BOOLEAN			enable_auto_report
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s BT Auto report = %s\n",  
+		(force_exec? "force to":""), ((enable_auto_report)? "Enabled":"Disabled")));
+	coex_dm->cur_bt_auto_report = enable_auto_report;
+
+	if(!force_exec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], pre_bt_auto_report=%d, cur_bt_auto_report=%d\n", 
+			coex_dm->pre_bt_auto_report, coex_dm->cur_bt_auto_report));
+
+		if(coex_dm->pre_bt_auto_report == coex_dm->cur_bt_auto_report) 
+			return;
+	}
+	halbtc8812a1ant_SetBtAutoReport(btcoexist, coex_dm->cur_bt_auto_report);
+
+	coex_dm->pre_bt_auto_report = coex_dm->cur_bt_auto_report;
+}
+
+void
+halbtc8812a1ant_FwDacSwingLvl(
+	 	PBTC_COEXIST		btcoexist,
+	 	BOOLEAN			force_exec,
+	 	u1Byte			fw_dac_swing_lvl
+	)
+{
+	return;
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s set FW Dac Swing level = %d\n",  
+		(force_exec? "force to":""), fw_dac_swing_lvl));
+	coex_dm->cur_fw_dac_swing_lvl = fw_dac_swing_lvl;
+
+	if(!force_exec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], pre_fw_dac_swing_lvl=%d, cur_fw_dac_swing_lvl=%d\n", 
+			coex_dm->pre_fw_dac_swing_lvl, coex_dm->cur_fw_dac_swing_lvl));
+
+		if(coex_dm->pre_fw_dac_swing_lvl == coex_dm->cur_fw_dac_swing_lvl) 
+			return;
+	}
+
+	halbtc8812a1ant_SetFwDacSwingLevel(btcoexist, coex_dm->cur_fw_dac_swing_lvl);
+
+	coex_dm->pre_fw_dac_swing_lvl = coex_dm->cur_fw_dac_swing_lvl;
+}
+
+void
+halbtc8812a1ant_SetSwRfRxLpfCorner(
+	 	PBTC_COEXIST		btcoexist,
+	 	BOOLEAN			rx_rf_shrink_on
+	)
+{
+	if(rx_rf_shrink_on)
+	{
+		//Shrink RF Rx LPF corner
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Shrink RF Rx LPF corner!!\n"));
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1e, 0xfffff, 0xf0ff7);
+	}
+	else
+	{
+		//Resume RF Rx LPF corner
+		// After initialized, we can use coex_dm->bt_rf0x1e_backup
+		if(btcoexist->bInitilized)
+		{
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Resume RF Rx LPF corner!!\n"));
+			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1e, 0xfffff, coex_dm->bt_rf0x1e_backup);
+		}
+	}
+}
+
+void
+halbtc8812a1ant_RfShrink(
+	 	PBTC_COEXIST		btcoexist,
+	 	BOOLEAN			force_exec,
+	 	BOOLEAN			rx_rf_shrink_on
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s turn Rx RF Shrink = %s\n",  
+		(force_exec? "force to":""), ((rx_rf_shrink_on)? "ON":"OFF")));
+	coex_dm->cur_rf_rx_lpf_shrink = rx_rf_shrink_on;
+
+	if(!force_exec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], pre_rf_rx_lpf_shrink=%d, cur_rf_rx_lpf_shrink=%d\n", 
+			coex_dm->pre_rf_rx_lpf_shrink, coex_dm->cur_rf_rx_lpf_shrink));
+
+		if(coex_dm->pre_rf_rx_lpf_shrink == coex_dm->cur_rf_rx_lpf_shrink) 
+			return;
+	}
+	halbtc8812a1ant_SetSwRfRxLpfCorner(btcoexist, coex_dm->cur_rf_rx_lpf_shrink);
+
+	coex_dm->pre_rf_rx_lpf_shrink = coex_dm->cur_rf_rx_lpf_shrink;
+}
+
+void
+halbtc8812a1ant_SetSwPenaltyTxRateAdaptive(
+	 	PBTC_COEXIST		btcoexist,
+	 	BOOLEAN			low_penalty_ra
+	)
+{
+	u1Byte	u1_tmp;
+
+	u1_tmp = btcoexist->btc_read_1byte(btcoexist, 0x4fd);
+	u1_tmp |= BIT0;
+	if(low_penalty_ra)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Tx rate adaptive, set low penalty!!\n"));
+		u1_tmp &= ~BIT2;
+	}
+	else
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Tx rate adaptive, set normal!!\n"));
+		u1_tmp |= BIT2;
+	}
+
+	btcoexist->btc_write_1byte(btcoexist, 0x4fd, u1_tmp);
+}
+
+void
+halbtc8812a1ant_LowPenaltyRa(
+	 	PBTC_COEXIST		btcoexist,
+	 	BOOLEAN			force_exec,
+	 	BOOLEAN			low_penalty_ra
+	)
+{
+	return;
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s turn LowPenaltyRA = %s\n",  
+		(force_exec? "force to":""), ((low_penalty_ra)? "ON":"OFF")));
+	coex_dm->cur_low_penalty_ra = low_penalty_ra;
+
+	if(!force_exec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], pre_low_penalty_ra=%d, cur_low_penalty_ra=%d\n", 
+			coex_dm->pre_low_penalty_ra, coex_dm->cur_low_penalty_ra));
+
+		if(coex_dm->pre_low_penalty_ra == coex_dm->cur_low_penalty_ra) 
+			return;
+	}
+	halbtc8812a1ant_SetSwPenaltyTxRateAdaptive(btcoexist, coex_dm->cur_low_penalty_ra);
+
+	coex_dm->pre_low_penalty_ra = coex_dm->cur_low_penalty_ra;
+}
+
+void
+halbtc8812a1ant_SetDacSwingReg(
+	 	PBTC_COEXIST		btcoexist,
+	 	u4Byte			level
+	)
+{
+	u1Byte	val=(u1Byte)level;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Write SwDacSwing = 0x%x\n", level));
+	btcoexist->btc_write_1byte_bitmask(btcoexist, 0xc5b, 0x3e, val);
+}
+
+void
+halbtc8812a1ant_SetSwFullTimeDacSwing(
+	 	PBTC_COEXIST		btcoexist,
+	 	BOOLEAN			sw_dac_swing_on,
+	 	u4Byte			sw_dac_swing_lvl
+	)
+{
+	if(sw_dac_swing_on)
+	{
+		halbtc8812a1ant_SetDacSwingReg(btcoexist, sw_dac_swing_lvl);
+	}
+	else
+	{
+		halbtc8812a1ant_SetDacSwingReg(btcoexist, 0x18);
+	}
+}
+
+
+void
+halbtc8812a1ant_DacSwing(
+	 	PBTC_COEXIST		btcoexist,
+	 	BOOLEAN			force_exec,
+	 	BOOLEAN			dac_swing_on,
+	 	u4Byte			dac_swing_lvl
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s turn DacSwing=%s, dac_swing_lvl=0x%x\n",  
+		(force_exec? "force to":""), ((dac_swing_on)? "ON":"OFF"), dac_swing_lvl));
+	coex_dm->cur_dac_swing_on = dac_swing_on;
+	coex_dm->cur_dac_swing_lvl = dac_swing_lvl;
+
+	if(!force_exec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], pre_dac_swing_on=%d, pre_dac_swing_lvl=0x%x, cur_dac_swing_on=%d, cur_dac_swing_lvl=0x%x\n", 
+			coex_dm->pre_dac_swing_on, coex_dm->pre_dac_swing_lvl,
+			coex_dm->cur_dac_swing_on, coex_dm->cur_dac_swing_lvl));
+
+		if( (coex_dm->pre_dac_swing_on == coex_dm->cur_dac_swing_on) &&
+			(coex_dm->pre_dac_swing_lvl == coex_dm->cur_dac_swing_lvl) )
+			return;
+	}
+	delay_ms(30);
+	halbtc8812a1ant_SetSwFullTimeDacSwing(btcoexist, dac_swing_on, dac_swing_lvl);
+
+	coex_dm->pre_dac_swing_on = coex_dm->cur_dac_swing_on;
+	coex_dm->pre_dac_swing_lvl = coex_dm->cur_dac_swing_lvl;
+}
+
+void
+halbtc8812a1ant_SetAdcBackOff(
+	 	PBTC_COEXIST		btcoexist,
+	 	BOOLEAN			adc_back_off
+	)
+{
+	if(adc_back_off)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], BB BackOff Level On!\n"));
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x8db, 0x60, 0x3);
+	}
+	else
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], BB BackOff Level Off!\n"));
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x8db, 0x60, 0x1);
+	}
+}
+
+void
+halbtc8812a1ant_AdcBackOff(
+	 	PBTC_COEXIST		btcoexist,
+	 	BOOLEAN			force_exec,
+	 	BOOLEAN			adc_back_off
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s turn AdcBackOff = %s\n",  
+		(force_exec? "force to":""), ((adc_back_off)? "ON":"OFF")));
+	coex_dm->cur_adc_back_off = adc_back_off;
+
+	if(!force_exec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], pre_adc_back_off=%d, cur_adc_back_off=%d\n", 
+			coex_dm->pre_adc_back_off, coex_dm->cur_adc_back_off));
+
+		if(coex_dm->pre_adc_back_off == coex_dm->cur_adc_back_off) 
+			return;
+	}
+	halbtc8812a1ant_SetAdcBackOff(btcoexist, coex_dm->cur_adc_back_off);
+
+	coex_dm->pre_adc_back_off = coex_dm->cur_adc_back_off;
+}
+
+void
+halbtc8812a1ant_SetAgcTable(
+	 	PBTC_COEXIST		btcoexist,
+	 	BOOLEAN			agc_table_en
+	)
+{
+	u1Byte		rssi_adjust_val=0;
+	
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0xef, 0xfffff, 0x02000);
+	if(agc_table_en)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Agc Table On!\n"));
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b, 0xfffff, 0x3fa58);
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b, 0xfffff, 0x37a58);
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b, 0xfffff, 0x2fa58);
+		rssi_adjust_val = 8;
+	}
+	else
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Agc Table Off!\n"));
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b, 0xfffff, 0x39258);
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b, 0xfffff, 0x31258);
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b, 0xfffff, 0x29258);
+	}
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0xef, 0xfffff, 0x0);
+
+	// set rssi_adjust_val for wifi module.
+	btcoexist->btc_set(btcoexist, BTC_SET_U1_RSSI_ADJ_VAL_FOR_AGC_TABLE_ON, &rssi_adjust_val);
+}
+
+
+void
+halbtc8812a1ant_AgcTable(
+	 	PBTC_COEXIST		btcoexist,
+	 	BOOLEAN			force_exec,
+	 	BOOLEAN			agc_table_en
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s %s Agc Table\n",  
+		(force_exec? "force to":""), ((agc_table_en)? "Enable":"Disable")));
+	coex_dm->cur_agc_table_en = agc_table_en;
+
+	if(!force_exec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], pre_agc_table_en=%d, cur_agc_table_en=%d\n", 
+			coex_dm->pre_agc_table_en, coex_dm->cur_agc_table_en));
+
+		if(coex_dm->pre_agc_table_en == coex_dm->cur_agc_table_en) 
+			return;
+	}
+	halbtc8812a1ant_SetAgcTable(btcoexist, agc_table_en);
+
+	coex_dm->pre_agc_table_en = coex_dm->cur_agc_table_en;
+}
+
+void
+halbtc8812a1ant_SetCoexTable(
+	 	PBTC_COEXIST	btcoexist,
+	 	u4Byte		val0x6c0,
+	 	u4Byte		val0x6c4,
+	 	u4Byte		val0x6c8,
+	 	u1Byte		val0x6cc
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], set coex table, set 0x6c0=0x%x\n", val0x6c0));
+	btcoexist->btc_write_4byte(btcoexist, 0x6c0, val0x6c0);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], set coex table, set 0x6c4=0x%x\n", val0x6c4));
+	btcoexist->btc_write_4byte(btcoexist, 0x6c4, val0x6c4);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], set coex table, set 0x6c8=0x%x\n", val0x6c8));
+	btcoexist->btc_write_4byte(btcoexist, 0x6c8, val0x6c8);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], set coex table, set 0x6cc=0x%x\n", val0x6cc));
+	btcoexist->btc_write_1byte(btcoexist, 0x6cc, val0x6cc);
+}
+
+void
+halbtc8812a1ant_CoexTable(
+	 	PBTC_COEXIST		btcoexist,
+	 	BOOLEAN			force_exec,
+	 	u4Byte			val0x6c0,
+	 	u4Byte			val0x6c4,
+	 	u4Byte			val0x6c8,
+	 	u1Byte			val0x6cc
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s write Coex Table 0x6c0=0x%x, 0x6c4=0x%x, 0x6c8=0x%x, 0x6cc=0x%x\n", 
+		(force_exec? "force to":""), val0x6c0, val0x6c4, val0x6c8, val0x6cc));
+	coex_dm->cur_val0x6c0 = val0x6c0;
+	coex_dm->cur_val0x6c4 = val0x6c4;
+	coex_dm->cur_val0x6c8 = val0x6c8;
+	coex_dm->cur_val0x6cc = val0x6cc;
+
+	if(!force_exec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], pre_val0x6c0=0x%x, pre_val0x6c4=0x%x, pre_val0x6c8=0x%x, pre_val0x6cc=0x%x !!\n", 
+			coex_dm->pre_val0x6c0, coex_dm->pre_val0x6c4, coex_dm->pre_val0x6c8, coex_dm->pre_val0x6cc));
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], cur_val0x6c0=0x%x, cur_val0x6c4=0x%x, cur_val0x6c8=0x%x, cur_val0x6cc=0x%x !!\n", 
+			coex_dm->cur_val0x6c0, coex_dm->cur_val0x6c4, coex_dm->cur_val0x6c8, coex_dm->cur_val0x6cc));
+	
+		if( (coex_dm->pre_val0x6c0 == coex_dm->cur_val0x6c0) &&
+			(coex_dm->pre_val0x6c4 == coex_dm->cur_val0x6c4) &&
+			(coex_dm->pre_val0x6c8 == coex_dm->cur_val0x6c8) &&
+			(coex_dm->pre_val0x6cc == coex_dm->cur_val0x6cc) )
+			return;
+	}
+	halbtc8812a1ant_SetCoexTable(btcoexist, val0x6c0, val0x6c4, val0x6c8, val0x6cc);
+
+	coex_dm->pre_val0x6c0 = coex_dm->cur_val0x6c0;
+	coex_dm->pre_val0x6c4 = coex_dm->cur_val0x6c4;
+	coex_dm->pre_val0x6c8 = coex_dm->cur_val0x6c8;
+	coex_dm->pre_val0x6cc = coex_dm->cur_val0x6cc;
+}
+
+void
+halbtc8812a1ant_SetFwIgnoreWlanAct(
+	 	PBTC_COEXIST		btcoexist,
+	 	BOOLEAN			enable
+	)
+{
+	u1Byte	dataLen=3;
+	u1Byte	buf[5] = {0};
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], %s BT Ignore Wlan_Act\n",
+		(enable? "Enable":"Disable")));
+
+	buf[0] = dataLen;
+	buf[1] = 0x1;			// OP_Code
+	buf[2] = 0x1;			// OP_Code_Length
+	if(enable)
+		buf[3] = 0x1; 		// OP_Code_Content
+	else
+		buf[3] = 0x0;
+		
+	btcoexist->btc_set(btcoexist, BTC_SET_ACT_CTRL_BT_COEX, (PVOID)&buf[0]);	
+}
+
+void
+halbtc8812a1ant_IgnoreWlanAct(
+	 	PBTC_COEXIST		btcoexist,
+	 	BOOLEAN			force_exec,
+	 	BOOLEAN			enable
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s turn Ignore WlanAct %s\n", 
+		(force_exec? "force to":""), (enable? "ON":"OFF")));
+	coex_dm->cur_ignore_wlan_act = enable;
+
+	if(!force_exec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], pre_ignore_wlan_act = %d, cur_ignore_wlan_act = %d!!\n", 
+			coex_dm->pre_ignore_wlan_act, coex_dm->cur_ignore_wlan_act));
+
+		if(coex_dm->pre_ignore_wlan_act == coex_dm->cur_ignore_wlan_act)
+			return;
+	}
+	halbtc8812a1ant_SetFwIgnoreWlanAct(btcoexist, enable);
+
+	coex_dm->pre_ignore_wlan_act = coex_dm->cur_ignore_wlan_act;
+}
+
+void
+halbtc8812a1ant_SetFwPstdma(
+	 	PBTC_COEXIST		btcoexist,
+	 	u1Byte			byte1,
+	 	u1Byte			byte2,
+	 	u1Byte			byte3,
+	 	u1Byte			byte4,
+	 	u1Byte			byte5
+	)
+{
+	u1Byte			h2c_parameter[5] ={0};
+
+	h2c_parameter[0] = byte1;	
+	h2c_parameter[1] = byte2;	
+	h2c_parameter[2] = byte3;
+	h2c_parameter[3] = byte4;
+	h2c_parameter[4] = byte5;
+
+	coex_dm->ps_tdma_para[0] = byte1;
+	coex_dm->ps_tdma_para[1] = byte2;
+	coex_dm->ps_tdma_para[2] = byte3;
+	coex_dm->ps_tdma_para[3] = byte4;
+	coex_dm->ps_tdma_para[4] = byte5;
+	
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], FW write 0x60(5bytes)=0x%x%08x\n", 
+		h2c_parameter[0], 
+		h2c_parameter[1]<<24|h2c_parameter[2]<<16|h2c_parameter[3]<<8|h2c_parameter[4]));
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x60, 5, h2c_parameter);
+}
+
+void
+halbtc8812a1ant_SetLpsRpwm(
+	 	PBTC_COEXIST		btcoexist,
+	 	u1Byte			lps_val,
+	 	u1Byte			rpwm_val
+	)
+{
+	u1Byte	lps=lps_val;
+	u1Byte	rpwm=rpwm_val;
+	
+	btcoexist->btc_set(btcoexist, BTC_SET_U1_1ANT_LPS, &lps);
+	btcoexist->btc_set(btcoexist, BTC_SET_U1_1ANT_RPWM, &rpwm);
+	
+	btcoexist->btc_set(btcoexist, BTC_SET_ACT_INC_FORCE_EXEC_PWR_CMD_CNT, NULL);
+}
+
+void
+halbtc8812a1ant_LpsRpwm(
+	 	PBTC_COEXIST		btcoexist,
+	 	BOOLEAN			force_exec,
+	 	u1Byte			lps_val,
+	 	u1Byte			rpwm_val
+	)
+{
+	BOOLEAN	bForceExecPwrCmd=false;
+	
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s set lps/rpwm=0x%x/0x%x \n", 
+		(force_exec? "force to":""), lps_val, rpwm_val));
+	coex_dm->cur_lps = lps_val;
+	coex_dm->cur_rpwm = rpwm_val;
+
+	if(!force_exec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], pre_lps/cur_lps=0x%x/0x%x, pre_rpwm/cur_rpwm=0x%x/0x%x!!\n", 
+			coex_dm->pre_lps, coex_dm->cur_lps, coex_dm->pre_rpwm, coex_dm->cur_rpwm));
+
+		if( (coex_dm->pre_lps == coex_dm->cur_lps) &&
+			(coex_dm->pre_rpwm == coex_dm->cur_rpwm) )
+		{
+			return;
+		}
+	}
+	halbtc8812a1ant_SetLpsRpwm(btcoexist, lps_val, rpwm_val);
+
+	coex_dm->pre_lps = coex_dm->cur_lps;
+	coex_dm->pre_rpwm = coex_dm->cur_rpwm;
+}
+
+void
+halbtc8812a1ant_SwMechanism1(
+	 	PBTC_COEXIST	btcoexist,	
+	 	BOOLEAN		shrink_rx_lpf,
+	 	BOOLEAN 	low_penalty_ra,
+	 	BOOLEAN		limited_dig, 
+	 	BOOLEAN		bt_lna_constrain
+	) 
+{
+	//halbtc8812a1ant_RfShrink(btcoexist, NORMAL_EXEC, shrink_rx_lpf);
+	//halbtc8812a1ant_LowPenaltyRa(btcoexist, NORMAL_EXEC, low_penalty_ra);
+
+	//no limited DIG
+	//halbtc8812a1ant_SetBtLnaConstrain(btcoexist, NORMAL_EXEC, bt_lna_constrain);
+}
+
+void
+halbtc8812a1ant_SwMechanism2(
+	 	PBTC_COEXIST	btcoexist,	
+	 	BOOLEAN		agc_table_shift,
+	 	BOOLEAN 	adc_back_off,
+	 	BOOLEAN		sw_dac_swing,
+	 	u4Byte		dac_swing_lvl
+	)
+{
+	//halbtc8812a1ant_AgcTable(btcoexist, NORMAL_EXEC, agc_table_shift);
+	//halbtc8812a1ant_AdcBackOff(btcoexist, NORMAL_EXEC, adc_back_off);
+	//halbtc8812a1ant_DacSwing(btcoexist, NORMAL_EXEC, sw_dac_swing, dac_swing_lvl);
+}
+
+void
+halbtc8812a1ant_PsTdma(
+	 	PBTC_COEXIST		btcoexist,
+	 	BOOLEAN			force_exec,
+	 	BOOLEAN			turn_on,
+	 	u1Byte			type
+	)
+{
+	BOOLEAN			bTurnOnByCnt=false;
+	u1Byte			psTdmaTypeByCnt=0, rssi_adjust_val=0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s turn %s PS TDMA, type=%d\n", 
+		(force_exec? "force to":""), (turn_on? "ON":"OFF"), type));
+	coex_dm->cur_ps_tdma_on = turn_on;
+	coex_dm->cur_ps_tdma = type;
+
+	if(!force_exec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], pre_ps_tdma_on = %d, cur_ps_tdma_on = %d!!\n", 
+			coex_dm->pre_ps_tdma_on, coex_dm->cur_ps_tdma_on));
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], pre_ps_tdma = %d, cur_ps_tdma = %d!!\n", 
+			coex_dm->pre_ps_tdma, coex_dm->cur_ps_tdma));
+
+		if( (coex_dm->pre_ps_tdma_on == coex_dm->cur_ps_tdma_on) &&
+			(coex_dm->pre_ps_tdma == coex_dm->cur_ps_tdma) )
+			return;
+	}
+	if(turn_on)
+	{
+		switch(type)
+		{
+			default:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xd3, 0x1a, 0x1a, 0x0, 0x58);
+				break;
+			case 1:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xd3, 0x1a, 0x1a, 0x0, 0x48);
+				rssi_adjust_val = 11;
+				break;
+			case 2:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xd3, 0x12, 0x12, 0x0, 0x48);
+				rssi_adjust_val = 14;
+				break;
+			case 3:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x93, 0x25, 0x3, 0x10, 0x40);
+				break;
+			case 4:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x93, 0x15, 0x3, 0x14, 0x0);
+				rssi_adjust_val = 17;
+				break;
+			case 5:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x61, 0x15, 0x3, 0x31, 0x0);
+				break;
+			case 6:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x13, 0xa, 0x3, 0x0, 0x0);
+				break;
+			case 7:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x13, 0xc, 0x5, 0x0, 0x0);
+				break;
+			case 8:	
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x93, 0x25, 0x3, 0x10, 0x0);
+				break;
+			case 9:	
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xd3, 0xa, 0xa, 0x0, 0x48);
+				rssi_adjust_val = 18;
+				break;
+			case 10:	
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x13, 0xa, 0xa, 0x0, 0x40);
+				break;
+			case 11:	
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xd3, 0x5, 0x5, 0x0, 0x48);
+				rssi_adjust_val = 20;
+				break;
+			case 12:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xeb, 0xa, 0x3, 0x31, 0x18);
+				break;
+
+			case 15:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x13, 0xa, 0x3, 0x8, 0x0);
+				break;
+			case 16:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x93, 0x15, 0x3, 0x10, 0x0);
+				rssi_adjust_val = 18;
+				break;
+
+			case 18:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x93, 0x25, 0x3, 0x10, 0x0);
+				rssi_adjust_val = 14;
+				break;			
+				
+			case 20:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x13, 0x25, 0x25, 0x0, 0x0);
+				break;
+			case 21:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x93, 0x20, 0x3, 0x10, 0x40);
+				break;
+			case 22:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x13, 0x8, 0x8, 0x0, 0x40);
+				break;
+			case 23:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xe3, 0x25, 0x3, 0x31, 0x18);
+				rssi_adjust_val = 22;
+				break;
+			case 24:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xe3, 0x15, 0x3, 0x31, 0x18);
+				rssi_adjust_val = 22;
+				break;
+			case 25:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xe3, 0xa, 0x3, 0x31, 0x18);
+				rssi_adjust_val = 22;
+				break;
+			case 26:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xe3, 0xa, 0x3, 0x31, 0x18);
+				rssi_adjust_val = 22;
+				break;
+			case 27:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xe3, 0x25, 0x3, 0x31, 0x98);
+				rssi_adjust_val = 22;
+				break;
+			case 28:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x69, 0x25, 0x3, 0x31, 0x0);
+				break;
+			case 29:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xab, 0x1a, 0x1a, 0x1, 0x8);
+				break;
+			case 30:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x93, 0x15, 0x3, 0x14, 0x0);
+				break;
+			case 31:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xd3, 0x1a, 0x1a, 0, 0x58);
+				break;
+			case 32:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xab, 0xa, 0x3, 0x31, 0x88);
+				break;
+			case 33:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xa3, 0x25, 0x3, 0x30, 0x88);
+				break;
+			case 34:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xd3, 0x1a, 0x1a, 0x0, 0x8);
+				break;
+			case 35:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xe3, 0x1a, 0x1a, 0x0, 0x8);
+				break;
+			case 36:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xd3, 0x12, 0x3, 0x14, 0x58);
+				break;
+		}
+	}
+	else
+	{
+		// disable PS tdma
+		switch(type)
+		{
+			case 8:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x8, 0x0, 0x0, 0x0, 0x0);
+				btcoexist->btc_write_1byte(btcoexist, 0x92c, 0x4);
+				break;
+			case 0:
+			default:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x0, 0x0, 0x0, 0x0, 0x0);
+				delay_ms(5);
+				btcoexist->btc_write_1byte(btcoexist, 0x92c, 0x20);
+				break;
+			case 9:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x0, 0x0, 0x0, 0x0, 0x0);
+				btcoexist->btc_write_1byte(btcoexist, 0x92c, 0x4);
+				break;
+			case 10:
+				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x0, 0x0, 0x0, 0x8, 0x0);
+				delay_ms(5);
+				btcoexist->btc_write_1byte(btcoexist, 0x92c, 0x20);
+				break;
+		}
+	}
+	rssi_adjust_val =0;
+	btcoexist->btc_set(btcoexist, BTC_SET_U1_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE, &rssi_adjust_val);
+
+	// update pre state
+	coex_dm->pre_ps_tdma_on = coex_dm->cur_ps_tdma_on;
+	coex_dm->pre_ps_tdma = coex_dm->cur_ps_tdma;
+}
+
+void
+halbtc8812a1ant_CoexAllOff(
+	 	PBTC_COEXIST		btcoexist
+	)
+{
+	// fw all off
+	halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 6);
+	halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
+
+	// sw all off
+	halbtc8812a1ant_SwMechanism1(btcoexist,false,false,false,false);
+	halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
+
+
+	// hw all off
+	halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
+}
+
+void
+halbtc8812a1ant_WifiParaAdjust(
+	 	PBTC_COEXIST		btcoexist,
+	 	BOOLEAN			enable
+	)
+{
+	if(enable)
+	{
+		halbtc8812a1ant_LowPenaltyRa(btcoexist, NORMAL_EXEC, true);
+	}
+	else
+	{
+		halbtc8812a1ant_LowPenaltyRa(btcoexist, NORMAL_EXEC, false);
+	}
+}
+
+BOOLEAN
+halbtc8812a1ant_IsCommonAction(
+	 	PBTC_COEXIST		btcoexist
+	)
+{
+	BOOLEAN			common=false, wifi_connected=false, wifi_busy=false;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED, &wifi_connected);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+
+	//halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
+
+	if(!wifi_connected && 
+		BT_8812A_1ANT_BT_STATUS_NON_CONNECTED_IDLE == coex_dm->bt_status)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi non connected-idle + BT non connected-idle!!\n"));
+		halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 6);
+		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
+		
+ 		halbtc8812a1ant_SwMechanism1(btcoexist,false,false,false,false);
+		halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
+
+		common = true;
+	}
+	else if(wifi_connected && 
+		(BT_8812A_1ANT_BT_STATUS_NON_CONNECTED_IDLE == coex_dm->bt_status) )
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi connected + BT non connected-idle!!\n"));
+		halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 6);
+		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, true);
+
+      		halbtc8812a1ant_SwMechanism1(btcoexist,false,false,false,false);
+		halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
+
+		common = true;
+	}
+	else if(!wifi_connected && 
+		(BT_8812A_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status) )
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi non connected-idle + BT connected-idle!!\n"));
+		halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 6);
+		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
+
+		halbtc8812a1ant_SwMechanism1(btcoexist,false,false,false,false);
+		halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
+
+		common = true;
+	}
+	else if(wifi_connected && 
+		(BT_8812A_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status) )
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi connected + BT connected-idle!!\n"));
+		halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 6);
+		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
+
+		halbtc8812a1ant_SwMechanism1(btcoexist,true,true,true,true);
+		halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
+
+		common = true;
+	}
+	else if(!wifi_connected && 
+		(BT_8812A_1ANT_BT_STATUS_CONNECTED_IDLE != coex_dm->bt_status) )
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi non connected-idle + BT Busy!!\n"));
+		halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 6);
+		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
+
+		halbtc8812a1ant_SwMechanism1(btcoexist,false,false,false,false);
+		halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
+		
+		common = true;
+	}
+	else
+	{
+		halbtc8812a1ant_SwMechanism1(btcoexist,true,true,true,true);
+		
+		common = false;
+	}
+	
+	return common;
+}
+
+
+void
+halbtc8812a1ant_TdmaDurationAdjustForAcl(
+	 	PBTC_COEXIST		btcoexist
+	)
+{
+	static s4Byte		up,dn,m,n,wait_count;
+	s4Byte			result;   //0: no change, +1: increase WiFi duration, -1: decrease WiFi duration
+	u1Byte			retry_count=0, bt_info_ext;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], halbtc8812a1ant_TdmaDurationAdjustForAcl()\n"));
+	if(coex_dm->reset_tdma_adjust)
+	{
+		coex_dm->reset_tdma_adjust = false;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], first run TdmaDurationAdjust()!!\n"));
+
+		halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 2);
+		coex_dm->ps_tdma_du_adj_type = 2;
+		//============
+		up = 0;
+		dn = 0;
+		m = 1;
+		n= 3;
+		result = 0;
+		wait_count = 0;
+	}
+	else
+	{
+		//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)	// if ³sÄò n ­Ó2¬í retry count¬°0, «h½Õ¼eWiFi duration
+			{
+				wait_count = 0; 
+				n = 3;
+				up = 0;
+				dn = 0;
+				result = 1; 
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], Increase wifi duration!!\n"));
+			}
+		}
+		else if (retry_count <= 3)	// <=3 retry in the last 2-second duration
+		{
+			up--; 
+			dn++;
+
+			if (up <= 0)
+				up = 0;
+
+			if (dn == 2)	// if ³sÄò 2 ­Ó2¬í retry count< 3, «h½Õ¯¶WiFi duration
+			{
+				if (wait_count <= 2)
+					m++; // ÁקK¤@ª½¦b¨â­Ólevel¤¤¨Ó¦^
+				else
+					m = 1;
+
+				if ( m >= 20) //m ³Ì¤j­È = 20 ' ³Ì¤j120¬í recheck¬O§_½Õ¾ã WiFi duration.
+					m = 20;
+
+				n = 3*m;
+				up = 0;
+				dn = 0;
+				wait_count = 0;
+				result = -1; 
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], Decrease wifi duration for retryCounter<3!!\n"));
+			}
+		}
+		else  //retry count > 3, ¥u­n1¦¸ retry count > 3, «h½Õ¯¶WiFi duration
+		{
+			if (wait_count == 1)
+				m++; // ÁקK¤@ª½¦b¨â­Ólevel¤¤¨Ó¦^
+			else
+				m = 1;
+
+			if ( m >= 20) //m ³Ì¤j­È = 20 ' ³Ì¤j120¬í recheck¬O§_½Õ¾ã WiFi duration.
+				m = 20;
+
+			n = 3*m;
+			up = 0;
+			dn = 0;
+			wait_count = 0; 
+			result = -1;
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], Decrease wifi duration for retryCounter>3!!\n"));
+		}
+
+		if(result == -1)
+		{
+			if( (BT_INFO_8812A_1ANT_A2DP_BASIC_RATE(bt_info_ext)) &&
+				((coex_dm->cur_ps_tdma == 1) ||(coex_dm->cur_ps_tdma == 2)) )
+			{
+				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 9);
+				coex_dm->ps_tdma_du_adj_type = 9;
+			}
+			else if(coex_dm->cur_ps_tdma == 1)
+			{
+				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 2);
+				coex_dm->ps_tdma_du_adj_type = 2;
+			}
+			else if(coex_dm->cur_ps_tdma == 2)
+			{
+				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 9);
+				coex_dm->ps_tdma_du_adj_type = 9;
+			}
+			else if(coex_dm->cur_ps_tdma == 9)
+			{
+				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 11);
+				coex_dm->ps_tdma_du_adj_type = 11;
+			}
+		}
+		else if(result == 1)
+		{
+			if( (BT_INFO_8812A_1ANT_A2DP_BASIC_RATE(bt_info_ext)) &&
+				((coex_dm->cur_ps_tdma == 1) ||(coex_dm->cur_ps_tdma == 2)) )
+			{
+				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 9);
+				coex_dm->ps_tdma_du_adj_type = 9;
+			}
+			else if(coex_dm->cur_ps_tdma == 11)
+			{
+				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 9);
+				coex_dm->ps_tdma_du_adj_type = 9;
+			}
+			else if(coex_dm->cur_ps_tdma == 9)
+			{
+				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 2);
+				coex_dm->ps_tdma_du_adj_type = 2;
+			}
+			else if(coex_dm->cur_ps_tdma == 2)
+			{
+				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 1);
+				coex_dm->ps_tdma_du_adj_type = 1;
+			}
+		}
+
+		if( coex_dm->cur_ps_tdma != 1 &&
+			coex_dm->cur_ps_tdma != 2 &&
+			coex_dm->cur_ps_tdma != 9 &&
+			coex_dm->cur_ps_tdma != 11 )
+		{
+			// recover to previous adjust type
+			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, coex_dm->ps_tdma_du_adj_type);
+		}
+	}
+}
+
+u1Byte
+halbtc8812a1ant_PsTdmaTypeByWifiRssi(
+	 	s4Byte	wifi_rssi,
+	 	s4Byte	pre_wifi_rssi,
+	 	u1Byte	wifi_rssi_thresh
+	)
+{
+	u1Byte	ps_tdma_type=0;
+	
+	if(wifi_rssi > pre_wifi_rssi)
+	{
+		if(wifi_rssi > (wifi_rssi_thresh+5))
+		{
+			ps_tdma_type = 26;
+		}
+		else
+		{
+			ps_tdma_type = 25;
+		}
+	}
+	else
+	{
+		if(wifi_rssi > wifi_rssi_thresh)
+		{
+			ps_tdma_type = 26;
+		}
+		else
+		{
+			ps_tdma_type = 25;
+		}
+	}
+
+	return ps_tdma_type;
+}
+
+void
+halbtc8812a1ant_PsTdmaCheckForPowerSaveState(
+	 	PBTC_COEXIST		btcoexist,
+	 	BOOLEAN			new_ps_state
+	)
+{
+	u1Byte	lps_mode=0x0;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_LPS_MODE, &lps_mode);
+	
+	if(lps_mode)	// already under LPS state
+	{
+		if(new_ps_state)		
+		{
+			// keep state under LPS, do nothing.
+		}
+		else
+		{
+			// will leave LPS state, turn off psTdma first
+			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 0);
+		}
+	}
+	else						// NO PS state
+	{
+		if(new_ps_state)
+		{
+			// will enter LPS state, turn off psTdma first
+			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 0);
+		}
+		else
+		{
+			// keep state under NO PS state, do nothing.
+		}
+	}
+}
+
+// SCO only or SCO+PAN(HS)
+void
+halbtc8812a1ant_ActionSco(
+	 	PBTC_COEXIST		btcoexist
+	)
+{
+	u1Byte	wifi_rssi_state;
+	u4Byte	wifi_bw;
+
+	wifi_rssi_state = halbtc8812a1ant_WifiRssiState(btcoexist, 0, 2, 25, 0);
+
+	halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 4);
+
+	if(halbtc8812a1ant_NeedToDecBtPwr(btcoexist))
+		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, true);
+	else	
+		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
+	
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if(BTC_WIFI_BW_HT40 == wifi_bw)
+	{
+		// sw mechanism
+		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			  halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
+		}
+		else
+		{
+			  halbtc8812a1ant_SwMechanism2(btcoexist,false,true,false,0x18);
+		}
+	}
+	else
+	{
+		// sw mechanism
+		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
+		}
+		else
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
+		}		
+	}
+}
+
+
+void
+halbtc8812a1ant_ActionHid(
+	 	PBTC_COEXIST		btcoexist
+	)
+{
+	u1Byte	wifi_rssi_state, bt_rssi_state;	
+	u4Byte	wifi_bw;
+
+	wifi_rssi_state = halbtc8812a1ant_WifiRssiState(btcoexist, 0, 2, 25, 0);
+	bt_rssi_state = halbtc8812a1ant_BtRssiState(2, 50, 0);
+
+	halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 6);
+
+	if(halbtc8812a1ant_NeedToDecBtPwr(btcoexist))
+		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, true);
+	else	
+		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if(BTC_WIFI_BW_HT40 == wifi_bw)
+	{
+		// sw mechanism
+		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,true,false,false,0x18);
+		}
+		else
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
+		}
+	}
+	else
+	{
+		// sw mechanism
+		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
+		}
+		else
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
+		}		
+	}
+}
+
+//A2DP only / PAN(EDR) only/ A2DP+PAN(HS)
+void
+halbtc8812a1ant_ActionA2dp(
+	 	PBTC_COEXIST		btcoexist
+	)
+{
+	u1Byte		wifi_rssi_state, bt_rssi_state;
+	u4Byte		wifi_bw;
+
+	wifi_rssi_state = halbtc8812a1ant_WifiRssiState(btcoexist, 0, 2, 25, 0);
+	bt_rssi_state = halbtc8812a1ant_BtRssiState(2, 50, 0);
+
+	halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 6);
+
+	if(halbtc8812a1ant_NeedToDecBtPwr(btcoexist))
+		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, true);
+	else	
+		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if(BTC_WIFI_BW_HT40 == wifi_bw)
+	{
+		// sw mechanism
+		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
+		}
+		else
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,false,true,false,0x18);
+		}
+	}
+	else
+	{
+		// sw mechanism
+		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
+		}
+		else
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
+		}		
+	}
+}
+
+void
+halbtc8812a1ant_ActionA2dpPanHs(
+	 	PBTC_COEXIST		btcoexist
+	)
+{
+	u1Byte		wifi_rssi_state, bt_rssi_state, bt_info_ext;
+	u4Byte		wifi_bw;
+
+	bt_info_ext = coex_sta->bt_info_ext;
+	wifi_rssi_state = halbtc8812a1ant_WifiRssiState(btcoexist, 0, 2, 25, 0);
+	bt_rssi_state = halbtc8812a1ant_BtRssiState(2, 50, 0);
+
+	halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 6);
+
+	if(halbtc8812a1ant_NeedToDecBtPwr(btcoexist))
+		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, true);
+	else	
+		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if(BTC_WIFI_BW_HT40 == wifi_bw)
+	{
+		// sw mechanism
+		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
+		}
+		else
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,false,true,false,0x18);
+		}
+	}
+	else
+	{
+		// sw mechanism
+		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
+		}
+		else
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
+		}		
+	}
+}
+
+void
+halbtc8812a1ant_ActionPanEdr(
+	 	PBTC_COEXIST		btcoexist
+	)
+{
+	u1Byte		wifi_rssi_state, bt_rssi_state;
+	u4Byte		wifi_bw;
+
+	wifi_rssi_state = halbtc8812a1ant_WifiRssiState(btcoexist, 0, 2, 25, 0);
+	bt_rssi_state = halbtc8812a1ant_BtRssiState(2, 50, 0);
+
+	halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 6);
+
+	if(halbtc8812a1ant_NeedToDecBtPwr(btcoexist))
+		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, true);
+	else	
+		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if(BTC_WIFI_BW_HT40 == wifi_bw)
+	{
+		// sw mechanism
+		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
+		}
+		else
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,false,true,false,0x18);
+		}
+	}
+	else
+	{
+		// sw mechanism
+		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
+		}
+		else
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
+		}
+	}
+}
+
+
+//PAN(HS) only
+void
+halbtc8812a1ant_ActionPanHs(
+	 	PBTC_COEXIST		btcoexist
+	)
+{
+	u1Byte		wifi_rssi_state, bt_rssi_state;
+	u4Byte		wifi_bw;
+
+	wifi_rssi_state = halbtc8812a1ant_WifiRssiState(btcoexist, 0, 2, 25, 0);
+	bt_rssi_state = halbtc8812a1ant_BtRssiState(2, 50, 0);
+
+	halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 6);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if(BTC_WIFI_BW_HT40 == wifi_bw)
+	{
+		// fw mechanism
+		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, true);
+		}
+		else
+		{
+			halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
+		}
+
+		// sw mechanism
+		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
+		}
+		else
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,false,true,false,0x18);
+		}
+	}
+	else
+	{
+		// fw mechanism
+		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, true);
+		}
+		else
+		{
+			halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
+		}
+
+		// sw mechanism
+		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
+		}
+		else
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
+		}
+	}
+}
+
+//PAN(EDR)+A2DP
+void
+halbtc8812a1ant_ActionPanEdrA2dp(
+	 	PBTC_COEXIST		btcoexist
+	)
+{
+	u1Byte		wifi_rssi_state, bt_rssi_state, bt_info_ext;
+	u4Byte		wifi_bw;
+
+	bt_info_ext = coex_sta->bt_info_ext;
+	wifi_rssi_state = halbtc8812a1ant_WifiRssiState(btcoexist, 0, 2, 25, 0);
+	bt_rssi_state = halbtc8812a1ant_BtRssiState(2, 50, 0);
+
+	halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 6);
+
+	if(halbtc8812a1ant_NeedToDecBtPwr(btcoexist))
+		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, true);
+	else	
+		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if(BTC_WIFI_BW_HT40 == wifi_bw)
+	{
+		// sw mechanism
+		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
+		}
+		else
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,false,true,false,0x18);
+		}
+	}
+	else
+	{
+		// sw mechanism
+		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
+		}
+		else
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
+		}
+	}
+}
+
+void
+halbtc8812a1ant_ActionPanEdrHid(
+	 	PBTC_COEXIST		btcoexist
+	)
+{
+	u1Byte		wifi_rssi_state, bt_rssi_state;
+	u4Byte		wifi_bw;
+
+	wifi_rssi_state = halbtc8812a1ant_WifiRssiState(btcoexist, 0, 2, 25, 0);
+	bt_rssi_state = halbtc8812a1ant_BtRssiState(2, 50, 0);
+
+	halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 6);
+
+	if(halbtc8812a1ant_NeedToDecBtPwr(btcoexist))
+		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, true);
+	else	
+		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if(BTC_WIFI_BW_HT40 == wifi_bw)
+	{
+		// sw mechanism
+		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
+		}
+		else
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,false,true,false,0x18);
+		}
+	}
+	else
+	{		
+		// sw mechanism
+		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
+		}
+		else
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
+		}
+	}
+}
+
+// HID+A2DP+PAN(EDR)
+void
+halbtc8812a1ant_ActionHidA2dpPanEdr(
+	 	PBTC_COEXIST		btcoexist
+	)
+{
+	u1Byte		wifi_rssi_state, bt_rssi_state, bt_info_ext;
+	u4Byte		wifi_bw;
+
+	bt_info_ext = coex_sta->bt_info_ext;
+	wifi_rssi_state = halbtc8812a1ant_WifiRssiState(btcoexist, 0, 2, 25, 0);
+	bt_rssi_state = halbtc8812a1ant_BtRssiState(2, 50, 0);
+
+	halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 6);
+
+	if(halbtc8812a1ant_NeedToDecBtPwr(btcoexist))
+		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, true);
+	else	
+		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if(BTC_WIFI_BW_HT40 == wifi_bw)
+	{	
+		// sw mechanism
+		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
+		}
+		else
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,false,true,false,0x18);
+		}
+	}
+	else
+	{
+		// sw mechanism
+		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
+		}
+		else
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
+		}
+	}
+}
+
+void
+halbtc8812a1ant_ActionHidA2dp(
+	 	PBTC_COEXIST		btcoexist
+	)
+{
+	u1Byte		wifi_rssi_state, bt_rssi_state, bt_info_ext;
+	u4Byte		wifi_bw;
+
+	bt_info_ext = coex_sta->bt_info_ext;
+	wifi_rssi_state = halbtc8812a1ant_WifiRssiState(btcoexist, 0, 2, 25, 0);
+	bt_rssi_state = halbtc8812a1ant_BtRssiState(2, 50, 0);
+
+	if(halbtc8812a1ant_NeedToDecBtPwr(btcoexist))
+		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, true);
+	else	
+		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if(BTC_WIFI_BW_HT40 == wifi_bw)
+	{		
+		// sw mechanism
+		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
+		}
+		else
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,false,true,false,0x18);
+		}
+	}
+	else
+	{
+		// sw mechanism
+		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
+		}
+		else
+		{
+			halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
+		}
+	}
+}
+
+void
+halbtc8812a1ant_ActionHs(
+	 	PBTC_COEXIST		btcoexist,
+	 	BOOLEAN			hs_connecting
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action for HS, hs_connecting=%d!!!\n", hs_connecting));
+	halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 8);
+
+	if(hs_connecting)
+	{
+		halbtc8812a1ant_CoexTable(btcoexist, FORCE_EXEC, 0xaaaaaaaa, 0xaaaaaaaa, 0xffff, 0x3);
+	}
+	else
+	{
+		if((coex_sta->high_priority_tx+coex_sta->high_priority_rx+
+			coex_sta->low_priority_tx+coex_sta->low_priority_rx)<=1200)
+			halbtc8812a1ant_CoexTable(btcoexist, FORCE_EXEC, 0xaaaaaaaa, 0xaaaaaaaa, 0xffff, 0x3);
+		else
+			halbtc8812a1ant_CoexTable(btcoexist, FORCE_EXEC, 0xffffffff, 0xffffffff, 0xffff, 0x3);	
+	}
+}
+
+
+void
+halbtc8812a1ant_ActionWifiNotConnected(
+	 	PBTC_COEXIST		btcoexist
+	)
+{
+	BOOLEAN		hs_connecting=false;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_CONNECTING, &hs_connecting);
+	
+	halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
+	btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
+
+	if(hs_connecting)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HS is connecting!!!\n"));
+		halbtc8812a1ant_ActionHs(btcoexist, hs_connecting);
+	}
+	else
+	{
+		halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 8);
+		halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
+	}
+}
+
+void
+halbtc8812a1ant_ActionWifiNotConnectedAssoAuthScan(
+	 	PBTC_COEXIST		btcoexist
+	)
+{
+	PBTC_STACK_INFO	stack_info=&btcoexist->stack_info;
+	BOOLEAN			hs_connecting=false;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_CONNECTING, &hs_connecting);
+	halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
+	btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
+
+	if(hs_connecting)
+	{
+		halbtc8812a1ant_ActionHs(btcoexist, hs_connecting);
+	}
+	else if(btcoexist->bt_info.bt_disabled)
+	{
+		halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 9);
+		halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
+	}
+	else if(BT_8812A_1ANT_BT_STATUS_INQ_PAGE == coex_dm->bt_status)
+{
+		halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 30);
+		halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
+	}
+	else if( (BT_8812A_1ANT_BT_STATUS_NON_CONNECTED_IDLE == coex_dm->bt_status) ||
+		(BT_8812A_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status) )
+	{
+		halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 28);
+		halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
+	}
+	else if(BT_8812A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status)
+	{
+		if(stack_info->hid_exist)
+			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 35);
+		else
+			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 29);
+		halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
+	}
+	else if( (BT_8812A_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
+		(BT_8812A_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status) )
+		{
+		halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 8);
+		halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x5aea5aea, 0x5aea5aea, 0xffff, 0x3);
+	}
+	else 
+	{
+		//error condition, should not reach here, record error number for debugging.
+		coex_dm->error_condition = 1;
+	}
+}
+
+void
+halbtc8812a1ant_ActionWifiConnectedScan(
+	 	PBTC_COEXIST		btcoexist
+	)
+{
+	PBTC_STACK_INFO	stack_info=&btcoexist->stack_info;
+	
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], ActionConnectedScan()===>\n"));
+
+	if(btcoexist->bt_info.bt_disabled)
+	{
+		halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
+		halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 9);
+		halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
+	}
+	else
+	{
+		halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, true);
+		halbtc8812a1ant_LpsRpwm(btcoexist, NORMAL_EXEC, 0x0, 0x4);
+		// power save must executed before psTdma.
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_ENTER_LPS, NULL);
+
+		// psTdma
+		if(BT_8812A_1ANT_BT_STATUS_INQ_PAGE == coex_dm->bt_status)
+		{
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], ActionConnectedScan(), bt is under inquiry/page scan\n"));
+			if(stack_info->sco_exist)
+			{
+				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 32);
+				halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
+			}
+			else
+			{
+				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 30);
+				halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
+			}
+		}
+		else if( (BT_8812A_1ANT_BT_STATUS_NON_CONNECTED_IDLE == coex_dm->bt_status) ||
+			(BT_8812A_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status) )
+		{
+			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 5);
+			halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
+		}
+		else if(BT_8812A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status)
+		{
+			if(stack_info->hid_exist)
+		{
+				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 34);
+				halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
+			}
+			else
+			{
+				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 4);
+				halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
+			}
+		}
+		else if( (BT_8812A_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
+			(BT_8812A_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status) )
+		{
+			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 33);
+			halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
+		}
+		else 
+		{
+			//error condition, should not reach here
+			coex_dm->error_condition = 2;
+		}
+	}
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], ActionConnectedScan()<===\n"));
+}
+
+void
+halbtc8812a1ant_ActionWifiConnectedSpecialPacket(
+	 	PBTC_COEXIST		btcoexist
+	)
+{
+	PBTC_STACK_INFO	stack_info=&btcoexist->stack_info;
+
+	halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
+	btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
+
+	if(btcoexist->bt_info.bt_disabled)
+	{	
+		halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 9);
+		halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
+	}
+	else
+	{
+		if(BT_8812A_1ANT_BT_STATUS_INQ_PAGE == coex_dm->bt_status)
+		{
+			if(stack_info->sco_exist)
+		{
+				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 32);
+				halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
+			}
+			else
+			{
+				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 30);
+				halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
+			}
+		}
+		else if( (BT_8812A_1ANT_BT_STATUS_NON_CONNECTED_IDLE == coex_dm->bt_status) ||
+			(BT_8812A_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status) )
+		{
+			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 28);
+			halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
+		}
+		else if(BT_8812A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status)
+		{
+			if(stack_info->hid_exist)
+				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 35);
+			else
+				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 29);
+			halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
+		}
+		else if( (BT_8812A_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
+			(BT_8812A_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status) )
+		{
+			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 8);
+			halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x5aea5aea, 0x5aea5aea, 0xffff, 0x3);
+		}
+		else 
+		{
+			//error condition, should not reach here
+			coex_dm->error_condition = 3;
+		}
+	}
+}
+
+void
+halbtc8812a1ant_ActionWifiConnected(
+	 	PBTC_COEXIST		btcoexist
+	)
+{
+	PBTC_STACK_INFO	stack_info=&btcoexist->stack_info;
+	BOOLEAN 	wifi_connected=false, wifi_busy=false, bt_hs_on=false;
+	BOOLEAN		scan=false, link=false, roam=false;
+	BOOLEAN		hs_connecting=false, under4way=false;
+	u4Byte		wifi_bw;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], CoexForWifiConnect()===>\n"));
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED, &wifi_connected);
+	if(!wifi_connected)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], CoexForWifiConnect(), return for wifi not connected<===\n"));
+		return;
+	}
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS, &under4way);
+	if(under4way)
+	{
+		halbtc8812a1ant_ActionWifiConnectedSpecialPacket(btcoexist);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], CoexForWifiConnect(), return for wifi is under 4way<===\n"));
+		return;
+	}
+	
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_CONNECTING, &hs_connecting);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
+	if(scan || link || roam)
+	{
+		halbtc8812a1ant_ActionWifiConnectedScan(btcoexist);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], CoexForWifiConnect(), return for wifi is under scan<===\n"));
+		return;
+	}
+	
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);	
+	if(!wifi_busy)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], wifi associated-idle!!!\n"));
+		if(btcoexist->bt_info.bt_disabled)
+		{
+			halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, true);
+			halbtc8812a1ant_LpsRpwm(btcoexist, NORMAL_EXEC, 0x0, 0x4);
+			btcoexist->btc_set(btcoexist, BTC_SET_ACT_ENTER_LPS, NULL);
+			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 9);
+			halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
+		}
+		else
+		{
+			if(BT_8812A_1ANT_BT_STATUS_INQ_PAGE == coex_dm->bt_status)
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], bt is under inquiry/page scan!!!\n"));
+				if(stack_info->sco_exist)
+				{
+					halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 32);
+					halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
+				}
+				else
+				{
+					halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, true);
+					halbtc8812a1ant_LpsRpwm(btcoexist, NORMAL_EXEC, 0x0, 0x4);
+					// power save must executed before psTdma.
+					btcoexist->btc_set(btcoexist, BTC_SET_ACT_ENTER_LPS, NULL);
+					halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 30);
+					halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
+				}
+			}
+			else if(BT_8812A_1ANT_BT_STATUS_NON_CONNECTED_IDLE == coex_dm->bt_status)
+			{
+				halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, true);
+				halbtc8812a1ant_LpsRpwm(btcoexist, NORMAL_EXEC, 0x26, 0x0);
+				// power save must executed before psTdma.
+				btcoexist->btc_set(btcoexist, BTC_SET_ACT_ENTER_LPS, NULL);
+				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 9);
+				halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
+			}
+			else if(BT_8812A_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status)
+			{
+				halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, true);
+				halbtc8812a1ant_LpsRpwm(btcoexist, NORMAL_EXEC, 0x26, 0x0);
+				// power save must executed before psTdma.
+				btcoexist->btc_set(btcoexist, BTC_SET_ACT_ENTER_LPS, NULL);
+				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 0);
+				halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
+			}
+			else if(BT_8812A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status)
+			{
+				if(stack_info->hid_exist && stack_info->numOfLink==1)
+				{
+					// hid only
+					halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
+					// power save must executed before psTdma.
+					btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
+					
+					halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 8);
+					halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x5fff5fff, 0x5fff5fff, 0xffff, 0x3);
+					coex_dm->reset_tdma_adjust = true;
+				}
+				else
+				{				
+					halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, true);
+					halbtc8812a1ant_LpsRpwm(btcoexist, NORMAL_EXEC, 0x0, 0x4);
+					// power save must executed before psTdma.
+					btcoexist->btc_set(btcoexist, BTC_SET_ACT_ENTER_LPS, NULL);
+
+					if(stack_info->hid_exist)
+					{
+						if(stack_info->a2dp_exist)
+						{
+							// hid+a2dp
+							halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 2);
+							halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
+						}
+						else if(stack_info->pan_exist)
+						{
+							if(bt_hs_on)
+							{
+								// hid+hs
+								halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 2);
+							}
+							else
+							{
+								// hid+pan
+								halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 2);
+							}
+							halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
+						}
+						else
+						{
+							coex_dm->error_condition = 4;
+						}
+						coex_dm->reset_tdma_adjust = true;
+					}
+					else if(stack_info->a2dp_exist)
+					{
+						if(stack_info->pan_exist)
+						{
+							if(bt_hs_on)
+							{
+								// a2dp+hs
+								halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 2);
+							}
+							else
+							{
+								// a2dp+pan
+								halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 36);
+							}
+							halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
+							coex_dm->reset_tdma_adjust = true;
+						}
+						else
+						{
+							// a2dp only
+							halbtc8812a1ant_TdmaDurationAdjustForAcl(btcoexist);
+							halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
+						}
+					}
+					else if(stack_info->pan_exist)
+					{
+						// pan only
+						if(bt_hs_on)
+						{
+							coex_dm->error_condition = 5;
+						}
+						else
+						{
+							halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 2);
+							halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
+						}
+						coex_dm->reset_tdma_adjust = true;
+					}
+					else
+					{
+						// temp state, do nothing!!!
+						//DbgPrint("error 6, coex_dm->bt_status=%d\n", coex_dm->bt_status);
+						//DbgPrint("error 6, stack_info->numOfLink=%d, stack_info->hid_exist=%d, stack_info->a2dp_exist=%d, stack_info->pan_exist=%d, stack_info->sco_exist=%d\n", 
+							//stack_info->numOfLink, stack_info->hid_exist, stack_info->a2dp_exist, stack_info->pan_exist, stack_info->sco_exist);
+						//coex_dm->error_condition = 6;
+					}
+				}
+			}
+			else if( (BT_8812A_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
+				(BT_8812A_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status) )
+			{
+				halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
+				// power save must executed before psTdma.
+				btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
+
+				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 8);
+				halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x5aea5aea, 0x5aea5aea, 0xffff, 0x3);
+			}
+			else 
+			{
+				//error condition, should not reach here
+				coex_dm->error_condition = 7;
+			}
+		}
+	}
+	else
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], wifi busy!!!\n"));
+		if(btcoexist->bt_info.bt_disabled)
+		{
+			halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
+			btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
+			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 9);
+			halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
+		}
+		else
+		{
+			if(bt_hs_on)
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HS is under progress!!!\n"));
+				//DbgPrint("coex_dm->bt_status = 0x%x\n", coex_dm->bt_status);
+				halbtc8812a1ant_ActionHs(btcoexist, hs_connecting);
+			}
+			else if(BT_8812A_1ANT_BT_STATUS_INQ_PAGE == coex_dm->bt_status)
+			{
+				if(stack_info->sco_exist)
+				{
+					halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 32);
+					halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
+				}
+				else
+				{
+					halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, true);
+					halbtc8812a1ant_LpsRpwm(btcoexist, NORMAL_EXEC, 0x0, 0x4);
+					// power save must executed before psTdma.
+					btcoexist->btc_set(btcoexist, BTC_SET_ACT_ENTER_LPS, NULL);
+					halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 30);
+					halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
+				}
+			}
+			else if(BT_8812A_1ANT_BT_STATUS_NON_CONNECTED_IDLE == coex_dm->bt_status)
+			{
+				halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
+				// power save must executed before psTdma.
+				btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
+				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 5);
+				halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x5a5a5a5a, 0x5a5a5a5a, 0xffff, 0x3);
+			}
+			else if(BT_8812A_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status)
+			{
+				halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
+				// power save must executed before psTdma.
+				btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
+				if(bt_hs_on)
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HS is under progress!!!\n"));
+					halbtc8812a1ant_ActionHs(btcoexist, hs_connecting);
+				}
+				else
+				{
+					halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 5);
+					halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x5a5a5a5a, 0x5a5a5a5a, 0xffff, 0x3);
+				}
+			}
+			else if(BT_8812A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status)
+			{
+				if(stack_info->hid_exist && stack_info->numOfLink==1)
+				{
+					// hid only
+					halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
+					// power save must executed before psTdma.
+					btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
+					
+					halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 8);
+					halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x5fff5fff, 0x5fff5fff, 0xffff, 0x3);
+					coex_dm->reset_tdma_adjust = true;
+				}
+				else
+				{
+					halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, true);
+					halbtc8812a1ant_LpsRpwm(btcoexist, NORMAL_EXEC, 0x0, 0x4);
+					// power save must executed before psTdma.
+					btcoexist->btc_set(btcoexist, BTC_SET_ACT_ENTER_LPS, NULL);
+
+					if(stack_info->hid_exist)
+					{
+						if(stack_info->a2dp_exist)
+						{
+							// hid+a2dp
+							halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 2);
+							halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
+						}
+						else if(stack_info->pan_exist)
+						{
+							if(bt_hs_on)
+							{
+								// hid+hs
+								halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 2);
+							}
+							else
+							{
+								// hid+pan
+								halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 2);
+							}
+							halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
+						}
+						else
+						{
+							coex_dm->error_condition = 8;
+						}
+						coex_dm->reset_tdma_adjust = true;
+					}
+					else if(stack_info->a2dp_exist)
+					{
+						if(stack_info->pan_exist)
+						{
+							if(bt_hs_on)
+							{
+								// a2dp+hs
+								halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 2);
+							}
+							else
+							{
+								// a2dp+pan
+								halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 36);
+							}
+							halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
+							coex_dm->reset_tdma_adjust = true;
+						}
+						else
+						{
+							// a2dp only
+							halbtc8812a1ant_TdmaDurationAdjustForAcl(btcoexist);
+							halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
+						}
+					}
+					else if(stack_info->pan_exist)
+					{
+						// pan only
+						if(bt_hs_on)
+						{
+							coex_dm->error_condition = 9;
+						}
+						else
+						{
+							halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 2);
+							halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
+						}
+						coex_dm->reset_tdma_adjust = true;
+					}
+					else
+					{
+						//DbgPrint("error 10, stack_info->numOfLink=%d, stack_info->hid_exist=%d, stack_info->a2dp_exist=%d, stack_info->pan_exist=%d, stack_info->sco_exist=%d\n", 
+							//stack_info->numOfLink, stack_info->hid_exist, stack_info->a2dp_exist, stack_info->pan_exist, stack_info->sco_exist);
+						coex_dm->error_condition = 10;
+					}
+				}
+			}
+			else if( (BT_8812A_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
+				(BT_8812A_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status) )
+			{
+				halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
+				// power save must executed before psTdma.
+				btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
+
+				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 8);
+				halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x5aea5aea, 0x5aea5aea, 0xffff, 0x3);
+			}
+			else 
+			{
+				//DbgPrint("error 11, coex_dm->bt_status=%d\n", coex_dm->bt_status);
+				//DbgPrint("error 11, stack_info->numOfLink=%d, stack_info->hid_exist=%d, stack_info->a2dp_exist=%d, stack_info->pan_exist=%d, stack_info->sco_exist=%d\n", 
+					//stack_info->numOfLink, stack_info->hid_exist, stack_info->a2dp_exist, stack_info->pan_exist, stack_info->sco_exist);
+				//error condition, should not reach here
+				coex_dm->error_condition = 11;
+			}
+		}
+	}
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], CoexForWifiConnect()<===\n"));
+}
+
+void
+halbtc8812a1ant_RunSwCoexistMechanism(
+	 	PBTC_COEXIST		btcoexist
+	)
+{
+	PBTC_STACK_INFO		stack_info=&btcoexist->stack_info;
+	BOOLEAN				wifi_under5g=false, wifi_busy=false, wifi_connected=false;
+	u1Byte				bt_info_original=0, bt_retry_cnt=0;
+	u1Byte				algorithm=0;
+
+	return;
+	if(stack_info->bProfileNotified)
+	{
+		algorithm = halbtc8812a1ant_ActionAlgorithm(btcoexist);
+		coex_dm->cur_algorithm = algorithm;		
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Algorithm = %d \n", coex_dm->cur_algorithm));
+
+		if(halbtc8812a1ant_IsCommonAction(btcoexist))
+		{
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action common.\n"));
+		}
+		else
+		{
+			switch(coex_dm->cur_algorithm)
+			{
+				case BT_8812A_1ANT_COEX_ALGO_SCO:
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = SCO.\n"));
+					halbtc8812a1ant_ActionSco(btcoexist);
+					break;
+				case BT_8812A_1ANT_COEX_ALGO_HID:
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = HID.\n"));
+					halbtc8812a1ant_ActionHid(btcoexist);
+					break;
+				case BT_8812A_1ANT_COEX_ALGO_A2DP:
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = A2DP.\n"));
+					halbtc8812a1ant_ActionA2dp(btcoexist);
+					break;
+				case BT_8812A_1ANT_COEX_ALGO_A2DP_PANHS:
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = A2DP+PAN(HS).\n"));
+					halbtc8812a1ant_ActionA2dpPanHs(btcoexist);
+					break;
+				case BT_8812A_1ANT_COEX_ALGO_PANEDR:
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = PAN(EDR).\n"));
+					halbtc8812a1ant_ActionPanEdr(btcoexist);
+					break;
+				case BT_8812A_1ANT_COEX_ALGO_PANHS:
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = HS mode.\n"));
+					halbtc8812a1ant_ActionPanHs(btcoexist);
+					break;
+				case BT_8812A_1ANT_COEX_ALGO_PANEDR_A2DP:
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = PAN+A2DP.\n"));
+					halbtc8812a1ant_ActionPanEdrA2dp(btcoexist);
+					break;
+				case BT_8812A_1ANT_COEX_ALGO_PANEDR_HID:
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = PAN(EDR)+HID.\n"));
+					halbtc8812a1ant_ActionPanEdrHid(btcoexist);
+					break;
+				case BT_8812A_1ANT_COEX_ALGO_HID_A2DP_PANEDR:
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = HID+A2DP+PAN.\n"));
+					halbtc8812a1ant_ActionHidA2dpPanEdr(btcoexist);
+					break;
+				case BT_8812A_1ANT_COEX_ALGO_HID_A2DP:
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = HID+A2DP.\n"));
+					halbtc8812a1ant_ActionHidA2dp(btcoexist);
+					break;
+				default:
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = coexist All Off!!\n"));
+					halbtc8812a1ant_CoexAllOff(btcoexist);
+					break;
+			}
+			coex_dm->pre_algorithm = coex_dm->cur_algorithm;
+		}
+	}
+}
+
+void
+halbtc8812a1ant_RunCoexistMechanism(
+	 	PBTC_COEXIST		btcoexist
+	)
+{
+	PBTC_STACK_INFO		stack_info=&btcoexist->stack_info;
+	BOOLEAN				wifi_under5g=false, wifi_busy=false, wifi_connected=false;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], RunCoexistMechanism()===>\n"));
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under5g);
+
+	if(wifi_under5g)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], RunCoexistMechanism(), return for 5G <===\n"));
+		return;
+	}
+
+	if(btcoexist->manual_control)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], RunCoexistMechanism(), return for Manual CTRL <===\n"));
+		return;
+	}
+
+	if(btcoexist->stop_coex_dm)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], RunCoexistMechanism(), return for Stop Coex DM <===\n"));
+		return;
+	}
+
+	halbtc8812a1ant_RunSwCoexistMechanism(btcoexist);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED, &wifi_connected);
+	if(btcoexist->bt_info.bt_disabled)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], bt is disabled!!!\n"));
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+		if(wifi_busy)
+		{			
+			halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
+			btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
+			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 9);
+		}
+		else
+		{
+			halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, true);
+			halbtc8812a1ant_LpsRpwm(btcoexist, NORMAL_EXEC, 0x0, 0x4);
+			// power save must executed before psTdma.
+			btcoexist->btc_set(btcoexist, BTC_SET_ACT_ENTER_LPS, NULL);
+			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 9);
+		}
+		halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
+	}
+	else if(coex_sta->under_ips)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], wifi is under IPS !!!\n"));
+		halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 0);
+		halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
+		halbtc8812a1ant_WifiParaAdjust(btcoexist, false);
+	}
+	else if(!wifi_connected)
+	{
+		BOOLEAN	scan=false, link=false, roam=false;
+		
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], wifi is non connected-idle !!!\n"));
+
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
+
+		if(scan || link || roam)
+			halbtc8812a1ant_ActionWifiNotConnectedAssoAuthScan(btcoexist);
+		else
+			halbtc8812a1ant_ActionWifiNotConnected(btcoexist);
+	}
+	else	// wifi LPS/Busy
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], wifi is NOT under IPS!!!\n"));
+		halbtc8812a1ant_WifiParaAdjust(btcoexist, true);
+		halbtc8812a1ant_ActionWifiConnected(btcoexist);
+	}
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], RunCoexistMechanism()<===\n"));
+}
+
+void
+halbtc8812a1ant_InitCoexDm(
+	 	PBTC_COEXIST		btcoexist
+	)
+{	
+	BOOLEAN		wifi_connected=false;
+	// force to reset coex mechanism
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED, &wifi_connected);
+	if(!wifi_connected) // non-connected scan
+	{
+		halbtc8812a1ant_ActionWifiNotConnected(btcoexist);
+	}
+	else	// wifi is connected
+	{
+		halbtc8812a1ant_ActionWifiConnected(btcoexist);
+	}
+
+	halbtc8812a1ant_FwDacSwingLvl(btcoexist, FORCE_EXEC, 6);
+	halbtc8812a1ant_DecBtPwr(btcoexist, FORCE_EXEC, false);
+
+	// sw all off
+	halbtc8812a1ant_SwMechanism1(btcoexist,false,false,false,false);
+	halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
+
+	halbtc8812a1ant_CoexTable(btcoexist, FORCE_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
+}
+
+//============================================================
+// work around function start with wa_halbtc8812a1ant_
+//============================================================
+//============================================================
+// extern function start with EXhalbtc8812a1ant_
+//============================================================
+void
+EXhalbtc8812a1ant_InitHwConfig(
+	 	PBTC_COEXIST		btcoexist
+	)
+{
+	u4Byte	u4_tmp=0;
+	u2Byte	u2Tmp=0;
+	u1Byte	u1_tmp=0;
+
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], 1Ant Init HW Config!!\n"));
+
+	// backup rf 0x1e value
+	coex_dm->bt_rf0x1e_backup = 
+		btcoexist->btc_get_rf_reg(btcoexist, BTC_RF_A, 0x1e, 0xfffff);
+	
+	//ant sw control to BT
+	 btcoexist->btc_write_4byte(btcoexist, 0x900, 0x00000400);
+	 btcoexist->btc_write_1byte(btcoexist, 0x76d, 0x1);
+	 btcoexist->btc_write_1byte(btcoexist, 0xcb3, 0x77);
+	 btcoexist->btc_write_1byte(btcoexist, 0xcb7, 0x40);	
+
+	// 0x790[5:0]=0x5
+	u1_tmp = btcoexist->btc_read_1byte(btcoexist, 0x790);
+	u1_tmp &= 0xc0;
+	u1_tmp |= 0x5;
+	btcoexist->btc_write_1byte(btcoexist, 0x790, u1_tmp);
+
+	// PTA parameter
+	btcoexist->btc_write_1byte(btcoexist, 0x6cc, 0x0);
+	btcoexist->btc_write_4byte(btcoexist, 0x6c8, 0xffff);
+	btcoexist->btc_write_4byte(btcoexist, 0x6c4, 0x55555555);
+	btcoexist->btc_write_4byte(btcoexist, 0x6c0, 0x55555555);
+
+	// coex parameters
+	btcoexist->btc_write_1byte(btcoexist, 0x778, 0x1);
+
+	// enable counter statistics
+	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0x4);
+
+	// enable PTA
+	btcoexist->btc_write_1byte(btcoexist, 0x40, 0x20);
+
+	// bt clock related
+	u1_tmp = btcoexist->btc_read_1byte(btcoexist, 0x4);
+	u1_tmp |= BIT7;
+	btcoexist->btc_write_1byte(btcoexist, 0x4, u1_tmp);
+
+	// bt clock related
+	u1_tmp = btcoexist->btc_read_1byte(btcoexist, 0x7);
+	u1_tmp |= BIT1;
+	btcoexist->btc_write_1byte(btcoexist, 0x7, u1_tmp);
+}
+
+void
+EXhalbtc8812a1ant_InitCoexDm(
+	 	PBTC_COEXIST		btcoexist
+	)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], Coex Mechanism Init!!\n"));
+
+	btcoexist->stop_coex_dm = false;
+
+	halbtc8812a1ant_InitCoexDm(btcoexist);
+}
+
+void
+EXhalbtc8812a1ant_DisplayCoexInfo(
+	 	PBTC_COEXIST		btcoexist
+	)
+{
+	PBTC_BOARD_INFO		board_info=&btcoexist->boardInfo;
+	PBTC_STACK_INFO		stack_info=&btcoexist->stack_info;
+	pu1Byte				cli_buf=btcoexist->cli_buf;
+	u1Byte				u1_tmp[4], i, bt_info_ext, psTdmaCase=0;
+	u4Byte				u4_tmp[4];
+	BOOLEAN				roam=false, scan=false, link=false, wifi_under5g=false;
+	BOOLEAN				bt_hs_on=false, wifi_busy=false;
+	s4Byte				wifi_rssi=0, bt_hs_rssi=0;
+	u4Byte				wifi_bw, wifiTrafficDir;
+	u1Byte				wifiDot11Chnl, wifiHsChnl;
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n ============[BT Coexist info]============");
+	CL_PRINTF(cli_buf);
+
+	if(btcoexist->manual_control)
+	{
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n ============[Under Manual Control]============");
+		CL_PRINTF(cli_buf);
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n ==========================================");
+		CL_PRINTF(cli_buf);
+	}
+	if(btcoexist->stop_coex_dm)
+	{
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n ============[Coex is STOPPED]============");
+		CL_PRINTF(cli_buf);
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n ==========================================");
+		CL_PRINTF(cli_buf);
+	}
+
+	if(!board_info->bBtExist)
+	{
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n BT not exists !!!");
+		CL_PRINTF(cli_buf);
+		return;
+	}
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ", "Ant PG number/ Ant mechanism:", \
+		board_info->pgAntNum, board_info->btdmAntNum);
+	CL_PRINTF(cli_buf);	
+	
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %d", "BT stack/ hci ext ver", \
+		((stack_info->bProfileNotified)? "Yes":"No"), stack_info->hciVersion);
+	CL_PRINTF(cli_buf);
+	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_BT_FW_VER);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_DOT11_CHNL, &wifiDot11Chnl);
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifiHsChnl);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d(%d)", "Dot11 channel / HsChnl(HsMode)", \
+		wifiDot11Chnl, wifiHsChnl, bt_hs_on);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x ", "H2C Wifi inform bt chnl Info", \
+		coex_dm->wifi_chnl_info[0], coex_dm->wifi_chnl_info[1],
+		coex_dm->wifi_chnl_info[2]);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
+	btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d", "Wifi rssi/ HS rssi", \
+		wifi_rssi, bt_hs_rssi);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ", "Wifi link/ roam/ scan", \
+		link, roam, scan);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under5g);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION, &wifiTrafficDir);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %s/ %s ", "Wifi status", \
+		(wifi_under5g? "5G":"2.4G"),
+		((BTC_WIFI_BW_LEGACY==wifi_bw)? "Legacy": (((BTC_WIFI_BW_HT40==wifi_bw)? "HT40":"HT20"))),
+		((!wifi_busy)? "idle": ((BTC_WIFI_TRAFFIC_TX==wifiTrafficDir)? "uplink":"downlink")));
+	CL_PRINTF(cli_buf);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = [%s/ %d/ %d] ", "BT [status/ rssi/ retryCnt]", \
+		((coex_sta->c2h_bt_inquiry_page)?("inquiry/page scan"):((BT_8812A_1ANT_BT_STATUS_NON_CONNECTED_IDLE == coex_dm->bt_status)? "non-connected idle":
+		(  (BT_8812A_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status)? "connected-idle":"busy"))),
+		coex_sta->bt_rssi, coex_sta->bt_retry_cnt);
+	CL_PRINTF(cli_buf);
+	
+	if(stack_info->bProfileNotified)
+	{			
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d / %d / %d", "SCO/HID/PAN/A2DP", \
+			stack_info->sco_exist, stack_info->hid_exist, stack_info->pan_exist, stack_info->a2dp_exist);
+		CL_PRINTF(cli_buf);	
+
+		btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_BT_LINK_INFO);
+	}
+
+	bt_info_ext = coex_sta->bt_info_ext;
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s", "BT Info A2DP rate", \
+		(bt_info_ext&BIT0)? "Basic rate":"EDR rate");
+	CL_PRINTF(cli_buf);	
+
+	for(i=0; i<BT_INFO_SRC_8812A_1ANT_MAX; i++)
+	{
+		if(coex_sta->bt_info_c2h_cnt[i])
+		{				
+			CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x %02x %02x %02x %02x(%d)", GLBtInfoSrc8812a1Ant[i], \
+				coex_sta->bt_info_c2h[i][0], coex_sta->bt_info_c2h[i][1],
+				coex_sta->bt_info_c2h[i][2], coex_sta->bt_info_c2h[i][3],
+				coex_sta->bt_info_c2h[i][4], coex_sta->bt_info_c2h[i][5],
+				coex_sta->bt_info_c2h[i][6], coex_sta->bt_info_c2h_cnt[i]);
+			CL_PRINTF(cli_buf);
+		}
+	}
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s/%s, (0x%x/0x%x)", "PS state, IPS/LPS, (lps/rpwm)", \
+		((coex_sta->under_ips? "IPS ON":"IPS OFF")),
+		((coex_sta->under_lps? "LPS ON":"LPS OFF")), 
+		btcoexist->bt_info.lps1Ant, 
+		btcoexist->bt_info.rpwm1Ant);
+	CL_PRINTF(cli_buf);
+	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_FW_PWR_MODE_CMD);
+
+	if(!btcoexist->manual_control)
+	{
+		// Sw mechanism	
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Sw mechanism]============");
+		CL_PRINTF(cli_buf);
+	
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d/ %d ", "SM1[ShRf/ LpRA/ LimDig/ btLna]", \
+			coex_dm->cur_rf_rx_lpf_shrink, coex_dm->cur_low_penalty_ra, coex_dm->limited_dig, coex_dm->bCurBtLnaConstrain);
+		CL_PRINTF(cli_buf);
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d(0x%x) ", "SM2[AgcT/ AdcB/ SwDacSwing(lvl)]", \
+			coex_dm->cur_agc_table_en, coex_dm->cur_adc_back_off, coex_dm->cur_dac_swing_on, coex_dm->cur_dac_swing_lvl);
+		CL_PRINTF(cli_buf);
+	
+		// Fw mechanism		
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Fw mechanism]============");
+		CL_PRINTF(cli_buf);	
+
+		psTdmaCase = coex_dm->cur_ps_tdma;
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x %02x %02x case-%d", "PS TDMA", \
+			coex_dm->ps_tdma_para[0], coex_dm->ps_tdma_para[1],
+			coex_dm->ps_tdma_para[2], coex_dm->ps_tdma_para[3],
+			coex_dm->ps_tdma_para[4], psTdmaCase);
+		CL_PRINTF(cli_buf);
+
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x ", "Latest error condition(should be 0)", \
+			coex_dm->error_condition);
+		CL_PRINTF(cli_buf);
+		
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ", "DecBtPwr/ IgnWlanAct", \
+			coex_dm->cur_dec_bt_pwr, coex_dm->cur_ignore_wlan_act);
+		CL_PRINTF(cli_buf);
+	}
+
+	// Hw setting		
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Hw setting]============");
+	CL_PRINTF(cli_buf);	
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "RF-A, 0x1e initVal", \
+		coex_dm->bt_rf0x1e_backup);
+	CL_PRINTF(cli_buf);
+
+	u1_tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x778);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x778", \
+		u1_tmp[0]);
+	CL_PRINTF(cli_buf);
+	
+	u1_tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x92c);
+	u4_tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x930);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x", "0x92c/ 0x930", \
+		(u1_tmp[0]), u4_tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	u1_tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x40);
+	u1_tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x4f);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x", "0x40/ 0x4f", \
+		u1_tmp[0], u1_tmp[1]);
+	CL_PRINTF(cli_buf);
+
+	u4_tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x550);
+	u1_tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x522);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x", "0x550(bcn ctrl)/0x522", \
+		u4_tmp[0], u1_tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	u4_tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xc50);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0xc50(dig)", \
+		u4_tmp[0]);
+	CL_PRINTF(cli_buf);
+
+#if 0
+	u4_tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xf48);
+	u4_tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0xf4c);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x", "0xf48/ 0xf4c (FA cnt)", \
+		u4_tmp[0], u4_tmp[1]);
+	CL_PRINTF(cli_buf);
+#endif
+
+	u4_tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x6c0);
+	u4_tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x6c4);
+	u4_tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x6c8);
+	u1_tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x6cc);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x", "0x6c0/0x6c4/0x6c8/0x6cc(coexTable)", \
+		u4_tmp[0], u4_tmp[1], u4_tmp[2], u1_tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d", "0x770(hp rx[31:16]/tx[15:0])", \
+		coex_sta->high_priority_rx, coex_sta->high_priority_tx);
+	CL_PRINTF(cli_buf);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d", "0x774(lp rx[31:16]/tx[15:0])", \
+		coex_sta->low_priority_rx, coex_sta->low_priority_tx);
+	CL_PRINTF(cli_buf);
+	
+	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_COEX_STATISTICS);
+}
+
+
+void
+EXhalbtc8812a1ant_IpsNotify(
+	 	PBTC_COEXIST		btcoexist,
+	 	u1Byte			type
+	)
+{
+	u4Byte	u4_tmp=0;
+
+	if(btcoexist->manual_control ||	btcoexist->stop_coex_dm)
+		return;
+
+	if(BTC_IPS_ENTER == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], IPS ENTER notify\n"));
+		coex_sta->under_ips = true;
+
+		// 0x4c[23]=1
+		u4_tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
+		u4_tmp |= BIT23;
+		btcoexist->btc_write_4byte(btcoexist, 0x4c, u4_tmp);
+		
+		halbtc8812a1ant_CoexAllOff(btcoexist);
+	}
+	else if(BTC_IPS_LEAVE == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], IPS LEAVE notify\n"));
+		coex_sta->under_ips = false;
+		//halbtc8812a1ant_InitCoexDm(btcoexist);
+	}
+}
+
+void
+EXhalbtc8812a1ant_LpsNotify(
+	 	PBTC_COEXIST		btcoexist,
+	 	u1Byte			type
+	)
+{
+	if(btcoexist->manual_control || btcoexist->stop_coex_dm)
+		return;
+
+	if(BTC_LPS_ENABLE == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], LPS ENABLE notify\n"));
+		coex_sta->under_lps = true;
+	}
+	else if(BTC_IPS_LEAVE == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], LPS DISABLE notify\n"));
+		coex_sta->under_lps = false;
+	}
+}
+
+void
+EXhalbtc8812a1ant_ScanNotify(
+	 	PBTC_COEXIST		btcoexist,
+	 	u1Byte			type
+	)
+{
+	PBTC_STACK_INFO 	stack_info=&btcoexist->stack_info;
+	BOOLEAN 		wifi_connected=false;	
+
+	if(btcoexist->manual_control ||btcoexist->stop_coex_dm)
+		return;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED, &wifi_connected);
+	if(BTC_SCAN_START == type)
+	{	
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], SCAN START notify\n"));
+		if(!wifi_connected)	// non-connected scan
+		{
+			//set 0x550[3]=1 before PsTdma
+			//halbtc8812a1ant_Reg0x550Bit3(btcoexist, true);
+			halbtc8812a1ant_ActionWifiNotConnectedAssoAuthScan(btcoexist);
+		}
+		else	// wifi is connected
+		{
+			halbtc8812a1ant_ActionWifiConnectedScan(btcoexist);
+		}
+	}
+	else if(BTC_SCAN_FINISH == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], SCAN FINISH notify\n"));
+		if(!wifi_connected)	// non-connected scan
+		{
+			//halbtc8812a1ant_Reg0x550Bit3(btcoexist, false);
+			halbtc8812a1ant_ActionWifiNotConnected(btcoexist);
+		}
+		else
+		{
+			halbtc8812a1ant_ActionWifiConnected(btcoexist);
+		}
+	}
+}
+
+void
+EXhalbtc8812a1ant_ConnectNotify(
+	 	PBTC_COEXIST		btcoexist,
+	 	u1Byte			type
+	)
+{
+	PBTC_STACK_INFO 	stack_info=&btcoexist->stack_info;
+	BOOLEAN			wifi_connected=false;	
+
+	if(btcoexist->manual_control ||btcoexist->stop_coex_dm)
+		return;
+
+	if(BTC_ASSOCIATE_START == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], CONNECT START notify\n"));
+		if(btcoexist->bt_info.bt_disabled)
+		{			
+			halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
+			btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
+			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 9);
+		}
+		else
+		{
+			halbtc8812a1ant_ActionWifiNotConnectedAssoAuthScan(btcoexist);
+		}
+	}
+	else if(BTC_ASSOCIATE_FINISH == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], CONNECT FINISH notify\n"));
+		
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED, &wifi_connected);
+		if(!wifi_connected) // non-connected scan
+		{
+			//halbtc8812a1ant_Reg0x550Bit3(btcoexist, false);
+			halbtc8812a1ant_ActionWifiNotConnected(btcoexist);
+		}
+		else
+		{
+			halbtc8812a1ant_ActionWifiConnected(btcoexist);
+		}
+	}
+}
+
+void
+EXhalbtc8812a1ant_MediaStatusNotify(
+	 	PBTC_COEXIST			btcoexist,
+	 	u1Byte				type
+	)
+{
+	u1Byte			dataLen=5;
+	u1Byte			buf[6] = {0};
+	u1Byte			h2c_parameter[3] ={0};
+	BOOLEAN			wifi_under5g=false;
+	u4Byte			wifi_bw;
+	u1Byte			wifi_central_chnl;
+
+	if(btcoexist->manual_control ||btcoexist->stop_coex_dm)
+		return;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under5g);
+
+	// only 2.4G we need to inform bt the chnl mask
+	if(!wifi_under5g)
+	{
+		if(BTC_MEDIA_CONNECT == type)
+		{
+			h2c_parameter[0] = 0x1;
+		}
+		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+		btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_CENTRAL_CHNL, &wifi_central_chnl);
+		h2c_parameter[1] = wifi_central_chnl;
+		if(BTC_WIFI_BW_HT40 == wifi_bw)
+			h2c_parameter[2] = 0x30;
+		else
+			h2c_parameter[2] = 0x20;
+	}
+		
+	coex_dm->wifi_chnl_info[0] = h2c_parameter[0];
+	coex_dm->wifi_chnl_info[1] = h2c_parameter[1];
+	coex_dm->wifi_chnl_info[2] = h2c_parameter[2];
+	
+	buf[0] = dataLen;
+	buf[1] = 0x5;				// OP_Code
+	buf[2] = 0x3;				// OP_Code_Length
+	buf[3] = h2c_parameter[0]; 	// OP_Code_Content
+	buf[4] = h2c_parameter[1];
+	buf[5] = h2c_parameter[2];
+		
+	btcoexist->btc_set(btcoexist, BTC_SET_ACT_CTRL_BT_COEX, (PVOID)&buf[0]);		
+}
+
+void
+EXhalbtc8812a1ant_SpecialPacketNotify(
+	 	PBTC_COEXIST			btcoexist,
+	 	u1Byte				type
+	)
+{
+	BOOLEAN 	bSecurityLink=false;
+
+	if(btcoexist->manual_control ||btcoexist->stop_coex_dm)
+		return;
+
+	//if(type == BTC_PACKET_DHCP)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], special Packet(%d) notify\n", type));
+		if(btcoexist->bt_info.bt_disabled)
+		{
+			halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
+			btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
+			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 9);	
+		}
+		else
+		{
+			halbtc8812a1ant_ActionWifiConnectedSpecialPacket(btcoexist);
+		}
+	}
+}
+
+void
+EXhalbtc8812a1ant_BtInfoNotify(
+	 	PBTC_COEXIST		btcoexist,
+	 	pu1Byte			tmp_buf,
+	 	u1Byte			length
+	)
+{
+	u1Byte			bt_info=0;
+	u1Byte			i, rsp_source=0;
+	static u4Byte		set_bt_lna_cnt=0, set_bt_psd_mode=0;
+	BOOLEAN			bt_busy=false, limited_dig=false;
+	BOOLEAN			wifi_connected=false;
+	BOOLEAN			bRejApAggPkt=false;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify()===>\n"));
+
+
+	rsp_source = tmp_buf[0]&0xf;
+	if(rsp_source >= BT_INFO_SRC_8812A_1ANT_MAX)
+		rsp_source = BT_INFO_SRC_8812A_1ANT_WIFI_FW;
+	coex_sta->bt_info_c2h_cnt[rsp_source]++;
+
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Bt info[%d], length=%d, hex data=[", rsp_source, length));
+	for(i=0; i<length; i++)
+	{
+		coex_sta->bt_info_c2h[rsp_source][i] = tmp_buf[i];
+		if(i == 1)
+			bt_info = tmp_buf[i];
+		if(i == length-1)
+		{
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("0x%2x]\n", tmp_buf[i]));
+		}
+		else
+		{
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("0x%2x, ", tmp_buf[i]));
+		}
+	}
+
+	if(btcoexist->manual_control)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), return for Manual CTRL<===\n"));
+		return;
+	}
+	if(btcoexist->stop_coex_dm)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), return for Coex STOPPED!!<===\n"));
+		return;
+	}
+
+	if(BT_INFO_SRC_8812A_1ANT_WIFI_FW != rsp_source)
+	{
+		coex_sta->bt_retry_cnt =
+			coex_sta->bt_info_c2h[rsp_source][2];
+
+		coex_sta->bt_rssi =
+			coex_sta->bt_info_c2h[rsp_source][3]*2+10;
+
+		coex_sta->bt_info_ext = 
+			coex_sta->bt_info_c2h[rsp_source][4];
+
+		// Here we need to resend some wifi info to BT
+		// because bt is reset and loss of the info.
+		if( (coex_sta->bt_info_ext & BIT1) )
+		{			
+			btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED, &wifi_connected);
+			if(wifi_connected)
+			{
+				EXhalbtc8812a1ant_MediaStatusNotify(btcoexist, BTC_MEDIA_CONNECT);
+			}
+			else
+			{
+				EXhalbtc8812a1ant_MediaStatusNotify(btcoexist, BTC_MEDIA_DISCONNECT);
+			}
+
+			set_bt_psd_mode = 0;
+		}
+
+		// test-chip bt patch doesn't support, temporary remove.
+		// need to add back when mp-chip. 12/20/2012
+#if 0		
+		if(set_bt_psd_mode <= 3)
+		{

+			halbtc8812a1ant_SetBtPsdMode(btcoexist, FORCE_EXEC, 0xd);
+			set_bt_psd_mode++;
+		}
+		
+		if(coex_dm->bCurBtLnaConstrain)
+		{
+			if( (coex_sta->bt_info_ext & BIT2) )
+			{
+			}
+			else
+			{
+				if(set_bt_lna_cnt <= 3)
+				{
+					halbtc8812a1ant_SetBtLnaConstrain(btcoexist, FORCE_EXEC, true);
+					set_bt_lna_cnt++;
+				}
+			}
+		}
+		else
+		{
+			set_bt_lna_cnt = 0;
+		}
+#endif
+		// test-chip bt patch only rsp the status for BT_RSP, 
+		// so temporary we consider the following only under BT_RSP
+		if(BT_INFO_SRC_8812A_1ANT_BT_RSP == rsp_source)
+		{
+			if( (coex_sta->bt_info_ext & BIT3) )
+			{
+			#if 0// temp disable because bt patch report the wrong value.
+				halbtc8812a1ant_IgnoreWlanAct(btcoexist, FORCE_EXEC, false);
+			#endif
+			}
+			else
+			{
+				// BT already NOT ignore Wlan active, do nothing here.
+			}
+
+			if( (coex_sta->bt_info_ext & BIT4) )
+			{
+				// BT auto report already enabled, do nothing
+			}
+			else
+			{
+				halbtc8812a1ant_BtAutoReport(btcoexist, FORCE_EXEC, true);
+			}
+		}
+	}
+		
+	// check BIT2 first ==> check if bt is under inquiry or page scan
+	if(bt_info & BT_INFO_8812A_1ANT_B_INQ_PAGE)
+	{
+		coex_sta->c2h_bt_inquiry_page = true;
+		coex_dm->bt_status = BT_8812A_1ANT_BT_STATUS_INQ_PAGE;
+	}
+	else
+	{
+		coex_sta->c2h_bt_inquiry_page = false;
+		if(!(bt_info&BT_INFO_8812A_1ANT_B_CONNECTION))
+		{
+			coex_dm->bt_status = BT_8812A_1ANT_BT_STATUS_NON_CONNECTED_IDLE;
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), bt non-connected idle!!!\n"));
+		}
+		else if(bt_info == BT_INFO_8812A_1ANT_B_CONNECTION)	// connection exists but no busy
+		{
+			coex_dm->bt_status = BT_8812A_1ANT_BT_STATUS_CONNECTED_IDLE;
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), bt connected-idle!!!\n"));
+		}		
+		else if((bt_info&BT_INFO_8812A_1ANT_B_SCO_ESCO) ||
+			(bt_info&BT_INFO_8812A_1ANT_B_SCO_BUSY))
+		{
+			coex_dm->bt_status = BT_8812A_1ANT_BT_STATUS_SCO_BUSY;
+			bRejApAggPkt = true;
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), bt sco busy!!!\n"));
+		}
+		else if(bt_info&BT_INFO_8812A_1ANT_B_ACL_BUSY)
+		{
+			if(BT_8812A_1ANT_BT_STATUS_ACL_BUSY != coex_dm->bt_status)
+				coex_dm->reset_tdma_adjust = true;
+			coex_dm->bt_status = BT_8812A_1ANT_BT_STATUS_ACL_BUSY;
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), bt acl busy!!!\n"));
+		}
+#if 0
+		else if(bt_info&BT_INFO_8812A_1ANT_B_SCO_ESCO)
+		{
+			coex_dm->bt_status = BT_8812A_1ANT_BT_STATUS_ACL_SCO_BUSY;
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), bt acl/sco busy!!!\n"));
+		}
+#endif
+		else
+		{
+			//DbgPrint("error, undefined bt_info=0x%x\n", bt_info);
+			coex_dm->bt_status = BT_8812A_1ANT_BT_STATUS_MAX;
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), bt non-defined state!!!\n"));
+		}
+
+		// send delete BA to disable aggregation
+		//btcoexist->btc_set(btcoexist, BTC_SET_BL_TO_REJ_AP_AGG_PKT, &bRejApAggPkt);
+	}
+
+	if( (BT_8812A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) ||
+		(BT_8812A_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
+		(BT_8812A_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status) )
+	{
+		bt_busy = true;
+	}
+	else
+	{
+		bt_busy = false;
+	}
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bt_busy);
+
+	if(bt_busy)
+	{
+		limited_dig = true;
+	}
+	else
+	{
+		limited_dig = false;
+	}
+	coex_dm->limited_dig = limited_dig;
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_LIMITED_DIG, &limited_dig);
+
+	halbtc8812a1ant_RunCoexistMechanism(btcoexist);
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify()<===\n"));
+}
+
+void
+EXhalbtc8812a1ant_StackOperationNotify(
+	 	PBTC_COEXIST			btcoexist,
+	 	u1Byte				type
+	)
+{
+	if(BTC_STACK_OP_INQ_PAGE_PAIR_START == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], StackOP Inquiry/page/pair start notify\n"));
+	}
+	else if(BTC_STACK_OP_INQ_PAGE_PAIR_FINISH == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], StackOP Inquiry/page/pair finish notify\n"));
+	}
+}
+
+void
+EXhalbtc8812a1ant_HaltNotify(
+	 	PBTC_COEXIST			btcoexist
+	)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Halt notify\n"));
+
+	halbtc8812a1ant_IgnoreWlanAct(btcoexist, FORCE_EXEC, true);
+	halbtc8812a1ant_PsTdma(btcoexist, FORCE_EXEC, false, 0);
+	btcoexist->btc_write_1byte(btcoexist, 0x4f, 0xf);
+	halbtc8812a1ant_WifiParaAdjust(btcoexist, false);
+}
+
+void
+EXhalbtc8812a1ant_PnpNotify(
+	 	PBTC_COEXIST			btcoexist,
+	 	u1Byte				pnpState
+	)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Pnp notify\n"));
+
+	if(BTC_WIFI_PNP_SLEEP == pnpState)
+	{
+		btcoexist->stop_coex_dm = true;
+		halbtc8812a1ant_IgnoreWlanAct(btcoexist, FORCE_EXEC, true);
+		halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
+		halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 9);	
+	}
+	else if(BTC_WIFI_PNP_WAKE_UP == pnpState)
+	{
+		
+	}
+}
+
+void
+EXhalbtc8812a1ant_Periodical(
+	 	PBTC_COEXIST			btcoexist
+	)
+{
+	BOOLEAN			wifi_under5g=false;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Periodical()===>\n"));
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], 1Ant Periodical!!\n"));
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under5g);
+
+	if(wifi_under5g)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Periodical(), return for 5G<===\n"));
+		halbtc8812a1ant_CoexAllOff(btcoexist);
+		return;
+	}
+
+	halbtc8812a1ant_QueryBtInfo(btcoexist);
+	halbtc8812a1ant_MonitorBtCtr(btcoexist);
+	halbtc8812a1ant_MonitorBtEnableDisable(btcoexist);
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Periodical()<===\n"));
+}
+
+void
+EXhalbtc8812a1ant_DbgControl(
+	 	PBTC_COEXIST			btcoexist,
+	 	u1Byte				opCode,
+	 	u1Byte				opLen,
+	 	pu1Byte				pData
+	)
+{
+	switch(opCode)
+	{
+		case BTC_DBG_SET_COEX_NORMAL:
+			btcoexist->manual_control = false;
+			halbtc8812a1ant_InitCoexDm(btcoexist);
+			break;
+		case BTC_DBG_SET_COEX_WIFI_ONLY:
+			btcoexist->manual_control = true;
+			halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
+			btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
+			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 9);	
+			break;
+		case BTC_DBG_SET_COEX_BT_ONLY:
+			// todo
+			break;
+		default:
+			break;
+	}
+}
+#endif
+
diff --git a/drivers/staging/rtl8821ae/btcoexist/HalBtc8812a1Ant.h b/drivers/staging/rtl8821ae/btcoexist/HalBtc8812a1Ant.h
new file mode 100644
index 000000000000..37bdab5ae9f1
--- /dev/null
+++ b/drivers/staging/rtl8821ae/btcoexist/HalBtc8812a1Ant.h
@@ -0,0 +1,205 @@
+//===========================================
+// The following is for 8812A_1ANT BT Co-exist definition
+//===========================================
+#define	BT_INFO_8812A_1ANT_B_FTP						BIT7
+#define	BT_INFO_8812A_1ANT_B_A2DP					BIT6
+#define	BT_INFO_8812A_1ANT_B_HID						BIT5
+#define	BT_INFO_8812A_1ANT_B_SCO_BUSY				BIT4
+#define	BT_INFO_8812A_1ANT_B_ACL_BUSY				BIT3
+#define	BT_INFO_8812A_1ANT_B_INQ_PAGE				BIT2
+#define	BT_INFO_8812A_1ANT_B_SCO_ESCO				BIT1
+#define	BT_INFO_8812A_1ANT_B_CONNECTION				BIT0
+
+#define	BT_INFO_8812A_1ANT_A2DP_BASIC_RATE(_BT_INFO_EXT_)	\
+		(((_BT_INFO_EXT_&BIT0))? true:false)
+
+#define	BTC_RSSI_COEX_THRESH_TOL_8812A_1ANT		2
+
+#define  
+#define OUT
+
+typedef enum _BT_INFO_SRC_8812A_1ANT{
+	BT_INFO_SRC_8812A_1ANT_WIFI_FW			= 0x0,
+	BT_INFO_SRC_8812A_1ANT_BT_RSP				= 0x1,
+	BT_INFO_SRC_8812A_1ANT_BT_ACTIVE_SEND		= 0x2,
+	BT_INFO_SRC_8812A_1ANT_MAX
+}BT_INFO_SRC_8812A_1ANT,*PBT_INFO_SRC_8812A_1ANT;
+
+typedef enum _BT_8812A_1ANT_BT_STATUS{
+	BT_8812A_1ANT_BT_STATUS_NON_CONNECTED_IDLE	= 0x0,
+	BT_8812A_1ANT_BT_STATUS_CONNECTED_IDLE		= 0x1,
+	BT_8812A_1ANT_BT_STATUS_INQ_PAGE				= 0x2,
+	BT_8812A_1ANT_BT_STATUS_ACL_BUSY				= 0x3,
+	BT_8812A_1ANT_BT_STATUS_SCO_BUSY				= 0x4,
+	BT_8812A_1ANT_BT_STATUS_ACL_SCO_BUSY			= 0x5,
+	BT_8812A_1ANT_BT_STATUS_MAX
+}BT_8812A_1ANT_BT_STATUS,*PBT_8812A_1ANT_BT_STATUS;
+
+typedef enum _BT_8812A_1ANT_COEX_ALGO{
+	BT_8812A_1ANT_COEX_ALGO_UNDEFINED			= 0x0,
+	BT_8812A_1ANT_COEX_ALGO_SCO				= 0x1,
+	BT_8812A_1ANT_COEX_ALGO_HID				= 0x2,
+	BT_8812A_1ANT_COEX_ALGO_A2DP				= 0x3,
+	BT_8812A_1ANT_COEX_ALGO_A2DP_PANHS		= 0x4,
+	BT_8812A_1ANT_COEX_ALGO_PANEDR			= 0x5,
+	BT_8812A_1ANT_COEX_ALGO_PANHS			= 0x6,
+	BT_8812A_1ANT_COEX_ALGO_PANEDR_A2DP		= 0x7,
+	BT_8812A_1ANT_COEX_ALGO_PANEDR_HID		= 0x8,
+	BT_8812A_1ANT_COEX_ALGO_HID_A2DP_PANEDR	= 0x9,
+	BT_8812A_1ANT_COEX_ALGO_HID_A2DP			= 0xa,
+	BT_8812A_1ANT_COEX_ALGO_MAX				= 0xb,
+}BT_8812A_1ANT_COEX_ALGO,*PBT_8812A_1ANT_COEX_ALGO;
+
+typedef struct _COEX_DM_8812A_1ANT{
+	// fw mechanism
+	bool		pre_dec_bt_pwr;
+	bool		cur_dec_bt_pwr;
+	bool		bPreBtLnaConstrain;
+	bool		bCurBtLnaConstrain;
+	u8		bPreBtPsdMode;
+	u8		bCurBtPsdMode;
+	u8		pre_fw_dac_swing_lvl;
+	u8		cur_fw_dac_swing_lvl;
+	bool		cur_ignore_wlan_act;
+	bool		pre_ignore_wlan_act;
+	u8		pre_ps_tdma;
+	u8		cur_ps_tdma;
+	u8		ps_tdma_para[5];
+	u8		ps_tdma_du_adj_type;
+	bool		reset_tdma_adjust;
+	bool		pre_ps_tdma_on;
+	bool		cur_ps_tdma_on;
+	bool		pre_bt_auto_report;
+	bool		cur_bt_auto_report;
+	u8		pre_lps;
+	u8		cur_lps;
+	u8		pre_rpwm;
+	u8		cur_rpwm;
+
+	// sw mechanism
+	bool		pre_rf_rx_lpf_shrink;
+	bool		cur_rf_rx_lpf_shrink;
+	u32		bt_rf0x1e_backup;
+	bool 	pre_low_penalty_ra;
+	bool		cur_low_penalty_ra;
+	bool		pre_dac_swing_on;
+	u32		pre_dac_swing_lvl;
+	bool		cur_dac_swing_on;
+	u32		cur_dac_swing_lvl;
+	bool		pre_adc_back_off;
+	bool		cur_adc_back_off;
+	bool 	pre_agc_table_en;
+	bool		cur_agc_table_en;
+	u32		pre_val0x6c0;
+	u32		cur_val0x6c0;
+	u32		pre_val0x6c4;
+	u32		cur_val0x6c4;
+	u32		pre_val0x6c8;
+	u32		cur_val0x6c8;
+	u8		pre_val0x6cc;
+	u8		cur_val0x6cc;
+	bool		limited_dig;
+
+	// algorithm related
+	u8		pre_algorithm;
+	u8		cur_algorithm;
+	u8		bt_status;
+	u8		wifi_chnl_info[3];
+
+	u8		error_condition;
+} COEX_DM_8812A_1ANT, *PCOEX_DM_8812A_1ANT;
+
+typedef struct _COEX_STA_8812A_1ANT{
+	bool					under_lps;
+	bool					under_ips;
+	u32					high_priority_tx;
+	u32					high_priority_rx;
+	u32					low_priority_tx;
+	u32					low_priority_rx;
+	u8					bt_rssi;
+	u8					pre_bt_rssi_state;
+	u8					pre_wifi_rssi_state[4];
+	bool					c2h_bt_info_req_sent;
+	u8					bt_info_c2h[BT_INFO_SRC_8812A_1ANT_MAX][10];
+	u32					bt_info_c2h_cnt[BT_INFO_SRC_8812A_1ANT_MAX];
+	bool					c2h_bt_inquiry_page;
+	u8					bt_retry_cnt;
+	u8					bt_info_ext;
+}COEX_STA_8812A_1ANT, *PCOEX_STA_8812A_1ANT;
+
+//===========================================
+// The following is interface which will notify coex module.
+//===========================================
+void
+EXhalbtc8812a1ant_InitHwConfig(
+	 	PBTC_COEXIST		btcoexist
+	);
+void
+EXhalbtc8812a1ant_InitCoexDm(
+	 	PBTC_COEXIST		btcoexist
+	);
+void
+EXhalbtc8812a1ant_IpsNotify(
+	 	PBTC_COEXIST		btcoexist,
+	 	u8			type
+	);
+void
+EXhalbtc8812a1ant_LpsNotify(
+	 	PBTC_COEXIST		btcoexist,
+	 	u8			type
+	);
+void
+EXhalbtc8812a1ant_ScanNotify(
+	 	PBTC_COEXIST		btcoexist,
+	 	u8			type
+	);
+void
+EXhalbtc8812a1ant_ConnectNotify(
+	 	PBTC_COEXIST		btcoexist,
+	 	u8			type
+	);
+void
+EXhalbtc8812a1ant_MediaStatusNotify(
+	 	PBTC_COEXIST			btcoexist,
+	 	u8				type
+	);
+void
+EXhalbtc8812a1ant_SpecialPacketNotify(
+	 	PBTC_COEXIST			btcoexist,
+	 	u8				type
+	);
+void
+EXhalbtc8812a1ant_BtInfoNotify(
+	 	PBTC_COEXIST		btcoexist,
+	 	u8			*tmp_buf,
+	 	u8			length
+	);
+void
+EXhalbtc8812a1ant_StackOperationNotify(
+	 	PBTC_COEXIST			btcoexist,
+	 	u8				type
+	);
+void
+EXhalbtc8812a1ant_HaltNotify(
+	 	PBTC_COEXIST			btcoexist
+	);
+void
+EXhalbtc8812a1ant_PnpNotify(
+	 	PBTC_COEXIST			btcoexist,
+	 	u8				pnpState
+	);
+void
+EXhalbtc8812a1ant_Periodical(
+	 	PBTC_COEXIST			btcoexist
+	);
+void
+EXhalbtc8812a1ant_DisplayCoexInfo(
+	 	PBTC_COEXIST		btcoexist
+	);
+void
+EXhalbtc8812a1ant_DbgControl(
+	 	PBTC_COEXIST			btcoexist,
+	 	u8				opCode,
+	 	u8				opLen,
+	 	u8 				*pData
+	);
diff --git a/drivers/staging/rtl8821ae/btcoexist/habtc8723a1ant.c b/drivers/staging/rtl8821ae/btcoexist/habtc8723a1ant.c
new file mode 100644
index 000000000000..e619923ef0ab
--- /dev/null
+++ b/drivers/staging/rtl8821ae/btcoexist/habtc8723a1ant.c
@@ -0,0 +1,1614 @@
+//============================================================
+// Description:
+//
+// This file is for RTL8723A Co-exist mechanism
+//
+// History
+// 2012/08/22 Cosa first check in.
+// 2012/11/14 Cosa Revise for 8723A 1Ant out sourcing.
+//
+//============================================================
+
+//============================================================
+// include files
+//============================================================
+#include "Mp_Precomp.h"
+#if(BT_30_SUPPORT == 1)
+//============================================================
+// Global variables, these are static variables
+//============================================================
+static COEX_DM_8723A_1ANT	GLCoexDm8723a1Ant;
+static PCOEX_DM_8723A_1ANT 	pCoexDm=&GLCoexDm8723a1Ant;
+static COEX_STA_8723A_1ANT	GLCoexSta8723a1Ant;
+static PCOEX_STA_8723A_1ANT	pCoexSta=&GLCoexSta8723a1Ant;
+
+const char *const GLBtInfoSrc8723a1Ant[]={
+	"BT Info[wifi fw]",
+	"BT Info[bt rsp]",
+	"BT Info[bt auto report]",
+};
+
+//============================================================
+// local function proto type if needed
+//============================================================
+//============================================================
+// local function start with halbtc8723a1ant_
+//============================================================
+VOID
+halbtc8723a1ant_Reg0x550Bit3(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bSet
+	)
+{
+	u1Byte	u1tmp=0;
+	
+	u1tmp = pBtCoexist->btc_read_1byte(pBtCoexist, 0x550);
+	if(bSet)
+	{
+		u1tmp |= BIT3;
+	}
+	else
+	{
+		u1tmp &= ~BIT3;
+	}
+	pBtCoexist->btc_write_1byte(pBtCoexist, 0x550, u1tmp);
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], set 0x550[3]=%d\n", (bSet? 1:0)));
+}
+
+VOID
+halbtc8723a1ant_NotifyFwScan(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			scanType
+	)
+{
+	u1Byte			H2C_Parameter[1] ={0};
+	
+	if(BTC_SCAN_START == scanType)
+		H2C_Parameter[0] = 0x1;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], Notify FW for wifi scan, write 0x3b=0x%x\n", 
+		H2C_Parameter[0]));
+
+	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x3b, 1, H2C_Parameter);
+}
+
+VOID
+halbtc8723a1ant_QueryBtInfo(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	u1Byte			H2C_Parameter[1] ={0};
+
+	pCoexSta->bC2hBtInfoReqSent = true;
+
+	H2C_Parameter[0] |= BIT0;	// trigger
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], Query Bt Info, FW write 0x38=0x%x\n", 
+		H2C_Parameter[0]));
+
+	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x38, 1, H2C_Parameter);
+}
+
+VOID
+halbtc8723a1ant_SetSwRfRxLpfCorner(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bRxRfShrinkOn
+	)
+{
+	if(bRxRfShrinkOn)
+	{
+		//Shrink RF Rx LPF corner
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Shrink RF Rx LPF corner!!\n"));
+		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x1e, 0xfffff, 0xf0ff7);
+	}
+	else
+	{
+		//Resume RF Rx LPF corner
+		// After initialized, we can use pCoexDm->btRf0x1eBackup
+		if(pBtCoexist->initilized)
+		{
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Resume RF Rx LPF corner!!\n"));
+			pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x1e, 0xfffff, pCoexDm->btRf0x1eBackup);
+		}
+	}
+}
+
+VOID
+halbtc8723a1ant_RfShrink(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bForceExec,
+	IN	BOOLEAN			bRxRfShrinkOn
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s turn Rx RF Shrink = %s\n",  
+		(bForceExec? "force to":""), ((bRxRfShrinkOn)? "ON":"OFF")));
+	pCoexDm->bCurRfRxLpfShrink = bRxRfShrinkOn;
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], bPreRfRxLpfShrink=%d, bCurRfRxLpfShrink=%d\n", 
+			pCoexDm->bPreRfRxLpfShrink, pCoexDm->bCurRfRxLpfShrink));
+
+		if(pCoexDm->bPreRfRxLpfShrink == pCoexDm->bCurRfRxLpfShrink) 
+			return;
+	}
+	halbtc8723a1ant_SetSwRfRxLpfCorner(pBtCoexist, pCoexDm->bCurRfRxLpfShrink);
+
+	pCoexDm->bPreRfRxLpfShrink = pCoexDm->bCurRfRxLpfShrink;
+}
+
+VOID
+halbtc8723a1ant_SetSwPenaltyTxRateAdaptive(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bLowPenaltyRa
+	)
+{
+	u1Byte	tmpU1;
+
+	tmpU1 = pBtCoexist->btc_read_1byte(pBtCoexist, 0x4fd);
+	tmpU1 |= BIT0;
+	if(bLowPenaltyRa)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Tx rate adaptive, set low penalty!!\n"));
+		tmpU1 &= ~BIT2;
+	}
+	else
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Tx rate adaptive, set normal!!\n"));
+		tmpU1 |= BIT2;
+	}
+
+	pBtCoexist->btc_write_1byte(pBtCoexist, 0x4fd, tmpU1);
+}
+
+VOID
+halbtc8723a1ant_LowPenaltyRa(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bForceExec,
+	IN	BOOLEAN			bLowPenaltyRa
+	)
+{
+	return;
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s turn LowPenaltyRA = %s\n",  
+		(bForceExec? "force to":""), ((bLowPenaltyRa)? "ON":"OFF")));
+	pCoexDm->bCurLowPenaltyRa = bLowPenaltyRa;
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], bPreLowPenaltyRa=%d, bCurLowPenaltyRa=%d\n", 
+			pCoexDm->bPreLowPenaltyRa, pCoexDm->bCurLowPenaltyRa));
+
+		if(pCoexDm->bPreLowPenaltyRa == pCoexDm->bCurLowPenaltyRa) 
+			return;
+	}
+	halbtc8723a1ant_SetSwPenaltyTxRateAdaptive(pBtCoexist, pCoexDm->bCurLowPenaltyRa);
+
+	pCoexDm->bPreLowPenaltyRa = pCoexDm->bCurLowPenaltyRa;
+}
+
+VOID
+halbtc8723a1ant_SetCoexTable(
+	IN	PBTC_COEXIST	pBtCoexist,
+	IN	u4Byte		val0x6c0,
+	IN	u4Byte		val0x6c8,
+	IN	u1Byte		val0x6cc
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], set coex table, set 0x6c0=0x%x\n", val0x6c0));
+	pBtCoexist->btc_write_4byte(pBtCoexist, 0x6c0, val0x6c0);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], set coex table, set 0x6c8=0x%x\n", val0x6c8));
+	pBtCoexist->btc_write_4byte(pBtCoexist, 0x6c8, val0x6c8);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], set coex table, set 0x6cc=0x%x\n", val0x6cc));
+	pBtCoexist->btc_write_1byte(pBtCoexist, 0x6cc, val0x6cc);
+}
+
+VOID
+halbtc8723a1ant_CoexTable(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bForceExec,
+	IN	u4Byte			val0x6c0,
+	IN	u4Byte			val0x6c8,
+	IN	u1Byte			val0x6cc
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s write Coex Table 0x6c0=0x%x, 0x6c8=0x%x, 0x6cc=0x%x\n", 
+		(bForceExec? "force to":""), val0x6c0, val0x6c8, val0x6cc));
+	pCoexDm->curVal0x6c0 = val0x6c0;
+	pCoexDm->curVal0x6c8 = val0x6c8;
+	pCoexDm->curVal0x6cc = val0x6cc;
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], preVal0x6c0=0x%x, preVal0x6c8=0x%x, preVal0x6cc=0x%x !!\n", 
+			pCoexDm->preVal0x6c0, pCoexDm->preVal0x6c8, pCoexDm->preVal0x6cc));
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], curVal0x6c0=0x%x, curVal0x6c8=0x%x, curVal0x6cc=0x%x !!\n", 
+			pCoexDm->curVal0x6c0, pCoexDm->curVal0x6c8, pCoexDm->curVal0x6cc));
+	
+		if( (pCoexDm->preVal0x6c0 == pCoexDm->curVal0x6c0) &&
+			(pCoexDm->preVal0x6c8 == pCoexDm->curVal0x6c8) &&
+			(pCoexDm->preVal0x6cc == pCoexDm->curVal0x6cc) )
+			return;
+	}
+	halbtc8723a1ant_SetCoexTable(pBtCoexist, val0x6c0, val0x6c8, val0x6cc);
+
+	pCoexDm->preVal0x6c0 = pCoexDm->curVal0x6c0;
+	pCoexDm->preVal0x6c8 = pCoexDm->curVal0x6c8;
+	pCoexDm->preVal0x6cc = pCoexDm->curVal0x6cc;
+}
+
+VOID
+halbtc8723a1ant_SetFwIgnoreWlanAct(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bEnable
+	)
+{
+	u1Byte			H2C_Parameter[1] ={0};
+		
+	if(bEnable)
+	{
+		H2C_Parameter[0] |= BIT0;		// function enable
+	}
+	
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], set FW for BT Ignore Wlan_Act, FW write 0x25=0x%x\n", 
+		H2C_Parameter[0]));
+
+	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x25, 1, H2C_Parameter);	
+}
+
+VOID
+halbtc8723a1ant_IgnoreWlanAct(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bForceExec,
+	IN	BOOLEAN			bEnable
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s turn Ignore WlanAct %s\n", 
+		(bForceExec? "force to":""), (bEnable? "ON":"OFF")));
+	pCoexDm->bCurIgnoreWlanAct = bEnable;
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], bPreIgnoreWlanAct = %d, bCurIgnoreWlanAct = %d!!\n", 
+			pCoexDm->bPreIgnoreWlanAct, pCoexDm->bCurIgnoreWlanAct));
+
+		if(pCoexDm->bPreIgnoreWlanAct == pCoexDm->bCurIgnoreWlanAct)
+			return;
+	}
+	halbtc8723a1ant_SetFwIgnoreWlanAct(pBtCoexist, bEnable);
+
+	pCoexDm->bPreIgnoreWlanAct = pCoexDm->bCurIgnoreWlanAct;
+}
+
+VOID
+halbtc8723a1ant_SetFwPstdma(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			type,
+	IN	u1Byte			byte1,
+	IN	u1Byte			byte2,
+	IN	u1Byte			byte3,
+	IN	u1Byte			byte4,
+	IN	u1Byte			byte5
+	)
+{
+	u1Byte			H2C_Parameter[5] ={0};
+	u1Byte			realByte1=byte1, realByte5=byte5;
+	BOOLEAN			bApEnable=FALSE;
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_AP_MODE_ENABLE, &bApEnable);
+
+	// byte1[1:0] != 0 means enable pstdma
+	// for 2Ant bt coexist, if byte1 != 0 means enable pstdma
+	if(byte1)
+	{
+		if(bApEnable)
+		{
+			if(type != 5 && type != 12)
+			{
+				BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], FW for 1Ant AP mode\n"));
+				realByte1 &= ~BIT4;
+				realByte1 |= BIT5;
+
+				realByte5 |= BIT5;
+				realByte5 &= ~BIT6;
+			}
+		}
+	}
+	H2C_Parameter[0] = realByte1;	
+	H2C_Parameter[1] = byte2;	
+	H2C_Parameter[2] = byte3;
+	H2C_Parameter[3] = byte4;
+	H2C_Parameter[4] = realByte5;
+
+	pCoexDm->psTdmaPara[0] = realByte1;
+	pCoexDm->psTdmaPara[1] = byte2;
+	pCoexDm->psTdmaPara[2] = byte3;
+	pCoexDm->psTdmaPara[3] = byte4;
+	pCoexDm->psTdmaPara[4] = realByte5;
+	
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], FW write 0x3a(5bytes)=0x%x%08x\n", 
+		H2C_Parameter[0], 
+		H2C_Parameter[1]<<24|H2C_Parameter[2]<<16|H2C_Parameter[3]<<8|H2C_Parameter[4]));
+
+	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x3a, 5, H2C_Parameter);
+}
+
+VOID
+halbtc8723a1ant_PsTdma(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bForceExec,
+	IN	BOOLEAN			bTurnOn,
+	IN	u1Byte			type
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s turn %s PS TDMA, type=%d\n", 
+		(bForceExec? "force to":""), (bTurnOn? "ON":"OFF"), type));		
+	pCoexDm->bCurPsTdmaOn = bTurnOn;
+	pCoexDm->curPsTdma = type;
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], bPrePsTdmaOn = %d, bCurPsTdmaOn = %d!!\n", 
+			pCoexDm->bPrePsTdmaOn, pCoexDm->bCurPsTdmaOn));
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], prePsTdma = %d, curPsTdma = %d!!\n", 
+			pCoexDm->prePsTdma, pCoexDm->curPsTdma));
+
+		if( (pCoexDm->bPrePsTdmaOn == pCoexDm->bCurPsTdmaOn) &&
+			(pCoexDm->prePsTdma == pCoexDm->curPsTdma) )
+			return;
+	}	
+	if(pCoexDm->bCurPsTdmaOn)
+	{
+		switch(pCoexDm->curPsTdma)
+		{
+			case 1:
+			default:
+				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x13, 0x1a, 0x1a, 0x0, 0x40);
+				break;
+			case 2:
+				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x13, 0x12, 0x12, 0x0, 0x40);
+				break;
+			case 3:
+				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x93, 0x3f, 0x3, 0x10, 0x40);
+				break;
+			case 4:
+				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x93, 0x15, 0x3, 0x10, 0x0);
+				break;
+			case 5:
+				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0xa9, 0x15, 0x3, 0x35, 0xc0);
+				break;
+			
+			case 8: 
+				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x93, 0x25, 0x3, 0x10, 0x0);
+				break;
+			case 9: 
+				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x13, 0xa, 0xa, 0x0, 0x40);
+				break;
+			case 10:	
+				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x13, 0xa, 0xa, 0x0, 0x40);
+				break;
+			case 11:	
+				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x13, 0x5, 0x5, 0x0, 0x40);
+				break;
+			case 12:
+				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0xa9, 0xa, 0x3, 0x15, 0xc0);
+				break;
+	
+			case 18:
+				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x93, 0x25, 0x3, 0x10, 0x0);
+				break;			
+
+			case 20:
+				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x13, 0x2a, 0x2a, 0x0, 0x0);
+				break;
+			case 21:
+				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x93, 0x20, 0x3, 0x10, 0x40);
+				break;
+			case 22:
+				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x13, 0x1a, 0x1a, 0x2, 0x40);
+				break;
+			case 23:
+				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x13, 0x12, 0x12, 0x2, 0x40);
+				break;
+			case 24:
+				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x13, 0xa, 0xa, 0x2, 0x40);
+				break;
+			case 25:
+				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x13, 0x5, 0x5, 0x2, 0x40);
+				break;
+			case 26:
+				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x93, 0x25, 0x3, 0x10, 0x0);
+				break;
+			case 27:
+				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x13, 0x5, 0x5, 0x2, 0x40);
+				break;
+			case 28:
+				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x3, 0x2f, 0x2f, 0x0, 0x0);
+				break;
+
+		}
+	}
+	else
+	{
+		// disable PS tdma
+		switch(pCoexDm->curPsTdma)
+		{
+			case 8:
+				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x8, 0x0, 0x0, 0x0, 0x0);		
+				break;
+			case 0:
+			default:
+				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x0, 0x0, 0x0, 0x0, 0x0);
+				pBtCoexist->btc_write_2byte(pBtCoexist, 0x860, 0x210);
+				break;
+			case 9:
+				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x0, 0x0, 0x0, 0x0, 0x0);
+				pBtCoexist->btc_write_2byte(pBtCoexist, 0x860, 0x110);
+				break;
+
+		}
+	}
+
+	// update pre state
+	pCoexDm->bPrePsTdmaOn = pCoexDm->bCurPsTdmaOn;
+	pCoexDm->prePsTdma = pCoexDm->curPsTdma;
+}
+
+
+VOID
+halbtc8723a1ant_CoexAllOff(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	// fw all off
+	halbtc8723a1ant_IgnoreWlanAct(pBtCoexist, NORMAL_EXEC, FALSE);
+	halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
+
+	// sw all off
+	halbtc8723a1ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, FALSE);
+	halbtc8723a1ant_RfShrink(pBtCoexist, NORMAL_EXEC, FALSE);
+
+	// hw all off
+	halbtc8723a1ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
+}
+
+VOID
+halbtc8723a1ant_InitCoexDm(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	// force to reset coex mechanism
+	halbtc8723a1ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, FALSE);
+}
+
+VOID
+halbtc8723a1ant_BtEnableAction(
+	IN 	PBTC_COEXIST		pBtCoexist
+	)
+{
+	halbtc8723a1ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, FALSE);
+}
+
+VOID
+halbtc8723a1ant_MonitorBtCtr(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	u4Byte 			regHPTxRx, regLPTxRx, u4Tmp;
+	u4Byte			regHPTx=0, regHPRx=0, regLPTx=0, regLPRx=0;
+	u1Byte			u1Tmp;
+	
+	regHPTxRx = 0x770;
+	regLPTxRx = 0x774;
+
+	u4Tmp = pBtCoexist->btc_read_4byte(pBtCoexist, regHPTxRx);
+	regHPTx = u4Tmp & MASKLWORD;
+	regHPRx = (u4Tmp & MASKHWORD)>>16;
+
+	u4Tmp = pBtCoexist->btc_read_4byte(pBtCoexist, regLPTxRx);
+	regLPTx = u4Tmp & MASKLWORD;
+	regLPRx = (u4Tmp & MASKHWORD)>>16;
+		
+	pCoexSta->highPriorityTx = regHPTx;
+	pCoexSta->highPriorityRx = regHPRx;
+	pCoexSta->lowPriorityTx = regLPTx;
+	pCoexSta->lowPriorityRx = regLPRx;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], High Priority Tx/Rx (reg 0x%x)=0x%x(%d)/0x%x(%d)\n", 
+		regHPTxRx, regHPTx, regHPTx, regHPRx, regHPRx));
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], Low Priority Tx/Rx (reg 0x%x)=0x%x(%d)/0x%x(%d)\n", 
+		regLPTxRx, regLPTx, regLPTx, regLPRx, regLPRx));
+
+	// reset counter
+	pBtCoexist->btc_write_1byte(pBtCoexist, 0x76e, 0xc);
+}
+
+VOID
+halbtc8723a1ant_MonitorBtEnableDisable(
+	IN 	PBTC_COEXIST		pBtCoexist
+	)
+{
+	static BOOLEAN	bPreBtDisabled=FALSE;
+	static u4Byte	btDisableCnt=0;
+	BOOLEAN			bBtActive=true, bBtDisabled=FALSE;
+
+	// This function check if bt is disabled
+	
+	if(	pCoexSta->highPriorityTx == 0 &&
+		pCoexSta->highPriorityRx == 0 &&
+		pCoexSta->lowPriorityTx == 0 &&
+		pCoexSta->lowPriorityRx == 0)
+	{
+		bBtActive = FALSE;
+	}
+	if(	pCoexSta->highPriorityTx == 0xffff &&
+		pCoexSta->highPriorityRx == 0xffff &&
+		pCoexSta->lowPriorityTx == 0xffff &&
+		pCoexSta->lowPriorityRx == 0xffff)
+	{
+		bBtActive = FALSE;
+	}
+	if(bBtActive)
+	{
+		btDisableCnt = 0;
+		bBtDisabled = FALSE;
+		pBtCoexist->btc_set(pBtCoexist, BTC_SET_BL_BT_DISABLE, &bBtDisabled);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], BT is enabled !!\n"));
+	}
+	else
+	{
+		btDisableCnt++;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], bt all counters=0, %d times!!\n", 
+				btDisableCnt));
+		if(btDisableCnt >= 2)
+		{
+			bBtDisabled = true;
+			pBtCoexist->btc_set(pBtCoexist, BTC_SET_BL_BT_DISABLE, &bBtDisabled);
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], BT is disabled !!\n"));
+		}
+	}
+	if(bPreBtDisabled != bBtDisabled)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], BT is from %s to %s!!\n", 
+			(bPreBtDisabled ? "disabled":"enabled"), 
+			(bBtDisabled ? "disabled":"enabled")));
+		bPreBtDisabled = bBtDisabled;
+		if(!bBtDisabled)
+		{
+			halbtc8723a1ant_BtEnableAction(pBtCoexist);
+		}
+		else
+		{
+			pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_NORMAL_LPS, NULL);
+		}
+	}
+}
+
+VOID
+halbtc8723a1ant_TdmaDurationAdjust(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	static s4Byte		up,dn,m,n,WaitCount;
+	s4Byte			result;   //0: no change, +1: increase WiFi duration, -1: decrease WiFi duration
+	u1Byte			retryCount=0;
+	u1Byte			btState;
+	BOOLEAN			bScan=FALSE, bLink=FALSE, bRoam=FALSE;
+	u4Byte			wifiBw;
+	
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+	btState = pCoexDm->btStatus;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], TdmaDurationAdjust()\n"));
+	if(pCoexDm->psTdmaGlobalCnt != pCoexDm->psTdmaMonitorCnt)
+	{
+		pCoexDm->psTdmaMonitorCnt = 0;
+		pCoexDm->psTdmaGlobalCnt = 0;
+	}
+	if(pCoexDm->psTdmaMonitorCnt == 0)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], first run BT A2DP + WiFi busy state!!\n"));
+		if(btState == BT_STATE_8723A_1ANT_ACL_ONLY_BUSY)
+		{
+			halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 1);
+			pCoexDm->psTdmaDuAdjType = 1;
+		}
+		else
+		{
+			halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 22);
+			pCoexDm->psTdmaDuAdjType = 22;
+		}
+		//============
+		up = 0;
+		dn = 0;
+		m = 1;
+		n= 3;
+		result = 0;
+		WaitCount = 0;
+	}
+	else
+	{
+		//accquire the BT TRx retry count from BT_Info byte2
+		retryCount = pCoexSta->btRetryCnt;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], retryCount = %d\n", retryCount));
+		result = 0;
+		WaitCount++; 
+		  
+		if(retryCount == 0)  // no retry in the last 2-second duration
+		{
+			up++;
+			dn--;
+
+			if (dn <= 0)
+				dn = 0; 			 
+
+			if(up >= n) // if ³sÄò n ­Ó2¬í retry count¬°0, «h½Õ¼eWiFi duration
+			{
+				WaitCount = 0; 
+				n = 3;
+				up = 0;
+				dn = 0;
+				result = 1; 
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Increase wifi duration!!\n"));
+			}
+		}
+		else if (retryCount <= 3)	// <=3 retry in the last 2-second duration
+		{
+			up--; 
+			dn++;
+
+			if (up <= 0)
+				up = 0;
+
+			if (dn == 2)	// if ³sÄò 2 ­Ó2¬í retry count< 3, «h½Õ¯¶WiFi duration
+			{
+				if (WaitCount <= 2)
+					m++; // ÁקK¤@ª½¦b¨â­Ólevel¤¤¨Ó¦^
+				else
+					m = 1;
+
+				if ( m >= 20) //m ³Ì¤j­È = 20 ' ³Ì¤j120¬í recheck¬O§_½Õ¾ã WiFi duration.
+					m = 20;
+
+				n = 3*m;
+				up = 0;
+				dn = 0;
+				WaitCount = 0;	
+				result = -1; 
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Decrease wifi duration for retryCounter<3!!\n"));
+			}
+		}
+		else  //retry count > 3, ¥u­n1¦¸ retry count > 3, «h½Õ¯¶WiFi duration
+		{
+			if (WaitCount == 1)
+				m++; // ÁקK¤@ª½¦b¨â­Ólevel¤¤¨Ó¦^
+			else
+				m = 1;
+
+			if ( m >= 20) //m ³Ì¤j­È = 20 ' ³Ì¤j120¬í recheck¬O§_½Õ¾ã WiFi duration.
+				m = 20;
+
+			n = 3*m;
+			up = 0;
+			dn = 0;
+			WaitCount = 0; 
+			result = -1;
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Decrease wifi duration for retryCounter>3!!\n"));
+		}
+		
+		{
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BT TxRx counter H+L <= 1200\n"));
+			if(btState != BT_STATE_8723A_1ANT_ACL_ONLY_BUSY)
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], NOT ACL only busy!\n"));
+				if(BTC_WIFI_BW_HT40 != wifiBw)
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], 20MHz\n"));
+					if(result == -1)
+					{
+						if(pCoexDm->curPsTdma == 22)
+						{
+							halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 23);
+							pCoexDm->psTdmaDuAdjType = 23;
+						}
+						else if(pCoexDm->curPsTdma == 23)
+						{
+							halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 24);
+							pCoexDm->psTdmaDuAdjType = 24;
+						}
+						else if(pCoexDm->curPsTdma == 24)
+						{
+							halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 25);
+							pCoexDm->psTdmaDuAdjType = 25;
+						}
+					} 
+					else if (result == 1)
+					{
+						if(pCoexDm->curPsTdma == 25)
+						{
+							halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 24);
+							pCoexDm->psTdmaDuAdjType = 24;
+						}
+						else if(pCoexDm->curPsTdma == 24)
+						{
+							halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 23);
+							pCoexDm->psTdmaDuAdjType = 23;
+						}
+						else if(pCoexDm->curPsTdma == 23)
+						{
+							halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 22);
+							pCoexDm->psTdmaDuAdjType = 22;
+						}
+					}
+					// error handle, if not in the following state,
+					// set psTdma again.
+					if( (pCoexDm->psTdmaDuAdjType != 22) &&
+						(pCoexDm->psTdmaDuAdjType != 23) &&
+						(pCoexDm->psTdmaDuAdjType != 24) &&
+						(pCoexDm->psTdmaDuAdjType != 25) )
+					{
+						BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], duration case out of handle!!\n"));
+						halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 23);
+						pCoexDm->psTdmaDuAdjType = 23;
+					}
+				}
+				else
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], 40MHz\n"));
+					if(result == -1)
+					{
+						if(pCoexDm->curPsTdma == 23)
+						{
+							halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 24);
+							pCoexDm->psTdmaDuAdjType = 24;
+						}
+						else if(pCoexDm->curPsTdma == 24)
+						{
+							halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 25);
+							pCoexDm->psTdmaDuAdjType = 25;
+						}
+						else if(pCoexDm->curPsTdma == 25)
+						{
+							halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 27);
+							pCoexDm->psTdmaDuAdjType = 27;
+						}
+					} 
+					else if (result == 1)
+					{
+						if(pCoexDm->curPsTdma == 27)
+						{
+							halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 25);
+							pCoexDm->psTdmaDuAdjType = 25;
+						}
+						else if(pCoexDm->curPsTdma == 25)
+						{
+							halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 24);
+							pCoexDm->psTdmaDuAdjType = 24;
+						}
+						else if(pCoexDm->curPsTdma == 24)
+						{
+							halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 23);
+							pCoexDm->psTdmaDuAdjType = 23;
+						}
+					}
+					// error handle, if not in the following state,
+					// set psTdma again.
+					if( (pCoexDm->psTdmaDuAdjType != 23) &&
+						(pCoexDm->psTdmaDuAdjType != 24) &&
+						(pCoexDm->psTdmaDuAdjType != 25) &&
+						(pCoexDm->psTdmaDuAdjType != 27) )
+					{
+						BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], duration case out of handle!!\n"));
+						halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 24);
+						pCoexDm->psTdmaDuAdjType = 24;
+					}
+				}
+			}
+			else
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], ACL only busy\n"));
+				if (result == -1)
+				{
+					if(pCoexDm->curPsTdma == 1)
+					{
+						halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+						pCoexDm->psTdmaDuAdjType = 2;
+					}
+					else if(pCoexDm->curPsTdma == 2)
+					{
+						halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+						pCoexDm->psTdmaDuAdjType = 9;
+					}
+					else if(pCoexDm->curPsTdma == 9)
+					{
+						halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					}
+				}
+				else if (result == 1)
+				{
+					if(pCoexDm->curPsTdma == 11)
+					{
+						halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+						pCoexDm->psTdmaDuAdjType = 9;
+					}
+					else if(pCoexDm->curPsTdma == 9)
+					{
+						halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+						pCoexDm->psTdmaDuAdjType = 2;
+					}
+					else if(pCoexDm->curPsTdma == 2)
+					{
+						halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 1);
+						pCoexDm->psTdmaDuAdjType = 1;
+					}
+				}
+
+				// error handle, if not in the following state,
+				// set psTdma again.
+				if( (pCoexDm->psTdmaDuAdjType != 1) &&
+					(pCoexDm->psTdmaDuAdjType != 2) &&
+					(pCoexDm->psTdmaDuAdjType != 9) &&
+					(pCoexDm->psTdmaDuAdjType != 11) )
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], duration case out of handle!!\n"));
+					halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+					pCoexDm->psTdmaDuAdjType = 2;
+				}
+			}
+		}
+	}
+
+	// if current PsTdma not match with the recorded one (when scan, dhcp...), 
+	// then we have to adjust it back to the previous record one.
+	if(pCoexDm->curPsTdma != pCoexDm->psTdmaDuAdjType)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PsTdma type dismatch!!!, curPsTdma=%d, recordPsTdma=%d\n", 
+			pCoexDm->curPsTdma, pCoexDm->psTdmaDuAdjType));
+
+		pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_SCAN, &bScan);
+		pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_LINK, &bLink);
+		pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_ROAM, &bRoam);
+
+		if( !bScan && !bLink &&	!bRoam)
+		{
+			halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, pCoexDm->psTdmaDuAdjType);
+		}
+		else
+		{
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], roaming/link/scan is under progress, will adjust next time!!!\n"));
+		}
+	}
+	pCoexDm->psTdmaMonitorCnt++;
+}
+
+
+VOID
+halbtc8723a1ant_CoexForWifiConnect(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	BOOLEAN		bWifiConnected=FALSE, bWifiBusy=FALSE;
+	u1Byte		btState, btInfoOriginal=0;
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+
+	btState = pCoexDm->btStatus;
+	btInfoOriginal = pCoexSta->btInfoC2h[BT_INFO_SRC_8723A_1ANT_BT_RSP][0];
+
+	if(bWifiConnected)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], wifi connected!!\n"));
+		pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
+		
+		if( !bWifiBusy &&
+			((BT_STATE_8723A_1ANT_NO_CONNECTION == btState) ||
+			(BT_STATE_8723A_1ANT_CONNECT_IDLE == btState)) )
+		{
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], [Wifi is idle] or [Bt is non connected idle or Bt is connected idle]!!\n"));
+
+			if(BT_STATE_8723A_1ANT_NO_CONNECTION == btState)
+				halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 9);
+			else if(BT_STATE_8723A_1ANT_CONNECT_IDLE == btState)
+				halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
+
+			pBtCoexist->btc_setBbReg(pBtCoexist, 0x880, 0xff000000, 0xc0);
+		}
+		else
+		{
+			if( (BT_STATE_8723A_1ANT_SCO_ONLY_BUSY == btState) ||
+				(BT_STATE_8723A_1ANT_ACL_SCO_BUSY == btState) ||
+				(BT_STATE_8723A_1ANT_HID_BUSY == btState) ||
+				(BT_STATE_8723A_1ANT_HID_SCO_BUSY == btState) )
+			{
+				pBtCoexist->btc_setBbReg(pBtCoexist, 0x880, 0xff000000, 0x60);
+			}
+			else
+			{
+				pBtCoexist->btc_setBbReg(pBtCoexist, 0x880, 0xff000000, 0xc0);
+			}
+			switch(btState)
+			{
+				case BT_STATE_8723A_1ANT_NO_CONNECTION:
+					halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 5);
+					break;
+				case BT_STATE_8723A_1ANT_CONNECT_IDLE:
+					halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
+					break;
+				case BT_STATE_8723A_1ANT_INQ_OR_PAG:
+					halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+					break;
+				case BT_STATE_8723A_1ANT_SCO_ONLY_BUSY:
+				case BT_STATE_8723A_1ANT_ACL_SCO_BUSY:
+				case BT_STATE_8723A_1ANT_HID_BUSY:
+				case BT_STATE_8723A_1ANT_HID_SCO_BUSY:
+					halbtc8723a1ant_TdmaDurationAdjust(pBtCoexist);
+					break;
+				case BT_STATE_8723A_1ANT_ACL_ONLY_BUSY:
+					if (btInfoOriginal&BT_INFO_8723A_1ANT_B_A2DP)
+					{
+						halbtc8723a1ant_TdmaDurationAdjust(pBtCoexist);
+					}
+					else if(btInfoOriginal&BT_INFO_8723A_1ANT_B_FTP)
+					{
+						halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 1);
+					}
+					else if( (btInfoOriginal&BT_INFO_8723A_1ANT_B_A2DP) &&
+							(btInfoOriginal&BT_INFO_8723A_1ANT_B_FTP) )
+					{
+						halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+					}
+					else
+					{
+						halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 1);
+					}
+					break;
+				default:
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], error!!!, undefined case in halbtc8723a1ant_CoexForWifiConnect()!!\n"));
+					break;
+			}
+		}
+	}
+	else
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], wifi is disconnected!!\n"));
+	}
+
+	pCoexDm->psTdmaGlobalCnt++;
+}
+
+//============================================================
+// work around function start with wa_halbtc8723a1ant_
+//============================================================
+VOID
+wa_halbtc8723a1ant_MonitorC2h(
+	IN	PBTC_COEXIST			pBtCoexist
+	)
+{
+	u1Byte	tmp1b=0x0;
+	u4Byte	curC2hTotalCnt=0x0;
+	static u4Byte	preC2hTotalCnt=0x0, sameCntPollingTime=0x0;
+
+	curC2hTotalCnt+=pCoexSta->btInfoC2hCnt[BT_INFO_SRC_8723A_1ANT_BT_RSP];
+
+	if(curC2hTotalCnt == preC2hTotalCnt)
+	{
+		sameCntPollingTime++;
+	}
+	else
+	{
+		preC2hTotalCnt = curC2hTotalCnt;
+		sameCntPollingTime = 0;
+	}
+
+	if(sameCntPollingTime >= 2)
+	{
+		tmp1b = pBtCoexist->btc_read_1byte(pBtCoexist, 0x1af);
+		if(tmp1b != 0x0)
+		{
+			pCoexSta->c2hHangDetectCnt++;
+			pBtCoexist->btc_write_1byte(pBtCoexist, 0x1af, 0x0);
+		}
+	}
+}
+
+//============================================================
+// extern function start with EXhalbtc8723a1ant_
+//============================================================
+VOID
+EXhalbtc8723a1ant_InitHwConfig(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], 1Ant Init HW Config!!\n"));
+
+	// backup rf 0x1e value
+	pCoexDm->btRf0x1eBackup = 
+		pBtCoexist->btc_get_rf_reg(pBtCoexist, BTC_RF_A, 0x1e, 0xfffff);
+
+	pBtCoexist->btc_write_1byte(pBtCoexist, 0x40, 0x20);
+
+	// enable counter statistics
+	pBtCoexist->btc_write_1byte(pBtCoexist, 0x76e, 0x4);
+	
+	// coex table
+	pBtCoexist->btc_write_1byte(pBtCoexist, 0x6cc, 0x0);			// 1-Ant coex
+	pBtCoexist->btc_write_4byte(pBtCoexist, 0x6c8, 0xffff);		// wifi break table
+	pBtCoexist->btc_write_4byte(pBtCoexist, 0x6c4, 0x55555555);	//coex table
+
+	// antenna switch control parameter
+	pBtCoexist->btc_write_4byte(pBtCoexist, 0x858, 0xaaaaaaaa);
+	
+	pBtCoexist->btc_write_2byte(pBtCoexist, 0x860, 0x210);	//set antenna at wifi side if ANTSW is software control
+	pBtCoexist->btc_write_4byte(pBtCoexist, 0x870, 0x300);	//SPDT(connected with TRSW) control by hardware PTA
+	pBtCoexist->btc_write_4byte(pBtCoexist, 0x874, 0x22804000);	//ANTSW keep by GNT_BT
+
+	// coexistence parameters
+	pBtCoexist->btc_write_1byte(pBtCoexist, 0x778, 0x1);	// enable RTK mode PTA
+}
+
+VOID
+EXhalbtc8723a1ant_InitCoexDm(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], Coex Mechanism Init!!\n"));
+	
+	halbtc8723a1ant_InitCoexDm(pBtCoexist);
+}
+
+VOID
+EXhalbtc8723a1ant_DisplayCoexInfo(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	struct btc_board_info *		pBoardInfo=&pBtCoexist->board_info;
+	PBTC_STACK_INFO		pStackInfo=&pBtCoexist->stack_info;
+	pu1Byte				cliBuf=pBtCoexist->cli_buf;
+	u1Byte				u1Tmp[4], i, btInfoExt, psTdmaCase=0;
+	u4Byte				u4Tmp[4];
+	BOOLEAN				bRoam=FALSE, bScan=FALSE, bLink=FALSE, bWifiUnder5G=FALSE;
+	BOOLEAN				bBtHsOn=FALSE, bWifiBusy=FALSE;
+	s4Byte				wifiRssi=0, btHsRssi=0;
+	u4Byte				wifiBw, wifiTrafficDir;
+	u1Byte				wifiDot11Chnl, wifiHsChnl;
+
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n ============[BT Coexist info]============");
+	CL_PRINTF(cliBuf);
+
+	if(!pBoardInfo->bt_exist)
+	{
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n BT not exists !!!");
+		CL_PRINTF(cliBuf);
+		return;
+	}
+
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ", "Ant PG number/ Ant mechanism:", \
+		pBoardInfo->pg_ant_num, pBoardInfo->btdm_ant_num);
+	CL_PRINTF(cliBuf);	
+	
+	if(pBtCoexist->manual_control)
+	{
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "[Action Manual control]!!");
+		CL_PRINTF(cliBuf);
+	}
+	
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %d", "BT stack/ hci ext ver", \
+		((pStackInfo->bProfileNotified)? "Yes":"No"), pStackInfo->hciVersion);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U1_WIFI_DOT11_CHNL, &wifiDot11Chnl);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifiHsChnl);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d(%d)", "Dot11 channel / HsChnl(HsMode)", \
+		wifiDot11Chnl, wifiHsChnl, bBtHsOn);
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x ", "H2C Wifi inform bt chnl Info", \
+		pCoexDm->wifiChnlInfo[0], pCoexDm->wifiChnlInfo[1],
+		pCoexDm->wifiChnlInfo[2]);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_S4_WIFI_RSSI, &wifiRssi);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_S4_HS_RSSI, &btHsRssi);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d", "Wifi rssi/ HS rssi", \
+		wifiRssi, btHsRssi);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_SCAN, &bScan);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_LINK, &bLink);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_ROAM, &bRoam);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ", "Wifi bLink/ bRoam/ bScan", \
+		bLink, bRoam, bScan);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_UNDER_5G, &bWifiUnder5G);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION, &wifiTrafficDir);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %s/ %s ", "Wifi status", \
+		(bWifiUnder5G? "5G":"2.4G"),
+		((BTC_WIFI_BW_LEGACY==wifiBw)? "Legacy": (((BTC_WIFI_BW_HT40==wifiBw)? "HT40":"HT20"))),
+		((!bWifiBusy)? "idle": ((BTC_WIFI_TRAFFIC_TX==wifiTrafficDir)? "uplink":"downlink")));
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = [%s/ %d/ %d] ", "BT [status/ rssi/ retryCnt]", \
+		((pCoexSta->bC2hBtInquiryPage)?("inquiry/page scan"):((BT_8723A_1ANT_BT_STATUS_IDLE == pCoexDm->btStatus)? "idle":(  (BT_8723A_1ANT_BT_STATUS_CONNECTED_IDLE == pCoexDm->btStatus)? "connected-idle":"busy"))),
+		pCoexSta->btRssi, pCoexSta->btRetryCnt);
+	CL_PRINTF(cliBuf);
+	
+	if(pStackInfo->bProfileNotified)
+	{			
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d / %d / %d", "SCO/HID/PAN/A2DP", \
+			pStackInfo->bScoExist, pStackInfo->bHidExist, pStackInfo->bPanExist, pStackInfo->bA2dpExist);
+		CL_PRINTF(cliBuf);	
+
+		pBtCoexist->btc_disp_dbg_msg(pBtCoexist, BTC_DBG_DISP_BT_LINK_INFO);
+	}
+
+	btInfoExt = pCoexSta->btInfoExt;
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s", "BT Info A2DP rate", \
+		(btInfoExt&BIT0)? "Basic rate":"EDR rate");
+	CL_PRINTF(cliBuf);	
+
+	for(i=0; i<BT_INFO_SRC_8723A_1ANT_MAX; i++)
+	{
+		if(pCoexSta->btInfoC2hCnt[i])
+		{				
+			CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x %02x %02x %02x %02x(%d)", GLBtInfoSrc8723a1Ant[i], \
+				pCoexSta->btInfoC2h[i][0], pCoexSta->btInfoC2h[i][1],
+				pCoexSta->btInfoC2h[i][2], pCoexSta->btInfoC2h[i][3],
+				pCoexSta->btInfoC2h[i][4], pCoexSta->btInfoC2h[i][5],
+				pCoexSta->btInfoC2h[i][6], pCoexSta->btInfoC2hCnt[i]);
+			CL_PRINTF(cliBuf);
+		}
+	}
+
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d", "write 0x1af=0x0 num", \
+		pCoexSta->c2hHangDetectCnt);
+	CL_PRINTF(cliBuf);
+	
+	// Sw mechanism	
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Sw mechanism]============");
+	CL_PRINTF(cliBuf);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d", "SM1[ShRf/ LpRA/ LimDig]", \
+		pCoexDm->bCurRfRxLpfShrink, pCoexDm->bCurLowPenaltyRa, pCoexDm->limited_dig);
+	CL_PRINTF(cliBuf);
+
+	// Fw mechanism		
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Fw mechanism]============");
+	CL_PRINTF(cliBuf);	
+	
+	if(!pBtCoexist->manual_control)
+	{
+		psTdmaCase = pCoexDm->curPsTdma;
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x %02x %02x case-%d", "PS TDMA", \
+			pCoexDm->psTdmaPara[0], pCoexDm->psTdmaPara[1],
+			pCoexDm->psTdmaPara[2], pCoexDm->psTdmaPara[3],
+			pCoexDm->psTdmaPara[4], psTdmaCase);
+		CL_PRINTF(cliBuf);
+	
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d ", "IgnWlanAct", \
+			pCoexDm->bCurIgnoreWlanAct);
+		CL_PRINTF(cliBuf);
+	}
+
+	// Hw setting		
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Hw setting]============");
+	CL_PRINTF(cliBuf);	
+
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "RF-A, 0x1e initVal", \
+		pCoexDm->btRf0x1eBackup);
+	CL_PRINTF(cliBuf);
+
+	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x778);
+	u1Tmp[1] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x783);
+	u1Tmp[2] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x796);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x", "0x778/ 0x783/ 0x796", \
+		u1Tmp[0], u1Tmp[1], u1Tmp[2]);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x880);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x880", \
+		u4Tmp[0]);
+	CL_PRINTF(cliBuf);
+
+	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x40);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x40", \
+		u1Tmp[0]);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x550);
+	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x522);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x", "0x550(bcn ctrl)/0x522", \
+		u4Tmp[0], u1Tmp[0]);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x484);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x484(rate adaptive)", \
+		u4Tmp[0]);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0xc50);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0xc50(dig)", \
+		u4Tmp[0]);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0xda0);
+	u4Tmp[1] = pBtCoexist->btc_read_4byte(pBtCoexist, 0xda4);
+	u4Tmp[2] = pBtCoexist->btc_read_4byte(pBtCoexist, 0xda8);
+	u4Tmp[3] = pBtCoexist->btc_read_4byte(pBtCoexist, 0xdac);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x", "0xda0/0xda4/0xda8/0xdac(FA cnt)", \
+		u4Tmp[0], u4Tmp[1], u4Tmp[2], u4Tmp[3]);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x6c0);
+	u4Tmp[1] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x6c4);
+	u4Tmp[2] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x6c8);
+	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x6cc);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x", "0x6c0/0x6c4/0x6c8/0x6cc(coexTable)", \
+		u4Tmp[0], u4Tmp[1], u4Tmp[2], u1Tmp[0]);
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d", "0x770 (hp rx[31:16]/tx[15:0])", \
+		pCoexSta->highPriorityRx, pCoexSta->highPriorityTx);
+	CL_PRINTF(cliBuf);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d", "0x774(lp rx[31:16]/tx[15:0])", \
+		pCoexSta->lowPriorityRx, pCoexSta->lowPriorityTx);
+	CL_PRINTF(cliBuf);
+
+	// Tx mgnt queue hang or not, 0x41b should = 0xf, ex: 0xd ==>hang
+	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x41b);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x41b (mgntQ hang chk == 0xf)", \
+		u1Tmp[0]);
+	CL_PRINTF(cliBuf);	
+
+	pBtCoexist->btc_disp_dbg_msg(pBtCoexist, BTC_DBG_DISP_COEX_STATISTICS);
+}
+
+
+VOID
+EXhalbtc8723a1ant_IpsNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			type
+	)
+{
+	if(BTC_IPS_ENTER == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], IPS ENTER notify\n"));
+		halbtc8723a1ant_CoexAllOff(pBtCoexist);
+	}
+	else if(BTC_IPS_LEAVE == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], IPS LEAVE notify\n"));
+		//halbtc8723a1ant_InitCoexDm(pBtCoexist);
+	}
+}
+
+VOID
+EXhalbtc8723a1ant_LpsNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			type
+	)
+{
+	if(BTC_LPS_ENABLE == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], LPS ENABLE notify\n"));
+	}
+	else if(BTC_LPS_DISABLE == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], LPS DISABLE notify\n"));
+		halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 8);
+	}
+}
+
+VOID
+EXhalbtc8723a1ant_ScanNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			type
+	)
+{
+	BOOLEAN		bWifiConnected=FALSE;
+	
+	halbtc8723a1ant_NotifyFwScan(pBtCoexist, type);
+
+	if(pBtCoexist->btInfo.bBtDisabled)
+	{
+		halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 9); 
+	}
+	else
+	{
+		pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+		if(BTC_SCAN_START == type)
+		{
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], SCAN START notify\n"));
+			if(!bWifiConnected)	// non-connected scan
+			{
+				//set 0x550[3]=1 before PsTdma
+				halbtc8723a1ant_Reg0x550Bit3(pBtCoexist, true);
+			}
+
+			halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
+		}
+		else if(BTC_SCAN_FINISH == type)
+		{
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], SCAN FINISH notify\n"));
+			if(!bWifiConnected)	// non-connected scan
+			{
+				halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0); 
+			}
+			else
+			{
+				halbtc8723a1ant_CoexForWifiConnect(pBtCoexist);
+			}
+		}
+	}
+}
+
+VOID
+EXhalbtc8723a1ant_ConnectNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			type
+	)
+{
+	BOOLEAN		bWifiConnected=FALSE;
+		
+	if(pBtCoexist->btInfo.bBtDisabled)
+	{
+		halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 9); 
+	}
+	else
+	{
+		if(BTC_ASSOCIATE_START == type)
+		{
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], CONNECT START notify\n"));
+			//set 0x550[3]=1 before PsTdma
+			halbtc8723a1ant_Reg0x550Bit3(pBtCoexist, true);
+			halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);	// extend wifi slot	
+		}
+		else if(BTC_ASSOCIATE_FINISH == type)
+		{
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], CONNECT FINISH notify\n"));
+			pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+			if(!bWifiConnected)	// non-connected scan
+			{
+				halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
+			}
+			else
+			{
+				halbtc8723a1ant_CoexForWifiConnect(pBtCoexist);
+			}
+		}
+	}
+}
+
+VOID
+EXhalbtc8723a1ant_MediaStatusNotify(
+	IN	PBTC_COEXIST			pBtCoexist,
+	IN	u1Byte				type
+	)
+{
+	if(BTC_MEDIA_CONNECT == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], MEDIA connect notify\n"));
+	}
+	else
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], MEDIA disconnect notify\n"));
+	}
+}
+
+VOID
+EXhalbtc8723a1ant_SpecialPacketNotify(
+	IN	PBTC_COEXIST			pBtCoexist,
+	IN	u1Byte				type
+	)
+{
+	if(type == BTC_PACKET_DHCP)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], DHCP Packet notify\n"));
+		if(pBtCoexist->btInfo.bBtDisabled)
+		{
+			halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 9);	
+		}
+		else
+		{
+			halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 18);
+		}		
+	}
+}
+
+VOID
+EXhalbtc8723a1ant_BtInfoNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	pu1Byte			tmpBuf,
+	IN	u1Byte			length
+	)
+{
+	u1Byte			btInfo=0;
+	u1Byte			i, rspSource=0;
+	BOOLEAN			bBtHsOn=FALSE, bBtBusy=FALSE, bForceLps=FALSE;
+
+	pCoexSta->bC2hBtInfoReqSent = FALSE;
+	
+	rspSource = BT_INFO_SRC_8723A_1ANT_BT_RSP;
+	pCoexSta->btInfoC2hCnt[rspSource]++;
+
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Bt info[%d], length=%d, hex data=[", rspSource, length));
+	for(i=0; i<length; i++)
+	{
+		pCoexSta->btInfoC2h[rspSource][i] = tmpBuf[i];
+		if(i == 0)
+			btInfo = tmpBuf[i];
+		if(i == length-1)
+		{
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("0x%02x]\n", tmpBuf[i]));
+		}
+		else
+		{
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("0x%02x, ", tmpBuf[i]));
+		}
+	}
+
+	if(BT_INFO_SRC_8723A_1ANT_WIFI_FW != rspSource)
+	{
+		pCoexSta->btRetryCnt =
+			pCoexSta->btInfoC2h[rspSource][1];
+
+		pCoexSta->btRssi =
+			pCoexSta->btInfoC2h[rspSource][2]*2+10;
+
+		pCoexSta->btInfoExt = 
+			pCoexSta->btInfoC2h[rspSource][3];
+	}
+		
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	// check BIT2 first ==> check if bt is under inquiry or page scan
+	if(btInfo & BT_INFO_8723A_1ANT_B_INQ_PAGE)
+	{
+		pCoexSta->bC2hBtInquiryPage = true;
+	}
+	else
+	{
+		pCoexSta->bC2hBtInquiryPage = FALSE;
+	}
+	btInfo &= ~BIT2;
+	if(!(btInfo & BIT0))
+	{
+		pCoexDm->btStatus = BT_STATE_8723A_1ANT_NO_CONNECTION;
+		bForceLps = FALSE;
+	}
+	else
+	{
+		bForceLps = true;
+		if(btInfo == 0x1)
+		{
+			pCoexDm->btStatus = BT_STATE_8723A_1ANT_CONNECT_IDLE;
+		}
+		else if(btInfo == 0x9)
+		{
+			pCoexDm->btStatus = BT_STATE_8723A_1ANT_ACL_ONLY_BUSY;
+			bBtBusy = true;
+		}
+		else if(btInfo == 0x13)
+		{
+			pCoexDm->btStatus = BT_STATE_8723A_1ANT_SCO_ONLY_BUSY;
+			bBtBusy = true;
+		}
+		else if(btInfo == 0x1b)
+		{
+			pCoexDm->btStatus = BT_STATE_8723A_1ANT_ACL_SCO_BUSY;
+			bBtBusy = true;
+		}
+		else if(btInfo == 0x29)
+		{
+			pCoexDm->btStatus = BT_STATE_8723A_1ANT_HID_BUSY;
+			bBtBusy = true;
+		}
+		else if(btInfo == 0x3b)
+		{
+			pCoexDm->btStatus = BT_STATE_8723A_1ANT_HID_SCO_BUSY;
+			bBtBusy = true;
+		}
+	}
+	pBtCoexist->btc_set(pBtCoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bBtBusy);
+	pBtCoexist->btc_set(pBtCoexist, BTC_SET_BL_BT_LIMITED_DIG, &bBtBusy);
+	if(bForceLps)
+		pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_ENTER_LPS, NULL);
+	else
+		pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_NORMAL_LPS, NULL);
+
+	if( (BT_STATE_8723A_1ANT_NO_CONNECTION == pCoexDm->btStatus) ||
+		(BT_STATE_8723A_1ANT_CONNECT_IDLE == pCoexDm->btStatus) )
+	{
+		if(pCoexSta->bC2hBtInquiryPage)
+			pCoexDm->btStatus = BT_STATE_8723A_1ANT_INQ_OR_PAG;
+	}
+}
+
+VOID
+EXhalbtc8723a1ant_StackOperationNotify(
+	IN	PBTC_COEXIST			pBtCoexist,
+	IN	u1Byte				type
+	)
+{
+	if(BTC_STACK_OP_INQ_PAGE_PAIR_START == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], StackOP Inquiry/page/pair start notify\n"));
+	}
+	else if(BTC_STACK_OP_INQ_PAGE_PAIR_FINISH == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], StackOP Inquiry/page/pair finish notify\n"));
+	}
+}
+
+VOID
+EXhalbtc8723a1ant_HaltNotify(
+	IN	PBTC_COEXIST			pBtCoexist
+	)
+{
+	halbtc8723a1ant_PsTdma(pBtCoexist, FORCE_EXEC, FALSE, 0);
+	
+	halbtc8723a1ant_LowPenaltyRa(pBtCoexist, FORCE_EXEC, FALSE);
+	halbtc8723a1ant_RfShrink(pBtCoexist, FORCE_EXEC, FALSE);
+
+	halbtc8723a1ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, true);
+	EXhalbtc8723a1ant_MediaStatusNotify(pBtCoexist, BTC_MEDIA_DISCONNECT);
+}
+
+VOID
+EXhalbtc8723a1ant_Periodical(
+	IN	PBTC_COEXIST			pBtCoexist
+	)
+{
+	BOOLEAN		bScan=FALSE, bLink=FALSE, bRoam=FALSE, bWifiConnected=FALSE;
+	
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], 1Ant Periodical!!\n"));
+	
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_SCAN, &bScan);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_LINK, &bLink);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_ROAM, &bRoam);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+
+	// work around for c2h hang
+	wa_halbtc8723a1ant_MonitorC2h(pBtCoexist);	
+
+	halbtc8723a1ant_QueryBtInfo(pBtCoexist);
+	halbtc8723a1ant_MonitorBtCtr(pBtCoexist);
+	halbtc8723a1ant_MonitorBtEnableDisable(pBtCoexist);
+
+	
+	if(bScan)
+		return;
+	if(bLink)
+		return;
+
+	if(bWifiConnected)
+	{
+		if(pBtCoexist->btInfo.bBtDisabled)
+		{
+			halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 9);
+			
+			halbtc8723a1ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, FALSE);
+			halbtc8723a1ant_RfShrink(pBtCoexist, NORMAL_EXEC, FALSE);
+		}
+		else
+		{
+			halbtc8723a1ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, true);
+			halbtc8723a1ant_RfShrink(pBtCoexist, NORMAL_EXEC, FALSE);
+			halbtc8723a1ant_CoexForWifiConnect(pBtCoexist);
+		}
+	}
+	else
+	{
+		halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
+
+		halbtc8723a1ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a1ant_RfShrink(pBtCoexist, NORMAL_EXEC, FALSE);
+	}
+}
+
+
+#endif
+
diff --git a/drivers/staging/rtl8821ae/btcoexist/habtc8723a1ant.h b/drivers/staging/rtl8821ae/btcoexist/habtc8723a1ant.h
new file mode 100644
index 000000000000..60992f59eb37
--- /dev/null
+++ b/drivers/staging/rtl8821ae/btcoexist/habtc8723a1ant.h
@@ -0,0 +1,176 @@
+//===========================================
+// The following is for 8723A 1Ant BT Co-exist definition
+//===========================================
+#define	BT_INFO_8723A_1ANT_B_FTP						BIT7
+#define	BT_INFO_8723A_1ANT_B_A2DP					BIT6
+#define	BT_INFO_8723A_1ANT_B_HID						BIT5
+#define	BT_INFO_8723A_1ANT_B_SCO_BUSY				BIT4
+#define	BT_INFO_8723A_1ANT_B_ACL_BUSY				BIT3
+#define	BT_INFO_8723A_1ANT_B_INQ_PAGE				BIT2
+#define	BT_INFO_8723A_1ANT_B_SCO_ESCO				BIT1
+#define	BT_INFO_8723A_1ANT_B_CONNECTION				BIT0
+
+typedef enum _BT_STATE_8723A_1ANT{
+	BT_STATE_8723A_1ANT_DISABLED				= 0,
+	BT_STATE_8723A_1ANT_NO_CONNECTION		= 1,
+	BT_STATE_8723A_1ANT_CONNECT_IDLE		= 2,
+	BT_STATE_8723A_1ANT_INQ_OR_PAG			= 3,
+	BT_STATE_8723A_1ANT_ACL_ONLY_BUSY		= 4,
+	BT_STATE_8723A_1ANT_SCO_ONLY_BUSY		= 5,
+	BT_STATE_8723A_1ANT_ACL_SCO_BUSY			= 6,
+	BT_STATE_8723A_1ANT_HID_BUSY				= 7,
+	BT_STATE_8723A_1ANT_HID_SCO_BUSY			= 8,
+	BT_STATE_8723A_1ANT_MAX
+}BT_STATE_8723A_1ANT, *PBT_STATE_8723A_1ANT;
+
+#define		BTC_RSSI_COEX_THRESH_TOL_8723A_1ANT		2
+
+typedef enum _BT_INFO_SRC_8723A_1ANT{
+	BT_INFO_SRC_8723A_1ANT_WIFI_FW			= 0x0,
+	BT_INFO_SRC_8723A_1ANT_BT_RSP				= 0x1,
+	BT_INFO_SRC_8723A_1ANT_BT_ACTIVE_SEND		= 0x2,
+	BT_INFO_SRC_8723A_1ANT_MAX
+}BT_INFO_SRC_8723A_1ANT,*PBT_INFO_SRC_8723A_1ANT;
+
+typedef enum _BT_8723A_1ANT_BT_STATUS{
+	BT_8723A_1ANT_BT_STATUS_IDLE				= 0x0,
+	BT_8723A_1ANT_BT_STATUS_CONNECTED_IDLE	= 0x1,
+	BT_8723A_1ANT_BT_STATUS_NON_IDLE			= 0x2,
+	BT_8723A_1ANT_BT_STATUS_MAX
+}BT_8723A_1ANT_BT_STATUS,*PBT_8723A_1ANT_BT_STATUS;
+
+typedef enum _BT_8723A_1ANT_COEX_ALGO{
+	BT_8723A_1ANT_COEX_ALGO_UNDEFINED			= 0x0,
+	BT_8723A_1ANT_COEX_ALGO_SCO				= 0x1,
+	BT_8723A_1ANT_COEX_ALGO_HID				= 0x2,
+	BT_8723A_1ANT_COEX_ALGO_A2DP				= 0x3,
+	BT_8723A_1ANT_COEX_ALGO_PANEDR			= 0x4,
+	BT_8723A_1ANT_COEX_ALGO_PANHS			= 0x5,
+	BT_8723A_1ANT_COEX_ALGO_PANEDR_A2DP		= 0x6,
+	BT_8723A_1ANT_COEX_ALGO_PANEDR_HID		= 0x7,
+	BT_8723A_1ANT_COEX_ALGO_HID_A2DP_PANEDR	= 0x8,
+	BT_8723A_1ANT_COEX_ALGO_HID_A2DP			= 0x9,
+	BT_8723A_1ANT_COEX_ALGO_MAX
+}BT_8723A_1ANT_COEX_ALGO,*PBT_8723A_1ANT_COEX_ALGO;
+
+typedef struct _COEX_DM_8723A_1ANT{
+	// fw mechanism
+	BOOLEAN		bCurIgnoreWlanAct;
+	BOOLEAN		bPreIgnoreWlanAct;
+	u1Byte		prePsTdma;
+	u1Byte		curPsTdma;
+	u1Byte		psTdmaPara[5];
+	u1Byte		psTdmaDuAdjType;
+	u4Byte		psTdmaMonitorCnt;
+	u4Byte		psTdmaGlobalCnt;
+	BOOLEAN		bResetTdmaAdjust;
+	BOOLEAN		bPrePsTdmaOn;
+	BOOLEAN		bCurPsTdmaOn;
+
+	// sw mechanism
+	BOOLEAN		bPreRfRxLpfShrink;
+	BOOLEAN		bCurRfRxLpfShrink;
+	u4Byte		btRf0x1eBackup;
+	BOOLEAN 	bPreLowPenaltyRa;
+	BOOLEAN		bCurLowPenaltyRa;
+	u4Byte		preVal0x6c0;
+	u4Byte		curVal0x6c0;
+	u4Byte		preVal0x6c8;
+	u4Byte		curVal0x6c8;
+	u1Byte		preVal0x6cc;
+	u1Byte		curVal0x6cc;
+	BOOLEAN		limited_dig;
+
+	// algorithm related
+	u1Byte		preAlgorithm;
+	u1Byte		curAlgorithm;
+	u1Byte		btStatus;
+	u1Byte		wifiChnlInfo[3];
+} COEX_DM_8723A_1ANT, *PCOEX_DM_8723A_1ANT;
+
+typedef struct _COEX_STA_8723A_1ANT{
+	u4Byte					highPriorityTx;
+	u4Byte					highPriorityRx;
+	u4Byte					lowPriorityTx;
+	u4Byte					lowPriorityRx;
+	u1Byte					btRssi;
+	u1Byte					preBtRssiState;
+	u1Byte					preBtRssiState1;
+	u1Byte					preWifiRssiState[4];
+	BOOLEAN					bC2hBtInfoReqSent;
+	u1Byte					btInfoC2h[BT_INFO_SRC_8723A_1ANT_MAX][10];
+	u4Byte					btInfoC2hCnt[BT_INFO_SRC_8723A_1ANT_MAX];
+	BOOLEAN					bC2hBtInquiryPage;
+	u1Byte					btRetryCnt;
+	u1Byte					btInfoExt;
+	//BOOLEAN					bHoldForStackOperation;
+	//u1Byte					bHoldPeriodCnt;
+	// this is for c2h hang work-around
+	u4Byte					c2hHangDetectCnt;
+}COEX_STA_8723A_1ANT, *PCOEX_STA_8723A_1ANT;
+
+//===========================================
+// The following is interface which will notify coex module.
+//===========================================
+VOID
+EXhalbtc8723a1ant_InitHwConfig(
+	IN	PBTC_COEXIST		pBtCoexist
+	);
+VOID
+EXhalbtc8723a1ant_InitCoexDm(
+	IN	PBTC_COEXIST		pBtCoexist
+	);
+VOID
+EXhalbtc8723a1ant_IpsNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			type
+	);
+VOID
+EXhalbtc8723a1ant_LpsNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			type
+	);
+VOID
+EXhalbtc8723a1ant_ScanNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			type
+	);
+VOID
+EXhalbtc8723a1ant_ConnectNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			type
+	);
+VOID
+EXhalbtc8723a1ant_MediaStatusNotify(
+	IN	PBTC_COEXIST			pBtCoexist,
+	IN	u1Byte				type
+	);
+VOID
+EXhalbtc8723a1ant_SpecialPacketNotify(
+	IN	PBTC_COEXIST			pBtCoexist,
+	IN	u1Byte				type
+	);
+VOID
+EXhalbtc8723a1ant_BtInfoNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	pu1Byte			tmpBuf,
+	IN	u1Byte			length
+	);
+VOID
+EXhalbtc8723a1ant_StackOperationNotify(
+	IN	PBTC_COEXIST			pBtCoexist,
+	IN	u1Byte				type
+	);
+VOID
+EXhalbtc8723a1ant_HaltNotify(
+	IN	PBTC_COEXIST			pBtCoexist
+	);
+VOID
+EXhalbtc8723a1ant_Periodical(
+	IN	PBTC_COEXIST			pBtCoexist
+	);
+VOID
+EXhalbtc8723a1ant_DisplayCoexInfo(
+	IN	PBTC_COEXIST		pBtCoexist
+	);
+
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbt_precomp.h b/drivers/staging/rtl8821ae/btcoexist/halbt_precomp.h
new file mode 100644
index 000000000000..d538ba3d0a2e
--- /dev/null
+++ b/drivers/staging/rtl8821ae/btcoexist/halbt_precomp.h
@@ -0,0 +1,99 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#ifndef	__HALBT_PRECOMP_H__
+#define __HALBT_PRECOMP_H__
+/*************************************************************
+ * include files
+ *************************************************************/
+#include "../wifi.h"
+#include "../efuse.h"
+#include "../base.h"
+#include "../regd.h"
+#include "../cam.h"
+#include "../ps.h"
+#include "../pci.h"
+#include "../rtl8821ae/reg.h"
+#include "../rtl8821ae/def.h"
+#include "../rtl8821ae/phy.h"
+#include "../rtl8821ae/dm.h"
+#include "../rtl8821ae/fw.h"
+#include "../rtl8821ae/led.h"
+#include "../rtl8821ae/hw.h"
+#include "../rtl8821ae/pwrseqcmd.h"
+#include "../rtl8821ae/pwrseq.h"
+
+#include "halbtcoutsrc.h"
+
+
+#include "halbtc8192e2ant.h"
+#include "halbtc8723b1ant.h"
+#include "halbtc8723b2ant.h"
+
+
+
+#define GetDefaultAdapter(padapter)	padapter
+
+
+#define BIT0	0x00000001
+#define BIT1	0x00000002
+#define BIT2	0x00000004
+#define BIT3	0x00000008
+#define BIT4	0x00000010
+#define BIT5	0x00000020
+#define BIT6	0x00000040
+#define BIT7	0x00000080
+#define BIT8	0x00000100
+#define BIT9	0x00000200
+#define BIT10	0x00000400
+#define BIT11	0x00000800
+#define BIT12	0x00001000
+#define BIT13	0x00002000
+#define BIT14	0x00004000
+#define BIT15	0x00008000
+#define BIT16	0x00010000
+#define BIT17	0x00020000
+#define BIT18	0x00040000
+#define BIT19	0x00080000
+#define BIT20	0x00100000
+#define BIT21	0x00200000
+#define BIT22	0x00400000
+#define BIT23	0x00800000
+#define BIT24	0x01000000
+#define BIT25	0x02000000
+#define BIT26	0x04000000
+#define BIT27	0x08000000
+#define BIT28	0x10000000
+#define BIT29	0x20000000
+#define BIT30	0x40000000
+#define BIT31	0x80000000
+
+#define	MASKBYTE0                	0xff
+#define	MASKBYTE1                	0xff00
+#define	MASKBYTE2                	0xff0000
+#define	MASKBYTE3                	0xff000000
+#define	MASKHWORD                	0xffff0000
+#define	MASKLWORD                	0x0000ffff
+#define	MASKDWORD			0xffffffff
+#define	MASK12BITS			0xfff
+#define	MASKH4BITS			0xf0000000
+#define MASKOFDM_D			0xffc00000
+#define	MASKCCK				0x3f3f3f3f
+
+#endif	/* __HALBT_PRECOMP_H__ */
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtc8192e1ant.c b/drivers/staging/rtl8821ae/btcoexist/halbtc8192e1ant.c
new file mode 100644
index 000000000000..973d0ea82cb8
--- /dev/null
+++ b/drivers/staging/rtl8821ae/btcoexist/halbtc8192e1ant.c
@@ -0,0 +1,3891 @@
+//============================================================
+// Description:
+//
+// This file is for 8192e1ant Co-exist mechanism
+//
+// History
+// 2012/11/15 Cosa first check in.
+//
+//============================================================
+
+//============================================================
+// include files
+//============================================================
+#include "Mp_Precomp.h"
+#if(BT_30_SUPPORT == 1)
+//============================================================
+// Global variables, these are static variables
+//============================================================
+static COEX_DM_8192E_1ANT		GLCoexDm8192e1Ant;
+static PCOEX_DM_8192E_1ANT 	pCoexDm=&GLCoexDm8192e1Ant;
+static COEX_STA_8192E_1ANT		GLCoexSta8192e1Ant;
+static PCOEX_STA_8192E_1ANT	pCoexSta=&GLCoexSta8192e1Ant;
+
+const char *const GLBtInfoSrc8192e1Ant[]={
+	"BT Info[wifi fw]",
+	"BT Info[bt rsp]",
+	"BT Info[bt auto report]",
+};
+
+u4Byte	GLCoexVerDate8192e1Ant=20130729;
+u4Byte	GLCoexVer8192e1Ant=0x10;
+
+//============================================================
+// local function proto type if needed
+//============================================================
+//============================================================
+// local function start with halbtc8192e1ant_
+//============================================================
+u1Byte
+halbtc8192e1ant_BtRssiState(
+	u1Byte			levelNum,
+	u1Byte			rssiThresh,
+	u1Byte			rssiThresh1
+	)
+{
+	s4Byte			btRssi=0;
+	u1Byte			btRssiState=pCoexSta->preBtRssiState;
+
+	btRssi = pCoexSta->btRssi;
+
+	if(levelNum == 2)
+	{			
+		if( (pCoexSta->preBtRssiState == BTC_RSSI_STATE_LOW) ||
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_STAY_LOW))
+		{
+			if(btRssi >= (rssiThresh+BTC_RSSI_COEX_THRESH_TOL_8192E_1ANT))
+			{
+				btRssiState = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to High\n"));
+			}
+			else
+			{
+				btRssiState = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at Low\n"));
+			}
+		}
+		else
+		{
+			if(btRssi < rssiThresh)
+			{
+				btRssiState = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Low\n"));
+			}
+			else
+			{
+				btRssiState = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at High\n"));
+			}
+		}
+	}
+	else if(levelNum == 3)
+	{
+		if(rssiThresh > rssiThresh1)
+		{
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi thresh error!!\n"));
+			return pCoexSta->preBtRssiState;
+		}
+		
+		if( (pCoexSta->preBtRssiState == BTC_RSSI_STATE_LOW) ||
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_STAY_LOW))
+		{
+			if(btRssi >= (rssiThresh+BTC_RSSI_COEX_THRESH_TOL_8192E_1ANT))
+			{
+				btRssiState = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Medium\n"));
+			}
+			else
+			{
+				btRssiState = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at Low\n"));
+			}
+		}
+		else if( (pCoexSta->preBtRssiState == BTC_RSSI_STATE_MEDIUM) ||
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_STAY_MEDIUM))
+		{
+			if(btRssi >= (rssiThresh1+BTC_RSSI_COEX_THRESH_TOL_8192E_1ANT))
+			{
+				btRssiState = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to High\n"));
+			}
+			else if(btRssi < rssiThresh)
+			{
+				btRssiState = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Low\n"));
+			}
+			else
+			{
+				btRssiState = BTC_RSSI_STATE_STAY_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at Medium\n"));
+			}
+		}
+		else
+		{
+			if(btRssi < rssiThresh1)
+			{
+				btRssiState = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Medium\n"));
+			}
+			else
+			{
+				btRssiState = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at High\n"));
+			}
+		}
+	}
+		
+	pCoexSta->preBtRssiState = btRssiState;
+
+	return btRssiState;
+}
+
+u1Byte
+halbtc8192e1ant_WifiRssiState(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			index,
+	IN	u1Byte			levelNum,
+	IN	u1Byte			rssiThresh,
+	IN	u1Byte			rssiThresh1
+	)
+{
+	s4Byte			wifiRssi=0;
+	u1Byte			wifiRssiState=pCoexSta->preWifiRssiState[index];
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_S4_WIFI_RSSI, &wifiRssi);
+	
+	if(levelNum == 2)
+	{
+		if( (pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_LOW) ||
+			(pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_STAY_LOW))
+		{
+			if(wifiRssi >= (rssiThresh+BTC_RSSI_COEX_THRESH_TOL_8192E_1ANT))
+			{
+				wifiRssiState = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to High\n"));
+			}
+			else
+			{
+				wifiRssiState = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at Low\n"));
+			}
+		}
+		else
+		{
+			if(wifiRssi < rssiThresh)
+			{
+				wifiRssiState = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Low\n"));
+			}
+			else
+			{
+				wifiRssiState = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at High\n"));
+			}
+		}
+	}
+	else if(levelNum == 3)
+	{
+		if(rssiThresh > rssiThresh1)
+		{
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI thresh error!!\n"));
+			return pCoexSta->preWifiRssiState[index];
+		}
+		
+		if( (pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_LOW) ||
+			(pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_STAY_LOW))
+		{
+			if(wifiRssi >= (rssiThresh+BTC_RSSI_COEX_THRESH_TOL_8192E_1ANT))
+			{
+				wifiRssiState = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Medium\n"));
+			}
+			else
+			{
+				wifiRssiState = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at Low\n"));
+			}
+		}
+		else if( (pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_MEDIUM) ||
+			(pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_STAY_MEDIUM))
+		{
+			if(wifiRssi >= (rssiThresh1+BTC_RSSI_COEX_THRESH_TOL_8192E_1ANT))
+			{
+				wifiRssiState = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to High\n"));
+			}
+			else if(wifiRssi < rssiThresh)
+			{
+				wifiRssiState = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Low\n"));
+			}
+			else
+			{
+				wifiRssiState = BTC_RSSI_STATE_STAY_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at Medium\n"));
+			}
+		}
+		else
+		{
+			if(wifiRssi < rssiThresh1)
+			{
+				wifiRssiState = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Medium\n"));
+			}
+			else
+			{
+				wifiRssiState = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at High\n"));
+			}
+		}
+	}
+		
+	pCoexSta->preWifiRssiState[index] = wifiRssiState;
+
+	return wifiRssiState;
+}
+
+VOID
+halbtc8192e1ant_Updatera_mask(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN				bForceExec,
+	IN	u1Byte				type,
+	IN	u4Byte				rateMask
+	)
+{
+	if(BTC_RATE_DISABLE == type)
+	{
+		pCoexDm->curra_mask |= rateMask;		// disable rate
+	}
+	else if(BTC_RATE_ENABLE == type)
+	{
+		pCoexDm->curra_mask &= ~rateMask;	// enable rate
+	}
+	
+	if( bForceExec || (pCoexDm->prera_mask != pCoexDm->curra_mask))
+	{
+		pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_UPDATE_ra_mask, &pCoexDm->curra_mask);
+	}
+	pCoexDm->prera_mask = pCoexDm->curra_mask;
+}
+
+VOID
+halbtc8192e1ant_MonitorBtCtr(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	u4Byte 			regHPTxRx, regLPTxRx, u4Tmp;
+	u4Byte			regHPTx=0, regHPRx=0, regLPTx=0, regLPRx=0;
+	u1Byte			u1Tmp;
+	
+	regHPTxRx = 0x770;
+	regLPTxRx = 0x774;
+
+	u4Tmp = pBtCoexist->btc_read_4byte(pBtCoexist, regHPTxRx);
+	regHPTx = u4Tmp & MASKLWORD;
+	regHPRx = (u4Tmp & MASKHWORD)>>16;
+
+	u4Tmp = pBtCoexist->btc_read_4byte(pBtCoexist, regLPTxRx);
+	regLPTx = u4Tmp & MASKLWORD;
+	regLPRx = (u4Tmp & MASKHWORD)>>16;
+		
+	pCoexSta->highPriorityTx = regHPTx;
+	pCoexSta->highPriorityRx = regHPRx;
+	pCoexSta->lowPriorityTx = regLPTx;
+	pCoexSta->lowPriorityRx = regLPRx;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], High Priority Tx/Rx (reg 0x%x)=0x%x(%d)/0x%x(%d)\n", 
+		regHPTxRx, regHPTx, regHPTx, regHPRx, regHPRx));
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], Low Priority Tx/Rx (reg 0x%x)=0x%x(%d)/0x%x(%d)\n", 
+		regLPTxRx, regLPTx, regLPTx, regLPRx, regLPRx));
+
+	// reset counter
+	pBtCoexist->btc_write_1byte(pBtCoexist, 0x76e, 0xc);
+}
+
+VOID
+halbtc8192e1ant_QueryBtInfo(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	u1Byte			H2C_Parameter[1] ={0};
+
+	pCoexSta->bC2hBtInfoReqSent = true;
+
+	H2C_Parameter[0] |= BIT0;	// trigger
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], Query Bt Info, FW write 0x61=0x%x\n", 
+		H2C_Parameter[0]));
+
+	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x61, 1, H2C_Parameter);
+}
+
+BOOLEAN
+halbtc8192e1ant_IsWifiStatusChanged(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	static BOOLEAN	bPreWifiBusy=FALSE, bPreUnder4way=FALSE, bPreBtHsOn=FALSE;
+	BOOLEAN	bWifiBusy=FALSE, bUnder4way=FALSE, bBtHsOn=FALSE;
+	BOOLEAN	bWifiConnected=FALSE;
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS, &bUnder4way);
+
+	if(bWifiConnected)
+	{
+		if(bWifiBusy != bPreWifiBusy)
+		{
+			bPreWifiBusy = bWifiBusy;
+			return true;
+		}
+		if(bUnder4way != bPreUnder4way)
+		{
+			bPreUnder4way = bUnder4way;
+			return true;
+		}
+		if(bBtHsOn != bPreBtHsOn)
+		{
+			bPreBtHsOn = bBtHsOn;
+			return true;
+		}
+	}
+
+	return FALSE;
+}
+
+VOID
+halbtc8192e1ant_UpdateBtLinkInfo(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	PBTC_BT_LINK_INFO	pBtLinkInfo=&pBtCoexist->bt_link_info;
+
+	pBtLinkInfo->bBtLinkExist = pCoexSta->bBtLinkExist;
+	pBtLinkInfo->bScoExist = pCoexSta->bScoExist;
+	pBtLinkInfo->bA2dpExist = pCoexSta->bA2dpExist;
+	pBtLinkInfo->bPanExist = pCoexSta->bPanExist;
+	pBtLinkInfo->bHidExist = pCoexSta->bHidExist;
+
+	// check if Sco only
+	if( pBtLinkInfo->bScoExist &&
+		!pBtLinkInfo->bA2dpExist &&
+		!pBtLinkInfo->bPanExist &&
+		!pBtLinkInfo->bHidExist )
+		pBtLinkInfo->bScoOnly = true;
+	else
+		pBtLinkInfo->bScoOnly = FALSE;
+
+	// check if A2dp only
+	if( !pBtLinkInfo->bScoExist &&
+		pBtLinkInfo->bA2dpExist &&
+		!pBtLinkInfo->bPanExist &&
+		!pBtLinkInfo->bHidExist )
+		pBtLinkInfo->bA2dpOnly = true;
+	else
+		pBtLinkInfo->bA2dpOnly = FALSE;
+
+	// check if Pan only
+	if( !pBtLinkInfo->bScoExist &&
+		!pBtLinkInfo->bA2dpExist &&
+		pBtLinkInfo->bPanExist &&
+		!pBtLinkInfo->bHidExist )
+		pBtLinkInfo->bPanOnly = true;
+	else
+		pBtLinkInfo->bPanOnly = FALSE;
+	
+	// check if Hid only
+	if( !pBtLinkInfo->bScoExist &&
+		!pBtLinkInfo->bA2dpExist &&
+		!pBtLinkInfo->bPanExist &&
+		pBtLinkInfo->bHidExist )
+		pBtLinkInfo->bHidOnly = true;
+	else
+		pBtLinkInfo->bHidOnly = FALSE;
+}
+
+u1Byte
+halbtc8192e1ant_ActionAlgorithm(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	PBTC_BT_LINK_INFO	pBtLinkInfo=&pBtCoexist->bt_link_info;
+	BOOLEAN				bBtHsOn=FALSE;
+	u1Byte				algorithm=BT_8192E_1ANT_COEX_ALGO_UNDEFINED;
+	u1Byte				numOfDiffProfile=0;
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+		
+	if(!pBtLinkInfo->bBtLinkExist)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], No BT link exists!!!\n"));
+		return algorithm;
+	}
+
+	if(pBtLinkInfo->bScoExist)
+		numOfDiffProfile++;
+	if(pBtLinkInfo->bHidExist)
+		numOfDiffProfile++;
+	if(pBtLinkInfo->bPanExist)
+		numOfDiffProfile++;
+	if(pBtLinkInfo->bA2dpExist)
+		numOfDiffProfile++;
+	
+	if(numOfDiffProfile == 1)
+	{
+		if(pBtLinkInfo->bScoExist)
+		{
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO only\n"));
+			algorithm = BT_8192E_1ANT_COEX_ALGO_SCO;
+		}
+		else
+		{
+			if(pBtLinkInfo->bHidExist)
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID only\n"));
+				algorithm = BT_8192E_1ANT_COEX_ALGO_HID;
+			}
+			else if(pBtLinkInfo->bA2dpExist)
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], A2DP only\n"));
+				algorithm = BT_8192E_1ANT_COEX_ALGO_A2DP;
+			}
+			else if(pBtLinkInfo->bPanExist)
+			{
+				if(bBtHsOn)
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PAN(HS) only\n"));
+					algorithm = BT_8192E_1ANT_COEX_ALGO_PANHS;
+				}
+				else
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PAN(EDR) only\n"));
+					algorithm = BT_8192E_1ANT_COEX_ALGO_PANEDR;
+				}
+			}
+		}
+	}
+	else if(numOfDiffProfile == 2)
+	{
+		if(pBtLinkInfo->bScoExist)
+		{
+			if(pBtLinkInfo->bHidExist)
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID\n"));
+				algorithm = BT_8192E_1ANT_COEX_ALGO_HID;
+			}
+			else if(pBtLinkInfo->bA2dpExist)
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + A2DP ==> SCO\n"));
+				algorithm = BT_8192E_1ANT_COEX_ALGO_SCO;
+			}
+			else if(pBtLinkInfo->bPanExist)
+			{
+				if(bBtHsOn)
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + PAN(HS)\n"));
+					algorithm = BT_8192E_1ANT_COEX_ALGO_SCO;
+				}
+				else
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + PAN(EDR)\n"));
+					algorithm = BT_8192E_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		}
+		else
+		{
+			if( pBtLinkInfo->bHidExist &&
+				pBtLinkInfo->bA2dpExist )
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + A2DP\n"));
+				algorithm = BT_8192E_1ANT_COEX_ALGO_HID_A2DP;
+			}
+			else if( pBtLinkInfo->bHidExist &&
+				pBtLinkInfo->bPanExist )
+			{
+				if(bBtHsOn)
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + PAN(HS)\n"));
+					algorithm = BT_8192E_1ANT_COEX_ALGO_HID_A2DP;
+				}
+				else
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + PAN(EDR)\n"));
+					algorithm = BT_8192E_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+			else if( pBtLinkInfo->bPanExist &&
+				pBtLinkInfo->bA2dpExist )
+			{
+				if(bBtHsOn)
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], A2DP + PAN(HS)\n"));
+					algorithm = BT_8192E_1ANT_COEX_ALGO_A2DP_PANHS;
+				}
+				else
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], A2DP + PAN(EDR)\n"));
+					algorithm = BT_8192E_1ANT_COEX_ALGO_PANEDR_A2DP;
+				}
+			}
+		}
+	}
+	else if(numOfDiffProfile == 3)
+	{
+		if(pBtLinkInfo->bScoExist)
+		{
+			if( pBtLinkInfo->bHidExist &&
+				pBtLinkInfo->bA2dpExist )
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID + A2DP ==> HID\n"));
+				algorithm = BT_8192E_1ANT_COEX_ALGO_HID;
+			}
+			else if( pBtLinkInfo->bHidExist &&
+				pBtLinkInfo->bPanExist )
+			{
+				if(bBtHsOn)
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID + PAN(HS)\n"));
+					algorithm = BT_8192E_1ANT_COEX_ALGO_HID_A2DP;
+				}
+				else
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID + PAN(EDR)\n"));
+					algorithm = BT_8192E_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+			else if( pBtLinkInfo->bPanExist &&
+				pBtLinkInfo->bA2dpExist )
+			{
+				if(bBtHsOn)
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + A2DP + PAN(HS)\n"));
+					algorithm = BT_8192E_1ANT_COEX_ALGO_SCO;
+				}
+				else
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + A2DP + PAN(EDR) ==> HID\n"));
+					algorithm = BT_8192E_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		}
+		else
+		{
+			if( pBtLinkInfo->bHidExist &&
+				pBtLinkInfo->bPanExist &&
+				pBtLinkInfo->bA2dpExist )
+			{
+				if(bBtHsOn)
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + A2DP + PAN(HS)\n"));
+					algorithm = BT_8192E_1ANT_COEX_ALGO_HID_A2DP;
+				}
+				else
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + A2DP + PAN(EDR)\n"));
+					algorithm = BT_8192E_1ANT_COEX_ALGO_HID_A2DP_PANEDR;
+				}
+			}
+		}
+	}
+	else if(numOfDiffProfile >= 3)
+	{
+		if(pBtLinkInfo->bScoExist)
+		{
+			if( pBtLinkInfo->bHidExist &&
+				pBtLinkInfo->bPanExist &&
+				pBtLinkInfo->bA2dpExist )
+			{
+				if(bBtHsOn)
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Error!!! SCO + HID + A2DP + PAN(HS)\n"));
+
+				}
+				else
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID + A2DP + PAN(EDR)==>PAN(EDR)+HID\n"));
+					algorithm = BT_8192E_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		}
+	}
+
+	return algorithm;
+}
+
+VOID
+halbtc8192e1ant_SetFwDacSwingLevel(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			dacSwingLvl
+	)
+{
+	u1Byte			H2C_Parameter[1] ={0};
+
+	// There are several type of dacswing
+	// 0x18/ 0x10/ 0xc/ 0x8/ 0x4/ 0x6
+	H2C_Parameter[0] = dacSwingLvl;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], Set Dac Swing Level=0x%x\n", dacSwingLvl));
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], FW write 0x64=0x%x\n", H2C_Parameter[0]));
+
+	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x64, 1, H2C_Parameter);
+}
+
+VOID
+halbtc8192e1ant_SetFwDecBtPwr(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte				decBtPwrLvl
+	)
+{
+	u1Byte			H2C_Parameter[1] ={0};
+	
+	H2C_Parameter[0] = decBtPwrLvl;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], decrease Bt Power level = %d, FW write 0x62=0x%x\n", 
+		decBtPwrLvl, H2C_Parameter[0]));
+
+	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x62, 1, H2C_Parameter);
+}
+
+VOID
+halbtc8192e1ant_DecBtPwr(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN				bForceExec,
+	IN	u1Byte				decBtPwrLvl
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s Dec BT power level = %d\n",  
+		(bForceExec? "force to":""), decBtPwrLvl));
+	pCoexDm->curBtDecPwrLvl = decBtPwrLvl;
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], BtDecPwrLvl=%d, curBtDecPwrLvl=%d\n", 
+			pCoexDm->preBtDecPwrLvl, pCoexDm->curBtDecPwrLvl));
+
+		if(pCoexDm->preBtDecPwrLvl == pCoexDm->curBtDecPwrLvl) 
+			return;
+	}
+	halbtc8192e1ant_SetFwDecBtPwr(pBtCoexist, pCoexDm->curBtDecPwrLvl);
+
+	pCoexDm->preBtDecPwrLvl = pCoexDm->curBtDecPwrLvl;
+}
+
+VOID
+halbtc8192e1ant_SetFwBtLnaConstrain(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bBtLnaConsOn
+	)
+{
+	u1Byte			H2C_Parameter[2] ={0};
+	
+	H2C_Parameter[0] = 0x3;	// opCode, 0x3=BT_SET_LNA_CONSTRAIN
+
+	if(bBtLnaConsOn)
+	{
+		H2C_Parameter[1] |= BIT0;
+	}
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], set BT LNA Constrain: %s, FW write 0x69=0x%x\n", 
+		(bBtLnaConsOn? "ON!!":"OFF!!"), 
+		H2C_Parameter[0]<<8|H2C_Parameter[1]));
+
+	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x69, 2, H2C_Parameter);	
+}
+
+VOID
+halbtc8192e1ant_SetBtLnaConstrain(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bForceExec,
+	IN	BOOLEAN			bBtLnaConsOn
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s BT Constrain = %s\n",  
+		(bForceExec? "force":""), ((bBtLnaConsOn)? "ON":"OFF")));
+	pCoexDm->bCurBtLnaConstrain = bBtLnaConsOn;
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], bPreBtLnaConstrain=%d, bCurBtLnaConstrain=%d\n", 
+			pCoexDm->bPreBtLnaConstrain, pCoexDm->bCurBtLnaConstrain));
+
+		if(pCoexDm->bPreBtLnaConstrain == pCoexDm->bCurBtLnaConstrain) 
+			return;
+	}
+	halbtc8192e1ant_SetFwBtLnaConstrain(pBtCoexist, pCoexDm->bCurBtLnaConstrain);
+
+	pCoexDm->bPreBtLnaConstrain = pCoexDm->bCurBtLnaConstrain;
+}
+
+VOID
+halbtc8192e1ant_SetFwBtPsdMode(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			btPsdMode
+	)
+{
+	u1Byte			H2C_Parameter[2] ={0};
+	
+	H2C_Parameter[0] = 0x2;	// opCode, 0x2=BT_SET_PSD_MODE
+
+	H2C_Parameter[1] = btPsdMode;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], set BT PSD mode=0x%x, FW write 0x69=0x%x\n", 
+		H2C_Parameter[1], 
+		H2C_Parameter[0]<<8|H2C_Parameter[1]));
+
+	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x69, 2, H2C_Parameter);	
+}
+
+
+VOID
+halbtc8192e1ant_SetBtPsdMode(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bForceExec,
+	IN	u1Byte			btPsdMode
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s BT PSD mode = 0x%x\n",  
+		(bForceExec? "force":""), btPsdMode));
+	pCoexDm->bCurBtPsdMode = btPsdMode;
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], bPreBtPsdMode=0x%x, bCurBtPsdMode=0x%x\n", 
+			pCoexDm->bPreBtPsdMode, pCoexDm->bCurBtPsdMode));
+
+		if(pCoexDm->bPreBtPsdMode == pCoexDm->bCurBtPsdMode) 
+			return;
+	}
+	halbtc8192e1ant_SetFwBtPsdMode(pBtCoexist, pCoexDm->bCurBtPsdMode);
+
+	pCoexDm->bPreBtPsdMode = pCoexDm->bCurBtPsdMode;
+}
+
+
+VOID
+halbtc8192e1ant_SetBtAutoReport(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bEnableAutoReport
+	)
+{
+	u1Byte			H2C_Parameter[1] ={0};
+	
+	H2C_Parameter[0] = 0;
+
+	if(bEnableAutoReport)
+	{
+		H2C_Parameter[0] |= BIT0;
+	}
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], BT FW auto report : %s, FW write 0x68=0x%x\n", 
+		(bEnableAutoReport? "Enabled!!":"Disabled!!"), H2C_Parameter[0]));
+
+	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x68, 1, H2C_Parameter);	
+}
+
+VOID
+halbtc8192e1ant_BtAutoReport(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bForceExec,
+	IN	BOOLEAN			bEnableAutoReport
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s BT Auto report = %s\n",  
+		(bForceExec? "force to":""), ((bEnableAutoReport)? "Enabled":"Disabled")));
+	pCoexDm->bCurBtAutoReport = bEnableAutoReport;
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], bPreBtAutoReport=%d, bCurBtAutoReport=%d\n", 
+			pCoexDm->bPreBtAutoReport, pCoexDm->bCurBtAutoReport));
+
+		if(pCoexDm->bPreBtAutoReport == pCoexDm->bCurBtAutoReport) 
+			return;
+	}
+	halbtc8192e1ant_SetBtAutoReport(pBtCoexist, pCoexDm->bCurBtAutoReport);
+
+	pCoexDm->bPreBtAutoReport = pCoexDm->bCurBtAutoReport;
+}
+
+VOID
+halbtc8192e1ant_FwDacSwingLvl(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bForceExec,
+	IN	u1Byte			fwDacSwingLvl
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s set FW Dac Swing level = %d\n",  
+		(bForceExec? "force to":""), fwDacSwingLvl));
+	pCoexDm->curFwDacSwingLvl = fwDacSwingLvl;
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], preFwDacSwingLvl=%d, curFwDacSwingLvl=%d\n", 
+			pCoexDm->preFwDacSwingLvl, pCoexDm->curFwDacSwingLvl));
+
+		if(pCoexDm->preFwDacSwingLvl == pCoexDm->curFwDacSwingLvl) 
+			return;
+	}
+
+	halbtc8192e1ant_SetFwDacSwingLevel(pBtCoexist, pCoexDm->curFwDacSwingLvl);
+
+	pCoexDm->preFwDacSwingLvl = pCoexDm->curFwDacSwingLvl;
+}
+
+VOID
+halbtc8192e1ant_SetSwRfRxLpfCorner(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bRxRfShrinkOn
+	)
+{
+	if(bRxRfShrinkOn)
+	{
+		//Shrink RF Rx LPF corner
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Shrink RF Rx LPF corner!!\n"));
+		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x1e, 0xfffff, 0xf0ff7);
+	}
+	else
+	{
+		//Resume RF Rx LPF corner
+		// After initialized, we can use pCoexDm->btRf0x1eBackup
+		if(pBtCoexist->initilized)
+		{
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Resume RF Rx LPF corner!!\n"));
+			pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x1e, 0xfffff, pCoexDm->btRf0x1eBackup);
+		}
+	}
+}
+
+VOID
+halbtc8192e1ant_RfShrink(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bForceExec,
+	IN	BOOLEAN			bRxRfShrinkOn
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s turn Rx RF Shrink = %s\n",  
+		(bForceExec? "force to":""), ((bRxRfShrinkOn)? "ON":"OFF")));
+	pCoexDm->bCurRfRxLpfShrink = bRxRfShrinkOn;
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], bPreRfRxLpfShrink=%d, bCurRfRxLpfShrink=%d\n", 
+			pCoexDm->bPreRfRxLpfShrink, pCoexDm->bCurRfRxLpfShrink));
+
+		if(pCoexDm->bPreRfRxLpfShrink == pCoexDm->bCurRfRxLpfShrink) 
+			return;
+	}
+	halbtc8192e1ant_SetSwRfRxLpfCorner(pBtCoexist, pCoexDm->bCurRfRxLpfShrink);
+
+	pCoexDm->bPreRfRxLpfShrink = pCoexDm->bCurRfRxLpfShrink;
+}
+
+VOID
+halbtc8192e1ant_SetSwPenaltyTxRateAdaptive(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bLowPenaltyRa
+	)
+{
+	u1Byte	tmpU1;
+
+	tmpU1 = pBtCoexist->btc_read_1byte(pBtCoexist, 0x4fd);
+	tmpU1 |= BIT0;
+	if(bLowPenaltyRa)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Tx rate adaptive, set low penalty!!\n"));
+		tmpU1 &= ~BIT2;
+	}
+	else
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Tx rate adaptive, set normal!!\n"));
+		tmpU1 |= BIT2;
+	}
+
+	pBtCoexist->btc_write_1byte(pBtCoexist, 0x4fd, tmpU1);
+}
+
+VOID
+halbtc8192e1ant_LowPenaltyRa(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bForceExec,
+	IN	BOOLEAN			bLowPenaltyRa
+	)
+{
+	return;
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s turn LowPenaltyRA = %s\n",  
+		(bForceExec? "force to":""), ((bLowPenaltyRa)? "ON":"OFF")));
+	pCoexDm->bCurLowPenaltyRa = bLowPenaltyRa;
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], bPreLowPenaltyRa=%d, bCurLowPenaltyRa=%d\n", 
+			pCoexDm->bPreLowPenaltyRa, pCoexDm->bCurLowPenaltyRa));
+
+		if(pCoexDm->bPreLowPenaltyRa == pCoexDm->bCurLowPenaltyRa) 
+			return;
+	}
+	halbtc8192e1ant_SetSwPenaltyTxRateAdaptive(pBtCoexist, pCoexDm->bCurLowPenaltyRa);
+
+	pCoexDm->bPreLowPenaltyRa = pCoexDm->bCurLowPenaltyRa;
+}
+
+VOID
+halbtc8192e1ant_SetDacSwingReg(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u4Byte			level
+	)
+{
+	u1Byte	val=(u1Byte)level;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Write SwDacSwing = 0x%x\n", level));
+	pBtCoexist->btc_write_1byte_bitmask(pBtCoexist, 0x883, 0x3e, val);
+}
+
+VOID
+halbtc8192e1ant_SetSwFullTimeDacSwing(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bSwDacSwingOn,
+	IN	u4Byte			swDacSwingLvl
+	)
+{
+	if(bSwDacSwingOn)
+	{
+		halbtc8192e1ant_SetDacSwingReg(pBtCoexist, swDacSwingLvl);
+	}
+	else
+	{
+		halbtc8192e1ant_SetDacSwingReg(pBtCoexist, 0x18);
+	}
+}
+
+
+VOID
+halbtc8192e1ant_DacSwing(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bForceExec,
+	IN	BOOLEAN			bDacSwingOn,
+	IN	u4Byte			dacSwingLvl
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s turn DacSwing=%s, dacSwingLvl=0x%x\n",  
+		(bForceExec? "force to":""), ((bDacSwingOn)? "ON":"OFF"), dacSwingLvl));
+	pCoexDm->bCurDacSwingOn = bDacSwingOn;
+	pCoexDm->curDacSwingLvl = dacSwingLvl;
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], bPreDacSwingOn=%d, preDacSwingLvl=0x%x, bCurDacSwingOn=%d, curDacSwingLvl=0x%x\n", 
+			pCoexDm->bPreDacSwingOn, pCoexDm->preDacSwingLvl,
+			pCoexDm->bCurDacSwingOn, pCoexDm->curDacSwingLvl));
+
+		if( (pCoexDm->bPreDacSwingOn == pCoexDm->bCurDacSwingOn) &&
+			(pCoexDm->preDacSwingLvl == pCoexDm->curDacSwingLvl) )
+			return;
+	}
+	mdelay(30);
+	halbtc8192e1ant_SetSwFullTimeDacSwing(pBtCoexist, bDacSwingOn, dacSwingLvl);
+
+	pCoexDm->bPreDacSwingOn = pCoexDm->bCurDacSwingOn;
+	pCoexDm->preDacSwingLvl = pCoexDm->curDacSwingLvl;
+}
+
+VOID
+halbtc8192e1ant_SetAdcBackOff(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bAdcBackOff
+	)
+{
+	if(bAdcBackOff)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], BB BackOff Level On!\n"));
+		pBtCoexist->btc_write_1byte_bitmask(pBtCoexist, 0x8db, 0x60, 0x3);
+	}
+	else
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], BB BackOff Level Off!\n"));
+		pBtCoexist->btc_write_1byte_bitmask(pBtCoexist, 0x8db, 0x60, 0x1);
+	}
+}
+
+VOID
+halbtc8192e1ant_AdcBackOff(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bForceExec,
+	IN	BOOLEAN			bAdcBackOff
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s turn AdcBackOff = %s\n",  
+		(bForceExec? "force to":""), ((bAdcBackOff)? "ON":"OFF")));
+	pCoexDm->bCurAdcBackOff = bAdcBackOff;
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], bPreAdcBackOff=%d, bCurAdcBackOff=%d\n", 
+			pCoexDm->bPreAdcBackOff, pCoexDm->bCurAdcBackOff));
+
+		if(pCoexDm->bPreAdcBackOff == pCoexDm->bCurAdcBackOff) 
+			return;
+	}
+	halbtc8192e1ant_SetAdcBackOff(pBtCoexist, pCoexDm->bCurAdcBackOff);
+
+	pCoexDm->bPreAdcBackOff = pCoexDm->bCurAdcBackOff;
+}
+
+VOID
+halbtc8192e1ant_SetAgcTable(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bAgcTableEn
+	)
+{
+	u1Byte		rssiAdjustVal=0;
+	
+	pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0xef, 0xfffff, 0x02000);
+	if(bAgcTableEn)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Agc Table On!\n"));
+		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x3b, 0xfffff, 0x3fa58);
+		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x3b, 0xfffff, 0x37a58);
+		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x3b, 0xfffff, 0x2fa58);
+		rssiAdjustVal = 8;
+	}
+	else
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Agc Table Off!\n"));
+		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x3b, 0xfffff, 0x39258);
+		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x3b, 0xfffff, 0x31258);
+		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x3b, 0xfffff, 0x29258);
+	}
+	pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0xef, 0xfffff, 0x0);
+
+	// set rssiAdjustVal for wifi module.
+	pBtCoexist->btc_set(pBtCoexist, BTC_SET_U1_RSSI_ADJ_VAL_FOR_AGC_TABLE_ON, &rssiAdjustVal);
+}
+
+
+VOID
+halbtc8192e1ant_AgcTable(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bForceExec,
+	IN	BOOLEAN			bAgcTableEn
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s %s Agc Table\n",  
+		(bForceExec? "force to":""), ((bAgcTableEn)? "Enable":"Disable")));
+	pCoexDm->bCurAgcTableEn = bAgcTableEn;
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], bPreAgcTableEn=%d, bCurAgcTableEn=%d\n", 
+			pCoexDm->bPreAgcTableEn, pCoexDm->bCurAgcTableEn));
+
+		if(pCoexDm->bPreAgcTableEn == pCoexDm->bCurAgcTableEn) 
+			return;
+	}
+	halbtc8192e1ant_SetAgcTable(pBtCoexist, bAgcTableEn);
+
+	pCoexDm->bPreAgcTableEn = pCoexDm->bCurAgcTableEn;
+}
+
+VOID
+halbtc8192e1ant_SetCoexTable(
+	IN	PBTC_COEXIST	pBtCoexist,
+	IN	u4Byte		val0x6c0,
+	IN	u4Byte		val0x6c4,
+	IN	u4Byte		val0x6c8,
+	IN	u1Byte		val0x6cc
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], set coex table, set 0x6c0=0x%x\n", val0x6c0));
+	pBtCoexist->btc_write_4byte(pBtCoexist, 0x6c0, val0x6c0);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], set coex table, set 0x6c4=0x%x\n", val0x6c4));
+	pBtCoexist->btc_write_4byte(pBtCoexist, 0x6c4, val0x6c4);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], set coex table, set 0x6c8=0x%x\n", val0x6c8));
+	pBtCoexist->btc_write_4byte(pBtCoexist, 0x6c8, val0x6c8);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], set coex table, set 0x6cc=0x%x\n", val0x6cc));
+	pBtCoexist->btc_write_1byte(pBtCoexist, 0x6cc, val0x6cc);
+}
+
+VOID
+halbtc8192e1ant_CoexTable(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bForceExec,
+	IN	u4Byte			val0x6c0,
+	IN	u4Byte			val0x6c4,
+	IN	u4Byte			val0x6c8,
+	IN	u1Byte			val0x6cc
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s write Coex Table 0x6c0=0x%x, 0x6c4=0x%x, 0x6c8=0x%x, 0x6cc=0x%x\n", 
+		(bForceExec? "force to":""), val0x6c0, val0x6c4, val0x6c8, val0x6cc));
+	pCoexDm->curVal0x6c0 = val0x6c0;
+	pCoexDm->curVal0x6c4 = val0x6c4;
+	pCoexDm->curVal0x6c8 = val0x6c8;
+	pCoexDm->curVal0x6cc = val0x6cc;
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], preVal0x6c0=0x%x, preVal0x6c4=0x%x, preVal0x6c8=0x%x, preVal0x6cc=0x%x !!\n", 
+			pCoexDm->preVal0x6c0, pCoexDm->preVal0x6c4, pCoexDm->preVal0x6c8, pCoexDm->preVal0x6cc));
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], curVal0x6c0=0x%x, curVal0x6c4=0x%x, curVal0x6c8=0x%x, curVal0x6cc=0x%x !!\n", 
+			pCoexDm->curVal0x6c0, pCoexDm->curVal0x6c4, pCoexDm->curVal0x6c8, pCoexDm->curVal0x6cc));
+	
+		if( (pCoexDm->preVal0x6c0 == pCoexDm->curVal0x6c0) &&
+			(pCoexDm->preVal0x6c4 == pCoexDm->curVal0x6c4) &&
+			(pCoexDm->preVal0x6c8 == pCoexDm->curVal0x6c8) &&
+			(pCoexDm->preVal0x6cc == pCoexDm->curVal0x6cc) )
+			return;
+	}
+	halbtc8192e1ant_SetCoexTable(pBtCoexist, val0x6c0, val0x6c4, val0x6c8, val0x6cc);
+
+	pCoexDm->preVal0x6c0 = pCoexDm->curVal0x6c0;
+	pCoexDm->preVal0x6c4 = pCoexDm->curVal0x6c4;
+	pCoexDm->preVal0x6c8 = pCoexDm->curVal0x6c8;
+	pCoexDm->preVal0x6cc = pCoexDm->curVal0x6cc;
+}
+
+VOID
+halbtc8192e1ant_CoexTableWithType(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN				bForceExec,
+	IN	u1Byte				type
+	)
+{
+	switch(type)
+	{
+		case 0:
+			halbtc8192e1ant_CoexTable(pBtCoexist, bForceExec, 0x55555555, 0x55555555, 0xffffff, 0x3);
+			break;
+		case 1:
+			halbtc8192e1ant_CoexTable(pBtCoexist, bForceExec, 0x55555555, 0x5a5a5a5a, 0xffffff, 0x3);
+			break;
+		case 2:
+			halbtc8192e1ant_CoexTable(pBtCoexist, bForceExec, 0x5a5a5a5a, 0x5a5a5a5a, 0xffffff, 0x3);
+			break;
+		case 3:
+			halbtc8192e1ant_CoexTable(pBtCoexist, bForceExec, 0xaaaaaaaa, 0xaaaaaaaa, 0xffffff, 0x3);
+			break;
+		case 4:
+			halbtc8192e1ant_CoexTable(pBtCoexist, bForceExec, 0xffffffff, 0xffffffff, 0xffffff, 0x3);
+			break;
+		case 5:
+			halbtc8192e1ant_CoexTable(pBtCoexist, bForceExec, 0x5fff5fff, 0x5fff5fff, 0xffffff, 0x3);
+			break;
+		case 6:
+			halbtc8192e1ant_CoexTable(pBtCoexist, bForceExec, 0x55ff55ff, 0x5a5a5a5a, 0xffffff, 0x3);
+			break;
+		case 7:
+			halbtc8192e1ant_CoexTable(pBtCoexist, bForceExec, 0xddffddff, 0xddffddff, 0xffffff, 0x3);
+			break;
+		case 8:
+			halbtc8192e1ant_CoexTable(pBtCoexist, bForceExec, 0x55ff55ff, 0x5afa5afa, 0xffffff, 0x3);
+			break;		
+		case 9:
+			halbtc8192e1ant_CoexTable(pBtCoexist, bForceExec, 0x5f5f5f5f, 0x5f5f5f5f, 0xffffff, 0x3);
+			break;
+		default:
+			break;
+	}
+}
+
+VOID
+halbtc8192e1ant_SetFwIgnoreWlanAct(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bEnable
+	)
+{
+	u1Byte			H2C_Parameter[1] ={0};
+		
+	if(bEnable)
+	{
+		H2C_Parameter[0] |= BIT0;		// function enable
+	}
+	
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], set FW for BT Ignore Wlan_Act, FW write 0x63=0x%x\n", 
+		H2C_Parameter[0]));
+
+	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x63, 1, H2C_Parameter);
+}
+
+VOID
+halbtc8192e1ant_IgnoreWlanAct(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bForceExec,
+	IN	BOOLEAN			bEnable
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s turn Ignore WlanAct %s\n", 
+		(bForceExec? "force to":""), (bEnable? "ON":"OFF")));
+	pCoexDm->bCurIgnoreWlanAct = bEnable;
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], bPreIgnoreWlanAct = %d, bCurIgnoreWlanAct = %d!!\n", 
+			pCoexDm->bPreIgnoreWlanAct, pCoexDm->bCurIgnoreWlanAct));
+
+		if(pCoexDm->bPreIgnoreWlanAct == pCoexDm->bCurIgnoreWlanAct)
+			return;
+	}
+	halbtc8192e1ant_SetFwIgnoreWlanAct(pBtCoexist, bEnable);
+
+	pCoexDm->bPreIgnoreWlanAct = pCoexDm->bCurIgnoreWlanAct;
+}
+
+VOID
+halbtc8192e1ant_SetFwPstdma(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			byte1,
+	IN	u1Byte			byte2,
+	IN	u1Byte			byte3,
+	IN	u1Byte			byte4,
+	IN	u1Byte			byte5
+	)
+{
+	u1Byte			H2C_Parameter[5] ={0};
+
+	H2C_Parameter[0] = byte1;	
+	H2C_Parameter[1] = byte2;	
+	H2C_Parameter[2] = byte3;
+	H2C_Parameter[3] = byte4;
+	H2C_Parameter[4] = byte5;
+
+	pCoexDm->psTdmaPara[0] = byte1;
+	pCoexDm->psTdmaPara[1] = byte2;
+	pCoexDm->psTdmaPara[2] = byte3;
+	pCoexDm->psTdmaPara[3] = byte4;
+	pCoexDm->psTdmaPara[4] = byte5;
+	
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], FW write 0x60(5bytes)=0x%x%08x\n", 
+		H2C_Parameter[0], 
+		H2C_Parameter[1]<<24|H2C_Parameter[2]<<16|H2C_Parameter[3]<<8|H2C_Parameter[4]));
+
+	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x60, 5, H2C_Parameter);
+}
+
+VOID
+halbtc8192e1ant_SetLpsRpwm(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			lpsVal,
+	IN	u1Byte			rpwmVal
+	)
+{
+	u1Byte	lps=lpsVal;
+	u1Byte	rpwm=rpwmVal;
+	
+	pBtCoexist->btc_set(pBtCoexist, BTC_SET_U1_1ANT_LPS, &lps);
+	pBtCoexist->btc_set(pBtCoexist, BTC_SET_U1_1ANT_RPWM, &rpwm);
+}
+
+VOID
+halbtc8192e1ant_LpsRpwm(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bForceExec,
+	IN	u1Byte			lpsVal,
+	IN	u1Byte			rpwmVal
+	)
+{
+	BOOLEAN	bForceExecPwrCmd=FALSE;
+	
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s set lps/rpwm=0x%x/0x%x \n", 
+		(bForceExec? "force to":""), lpsVal, rpwmVal));
+	pCoexDm->curLps = lpsVal;
+	pCoexDm->curRpwm = rpwmVal;
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], preLps/curLps=0x%x/0x%x, preRpwm/curRpwm=0x%x/0x%x!!\n", 
+			pCoexDm->preLps, pCoexDm->curLps, pCoexDm->preRpwm, pCoexDm->curRpwm));
+
+		if( (pCoexDm->preLps == pCoexDm->curLps) &&
+			(pCoexDm->preRpwm == pCoexDm->curRpwm) )
+		{
+			return;
+		}
+	}
+	halbtc8192e1ant_SetLpsRpwm(pBtCoexist, lpsVal, rpwmVal);
+
+	pCoexDm->preLps = pCoexDm->curLps;
+	pCoexDm->preRpwm = pCoexDm->curRpwm;
+}
+
+VOID
+halbtc8192e1ant_SwMechanism1(
+	IN	PBTC_COEXIST	pBtCoexist,	
+	IN	BOOLEAN		bShrinkRxLPF,
+	IN	BOOLEAN 	bLowPenaltyRA,
+	IN	BOOLEAN		limited_dig, 
+	IN	BOOLEAN		bBTLNAConstrain
+	) 
+{
+	//halbtc8192e1ant_RfShrink(pBtCoexist, NORMAL_EXEC, bShrinkRxLPF);
+	//halbtc8192e1ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, bLowPenaltyRA);
+
+	//no limited DIG
+	//halbtc8192e1ant_SetBtLnaConstrain(pBtCoexist, NORMAL_EXEC, bBTLNAConstrain);
+}
+
+VOID
+halbtc8192e1ant_SwMechanism2(
+	IN	PBTC_COEXIST	pBtCoexist,	
+	IN	BOOLEAN		bAGCTableShift,
+	IN	BOOLEAN 	bADCBackOff,
+	IN	BOOLEAN		bSWDACSwing,
+	IN	u4Byte		dacSwingLvl
+	)
+{
+	//halbtc8192e1ant_AgcTable(pBtCoexist, NORMAL_EXEC, bAGCTableShift);
+	//halbtc8192e1ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, bADCBackOff);
+	//halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, bSWDACSwing, dacSwingLvl);
+}
+
+VOID
+halbtc8192e1ant_PsTdma(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bForceExec,
+	IN	BOOLEAN			bTurnOn,
+	IN	u1Byte			type
+	)
+{
+	BOOLEAN			bTurnOnByCnt=FALSE;
+	u1Byte			psTdmaTypeByCnt=0, rssiAdjustVal=0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s turn %s PS TDMA, type=%d\n", 
+		(bForceExec? "force to":""), (bTurnOn? "ON":"OFF"), type));
+	pCoexDm->bCurPsTdmaOn = bTurnOn;
+	pCoexDm->curPsTdma = type;
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], bPrePsTdmaOn = %d, bCurPsTdmaOn = %d!!\n", 
+			pCoexDm->bPrePsTdmaOn, pCoexDm->bCurPsTdmaOn));
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], prePsTdma = %d, curPsTdma = %d!!\n", 
+			pCoexDm->prePsTdma, pCoexDm->curPsTdma));
+
+		if( (pCoexDm->bPrePsTdmaOn == pCoexDm->bCurPsTdmaOn) &&
+			(pCoexDm->prePsTdma == pCoexDm->curPsTdma) )
+			return;
+	}
+	if(bTurnOn)
+	{
+		switch(type)
+		{
+			default:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x53, 0x2c, 0x03, 0x10, 0x50);
+				break;
+			case 1:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x53, 0x2c, 0x03, 0x10, 0x50);
+				rssiAdjustVal = 11;
+				break;
+			case 2:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x53, 0x25, 0x03, 0x10, 0x50);
+				rssiAdjustVal = 14;
+				break;
+			case 3:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x93, 0x25, 0x3, 0x10, 0x40);
+				break;
+			case 4:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x93, 0x15, 0x3, 0x14, 0x0);
+				rssiAdjustVal = 17;
+				break;
+			case 5:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x61, 0x15, 0x3, 0x31, 0x0);
+				break;
+			case 6:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x13, 0xa, 0x3, 0x0, 0x0);
+				break;
+			case 7:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x13, 0xc, 0x5, 0x0, 0x0);
+				break;
+			case 8:	
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x93, 0x25, 0x3, 0x10, 0x0);
+				break;
+			case 9:	
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x53, 0x1e, 0x03, 0x10, 0x50);
+				rssiAdjustVal = 18;
+				break;
+			case 10:	
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x13, 0xa, 0xa, 0x0, 0x40);
+				break;
+			case 11:	
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x53, 0x12, 0x03, 0x10, 0x50);
+				rssiAdjustVal = 20;
+				break;
+			case 12:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0xeb, 0xa, 0x3, 0x31, 0x18);
+				break;
+
+			case 15:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x13, 0xa, 0x3, 0x8, 0x0);
+				break;
+			case 16:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x93, 0x15, 0x3, 0x10, 0x0);
+				rssiAdjustVal = 18;
+				break;
+
+			case 18:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x93, 0x25, 0x3, 0x10, 0x0);
+				rssiAdjustVal = 14;
+				break;			
+				
+			case 20:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x13, 0x25, 0x25, 0x0, 0x0);
+				break;
+			case 21:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x93, 0x20, 0x3, 0x10, 0x40);
+				break;
+			case 22:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x13, 0x8, 0x8, 0x0, 0x40);
+				break;
+			case 23:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0xe3, 0x25, 0x3, 0x31, 0x18);
+				rssiAdjustVal = 22;
+				break;
+			case 24:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0xe3, 0x15, 0x3, 0x31, 0x18);
+				rssiAdjustVal = 22;
+				break;
+			case 25:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0xe3, 0xa, 0x3, 0x31, 0x18);
+				rssiAdjustVal = 22;
+				break;
+			case 26:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0xe3, 0xa, 0x3, 0x31, 0x18);
+				rssiAdjustVal = 22;
+				break;
+			case 27:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0xe3, 0x25, 0x3, 0x31, 0x98);
+				rssiAdjustVal = 22;
+				break;
+			case 28:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x69, 0x25, 0x3, 0x31, 0x0);
+				break;
+			case 29:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0xab, 0x1a, 0x1a, 0x1, 0x10);
+				break;
+			case 30:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x93, 0x15, 0x3, 0x14, 0x0);
+				break;
+			case 31:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0xd3, 0x1a, 0x1a, 0, 0x58);
+				break;
+			case 32:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0xab, 0xa, 0x3, 0x31, 0x90);
+				break;
+			case 33:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0xa3, 0x25, 0x3, 0x30, 0x90);
+				break;
+			case 34:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0xd3, 0x1a, 0x1a, 0x0, 0x10);
+				break;
+			case 35:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0xe3, 0x1a, 0x1a, 0x0, 0x10);
+				break;
+			case 36:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0xd3, 0x12, 0x3, 0x14, 0x50);
+				break;
+			case 37:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x53, 0x25, 0x3, 0x10, 0x50);
+				break;
+			case 38:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0xe3, 0x12, 0x12, 0xe1, 0x90);
+				break;
+		}
+	}
+	else
+	{
+		// disable PS tdma
+		switch(type)
+		{
+			case 8:		//0x778 = 1, ant2PTA
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x8, 0x0, 0x0, 0x0, 0x0);
+				pBtCoexist->btc_write_1byte(pBtCoexist, 0x92c, 0x4);
+				break;
+			case 0:		//0x778 = 1, ant2BT
+			default:
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x0, 0x0, 0x0, 0x0, 0x0);
+				mdelay(5);
+				pBtCoexist->btc_write_1byte(pBtCoexist, 0x92c, 0x20);
+				break;
+			case 9:		//0x778 = 1, ant2WIFI
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x0, 0x0, 0x0, 0x0, 0x0);
+				pBtCoexist->btc_write_1byte(pBtCoexist, 0x92c, 0x4);
+				break;
+			case 10:	//0x778 = 3, ant2BT
+				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x0, 0x0, 0x0, 0x8, 0x0);
+				mdelay(5);
+				pBtCoexist->btc_write_1byte(pBtCoexist, 0x92c, 0x20);
+				break;
+		}
+	}
+	rssiAdjustVal =0;
+	pBtCoexist->btc_set(pBtCoexist, BTC_SET_U1_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE, &rssiAdjustVal);
+
+	// update pre state
+	pCoexDm->bPrePsTdmaOn = pCoexDm->bCurPsTdmaOn;
+	pCoexDm->prePsTdma = pCoexDm->curPsTdma;
+}
+
+VOID
+halbtc8192e1ant_SetSwitchSsType(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte				ssType
+	)
+{
+	u1Byte	mimoPs=BTC_MIMO_PS_DYNAMIC;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], REAL set SS Type = %d\n", ssType));
+
+	if(ssType == 1)
+	{
+		halbtc8192e1ant_Updatera_mask(pBtCoexist, FORCE_EXEC, BTC_RATE_DISABLE, 0xfff00000);	// disable 2ss
+		halbtc8192e1ant_PsTdma(pBtCoexist, FORCE_EXEC, FALSE, 0);
+		// switch ofdm path
+		pBtCoexist->btc_write_1byte(pBtCoexist, 0xc04, 0x11);
+		pBtCoexist->btc_write_1byte(pBtCoexist, 0xd04, 0x1);
+		pBtCoexist->btc_write_4byte(pBtCoexist, 0x90c, 0x81111111);
+		// switch cck patch
+		pBtCoexist->btc_write_1byte_bitmask(pBtCoexist, 0xe77, 0x4, 0x1);
+		pBtCoexist->btc_write_1byte(pBtCoexist, 0xa07, 0x81);
+		mimoPs=BTC_MIMO_PS_STATIC;
+	}
+	else if(ssType == 2)
+	{
+		halbtc8192e1ant_Updatera_mask(pBtCoexist, FORCE_EXEC, BTC_RATE_ENABLE, 0xfff00000);	// enable 2ss
+		halbtc8192e1ant_PsTdma(pBtCoexist, FORCE_EXEC, FALSE, 8);
+		pBtCoexist->btc_write_1byte(pBtCoexist, 0xc04, 0x33);
+		pBtCoexist->btc_write_1byte(pBtCoexist, 0xd04, 0x3);
+		pBtCoexist->btc_write_4byte(pBtCoexist, 0x90c, 0x81121313);
+		pBtCoexist->btc_write_1byte_bitmask(pBtCoexist, 0xe77, 0x4, 0x0);
+		pBtCoexist->btc_write_1byte(pBtCoexist, 0xa07, 0x41);
+		mimoPs=BTC_MIMO_PS_DYNAMIC;
+	}
+	
+	pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_SEND_MIMO_PS, &mimoPs);	// set rx 1ss or 2ss
+}
+
+VOID
+halbtc8192e1ant_SwitchSsType(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN				bForceExec,
+	IN	u1Byte				newSsType
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], %s Switch SS Type = %d\n",  
+		(bForceExec? "force to":""), newSsType));
+	pCoexDm->curSsType = newSsType;
+
+	if(!bForceExec)
+	{
+		if(pCoexDm->preSsType == pCoexDm->curSsType) 
+			return;
+	}
+	halbtc8192e1ant_SetSwitchSsType(pBtCoexist, pCoexDm->curSsType);
+
+	pCoexDm->preSsType = pCoexDm->curSsType;
+}
+
+VOID
+halbtc8192e1ant_CoexAllOff(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+	// sw all off
+	halbtc8192e1ant_SwMechanism1(pBtCoexist,FALSE,FALSE,FALSE,FALSE);
+	halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
+
+
+	// hw all off
+	halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
+}
+
+BOOLEAN
+halbtc8192e1ant_IsCommonAction(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	BOOLEAN			bCommon=FALSE, bWifiConnected=FALSE, bWifiBusy=FALSE;
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
+
+	if(!bWifiConnected && 
+		BT_8192E_1ANT_BT_STATUS_NON_CONNECTED_IDLE == pCoexDm->btStatus)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi non connected-idle + BT non connected-idle!!\n"));		
+ 		halbtc8192e1ant_SwMechanism1(pBtCoexist,FALSE,FALSE,FALSE,FALSE);
+		halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
+
+		bCommon = true;
+	}
+	else if(bWifiConnected && 
+		(BT_8192E_1ANT_BT_STATUS_NON_CONNECTED_IDLE == pCoexDm->btStatus) )
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi connected + BT non connected-idle!!\n"));
+      	halbtc8192e1ant_SwMechanism1(pBtCoexist,FALSE,FALSE,FALSE,FALSE);
+		halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
+
+		bCommon = true;
+	}
+	else if(!bWifiConnected && 
+		(BT_8192E_1ANT_BT_STATUS_CONNECTED_IDLE == pCoexDm->btStatus) )
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi non connected-idle + BT connected-idle!!\n"));
+		halbtc8192e1ant_SwMechanism1(pBtCoexist,FALSE,FALSE,FALSE,FALSE);
+		halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
+
+		bCommon = true;
+	}
+	else if(bWifiConnected && 
+		(BT_8192E_1ANT_BT_STATUS_CONNECTED_IDLE == pCoexDm->btStatus) )
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi connected + BT connected-idle!!\n"));
+		halbtc8192e1ant_SwMechanism1(pBtCoexist,true,true,true,true);
+		halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
+
+		bCommon = true;
+	}
+	else if(!bWifiConnected && 
+		(BT_8192E_1ANT_BT_STATUS_CONNECTED_IDLE != pCoexDm->btStatus) )
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi non connected-idle + BT Busy!!\n"));
+		halbtc8192e1ant_SwMechanism1(pBtCoexist,FALSE,FALSE,FALSE,FALSE);
+		halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
+		
+		bCommon = true;
+	}
+	else
+	{
+		halbtc8192e1ant_SwMechanism1(pBtCoexist,true,true,true,true);
+		
+		bCommon = FALSE;
+	}
+	
+	return bCommon;
+}
+
+
+VOID
+halbtc8192e1ant_TdmaDurationAdjustForAcl(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte				wifiStatus
+	)
+{
+	static s4Byte		up,dn,m,n,WaitCount;
+	s4Byte			result;   //0: no change, +1: increase WiFi duration, -1: decrease WiFi duration
+	u1Byte			retryCount=0, btInfoExt;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], TdmaDurationAdjustForAcl()\n"));
+
+	if( (BT_8192E_1ANT_WIFI_STATUS_NON_CONNECTED_ASSO_AUTH_SCAN == wifiStatus) ||
+		(BT_8192E_1ANT_WIFI_STATUS_CONNECTED_SCAN == wifiStatus) ||
+		(BT_8192E_1ANT_WIFI_STATUS_CONNECTED_SPECIAL_PKT == wifiStatus) )
+	{
+		if( pCoexDm->curPsTdma != 1 &&
+			pCoexDm->curPsTdma != 2 &&
+			pCoexDm->curPsTdma != 3 &&
+			pCoexDm->curPsTdma != 9 )
+		{
+			halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+			pCoexDm->psTdmaDuAdjType = 9;
+
+			up = 0;
+			dn = 0;
+			m = 1;
+			n= 3;
+			result = 0;
+			WaitCount = 0;
+		}		
+		return;
+	}
+
+	if(!pCoexDm->bAutoTdmaAdjust)
+	{
+		pCoexDm->bAutoTdmaAdjust = true;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], first run TdmaDurationAdjust()!!\n"));
+
+		halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+		pCoexDm->psTdmaDuAdjType = 2;
+		//============
+		up = 0;
+		dn = 0;
+		m = 1;
+		n= 3;
+		result = 0;
+		WaitCount = 0;
+	}
+	else
+	{
+		//accquire the BT TRx retry count from BT_Info byte2
+		retryCount = pCoexSta->btRetryCnt;
+		btInfoExt = pCoexSta->btInfoExt;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], retryCount = %d\n", retryCount));
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], up=%d, dn=%d, m=%d, n=%d, WaitCount=%d\n", 
+			up, dn, m, n, WaitCount));
+		result = 0;
+		WaitCount++; 
+		  
+		if(retryCount == 0)  // no retry in the last 2-second duration
+		{
+			up++;
+			dn--;
+
+			if (dn <= 0)
+				dn = 0;				 
+
+			if(up >= n)	// if ³sÄò n ­Ó2¬í retry count¬°0, «h½Õ¼eWiFi duration
+			{
+				WaitCount = 0; 
+				n = 3;
+				up = 0;
+				dn = 0;
+				result = 1; 
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], Increase wifi duration!!\n"));
+			}
+		}
+		else if (retryCount <= 3)	// <=3 retry in the last 2-second duration
+		{
+			up--; 
+			dn++;
+
+			if (up <= 0)
+				up = 0;
+
+			if (dn == 2)	// if ³sÄò 2 ­Ó2¬í retry count< 3, «h½Õ¯¶WiFi duration
+			{
+				if (WaitCount <= 2)
+					m++; // ÁקK¤@ª½¦b¨â­Ólevel¤¤¨Ó¦^
+				else
+					m = 1;
+
+				if ( m >= 20) //m ³Ì¤j­È = 20 ' ³Ì¤j120¬í recheck¬O§_½Õ¾ã WiFi duration.
+					m = 20;
+
+				n = 3*m;
+				up = 0;
+				dn = 0;
+				WaitCount = 0;
+				result = -1; 
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], Decrease wifi duration for retryCounter<3!!\n"));
+			}
+		}
+		else  //retry count > 3, ¥u­n1¦¸ retry count > 3, «h½Õ¯¶WiFi duration
+		{
+			if (WaitCount == 1)
+				m++; // ÁקK¤@ª½¦b¨â­Ólevel¤¤¨Ó¦^
+			else
+				m = 1;
+
+			if ( m >= 20) //m ³Ì¤j­È = 20 ' ³Ì¤j120¬í recheck¬O§_½Õ¾ã WiFi duration.
+				m = 20;
+
+			n = 3*m;
+			up = 0;
+			dn = 0;
+			WaitCount = 0; 
+			result = -1;
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], Decrease wifi duration for retryCounter>3!!\n"));
+		}
+
+		if(result == -1)
+		{
+			if( (BT_INFO_8192E_1ANT_A2DP_BASIC_RATE(btInfoExt)) &&
+				((pCoexDm->curPsTdma == 1) ||(pCoexDm->curPsTdma == 2)) )
+			{
+				halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+				pCoexDm->psTdmaDuAdjType = 9;
+			}
+			else if(pCoexDm->curPsTdma == 1)
+			{
+				halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+				pCoexDm->psTdmaDuAdjType = 2;
+			}
+			else if(pCoexDm->curPsTdma == 2)
+			{
+				halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+				pCoexDm->psTdmaDuAdjType = 9;
+			}
+			else if(pCoexDm->curPsTdma == 9)
+			{
+				halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+				pCoexDm->psTdmaDuAdjType = 11;
+			}
+		}
+		else if(result == 1)
+		{
+			if( (BT_INFO_8192E_1ANT_A2DP_BASIC_RATE(btInfoExt)) &&
+				((pCoexDm->curPsTdma == 1) ||(pCoexDm->curPsTdma == 2)) )
+			{
+				halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+				pCoexDm->psTdmaDuAdjType = 9;
+			}
+			else if(pCoexDm->curPsTdma == 11)
+			{
+				halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+				pCoexDm->psTdmaDuAdjType = 9;
+			}
+			else if(pCoexDm->curPsTdma == 9)
+			{
+				halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+				pCoexDm->psTdmaDuAdjType = 2;
+			}
+			else if(pCoexDm->curPsTdma == 2)
+			{
+				halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 1);
+				pCoexDm->psTdmaDuAdjType = 1;
+			}
+		}
+
+		if( pCoexDm->curPsTdma != 1 &&
+			pCoexDm->curPsTdma != 2 &&
+			pCoexDm->curPsTdma != 9 &&
+			pCoexDm->curPsTdma != 11 )
+		{
+			// recover to previous adjust type
+			halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, pCoexDm->psTdmaDuAdjType);
+		}
+	}
+}
+
+u1Byte
+halbtc8192e1ant_PsTdmaTypeByWifiRssi(
+	IN	s4Byte	wifiRssi,
+	IN	s4Byte	preWifiRssi,
+	IN	u1Byte	wifiRssiThresh
+	)
+{
+	u1Byte	psTdmaType=0;
+	
+	if(wifiRssi > preWifiRssi)
+	{
+		if(wifiRssi > (wifiRssiThresh+5))
+		{
+			psTdmaType = 26;
+		}
+		else
+		{
+			psTdmaType = 25;
+		}
+	}
+	else
+	{
+		if(wifiRssi > wifiRssiThresh)
+		{
+			psTdmaType = 26;
+		}
+		else
+		{
+			psTdmaType = 25;
+		}
+	}
+
+	return psTdmaType;
+}
+
+VOID
+halbtc8192e1ant_PsTdmaCheckForPowerSaveState(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bNewPsState
+	)
+{
+	u1Byte	lpsMode=0x0;
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U1_LPS_MODE, &lpsMode);
+	
+	if(lpsMode)	// already under LPS state
+	{
+		if(bNewPsState)		
+		{
+			// keep state under LPS, do nothing.
+		}
+		else
+		{
+			// will leave LPS state, turn off psTdma first
+			halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
+		}
+	}
+	else						// NO PS state
+	{
+		if(bNewPsState)
+		{
+			// will enter LPS state, turn off psTdma first
+			halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
+		}
+		else
+		{
+			// keep state under NO PS state, do nothing.
+		}
+	}
+}
+
+VOID
+halbtc8192e1ant_PowerSaveState(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte				psType,
+	IN	u1Byte				lpsVal,
+	IN	u1Byte				rpwmVal
+	)
+{
+	BOOLEAN		bLowPwrDisable=FALSE;
+
+	switch(psType)
+	{
+		case BTC_PS_WIFI_NATIVE:
+			// recover to original 32k low power setting
+			bLowPwrDisable = FALSE;
+			pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_DISABLE_LOW_POWER, &bLowPwrDisable);
+			pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_NORMAL_LPS, NULL);
+			break;
+		case BTC_PS_LPS_ON:
+			halbtc8192e1ant_PsTdmaCheckForPowerSaveState(pBtCoexist, true);
+			halbtc8192e1ant_LpsRpwm(pBtCoexist, NORMAL_EXEC, lpsVal, rpwmVal);
+			// when coex force to enter LPS, do not enter 32k low power.
+			bLowPwrDisable = true;
+			pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_DISABLE_LOW_POWER, &bLowPwrDisable);
+			// power save must executed before psTdma.
+			pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_ENTER_LPS, NULL);
+			break;
+		case BTC_PS_LPS_OFF:
+			halbtc8192e1ant_PsTdmaCheckForPowerSaveState(pBtCoexist, FALSE);
+			pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
+			break;
+		default:
+			break;
+	}
+}
+
+
+VOID
+halbtc8192e1ant_ActionWifiOnly(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
+	halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 9);	
+}
+
+VOID
+halbtc8192e1ant_MonitorBtEnableDisable(
+	IN 	PBTC_COEXIST		pBtCoexist
+	)
+{
+	static BOOLEAN	bPreBtDisabled=FALSE;
+	static u4Byte		btDisableCnt=0;
+	BOOLEAN			bBtActive=true, bBtDisabled=FALSE;
+
+	// This function check if bt is disabled
+
+	if(	pCoexSta->highPriorityTx == 0 &&
+		pCoexSta->highPriorityRx == 0 &&
+		pCoexSta->lowPriorityTx == 0 &&
+		pCoexSta->lowPriorityRx == 0)
+	{
+		bBtActive = FALSE;
+	}
+	if(	pCoexSta->highPriorityTx == 0xffff &&
+		pCoexSta->highPriorityRx == 0xffff &&
+		pCoexSta->lowPriorityTx == 0xffff &&
+		pCoexSta->lowPriorityRx == 0xffff)
+	{
+		bBtActive = FALSE;
+	}
+	if(bBtActive)
+	{
+		btDisableCnt = 0;
+		bBtDisabled = FALSE;
+		pBtCoexist->btc_set(pBtCoexist, BTC_SET_BL_BT_DISABLE, &bBtDisabled);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], BT is enabled !!\n"));
+	}
+	else
+	{
+		btDisableCnt++;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], bt all counters=0, %d times!!\n", 
+				btDisableCnt));
+		if(btDisableCnt >= 2)
+		{
+			bBtDisabled = true;
+			pBtCoexist->btc_set(pBtCoexist, BTC_SET_BL_BT_DISABLE, &bBtDisabled);
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], BT is disabled !!\n"));
+			halbtc8192e1ant_ActionWifiOnly(pBtCoexist);
+		}
+	}
+	if(bPreBtDisabled != bBtDisabled)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], BT is from %s to %s!!\n", 
+			(bPreBtDisabled ? "disabled":"enabled"), 
+			(bBtDisabled ? "disabled":"enabled")));
+		bPreBtDisabled = bBtDisabled;
+		if(!bBtDisabled)
+		{
+		}
+		else
+		{
+			pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
+			pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_NORMAL_LPS, NULL);
+		}
+	}
+}
+
+//=============================================
+//
+//	Software Coex Mechanism start
+//
+//=============================================
+
+// SCO only or SCO+PAN(HS)
+VOID
+halbtc8192e1ant_ActionSco(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	u1Byte	wifiRssiState;
+	u4Byte	wifiBw;
+
+	wifiRssiState = halbtc8192e1ant_WifiRssiState(pBtCoexist, 0, 2, 25, 0);
+	
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+
+	if(BTC_WIFI_BW_HT40 == wifiBw)
+	{
+		// sw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			  halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
+		}
+		else
+		{
+			  halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,true,FALSE,0x18);
+		}
+	}
+	else
+	{
+		// sw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
+		}
+		else
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
+		}		
+	}
+}
+
+
+VOID
+halbtc8192e1ant_ActionHid(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	u1Byte	wifiRssiState;	
+	u4Byte	wifiBw;
+
+	wifiRssiState = halbtc8192e1ant_WifiRssiState(pBtCoexist, 0, 2, 25, 0);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+
+	if(BTC_WIFI_BW_HT40 == wifiBw)
+	{
+		// sw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,FALSE,FALSE,0x18);
+		}
+		else
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
+		}
+	}
+	else
+	{
+		// sw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
+		}
+		else
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
+		}		
+	}
+}
+
+//A2DP only / PAN(EDR) only/ A2DP+PAN(HS)
+VOID
+halbtc8192e1ant_ActionA2dp(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	u1Byte		wifiRssiState;
+	u4Byte		wifiBw;
+
+	wifiRssiState = halbtc8192e1ant_WifiRssiState(pBtCoexist, 0, 2, 25, 0);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+
+	if(BTC_WIFI_BW_HT40 == wifiBw)
+	{
+		// sw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
+		}
+		else
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,true,FALSE,0x18);
+		}
+	}
+	else
+	{
+		// sw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
+		}
+		else
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
+		}		
+	}
+}
+
+VOID
+halbtc8192e1ant_ActionA2dpPanHs(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	u1Byte		wifiRssiState, btInfoExt;
+	u4Byte		wifiBw;
+
+	btInfoExt = pCoexSta->btInfoExt;
+	wifiRssiState = halbtc8192e1ant_WifiRssiState(pBtCoexist, 0, 2, 25, 0);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+
+	if(BTC_WIFI_BW_HT40 == wifiBw)
+	{
+		// sw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
+		}
+		else
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,true,FALSE,0x18);
+		}
+	}
+	else
+	{
+		// sw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
+		}
+		else
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
+		}		
+	}
+}
+
+VOID
+halbtc8192e1ant_ActionPanEdr(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	u1Byte		wifiRssiState;
+	u4Byte		wifiBw;
+
+	wifiRssiState = halbtc8192e1ant_WifiRssiState(pBtCoexist, 0, 2, 25, 0);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+
+	if(BTC_WIFI_BW_HT40 == wifiBw)
+	{
+		// sw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
+		}
+		else
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,true,FALSE,0x18);
+		}
+	}
+	else
+	{
+		// sw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
+		}
+		else
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
+		}
+	}
+}
+
+
+//PAN(HS) only
+VOID
+halbtc8192e1ant_ActionPanHs(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	u1Byte		wifiRssiState;
+	u4Byte		wifiBw;
+
+	wifiRssiState = halbtc8192e1ant_WifiRssiState(pBtCoexist, 0, 2, 25, 0);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+
+	if(BTC_WIFI_BW_HT40 == wifiBw)
+	{
+		// sw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
+		}
+		else
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,true,FALSE,0x18);
+		}
+	}
+	else
+	{
+		// sw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
+		}
+		else
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
+		}
+	}
+}
+
+//PAN(EDR)+A2DP
+VOID
+halbtc8192e1ant_ActionPanEdrA2dp(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	u1Byte		wifiRssiState, btInfoExt;
+	u4Byte		wifiBw;
+
+	btInfoExt = pCoexSta->btInfoExt;
+	wifiRssiState = halbtc8192e1ant_WifiRssiState(pBtCoexist, 0, 2, 25, 0);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+
+	if(BTC_WIFI_BW_HT40 == wifiBw)
+	{
+		// sw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
+		}
+		else
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,true,FALSE,0x18);
+		}
+	}
+	else
+	{
+		// sw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
+		}
+		else
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
+		}
+	}
+}
+
+VOID
+halbtc8192e1ant_ActionPanEdrHid(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	u1Byte		wifiRssiState;
+	u4Byte		wifiBw;
+
+	wifiRssiState = halbtc8192e1ant_WifiRssiState(pBtCoexist, 0, 2, 25, 0);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+
+	if(BTC_WIFI_BW_HT40 == wifiBw)
+	{
+		// sw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
+		}
+		else
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,true,FALSE,0x18);
+		}
+	}
+	else
+	{		
+		// sw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
+		}
+		else
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
+		}
+	}
+}
+
+// HID+A2DP+PAN(EDR)
+VOID
+halbtc8192e1ant_ActionHidA2dpPanEdr(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	u1Byte		wifiRssiState, btInfoExt;
+	u4Byte		wifiBw;
+
+	btInfoExt = pCoexSta->btInfoExt;
+	wifiRssiState = halbtc8192e1ant_WifiRssiState(pBtCoexist, 0, 2, 25, 0);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+
+	if(BTC_WIFI_BW_HT40 == wifiBw)
+	{	
+		// sw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
+		}
+		else
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,true,FALSE,0x18);
+		}
+	}
+	else
+	{
+		// sw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
+		}
+		else
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
+		}
+	}
+}
+
+VOID
+halbtc8192e1ant_ActionHidA2dp(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	u1Byte		wifiRssiState, btInfoExt;
+	u4Byte		wifiBw;
+
+	btInfoExt = pCoexSta->btInfoExt;
+	wifiRssiState = halbtc8192e1ant_WifiRssiState(pBtCoexist, 0, 2, 25, 0);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+
+	if(BTC_WIFI_BW_HT40 == wifiBw)
+	{		
+		// sw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
+		}
+		else
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,true,FALSE,0x18);
+		}
+	}
+	else
+	{
+		// sw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
+		}
+		else
+		{
+			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
+		}
+	}
+}
+
+//=============================================
+//
+//	Non-Software Coex Mechanism start
+//
+//=============================================
+VOID
+halbtc8192e1ant_ActionBtInquiry(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	PBTC_BT_LINK_INFO	pBtLinkInfo=&pBtCoexist->bt_link_info;
+	BOOLEAN				bWifiConnected=FALSE, bBtHsOn=FALSE;
+	
+	// Note:
+	// Do not do DacSwing here, use original setting.
+	
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	if(bBtHsOn)
+		return;
+
+	if(!bWifiConnected)
+	{
+		halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+		
+		halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+		halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 8);
+		halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
+	}
+	else if( (pBtLinkInfo->bScoExist) ||
+			(pBtLinkInfo->bHidOnly) )
+	{
+		// SCO/HID-only busy
+		halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+		
+		halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+		halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 32);
+		halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 1);
+	}
+	else
+	{
+		halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_LPS_ON, 0x50, 0x0);
+
+		halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+		halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 30);
+		halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
+	}
+}
+
+VOID
+halbtc8192e1ant_ActionBtScoHidOnlyBusy(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte				wifiStatus
+	)
+{
+	PBTC_BT_LINK_INFO pBtLinkInfo=&pBtCoexist->bt_link_info;
+	u1Byte		btRssiState=BTC_RSSI_STATE_HIGH;
+
+	if(BT_8192E_1ANT_WIFI_STATUS_NON_CONNECTED_ASSO_AUTH_SCAN == wifiStatus)
+	{		
+		halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+		halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0);
+	
+		halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 8);
+		halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
+	}
+	else
+	{
+		if(pBtLinkInfo->bHidOnly)
+		{
+			halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+			halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0);
+		
+			halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 8);
+			halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
+		}
+		else
+		{
+			// dec bt power for diff level
+			btRssiState = halbtc8192e1ant_BtRssiState(3, 34, 42);
+			if( (btRssiState == BTC_RSSI_STATE_LOW) ||
+				(btRssiState == BTC_RSSI_STATE_STAY_LOW) )
+			{
+				halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+			}
+			else if( (btRssiState == BTC_RSSI_STATE_MEDIUM) ||
+					(btRssiState == BTC_RSSI_STATE_STAY_MEDIUM) )
+			{
+				halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+			}
+			else if( (btRssiState == BTC_RSSI_STATE_HIGH) ||
+					(btRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+			{
+				halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 6);
+			}
+
+			// sw dacSwing
+			halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, true, 0xc);
+
+			halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
+			halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 7);			
+		}
+	}
+}
+
+VOID
+halbtc8192e1ant_ActionHs(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	PBTC_BT_LINK_INFO pBtLinkInfo=&pBtCoexist->bt_link_info;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action for HS!!!\n"));
+
+	halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	if(BT_8192E_1ANT_BT_STATUS_NON_CONNECTED_IDLE == pCoexDm->btStatus)
+	{
+		// error, should not be here
+		pCoexDm->errorCondition = 1;
+	}
+	else if(BT_8192E_1ANT_BT_STATUS_CONNECTED_IDLE == pCoexDm->btStatus)
+	{
+		halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+		halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, true, 6);
+
+		halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 10);
+		halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
+	}
+	else if(BT_8192E_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus && 
+			!pBtCoexist->bt_link_info.bHidOnly)
+	{
+		if(pCoexDm->curSsType == 1)
+		{
+			halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+			halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, true, 6);
+			halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 10);
+			//halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 38);
+			halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
+		}
+	}
+	else
+	{
+		halbtc8192e1ant_ActionBtScoHidOnlyBusy(pBtCoexist,
+			BT_8192E_1ANT_WIFI_STATUS_CONNECTED_BUSY);
+	}
+}
+
+VOID
+halbtc8192e1ant_ActionWifiConnectedBtAclBusy(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte				wifiStatus
+	)
+{
+	PBTC_BT_LINK_INFO pBtLinkInfo=&pBtCoexist->bt_link_info;
+
+	if(pBtLinkInfo->bHidOnly)
+	{
+		halbtc8192e1ant_ActionBtScoHidOnlyBusy(pBtCoexist, wifiStatus);
+		pCoexDm->bAutoTdmaAdjust = FALSE;
+		return;
+	}
+	
+	halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+	halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0);
+
+	if( (pBtLinkInfo->bA2dpOnly) ||
+		(pBtLinkInfo->bHidExist&&pBtLinkInfo->bA2dpExist) )
+	{
+		halbtc8192e1ant_TdmaDurationAdjustForAcl(pBtCoexist, wifiStatus);
+	}
+	else if( (pBtLinkInfo->bPanOnly) ||
+			(pBtLinkInfo->bHidExist&&pBtLinkInfo->bPanExist) )
+	{
+		halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+		pCoexDm->bAutoTdmaAdjust = FALSE;
+	}
+	else
+	{
+		if( (BT_8192E_1ANT_WIFI_STATUS_NON_CONNECTED_ASSO_AUTH_SCAN == wifiStatus) ||
+			(BT_8192E_1ANT_WIFI_STATUS_CONNECTED_SCAN == wifiStatus) ||
+			(BT_8192E_1ANT_WIFI_STATUS_CONNECTED_SPECIAL_PKT == wifiStatus) )
+			halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+		else
+			halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+		pCoexDm->bAutoTdmaAdjust = FALSE;
+	}
+		
+	halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 1);
+}
+
+
+VOID
+halbtc8192e1ant_ActionWifiNotConnected(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	// power save state
+	halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+	halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0);
+
+	halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 8);
+	halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);	
+}
+
+VOID
+halbtc8192e1ant_ActionWifiNotConnectedAssoAuthScan(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+	halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0);
+
+	halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 8);
+	halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
+}
+
+VOID
+halbtc8192e1ant_ActionWifiConnectedScan(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	// power save state
+	if(BT_8192E_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus && !pBtCoexist->bt_link_info.bHidOnly)
+		halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_LPS_ON, 0x50, 0x0);
+	else
+		halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	if(BT_8192E_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus)
+	{
+		halbtc8192e1ant_ActionWifiConnectedBtAclBusy(pBtCoexist,
+			BT_8192E_1ANT_WIFI_STATUS_CONNECTED_SCAN);
+	}
+	else if( (BT_8192E_1ANT_BT_STATUS_SCO_BUSY == pCoexDm->btStatus) ||
+			(BT_8192E_1ANT_BT_STATUS_ACL_SCO_BUSY == pCoexDm->btStatus) )
+	{
+		halbtc8192e1ant_ActionBtScoHidOnlyBusy(pBtCoexist,
+			BT_8192E_1ANT_WIFI_STATUS_CONNECTED_SCAN);
+	}
+	else
+	{		
+		halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+		halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0);
+		halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 8);
+		halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
+	}
+}
+
+
+VOID
+halbtc8192e1ant_ActionWifiConnectedSpecialPacket(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	// power save state
+	if(BT_8192E_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus && !pBtCoexist->bt_link_info.bHidOnly)
+		halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_LPS_ON, 0x50, 0x0);
+	else
+		halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	if(BT_8192E_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus)
+	{
+		halbtc8192e1ant_ActionWifiConnectedBtAclBusy(pBtCoexist,
+			BT_8192E_1ANT_WIFI_STATUS_CONNECTED_SPECIAL_PKT);
+	}
+	else
+	{		
+		halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+		halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0);
+		halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 8);
+		halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
+	}
+}
+
+VOID
+halbtc8192e1ant_ActionWifiConnected(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	BOOLEAN 	bWifiConnected=FALSE, bWifiBusy=FALSE;
+	BOOLEAN 	bScan=FALSE, bLink=FALSE, bRoam=FALSE;
+	BOOLEAN 	bUnder4way=FALSE;
+	u4Byte		wifiBw;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], CoexForWifiConnect()===>\n"));
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+	if(!bWifiConnected)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], CoexForWifiConnect(), return for wifi not connected<===\n"));
+		return;
+	}
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS, &bUnder4way);
+	if(bUnder4way)
+	{
+		halbtc8192e1ant_ActionWifiConnectedSpecialPacket(pBtCoexist);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], CoexForWifiConnect(), return for wifi is under 4way<===\n"));
+		return;
+	}
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_SCAN, &bScan);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_LINK, &bLink);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_ROAM, &bRoam);
+	if(bScan || bLink || bRoam)
+	{
+		halbtc8192e1ant_ActionWifiConnectedScan(pBtCoexist);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], CoexForWifiConnect(), return for wifi is under scan<===\n"));
+		return;
+	}
+
+	// power save state
+	if(BT_8192E_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus && !pBtCoexist->bt_link_info.bHidOnly)
+		halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_LPS_ON, 0x50, 0x0);
+	else
+		halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);	
+	if(!bWifiBusy)
+	{
+		if(BT_8192E_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus)
+		{
+			halbtc8192e1ant_ActionWifiConnectedBtAclBusy(pBtCoexist, 
+				BT_8192E_1ANT_WIFI_STATUS_CONNECTED_IDLE);
+		}
+		else if( (BT_8192E_1ANT_BT_STATUS_SCO_BUSY == pCoexDm->btStatus) ||
+			(BT_8192E_1ANT_BT_STATUS_ACL_SCO_BUSY == pCoexDm->btStatus) )
+		{
+			halbtc8192e1ant_ActionBtScoHidOnlyBusy(pBtCoexist,
+				BT_8192E_1ANT_WIFI_STATUS_CONNECTED_IDLE);
+		}
+		else
+		{				
+			halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+			halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0);
+			halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 8);
+			halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
+		}
+	}
+	else
+	{
+		if(BT_8192E_1ANT_BT_STATUS_NON_CONNECTED_IDLE == pCoexDm->btStatus)
+		{				
+			halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+			halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0);
+			halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 5);
+			halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
+		}
+		else if(BT_8192E_1ANT_BT_STATUS_CONNECTED_IDLE == pCoexDm->btStatus)
+		{				
+			halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+			halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0);			
+			halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 5);
+			halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
+		}
+		else if(BT_8192E_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus)
+		{
+			halbtc8192e1ant_ActionWifiConnectedBtAclBusy(pBtCoexist,
+				BT_8192E_1ANT_WIFI_STATUS_CONNECTED_BUSY);
+		}
+		else if( (BT_8192E_1ANT_BT_STATUS_SCO_BUSY == pCoexDm->btStatus) ||
+			(BT_8192E_1ANT_BT_STATUS_ACL_SCO_BUSY == pCoexDm->btStatus) )
+		{
+			halbtc8192e1ant_ActionBtScoHidOnlyBusy(pBtCoexist,
+				BT_8192E_1ANT_WIFI_STATUS_CONNECTED_BUSY);
+		}
+		else 
+		{				
+			halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+			halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0);
+			halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 8);
+			halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
+		}
+	}
+}
+
+VOID
+halbtc8192e1ant_RunSwCoexistMechanism(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	BOOLEAN				bWifiUnder5G=FALSE, bWifiBusy=FALSE, bWifiConnected=FALSE;
+	u1Byte				btInfoOriginal=0, btRetryCnt=0;
+	u1Byte				algorithm=0;
+
+	return;
+
+	algorithm = halbtc8192e1ant_ActionAlgorithm(pBtCoexist);
+	pCoexDm->curAlgorithm = algorithm;
+
+	if(halbtc8192e1ant_IsCommonAction(pBtCoexist))
+	{
+	}
+	else
+	{
+		switch(pCoexDm->curAlgorithm)
+		{
+			case BT_8192E_1ANT_COEX_ALGO_SCO:
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = SCO.\n"));
+				halbtc8192e1ant_ActionSco(pBtCoexist);
+				break;
+			case BT_8192E_1ANT_COEX_ALGO_HID:
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = HID.\n"));
+				halbtc8192e1ant_ActionHid(pBtCoexist);
+				break;
+			case BT_8192E_1ANT_COEX_ALGO_A2DP:
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = A2DP.\n"));
+				halbtc8192e1ant_ActionA2dp(pBtCoexist);
+				break;
+			case BT_8192E_1ANT_COEX_ALGO_A2DP_PANHS:
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = A2DP+PAN(HS).\n"));
+				halbtc8192e1ant_ActionA2dpPanHs(pBtCoexist);
+				break;
+			case BT_8192E_1ANT_COEX_ALGO_PANEDR:
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = PAN(EDR).\n"));
+				halbtc8192e1ant_ActionPanEdr(pBtCoexist);
+				break;
+			case BT_8192E_1ANT_COEX_ALGO_PANHS:
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = HS mode.\n"));
+				halbtc8192e1ant_ActionPanHs(pBtCoexist);
+				break;
+			case BT_8192E_1ANT_COEX_ALGO_PANEDR_A2DP:
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = PAN+A2DP.\n"));
+				halbtc8192e1ant_ActionPanEdrA2dp(pBtCoexist);
+				break;
+			case BT_8192E_1ANT_COEX_ALGO_PANEDR_HID:
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = PAN(EDR)+HID.\n"));
+				halbtc8192e1ant_ActionPanEdrHid(pBtCoexist);
+				break;
+			case BT_8192E_1ANT_COEX_ALGO_HID_A2DP_PANEDR:
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = HID+A2DP+PAN.\n"));
+				halbtc8192e1ant_ActionHidA2dpPanEdr(pBtCoexist);
+				break;
+			case BT_8192E_1ANT_COEX_ALGO_HID_A2DP:
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = HID+A2DP.\n"));
+				halbtc8192e1ant_ActionHidA2dp(pBtCoexist);
+				break;
+			default:
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = coexist All Off!!\n"));
+				halbtc8192e1ant_CoexAllOff(pBtCoexist);
+				break;
+		}
+		pCoexDm->preAlgorithm = pCoexDm->curAlgorithm;
+	}
+}
+
+VOID
+halbtc8192e1ant_RunCoexistMechanism(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	PBTC_BT_LINK_INFO	pBtLinkInfo=&pBtCoexist->bt_link_info;
+	BOOLEAN	bWifiConnected=FALSE, bBtHsOn=FALSE;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], RunCoexistMechanism()===>\n"));
+
+	if(pBtCoexist->manual_control)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], RunCoexistMechanism(), return for Manual CTRL <===\n"));
+		return;
+	}
+
+	if(pBtCoexist->bStopCoexDm)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], RunCoexistMechanism(), return for Stop Coex DM <===\n"));
+		return;
+	}
+
+	if(pCoexSta->bUnderIps)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], wifi is under IPS !!!\n"));
+		return;
+	}
+	
+	halbtc8192e1ant_RunSwCoexistMechanism(pBtCoexist);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	if(pCoexSta->bC2hBtInquiryPage)
+	{
+		halbtc8192e1ant_ActionBtInquiry(pBtCoexist);
+		return;
+	}
+
+	// 1ss or 2ss
+	if(pBtLinkInfo->bScoExist)
+	{
+		halbtc8192e1ant_SwitchSsType(pBtCoexist, NORMAL_EXEC, 1);
+	}
+	else if(bBtHsOn)
+	{
+		if(pBtLinkInfo->bHidOnly)
+			halbtc8192e1ant_SwitchSsType(pBtCoexist, NORMAL_EXEC, 2);
+		else
+			halbtc8192e1ant_SwitchSsType(pBtCoexist, NORMAL_EXEC, 1);
+	}
+	else
+		halbtc8192e1ant_SwitchSsType(pBtCoexist, NORMAL_EXEC, 2);
+	
+	if(bBtHsOn)
+	{
+		halbtc8192e1ant_ActionHs(pBtCoexist);
+		return;
+	}
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+	if(!bWifiConnected)
+	{
+		BOOLEAN	bScan=FALSE, bLink=FALSE, bRoam=FALSE;
+		
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], wifi is non connected-idle !!!\n"));
+
+		pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_SCAN, &bScan);
+		pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_LINK, &bLink);
+		pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_ROAM, &bRoam);
+
+		if(bScan || bLink || bRoam)
+			halbtc8192e1ant_ActionWifiNotConnectedAssoAuthScan(pBtCoexist);
+		else
+			halbtc8192e1ant_ActionWifiNotConnected(pBtCoexist);
+	}
+	else
+	{
+		halbtc8192e1ant_ActionWifiConnected(pBtCoexist);
+	}
+}
+
+VOID
+halbtc8192e1ant_InitCoexDm(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{	
+	// force to reset coex mechanism
+	halbtc8192e1ant_FwDacSwingLvl(pBtCoexist, FORCE_EXEC, 6);
+	halbtc8192e1ant_DecBtPwr(pBtCoexist, FORCE_EXEC, 0);
+
+	// sw all off
+	halbtc8192e1ant_SwMechanism1(pBtCoexist,FALSE,FALSE,FALSE,FALSE);
+	halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
+
+	halbtc8192e1ant_SwitchSsType(pBtCoexist, FORCE_EXEC, 2);
+
+	halbtc8192e1ant_PsTdma(pBtCoexist, FORCE_EXEC, FALSE, 8);
+	halbtc8192e1ant_CoexTableWithType(pBtCoexist, FORCE_EXEC, 0);
+}
+
+//============================================================
+// work around function start with wa_halbtc8192e1ant_
+//============================================================
+//============================================================
+// extern function start with EXhalbtc8192e1ant_
+//============================================================
+VOID
+EXhalbtc8192e1ant_InitHwConfig(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	u4Byte	u4Tmp=0;
+	u16	u2Tmp=0;
+	u1Byte	u1Tmp=0;
+
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], 1Ant Init HW Config!!\n"));
+
+	// backup rf 0x1e value
+	pCoexDm->btRf0x1eBackup = 
+		pBtCoexist->btc_get_rf_reg(pBtCoexist, BTC_RF_A, 0x1e, 0xfffff);
+
+	// antenna sw ctrl to bt
+	pBtCoexist->btc_write_1byte(pBtCoexist, 0x4f, 0x6);
+	pBtCoexist->btc_write_1byte(pBtCoexist, 0x944, 0x24);
+	pBtCoexist->btc_write_4byte(pBtCoexist, 0x930, 0x700700);
+	pBtCoexist->btc_write_1byte(pBtCoexist, 0x92c, 0x20);
+	if(pBtCoexist->chipInterface == BTC_INTF_USB)
+		pBtCoexist->btc_write_4byte(pBtCoexist, 0x64, 0x30430004);
+	else
+		pBtCoexist->btc_write_4byte(pBtCoexist, 0x64, 0x30030004);
+
+	halbtc8192e1ant_CoexTableWithType(pBtCoexist, FORCE_EXEC, 0);
+
+	// antenna switch control parameter
+	pBtCoexist->btc_write_4byte(pBtCoexist, 0x858, 0x55555555);
+
+	// coex parameters
+	pBtCoexist->btc_write_1byte(pBtCoexist, 0x778, 0x1);
+	// 0x790[5:0]=0x5
+	u1Tmp = pBtCoexist->btc_read_1byte(pBtCoexist, 0x790);
+	u1Tmp &= 0xc0;
+	u1Tmp |= 0x5;
+	pBtCoexist->btc_write_1byte(pBtCoexist, 0x790, u1Tmp);
+
+	// enable counter statistics
+	pBtCoexist->btc_write_1byte(pBtCoexist, 0x76e, 0x4);
+
+	// enable PTA
+	pBtCoexist->btc_write_1byte(pBtCoexist, 0x40, 0x20);
+	// enable mailbox interface
+	u2Tmp = pBtCoexist->btc_read_2byte(pBtCoexist, 0x40);
+	u2Tmp |= BIT9;
+	pBtCoexist->btc_write_2byte(pBtCoexist, 0x40, u2Tmp);
+
+	// enable PTA I2C mailbox 
+	u1Tmp = pBtCoexist->btc_read_1byte(pBtCoexist, 0x101);
+	u1Tmp |= BIT4;
+	pBtCoexist->btc_write_1byte(pBtCoexist, 0x101, u1Tmp);
+
+	// enable bt clock when wifi is disabled.
+	u1Tmp = pBtCoexist->btc_read_1byte(pBtCoexist, 0x93);
+	u1Tmp |= BIT0;
+	pBtCoexist->btc_write_1byte(pBtCoexist, 0x93, u1Tmp);
+	// enable bt clock when suspend.
+	u1Tmp = pBtCoexist->btc_read_1byte(pBtCoexist, 0x7);
+	u1Tmp |= BIT0;
+	pBtCoexist->btc_write_1byte(pBtCoexist, 0x7, u1Tmp);
+}
+
+VOID
+EXhalbtc8192e1ant_InitCoexDm(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], Coex Mechanism Init!!\n"));
+
+	pBtCoexist->bStopCoexDm = FALSE;
+	
+	halbtc8192e1ant_InitCoexDm(pBtCoexist);
+}
+
+VOID
+EXhalbtc8192e1ant_DisplayCoexInfo(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	struct btc_board_info *		pBoardInfo=&pBtCoexist->board_info;
+	PBTC_STACK_INFO		pStackInfo=&pBtCoexist->stack_info;
+	PBTC_BT_LINK_INFO	pBtLinkInfo=&pBtCoexist->bt_link_info;
+	pu1Byte				cliBuf=pBtCoexist->cli_buf;
+	u1Byte				u1Tmp[4], i, btInfoExt, psTdmaCase=0;
+	u4Byte				u4Tmp[4];
+	BOOLEAN				bRoam=FALSE, bScan=FALSE, bLink=FALSE, bWifiUnder5G=FALSE;
+	BOOLEAN				bBtHsOn=FALSE, bWifiBusy=FALSE;
+	s4Byte				wifiRssi=0, btHsRssi=0;
+	u4Byte				wifiBw, wifiTrafficDir;
+	u1Byte				wifiDot11Chnl, wifiHsChnl;
+	u4Byte				fwVer=0, btPatchVer=0;
+
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n ============[BT Coexist info]============");
+	CL_PRINTF(cliBuf);
+
+	if(pBtCoexist->manual_control)
+	{
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n ============[Under Manual Control]============");
+		CL_PRINTF(cliBuf);
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n ==========================================");
+		CL_PRINTF(cliBuf);
+	}	
+	if(pBtCoexist->bStopCoexDm)
+	{
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n ============[Coex is STOPPED]============");
+		CL_PRINTF(cliBuf);
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n ==========================================");
+		CL_PRINTF(cliBuf);
+	}
+
+	if(!pBoardInfo->bt_exist)
+	{
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n BT not exists !!!");
+		CL_PRINTF(cliBuf);
+		return;
+	}
+
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ", "Ant PG number/ Ant mechanism:", \
+		pBoardInfo->pg_ant_num, pBoardInfo->btdm_ant_num);
+	CL_PRINTF(cliBuf);	
+	
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %d", "BT stack/ hci ext ver", \
+		((pStackInfo->bProfileNotified)? "Yes":"No"), pStackInfo->hciVersion);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_BT_PATCH_VER, &btPatchVer);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_FW_VER, &fwVer);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d_%d/ 0x%x/ 0x%x(%d)", "CoexVer/ FwVer/ PatchVer", \
+		GLCoexVerDate8192e1Ant, GLCoexVer8192e1Ant, fwVer, btPatchVer, btPatchVer);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U1_WIFI_DOT11_CHNL, &wifiDot11Chnl);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifiHsChnl);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d(%d)", "Dot11 channel / HsChnl(HsMode)", \
+		wifiDot11Chnl, wifiHsChnl, bBtHsOn);
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x ", "H2C Wifi inform bt chnl Info", \
+		pCoexDm->wifiChnlInfo[0], pCoexDm->wifiChnlInfo[1],
+		pCoexDm->wifiChnlInfo[2]);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_S4_WIFI_RSSI, &wifiRssi);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_S4_HS_RSSI, &btHsRssi);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d", "Wifi rssi/ HS rssi", \
+		wifiRssi, btHsRssi);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_SCAN, &bScan);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_LINK, &bLink);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_ROAM, &bRoam);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ", "Wifi bLink/ bRoam/ bScan", \
+		bLink, bRoam, bScan);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_UNDER_5G, &bWifiUnder5G);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION, &wifiTrafficDir);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %s/ %s ", "Wifi status", \
+		(bWifiUnder5G? "5G":"2.4G"),
+		((BTC_WIFI_BW_LEGACY==wifiBw)? "Legacy": (((BTC_WIFI_BW_HT40==wifiBw)? "HT40":"HT20"))),
+		((!bWifiBusy)? "idle": ((BTC_WIFI_TRAFFIC_TX==wifiTrafficDir)? "uplink":"downlink")));
+	CL_PRINTF(cliBuf);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = [%s/ %d/ %d] ", "BT [status/ rssi/ retryCnt]", \
+		((pBtCoexist->btInfo.bBtDisabled)? ("disabled"):	((pCoexSta->bC2hBtInquiryPage)?("inquiry/page scan"):((BT_8192E_1ANT_BT_STATUS_NON_CONNECTED_IDLE == pCoexDm->btStatus)? "non-connected idle":
+		(  (BT_8192E_1ANT_BT_STATUS_CONNECTED_IDLE == pCoexDm->btStatus)? "connected-idle":"busy")))),
+		pCoexSta->btRssi, pCoexSta->btRetryCnt);
+	CL_PRINTF(cliBuf);
+	
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d / %d / %d", "SCO/HID/PAN/A2DP", \
+		pBtLinkInfo->bScoExist, pBtLinkInfo->bHidExist, pBtLinkInfo->bPanExist, pBtLinkInfo->bA2dpExist);
+	CL_PRINTF(cliBuf);
+	pBtCoexist->btc_disp_dbg_msg(pBtCoexist, BTC_DBG_DISP_BT_LINK_INFO);	
+
+	btInfoExt = pCoexSta->btInfoExt;
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s", "BT Info A2DP rate", \
+		(btInfoExt&BIT0)? "Basic rate":"EDR rate");
+	CL_PRINTF(cliBuf);	
+
+	for(i=0; i<BT_INFO_SRC_8192E_1ANT_MAX; i++)
+	{
+		if(pCoexSta->btInfoC2hCnt[i])
+		{				
+			CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x %02x %02x %02x %02x(%d)", GLBtInfoSrc8192e1Ant[i], \
+				pCoexSta->btInfoC2h[i][0], pCoexSta->btInfoC2h[i][1],
+				pCoexSta->btInfoC2h[i][2], pCoexSta->btInfoC2h[i][3],
+				pCoexSta->btInfoC2h[i][4], pCoexSta->btInfoC2h[i][5],
+				pCoexSta->btInfoC2h[i][6], pCoexSta->btInfoC2hCnt[i]);
+			CL_PRINTF(cliBuf);
+		}
+	}
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s/%s, (0x%x/0x%x)", "PS state, IPS/LPS, (lps/rpwm)", \
+		((pCoexSta->bUnderIps? "IPS ON":"IPS OFF")),
+		((pCoexSta->bUnderLps? "LPS ON":"LPS OFF")), 
+		pBtCoexist->btInfo.lps1Ant, 
+		pBtCoexist->btInfo.rpwm_1ant);
+	CL_PRINTF(cliBuf);
+	pBtCoexist->btc_disp_dbg_msg(pBtCoexist, BTC_DBG_DISP_FW_PWR_MODE_CMD);
+
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x ", "SS Type", \
+		pCoexDm->curSsType);
+	CL_PRINTF(cliBuf);
+
+	if(!pBtCoexist->manual_control)
+	{
+		// Sw mechanism	
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Sw mechanism]============");
+		CL_PRINTF(cliBuf);
+	
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d/ %d ", "SM1[ShRf/ LpRA/ LimDig/ btLna]", \
+			pCoexDm->bCurRfRxLpfShrink, pCoexDm->bCurLowPenaltyRa, pCoexDm->limited_dig, pCoexDm->bCurBtLnaConstrain);
+		CL_PRINTF(cliBuf);
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d(0x%x) ", "SM2[AgcT/ AdcB/ SwDacSwing(lvl)]", \
+			pCoexDm->bCurAgcTableEn, pCoexDm->bCurAdcBackOff, pCoexDm->bCurDacSwingOn, pCoexDm->curDacSwingLvl);
+		CL_PRINTF(cliBuf);
+
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s/ %s/ %d ", "DelBA/ BtCtrlAgg/ AggSize", \
+			(pBtCoexist->btInfo.reject_agg_pkt? "Yes":"No"), (pBtCoexist->btInfo.b_bt_ctrl_agg_buf_size? "Yes":"No"),
+				pBtCoexist->btInfo.aggBufSize);
+		CL_PRINTF(cliBuf);
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x ", "Rate Mask", \
+				pBtCoexist->btInfo.ra_mask);
+		CL_PRINTF(cliBuf);
+	
+		// Fw mechanism		
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Fw mechanism]============");
+		CL_PRINTF(cliBuf);	
+
+		psTdmaCase = pCoexDm->curPsTdma;
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x %02x %02x case-%d (auto:%d)", "PS TDMA", \
+			pCoexDm->psTdmaPara[0], pCoexDm->psTdmaPara[1],
+			pCoexDm->psTdmaPara[2], pCoexDm->psTdmaPara[3],
+			pCoexDm->psTdmaPara[4], psTdmaCase, pCoexDm->bAutoTdmaAdjust);
+		CL_PRINTF(cliBuf);
+
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x ", "Latest error condition(should be 0)", \
+			pCoexDm->errorCondition);
+		CL_PRINTF(cliBuf);
+		
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ", "DecBtPwrLvl/ IgnWlanAct", \
+			pCoexDm->curBtDecPwrLvl, pCoexDm->bCurIgnoreWlanAct);
+		CL_PRINTF(cliBuf);
+	}
+
+	// Hw setting		
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Hw setting]============");
+	CL_PRINTF(cliBuf);	
+
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "RF-A, 0x1e initVal", \
+		pCoexDm->btRf0x1eBackup);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0xc04);
+	u4Tmp[1] = pBtCoexist->btc_read_4byte(pBtCoexist, 0xd04);
+	u4Tmp[2] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x90c);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x", "0xc04/ 0xd04/ 0x90c", \
+		u4Tmp[0], u4Tmp[1], u4Tmp[2]);
+	CL_PRINTF(cliBuf);
+
+	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x778);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x778", \
+		u1Tmp[0]);
+	CL_PRINTF(cliBuf);
+	
+	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x92c);
+	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x930);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x", "0x92c/ 0x930", \
+		(u1Tmp[0]), u4Tmp[0]);
+	CL_PRINTF(cliBuf);
+
+	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x40);
+	u1Tmp[1] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x4f);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x", "0x40/ 0x4f", \
+		u1Tmp[0], u1Tmp[1]);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x550);
+	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x522);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x", "0x550(bcn ctrl)/0x522", \
+		u4Tmp[0], u1Tmp[0]);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0xc50);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0xc50(dig)", \
+		u4Tmp[0]);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x6c0);
+	u4Tmp[1] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x6c4);
+	u4Tmp[2] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x6c8);
+	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x6cc);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x", "0x6c0/0x6c4/0x6c8/0x6cc(coexTable)", \
+		u4Tmp[0], u4Tmp[1], u4Tmp[2], u1Tmp[0]);
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d", "0x770(hp rx[31:16]/tx[15:0])", \
+		pCoexSta->highPriorityRx, pCoexSta->highPriorityTx);
+	CL_PRINTF(cliBuf);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d", "0x774(lp rx[31:16]/tx[15:0])", \
+		pCoexSta->lowPriorityRx, pCoexSta->lowPriorityTx);
+	CL_PRINTF(cliBuf);
+#if(BT_AUTO_REPORT_ONLY_8192E_1ANT == 1)
+	halbtc8192e1ant_MonitorBtCtr(pBtCoexist);
+#endif
+	
+	pBtCoexist->btc_disp_dbg_msg(pBtCoexist, BTC_DBG_DISP_COEX_STATISTICS);
+}
+
+
+VOID
+EXhalbtc8192e1ant_IpsNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			type
+	)
+{
+	u4Byte	u4Tmp=0;
+
+	if(pBtCoexist->manual_control ||	pBtCoexist->bStopCoexDm)
+		return;
+
+	if(BTC_IPS_ENTER == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], IPS ENTER notify\n"));
+		pCoexSta->bUnderIps = true;
+		halbtc8192e1ant_CoexAllOff(pBtCoexist);
+	}
+	else if(BTC_IPS_LEAVE == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], IPS LEAVE notify\n"));
+		pCoexSta->bUnderIps = FALSE;
+	}
+}
+
+VOID
+EXhalbtc8192e1ant_LpsNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			type
+	)
+{
+	if(pBtCoexist->manual_control || pBtCoexist->bStopCoexDm)
+		return;
+
+	if(BTC_LPS_ENABLE == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], LPS ENABLE notify\n"));
+		pCoexSta->bUnderLps = true;
+	}
+	else if(BTC_LPS_DISABLE == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], LPS DISABLE notify\n"));
+		pCoexSta->bUnderLps = FALSE;
+	}
+}
+
+VOID
+EXhalbtc8192e1ant_ScanNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			type
+	)
+{
+	BOOLEAN 		bWifiConnected=FALSE, bBtHsOn=FALSE;	
+
+	if(pBtCoexist->manual_control ||
+		pBtCoexist->bStopCoexDm ||
+		pBtCoexist->btInfo.bBtDisabled )
+		return;
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	if(pCoexSta->bC2hBtInquiryPage)
+	{
+		halbtc8192e1ant_ActionBtInquiry(pBtCoexist);
+		return;
+	}
+	else if(bBtHsOn)
+	{
+		halbtc8192e1ant_ActionHs(pBtCoexist);
+		return;
+	}
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+	if(BTC_SCAN_START == type)
+	{	
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], SCAN START notify\n"));
+		if(!bWifiConnected)	// non-connected scan
+		{
+			halbtc8192e1ant_ActionWifiNotConnectedAssoAuthScan(pBtCoexist);
+		}
+		else	// wifi is connected
+		{
+			halbtc8192e1ant_ActionWifiConnectedScan(pBtCoexist);
+		}
+	}
+	else if(BTC_SCAN_FINISH == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], SCAN FINISH notify\n"));
+		if(!bWifiConnected)	// non-connected scan
+		{
+			halbtc8192e1ant_ActionWifiNotConnected(pBtCoexist);
+		}
+		else
+		{
+			halbtc8192e1ant_ActionWifiConnected(pBtCoexist);
+		}
+	}
+}
+
+VOID
+EXhalbtc8192e1ant_ConnectNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			type
+	)
+{
+	BOOLEAN	bWifiConnected=FALSE, bBtHsOn=FALSE;
+
+	if(pBtCoexist->manual_control ||
+		pBtCoexist->bStopCoexDm ||
+		pBtCoexist->btInfo.bBtDisabled )
+		return;
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	if(pCoexSta->bC2hBtInquiryPage)
+	{
+		halbtc8192e1ant_ActionBtInquiry(pBtCoexist);
+		return;
+	}
+	else if(bBtHsOn)
+	{
+		halbtc8192e1ant_ActionHs(pBtCoexist);
+		return;
+	}
+
+	if(BTC_ASSOCIATE_START == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], CONNECT START notify\n"));
+		halbtc8192e1ant_ActionWifiNotConnectedAssoAuthScan(pBtCoexist);
+	}
+	else if(BTC_ASSOCIATE_FINISH == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], CONNECT FINISH notify\n"));
+		
+		pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+		if(!bWifiConnected) // non-connected scan
+		{
+			halbtc8192e1ant_ActionWifiNotConnected(pBtCoexist);
+		}
+		else
+		{
+			halbtc8192e1ant_ActionWifiConnected(pBtCoexist);
+		}
+	}
+}
+
+VOID
+EXhalbtc8192e1ant_MediaStatusNotify(
+	IN	PBTC_COEXIST			pBtCoexist,
+	IN	u1Byte				type
+	)
+{
+	u1Byte			H2C_Parameter[3] ={0};
+	u4Byte			wifiBw;
+	u1Byte			wifiCentralChnl;
+
+	if(pBtCoexist->manual_control ||
+		pBtCoexist->bStopCoexDm ||
+		pBtCoexist->btInfo.bBtDisabled )
+		return;
+
+	if(BTC_MEDIA_CONNECT == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], MEDIA connect notify\n"));
+	}
+	else
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], MEDIA disconnect notify\n"));
+	}
+
+	// only 2.4G we need to inform bt the chnl mask
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U1_WIFI_CENTRAL_CHNL, &wifiCentralChnl);
+	if( (BTC_MEDIA_CONNECT == type) &&
+		(wifiCentralChnl <= 14) )
+	{
+		H2C_Parameter[0] = 0x1;
+		H2C_Parameter[1] = wifiCentralChnl;
+		pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+		if(BTC_WIFI_BW_HT40 == wifiBw)
+			H2C_Parameter[2] = 0x30;
+		else
+			H2C_Parameter[2] = 0x20;
+	}
+		
+	pCoexDm->wifiChnlInfo[0] = H2C_Parameter[0];
+	pCoexDm->wifiChnlInfo[1] = H2C_Parameter[1];
+	pCoexDm->wifiChnlInfo[2] = H2C_Parameter[2];
+	
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], FW write 0x66=0x%x\n", 
+		H2C_Parameter[0]<<16|H2C_Parameter[1]<<8|H2C_Parameter[2]));
+
+	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x66, 3, H2C_Parameter);
+}
+
+VOID
+EXhalbtc8192e1ant_SpecialPacketNotify(
+	IN	PBTC_COEXIST			pBtCoexist,
+	IN	u1Byte				type
+	)
+{
+	BOOLEAN	bBtHsOn=FALSE;
+
+	if(pBtCoexist->manual_control ||
+		pBtCoexist->bStopCoexDm ||
+		pBtCoexist->btInfo.bBtDisabled )
+		return;
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	if(pCoexSta->bC2hBtInquiryPage)
+	{
+		halbtc8192e1ant_ActionBtInquiry(pBtCoexist);
+		return;
+	}
+	else if(bBtHsOn)
+	{
+		halbtc8192e1ant_ActionHs(pBtCoexist);
+		return;
+	}
+
+	if( BTC_PACKET_DHCP == type ||
+		BTC_PACKET_EAPOL == type )
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], special Packet(%d) notify\n", type));
+		halbtc8192e1ant_ActionWifiConnectedSpecialPacket(pBtCoexist);
+	}
+}
+
+VOID
+EXhalbtc8192e1ant_BtInfoNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	pu1Byte			tmpBuf,
+	IN	u1Byte			length
+	)
+{
+	PBTC_BT_LINK_INFO	pBtLinkInfo=&pBtCoexist->bt_link_info;
+	u1Byte				btInfo=0;
+	u1Byte				i, rspSource=0;
+	static u4Byte		setBtPsdMode=0;
+	BOOLEAN				bBtBusy=FALSE, limited_dig=FALSE;
+	BOOLEAN				bWifiConnected=FALSE;
+	BOOLEAN				b_bt_ctrl_agg_buf_size=FALSE;
+
+	pCoexSta->bC2hBtInfoReqSent = FALSE;
+
+	rspSource = tmpBuf[0]&0xf;
+	if(rspSource >= BT_INFO_SRC_8192E_1ANT_MAX)
+		rspSource = BT_INFO_SRC_8192E_1ANT_WIFI_FW;
+	pCoexSta->btInfoC2hCnt[rspSource]++;
+
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Bt info[%d], length=%d, hex data=[", rspSource, length));
+	for(i=0; i<length; i++)
+	{
+		pCoexSta->btInfoC2h[rspSource][i] = tmpBuf[i];
+		if(i == 1)
+			btInfo = tmpBuf[i];
+		if(i == length-1)
+		{
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("0x%02x]\n", tmpBuf[i]));
+		}
+		else
+		{
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("0x%02x, ", tmpBuf[i]));
+		}
+	}
+
+	if(pBtCoexist->btInfo.bBtDisabled)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), return for BT is disabled <===\n"));
+		return;
+	}	
+
+	if(pBtCoexist->manual_control)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), return for Manual CTRL<===\n"));
+		return;
+	}
+	if(pBtCoexist->bStopCoexDm)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), return for Coex STOPPED!!<===\n"));
+		return;
+	}
+
+	if(BT_INFO_SRC_8192E_1ANT_WIFI_FW != rspSource)
+	{
+		pCoexSta->btRetryCnt =	// [3:0]
+			pCoexSta->btInfoC2h[rspSource][2]&0xf;
+
+		pCoexSta->btRssi =
+			pCoexSta->btInfoC2h[rspSource][3]*2+10;
+
+		pCoexSta->btInfoExt = 
+			pCoexSta->btInfoC2h[rspSource][4];
+
+		// Here we need to resend some wifi info to BT
+		// because bt is reset and loss of the info.
+		if( (pCoexSta->btInfoExt & BIT1) )
+		{			
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BT ext info bit1 check, send wifi BW&Chnl to BT!!\n"));
+			pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+			if(bWifiConnected)
+			{
+				EXhalbtc8192e1ant_MediaStatusNotify(pBtCoexist, BTC_MEDIA_CONNECT);
+			}
+			else
+			{
+				EXhalbtc8192e1ant_MediaStatusNotify(pBtCoexist, BTC_MEDIA_DISCONNECT);
+			}
+
+			setBtPsdMode = 0;
+		}
+
+		// test-chip bt patch only rsp the status for BT_RSP, 
+		// so temporary we consider the following only under BT_RSP
+		if(BT_INFO_SRC_8192E_1ANT_BT_RSP == rspSource)
+		{
+			if( (pCoexSta->btInfoExt & BIT3) )
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BT ext info bit3 check, set BT NOT to ignore Wlan active!!\n"));
+				halbtc8192e1ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, FALSE);
+			}
+			else
+			{
+				// BT already NOT ignore Wlan active, do nothing here.
+			}
+#if(BT_AUTO_REPORT_ONLY_8192E_1ANT == 0)
+			if( (pCoexSta->btInfoExt & BIT4) )
+			{
+				// BT auto report already enabled, do nothing
+			}
+			else
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BT ext info bit4 check, set BT to enable Auto Report!!\n"));
+				halbtc8192e1ant_BtAutoReport(pBtCoexist, FORCE_EXEC, true);
+			}
+#endif
+		}
+	}
+		
+	// check BIT2 first ==> check if bt is under inquiry or page scan
+	if(btInfo & BT_INFO_8192E_1ANT_B_INQ_PAGE)
+		pCoexSta->bC2hBtInquiryPage = true;
+	else
+		pCoexSta->bC2hBtInquiryPage = FALSE;
+
+	// set link exist status
+	if(!(btInfo&BT_INFO_8192E_1ANT_B_CONNECTION))
+	{
+		pCoexSta->bBtLinkExist = FALSE;
+		pCoexSta->bPanExist = FALSE;
+		pCoexSta->bA2dpExist = FALSE;
+		pCoexSta->bHidExist = FALSE;
+		pCoexSta->bScoExist = FALSE;
+	}
+	else // connection exists 
+	{
+		pCoexSta->bBtLinkExist = true;
+		if(btInfo & BT_INFO_8192E_1ANT_B_FTP)
+			pCoexSta->bPanExist = true;
+		else
+			pCoexSta->bPanExist = FALSE;
+		if(btInfo & BT_INFO_8192E_1ANT_B_A2DP)
+			pCoexSta->bA2dpExist = true;
+		else
+			pCoexSta->bA2dpExist = FALSE;
+		if(btInfo & BT_INFO_8192E_1ANT_B_HID)
+			pCoexSta->bHidExist = true;
+		else
+			pCoexSta->bHidExist = FALSE;
+		if(btInfo & BT_INFO_8192E_1ANT_B_SCO_ESCO)
+			pCoexSta->bScoExist = true;
+		else
+			pCoexSta->bScoExist = FALSE;
+	}
+
+	halbtc8192e1ant_UpdateBtLinkInfo(pBtCoexist);
+
+	if(!(btInfo&BT_INFO_8192E_1ANT_B_CONNECTION))
+	{
+		pCoexDm->btStatus = BT_8192E_1ANT_BT_STATUS_NON_CONNECTED_IDLE;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), bt non-connected idle!!!\n"));
+	}
+	else if(btInfo == BT_INFO_8192E_1ANT_B_CONNECTION)	// connection exists but no busy
+	{
+		pCoexDm->btStatus = BT_8192E_1ANT_BT_STATUS_CONNECTED_IDLE;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), bt connected-idle!!!\n"));
+	}		
+	else if((btInfo&BT_INFO_8192E_1ANT_B_SCO_ESCO) ||
+		(btInfo&BT_INFO_8192E_1ANT_B_SCO_BUSY))
+	{
+		pCoexDm->btStatus = BT_8192E_1ANT_BT_STATUS_SCO_BUSY;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), bt sco busy!!!\n"));
+	}
+	else if( (btInfo&BT_INFO_8192E_1ANT_B_ACL_BUSY) ||
+			(btInfo&BT_INFO_8192E_1ANT_B_A2DP) ||
+			(btInfo&BT_INFO_8192E_1ANT_B_FTP) )
+	{
+		if(BT_8192E_1ANT_BT_STATUS_ACL_BUSY != pCoexDm->btStatus)
+			pCoexDm->bAutoTdmaAdjust = FALSE;
+		pCoexDm->btStatus = BT_8192E_1ANT_BT_STATUS_ACL_BUSY;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), bt acl busy!!!\n"));
+	}
+	else
+	{
+		pCoexDm->btStatus = BT_8192E_1ANT_BT_STATUS_MAX;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), bt non-defined state!!!\n"));
+	}
+
+	// ra mask check
+	if(pBtLinkInfo->bScoExist || pBtLinkInfo->bHidExist)
+	{
+		halbtc8192e1ant_Updatera_mask(pBtCoexist, NORMAL_EXEC, BTC_RATE_DISABLE, 0x00000003);	// disable tx cck 1M/2M
+	}
+	else
+	{
+		halbtc8192e1ant_Updatera_mask(pBtCoexist, NORMAL_EXEC, BTC_RATE_ENABLE, 0x00000003);	// enable tx cck 1M/2M
+	}
+	
+	if( (BT_8192E_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus) ||
+		(BT_8192E_1ANT_BT_STATUS_SCO_BUSY == pCoexDm->btStatus) ||
+		(BT_8192E_1ANT_BT_STATUS_ACL_SCO_BUSY == pCoexDm->btStatus) )
+	{
+		bBtBusy = true;
+		limited_dig = true;
+		if(pBtLinkInfo->bHidExist)
+			b_bt_ctrl_agg_buf_size = true;
+	}
+	else
+	{
+		bBtBusy = FALSE;
+		limited_dig = FALSE;
+	}
+	pBtCoexist->btc_set(pBtCoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bBtBusy);
+
+	//============================================
+	//	Aggregation related setting
+	//============================================
+	// if sco, reject AddBA
+	//pBtCoexist->btc_set(pBtCoexist, BTC_SET_BL_TO_REJ_AP_AGG_PKT, &bRejApAggPkt);
+
+	// decide BT control aggregation buf size or not
+	pBtCoexist->btc_set(pBtCoexist, BTC_SET_BL_BT_CTRL_AGG_SIZE, &b_bt_ctrl_agg_buf_size);
+	// real update aggregation setting
+	pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_AGGREGATE_CTRL, NULL);
+	//============================================
+
+	pCoexDm->limited_dig = limited_dig;
+	pBtCoexist->btc_set(pBtCoexist, BTC_SET_BL_BT_LIMITED_DIG, &limited_dig);
+
+	halbtc8192e1ant_RunCoexistMechanism(pBtCoexist);
+}
+
+VOID
+EXhalbtc8192e1ant_StackOperationNotify(
+	IN	PBTC_COEXIST			pBtCoexist,
+	IN	u1Byte				type
+	)
+{
+	if(BTC_STACK_OP_INQ_PAGE_PAIR_START == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], StackOP Inquiry/page/pair start notify\n"));
+	}
+	else if(BTC_STACK_OP_INQ_PAGE_PAIR_FINISH == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], StackOP Inquiry/page/pair finish notify\n"));
+	}
+}
+
+VOID
+EXhalbtc8192e1ant_HaltNotify(
+	IN	PBTC_COEXIST			pBtCoexist
+	)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Halt notify\n"));
+
+	pBtCoexist->bStopCoexDm = true;
+	halbtc8192e1ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, true);
+
+	halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+	
+	halbtc8192e1ant_PsTdma(pBtCoexist, FORCE_EXEC, FALSE, 0);
+	pBtCoexist->btc_write_1byte(pBtCoexist, 0x4f, 0xf);
+
+	EXhalbtc8192e1ant_MediaStatusNotify(pBtCoexist, BTC_MEDIA_DISCONNECT);
+}
+
+VOID
+EXhalbtc8192e1ant_PnpNotify(
+	IN	PBTC_COEXIST			pBtCoexist,
+	IN	u1Byte				pnpState
+	)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Pnp notify\n"));
+
+	if(BTC_WIFI_PNP_SLEEP == pnpState)
+	{
+		pBtCoexist->bStopCoexDm = true;
+		halbtc8192e1ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, true);
+		halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+		halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 9);	
+	}
+	else if(BTC_WIFI_PNP_WAKE_UP == pnpState)
+	{
+		
+	}
+}
+
+VOID
+EXhalbtc8192e1ant_Periodical(
+	IN	PBTC_COEXIST			pBtCoexist
+	)
+{
+	static u1Byte		disVerInfoCnt=0;
+	u4Byte				fwVer=0, btPatchVer=0;
+	struct btc_board_info *		pBoardInfo=&pBtCoexist->board_info;
+	PBTC_STACK_INFO		pStackInfo=&pBtCoexist->stack_info;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], ==========================Periodical===========================\n"));
+
+	if(disVerInfoCnt <= 5)
+	{
+		disVerInfoCnt += 1;
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], ****************************************************************\n"));
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], Ant PG Num/ Ant Mech/ Ant Pos = %d/ %d/ %d\n", \
+			pBoardInfo->pg_ant_num, pBoardInfo->btdm_ant_num, pBoardInfo->btdm_ant_pos));
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], BT stack/ hci ext ver = %s / %d\n", \
+			((pStackInfo->bProfileNotified)? "Yes":"No"), pStackInfo->hciVersion));
+		pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_BT_PATCH_VER, &btPatchVer);
+		pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_FW_VER, &fwVer);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], CoexVer/ FwVer/ PatchVer = %d_%x/ 0x%x/ 0x%x(%d)\n", \
+			GLCoexVerDate8192e1Ant, GLCoexVer8192e1Ant, fwVer, btPatchVer, btPatchVer));
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], ****************************************************************\n"));
+	}
+#if(BT_AUTO_REPORT_ONLY_8192E_1ANT == 0)
+	halbtc8192e1ant_QueryBtInfo(pBtCoexist);
+	halbtc8192e1ant_MonitorBtCtr(pBtCoexist);
+	halbtc8192e1ant_MonitorBtEnableDisable(pBtCoexist);
+#else
+	if( halbtc8192e1ant_IsWifiStatusChanged(pBtCoexist) ||
+		pCoexDm->bAutoTdmaAdjust)
+	{
+		halbtc8192e1ant_RunCoexistMechanism(pBtCoexist);
+	}
+#endif
+}
+
+VOID
+EXhalbtc8192e1ant_DbgControl(
+	IN	PBTC_COEXIST			pBtCoexist,
+	IN	u1Byte				opCode,
+	IN	u1Byte				opLen,
+	IN	pu1Byte				pData
+	)
+{
+	switch(opCode)
+	{
+		case BTC_DBG_SET_COEX_NORMAL:
+			pBtCoexist->manual_control = FALSE;
+			halbtc8192e1ant_InitCoexDm(pBtCoexist);
+			break;
+		case BTC_DBG_SET_COEX_WIFI_ONLY:
+			pBtCoexist->manual_control = true;
+			halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+			halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 9);	
+			break;
+		case BTC_DBG_SET_COEX_BT_ONLY:
+			// todo
+			break;
+		default:
+			break;
+	}
+}
+#endif
+
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtc8192e1ant.h b/drivers/staging/rtl8821ae/btcoexist/halbtc8192e1ant.h
new file mode 100644
index 000000000000..a759b758faef
--- /dev/null
+++ b/drivers/staging/rtl8821ae/btcoexist/halbtc8192e1ant.h
@@ -0,0 +1,226 @@
+//===========================================
+// The following is for 8192E_1ANT BT Co-exist definition
+//===========================================
+#define	BT_AUTO_REPORT_ONLY_8192E_1ANT				0
+
+#define	BT_INFO_8192E_1ANT_B_FTP						BIT7
+#define	BT_INFO_8192E_1ANT_B_A2DP					BIT6
+#define	BT_INFO_8192E_1ANT_B_HID						BIT5
+#define	BT_INFO_8192E_1ANT_B_SCO_BUSY				BIT4
+#define	BT_INFO_8192E_1ANT_B_ACL_BUSY				BIT3
+#define	BT_INFO_8192E_1ANT_B_INQ_PAGE				BIT2
+#define	BT_INFO_8192E_1ANT_B_SCO_ESCO				BIT1
+#define	BT_INFO_8192E_1ANT_B_CONNECTION				BIT0
+
+#define	BT_INFO_8192E_1ANT_A2DP_BASIC_RATE(_BT_INFO_EXT_)	\
+		(((_BT_INFO_EXT_&BIT0))? true:FALSE)
+
+#define	BTC_RSSI_COEX_THRESH_TOL_8192E_1ANT		2
+
+typedef enum _BT_INFO_SRC_8192E_1ANT{
+	BT_INFO_SRC_8192E_1ANT_WIFI_FW			= 0x0,
+	BT_INFO_SRC_8192E_1ANT_BT_RSP				= 0x1,
+	BT_INFO_SRC_8192E_1ANT_BT_ACTIVE_SEND		= 0x2,
+	BT_INFO_SRC_8192E_1ANT_MAX
+}BT_INFO_SRC_8192E_1ANT,*PBT_INFO_SRC_8192E_1ANT;
+
+typedef enum _BT_8192E_1ANT_BT_STATUS{
+	BT_8192E_1ANT_BT_STATUS_NON_CONNECTED_IDLE	= 0x0,
+	BT_8192E_1ANT_BT_STATUS_CONNECTED_IDLE		= 0x1,
+	BT_8192E_1ANT_BT_STATUS_INQ_PAGE				= 0x2,
+	BT_8192E_1ANT_BT_STATUS_ACL_BUSY				= 0x3,
+	BT_8192E_1ANT_BT_STATUS_SCO_BUSY				= 0x4,
+	BT_8192E_1ANT_BT_STATUS_ACL_SCO_BUSY			= 0x5,
+	BT_8192E_1ANT_BT_STATUS_MAX
+}BT_8192E_1ANT_BT_STATUS,*PBT_8192E_1ANT_BT_STATUS;
+
+typedef enum _BT_8192E_1ANT_WIFI_STATUS{
+	BT_8192E_1ANT_WIFI_STATUS_NON_CONNECTED_IDLE				= 0x0,
+	BT_8192E_1ANT_WIFI_STATUS_NON_CONNECTED_ASSO_AUTH_SCAN		= 0x1,
+	BT_8192E_1ANT_WIFI_STATUS_CONNECTED_SCAN					= 0x2,
+	BT_8192E_1ANT_WIFI_STATUS_CONNECTED_SPECIAL_PKT				= 0x3,
+	BT_8192E_1ANT_WIFI_STATUS_CONNECTED_IDLE					= 0x4,
+	BT_8192E_1ANT_WIFI_STATUS_CONNECTED_BUSY					= 0x5,
+	BT_8192E_1ANT_WIFI_STATUS_MAX
+}BT_8192E_1ANT_WIFI_STATUS,*PBT_8192E_1ANT_WIFI_STATUS;
+
+typedef enum _BT_8192E_1ANT_COEX_ALGO{
+	BT_8192E_1ANT_COEX_ALGO_UNDEFINED			= 0x0,
+	BT_8192E_1ANT_COEX_ALGO_SCO				= 0x1,
+	BT_8192E_1ANT_COEX_ALGO_HID				= 0x2,
+	BT_8192E_1ANT_COEX_ALGO_A2DP				= 0x3,
+	BT_8192E_1ANT_COEX_ALGO_A2DP_PANHS		= 0x4,
+	BT_8192E_1ANT_COEX_ALGO_PANEDR			= 0x5,
+	BT_8192E_1ANT_COEX_ALGO_PANHS			= 0x6,
+	BT_8192E_1ANT_COEX_ALGO_PANEDR_A2DP		= 0x7,
+	BT_8192E_1ANT_COEX_ALGO_PANEDR_HID		= 0x8,
+	BT_8192E_1ANT_COEX_ALGO_HID_A2DP_PANEDR	= 0x9,
+	BT_8192E_1ANT_COEX_ALGO_HID_A2DP			= 0xa,
+	BT_8192E_1ANT_COEX_ALGO_MAX				= 0xb,
+}BT_8192E_1ANT_COEX_ALGO,*PBT_8192E_1ANT_COEX_ALGO;
+
+typedef struct _COEX_DM_8192E_1ANT{
+	// fw mechanism
+	u1Byte		preBtDecPwrLvl;
+	u1Byte		curBtDecPwrLvl;
+	BOOLEAN		bPreBtLnaConstrain;
+	BOOLEAN		bCurBtLnaConstrain;
+	u1Byte		bPreBtPsdMode;
+	u1Byte		bCurBtPsdMode;
+	u1Byte		preFwDacSwingLvl;
+	u1Byte		curFwDacSwingLvl;
+	BOOLEAN		bCurIgnoreWlanAct;
+	BOOLEAN		bPreIgnoreWlanAct;
+	u1Byte		prePsTdma;
+	u1Byte		curPsTdma;
+	u1Byte		psTdmaPara[5];
+	u1Byte		psTdmaDuAdjType;
+	BOOLEAN		bAutoTdmaAdjust;
+	BOOLEAN		bPrePsTdmaOn;
+	BOOLEAN		bCurPsTdmaOn;
+	BOOLEAN		bPreBtAutoReport;
+	BOOLEAN		bCurBtAutoReport;
+	u1Byte		preLps;
+	u1Byte		curLps;
+	u1Byte		preRpwm;
+	u1Byte		curRpwm;
+
+	// sw mechanism
+	BOOLEAN		bPreRfRxLpfShrink;
+	BOOLEAN		bCurRfRxLpfShrink;
+	u4Byte		btRf0x1eBackup;
+	BOOLEAN 	bPreLowPenaltyRa;
+	BOOLEAN		bCurLowPenaltyRa;
+	BOOLEAN		bPreDacSwingOn;
+	u4Byte		preDacSwingLvl;
+	BOOLEAN		bCurDacSwingOn;
+	u4Byte		curDacSwingLvl;
+	BOOLEAN		bPreAdcBackOff;
+	BOOLEAN		bCurAdcBackOff;
+	BOOLEAN 	bPreAgcTableEn;
+	BOOLEAN		bCurAgcTableEn;
+	u4Byte		preVal0x6c0;
+	u4Byte		curVal0x6c0;
+	u4Byte		preVal0x6c4;
+	u4Byte		curVal0x6c4;
+	u4Byte		preVal0x6c8;
+	u4Byte		curVal0x6c8;
+	u1Byte		preVal0x6cc;
+	u1Byte		curVal0x6cc;
+	BOOLEAN		limited_dig;
+
+	// algorithm related
+	u1Byte		preAlgorithm;
+	u1Byte		curAlgorithm;
+	u1Byte		btStatus;
+	u1Byte		wifiChnlInfo[3];
+
+	u1Byte		preSsType;
+	u1Byte		curSsType;
+
+	u4Byte		prera_mask;
+	u4Byte		curra_mask;
+
+	u1Byte		errorCondition;
+} COEX_DM_8192E_1ANT, *PCOEX_DM_8192E_1ANT;
+
+typedef struct _COEX_STA_8192E_1ANT{
+	BOOLEAN					bBtLinkExist;
+	BOOLEAN					bScoExist;
+	BOOLEAN					bA2dpExist;
+	BOOLEAN					bHidExist;
+	BOOLEAN					bPanExist;
+
+	BOOLEAN					bUnderLps;
+	BOOLEAN					bUnderIps;
+	u4Byte					highPriorityTx;
+	u4Byte					highPriorityRx;
+	u4Byte					lowPriorityTx;
+	u4Byte					lowPriorityRx;
+	u1Byte					btRssi;
+	u1Byte					preBtRssiState;
+	u1Byte					preWifiRssiState[4];
+	BOOLEAN					bC2hBtInfoReqSent;
+	u1Byte					btInfoC2h[BT_INFO_SRC_8192E_1ANT_MAX][10];
+	u4Byte					btInfoC2hCnt[BT_INFO_SRC_8192E_1ANT_MAX];
+	BOOLEAN					bC2hBtInquiryPage;
+	u1Byte					btRetryCnt;
+	u1Byte					btInfoExt;
+}COEX_STA_8192E_1ANT, *PCOEX_STA_8192E_1ANT;
+
+//===========================================
+// The following is interface which will notify coex module.
+//===========================================
+VOID
+EXhalbtc8192e1ant_InitHwConfig(
+	IN	PBTC_COEXIST		pBtCoexist
+	);
+VOID
+EXhalbtc8192e1ant_InitCoexDm(
+	IN	PBTC_COEXIST		pBtCoexist
+	);
+VOID
+EXhalbtc8192e1ant_IpsNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			type
+	);
+VOID
+EXhalbtc8192e1ant_LpsNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			type
+	);
+VOID
+EXhalbtc8192e1ant_ScanNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			type
+	);
+VOID
+EXhalbtc8192e1ant_ConnectNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			type
+	);
+VOID
+EXhalbtc8192e1ant_MediaStatusNotify(
+	IN	PBTC_COEXIST			pBtCoexist,
+	IN	u1Byte				type
+	);
+VOID
+EXhalbtc8192e1ant_SpecialPacketNotify(
+	IN	PBTC_COEXIST			pBtCoexist,
+	IN	u1Byte				type
+	);
+VOID
+EXhalbtc8192e1ant_BtInfoNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	pu1Byte			tmpBuf,
+	IN	u1Byte			length
+	);
+VOID
+EXhalbtc8192e1ant_StackOperationNotify(
+	IN	PBTC_COEXIST			pBtCoexist,
+	IN	u1Byte				type
+	);
+VOID
+EXhalbtc8192e1ant_HaltNotify(
+	IN	PBTC_COEXIST			pBtCoexist
+	);
+VOID
+EXhalbtc8192e1ant_PnpNotify(
+	IN	PBTC_COEXIST			pBtCoexist,
+	IN	u1Byte				pnpState
+	);
+VOID
+EXhalbtc8192e1ant_Periodical(
+	IN	PBTC_COEXIST			pBtCoexist
+	);
+VOID
+EXhalbtc8192e1ant_DisplayCoexInfo(
+	IN	PBTC_COEXIST		pBtCoexist
+	);
+VOID
+EXhalbtc8192e1ant_DbgControl(
+	IN	PBTC_COEXIST			pBtCoexist,
+	IN	u1Byte				opCode,
+	IN	u1Byte				opLen,
+	IN	pu1Byte 			pData
+	);
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtc8192e2ant.c b/drivers/staging/rtl8821ae/btcoexist/halbtc8192e2ant.c
new file mode 100644
index 000000000000..44ec78562e2d
--- /dev/null
+++ b/drivers/staging/rtl8821ae/btcoexist/halbtc8192e2ant.c
@@ -0,0 +1,4242 @@
+/**************************************************************
+ * Description:
+ *
+ * This file is for RTL8192E Co-exist mechanism
+ *
+ * History
+ * 2012/11/15 Cosa first check in.
+ *
+ **************************************************************/
+
+/**************************************************************
+ *   include files
+ **************************************************************/
+#include "halbt_precomp.h"
+#if 1
+/**************************************************************
+ *   Global variables, these are static variables
+ **************************************************************/
+static struct coex_dm_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;
+
+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;
+
+/**************************************************************
+ *   local function proto type if needed
+ **************************************************************/
+/**************************************************************
+ *   local function start with halbtc8192e2ant_
+ **************************************************************/
+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;
+}
+
+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;
+}
+
+void halbtc8192e2ant_monitor_bt_enable_disable(struct btc_coexist *btcoexist)
+{
+	static bool pre_bt_disabled = false;
+	static u32 bt_disable_cnt = 0;
+	bool bt_active = true, bt_disabled = false;
+
+	/* This function check if bt is disabled */
+
+	if (coex_sta->high_priority_tx == 0 &&
+	    coex_sta->high_priority_rx == 0 &&
+	    coex_sta->low_priority_tx == 0 &&
+	    coex_sta->low_priority_rx == 0)
+		bt_active = false;
+
+	if (coex_sta->high_priority_tx == 0xffff &&
+	    coex_sta->high_priority_rx == 0xffff &&
+	    coex_sta->low_priority_tx == 0xffff &&
+	    coex_sta->low_priority_rx == 0xffff)
+		bt_active = false;
+
+	if (bt_active) {
+		bt_disable_cnt = 0;
+		bt_disabled = false;
+		btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_DISABLE,
+				   &bt_disabled);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+			  "[BTCoex], BT is enabled !!\n");
+	} else {
+		bt_disable_cnt++;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+			  "[BTCoex], bt all counters=0, %d times!!\n",
+			  bt_disable_cnt);
+		if (bt_disable_cnt >= 2) {
+			bt_disabled = true;
+			btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_DISABLE,
+					   &bt_disabled);
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+				  "[BTCoex], BT is disabled !!\n");
+		}
+	}
+	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;
+	}
+}
+
+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;
+}
+
+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;
+}
+
+void halbtc8192e2ant_autorate_fallback_retry(struct btc_coexist *btcoexist,
+					     bool force_exec, u8 type)
+{
+	bool wifi_under_bmode = false;
+
+	coex_dm->cur_arfrtype = type;
+
+	if (force_exec || (coex_dm->pre_arfrtype != coex_dm->cur_arfrtype)) {
+		switch (coex_dm->cur_arfrtype) {
+		case 0:	/* normal mode */
+			btcoexist->btc_write_4byte(btcoexist, 0x430,
+						   coex_dm->backup_arfr_cnt1);
+			btcoexist->btc_write_4byte(btcoexist, 0x434,
+						   coex_dm->backup_arfr_cnt2);
+			break;
+		case 1:
+			btcoexist->btc_get(btcoexist,
+					   BTC_GET_BL_WIFI_UNDER_B_MODE,
+					   &wifi_under_bmode);
+			if (wifi_under_bmode) {
+				btcoexist->btc_write_4byte(btcoexist, 0x430,
+							   0x0);
+				btcoexist->btc_write_4byte(btcoexist, 0x434,
+							   0x01010101);
+			} else {
+				btcoexist->btc_write_4byte(btcoexist, 0x430,
+							   0x0);
+				btcoexist->btc_write_4byte(btcoexist, 0x434,
+							   0x04030201);
+			}
+			break;
+		default:
+			break;
+		}
+	}
+
+	coex_dm->pre_arfrtype = coex_dm->cur_arfrtype;
+}
+
+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;
+}
+
+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;
+}
+
+void halbtc8192e2ant_limited_tx(struct btc_coexist *btcoexist,
+				bool force_exec, u8 ra_masktype, u8 arfr_type,
+				u8 retrylimit_type, u8 ampdutime_type)
+{
+	u32 disra_mask = 0x0;
+
+	coex_dm->curra_masktype = ra_masktype;
+	disra_mask = halbtc8192e2ant_decidera_mask(btcoexist,
+						   coex_dm->cur_sstype,
+						   ra_masktype);
+	halbtc8192e2ant_Updatera_mask(btcoexist, force_exec, disra_mask);
+
+	halbtc8192e2ant_autorate_fallback_retry(btcoexist, force_exec,
+						arfr_type);
+	halbtc8192e2ant_retrylimit(btcoexist, force_exec, retrylimit_type);
+	halbtc8192e2ant_ampdu_maxtime(btcoexist, force_exec, ampdutime_type);
+}
+
+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);
+
+
+}
+
+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);
+}
+
+void halbtc8192e2ant_querybt_info(struct btc_coexist *btcoexist)
+{
+	u8 h2c_parameter[1] ={0};
+
+	coex_sta->c2h_bt_info_req_sent = true;
+
+	h2c_parameter[0] |= BIT0;	/* trigger */
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], Query Bt Info, FW write 0x61=0x%x\n",
+		  h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x61, 1, h2c_parameter);
+}
+
+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)
+{
+	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;
+}
+
+u8 halbtc8192e2ant_action_algorithm(struct btc_coexist *btcoexist)
+{
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	struct btc_stack_info *stack_info = &btcoexist->stack_info;
+	bool bt_hson=false;
+	u8 algorithm = BT_8192E_2ANT_COEX_ALGO_UNDEFINED;
+	u8 numOfDiffProfile = 0;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hson);
+
+	if (!bt_link_info->bt_link_exist) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "No BT link exists!!!\n");
+		return algorithm;
+	}
+
+	if (bt_link_info->sco_exist)
+		numOfDiffProfile++;
+	if (bt_link_info->hid_exist)
+		numOfDiffProfile++;
+	if (bt_link_info->pan_exist)
+		numOfDiffProfile++;
+	if (bt_link_info->a2dp_exist)
+		numOfDiffProfile++;
+
+	if (numOfDiffProfile == 1) {
+		if (bt_link_info->sco_exist) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "SCO only\n");
+			algorithm = BT_8192E_2ANT_COEX_ALGO_SCO;
+		} else {
+			if (bt_link_info->hid_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "HID only\n");
+				algorithm = BT_8192E_2ANT_COEX_ALGO_HID;
+			} else if (bt_link_info->a2dp_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "A2DP only\n");
+				algorithm = BT_8192E_2ANT_COEX_ALGO_A2DP;
+			} else if (bt_link_info->pan_exist) {
+				if (bt_hson) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "PAN(HS) only\n");
+					algorithm =
+						BT_8192E_2ANT_COEX_ALGO_PANHS;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "PAN(EDR) only\n");
+					algorithm =
+						BT_8192E_2ANT_COEX_ALGO_PANEDR;
+				}
+			}
+		}
+	} else if (numOfDiffProfile == 2) {
+		if (bt_link_info->sco_exist) {
+			if (bt_link_info->hid_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "SCO + HID\n");
+				algorithm = BT_8192E_2ANT_COEX_ALGO_SCO;
+			} else if (bt_link_info->a2dp_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "SCO + A2DP ==> SCO\n");
+				algorithm = BT_8192E_2ANT_COEX_ALGO_PANEDR_HID;
+			} else if (bt_link_info->pan_exist) {
+				if (bt_hson) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "SCO + PAN(HS)\n");
+					algorithm = BT_8192E_2ANT_COEX_ALGO_SCO;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "SCO + PAN(EDR)\n");
+					algorithm =
+						BT_8192E_2ANT_COEX_ALGO_SCO_PAN;
+				}
+			}
+		} else {
+			if (bt_link_info->hid_exist &&
+			    bt_link_info->a2dp_exist) {
+				if (stack_info->num_of_hid >= 2) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "HID*2 + A2DP\n");
+					algorithm =
+					BT_8192E_2ANT_COEX_ALGO_HID_A2DP_PANEDR;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "HID + A2DP\n");
+					algorithm =
+					    BT_8192E_2ANT_COEX_ALGO_HID_A2DP;
+				}
+			} else if (bt_link_info->hid_exist &&
+				   bt_link_info->pan_exist) {
+				if (bt_hson) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "HID + PAN(HS)\n");
+					algorithm = BT_8192E_2ANT_COEX_ALGO_HID;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "HID + PAN(EDR)\n");
+					algorithm =
+					    BT_8192E_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			} else if (bt_link_info->pan_exist &&
+				   bt_link_info->a2dp_exist) {
+				if (bt_hson) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "A2DP + PAN(HS)\n");
+					algorithm =
+					    BT_8192E_2ANT_COEX_ALGO_A2DP_PANHS;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "A2DP + PAN(EDR)\n");
+					algorithm =
+					    BT_8192E_2ANT_COEX_ALGO_PANEDR_A2DP;
+				}
+			}
+		}
+	} else if (numOfDiffProfile == 3) {
+		if (bt_link_info->sco_exist) {
+			if (bt_link_info->hid_exist &&
+			    bt_link_info->a2dp_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "SCO + HID + A2DP ==> HID\n");
+				algorithm = BT_8192E_2ANT_COEX_ALGO_PANEDR_HID;
+			} else if (bt_link_info->hid_exist &&
+				   bt_link_info->pan_exist) {
+				if (bt_hson) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "SCO + HID + PAN(HS)\n");
+					algorithm = BT_8192E_2ANT_COEX_ALGO_SCO;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "SCO + HID + PAN(EDR)\n");
+					algorithm =
+						BT_8192E_2ANT_COEX_ALGO_SCO_PAN;
+				}
+			} else if (bt_link_info->pan_exist &&
+				   bt_link_info->a2dp_exist) {
+				if (bt_hson) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "SCO + A2DP + PAN(HS)\n");
+					algorithm = BT_8192E_2ANT_COEX_ALGO_SCO;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "SCO + A2DP + PAN(EDR)\n");
+					algorithm =
+					    BT_8192E_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		} else {
+			if (bt_link_info->hid_exist &&
+			    bt_link_info->pan_exist &&
+			    bt_link_info->a2dp_exist) {
+				if (bt_hson) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "HID + A2DP + PAN(HS)\n");
+					algorithm =
+					    BT_8192E_2ANT_COEX_ALGO_HID_A2DP;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "HID + A2DP + PAN(EDR)\n");
+					algorithm =
+					BT_8192E_2ANT_COEX_ALGO_HID_A2DP_PANEDR;
+				}
+			}
+		}
+	} else if (numOfDiffProfile >= 3) {
+		if (bt_link_info->sco_exist) {
+			if (bt_link_info->hid_exist &&
+			    bt_link_info->pan_exist &&
+			    bt_link_info->a2dp_exist) {
+				if (bt_hson) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "ErrorSCO+HID+A2DP+PAN(HS)\n");
+
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "SCO+HID+A2DP+PAN(EDR)\n");
+					algorithm =
+					    BT_8192E_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		}
+	}
+
+	return algorithm;
+}
+
+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);
+}
+
+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);
+}
+
+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;
+}
+
+void halbtc8192e2ant_set_bt_autoreport(struct btc_coexist *btcoexist,
+				       bool enable_autoreport)
+{
+	u8 h2c_parameter[1] ={0};
+
+	h2c_parameter[0] = 0;
+
+	if (enable_autoreport)
+		h2c_parameter[0] |= BIT0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], BT FW auto report : %s, FW write 0x68=0x%x\n",
+		  (enable_autoreport? "Enabled!!":"Disabled!!"),
+		  h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x68, 1, h2c_parameter);
+}
+
+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;
+}
+
+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;
+}
+
+void halbtc8192e2ant_set_sw_rf_rx_lpf_corner(struct btc_coexist *btcoexist,
+					     bool rx_rf_shrink_on)
+{
+	if (rx_rf_shrink_on) {
+		/* Shrink RF Rx LPF corner */
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+			  "[BTCoex], Shrink RF Rx LPF corner!!\n");
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1e,
+					  0xfffff, 0xffffc);
+	} else {
+		/* Resume RF Rx LPF corner
+		 * After initialized, we can use coex_dm->btRf0x1eBackup */
+		if (btcoexist->initilized) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+				  "[BTCoex], Resume RF Rx LPF corner!!\n");
+			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1e,
+						  0xfffff,
+						  coex_dm->bt_rf0x1e_backup);
+		}
+	}
+}
+
+void halbtc8192e2ant_rf_shrink(struct btc_coexist *btcoexist,
+			       bool force_exec, bool rx_rf_shrink_on)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
+		  "[BTCoex], %s turn Rx RF Shrink = %s\n",
+		  (force_exec? "force to":""), ((rx_rf_shrink_on)? "ON":"OFF"));
+	coex_dm->cur_rf_rx_lpf_shrink = rx_rf_shrink_on;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "[BTCoex]bPreRfRxLpfShrink=%d,bCurRfRxLpfShrink=%d\n",
+			  coex_dm->pre_rf_rx_lpf_shrink,
+			  coex_dm->cur_rf_rx_lpf_shrink);
+
+		if (coex_dm->pre_rf_rx_lpf_shrink ==
+		    coex_dm->cur_rf_rx_lpf_shrink)
+			return;
+	}
+	halbtc8192e2ant_set_sw_rf_rx_lpf_corner(btcoexist,
+						coex_dm->cur_rf_rx_lpf_shrink);
+
+	coex_dm->pre_rf_rx_lpf_shrink = coex_dm->cur_rf_rx_lpf_shrink;
+}
+
+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)
+{
+	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 halbtc8192e2ant_setsw_fulltime_dacswing(struct btc_coexist *btcoexist,
+					     bool sw_dac_swingon,
+					     u32 sw_dac_swinglvl)
+{
+	if (sw_dac_swingon)
+		halbtc8192e2ant_set_dac_swingreg(btcoexist, sw_dac_swinglvl);
+	else
+		halbtc8192e2ant_set_dac_swingreg(btcoexist, 0x18);
+}
+
+
+void halbtc8192e2ant_DacSwing(struct btc_coexist *btcoexist,
+			      bool force_exec, bool dac_swingon,
+			      u32 dac_swinglvl)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
+		  "[BTCoex], %s turn DacSwing=%s, dac_swinglvl=0x%x\n",
+		  (force_exec? "force to":""),
+		  ((dac_swingon)? "ON":"OFF"), dac_swinglvl);
+	coex_dm->cur_dac_swing_on = dac_swingon;
+	coex_dm->cur_dac_swing_lvl = dac_swinglvl;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "[BTCoex], bPreDacSwingOn=%d, preDacSwingLvl=0x%x, ",
+			  coex_dm->pre_dac_swing_on,
+			  coex_dm->pre_dac_swing_lvl);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "bCurDacSwingOn=%d, curDacSwingLvl=0x%x\n",
+			  coex_dm->cur_dac_swing_on,
+			  coex_dm->cur_dac_swing_lvl);
+
+		if ((coex_dm->pre_dac_swing_on == coex_dm->cur_dac_swing_on) &&
+		    (coex_dm->pre_dac_swing_lvl == coex_dm->cur_dac_swing_lvl))
+			return;
+	}
+	mdelay(30);
+	halbtc8192e2ant_setsw_fulltime_dacswing(btcoexist, dac_swingon,
+						dac_swinglvl);
+
+	coex_dm->pre_dac_swing_on = coex_dm->cur_dac_swing_on;
+	coex_dm->pre_dac_swing_lvl = coex_dm->cur_dac_swing_lvl;
+}
+
+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)
+{
+
+	/* 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);
+	}
+}
+
+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;
+}
+
+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);
+}
+
+void halbtc8192e2ant_coex_table(struct btc_coexist *btcoexist, bool force_exec,
+				u32 val0x6c0, u32 val0x6c4,
+				u32 val0x6c8, u8 val0x6cc)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
+		  "[BTCoex], %s write Coex Table 0x6c0=0x%x, ",
+		  (force_exec? "force to":""), val0x6c0);
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
+		  "0x6c4=0x%x, 0x6c8=0x%x, 0x6cc=0x%x\n",
+		  val0x6c4, val0x6c8, val0x6cc);
+	coex_dm->cur_val0x6c0 = val0x6c0;
+	coex_dm->cur_val0x6c4 = val0x6c4;
+	coex_dm->cur_val0x6c8 = val0x6c8;
+	coex_dm->cur_val0x6cc = val0x6cc;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "[BTCoex], preVal0x6c0=0x%x, preVal0x6c4=0x%x, ",
+			  coex_dm->pre_val0x6c0, coex_dm->pre_val0x6c4);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "preVal0x6c8=0x%x, preVal0x6cc=0x%x !!\n",
+			  coex_dm->pre_val0x6c8, coex_dm->pre_val0x6cc);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "[BTCoex], curVal0x6c0=0x%x, curVal0x6c4=0x%x, \n",
+			  coex_dm->cur_val0x6c0, coex_dm->cur_val0x6c4);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "curVal0x6c8=0x%x, curVal0x6cc=0x%x !!\n",
+			  coex_dm->cur_val0x6c8, coex_dm->cur_val0x6cc);
+
+		if ((coex_dm->pre_val0x6c0 == coex_dm->cur_val0x6c0) &&
+		    (coex_dm->pre_val0x6c4 == coex_dm->cur_val0x6c4) &&
+		    (coex_dm->pre_val0x6c8 == coex_dm->cur_val0x6c8) &&
+		    (coex_dm->pre_val0x6cc == coex_dm->cur_val0x6cc))
+			return;
+	}
+	halbtc8192e2ant_set_coex_table(btcoexist, val0x6c0, val0x6c4,
+				       val0x6c8, val0x6cc);
+
+	coex_dm->pre_val0x6c0 = coex_dm->cur_val0x6c0;
+	coex_dm->pre_val0x6c4 = coex_dm->cur_val0x6c4;
+	coex_dm->pre_val0x6c8 = coex_dm->cur_val0x6c8;
+	coex_dm->pre_val0x6cc = coex_dm->cur_val0x6cc;
+}
+
+void halbtc8192e2ant_coex_table_with_type(struct btc_coexist *btcoexist,
+					  bool force_exec, u8 type)
+{
+	switch (type) {
+	case 0:
+		halbtc8192e2ant_coex_table(btcoexist, force_exec, 0x55555555,
+					   0x5a5a5a5a, 0xffffff, 0x3);
+		break;
+	case 1:
+		halbtc8192e2ant_coex_table(btcoexist, force_exec, 0x5a5a5a5a,
+					   0x5a5a5a5a, 0xffffff, 0x3);
+		break;
+	case 2:
+		halbtc8192e2ant_coex_table(btcoexist, force_exec, 0x55555555,
+					   0x5ffb5ffb, 0xffffff, 0x3);
+		break;
+	case 3:
+		halbtc8192e2ant_coex_table(btcoexist, force_exec, 0xdfffdfff,
+					   0x5fdb5fdb, 0xffffff, 0x3);
+		break;
+	case 4:
+		halbtc8192e2ant_coex_table(btcoexist, force_exec, 0xdfffdfff,
+					   0x5ffb5ffb, 0xffffff, 0x3);
+		break;
+	default:
+		break;
+	}
+}
+
+void halbtc8192e2ant_set_fw_ignore_wlanact(struct btc_coexist *btcoexist,
+					   bool enable)
+{
+	u8 h2c_parameter[1] ={0};
+
+	if (enable)
+		h2c_parameter[0] |= BIT0; /* function enable */
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex]set FW for BT Ignore Wlan_Act, FW write 0x63=0x%x\n",
+		  h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x63, 1, h2c_parameter);
+}
+
+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;
+}
+
+void halbtc8192e2ant_SetFwPstdma(struct btc_coexist *btcoexist, u8 byte1,
+				 u8 byte2, u8 byte3, u8 byte4, u8 byte5)
+{
+	u8 h2c_parameter[5] ={0};
+
+	h2c_parameter[0] = byte1;
+	h2c_parameter[1] = byte2;
+	h2c_parameter[2] = byte3;
+	h2c_parameter[3] = byte4;
+	h2c_parameter[4] = byte5;
+
+	coex_dm->ps_tdma_para[0] = byte1;
+	coex_dm->ps_tdma_para[1] = byte2;
+	coex_dm->ps_tdma_para[2] = byte3;
+	coex_dm->ps_tdma_para[3] = byte4;
+	coex_dm->ps_tdma_para[4] = byte5;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], FW write 0x60(5bytes)=0x%x%08x\n",
+		  h2c_parameter[0],
+		  h2c_parameter[1] << 24 | h2c_parameter[2] << 16 |
+		  h2c_parameter[3] << 8 | h2c_parameter[4]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x60, 5, h2c_parameter);
+}
+
+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)
+{
+	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)
+{
+
+	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;
+}
+
+void halbtc8192e2ant_set_switch_sstype(struct btc_coexist *btcoexist, u8 sstype)
+{
+	u8 mimops = BTC_MIMO_PS_DYNAMIC;
+	u32 disra_mask = 0x0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+		  "[BTCoex], REAL set SS Type = %d\n", sstype);
+
+	disra_mask = halbtc8192e2ant_decidera_mask(btcoexist, sstype,
+						   coex_dm->curra_masktype);
+	halbtc8192e2ant_Updatera_mask(btcoexist, FORCE_EXEC, disra_mask);
+
+	if (sstype == 1) {
+		halbtc8192e2ant_ps_tdma(btcoexist, FORCE_EXEC, false, 1);
+		/* switch ofdm path */
+		btcoexist->btc_write_1byte(btcoexist, 0xc04, 0x11);
+		btcoexist->btc_write_1byte(btcoexist, 0xd04, 0x1);
+		btcoexist->btc_write_4byte(btcoexist, 0x90c, 0x81111111);
+		/* switch cck patch */
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0xe77, 0x4, 0x1);
+		btcoexist->btc_write_1byte(btcoexist, 0xa07, 0x81);
+		mimops=BTC_MIMO_PS_STATIC;
+	} else if (sstype == 2) {
+		halbtc8192e2ant_ps_tdma(btcoexist, FORCE_EXEC, false, 0);
+		btcoexist->btc_write_1byte(btcoexist, 0xc04, 0x33);
+		btcoexist->btc_write_1byte(btcoexist, 0xd04, 0x3);
+		btcoexist->btc_write_4byte(btcoexist, 0x90c, 0x81121313);
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0xe77, 0x4, 0x0);
+		btcoexist->btc_write_1byte(btcoexist, 0xa07, 0x41);
+		mimops=BTC_MIMO_PS_DYNAMIC;
+	}
+	/* set rx 1ss or 2ss */
+	btcoexist->btc_set(btcoexist, BTC_SET_ACT_SEND_MIMO_PS, &mimops);
+}
+
+void halbtc8192e2ant_switch_sstype(struct btc_coexist *btcoexist,
+				   bool force_exec, u8 new_sstype)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+		  "[BTCoex], %s Switch SS Type = %d\n",
+		  (force_exec? "force to":""), new_sstype);
+	coex_dm->cur_sstype = new_sstype;
+
+	if (!force_exec) {
+		if (coex_dm->pre_sstype == coex_dm->cur_sstype)
+			return;
+	}
+	halbtc8192e2ant_set_switch_sstype(btcoexist, coex_dm->cur_sstype);
+
+	coex_dm->pre_sstype = coex_dm->cur_sstype;
+}
+
+void halbtc8192e2ant_coex_alloff(struct btc_coexist *btcoexist)
+{
+	/* fw all off */
+	halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
+	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
+	halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+
+	/* sw all off */
+	halbtc8192e2ant_sw_mechanism1(btcoexist, false, false, false, false);
+	halbtc8192e2ant_sw_mechanism2(btcoexist, false, false, false, 0x18);
+
+	/* hw all off */
+	halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
+}
+
+void halbtc8192e2ant_init_coex_dm(struct btc_coexist *btcoexist)
+{
+	/* force to reset coex mechanism */
+
+	halbtc8192e2ant_ps_tdma(btcoexist, FORCE_EXEC, false, 1);
+	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, FORCE_EXEC, 6);
+	halbtc8192e2ant_dec_btpwr(btcoexist, FORCE_EXEC, 0);
+
+	halbtc8192e2ant_coex_table_with_type(btcoexist, FORCE_EXEC, 0);
+	halbtc8192e2ant_switch_sstype(btcoexist, FORCE_EXEC, 2);
+
+	halbtc8192e2ant_sw_mechanism1(btcoexist, false, false, false, false);
+	halbtc8192e2ant_sw_mechanism2(btcoexist, false, false, false, 0x18);
+}
+
+void halbtc8192e2ant_action_bt_inquiry(struct btc_coexist *btcoexist)
+{
+	bool low_pwr_disable = true;
+
+	btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
+			   &low_pwr_disable);
+
+	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
+
+	halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+	halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 3);
+	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
+	halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+
+	halbtc8192e2ant_sw_mechanism1(btcoexist, false, false, false, false);
+	halbtc8192e2ant_sw_mechanism2(btcoexist, false, false, false, 0x18);
+}
+
+bool halbtc8192e2ant_is_common_action(struct btc_coexist *btcoexist)
+{
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	bool common = false, wifi_connected = false, wifi_busy = false;
+	bool bt_hson = false, low_pwr_disable = false;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hson);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+			   &wifi_connected);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+
+	if (bt_link_info->sco_exist || bt_link_info->hid_exist)
+		halbtc8192e2ant_limited_tx(btcoexist, NORMAL_EXEC, 1, 0, 0, 0);
+	else
+		halbtc8192e2ant_limited_tx(btcoexist, NORMAL_EXEC, 0, 0, 0, 0);
+
+	if (!wifi_connected) {
+		low_pwr_disable = false;
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
+				   &low_pwr_disable);
+
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Wifi non-connected idle!!\n");
+
+		if ((BT_8192E_2ANT_BT_STATUS_NON_CONNECTED_IDLE ==
+		     coex_dm->bt_status) ||
+		    (BT_8192E_2ANT_BT_STATUS_CONNECTED_IDLE ==
+		     coex_dm->bt_status)) {
+			halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC,
+						      2);
+			halbtc8192e2ant_coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 1);
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, false,
+						0);
+		} else {
+			halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC,
+						      1);
+			halbtc8192e2ant_coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 0);
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, false,
+						1);
+		}
+
+		halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+
+ 		halbtc8192e2ant_sw_mechanism1(btcoexist, false, false, false,
+					      false);
+		halbtc8192e2ant_sw_mechanism2(btcoexist, false, false, false,
+					      0x18);
+
+		common = true;
+	} else {
+		if (BT_8192E_2ANT_BT_STATUS_NON_CONNECTED_IDLE ==
+		    coex_dm->bt_status) {
+			low_pwr_disable = false;
+			btcoexist->btc_set(btcoexist,
+					   BTC_SET_ACT_DISABLE_LOW_POWER,
+					   &low_pwr_disable);
+
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "Wifi connected + BT non connected-idle!!\n");
+
+			halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC,
+						      2);
+			halbtc8192e2ant_coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 1);
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, false,
+						0);
+			halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC,
+							6);
+			halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+
+			halbtc8192e2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+
+			common = true;
+		} else if (BT_8192E_2ANT_BT_STATUS_CONNECTED_IDLE ==
+			   coex_dm->bt_status) {
+			low_pwr_disable = true;
+			btcoexist->btc_set(btcoexist,
+					   BTC_SET_ACT_DISABLE_LOW_POWER,
+					   &low_pwr_disable);
+
+			if (bt_hson)
+				return false;
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "Wifi connected + BT connected-idle!!\n");
+
+			halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC,
+						      2);
+			halbtc8192e2ant_coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 1);
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, false,
+						0);
+			halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC,
+							6);
+			halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+
+			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+
+			common = true;
+		} else {
+			low_pwr_disable = true;
+			btcoexist->btc_set(btcoexist,
+					   BTC_SET_ACT_DISABLE_LOW_POWER,
+					   &low_pwr_disable);
+
+			if (wifi_busy) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "Wifi Connected-Busy + BT Busy!!\n");
+				common = false;
+			} else {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "Wifi Connected-Idle + BT Busy!!\n");
+
+				halbtc8192e2ant_switch_sstype(btcoexist,
+							      NORMAL_EXEC, 1);
+				halbtc8192e2ant_coex_table_with_type(btcoexist,
+								    NORMAL_EXEC,
+								    2);
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 21);
+				halbtc8192e2ant_fw_dac_swinglvl(btcoexist,
+								NORMAL_EXEC, 6);
+				halbtc8192e2ant_dec_btpwr(btcoexist,
+							  NORMAL_EXEC, 0);
+				halbtc8192e2ant_sw_mechanism1(btcoexist, false,
+							      false, false,
+							      false);
+				halbtc8192e2ant_sw_mechanism2(btcoexist, false,
+							      false, false,
+							      0x18);
+				common = true;
+			}
+		}
+	}
+	return common;
+}
+
+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) */
+void halbtc8192e2ant_action_sco(struct btc_coexist *btcoexist)
+{
+	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_STAY_LOW;
+	u32 wifi_bw;
+
+	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
+
+	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
+	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
+
+	halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 4);
+
+	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
+
+	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
+	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 13);
+	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
+	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
+	}
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	/* sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x6);
+		} else {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x6);
+		}
+	} else {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x6);
+		} else {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x6);
+		}
+	}
+}
+
+void halbtc8192e2ant_action_sco_pan(struct btc_coexist *btcoexist)
+{
+	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_STAY_LOW;
+	u32 wifi_bw;
+
+	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
+
+	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
+	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
+
+	halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 4);
+
+	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
+
+	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
+	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 14);
+	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 10);
+	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 10);
+	}
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	/* sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x6);
+		} else {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x6);
+		}
+	} else {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x6);
+		} else {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x6);
+		}
+	}
+}
+
+void halbtc8192e2ant_action_hid(struct btc_coexist *btcoexist)
+{
+	u8 wifirssi_state, btrssi_state=BTC_RSSI_STATE_HIGH;
+	u32 wifi_bw;
+
+	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
+	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
+
+	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
+	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 3);
+
+	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
+	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 13);
+	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
+	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
+	}
+
+	/* sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+ 			halbtc8192e2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+ 		} else {
+ 			halbtc8192e2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+/* A2DP only / PAN(EDR) only/ A2DP+PAN(HS) */
+void halbtc8192e2ant_action_a2dp(struct btc_coexist *btcoexist)
+{
+	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
+	u32 wifi_bw;
+	bool long_dist = false;
+
+	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
+	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
+
+	if ((btrssi_state == BTC_RSSI_STATE_LOW ||
+	     btrssi_state == BTC_RSSI_STATE_STAY_LOW) &&
+	    (wifirssi_state == BTC_RSSI_STATE_LOW ||
+	     wifirssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], A2dp, wifi/bt rssi both LOW!!\n");
+		long_dist = true;
+	}
+	if (long_dist) {
+		halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 2);
+		halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, true,
+					   0x4);
+	} else {
+		halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
+		halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false,
+					   0x8);
+	}
+
+	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (long_dist)
+		halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
+	else
+		halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+
+
+	if (long_dist) {
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 17);
+		coex_dm->auto_tdma_adjust = false;
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+	} else {
+		if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
+		    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+			halbtc8192e2ant_tdma_duration_adjust(btcoexist, false,
+							     true, 1);
+			halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+		} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
+			   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
+			halbtc8192e2ant_tdma_duration_adjust(btcoexist, false,
+							     false, 1);
+			halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
+		} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
+			   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8192e2ant_tdma_duration_adjust(btcoexist, false,
+							     false, 1);
+			halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
+		}
+	}
+
+	/* sw mechanism */
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+ 			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+void halbtc8192e2ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
+{
+	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
+	u32 wifi_bw;
+
+	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
+	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
+
+	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
+	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
+	halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+
+	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
+	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+		halbtc8192e2ant_tdma_duration_adjust(btcoexist, false, true, 2);
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
+		halbtc8192e2ant_tdma_duration_adjust(btcoexist, false, false,
+						     2);
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
+	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+		halbtc8192e2ant_tdma_duration_adjust(btcoexist, false, false,
+						     2);
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
+	}
+
+	/* sw mechanism */
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+ 			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
+						      true, 0x6);
+		} else {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
+						      true, 0x6);
+		}
+	} else {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
+						      true, 0x6);
+		} else {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
+						      true, 0x6);
+		}
+	}
+}
+
+void halbtc8192e2ant_action_pan_edr(struct btc_coexist *btcoexist)
+{
+	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
+	u32 wifi_bw;
+
+	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
+	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
+
+	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
+	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
+
+	halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+
+	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
+	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
+	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 1);
+	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 1);
+	}
+
+	/* sw mechanism */
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+/* PAN(HS) only */
+void halbtc8192e2ant_action_pan_hs(struct btc_coexist *btcoexist)
+{
+	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
+	u32 wifi_bw;
+
+	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
+	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
+
+	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
+	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
+
+	halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+
+	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
+	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
+	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
+	}
+	halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+/* PAN(EDR)+A2DP */
+void halbtc8192e2ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
+{
+	u8 wifirssi_state, btrssi_state=BTC_RSSI_STATE_HIGH;
+	u32 wifi_bw;
+
+	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
+	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
+
+	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
+	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
+
+	halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
+	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+		halbtc8192e2ant_tdma_duration_adjust(btcoexist, false, true, 3);
+	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
+		halbtc8192e2ant_tdma_duration_adjust(btcoexist, false, false,
+						     3);
+	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
+		halbtc8192e2ant_tdma_duration_adjust(btcoexist, false, false,
+						     3);
+	}
+
+	/* sw mechanism	*/
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+void halbtc8192e2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
+{
+	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
+	u32 wifi_bw;
+
+	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
+	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
+	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
+
+	halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 3);
+
+	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
+	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+	        halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 14);
+	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
+	        halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 10);
+	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
+	        halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 10);
+	}
+
+	/* sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+/* HID+A2DP+PAN(EDR) */
+void halbtc8192e2ant_action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
+{
+	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
+	u32 wifi_bw;
+
+	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
+	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
+
+	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
+	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 3);
+
+	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
+	   (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+		halbtc8192e2ant_tdma_duration_adjust(btcoexist, true, true, 3);
+	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
+		halbtc8192e2ant_tdma_duration_adjust(btcoexist, true, false, 3);
+	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
+		halbtc8192e2ant_tdma_duration_adjust(btcoexist, true, false, 3);
+	}
+
+	/* sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+void halbtc8192e2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
+{
+	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
+	u32 wifi_bw;
+
+	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
+	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
+
+	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
+	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 3);
+
+	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
+	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+		halbtc8192e2ant_tdma_duration_adjust(btcoexist, true, true, 2);
+	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) 	{
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
+		halbtc8192e2ant_tdma_duration_adjust(btcoexist, true, false, 2);
+	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
+		halbtc8192e2ant_tdma_duration_adjust(btcoexist, true, false, 2);
+	}
+
+	/* sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+void halbtc8192e2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
+{
+	u8 algorithm = 0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+		  "[BTCoex], RunCoexistMechanism()===>\n");
+
+	if (btcoexist->manual_control) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], return for Manual CTRL <===\n");
+		return;
+	}
+
+	if (coex_sta->under_ips) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], wifi is under IPS !!!\n");
+		return;
+	}
+
+	algorithm = halbtc8192e2ant_action_algorithm(btcoexist);
+	if (coex_sta->c2h_bt_inquiry_page &&
+	    (BT_8192E_2ANT_COEX_ALGO_PANHS != algorithm)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BT is under inquiry/page scan !!\n");
+		halbtc8192e2ant_action_bt_inquiry(btcoexist);
+		return;
+	}
+
+	coex_dm->cur_algorithm = algorithm;
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+		  "[BTCoex], Algorithm = %d \n", coex_dm->cur_algorithm);
+
+	if (halbtc8192e2ant_is_common_action(btcoexist)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Action 2-Ant common.\n");
+		coex_dm->auto_tdma_adjust = false;
+	} else {
+		if (coex_dm->cur_algorithm != coex_dm->pre_algorithm) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex] preAlgorithm=%d, curAlgorithm=%d\n",
+				  coex_dm->pre_algorithm,
+				  coex_dm->cur_algorithm);
+			coex_dm->auto_tdma_adjust = false;
+		}
+		switch (coex_dm->cur_algorithm) {
+		case BT_8192E_2ANT_COEX_ALGO_SCO:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "Action 2-Ant, algorithm = SCO.\n");
+			halbtc8192e2ant_action_sco(btcoexist);
+			break;
+		case BT_8192E_2ANT_COEX_ALGO_SCO_PAN:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "Action 2-Ant, algorithm = SCO+PAN(EDR).\n");
+			halbtc8192e2ant_action_sco_pan(btcoexist);
+			break;
+		case BT_8192E_2ANT_COEX_ALGO_HID:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "Action 2-Ant, algorithm = HID.\n");
+			halbtc8192e2ant_action_hid(btcoexist);
+			break;
+		case BT_8192E_2ANT_COEX_ALGO_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "Action 2-Ant, algorithm = A2DP.\n");
+			halbtc8192e2ant_action_a2dp(btcoexist);
+			break;
+		case BT_8192E_2ANT_COEX_ALGO_A2DP_PANHS:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "Action 2-Ant, algorithm = A2DP+PAN(HS).\n");
+			halbtc8192e2ant_action_a2dp_pan_hs(btcoexist);
+			break;
+		case BT_8192E_2ANT_COEX_ALGO_PANEDR:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "Action 2-Ant, algorithm = PAN(EDR).\n");
+			halbtc8192e2ant_action_pan_edr(btcoexist);
+			break;
+		case BT_8192E_2ANT_COEX_ALGO_PANHS:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "Action 2-Ant, algorithm = HS mode.\n");
+			halbtc8192e2ant_action_pan_hs(btcoexist);
+			break;
+		case BT_8192E_2ANT_COEX_ALGO_PANEDR_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "Action 2-Ant, algorithm = PAN+A2DP.\n");
+			halbtc8192e2ant_action_pan_edr_a2dp(btcoexist);
+			break;
+		case BT_8192E_2ANT_COEX_ALGO_PANEDR_HID:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "Action 2-Ant, algorithm = PAN(EDR)+HID.\n");
+			halbtc8192e2ant_action_pan_edr_hid(btcoexist);
+			break;
+		case BT_8192E_2ANT_COEX_ALGO_HID_A2DP_PANEDR:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "Action 2-Ant, algorithm = HID+A2DP+PAN.\n");
+			halbtc8192e2ant_action_hid_a2dp_pan_edr(btcoexist);
+			break;
+		case BT_8192E_2ANT_COEX_ALGO_HID_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "Action 2-Ant, algorithm = HID+A2DP.\n");
+			halbtc8192e2ant_action_hid_a2dp(btcoexist);
+			break;
+		default:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "Action 2-Ant, algorithm = unknown!!\n");
+			/* halbtc8192e2ant_coex_alloff(btcoexist); */
+			break;
+		}
+		coex_dm->pre_algorithm = coex_dm->cur_algorithm;
+	}
+}
+
+void halbtc8192e2ant_init_hwconfig(struct btc_coexist *btcoexist, bool backup)
+{
+	u16 u16tmp = 0;
+	u8 u8tmp = 0;
+
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+		  "[BTCoex], 2Ant Init HW Config!!\n");
+
+	if (backup) {
+		/* backup rf 0x1e value */
+		coex_dm->bt_rf0x1e_backup =
+			btcoexist->btc_get_rf_reg(btcoexist, BTC_RF_A,
+						  0x1e, 0xfffff);
+
+		coex_dm->backup_arfr_cnt1 = btcoexist->btc_read_4byte(btcoexist,
+								      0x430);
+		coex_dm->backup_arfr_cnt2 = btcoexist->btc_read_4byte(btcoexist,
+								     0x434);
+		coex_dm->backup_retrylimit = btcoexist->btc_read_2byte(
+								    btcoexist,
+								    0x42a);
+		coex_dm->backup_ampdu_maxtime = btcoexist->btc_read_1byte(
+								    btcoexist,
+								    0x456);
+	}
+
+	/* antenna sw ctrl to bt */
+	btcoexist->btc_write_1byte(btcoexist, 0x4f, 0x6);
+	btcoexist->btc_write_1byte(btcoexist, 0x944, 0x24);
+	btcoexist->btc_write_4byte(btcoexist, 0x930, 0x700700);
+	btcoexist->btc_write_1byte(btcoexist, 0x92c, 0x20);
+	if (btcoexist->chip_interface == BTC_INTF_USB)
+		btcoexist->btc_write_4byte(btcoexist, 0x64, 0x30430004);
+	else
+		btcoexist->btc_write_4byte(btcoexist, 0x64, 0x30030004);
+
+	halbtc8192e2ant_coex_table_with_type(btcoexist, FORCE_EXEC, 0);
+
+	/* antenna switch control parameter */
+	btcoexist->btc_write_4byte(btcoexist, 0x858, 0x55555555);
+
+	/* coex parameters */
+	btcoexist->btc_write_1byte(btcoexist, 0x778, 0x3);
+	/* 0x790[5:0]=0x5 */
+	u8tmp = btcoexist->btc_read_1byte(btcoexist, 0x790);
+	u8tmp &= 0xc0;
+	u8tmp |= 0x5;
+	btcoexist->btc_write_1byte(btcoexist, 0x790, u8tmp);
+
+	/* enable counter statistics */
+	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0x4);
+
+	/* enable PTA */
+	btcoexist->btc_write_1byte(btcoexist, 0x40, 0x20);
+	/* enable mailbox interface */
+	u16tmp = btcoexist->btc_read_2byte(btcoexist, 0x40);
+	u16tmp |= BIT9;
+	btcoexist->btc_write_2byte(btcoexist, 0x40, u16tmp);
+
+	/* enable PTA I2C mailbox  */
+	u8tmp = btcoexist->btc_read_1byte(btcoexist, 0x101);
+	u8tmp |= BIT4;
+	btcoexist->btc_write_1byte(btcoexist, 0x101, u8tmp);
+
+	/* enable bt clock when wifi is disabled. */
+	u8tmp = btcoexist->btc_read_1byte(btcoexist, 0x93);
+	u8tmp |= BIT0;
+	btcoexist->btc_write_1byte(btcoexist, 0x93, u8tmp);
+	/* enable bt clock when suspend. */
+	u8tmp = btcoexist->btc_read_1byte(btcoexist, 0x7);
+	u8tmp |= BIT0;
+	btcoexist->btc_write_1byte(btcoexist, 0x7, u8tmp);
+}
+
+/*************************************************************
+ *   work around function start with wa_halbtc8192e2ant_
+ *************************************************************/
+
+/************************************************************
+ *   extern function start with EXhalbtc8192e2ant_
+ ************************************************************/
+
+void ex_halbtc8192e2ant_init_hwconfig(struct btc_coexist *btcoexist)
+{
+	halbtc8192e2ant_init_hwconfig(btcoexist, true);
+}
+
+void ex_halbtc8192e2ant_init_coex_dm(struct btc_coexist *btcoexist)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+		  "[BTCoex], Coex Mechanism Init!!\n");
+	halbtc8192e2ant_init_coex_dm(btcoexist);
+}
+
+void ex_halbtc8192e2ant_display_coex_info(struct btc_coexist *btcoexist)
+{
+	struct btc_board_info *board_info = &btcoexist->board_info;
+	struct btc_stack_info*stack_info = &btcoexist->stack_info;
+	u8 *cli_buf = btcoexist->cli_buf;
+	u8 u8tmp[4], i, bt_info_ext, ps_tdma_case = 0;
+	u16 u16tmp[4];
+	u32 u32tmp[4];
+	bool roam = false, scan = false, link = false, wifi_under_5g = false;
+	bool bt_hson = false, wifi_busy = false;
+	int wifirssi = 0, bt_hs_rssi = 0;
+	u32 wifi_bw, wifi_traffic_dir;
+	u8 wifi_dot11_chnl, wifi_hs_chnl;
+	u32 fw_ver = 0, bt_patch_ver = 0;
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n ============[BT Coexist info]============");
+	CL_PRINTF(cli_buf);
+
+	if (btcoexist->manual_control) {
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\r\n ===========[Under Manual Control]===========");
+		CL_PRINTF(cli_buf);
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\r\n ==========================================");
+		CL_PRINTF(cli_buf);
+	}
+
+	if (!board_info->bt_exist) {
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n BT not exists !!!");
+		CL_PRINTF(cli_buf);
+		return;
+	}
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n %-35s = %d/ %d ", "Ant PG number/ Ant mechanism:",
+		   board_info->pg_ant_num, board_info->btdm_ant_num);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %d",
+		   "BT stack/ hci ext ver",
+		   ((stack_info->profile_notified) ? "Yes" : "No"),
+		   stack_info->hci_version);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER, &bt_patch_ver);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n %-35s = %d_%d/ 0x%x/ 0x%x(%d)",
+		   "CoexVer/ FwVer/ PatchVer",
+		   glcoex_ver_date_8192e_2ant, glcoex_ver_8192e_2ant,
+		   fw_ver, bt_patch_ver, bt_patch_ver);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hson);
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_DOT11_CHNL,
+			   &wifi_dot11_chnl);
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifi_hs_chnl);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d(%d)",
+		   "Dot11 channel / HsMode(HsChnl)",
+		   wifi_dot11_chnl, bt_hson, wifi_hs_chnl);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x ",
+		   "H2C Wifi inform bt chnl Info", coex_dm->wifi_chnl_info[0],
+		   coex_dm->wifi_chnl_info[1], coex_dm->wifi_chnl_info[2]);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifirssi);
+	btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
+		   "Wifi rssi/ HS rssi", wifirssi, bt_hs_rssi);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ",
+		   "Wifi link/ roam/ scan", link, roam, scan);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under_5g);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION,
+			   &wifi_traffic_dir);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %s/ %s ",
+		   "Wifi status", (wifi_under_5g ? "5G" : "2.4G"),
+		   ((BTC_WIFI_BW_LEGACY == wifi_bw) ? "Legacy" :
+		   	(((BTC_WIFI_BW_HT40 == wifi_bw) ? "HT40" : "HT20"))),
+		   ((!wifi_busy) ? "idle" :
+		   	((BTC_WIFI_TRAFFIC_TX == wifi_traffic_dir) ?
+				"uplink" : "downlink")));
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = [%s/ %d/ %d] ",
+		   "BT [status/ rssi/ retryCnt]",
+		   ((btcoexist->bt_info.bt_disabled) ? ("disabled") :
+		    ((coex_sta->c2h_bt_inquiry_page) ?
+		     ("inquiry/page scan") :
+		      ((BT_8192E_2ANT_BT_STATUS_NON_CONNECTED_IDLE ==
+			coex_dm->bt_status) ? "non-connected idle" :
+			 ((BT_8192E_2ANT_BT_STATUS_CONNECTED_IDLE ==
+			   coex_dm->bt_status) ? "connected-idle" : "busy")))),
+		   coex_sta->bt_rssi, coex_sta->bt_retry_cnt);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d / %d / %d",
+		   "SCO/HID/PAN/A2DP", stack_info->sco_exist,
+		   stack_info->hid_exist, stack_info->pan_exist,
+		   stack_info->a2dp_exist);
+	CL_PRINTF(cli_buf);
+	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_BT_LINK_INFO);
+
+	bt_info_ext = coex_sta->bt_info_ext;
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s",
+		   "BT Info A2DP rate",
+		   (bt_info_ext&BIT0) ? "Basic rate" : "EDR rate");
+	CL_PRINTF(cli_buf);
+
+	for (i=0; i<BT_INFO_SRC_8192E_2ANT_MAX; i++) {
+		if (coex_sta->bt_info_c2h_cnt[i]) {
+			CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+				   "\r\n %-35s = %02x %02x %02x %02x ",
+				   GLBtInfoSrc8192e2Ant[i],
+				   coex_sta->bt_info_c2h[i][0],
+				   coex_sta->bt_info_c2h[i][1],
+				   coex_sta->bt_info_c2h[i][2],
+				   coex_sta->bt_info_c2h[i][3]);
+			CL_PRINTF(cli_buf);
+			CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+				   "%02x %02x %02x(%d)",
+				   coex_sta->bt_info_c2h[i][4],
+				   coex_sta->bt_info_c2h[i][5],
+				   coex_sta->bt_info_c2h[i][6],
+				   coex_sta->bt_info_c2h_cnt[i]);
+			CL_PRINTF(cli_buf);
+		}
+	}
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s/%s",
+		   "PS state, IPS/LPS",
+		   ((coex_sta->under_ips ? "IPS ON" : "IPS OFF")),
+		   ((coex_sta->under_lps ? "LPS ON" : "LPS OFF")));
+	CL_PRINTF(cli_buf);
+	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_FW_PWR_MODE_CMD);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x ", "SS Type",
+		   coex_dm->cur_sstype);
+	CL_PRINTF(cli_buf);
+
+	/* Sw mechanism	*/
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
+		   "============[Sw mechanism]============");
+	CL_PRINTF(cli_buf);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ",
+		   "SM1[ShRf/ LpRA/ LimDig]", coex_dm->cur_rf_rx_lpf_shrink,
+		   coex_dm->cur_low_penalty_ra, coex_dm->limited_dig);
+	CL_PRINTF(cli_buf);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d(0x%x) ",
+		   "SM2[AgcT/ AdcB/ SwDacSwing(lvl)]",
+		   coex_dm->cur_agc_table_en, coex_dm->cur_adc_back_off,
+		   coex_dm->cur_dac_swing_on, coex_dm->cur_dac_swing_lvl);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x ", "Rate Mask",
+		   btcoexist->bt_info.ra_mask);
+	CL_PRINTF(cli_buf);
+
+	/* Fw mechanism	*/
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
+		   "============[Fw mechanism]============");
+	CL_PRINTF(cli_buf);
+
+	ps_tdma_case = coex_dm->cur_ps_tdma;
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n %-35s = %02x %02x %02x %02x %02x case-%d (auto:%d)",
+		   "PS TDMA", coex_dm->ps_tdma_para[0],
+		   coex_dm->ps_tdma_para[1], coex_dm->ps_tdma_para[2],
+		   coex_dm->ps_tdma_para[3], coex_dm->ps_tdma_para[4],
+		   ps_tdma_case, coex_dm->auto_tdma_adjust);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ",
+		   "DecBtPwr/ IgnWlanAct",
+		   coex_dm->cur_dec_bt_pwr, coex_dm->cur_ignore_wlan_act);
+	CL_PRINTF(cli_buf);
+
+	/* Hw setting */
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
+		   "============[Hw setting]============");
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x",
+		   "RF-A, 0x1e initVal", coex_dm->bt_rf0x1e_backup);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/0x%x/0x%x/0x%x",
+		   "backup ARFR1/ARFR2/RL/AMaxTime", coex_dm->backup_arfr_cnt1,
+		   coex_dm->backup_arfr_cnt2, coex_dm->backup_retrylimit,
+		   coex_dm->backup_ampdu_maxtime);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x430);
+	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x434);
+	u16tmp[0] = btcoexist->btc_read_2byte(btcoexist, 0x42a);
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x456);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/0x%x/0x%x/0x%x",
+		   "0x430/0x434/0x42a/0x456",
+		   u32tmp[0], u32tmp[1], u16tmp[0], u8tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xc04);
+	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0xd04);
+	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x90c);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
+		   "0xc04/ 0xd04/ 0x90c", u32tmp[0], u32tmp[1], u32tmp[2]);
+	CL_PRINTF(cli_buf);
+
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x778);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x778",
+		   u8tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x92c);
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x930);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
+		   "0x92c/ 0x930", (u8tmp[0]), u32tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x40);
+	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x4f);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
+		   "0x40/ 0x4f", u8tmp[0], u8tmp[1]);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x550);
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x522);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
+		   "0x550(bcn ctrl)/0x522", u32tmp[0], u8tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xc50);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0xc50(dig)",
+		   u32tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x6c0);
+	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x6c4);
+	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x6c8);
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x6cc);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x",
+		   "0x6c0/0x6c4/0x6c8/0x6cc(coexTable)",
+		   u32tmp[0], u32tmp[1], u32tmp[2], u8tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
+		   "0x770(hp rx[31:16]/tx[15:0])",
+		   coex_sta->high_priority_rx, coex_sta->high_priority_tx);
+	CL_PRINTF(cli_buf);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
+		   "0x774(lp rx[31:16]/tx[15:0])",
+		   coex_sta->low_priority_rx, coex_sta->low_priority_tx);
+	CL_PRINTF(cli_buf);
+#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 & BIT1)) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "bit1, send wifi BW&Chnl to BT!!\n");
+			btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+					   &wifi_connected);
+			if (wifi_connected)
+				ex_halbtc8192e2ant_media_status_notify(
+							btcoexist,
+							BTC_MEDIA_CONNECT);
+			else
+				ex_halbtc8192e2ant_media_status_notify(
+							btcoexist,
+							BTC_MEDIA_DISCONNECT);
+		}
+
+		if ((coex_sta->bt_info_ext & BIT3)) {
+			if (!btcoexist->manual_control &&
+			    !btcoexist->stop_coex_dm) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "bit3, BT NOT ignore Wlan active!\n");
+				halbtc8192e2ant_IgnoreWlanAct(btcoexist,
+							      FORCE_EXEC,
+							      false);
+			}
+		} else {
+			/* BT already NOT ignore Wlan active,
+			 * do nothing here. */
+		}
+
+#if(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)
+		coex_sta->c2h_bt_inquiry_page = true;
+	else
+		coex_sta->c2h_bt_inquiry_page = false;
+
+	/* set link exist status */
+	if (!(bt_info&BT_INFO_8192E_2ANT_B_CONNECTION)) {
+		coex_sta->bt_link_exist = false;
+		coex_sta->pan_exist = false;
+		coex_sta->a2dp_exist = false;
+		coex_sta->hid_exist = false;
+		coex_sta->sco_exist = false;
+	} else {/* connection exists */
+		coex_sta->bt_link_exist = true;
+		if (bt_info & BT_INFO_8192E_2ANT_B_FTP)
+			coex_sta->pan_exist = true;
+		else
+			coex_sta->pan_exist = false;
+		if (bt_info & BT_INFO_8192E_2ANT_B_A2DP)
+			coex_sta->a2dp_exist = true;
+		else
+			coex_sta->a2dp_exist = false;
+		if (bt_info & BT_INFO_8192E_2ANT_B_HID)
+			coex_sta->hid_exist = true;
+		else
+			coex_sta->hid_exist = false;
+		if (bt_info & BT_INFO_8192E_2ANT_B_SCO_ESCO)
+			coex_sta->sco_exist = true;
+		else
+			coex_sta->sco_exist = false;
+	}
+
+	halbtc8192e2ant_update_btlink_info(btcoexist);
+
+	if (!(bt_info&BT_INFO_8192E_2ANT_B_CONNECTION)) {
+		coex_dm->bt_status = BT_8192E_2ANT_BT_STATUS_NON_CONNECTED_IDLE;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BT Non-Connected idle!!!\n");
+	} else if (bt_info == BT_INFO_8192E_2ANT_B_CONNECTION) {
+		coex_dm->bt_status = BT_8192E_2ANT_BT_STATUS_CONNECTED_IDLE;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], bt_infoNotify(), BT Connected-idle!!!\n");
+	} else if ((bt_info&BT_INFO_8192E_2ANT_B_SCO_ESCO) ||
+		   (bt_info&BT_INFO_8192E_2ANT_B_SCO_BUSY)) {
+		coex_dm->bt_status = BT_8192E_2ANT_BT_STATUS_SCO_BUSY;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], bt_infoNotify(), BT SCO busy!!!\n");
+	} else if (bt_info&BT_INFO_8192E_2ANT_B_ACL_BUSY) {
+		coex_dm->bt_status = BT_8192E_2ANT_BT_STATUS_ACL_BUSY;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], bt_infoNotify(), BT ACL busy!!!\n");
+	} else {
+		coex_dm->bt_status = BT_8192E_2ANT_BT_STATUS_MAX;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex]bt_infoNotify(), BT Non-Defined state!!!\n");
+	}
+
+	if ((BT_8192E_2ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) ||
+	    (BT_8192E_2ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
+	    (BT_8192E_2ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status)) {
+		bt_busy = true;
+		limited_dig = true;
+	} else {
+		bt_busy = false;
+		limited_dig = false;
+	}
+
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bt_busy);
+
+	coex_dm->limited_dig = limited_dig;
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_LIMITED_DIG, &limited_dig);
+
+	halbtc8192e2ant_run_coexist_mechanism(btcoexist);
+}
+
+void ex_halbtc8192e2ant_stack_operation_notify(struct btc_coexist *btcoexist,
+					       u8 type)
+{
+	if (BTC_STACK_OP_INQ_PAGE_PAIR_START == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+		          "[BTCoex] StackOP Inquiry/page/pair start notify\n");
+	else if(BTC_STACK_OP_INQ_PAGE_PAIR_FINISH == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+		          "[BTCoex] StackOP Inquiry/page/pair finish notify\n");
+}
+
+void ex_halbtc8192e2ant_halt_notify(struct btc_coexist *btcoexist)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, "[BTCoex], Halt notify\n");
+
+	halbtc8192e2ant_IgnoreWlanAct(btcoexist, FORCE_EXEC, true);
+	ex_halbtc8192e2ant_media_status_notify(btcoexist, BTC_MEDIA_DISCONNECT);
+}
+
+void ex_halbtc8192e2ant_periodical(struct btc_coexist *btcoexist)
+{
+	static u8 dis_ver_info_cnt = 0;
+	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);
+	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
new file mode 100644
index 000000000000..6d109edb8950
--- /dev/null
+++ b/drivers/staging/rtl8821ae/btcoexist/halbtc8192e2ant.h
@@ -0,0 +1,162 @@
+/*****************************************************************
+ *   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
+#define	BT_INFO_8192E_2ANT_B_SCO_BUSY			BIT4
+#define	BT_INFO_8192E_2ANT_B_ACL_BUSY			BIT3
+#define	BT_INFO_8192E_2ANT_B_INQ_PAGE			BIT2
+#define	BT_INFO_8192E_2ANT_B_SCO_ESCO			BIT1
+#define	BT_INFO_8192E_2ANT_B_CONNECTION			BIT0
+
+#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/rtl8821ae/btcoexist/halbtc8723a2ant.c b/drivers/staging/rtl8821ae/btcoexist/halbtc8723a2ant.c
new file mode 100644
index 000000000000..180d6f12e7b5
--- /dev/null
+++ b/drivers/staging/rtl8821ae/btcoexist/halbtc8723a2ant.c
@@ -0,0 +1,3780 @@
+//============================================================
+// Description:
+//
+// This file is for RTL8723A Co-exist mechanism
+//
+// History
+// 2012/08/22 Cosa first check in.
+// 2012/11/14 Cosa Revise for 8723A 2Ant out sourcing.
+//
+//============================================================
+
+//============================================================
+// include files
+//============================================================
+#include "Mp_Precomp.h"
+#if(BT_30_SUPPORT == 1)
+//============================================================
+// Global variables, these are static variables
+//============================================================
+static COEX_DM_8723A_2ANT	GLCoexDm8723a2Ant;
+static PCOEX_DM_8723A_2ANT 	pCoexDm=&GLCoexDm8723a2Ant;
+static COEX_STA_8723A_2ANT	GLCoexSta8723a2Ant;
+static PCOEX_STA_8723A_2ANT	pCoexSta=&GLCoexSta8723a2Ant;
+
+const char *const GLBtInfoSrc8723a2Ant[]={
+	"BT Info[wifi fw]",
+	"BT Info[bt rsp]",
+	"BT Info[bt auto report]",
+};
+
+//============================================================
+// local function proto type if needed
+//============================================================
+//============================================================
+// local function start with halbtc8723a2ant_
+//============================================================
+BOOLEAN
+halbtc8723a2ant_IsWifiIdle(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	BOOLEAN		bWifiConnected=FALSE, bScan=FALSE, bLink=FALSE, bRoam=FALSE;
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_SCAN, &bScan);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_LINK, &bLink);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_ROAM, &bRoam);
+
+	if(bWifiConnected)
+		return FALSE;
+	if(bScan)
+		return FALSE;
+	if(bLink)
+		return FALSE;
+	if(bRoam)
+		return FALSE;
+
+	return true;
+}
+
+u1Byte
+halbtc8723a2ant_BtRssiState(
+	u1Byte			levelNum,
+	u1Byte			rssiThresh,
+	u1Byte			rssiThresh1
+	)
+{
+	s4Byte			btRssi=0;
+	u1Byte			btRssiState=pCoexSta->preBtRssiState;
+
+	btRssi = pCoexSta->btRssi;
+
+	if(levelNum == 2)
+	{			
+		if( (pCoexSta->preBtRssiState == BTC_RSSI_STATE_LOW) ||
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_STAY_LOW))
+		{
+			if(btRssi >= (rssiThresh+BTC_RSSI_COEX_THRESH_TOL_8723A_2ANT))
+			{
+				btRssiState = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to High\n"));
+			}
+			else
+			{
+				btRssiState = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at Low\n"));
+			}
+		}
+		else
+		{
+			if(btRssi < rssiThresh)
+			{
+				btRssiState = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Low\n"));
+			}
+			else
+			{
+				btRssiState = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at High\n"));
+			}
+		}
+	}
+	else if(levelNum == 3)
+	{
+		if(rssiThresh > rssiThresh1)
+		{
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi thresh error!!\n"));
+			return pCoexSta->preBtRssiState;
+		}
+		
+		if( (pCoexSta->preBtRssiState == BTC_RSSI_STATE_LOW) ||
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_STAY_LOW))
+		{
+			if(btRssi >= (rssiThresh+BTC_RSSI_COEX_THRESH_TOL_8723A_2ANT))
+			{
+				btRssiState = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Medium\n"));
+			}
+			else
+			{
+				btRssiState = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at Low\n"));
+			}
+		}
+		else if( (pCoexSta->preBtRssiState == BTC_RSSI_STATE_MEDIUM) ||
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_STAY_MEDIUM))
+		{
+			if(btRssi >= (rssiThresh1+BTC_RSSI_COEX_THRESH_TOL_8723A_2ANT))
+			{
+				btRssiState = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to High\n"));
+			}
+			else if(btRssi < rssiThresh)
+			{
+				btRssiState = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Low\n"));
+			}
+			else
+			{
+				btRssiState = BTC_RSSI_STATE_STAY_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at Medium\n"));
+			}
+		}
+		else
+		{
+			if(btRssi < rssiThresh1)
+			{
+				btRssiState = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Medium\n"));
+			}
+			else
+			{
+				btRssiState = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at High\n"));
+			}
+		}
+	}
+		
+	pCoexSta->preBtRssiState = btRssiState;
+
+	return btRssiState;
+}
+
+u1Byte
+halbtc8723a2ant_WifiRssiState(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			index,
+	IN	u1Byte			levelNum,
+	IN	u1Byte			rssiThresh,
+	IN	u1Byte			rssiThresh1
+	)
+{
+	s4Byte			wifiRssi=0;
+	u1Byte			wifiRssiState=pCoexSta->preWifiRssiState[index];
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_S4_WIFI_RSSI, &wifiRssi);
+	
+	if(levelNum == 2)
+	{
+		if( (pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_LOW) ||
+			(pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_STAY_LOW))
+		{
+			if(wifiRssi >= (rssiThresh+BTC_RSSI_COEX_THRESH_TOL_8723A_2ANT))
+			{
+				wifiRssiState = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to High\n"));
+			}
+			else
+			{
+				wifiRssiState = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at Low\n"));
+			}
+		}
+		else
+		{
+			if(wifiRssi < rssiThresh)
+			{
+				wifiRssiState = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Low\n"));
+			}
+			else
+			{
+				wifiRssiState = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at High\n"));
+			}
+		}
+	}
+	else if(levelNum == 3)
+	{
+		if(rssiThresh > rssiThresh1)
+		{
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI thresh error!!\n"));
+			return pCoexSta->preWifiRssiState[index];
+		}
+		
+		if( (pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_LOW) ||
+			(pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_STAY_LOW))
+		{
+			if(wifiRssi >= (rssiThresh+BTC_RSSI_COEX_THRESH_TOL_8723A_2ANT))
+			{
+				wifiRssiState = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Medium\n"));
+			}
+			else
+			{
+				wifiRssiState = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at Low\n"));
+			}
+		}
+		else if( (pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_MEDIUM) ||
+			(pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_STAY_MEDIUM))
+		{
+			if(wifiRssi >= (rssiThresh1+BTC_RSSI_COEX_THRESH_TOL_8723A_2ANT))
+			{
+				wifiRssiState = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to High\n"));
+			}
+			else if(wifiRssi < rssiThresh)
+			{
+				wifiRssiState = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Low\n"));
+			}
+			else
+			{
+				wifiRssiState = BTC_RSSI_STATE_STAY_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at Medium\n"));
+			}
+		}
+		else
+		{
+			if(wifiRssi < rssiThresh1)
+			{
+				wifiRssiState = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Medium\n"));
+			}
+			else
+			{
+				wifiRssiState = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at High\n"));
+			}
+		}
+	}
+		
+	pCoexSta->preWifiRssiState[index] = wifiRssiState;
+
+	return wifiRssiState;
+}
+
+VOID
+halbtc8723a2ant_IndicateWifiChnlBwInfo(
+	IN	PBTC_COEXIST			pBtCoexist,
+	IN	u1Byte				type
+	)
+{
+	u1Byte			H2C_Parameter[3] ={0};
+	u4Byte			wifiBw;
+	u1Byte			wifiCentralChnl;
+	
+	// only 2.4G we need to inform bt the chnl mask
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U1_WIFI_CENTRAL_CHNL, &wifiCentralChnl);
+	if( (BTC_MEDIA_CONNECT == type) &&
+		(wifiCentralChnl <= 14) )
+	{
+		H2C_Parameter[0] = 0x1;
+		H2C_Parameter[1] = wifiCentralChnl;
+		pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+		if(BTC_WIFI_BW_HT40 == wifiBw)
+			H2C_Parameter[2] = 0x30;
+		else
+			H2C_Parameter[2] = 0x20;
+	}
+		
+	pCoexDm->wifiChnlInfo[0] = H2C_Parameter[0];
+	pCoexDm->wifiChnlInfo[1] = H2C_Parameter[1];
+	pCoexDm->wifiChnlInfo[2] = H2C_Parameter[2];
+	
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], FW write 0x19=0x%x\n", 
+		H2C_Parameter[0]<<16|H2C_Parameter[1]<<8|H2C_Parameter[2]));
+
+	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x19, 3, H2C_Parameter);
+}
+
+VOID
+halbtc8723a2ant_QueryBtInfo(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	u1Byte			H2C_Parameter[1] ={0};
+
+	pCoexSta->bC2hBtInfoReqSent = true;
+
+	H2C_Parameter[0] |= BIT0;	// trigger
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], Query Bt Info, FW write 0x38=0x%x\n", 
+		H2C_Parameter[0]));
+
+	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x38, 1, H2C_Parameter);
+}
+u1Byte
+halbtc8723a2ant_ActionAlgorithm(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	PBTC_STACK_INFO		pStackInfo=&pBtCoexist->stack_info;
+	BOOLEAN				bBtHsOn=FALSE, bBtBusy=FALSE, limited_dig=FALSE;
+	u1Byte				algorithm=BT_8723A_2ANT_COEX_ALGO_UNDEFINED;
+	u1Byte				numOfDiffProfile=0;
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+
+	//======================
+	// here we get BT status first
+	//======================
+	pCoexDm->btStatus = BT_8723A_2ANT_BT_STATUS_IDLE;
+	
+	if((pStackInfo->bScoExist) ||(bBtHsOn) ||(pStackInfo->bHidExist))
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO or HID or HS exists, set BT non-idle !!!\n"));
+		pCoexDm->btStatus = BT_8723A_2ANT_BT_STATUS_NON_IDLE;
+	}
+	else
+	{
+		// A2dp profile
+		if( (pBtCoexist->stack_info.numOfLink == 1) &&
+			(pStackInfo->bA2dpExist) )
+		{		
+			if( (pCoexSta->lowPriorityTx+	pCoexSta->lowPriorityRx) < 100)
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], A2DP, low priority tx+rx < 100, set BT connected-idle!!!\n"));
+				pCoexDm->btStatus = BT_8723A_2ANT_BT_STATUS_CONNECTED_IDLE;
+			}
+			else
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], A2DP, low priority tx+rx >= 100, set BT non-idle!!!\n"));
+				pCoexDm->btStatus = BT_8723A_2ANT_BT_STATUS_NON_IDLE;
+			}
+		}
+		// Pan profile
+		if( (pBtCoexist->stack_info.numOfLink == 1) &&
+			(pStackInfo->bPanExist) )
+		{		
+			if((pCoexSta->lowPriorityTx+	pCoexSta->lowPriorityRx) < 600)
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PAN, low priority tx+rx < 600, set BT connected-idle!!!\n"));
+				pCoexDm->btStatus = BT_8723A_2ANT_BT_STATUS_CONNECTED_IDLE;
+			}
+			else
+			{
+				if(pCoexSta->lowPriorityTx)
+				{
+					if((pCoexSta->lowPriorityRx /pCoexSta->lowPriorityTx)>9 )
+					{
+						BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PAN, low priority rx/tx > 9, set BT connected-idle!!!\n"));
+						pCoexDm->btStatus = BT_8723A_2ANT_BT_STATUS_CONNECTED_IDLE;
+					}
+				}
+			}
+			if(BT_8723A_2ANT_BT_STATUS_CONNECTED_IDLE != pCoexDm->btStatus)
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PAN, set BT non-idle!!!\n"));
+				pCoexDm->btStatus = BT_8723A_2ANT_BT_STATUS_NON_IDLE;
+			}
+		}
+		// Pan+A2dp profile
+		if( (pBtCoexist->stack_info.numOfLink == 2) &&
+			(pStackInfo->bA2dpExist) &&
+			(pStackInfo->bPanExist) )
+		{
+			if((pCoexSta->lowPriorityTx+	pCoexSta->lowPriorityRx) < 600)
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PAN+A2DP, low priority tx+rx < 600, set BT connected-idle!!!\n"));
+				pCoexDm->btStatus = BT_8723A_2ANT_BT_STATUS_CONNECTED_IDLE;
+			}
+			else
+			{
+				if(pCoexSta->lowPriorityTx)
+				{
+					if((pCoexSta->lowPriorityRx /pCoexSta->lowPriorityTx)>9 )
+					{
+						BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PAN+A2DP, low priority rx/tx > 9, set BT connected-idle!!!\n"));
+						pCoexDm->btStatus = BT_8723A_2ANT_BT_STATUS_CONNECTED_IDLE;
+					}
+				}
+			}
+			if(BT_8723A_2ANT_BT_STATUS_CONNECTED_IDLE != pCoexDm->btStatus)
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PAN+A2DP, set BT non-idle!!!\n"));
+				pCoexDm->btStatus = BT_8723A_2ANT_BT_STATUS_NON_IDLE;
+			}
+		}
+	}
+	if(BT_8723A_2ANT_BT_STATUS_IDLE != pCoexDm->btStatus)
+	{
+		bBtBusy = true;
+		limited_dig = true;
+	}
+	else
+	{
+		bBtBusy = FALSE;
+		limited_dig = FALSE;
+	}
+	pBtCoexist->btc_set(pBtCoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bBtBusy);
+	pBtCoexist->btc_set(pBtCoexist, BTC_SET_BL_BT_LIMITED_DIG, &limited_dig);
+	//======================
+
+	if(!pStackInfo->bBtLinkExist)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], No profile exists!!!\n"));
+		return algorithm;
+	}
+
+	if(pStackInfo->bScoExist)
+		numOfDiffProfile++;
+	if(pStackInfo->bHidExist)
+		numOfDiffProfile++;
+	if(pStackInfo->bPanExist)
+		numOfDiffProfile++;
+	if(pStackInfo->bA2dpExist)
+		numOfDiffProfile++;
+	
+	if(numOfDiffProfile == 1)
+	{
+		if(pStackInfo->bScoExist)
+		{
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO only\n"));
+			algorithm = BT_8723A_2ANT_COEX_ALGO_SCO;
+		}
+		else
+		{
+			if(pStackInfo->bHidExist)
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID only\n"));
+				algorithm = BT_8723A_2ANT_COEX_ALGO_HID;
+			}
+			else if(pStackInfo->bA2dpExist)
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], A2DP only\n"));
+				algorithm = BT_8723A_2ANT_COEX_ALGO_A2DP;
+			}
+			else if(pStackInfo->bPanExist)
+			{
+				if(bBtHsOn)
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PAN(HS) only\n"));
+					algorithm = BT_8723A_2ANT_COEX_ALGO_PANHS;
+				}
+				else
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PAN(EDR) only\n"));
+					algorithm = BT_8723A_2ANT_COEX_ALGO_PANEDR;
+				}
+			}
+		}
+	}
+	else if(numOfDiffProfile == 2)
+	{
+		if(pStackInfo->bScoExist)
+		{
+			if(pStackInfo->bHidExist)
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID\n"));
+				algorithm = BT_8723A_2ANT_COEX_ALGO_HID;
+			}
+			else if(pStackInfo->bA2dpExist)
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + A2DP ==> SCO\n"));
+				algorithm = BT_8723A_2ANT_COEX_ALGO_SCO;
+			}
+			else if(pStackInfo->bPanExist)
+			{
+				if(bBtHsOn)
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + PAN(HS)\n"));
+					algorithm = BT_8723A_2ANT_COEX_ALGO_SCO;
+				}
+				else
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + PAN(EDR)\n"));
+					algorithm = BT_8723A_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		}
+		else
+		{
+			if( pStackInfo->bHidExist &&
+				pStackInfo->bA2dpExist )
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + A2DP\n"));
+				algorithm = BT_8723A_2ANT_COEX_ALGO_HID_A2DP;
+			}
+			else if( pStackInfo->bHidExist &&
+				pStackInfo->bPanExist )
+			{
+				if(bBtHsOn)
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + PAN(HS)\n"));
+					algorithm = BT_8723A_2ANT_COEX_ALGO_HID_A2DP;
+				}
+				else
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + PAN(EDR)\n"));
+					algorithm = BT_8723A_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+			else if( pStackInfo->bPanExist &&
+				pStackInfo->bA2dpExist )
+			{
+				if(bBtHsOn)
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], A2DP + PAN(HS)\n"));
+					algorithm = BT_8723A_2ANT_COEX_ALGO_A2DP;
+				}
+				else
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], A2DP + PAN(EDR)\n"));
+					algorithm = BT_8723A_2ANT_COEX_ALGO_PANEDR_A2DP;
+				}
+			}
+		}
+	}
+	else if(numOfDiffProfile == 3)
+	{
+		if(pStackInfo->bScoExist)
+		{
+			if( pStackInfo->bHidExist &&
+				pStackInfo->bA2dpExist )
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID + A2DP ==> HID\n"));
+				algorithm = BT_8723A_2ANT_COEX_ALGO_HID;
+			}
+			else if( pStackInfo->bHidExist &&
+				pStackInfo->bPanExist )
+			{
+				if(bBtHsOn)
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID + PAN(HS)\n"));
+					algorithm = BT_8723A_2ANT_COEX_ALGO_HID_A2DP;
+				}
+				else
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID + PAN(EDR)\n"));
+					algorithm = BT_8723A_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+			else if( pStackInfo->bPanExist &&
+				pStackInfo->bA2dpExist )
+			{
+				if(bBtHsOn)
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + A2DP + PAN(HS)\n"));
+					algorithm = BT_8723A_2ANT_COEX_ALGO_SCO;
+				}
+				else
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + A2DP + PAN(EDR) ==> HID\n"));
+					algorithm = BT_8723A_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		}
+		else
+		{
+			if( pStackInfo->bHidExist &&
+				pStackInfo->bPanExist &&
+				pStackInfo->bA2dpExist )
+			{
+				if(bBtHsOn)
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + A2DP + PAN(HS)\n"));
+					algorithm = BT_8723A_2ANT_COEX_ALGO_HID_A2DP;
+				}
+				else
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + A2DP + PAN(EDR)\n"));
+					algorithm = BT_8723A_2ANT_COEX_ALGO_HID_A2DP_PANEDR;
+				}
+			}
+		}
+	}
+	else if(numOfDiffProfile >= 3)
+	{
+		if(pStackInfo->bScoExist)
+		{
+			if( pStackInfo->bHidExist &&
+				pStackInfo->bPanExist &&
+				pStackInfo->bA2dpExist )
+			{
+				if(bBtHsOn)
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Error!!! SCO + HID + A2DP + PAN(HS)\n"));
+
+				}
+				else
+				{
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID + A2DP + PAN(EDR)==>PAN(EDR)+HID\n"));
+					algorithm = BT_8723A_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		}
+	}
+
+	return algorithm;
+}
+
+BOOLEAN
+halbtc8723a2ant_NeedToDecBtPwr(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	BOOLEAN		bRet=FALSE;
+	BOOLEAN		bBtHsOn=FALSE, bWifiConnected=FALSE;
+	s4Byte		btHsRssi=0;
+
+	if(!pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn))
+		return FALSE;
+	if(!pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected))
+		return FALSE;
+	if(!pBtCoexist->btc_get(pBtCoexist, BTC_GET_S4_HS_RSSI, &btHsRssi))
+		return FALSE;
+
+	if(bWifiConnected)
+	{
+		if(bBtHsOn)
+		{
+			if(btHsRssi > 37)
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], Need to decrease bt power for HS mode!!\n"));
+				bRet = true;
+			}
+		}
+		else
+		{
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], Need to decrease bt power for Wifi is connected!!\n"));
+			bRet = true;
+		}
+	}
+	
+	return bRet;
+}
+
+VOID
+halbtc8723a2ant_SetFwDacSwingLevel(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			dacSwingLvl
+	)
+{
+	u1Byte			H2C_Parameter[1] ={0};
+
+	// There are several type of dacswing
+	// 0x18/ 0x10/ 0xc/ 0x8/ 0x4/ 0x6
+	H2C_Parameter[0] = dacSwingLvl;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], Set Dac Swing Level=0x%x\n", dacSwingLvl));
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], FW write 0x29=0x%x\n", H2C_Parameter[0]));
+
+	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x29, 1, H2C_Parameter);
+}
+
+VOID
+halbtc8723a2ant_SetFwDecBtPwr(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bDecBtPwr
+	)
+{
+	u1Byte			H2C_Parameter[1] ={0};
+	
+	H2C_Parameter[0] = 0;
+
+	if(bDecBtPwr)
+	{
+		H2C_Parameter[0] |= BIT1;
+	}
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], decrease Bt Power : %s, FW write 0x21=0x%x\n", 
+		(bDecBtPwr? "Yes!!":"No!!"), H2C_Parameter[0]));
+
+	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x21, 1, H2C_Parameter);	
+}
+
+VOID
+halbtc8723a2ant_DecBtPwr(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bForceExec,
+	IN	BOOLEAN			bDecBtPwr
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s Dec BT power = %s\n",  
+		(bForceExec? "force to":""), ((bDecBtPwr)? "ON":"OFF")));
+	pCoexDm->bCurDecBtPwr = bDecBtPwr;
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], bPreDecBtPwr=%d, bCurDecBtPwr=%d\n", 
+			pCoexDm->bPreDecBtPwr, pCoexDm->bCurDecBtPwr));
+
+		if(pCoexDm->bPreDecBtPwr == pCoexDm->bCurDecBtPwr) 
+			return;
+	}
+	halbtc8723a2ant_SetFwDecBtPwr(pBtCoexist, pCoexDm->bCurDecBtPwr);
+
+	pCoexDm->bPreDecBtPwr = pCoexDm->bCurDecBtPwr;
+}
+
+VOID
+halbtc8723a2ant_FwDacSwingLvl(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bForceExec,
+	IN	u1Byte			fwDacSwingLvl
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s set FW Dac Swing level = %d\n",  
+		(bForceExec? "force to":""), fwDacSwingLvl));
+	pCoexDm->curFwDacSwingLvl = fwDacSwingLvl;
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], preFwDacSwingLvl=%d, curFwDacSwingLvl=%d\n", 
+			pCoexDm->preFwDacSwingLvl, pCoexDm->curFwDacSwingLvl));
+
+		if(pCoexDm->preFwDacSwingLvl == pCoexDm->curFwDacSwingLvl) 
+			return;
+	}
+
+	halbtc8723a2ant_SetFwDacSwingLevel(pBtCoexist, pCoexDm->curFwDacSwingLvl);
+
+	pCoexDm->preFwDacSwingLvl = pCoexDm->curFwDacSwingLvl;
+}
+
+VOID
+halbtc8723a2ant_SetSwRfRxLpfCorner(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bRxRfShrinkOn
+	)
+{
+	if(bRxRfShrinkOn)
+	{
+		//Shrink RF Rx LPF corner
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Shrink RF Rx LPF corner!!\n"));
+		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x1e, 0xfffff, 0xf0ff7);
+	}
+	else
+	{
+		//Resume RF Rx LPF corner
+		// After initialized, we can use pCoexDm->btRf0x1eBackup
+		if(pBtCoexist->initilized)
+		{
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Resume RF Rx LPF corner!!\n"));
+			pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x1e, 0xfffff, pCoexDm->btRf0x1eBackup);
+		}
+	}
+}
+
+VOID
+halbtc8723a2ant_RfShrink(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bForceExec,
+	IN	BOOLEAN			bRxRfShrinkOn
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s turn Rx RF Shrink = %s\n",  
+		(bForceExec? "force to":""), ((bRxRfShrinkOn)? "ON":"OFF")));
+	pCoexDm->bCurRfRxLpfShrink = bRxRfShrinkOn;
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], bPreRfRxLpfShrink=%d, bCurRfRxLpfShrink=%d\n", 
+			pCoexDm->bPreRfRxLpfShrink, pCoexDm->bCurRfRxLpfShrink));
+
+		if(pCoexDm->bPreRfRxLpfShrink == pCoexDm->bCurRfRxLpfShrink) 
+			return;
+	}
+	halbtc8723a2ant_SetSwRfRxLpfCorner(pBtCoexist, pCoexDm->bCurRfRxLpfShrink);
+
+	pCoexDm->bPreRfRxLpfShrink = pCoexDm->bCurRfRxLpfShrink;
+}
+
+VOID
+halbtc8723a2ant_SetSwPenaltyTxRateAdaptive(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bLowPenaltyRa
+	)
+{
+	u1Byte	tmpU1;
+
+	tmpU1 = pBtCoexist->btc_read_1byte(pBtCoexist, 0x4fd);
+	tmpU1 |= BIT0;
+	if(bLowPenaltyRa)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Tx rate adaptive, set low penalty!!\n"));
+		tmpU1 &= ~BIT2;
+	}
+	else
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Tx rate adaptive, set normal!!\n"));
+		tmpU1 |= BIT2;
+	}
+
+	pBtCoexist->btc_write_1byte(pBtCoexist, 0x4fd, tmpU1);
+}
+
+VOID
+halbtc8723a2ant_LowPenaltyRa(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bForceExec,
+	IN	BOOLEAN			bLowPenaltyRa
+	)
+{
+	return;
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s turn LowPenaltyRA = %s\n",  
+		(bForceExec? "force to":""), ((bLowPenaltyRa)? "ON":"OFF")));
+	pCoexDm->bCurLowPenaltyRa = bLowPenaltyRa;
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], bPreLowPenaltyRa=%d, bCurLowPenaltyRa=%d\n", 
+			pCoexDm->bPreLowPenaltyRa, pCoexDm->bCurLowPenaltyRa));
+
+		if(pCoexDm->bPreLowPenaltyRa == pCoexDm->bCurLowPenaltyRa) 
+			return;
+	}
+	halbtc8723a2ant_SetSwPenaltyTxRateAdaptive(pBtCoexist, pCoexDm->bCurLowPenaltyRa);
+
+	pCoexDm->bPreLowPenaltyRa = pCoexDm->bCurLowPenaltyRa;
+}
+
+VOID
+halbtc8723a2ant_SetSwFullTimeDacSwing(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bSwDacSwingOn,
+	IN	u4Byte			swDacSwingLvl
+	)
+{
+	if(bSwDacSwingOn)
+	{
+		pBtCoexist->btc_setBbReg(pBtCoexist, 0x880, 0xff000000, swDacSwingLvl);
+	}
+	else
+	{
+		pBtCoexist->btc_setBbReg(pBtCoexist, 0x880, 0xff000000, 0xc0);
+	}
+}
+
+
+VOID
+halbtc8723a2ant_DacSwing(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bForceExec,
+	IN	BOOLEAN			bDacSwingOn,
+	IN	u4Byte			dacSwingLvl
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s turn DacSwing=%s, dacSwingLvl=0x%x\n",  
+		(bForceExec? "force to":""), ((bDacSwingOn)? "ON":"OFF"), dacSwingLvl));
+	pCoexDm->bCurDacSwingOn = bDacSwingOn;
+	pCoexDm->curDacSwingLvl = dacSwingLvl;
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], bPreDacSwingOn=%d, preDacSwingLvl=0x%x, bCurDacSwingOn=%d, curDacSwingLvl=0x%x\n", 
+			pCoexDm->bPreDacSwingOn, pCoexDm->preDacSwingLvl,
+			pCoexDm->bCurDacSwingOn, pCoexDm->curDacSwingLvl));
+
+		if( (pCoexDm->bPreDacSwingOn == pCoexDm->bCurDacSwingOn) &&
+			(pCoexDm->preDacSwingLvl == pCoexDm->curDacSwingLvl) )
+			return;
+	}
+	mdelay(30);
+	halbtc8723a2ant_SetSwFullTimeDacSwing(pBtCoexist, bDacSwingOn, dacSwingLvl);
+
+	pCoexDm->bPreDacSwingOn = pCoexDm->bCurDacSwingOn;
+	pCoexDm->preDacSwingLvl = pCoexDm->curDacSwingLvl;
+}
+
+VOID
+halbtc8723a2ant_SetAdcBackOff(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bAdcBackOff
+	)
+{
+	if(bAdcBackOff)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], BB BackOff Level On!\n"));
+		pBtCoexist->btc_write_4byte(pBtCoexist, 0xc04,0x3a07611);
+	}
+	else
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], BB BackOff Level Off!\n"));		
+		pBtCoexist->btc_write_4byte(pBtCoexist, 0xc04,0x3a05611);
+	}
+}
+
+VOID
+halbtc8723a2ant_AdcBackOff(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bForceExec,
+	IN	BOOLEAN			bAdcBackOff
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s turn AdcBackOff = %s\n",  
+		(bForceExec? "force to":""), ((bAdcBackOff)? "ON":"OFF")));
+	pCoexDm->bCurAdcBackOff = bAdcBackOff;
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], bPreAdcBackOff=%d, bCurAdcBackOff=%d\n", 
+			pCoexDm->bPreAdcBackOff, pCoexDm->bCurAdcBackOff));
+
+		if(pCoexDm->bPreAdcBackOff == pCoexDm->bCurAdcBackOff) 
+			return;
+	}
+	halbtc8723a2ant_SetAdcBackOff(pBtCoexist, pCoexDm->bCurAdcBackOff);
+
+	pCoexDm->bPreAdcBackOff = pCoexDm->bCurAdcBackOff;
+}
+
+VOID
+halbtc8723a2ant_SetAgcTable(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bAgcTableEn
+	)
+{
+	u1Byte		rssiAdjustVal=0;
+
+	if(bAgcTableEn)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Agc Table On!\n"));
+		pBtCoexist->btc_write_4byte(pBtCoexist, 0xc78,0x4e1c0001);
+		pBtCoexist->btc_write_4byte(pBtCoexist, 0xc78,0x4d1d0001);
+		pBtCoexist->btc_write_4byte(pBtCoexist, 0xc78,0x4c1e0001);
+		pBtCoexist->btc_write_4byte(pBtCoexist, 0xc78,0x4b1f0001);
+		pBtCoexist->btc_write_4byte(pBtCoexist, 0xc78,0x4a200001);
+		
+		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x12, 0xfffff, 0xdc000);
+		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x12, 0xfffff, 0x90000);
+		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x12, 0xfffff, 0x51000);
+		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x12, 0xfffff, 0x12000);
+		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x1a, 0xfffff, 0x00355);
+		
+		rssiAdjustVal = 6;
+	}
+	else
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Agc Table Off!\n"));
+		pBtCoexist->btc_write_4byte(pBtCoexist, 0xc78,0x641c0001);
+		pBtCoexist->btc_write_4byte(pBtCoexist, 0xc78,0x631d0001);
+		pBtCoexist->btc_write_4byte(pBtCoexist, 0xc78,0x621e0001);
+		pBtCoexist->btc_write_4byte(pBtCoexist, 0xc78,0x611f0001);
+		pBtCoexist->btc_write_4byte(pBtCoexist, 0xc78,0x60200001);
+
+		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x12, 0xfffff, 0x32000);
+		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x12, 0xfffff, 0x71000);
+		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x12, 0xfffff, 0xb0000);
+		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x12, 0xfffff, 0xfc000);
+		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x1a, 0xfffff, 0x30355);
+	}
+
+	// set rssiAdjustVal for wifi module.
+	pBtCoexist->btc_set(pBtCoexist, BTC_SET_U1_RSSI_ADJ_VAL_FOR_AGC_TABLE_ON, &rssiAdjustVal);
+}
+
+
+VOID
+halbtc8723a2ant_AgcTable(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bForceExec,
+	IN	BOOLEAN			bAgcTableEn
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s %s Agc Table\n",  
+		(bForceExec? "force to":""), ((bAgcTableEn)? "Enable":"Disable")));
+	pCoexDm->bCurAgcTableEn = bAgcTableEn;
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], bPreAgcTableEn=%d, bCurAgcTableEn=%d\n", 
+			pCoexDm->bPreAgcTableEn, pCoexDm->bCurAgcTableEn));
+
+		if(pCoexDm->bPreAgcTableEn == pCoexDm->bCurAgcTableEn) 
+			return;
+	}
+	halbtc8723a2ant_SetAgcTable(pBtCoexist, bAgcTableEn);
+
+	pCoexDm->bPreAgcTableEn = pCoexDm->bCurAgcTableEn;
+}
+
+VOID
+halbtc8723a2ant_SetCoexTable(
+	IN	PBTC_COEXIST	pBtCoexist,
+	IN	u4Byte		val0x6c0,
+	IN	u4Byte		val0x6c8,
+	IN	u1Byte		val0x6cc
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], set coex table, set 0x6c0=0x%x\n", val0x6c0));
+	pBtCoexist->btc_write_4byte(pBtCoexist, 0x6c0, val0x6c0);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], set coex table, set 0x6c8=0x%x\n", val0x6c8));
+	pBtCoexist->btc_write_4byte(pBtCoexist, 0x6c8, val0x6c8);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], set coex table, set 0x6cc=0x%x\n", val0x6cc));
+	pBtCoexist->btc_write_1byte(pBtCoexist, 0x6cc, val0x6cc);
+}
+
+VOID
+halbtc8723a2ant_CoexTable(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bForceExec,
+	IN	u4Byte			val0x6c0,
+	IN	u4Byte			val0x6c8,
+	IN	u1Byte			val0x6cc
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s write Coex Table 0x6c0=0x%x, 0x6c8=0x%x, 0x6cc=0x%x\n", 
+		(bForceExec? "force to":""), val0x6c0, val0x6c8, val0x6cc));
+	pCoexDm->curVal0x6c0 = val0x6c0;
+	pCoexDm->curVal0x6c8 = val0x6c8;
+	pCoexDm->curVal0x6cc = val0x6cc;
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], preVal0x6c0=0x%x, preVal0x6c8=0x%x, preVal0x6cc=0x%x !!\n", 
+			pCoexDm->preVal0x6c0, pCoexDm->preVal0x6c8, pCoexDm->preVal0x6cc));
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], curVal0x6c0=0x%x, curVal0x6c8=0x%x, curVal0x6cc=0x%x !!\n", 
+			pCoexDm->curVal0x6c0, pCoexDm->curVal0x6c8, pCoexDm->curVal0x6cc));
+	
+		if( (pCoexDm->preVal0x6c0 == pCoexDm->curVal0x6c0) &&
+			(pCoexDm->preVal0x6c8 == pCoexDm->curVal0x6c8) &&
+			(pCoexDm->preVal0x6cc == pCoexDm->curVal0x6cc) )
+			return;
+	}
+	halbtc8723a2ant_SetCoexTable(pBtCoexist, val0x6c0, val0x6c8, val0x6cc);
+
+	pCoexDm->preVal0x6c0 = pCoexDm->curVal0x6c0;
+	pCoexDm->preVal0x6c8 = pCoexDm->curVal0x6c8;
+	pCoexDm->preVal0x6cc = pCoexDm->curVal0x6cc;
+}
+
+VOID
+halbtc8723a2ant_SetFwIgnoreWlanAct(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bEnable
+	)
+{
+	u1Byte			H2C_Parameter[1] ={0};
+		
+	if(bEnable)
+	{
+		H2C_Parameter[0] |= BIT0;		// function enable
+	}
+	
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], set FW for BT Ignore Wlan_Act, FW write 0x25=0x%x\n", 
+		H2C_Parameter[0]));
+
+	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x25, 1, H2C_Parameter);	
+}
+
+VOID
+halbtc8723a2ant_IgnoreWlanAct(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bForceExec,
+	IN	BOOLEAN			bEnable
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s turn Ignore WlanAct %s\n", 
+		(bForceExec? "force to":""), (bEnable? "ON":"OFF")));
+	pCoexDm->bCurIgnoreWlanAct = bEnable;
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], bPreIgnoreWlanAct = %d, bCurIgnoreWlanAct = %d!!\n", 
+			pCoexDm->bPreIgnoreWlanAct, pCoexDm->bCurIgnoreWlanAct));
+
+		if(pCoexDm->bPreIgnoreWlanAct == pCoexDm->bCurIgnoreWlanAct)
+			return;
+	}
+	halbtc8723a2ant_SetFwIgnoreWlanAct(pBtCoexist, bEnable);
+
+	pCoexDm->bPreIgnoreWlanAct = pCoexDm->bCurIgnoreWlanAct;
+}
+
+VOID
+halbtc8723a2ant_SetFwPstdma(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			byte1,
+	IN	u1Byte			byte2,
+	IN	u1Byte			byte3,
+	IN	u1Byte			byte4,
+	IN	u1Byte			byte5
+	)
+{
+	u1Byte			H2C_Parameter[5] ={0};
+
+	H2C_Parameter[0] = byte1;	
+	H2C_Parameter[1] = byte2;	
+	H2C_Parameter[2] = byte3;
+	H2C_Parameter[3] = byte4;
+	H2C_Parameter[4] = byte5;
+
+	pCoexDm->psTdmaPara[0] = byte1;
+	pCoexDm->psTdmaPara[1] = byte2;
+	pCoexDm->psTdmaPara[2] = byte3;
+	pCoexDm->psTdmaPara[3] = byte4;
+	pCoexDm->psTdmaPara[4] = byte5;
+	
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], FW write 0x3a(5bytes)=0x%x%08x\n", 
+		H2C_Parameter[0], 
+		H2C_Parameter[1]<<24|H2C_Parameter[2]<<16|H2C_Parameter[3]<<8|H2C_Parameter[4]));
+
+	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x3a, 5, H2C_Parameter);
+}
+
+VOID
+halbtc8723a2ant_PsTdma(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bForceExec,
+	IN	BOOLEAN			bTurnOn,
+	IN	u1Byte			type
+	)
+{
+	u4Byte	btTxRxCnt=0;
+
+	btTxRxCnt = pCoexSta->highPriorityTx+pCoexSta->highPriorityRx+
+				pCoexSta->lowPriorityTx+pCoexSta->lowPriorityRx;
+
+	if(btTxRxCnt > 3000)
+	{		
+		pCoexDm->bCurPsTdmaOn = true;
+		pCoexDm->curPsTdma = 8;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], turn ON PS TDMA, type=%d for BT tx/rx counters=%d(>3000)\n", 
+			pCoexDm->curPsTdma, btTxRxCnt));
+	}
+	else
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s turn %s PS TDMA, type=%d\n", 
+			(bForceExec? "force to":""), (bTurnOn? "ON":"OFF"), type));		
+		pCoexDm->bCurPsTdmaOn = bTurnOn;
+		pCoexDm->curPsTdma = type;
+	}
+
+	if(!bForceExec)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], bPrePsTdmaOn = %d, bCurPsTdmaOn = %d!!\n", 
+			pCoexDm->bPrePsTdmaOn, pCoexDm->bCurPsTdmaOn));
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], prePsTdma = %d, curPsTdma = %d!!\n", 
+			pCoexDm->prePsTdma, pCoexDm->curPsTdma));
+
+		if( (pCoexDm->bPrePsTdmaOn == pCoexDm->bCurPsTdmaOn) &&
+			(pCoexDm->prePsTdma == pCoexDm->curPsTdma) )
+			return;
+	}	
+	if(pCoexDm->bCurPsTdmaOn)
+	{
+		switch(pCoexDm->curPsTdma)
+		{
+			case 1:
+			default:
+				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x1a, 0x1a, 0xe1, 0x98);
+				break;
+			case 2:
+				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x12, 0x12, 0xe1, 0x98);
+				break;
+			case 3:
+				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0xa, 0xa, 0xe1, 0x98);
+				break;
+			case 4:
+				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xa3, 0x5, 0x5, 0xe1, 0x80);
+				break;
+			case 5:
+				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x1a, 0x1a, 0x60, 0x98);
+				break;
+			case 6:
+				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x12, 0x12, 0x60, 0x98);
+				break;
+			case 7:
+				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0xa, 0xa, 0x60, 0x98);
+				break;
+			case 8: 
+				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xa3, 0x5, 0x5, 0x60, 0x80);
+				break;
+			case 9: 
+				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x1a, 0x1a, 0xe1, 0x98);
+				break;
+			case 10:	
+				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x12, 0x12, 0xe1, 0x98);
+				break;
+			case 11:	
+				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0xa, 0xa, 0xe1, 0x98);
+				break;
+			case 12:
+				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x5, 0x5, 0xe1, 0x98);
+				break;
+			case 13:
+				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x1a, 0x1a, 0x60, 0x98);
+				break;
+			case 14:
+				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x12, 0x12, 0x60, 0x98);
+				break;
+			case 15:
+				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0xa, 0xa, 0x60, 0x98);
+				break;
+			case 16:
+				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x5, 0x5, 0x60, 0x98);
+				break;
+			case 17:
+				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xa3, 0x2f, 0x2f, 0x60, 0x80);
+				break;
+			case 18:
+				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x5, 0x5, 0xe1, 0x98);
+				break;			
+			case 19:
+				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x25, 0x25, 0xe1, 0x98);
+				break;
+			case 20:
+				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x25, 0x25, 0x60, 0x98);
+				break;
+		}
+	}
+	else
+	{
+		// disable PS tdma
+		switch(pCoexDm->curPsTdma)
+		{
+			case 0:
+				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0x0, 0x0, 0x0, 0x8, 0x0);
+				break;
+			case 1:
+				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0x0, 0x0, 0x0, 0x0, 0x0);
+				break;
+			default:
+				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0x0, 0x0, 0x0, 0x8, 0x0);
+				break;
+		}
+	}
+
+	// update pre state
+	pCoexDm->bPrePsTdmaOn = pCoexDm->bCurPsTdmaOn;
+	pCoexDm->prePsTdma = pCoexDm->curPsTdma;
+}
+
+
+VOID
+halbtc8723a2ant_CoexAllOff(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	// fw all off
+	halbtc8723a2ant_IgnoreWlanAct(pBtCoexist, NORMAL_EXEC, FALSE);
+	halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
+	halbtc8723a2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 0x20);
+	halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
+
+	// sw all off
+	halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
+	halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
+	halbtc8723a2ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, FALSE);
+	halbtc8723a2ant_RfShrink(pBtCoexist, NORMAL_EXEC, FALSE);
+	halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+
+	// hw all off
+	halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
+}
+
+VOID
+halbtc8723a2ant_InitCoexDm(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	// force to reset coex mechanism
+	halbtc8723a2ant_CoexTable(pBtCoexist, FORCE_EXEC, 0x55555555, 0xffff, 0x3);
+	halbtc8723a2ant_PsTdma(pBtCoexist, FORCE_EXEC, FALSE, 0);
+	halbtc8723a2ant_FwDacSwingLvl(pBtCoexist, FORCE_EXEC, 0x20);
+	halbtc8723a2ant_DecBtPwr(pBtCoexist, FORCE_EXEC, FALSE);
+	halbtc8723a2ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, FALSE);
+
+	halbtc8723a2ant_AgcTable(pBtCoexist, FORCE_EXEC, FALSE);
+	halbtc8723a2ant_AdcBackOff(pBtCoexist, FORCE_EXEC, FALSE);
+	halbtc8723a2ant_LowPenaltyRa(pBtCoexist, FORCE_EXEC, FALSE);
+	halbtc8723a2ant_RfShrink(pBtCoexist, FORCE_EXEC, FALSE);
+	halbtc8723a2ant_DacSwing(pBtCoexist, FORCE_EXEC, FALSE, 0xc0);
+}
+
+VOID
+halbtc8723a2ant_BtInquiryPage(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	BOOLEAN	bLowPwrDisable=true;
+	
+	pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_DISABLE_LOW_POWER, &bLowPwrDisable);
+
+	halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
+	halbtc8723a2ant_IgnoreWlanAct(pBtCoexist, NORMAL_EXEC, FALSE);
+	halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
+}
+
+VOID
+halbtc8723a2ant_BtEnableAction(
+	IN 	PBTC_COEXIST		pBtCoexist
+	)
+{
+	BOOLEAN		bWifiConnected=FALSE;
+	
+	// Here we need to resend some wifi info to BT
+	// because bt is reset and loss of the info.						
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+	if(bWifiConnected)
+	{
+		halbtc8723a2ant_IndicateWifiChnlBwInfo(pBtCoexist, BTC_MEDIA_CONNECT);
+	}
+	else
+	{
+		halbtc8723a2ant_IndicateWifiChnlBwInfo(pBtCoexist, BTC_MEDIA_DISCONNECT);
+	}
+
+	halbtc8723a2ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, FALSE);
+}
+
+VOID
+halbtc8723a2ant_MonitorBtCtr(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	u4Byte 			regHPTxRx, regLPTxRx, u4Tmp;
+	u4Byte			regHPTx=0, regHPRx=0, regLPTx=0, regLPRx=0;
+	u1Byte			u1Tmp;
+	
+	regHPTxRx = 0x770;
+	regLPTxRx = 0x774;
+
+	u4Tmp = pBtCoexist->btc_read_4byte(pBtCoexist, regHPTxRx);
+	regHPTx = u4Tmp & MASKLWORD;
+	regHPRx = (u4Tmp & MASKHWORD)>>16;
+
+	u4Tmp = pBtCoexist->btc_read_4byte(pBtCoexist, regLPTxRx);
+	regLPTx = u4Tmp & MASKLWORD;
+	regLPRx = (u4Tmp & MASKHWORD)>>16;
+		
+	pCoexSta->highPriorityTx = regHPTx;
+	pCoexSta->highPriorityRx = regHPRx;
+	pCoexSta->lowPriorityTx = regLPTx;
+	pCoexSta->lowPriorityRx = regLPRx;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], High Priority Tx/Rx (reg 0x%x)=0x%x(%d)/0x%x(%d)\n", 
+		regHPTxRx, regHPTx, regHPTx, regHPRx, regHPRx));
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], Low Priority Tx/Rx (reg 0x%x)=0x%x(%d)/0x%x(%d)\n", 
+		regLPTxRx, regLPTx, regLPTx, regLPRx, regLPRx));
+
+	// reset counter
+	pBtCoexist->btc_write_1byte(pBtCoexist, 0x76e, 0xc);
+}
+
+VOID
+halbtc8723a2ant_MonitorBtEnableDisable(
+	IN 	PBTC_COEXIST		pBtCoexist
+	)
+{
+	static BOOLEAN	bPreBtDisabled=FALSE;
+	static u4Byte	btDisableCnt=0;
+	BOOLEAN			bBtActive=true, bBtDisabled=FALSE;
+
+	// This function check if bt is disabled
+
+	if(	pCoexSta->highPriorityTx == 0 &&
+		pCoexSta->highPriorityRx == 0 &&
+		pCoexSta->lowPriorityTx == 0 &&
+		pCoexSta->lowPriorityRx == 0)
+	{
+		bBtActive = FALSE;
+	}
+	if(	pCoexSta->highPriorityTx == 0xffff &&
+		pCoexSta->highPriorityRx == 0xffff &&
+		pCoexSta->lowPriorityTx == 0xffff &&
+		pCoexSta->lowPriorityRx == 0xffff)
+	{
+		bBtActive = FALSE;
+	}
+	if(bBtActive)
+	{
+		btDisableCnt = 0;
+		bBtDisabled = FALSE;
+		pBtCoexist->btc_set(pBtCoexist, BTC_SET_BL_BT_DISABLE, &bBtDisabled);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], BT is enabled !!\n"));
+	}
+	else
+	{
+		btDisableCnt++;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], bt all counters=0, %d times!!\n", 
+				btDisableCnt));
+		if(btDisableCnt >= 2)
+		{
+			bBtDisabled = true;
+			pBtCoexist->btc_set(pBtCoexist, BTC_SET_BL_BT_DISABLE, &bBtDisabled);
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], BT is disabled !!\n"));
+		}
+	}
+	if(bPreBtDisabled != bBtDisabled)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], BT is from %s to %s!!\n", 
+			(bPreBtDisabled ? "disabled":"enabled"), 
+			(bBtDisabled ? "disabled":"enabled")));
+		bPreBtDisabled = bBtDisabled;
+		if(!bBtDisabled)
+		{
+			halbtc8723a2ant_BtEnableAction(pBtCoexist);
+		}
+	}
+}
+
+BOOLEAN
+halbtc8723a2ant_IsCommonAction(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	PBTC_STACK_INFO		pStackInfo=&pBtCoexist->stack_info;
+	BOOLEAN			bCommon=FALSE, bWifiConnected=FALSE;
+	BOOLEAN			bLowPwrDisable=FALSE;
+
+	if(!pStackInfo->bBtLinkExist)
+	{
+		bLowPwrDisable = FALSE;
+		pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_DISABLE_LOW_POWER, &bLowPwrDisable);
+	}
+	else
+	{
+		bLowPwrDisable = true;
+		pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_DISABLE_LOW_POWER, &bLowPwrDisable);
+	}
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+
+	if(halbtc8723a2ant_IsWifiIdle(pBtCoexist) && 
+		BT_8723A_2ANT_BT_STATUS_IDLE == pCoexDm->btStatus)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi idle + Bt idle!!\n"));
+			
+		halbtc8723a2ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a2ant_RfShrink(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
+
+		halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
+		halbtc8723a2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 0x20);
+		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
+
+		halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+
+		bCommon = true;
+	}
+	else if(!halbtc8723a2ant_IsWifiIdle(pBtCoexist) && 
+			(BT_8723A_2ANT_BT_STATUS_IDLE == pCoexDm->btStatus) )
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi non-idle + BT idle!!\n"));
+
+		halbtc8723a2ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, true);
+		halbtc8723a2ant_RfShrink(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
+
+		halbtc8723a2ant_IgnoreWlanAct(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
+		halbtc8723a2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 0x20);
+		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, true);
+
+		halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+
+		bCommon = true;
+	}
+	else if(halbtc8723a2ant_IsWifiIdle(pBtCoexist) && 
+		(BT_8723A_2ANT_BT_STATUS_CONNECTED_IDLE == pCoexDm->btStatus) )
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi idle + Bt connected idle!!\n"));
+		
+		halbtc8723a2ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, true);
+		halbtc8723a2ant_RfShrink(pBtCoexist, NORMAL_EXEC, true);
+		halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
+
+		halbtc8723a2ant_IgnoreWlanAct(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
+		halbtc8723a2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 0x20);
+		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
+
+		halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+
+		bCommon = true;
+	}
+	else if(!halbtc8723a2ant_IsWifiIdle(pBtCoexist) && 
+		(BT_8723A_2ANT_BT_STATUS_CONNECTED_IDLE == pCoexDm->btStatus) )
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi non-idle + Bt connected idle!!\n"));
+
+		halbtc8723a2ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, true);
+		halbtc8723a2ant_RfShrink(pBtCoexist, NORMAL_EXEC, true);
+		halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
+
+		halbtc8723a2ant_IgnoreWlanAct(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
+		halbtc8723a2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 0x20);
+		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
+
+		halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+
+		bCommon = true;
+	}
+	else if(halbtc8723a2ant_IsWifiIdle(pBtCoexist) && 
+			(BT_8723A_2ANT_BT_STATUS_NON_IDLE == pCoexDm->btStatus) )
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi idle + BT non-idle!!\n"));
+		
+		halbtc8723a2ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, true);
+		halbtc8723a2ant_RfShrink(pBtCoexist, NORMAL_EXEC, true);
+		halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
+
+		halbtc8723a2ant_IgnoreWlanAct(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
+		halbtc8723a2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 0x20);
+		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
+
+		halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+		
+		bCommon = true;
+	}
+	else
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi non-idle + BT non-idle!!\n"));
+		halbtc8723a2ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, true);
+		halbtc8723a2ant_RfShrink(pBtCoexist, NORMAL_EXEC, true);
+		halbtc8723a2ant_IgnoreWlanAct(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 0x20);
+		
+		bCommon = FALSE;
+	}
+	
+	return bCommon;
+}
+VOID
+halbtc8723a2ant_TdmaDurationAdjust(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	BOOLEAN			bScoHid,
+	IN	BOOLEAN			bTxPause,
+	IN	u1Byte			maxInterval
+	)
+{
+	static s4Byte		up,dn,m,n,WaitCount;
+	s4Byte			result;   //0: no change, +1: increase WiFi duration, -1: decrease WiFi duration
+	u1Byte			retryCount=0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], TdmaDurationAdjust()\n"));
+
+	if(pCoexDm->bResetTdmaAdjust)
+	{
+		pCoexDm->bResetTdmaAdjust = FALSE;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], first run TdmaDurationAdjust()!!\n"));
+		{
+			if(bScoHid)
+			{
+				if(bTxPause)
+				{
+					if(maxInterval == 1)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 13);
+						pCoexDm->psTdmaDuAdjType = 13;	
+					}
+					else if(maxInterval == 2)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+						pCoexDm->psTdmaDuAdjType = 14;	
+					}
+					else if(maxInterval == 3)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;	
+					}
+					else
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					}
+				}
+				else
+				{
+					if(maxInterval == 1)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+						pCoexDm->psTdmaDuAdjType = 9;	
+					}
+					else if(maxInterval == 2)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+						pCoexDm->psTdmaDuAdjType = 10;	
+					}
+					else if(maxInterval == 3)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					}
+					else
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					}
+				}
+			}
+			else
+			{
+				if(bTxPause)
+				{
+					if(maxInterval == 1)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 5);
+						pCoexDm->psTdmaDuAdjType = 5;	
+					}
+					else if(maxInterval == 2)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+						pCoexDm->psTdmaDuAdjType = 6;	
+					}
+					else if(maxInterval == 3)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					}
+					else
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					}
+				}
+				else
+				{
+					if(maxInterval == 1)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 1);
+						pCoexDm->psTdmaDuAdjType = 1;	
+					}
+					else if(maxInterval == 2)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+						pCoexDm->psTdmaDuAdjType = 2;	
+					}
+					else if(maxInterval == 3)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					}
+					else
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					}
+				}
+			}
+		}
+		//============
+		up = 0;
+		dn = 0;
+		m = 1;
+		n= 3;
+		result = 0;
+		WaitCount = 0;
+	}
+	else
+	{
+		//accquire the BT TRx retry count from BT_Info byte2
+		retryCount = pCoexSta->btRetryCnt;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], retryCount = %d\n", retryCount));
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], up=%d, dn=%d, m=%d, n=%d, WaitCount=%d\n", 
+			up, dn, m, n, WaitCount));
+		result = 0;
+		WaitCount++; 
+		  
+		if(retryCount == 0)  // no retry in the last 2-second duration
+		{
+			up++;
+			dn--;
+
+			if (dn <= 0)
+				dn = 0;				 
+
+			if(up >= n)	// if ³sÄò n ­Ó2¬í retry count¬°0, «h½Õ¼eWiFi duration
+			{
+				WaitCount = 0; 
+				n = 3;
+				up = 0;
+				dn = 0;
+				result = 1; 
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], Increase wifi duration!!\n"));
+			}
+		}
+		else if (retryCount <= 3)	// <=3 retry in the last 2-second duration
+		{
+			up--; 
+			dn++;
+
+			if (up <= 0)
+				up = 0;
+
+			if (dn == 2)	// if ³sÄò 2 ­Ó2¬í retry count< 3, «h½Õ¯¶WiFi duration
+			{
+				if (WaitCount <= 2)
+					m++; // ÁקK¤@ª½¦b¨â­Ólevel¤¤¨Ó¦^
+				else
+					m = 1;
+
+				if ( m >= 20) //m ³Ì¤j­È = 20 ' ³Ì¤j120¬í recheck¬O§_½Õ¾ã WiFi duration.
+					m = 20;
+
+				n = 3*m;
+				up = 0;
+				dn = 0;
+				WaitCount = 0;
+				result = -1; 
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], Decrease wifi duration for retryCounter<3!!\n"));
+			}
+		}
+		else  //retry count > 3, ¥u­n1¦¸ retry count > 3, «h½Õ¯¶WiFi duration
+		{
+			if (WaitCount == 1)
+				m++; // ÁקK¤@ª½¦b¨â­Ólevel¤¤¨Ó¦^
+			else
+				m = 1;
+
+			if ( m >= 20) //m ³Ì¤j­È = 20 ' ³Ì¤j120¬í recheck¬O§_½Õ¾ã WiFi duration.
+				m = 20;
+
+			n = 3*m;
+			up = 0;
+			dn = 0;
+			WaitCount = 0; 
+			result = -1;
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], Decrease wifi duration for retryCounter>3!!\n"));
+		}
+
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], max Interval = %d\n", maxInterval));
+		if(maxInterval == 1)
+		{
+			if(bTxPause)
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], TxPause = 1\n"));
+
+				if(pCoexDm->curPsTdma == 1)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 5);
+					pCoexDm->psTdmaDuAdjType = 5;
+				}
+				else if(pCoexDm->curPsTdma == 2)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+					pCoexDm->psTdmaDuAdjType = 6;
+				}
+				else if(pCoexDm->curPsTdma == 3)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+					pCoexDm->psTdmaDuAdjType = 7;
+				}
+				else if(pCoexDm->curPsTdma == 4)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
+					pCoexDm->psTdmaDuAdjType = 8;
+				}
+				if(pCoexDm->curPsTdma == 9)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 13);
+					pCoexDm->psTdmaDuAdjType = 13;
+				}
+				else if(pCoexDm->curPsTdma == 10)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+					pCoexDm->psTdmaDuAdjType = 14;
+				}
+				else if(pCoexDm->curPsTdma == 11)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+					pCoexDm->psTdmaDuAdjType = 15;
+				}
+				else if(pCoexDm->curPsTdma == 12)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 16);
+					pCoexDm->psTdmaDuAdjType = 16;
+				}
+				
+				if(result == -1)
+				{					
+					if(pCoexDm->curPsTdma == 5)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+						pCoexDm->psTdmaDuAdjType = 6;
+					}
+					else if(pCoexDm->curPsTdma == 6)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					}
+					else if(pCoexDm->curPsTdma == 7)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
+						pCoexDm->psTdmaDuAdjType = 8;
+					}
+					else if(pCoexDm->curPsTdma == 13)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+						pCoexDm->psTdmaDuAdjType = 14;
+					}
+					else if(pCoexDm->curPsTdma == 14)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					}
+					else if(pCoexDm->curPsTdma == 15)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 16);
+						pCoexDm->psTdmaDuAdjType = 16;
+					}
+				} 
+				else if (result == 1)
+				{
+					if(pCoexDm->curPsTdma == 8)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					}
+					else if(pCoexDm->curPsTdma == 7)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+						pCoexDm->psTdmaDuAdjType = 6;
+					}
+					else if(pCoexDm->curPsTdma == 6)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 5);
+						pCoexDm->psTdmaDuAdjType = 5;
+					}
+					else if(pCoexDm->curPsTdma == 16)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					}
+					else if(pCoexDm->curPsTdma == 15)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+						pCoexDm->psTdmaDuAdjType = 14;
+					}
+					else if(pCoexDm->curPsTdma == 14)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 13);
+						pCoexDm->psTdmaDuAdjType = 13;
+					}
+				}
+			}
+			else
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], TxPause = 0\n"));
+				if(pCoexDm->curPsTdma == 5)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 1);
+					pCoexDm->psTdmaDuAdjType = 1;
+				}
+				else if(pCoexDm->curPsTdma == 6)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+					pCoexDm->psTdmaDuAdjType = 2;
+				}
+				else if(pCoexDm->curPsTdma == 7)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+					pCoexDm->psTdmaDuAdjType = 3;
+				}
+				else if(pCoexDm->curPsTdma == 8)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
+					pCoexDm->psTdmaDuAdjType = 4;
+				}
+				if(pCoexDm->curPsTdma == 13)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+					pCoexDm->psTdmaDuAdjType = 9;
+				}
+				else if(pCoexDm->curPsTdma == 14)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+					pCoexDm->psTdmaDuAdjType = 10;
+				}
+				else if(pCoexDm->curPsTdma == 15)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+					pCoexDm->psTdmaDuAdjType = 11;
+				}
+				else if(pCoexDm->curPsTdma == 16)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
+					pCoexDm->psTdmaDuAdjType = 12;
+				}
+				
+				if(result == -1)
+				{
+					if(pCoexDm->curPsTdma == 1)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+						pCoexDm->psTdmaDuAdjType = 2;
+					}
+					else if(pCoexDm->curPsTdma == 2)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					}
+					else if(pCoexDm->curPsTdma == 3)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
+						pCoexDm->psTdmaDuAdjType = 4;
+					}
+					else if(pCoexDm->curPsTdma == 9)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+						pCoexDm->psTdmaDuAdjType = 10;
+					}
+					else if(pCoexDm->curPsTdma == 10)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					}
+					else if(pCoexDm->curPsTdma == 11)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
+						pCoexDm->psTdmaDuAdjType = 12;
+					}
+				} 
+				else if (result == 1)
+				{
+					if(pCoexDm->curPsTdma == 4)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					}
+					else if(pCoexDm->curPsTdma == 3)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+						pCoexDm->psTdmaDuAdjType = 2;
+					}
+					else if(pCoexDm->curPsTdma == 2)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 1);
+						pCoexDm->psTdmaDuAdjType = 1;
+					}
+					else if(pCoexDm->curPsTdma == 12)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					}
+					else if(pCoexDm->curPsTdma == 11)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+						pCoexDm->psTdmaDuAdjType = 10;
+					}
+					else if(pCoexDm->curPsTdma == 10)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+						pCoexDm->psTdmaDuAdjType = 9;
+					}
+				}
+			}
+		}
+		else if(maxInterval == 2)
+		{
+			if(bTxPause)
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], TxPause = 1\n"));
+				if(pCoexDm->curPsTdma == 1)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+					pCoexDm->psTdmaDuAdjType = 6;
+				}
+				else if(pCoexDm->curPsTdma == 2)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+					pCoexDm->psTdmaDuAdjType = 6;
+				}
+				else if(pCoexDm->curPsTdma == 3)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+					pCoexDm->psTdmaDuAdjType = 7;
+				}
+				else if(pCoexDm->curPsTdma == 4)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
+					pCoexDm->psTdmaDuAdjType = 8;
+				}
+				if(pCoexDm->curPsTdma == 9)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+					pCoexDm->psTdmaDuAdjType = 14;
+				}
+				else if(pCoexDm->curPsTdma == 10)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+					pCoexDm->psTdmaDuAdjType = 14;
+				}
+				else if(pCoexDm->curPsTdma == 11)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+					pCoexDm->psTdmaDuAdjType = 15;
+				}
+				else if(pCoexDm->curPsTdma == 12)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 16);
+					pCoexDm->psTdmaDuAdjType = 16;
+				}
+				if(result == -1)
+				{
+					if(pCoexDm->curPsTdma == 5) 
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+						pCoexDm->psTdmaDuAdjType = 6;
+					}
+					else if(pCoexDm->curPsTdma == 6)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					}
+					else if(pCoexDm->curPsTdma == 7)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
+						pCoexDm->psTdmaDuAdjType = 8;
+					}
+					else if(pCoexDm->curPsTdma == 13)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+						pCoexDm->psTdmaDuAdjType = 14;
+					}
+					else if(pCoexDm->curPsTdma == 14)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					}
+					else if(pCoexDm->curPsTdma == 15)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 16);
+						pCoexDm->psTdmaDuAdjType = 16;
+					}
+				} 
+				else if (result == 1)
+				{
+					if(pCoexDm->curPsTdma == 8)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					}
+					else if(pCoexDm->curPsTdma == 7)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+						pCoexDm->psTdmaDuAdjType = 6;
+					}
+					else if(pCoexDm->curPsTdma == 6)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+						pCoexDm->psTdmaDuAdjType = 6;
+					}					
+					else if(pCoexDm->curPsTdma == 16)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					}
+					else if(pCoexDm->curPsTdma == 15)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+						pCoexDm->psTdmaDuAdjType = 14;
+					}
+					else if(pCoexDm->curPsTdma == 14)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+						pCoexDm->psTdmaDuAdjType = 14;
+					}
+				}
+			}
+			else
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], TxPause = 0\n"));
+				if(pCoexDm->curPsTdma == 5)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+					pCoexDm->psTdmaDuAdjType = 2;
+				}
+				else if(pCoexDm->curPsTdma == 6)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+					pCoexDm->psTdmaDuAdjType = 2;
+				}
+				else if(pCoexDm->curPsTdma == 7)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+					pCoexDm->psTdmaDuAdjType = 3;
+				}
+				else if(pCoexDm->curPsTdma == 8)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
+					pCoexDm->psTdmaDuAdjType = 4;
+				}
+				if(pCoexDm->curPsTdma == 13)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+					pCoexDm->psTdmaDuAdjType = 10;
+				}
+				else if(pCoexDm->curPsTdma == 14)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+					pCoexDm->psTdmaDuAdjType = 10;
+				}
+				else if(pCoexDm->curPsTdma == 15)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+					pCoexDm->psTdmaDuAdjType = 11;
+				}
+				else if(pCoexDm->curPsTdma == 16)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
+					pCoexDm->psTdmaDuAdjType = 12;
+				}
+				if(result == -1)
+				{
+					if(pCoexDm->curPsTdma == 1)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+						pCoexDm->psTdmaDuAdjType = 2;
+					}
+					else if(pCoexDm->curPsTdma == 2)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					}
+					else if(pCoexDm->curPsTdma == 3)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
+						pCoexDm->psTdmaDuAdjType = 4;
+					}
+					else if(pCoexDm->curPsTdma == 9)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+						pCoexDm->psTdmaDuAdjType = 10;
+					}
+					else if(pCoexDm->curPsTdma == 10)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					}
+					else if(pCoexDm->curPsTdma == 11)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
+						pCoexDm->psTdmaDuAdjType = 12;
+					}
+				} 
+				else if (result == 1)
+				{
+					if(pCoexDm->curPsTdma == 4)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					}
+					else if(pCoexDm->curPsTdma == 3)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+						pCoexDm->psTdmaDuAdjType = 2;
+					}
+					else if(pCoexDm->curPsTdma == 2)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+						pCoexDm->psTdmaDuAdjType = 2;
+					}
+					else if(pCoexDm->curPsTdma == 12)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					}
+					else if(pCoexDm->curPsTdma == 11)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+						pCoexDm->psTdmaDuAdjType = 10;
+					}
+					else if(pCoexDm->curPsTdma == 10)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+						pCoexDm->psTdmaDuAdjType = 10;
+					}
+				}
+			}
+		}
+		else if(maxInterval == 3)
+		{
+			if(bTxPause)
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], TxPause = 1\n"));
+				if(pCoexDm->curPsTdma == 1)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+					pCoexDm->psTdmaDuAdjType = 7;
+				}
+				else if(pCoexDm->curPsTdma == 2)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+					pCoexDm->psTdmaDuAdjType = 7;
+				}
+				else if(pCoexDm->curPsTdma == 3)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+					pCoexDm->psTdmaDuAdjType = 7;
+				}
+				else if(pCoexDm->curPsTdma == 4)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
+					pCoexDm->psTdmaDuAdjType = 8;
+				}
+				if(pCoexDm->curPsTdma == 9)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+					pCoexDm->psTdmaDuAdjType = 15;
+				}
+				else if(pCoexDm->curPsTdma == 10)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+					pCoexDm->psTdmaDuAdjType = 15;
+				}
+				else if(pCoexDm->curPsTdma == 11)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+					pCoexDm->psTdmaDuAdjType = 15;
+				}
+				else if(pCoexDm->curPsTdma == 12)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 16);
+					pCoexDm->psTdmaDuAdjType = 16;
+				}
+				if(result == -1)
+				{
+					if(pCoexDm->curPsTdma == 5) 
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					}
+					else if(pCoexDm->curPsTdma == 6)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					}
+					else if(pCoexDm->curPsTdma == 7)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
+						pCoexDm->psTdmaDuAdjType = 8;
+					}
+					else if(pCoexDm->curPsTdma == 13)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					}
+					else if(pCoexDm->curPsTdma == 14)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					}
+					else if(pCoexDm->curPsTdma == 15)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 16);
+						pCoexDm->psTdmaDuAdjType = 16;
+					}
+				} 
+				else if (result == 1)
+				{
+					if(pCoexDm->curPsTdma == 8)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					}
+					else if(pCoexDm->curPsTdma == 7)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					}
+					else if(pCoexDm->curPsTdma == 6)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					}					
+					else if(pCoexDm->curPsTdma == 16)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					}
+					else if(pCoexDm->curPsTdma == 15)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					}
+					else if(pCoexDm->curPsTdma == 14)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					}
+				}
+			}
+			else
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], TxPause = 0\n"));
+				if(pCoexDm->curPsTdma == 5)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+					pCoexDm->psTdmaDuAdjType = 3;
+				}
+				else if(pCoexDm->curPsTdma == 6)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+					pCoexDm->psTdmaDuAdjType = 3;
+				}
+				else if(pCoexDm->curPsTdma == 7)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+					pCoexDm->psTdmaDuAdjType = 3;
+				}
+				else if(pCoexDm->curPsTdma == 8)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
+					pCoexDm->psTdmaDuAdjType = 4;
+				}
+				if(pCoexDm->curPsTdma == 13)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+					pCoexDm->psTdmaDuAdjType = 11;
+				}
+				else if(pCoexDm->curPsTdma == 14)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+					pCoexDm->psTdmaDuAdjType = 11;
+				}
+				else if(pCoexDm->curPsTdma == 15)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+					pCoexDm->psTdmaDuAdjType = 11;
+				}
+				else if(pCoexDm->curPsTdma == 16)
+				{
+					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
+					pCoexDm->psTdmaDuAdjType = 12;
+				}
+				if(result == -1)
+				{
+					if(pCoexDm->curPsTdma == 1)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					}
+					else if(pCoexDm->curPsTdma == 2)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					}
+					else if(pCoexDm->curPsTdma == 3)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
+						pCoexDm->psTdmaDuAdjType = 4;
+					}
+					else if(pCoexDm->curPsTdma == 9)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					}
+					else if(pCoexDm->curPsTdma == 10)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					}
+					else if(pCoexDm->curPsTdma == 11)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
+						pCoexDm->psTdmaDuAdjType = 12;
+					}
+				} 
+				else if (result == 1)
+				{
+					if(pCoexDm->curPsTdma == 4)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					}
+					else if(pCoexDm->curPsTdma == 3)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					}
+					else if(pCoexDm->curPsTdma == 2)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					}
+					else if(pCoexDm->curPsTdma == 12)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					}
+					else if(pCoexDm->curPsTdma == 11)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					}
+					else if(pCoexDm->curPsTdma == 10)
+					{
+						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					}
+				}
+			}
+		}
+	}
+
+	// if current PsTdma not match with the recorded one (when scan, dhcp...), 
+	// then we have to adjust it back to the previous record one.
+	if(pCoexDm->curPsTdma != pCoexDm->psTdmaDuAdjType)
+	{
+		BOOLEAN	bScan=FALSE, bLink=FALSE, bRoam=FALSE;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], PsTdma type dismatch!!!, curPsTdma=%d, recordPsTdma=%d\n", 
+			pCoexDm->curPsTdma, pCoexDm->psTdmaDuAdjType));
+
+		pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_SCAN, &bScan);
+		pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_LINK, &bLink);
+		pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_ROAM, &bRoam);
+		
+		if( !bScan && !bLink && !bRoam)
+		{
+			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, pCoexDm->psTdmaDuAdjType);
+		}
+		else
+		{
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], roaming/link/scan is under progress, will adjust next time!!!\n"));
+		}
+	}
+}
+
+// SCO only or SCO+PAN(HS)
+VOID
+halbtc8723a2ant_ActionSco(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	u1Byte	wifiRssiState, wifiRssiState1;
+	u4Byte	wifiBw;
+
+	if(halbtc8723a2ant_NeedToDecBtPwr(pBtCoexist))
+		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, true);
+	else	
+		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
+	halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
+	
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+	if(BTC_WIFI_BW_HT40 == wifiBw)
+	{
+		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 37, 0);
+		// fw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+		}
+		else
+		{
+			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+		}
+
+		// sw mechanism		
+		halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
+		halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+	}
+	else
+	{
+		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 27, 0);
+		wifiRssiState1 = halbtc8723a2ant_WifiRssiState(pBtCoexist, 1, 2, 47, 0);
+		
+		// fw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+		}
+		else
+		{
+			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+		}
+		
+		// sw mechanism
+		if( (wifiRssiState1 == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState1 == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, true);
+			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
+			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+		}
+		else
+		{
+			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
+			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
+			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+		}		
+	}
+}
+
+
+VOID
+halbtc8723a2ant_ActionHid(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	u1Byte	wifiRssiState, wifiRssiState1;
+	u4Byte	wifiBw;
+
+	if(halbtc8723a2ant_NeedToDecBtPwr(pBtCoexist))
+		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, true);
+	else	
+		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
+	halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+	if(BTC_WIFI_BW_HT40 == wifiBw)
+	{
+		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 37, 0);
+		// fw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+		}
+		else
+		{
+			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 13);
+		}
+
+		// sw mechanism
+		halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+	}
+	else
+	{
+		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 27, 0);
+		wifiRssiState1 = halbtc8723a2ant_WifiRssiState(pBtCoexist, 1, 2, 47, 0);
+
+		// fw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+		}
+		else
+		{
+			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 13);
+		}
+
+		// sw mechanism
+		if( (wifiRssiState1 == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState1 == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, true);
+			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
+			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+		}
+		else
+		{
+			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
+			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
+			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+		}		
+	}
+}
+
+//A2DP only / PAN(EDR) only/ A2DP+PAN(HS)
+VOID
+halbtc8723a2ant_ActionA2dp(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	u1Byte		wifiRssiState, wifiRssiState1, btInfoExt;
+	u4Byte		wifiBw;
+
+	btInfoExt = pCoexSta->btInfoExt;
+
+	if(halbtc8723a2ant_NeedToDecBtPwr(pBtCoexist))
+		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, true);
+	else	
+		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
+	halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
+	
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+	if(BTC_WIFI_BW_HT40 == wifiBw)
+	{
+		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 37, 0);
+
+		// fw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			if(btInfoExt&BIT0)	//a2dp rate, 1:basic /0:edr
+			{
+				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, FALSE, FALSE, 3);
+			}
+			else
+			{
+				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, FALSE, FALSE, 1);
+			}
+		}
+		else
+		{
+			if(btInfoExt&BIT0)	//a2dp rate, 1:basic /0:edr
+			{
+				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, FALSE, true, 3);
+			}
+			else
+			{
+				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, FALSE, true, 1);
+			}
+		}
+
+		// sw mechanism		
+		halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
+		halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+	}
+	else
+	{
+		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 27, 0);
+		wifiRssiState1 = halbtc8723a2ant_WifiRssiState(pBtCoexist, 1, 2, 47, 0);
+		
+		// fw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			if(btInfoExt&BIT0)	//a2dp rate, 1:basic /0:edr
+			{
+				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, FALSE, FALSE, 3);
+			}
+			else
+			{
+				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, FALSE, FALSE, 1);
+			}
+		}
+		else
+		{
+			if(btInfoExt&BIT0)	//a2dp rate, 1:basic /0:edr
+			{
+				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, FALSE, true, 3);
+			}
+			else
+			{
+				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, FALSE, true, 1);
+			}
+		}
+		
+		// sw mechanism
+		if( (wifiRssiState1 == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState1 == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, true);
+			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
+			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+		}
+		else
+		{
+			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
+			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
+			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+		}		
+	}
+}
+
+VOID
+halbtc8723a2ant_ActionPanEdr(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	u1Byte		wifiRssiState, wifiRssiState1, btInfoExt;
+	u4Byte		wifiBw;
+	
+	btInfoExt = pCoexSta->btInfoExt;
+
+	if(halbtc8723a2ant_NeedToDecBtPwr(pBtCoexist))
+		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, true);
+	else	
+		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
+	halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+	if(BTC_WIFI_BW_HT40 == wifiBw)
+	{
+		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 37, 0);
+			
+		// fw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+		}
+		else
+		{
+			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+		}
+
+		// sw mechanism
+		halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
+		halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+	}
+	else
+	{
+		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 27, 0);
+		wifiRssiState1 = halbtc8723a2ant_WifiRssiState(pBtCoexist, 1, 2, 47, 0);
+		
+		// fw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+		}
+		else
+		{
+			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+		}
+
+		// sw mechanism
+		if( (wifiRssiState1 == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState1 == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, true);
+			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
+			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+		}
+		else
+		{
+			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
+			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
+			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+		}
+	}
+}
+
+
+//PAN(HS) only
+VOID
+halbtc8723a2ant_ActionPanHs(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	u1Byte		wifiRssiState;
+	u4Byte		wifiBw;
+
+	halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+	if(BTC_WIFI_BW_HT40 == wifiBw)
+	{
+		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 37, 0);
+
+		// fw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, true);
+		}
+		else
+		{
+			halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
+		}
+		halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
+
+		// sw mechanism		
+		halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
+		halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+	}
+	else
+	{
+		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 37, 0);
+
+		// fw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, true);
+			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
+		}
+		else
+		{
+			halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
+			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
+		}
+
+		// sw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, true);
+			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
+			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+		}
+		else
+		{
+			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
+			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
+			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+		}
+	}
+}
+
+//PAN(EDR)+A2DP
+VOID
+halbtc8723a2ant_ActionPanEdrA2dp(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	u1Byte		wifiRssiState, wifiRssiState1, btInfoExt;
+	u4Byte		wifiBw;
+
+	btInfoExt = pCoexSta->btInfoExt;
+
+	if(halbtc8723a2ant_NeedToDecBtPwr(pBtCoexist))
+		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, true);
+	else	
+		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
+	halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+	if(BTC_WIFI_BW_HT40 == wifiBw)
+	{
+		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 37, 0);
+
+		// fw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			if(btInfoExt&BIT0)	//a2dp basic rate
+			{
+				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
+			}
+			else				//a2dp edr rate
+			{
+				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+			}
+		}
+		else
+		{
+			if(btInfoExt&BIT0)	//a2dp basic rate
+			{
+				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
+			}
+			else				//a2dp edr rate
+			{
+				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+			}
+		}
+
+		// sw mechanism
+		halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
+		halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+	}
+	else
+	{
+		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 27, 0);
+		wifiRssiState1 = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 47, 0);
+		
+		// fw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			if(btInfoExt&BIT0)	//a2dp basic rate
+			{
+				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
+			}
+			else				//a2dp edr rate
+			{
+				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+			}
+		}
+		else
+		{
+			if(btInfoExt&BIT0)	//a2dp basic rate
+			{
+				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
+			}
+			else				//a2dp edr rate
+			{
+				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+			}
+		}
+
+		// sw mechanism
+		if( (wifiRssiState1 == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState1 == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, true);
+			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
+			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+		}
+		else
+		{
+			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
+			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
+			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+		}
+	}
+}
+
+VOID
+halbtc8723a2ant_ActionPanEdrHid(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	u1Byte		wifiRssiState, wifiRssiState1;
+	u4Byte		wifiBw;
+
+	if(halbtc8723a2ant_NeedToDecBtPwr(pBtCoexist))
+		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, true);
+	else	
+		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
+	halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+	if(BTC_WIFI_BW_HT40 == wifiBw)
+	{
+		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 37, 0);
+
+		// fw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10); 
+		}
+		else
+		{
+			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14); 
+		}
+
+		// sw mechanism
+		halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
+		halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+	}
+	else
+	{
+		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 27, 0);
+		wifiRssiState1 = halbtc8723a2ant_WifiRssiState(pBtCoexist, 1, 2, 47, 0);
+		
+		// fw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+		}
+		else
+		{
+			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+		}
+		
+		// sw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, true);
+			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
+			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+		}
+		else
+		{
+			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
+			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
+			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+		}
+	}
+}
+
+// HID+A2DP+PAN(EDR)
+VOID
+halbtc8723a2ant_ActionHidA2dpPanEdr(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	u1Byte		wifiRssiState, wifiRssiState1, btInfoExt;
+	u4Byte		wifiBw;
+
+	btInfoExt = pCoexSta->btInfoExt;
+
+	if(halbtc8723a2ant_NeedToDecBtPwr(pBtCoexist))
+		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, true);
+	else	
+		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
+	halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+	if(BTC_WIFI_BW_HT40 == wifiBw)
+	{
+		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 37, 0);
+			
+		// fw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			if(btInfoExt&BIT0)	//a2dp basic rate
+			{
+				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
+			}
+			else				//a2dp edr rate
+			{
+				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+			}
+		}
+		else
+		{
+			if(btInfoExt&BIT0)	//a2dp basic rate
+			{
+				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 16);
+			}
+			else				//a2dp edr rate
+			{
+				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+			}
+		}
+		
+		// sw mechanism
+		halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
+		halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+	}
+	else
+	{
+		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 27, 0);
+		wifiRssiState1 = halbtc8723a2ant_WifiRssiState(pBtCoexist, 1, 2, 47, 0);
+		
+		// fw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			if(btInfoExt&BIT0)	//a2dp basic rate
+			{
+				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
+			}
+			else				//a2dp edr rate
+			{
+				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+			}
+		}
+		else
+		{
+			if(btInfoExt&BIT0)	//a2dp basic rate
+			{
+				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 16);
+			}
+			else				//a2dp edr rate
+			{
+				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+			}
+		}
+
+		// sw mechanism
+		if( (wifiRssiState1 == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState1 == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, true);
+			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
+			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+		}
+		else
+		{
+			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
+			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
+			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+		}
+	}
+}
+
+VOID
+halbtc8723a2ant_ActionHidA2dp(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	u1Byte		wifiRssiState, wifiRssiState1, btInfoExt;
+	u4Byte		wifiBw;
+
+	btInfoExt = pCoexSta->btInfoExt;
+
+	if(halbtc8723a2ant_NeedToDecBtPwr(pBtCoexist))
+		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, true);
+	else	
+		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
+	halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+	if(BTC_WIFI_BW_HT40 == wifiBw)
+	{
+		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 37, 0);
+		
+		// fw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			if(btInfoExt&BIT0)	//a2dp basic rate
+			{
+				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, true, FALSE, 3);
+			}
+			else				//a2dp edr rate
+			{
+				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, true, FALSE, 1);
+			}
+		}
+		else
+		{
+			if(btInfoExt&BIT0)	//a2dp basic rate
+			{
+				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, true, true, 3);
+			}
+			else				//a2dp edr rate
+			{
+				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, true, true, 1);
+			}
+		}
+		
+		// sw mechanism
+		halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
+		halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
+		halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+	}
+	else
+	{
+		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 27, 0);
+		wifiRssiState1 = halbtc8723a2ant_WifiRssiState(pBtCoexist, 1, 2, 47, 0);
+		
+		// fw mechanism
+		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			if(btInfoExt&BIT0)	//a2dp basic rate
+			{
+				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, true, FALSE, 3);
+			}
+			else				//a2dp edr rate
+			{
+				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, true, FALSE, 1);
+			}
+		}
+		else
+		{
+			if(btInfoExt&BIT0)	//a2dp basic rate
+			{
+				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, true, true, 3);
+			}
+			else				//a2dp edr rate
+			{
+				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, true, true, 1);
+			}
+		}
+
+		// sw mechanism
+		if( (wifiRssiState1 == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState1 == BTC_RSSI_STATE_STAY_HIGH) )
+		{
+			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, true);
+			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
+			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+		}
+		else
+		{
+			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
+			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
+			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
+		}
+	}
+}
+
+VOID
+halbtc8723a2ant_RunCoexistMechanism(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	PBTC_STACK_INFO		pStackInfo=&pBtCoexist->stack_info;
+	u1Byte				btInfoOriginal=0, btRetryCnt=0;
+	u1Byte				algorithm=0;
+
+	if(pBtCoexist->manual_control)
+	{
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Manual control!!!\n"));
+		return;
+	}
+
+	if(pStackInfo->bProfileNotified)
+	{
+		if(pCoexSta->bHoldForStackOperation)
+		{
+			// if bt inquiry/page/pair, do not execute.
+			return;
+		}
+		
+		algorithm = halbtc8723a2ant_ActionAlgorithm(pBtCoexist);
+		if(pCoexSta->bHoldPeriodCnt && (BT_8723A_2ANT_COEX_ALGO_PANHS!=algorithm))
+		{
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex],Hold BT inquiry/page scan setting (cnt = %d)!!\n", 
+				pCoexSta->bHoldPeriodCnt));
+			if(pCoexSta->bHoldPeriodCnt >= 6)
+			{
+				pCoexSta->bHoldPeriodCnt = 0;
+				// next time the coexist parameters should be reset again.
+			}
+			else
+				pCoexSta->bHoldPeriodCnt++;
+			return;
+		}
+
+		pCoexDm->curAlgorithm = algorithm;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Algorithm = %d \n", pCoexDm->curAlgorithm));
+		if(halbtc8723a2ant_IsCommonAction(pBtCoexist))
+		{
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant common.\n"));
+			pCoexDm->bResetTdmaAdjust = true;
+		}
+		else
+		{
+			if(pCoexDm->curAlgorithm != pCoexDm->preAlgorithm)
+			{
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], preAlgorithm=%d, curAlgorithm=%d\n", 
+					pCoexDm->preAlgorithm, pCoexDm->curAlgorithm));
+				pCoexDm->bResetTdmaAdjust = true;
+			}
+			switch(pCoexDm->curAlgorithm)
+			{
+				case BT_8723A_2ANT_COEX_ALGO_SCO:
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = SCO.\n"));
+					halbtc8723a2ant_ActionSco(pBtCoexist);
+					break;
+				case BT_8723A_2ANT_COEX_ALGO_HID:
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = HID.\n"));
+					halbtc8723a2ant_ActionHid(pBtCoexist);
+					break;
+				case BT_8723A_2ANT_COEX_ALGO_A2DP:
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = A2DP.\n"));
+					halbtc8723a2ant_ActionA2dp(pBtCoexist);
+					break;
+				case BT_8723A_2ANT_COEX_ALGO_PANEDR:
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = PAN(EDR).\n"));
+					halbtc8723a2ant_ActionPanEdr(pBtCoexist);
+					break;
+				case BT_8723A_2ANT_COEX_ALGO_PANHS:
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = HS mode.\n"));
+					halbtc8723a2ant_ActionPanHs(pBtCoexist);
+					break;
+				case BT_8723A_2ANT_COEX_ALGO_PANEDR_A2DP:
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = PAN+A2DP.\n"));
+					halbtc8723a2ant_ActionPanEdrA2dp(pBtCoexist);
+					break;
+				case BT_8723A_2ANT_COEX_ALGO_PANEDR_HID:
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = PAN(EDR)+HID.\n"));
+					halbtc8723a2ant_ActionPanEdrHid(pBtCoexist);
+					break;
+				case BT_8723A_2ANT_COEX_ALGO_HID_A2DP_PANEDR:
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = HID+A2DP+PAN.\n"));
+					halbtc8723a2ant_ActionHidA2dpPanEdr(pBtCoexist);
+					break;
+				case BT_8723A_2ANT_COEX_ALGO_HID_A2DP:
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = HID+A2DP.\n"));
+					halbtc8723a2ant_ActionHidA2dp(pBtCoexist);
+					break;
+				default:
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = coexist All Off!!\n"));
+					halbtc8723a2ant_CoexAllOff(pBtCoexist);
+					break;
+			}
+			pCoexDm->preAlgorithm = pCoexDm->curAlgorithm;
+		}
+	}
+}
+
+//============================================================
+// work around function start with wa_halbtc8723a2ant_
+//============================================================
+VOID
+wa_halbtc8723a2ant_MonitorC2h(
+	IN	PBTC_COEXIST			pBtCoexist
+	)
+{
+	u1Byte	tmp1b=0x0;
+	u4Byte	curC2hTotalCnt=0x0;
+	static u4Byte	preC2hTotalCnt=0x0, sameCntPollingTime=0x0;
+
+	curC2hTotalCnt+=pCoexSta->btInfoC2hCnt[BT_INFO_SRC_8723A_2ANT_BT_RSP];
+
+	if(curC2hTotalCnt == preC2hTotalCnt)
+	{
+		sameCntPollingTime++;
+	}
+	else
+	{
+		preC2hTotalCnt = curC2hTotalCnt;
+		sameCntPollingTime = 0;
+	}
+
+	if(sameCntPollingTime >= 2)
+	{
+		tmp1b = pBtCoexist->btc_read_1byte(pBtCoexist, 0x1af);
+		if(tmp1b != 0x0)
+		{
+			pCoexSta->c2hHangDetectCnt++;
+			pBtCoexist->btc_write_1byte(pBtCoexist, 0x1af, 0x0);
+		}
+	}
+}
+
+//============================================================
+// extern function start with EXhalbtc8723a2ant_
+//============================================================
+VOID
+EXhalbtc8723a2ant_InitHwConfig(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	u4Byte	u4Tmp=0;
+	u1Byte	u1Tmp=0;
+
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], 2Ant Init HW Config!!\n"));
+
+	// backup rf 0x1e value
+	pCoexDm->btRf0x1eBackup = 
+		pBtCoexist->btc_get_rf_reg(pBtCoexist, BTC_RF_A, 0x1e, 0xfffff);
+
+	// Enable counter statistics
+	pBtCoexist->btc_write_1byte(pBtCoexist, 0x76e, 0x4);
+	pBtCoexist->btc_write_1byte(pBtCoexist, 0x778, 0x3);
+	pBtCoexist->btc_write_1byte(pBtCoexist, 0x40, 0x20);
+}
+
+VOID
+EXhalbtc8723a2ant_InitCoexDm(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], Coex Mechanism Init!!\n"));
+	
+	halbtc8723a2ant_InitCoexDm(pBtCoexist);
+}
+
+VOID
+EXhalbtc8723a2ant_DisplayCoexInfo(
+	IN	PBTC_COEXIST		pBtCoexist
+	)
+{
+	struct btc_board_info *		pBoardInfo=&pBtCoexist->board_info;
+	PBTC_STACK_INFO		pStackInfo=&pBtCoexist->stack_info;
+	pu1Byte				cliBuf=pBtCoexist->cli_buf;
+	u1Byte				u1Tmp[4], i, btInfoExt, psTdmaCase=0;
+	u4Byte				u4Tmp[4];
+	BOOLEAN				bRoam=FALSE, bScan=FALSE, bLink=FALSE, bWifiUnder5G=FALSE;
+	BOOLEAN				bBtHsOn=FALSE, bWifiBusy=FALSE;
+	s4Byte				wifiRssi=0, btHsRssi=0;
+	u4Byte				wifiBw, wifiTrafficDir;
+	u1Byte				wifiDot11Chnl, wifiHsChnl;
+
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n ============[BT Coexist info]============");
+	CL_PRINTF(cliBuf);
+
+	if(!pBoardInfo->bt_exist)
+	{
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n BT not exists !!!");
+		CL_PRINTF(cliBuf);
+		return;
+	}
+
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ", "Ant PG number/ Ant mechanism:", \
+		pBoardInfo->pg_ant_num, pBoardInfo->btdm_ant_num);
+	CL_PRINTF(cliBuf);	
+	
+	if(pBtCoexist->manual_control)
+	{
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "[Action Manual control]!!");
+		CL_PRINTF(cliBuf);
+	}
+	
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %d", "BT stack/ hci ext ver", \
+		((pStackInfo->bProfileNotified)? "Yes":"No"), pStackInfo->hciVersion);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U1_WIFI_DOT11_CHNL, &wifiDot11Chnl);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifiHsChnl);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d(%d)", "Dot11 channel / HsChnl(HsMode)", \
+		wifiDot11Chnl, wifiHsChnl, bBtHsOn);
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x ", "H2C Wifi inform bt chnl Info", \
+		pCoexDm->wifiChnlInfo[0], pCoexDm->wifiChnlInfo[1],
+		pCoexDm->wifiChnlInfo[2]);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_S4_WIFI_RSSI, &wifiRssi);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_S4_HS_RSSI, &btHsRssi);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d", "Wifi rssi/ HS rssi", \
+		wifiRssi, btHsRssi);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_SCAN, &bScan);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_LINK, &bLink);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_ROAM, &bRoam);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ", "Wifi bLink/ bRoam/ bScan", \
+		bLink, bRoam, bScan);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_UNDER_5G, &bWifiUnder5G);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION, &wifiTrafficDir);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %s/ %s ", "Wifi status", \
+		(bWifiUnder5G? "5G":"2.4G"),
+		((BTC_WIFI_BW_LEGACY==wifiBw)? "Legacy": (((BTC_WIFI_BW_HT40==wifiBw)? "HT40":"HT20"))),
+		((!bWifiBusy)? "idle": ((BTC_WIFI_TRAFFIC_TX==wifiTrafficDir)? "uplink":"downlink")));
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = [%s/ %d/ %d] ", "BT [status/ rssi/ retryCnt]", \
+		((pCoexSta->bC2hBtInquiryPage)?("inquiry/page scan"):((BT_8723A_2ANT_BT_STATUS_IDLE == pCoexDm->btStatus)? "idle":(  (BT_8723A_2ANT_BT_STATUS_CONNECTED_IDLE == pCoexDm->btStatus)? "connected-idle":"busy"))),
+		pCoexSta->btRssi, pCoexSta->btRetryCnt);
+	CL_PRINTF(cliBuf);
+	
+	if(pStackInfo->bProfileNotified)
+	{			
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d / %d / %d", "SCO/HID/PAN/A2DP", \
+			pStackInfo->bScoExist, pStackInfo->bHidExist, pStackInfo->bPanExist, pStackInfo->bA2dpExist);
+		CL_PRINTF(cliBuf);	
+
+		pBtCoexist->btc_disp_dbg_msg(pBtCoexist, BTC_DBG_DISP_BT_LINK_INFO);
+	}
+
+	btInfoExt = pCoexSta->btInfoExt;
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s", "BT Info A2DP rate", \
+		(btInfoExt&BIT0)? "Basic rate":"EDR rate");
+	CL_PRINTF(cliBuf);	
+
+	for(i=0; i<BT_INFO_SRC_8723A_2ANT_MAX; i++)
+	{
+		if(pCoexSta->btInfoC2hCnt[i])
+		{				
+			CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x %02x %02x %02x %02x(%d)", GLBtInfoSrc8723a2Ant[i], \
+				pCoexSta->btInfoC2h[i][0], pCoexSta->btInfoC2h[i][1],
+				pCoexSta->btInfoC2h[i][2], pCoexSta->btInfoC2h[i][3],
+				pCoexSta->btInfoC2h[i][4], pCoexSta->btInfoC2h[i][5],
+				pCoexSta->btInfoC2h[i][6], pCoexSta->btInfoC2hCnt[i]);
+			CL_PRINTF(cliBuf);
+		}
+	}
+
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d", "write 0x1af=0x0 num", \
+		pCoexSta->c2hHangDetectCnt);
+	CL_PRINTF(cliBuf);
+	
+	// Sw mechanism	
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Sw mechanism]============");
+	CL_PRINTF(cliBuf);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d", "SM1[ShRf/ LpRA/ LimDig]", \
+		pCoexDm->bCurRfRxLpfShrink, pCoexDm->bCurLowPenaltyRa, pCoexDm->limited_dig);
+	CL_PRINTF(cliBuf);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d(0x%x) ", "SM2[AgcT/ AdcB/ SwDacSwing(lvl)]", \
+		pCoexDm->bCurAgcTableEn, pCoexDm->bCurAdcBackOff, pCoexDm->bCurDacSwingOn, pCoexDm->curDacSwingLvl);
+	CL_PRINTF(cliBuf);
+
+	// Fw mechanism		
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Fw mechanism]============");
+	CL_PRINTF(cliBuf);	
+	
+	if(!pBtCoexist->manual_control)
+	{
+		psTdmaCase = pCoexDm->curPsTdma;
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x %02x %02x case-%d", "PS TDMA", \
+			pCoexDm->psTdmaPara[0], pCoexDm->psTdmaPara[1],
+			pCoexDm->psTdmaPara[2], pCoexDm->psTdmaPara[3],
+			pCoexDm->psTdmaPara[4], psTdmaCase);
+		CL_PRINTF(cliBuf);
+	
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ", "DecBtPwr/ IgnWlanAct", \
+			pCoexDm->bCurDecBtPwr, pCoexDm->bCurIgnoreWlanAct);
+		CL_PRINTF(cliBuf);
+	}
+
+	// Hw setting		
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Hw setting]============");
+	CL_PRINTF(cliBuf);	
+
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "RF-A, 0x1e initVal", \
+		pCoexDm->btRf0x1eBackup);
+	CL_PRINTF(cliBuf);
+
+	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x778);
+	u1Tmp[1] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x783);
+	u1Tmp[2] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x796);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x", "0x778/ 0x783/ 0x796", \
+		u1Tmp[0], u1Tmp[1], u1Tmp[2]);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x880);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x880", \
+		u4Tmp[0]);
+	CL_PRINTF(cliBuf);
+
+	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x40);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x40", \
+		u1Tmp[0]);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x550);
+	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x522);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x", "0x550(bcn ctrl)/0x522", \
+		u4Tmp[0], u1Tmp[0]);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x484);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x484(rate adaptive)", \
+		u4Tmp[0]);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0xc50);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0xc50(dig)", \
+		u4Tmp[0]);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0xda0);
+	u4Tmp[1] = pBtCoexist->btc_read_4byte(pBtCoexist, 0xda4);
+	u4Tmp[2] = pBtCoexist->btc_read_4byte(pBtCoexist, 0xda8);
+	u4Tmp[3] = pBtCoexist->btc_read_4byte(pBtCoexist, 0xdac);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x", "0xda0/0xda4/0xda8/0xdac(FA cnt)", \
+		u4Tmp[0], u4Tmp[1], u4Tmp[2], u4Tmp[3]);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x6c0);
+	u4Tmp[1] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x6c4);
+	u4Tmp[2] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x6c8);
+	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x6cc);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x", "0x6c0/0x6c4/0x6c8/0x6cc(coexTable)", \
+		u4Tmp[0], u4Tmp[1], u4Tmp[2], u1Tmp[0]);
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d", "0x770 (hp rx[31:16]/tx[15:0])", \
+		pCoexSta->highPriorityRx, pCoexSta->highPriorityTx);
+	CL_PRINTF(cliBuf);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d", "0x774(lp rx[31:16]/tx[15:0])", \
+		pCoexSta->lowPriorityRx, pCoexSta->lowPriorityTx);
+	CL_PRINTF(cliBuf);
+
+	// Tx mgnt queue hang or not, 0x41b should = 0xf, ex: 0xd ==>hang
+	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x41b);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x41b (mgntQ hang chk == 0xf)", \
+		u1Tmp[0]);
+	CL_PRINTF(cliBuf);	
+
+	pBtCoexist->btc_disp_dbg_msg(pBtCoexist, BTC_DBG_DISP_COEX_STATISTICS);
+}
+
+
+VOID
+EXhalbtc8723a2ant_IpsNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			type
+	)
+{
+	if(BTC_IPS_ENTER == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], IPS ENTER notify\n"));
+		halbtc8723a2ant_CoexAllOff(pBtCoexist);
+	}
+	else if(BTC_IPS_LEAVE == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], IPS LEAVE notify\n"));
+		//halbtc8723a2ant_InitCoexDm(pBtCoexist);
+	}
+}
+
+VOID
+EXhalbtc8723a2ant_LpsNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			type
+	)
+{
+	if(BTC_LPS_ENABLE == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], LPS ENABLE notify\n"));
+	}
+	else if(BTC_LPS_DISABLE == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], LPS DISABLE notify\n"));
+	}
+}
+
+VOID
+EXhalbtc8723a2ant_ScanNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			type
+	)
+{
+	if(BTC_SCAN_START == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], SCAN START notify\n"));
+	}
+	else if(BTC_SCAN_FINISH == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], SCAN FINISH notify\n"));
+	}
+}
+
+VOID
+EXhalbtc8723a2ant_ConnectNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			type
+	)
+{
+	if(BTC_ASSOCIATE_START == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], CONNECT START notify\n"));
+	}
+	else if(BTC_ASSOCIATE_FINISH == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], CONNECT FINISH notify\n"));
+	}
+}
+
+VOID
+EXhalbtc8723a2ant_MediaStatusNotify(
+	IN	PBTC_COEXIST			pBtCoexist,
+	IN	u1Byte				type
+	)
+{
+	if(BTC_MEDIA_CONNECT == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], MEDIA connect notify\n"));
+	}
+	else
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], MEDIA disconnect notify\n"));
+	}
+
+	halbtc8723a2ant_IndicateWifiChnlBwInfo(pBtCoexist, type);
+}
+
+VOID
+EXhalbtc8723a2ant_SpecialPacketNotify(
+	IN	PBTC_COEXIST			pBtCoexist,
+	IN	u1Byte				type
+	)
+{
+	if(type == BTC_PACKET_DHCP)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], DHCP Packet notify\n"));
+	}
+}
+
+VOID
+EXhalbtc8723a2ant_BtInfoNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	pu1Byte			tmpBuf,
+	IN	u1Byte			length
+	)
+{
+	u1Byte			btInfo=0;
+	u1Byte			i, rspSource=0;
+	BOOLEAN			bBtBusy=FALSE, limited_dig=FALSE;
+	BOOLEAN			bWifiConnected=FALSE, bBtHsOn=FALSE;
+
+	pCoexSta->bC2hBtInfoReqSent = FALSE;
+	
+	rspSource = BT_INFO_SRC_8723A_2ANT_BT_RSP;
+	pCoexSta->btInfoC2hCnt[rspSource]++;
+
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Bt info[%d], length=%d, hex data=[", rspSource, length));
+	for(i=0; i<length; i++)
+	{
+		pCoexSta->btInfoC2h[rspSource][i] = tmpBuf[i];
+		if(i == 0)
+			btInfo = tmpBuf[i];
+		if(i == length-1)
+		{
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("0x%02x]\n", tmpBuf[i]));
+		}
+		else
+		{
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("0x%02x, ", tmpBuf[i]));
+		}
+	}
+
+	if(BT_INFO_SRC_8723A_2ANT_WIFI_FW != rspSource)
+	{
+		pCoexSta->btRetryCnt =
+			pCoexSta->btInfoC2h[rspSource][1];
+
+		pCoexSta->btRssi =
+			pCoexSta->btInfoC2h[rspSource][2]*2+10;
+
+		pCoexSta->btInfoExt = 
+			pCoexSta->btInfoC2h[rspSource][3];
+	}
+		
+	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	// check BIT2 first ==> check if bt is under inquiry or page scan
+	if(btInfo & BT_INFO_8723A_2ANT_B_INQ_PAGE)
+	{
+		pCoexSta->bC2hBtInquiryPage = true;
+	}
+	else
+	{
+		pCoexSta->bC2hBtInquiryPage = FALSE;
+	}
+}
+
+VOID
+EXhalbtc8723a2ant_StackOperationNotify(
+	IN	PBTC_COEXIST			pBtCoexist,
+	IN	u1Byte				type
+	)
+{
+	if(BTC_STACK_OP_INQ_PAGE_PAIR_START == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], StackOP Inquiry/page/pair start notify\n"));
+		pCoexSta->bHoldForStackOperation = true;
+		pCoexSta->bHoldPeriodCnt = 1;
+		halbtc8723a2ant_BtInquiryPage(pBtCoexist);
+	}
+	else if(BTC_STACK_OP_INQ_PAGE_PAIR_FINISH == type)
+	{
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], StackOP Inquiry/page/pair finish notify\n"));
+		pCoexSta->bHoldForStackOperation = FALSE;
+	}
+}
+
+VOID
+EXhalbtc8723a2ant_HaltNotify(
+	IN	PBTC_COEXIST			pBtCoexist
+	)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Halt notify\n"));
+
+	halbtc8723a2ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, true);
+	EXhalbtc8723a2ant_MediaStatusNotify(pBtCoexist, BTC_MEDIA_DISCONNECT);
+}
+
+VOID
+EXhalbtc8723a2ant_Periodical(
+	IN	PBTC_COEXIST			pBtCoexist
+	)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], 2Ant Periodical!!\n"));
+
+	// work around for c2h hang
+	wa_halbtc8723a2ant_MonitorC2h(pBtCoexist);
+	
+	halbtc8723a2ant_QueryBtInfo(pBtCoexist);
+	halbtc8723a2ant_MonitorBtCtr(pBtCoexist);
+	halbtc8723a2ant_MonitorBtEnableDisable(pBtCoexist);
+
+	halbtc8723a2ant_RunCoexistMechanism(pBtCoexist);
+}
+
+
+#endif
+
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtc8723a2ant.h b/drivers/staging/rtl8821ae/btcoexist/halbtc8723a2ant.h
new file mode 100644
index 000000000000..c07d3738aadc
--- /dev/null
+++ b/drivers/staging/rtl8821ae/btcoexist/halbtc8723a2ant.h
@@ -0,0 +1,179 @@
+//===========================================
+// The following is for 8723A 2Ant BT Co-exist definition
+//===========================================
+#define	BT_INFO_8723A_2ANT_B_FTP						BIT7
+#define	BT_INFO_8723A_2ANT_B_A2DP					BIT6
+#define	BT_INFO_8723A_2ANT_B_HID						BIT5
+#define	BT_INFO_8723A_2ANT_B_SCO_BUSY				BIT4
+#define	BT_INFO_8723A_2ANT_B_ACL_BUSY				BIT3
+#define	BT_INFO_8723A_2ANT_B_INQ_PAGE				BIT2
+#define	BT_INFO_8723A_2ANT_B_SCO_ESCO				BIT1
+#define	BT_INFO_8723A_2ANT_B_CONNECTION				BIT0
+
+#define		BTC_RSSI_COEX_THRESH_TOL_8723A_2ANT		2
+
+typedef enum _BT_INFO_SRC_8723A_2ANT{
+	BT_INFO_SRC_8723A_2ANT_WIFI_FW			= 0x0,
+	BT_INFO_SRC_8723A_2ANT_BT_RSP				= 0x1,
+	BT_INFO_SRC_8723A_2ANT_BT_ACTIVE_SEND		= 0x2,
+	BT_INFO_SRC_8723A_2ANT_MAX
+}BT_INFO_SRC_8723A_2ANT,*PBT_INFO_SRC_8723A_2ANT;
+
+typedef enum _BT_8723A_2ANT_BT_STATUS{
+	BT_8723A_2ANT_BT_STATUS_IDLE				= 0x0,
+	BT_8723A_2ANT_BT_STATUS_CONNECTED_IDLE	= 0x1,
+	BT_8723A_2ANT_BT_STATUS_NON_IDLE			= 0x2,
+	BT_8723A_2ANT_BT_STATUS_MAX
+}BT_8723A_2ANT_BT_STATUS,*PBT_8723A_2ANT_BT_STATUS;
+
+typedef enum _BT_8723A_2ANT_COEX_ALGO{
+	BT_8723A_2ANT_COEX_ALGO_UNDEFINED			= 0x0,
+	BT_8723A_2ANT_COEX_ALGO_SCO				= 0x1,
+	BT_8723A_2ANT_COEX_ALGO_HID				= 0x2,
+	BT_8723A_2ANT_COEX_ALGO_A2DP				= 0x3,
+	BT_8723A_2ANT_COEX_ALGO_PANEDR			= 0x4,
+	BT_8723A_2ANT_COEX_ALGO_PANHS			= 0x5,
+	BT_8723A_2ANT_COEX_ALGO_PANEDR_A2DP		= 0x6,
+	BT_8723A_2ANT_COEX_ALGO_PANEDR_HID		= 0x7,
+	BT_8723A_2ANT_COEX_ALGO_HID_A2DP_PANEDR	= 0x8,
+	BT_8723A_2ANT_COEX_ALGO_HID_A2DP			= 0x9,
+	BT_8723A_2ANT_COEX_ALGO_MAX
+}BT_8723A_2ANT_COEX_ALGO,*PBT_8723A_2ANT_COEX_ALGO;
+
+typedef struct _COEX_DM_8723A_2ANT{
+	// fw mechanism
+	BOOLEAN		bPreDecBtPwr;
+	BOOLEAN		bCurDecBtPwr;
+	//BOOLEAN		bPreBtLnaConstrain;
+	//BOOLEAN		bCurBtLnaConstrain;
+	//u1Byte		bPreBtPsdMode;
+	//u1Byte		bCurBtPsdMode;
+	u1Byte		preFwDacSwingLvl;
+	u1Byte		curFwDacSwingLvl;
+	BOOLEAN		bCurIgnoreWlanAct;
+	BOOLEAN		bPreIgnoreWlanAct;
+	u1Byte		prePsTdma;
+	u1Byte		curPsTdma;
+	u1Byte		psTdmaPara[5];
+	u1Byte		psTdmaDuAdjType;
+	BOOLEAN		bResetTdmaAdjust;
+	BOOLEAN		bPrePsTdmaOn;
+	BOOLEAN		bCurPsTdmaOn;
+	//BOOLEAN		bPreBtAutoReport;
+	//BOOLEAN		bCurBtAutoReport;
+
+	// sw mechanism
+	BOOLEAN		bPreRfRxLpfShrink;
+	BOOLEAN		bCurRfRxLpfShrink;
+	u4Byte		btRf0x1eBackup;
+	BOOLEAN 	bPreLowPenaltyRa;
+	BOOLEAN		bCurLowPenaltyRa;
+	BOOLEAN		bPreDacSwingOn;
+	u4Byte		preDacSwingLvl;
+	BOOLEAN		bCurDacSwingOn;
+	u4Byte		curDacSwingLvl;
+	BOOLEAN		bPreAdcBackOff;
+	BOOLEAN		bCurAdcBackOff;
+	BOOLEAN 	bPreAgcTableEn;
+	BOOLEAN		bCurAgcTableEn;
+	u4Byte		preVal0x6c0;
+	u4Byte		curVal0x6c0;
+	u4Byte		preVal0x6c8;
+	u4Byte		curVal0x6c8;
+	u1Byte		preVal0x6cc;
+	u1Byte		curVal0x6cc;
+	BOOLEAN		limited_dig;
+
+	// algorithm related
+	u1Byte		preAlgorithm;
+	u1Byte		curAlgorithm;
+	u1Byte		btStatus;
+	u1Byte		wifiChnlInfo[3];
+} COEX_DM_8723A_2ANT, *PCOEX_DM_8723A_2ANT;
+
+typedef struct _COEX_STA_8723A_2ANT{
+	u4Byte					highPriorityTx;
+	u4Byte					highPriorityRx;
+	u4Byte					lowPriorityTx;
+	u4Byte					lowPriorityRx;
+	u1Byte					btRssi;
+	u1Byte					preBtRssiState;
+	u1Byte					preBtRssiState1;
+	u1Byte					preWifiRssiState[4];
+	BOOLEAN					bC2hBtInfoReqSent;
+	u1Byte					btInfoC2h[BT_INFO_SRC_8723A_2ANT_MAX][10];
+	u4Byte					btInfoC2hCnt[BT_INFO_SRC_8723A_2ANT_MAX];
+	BOOLEAN					bC2hBtInquiryPage;
+	u1Byte					btRetryCnt;
+	u1Byte					btInfoExt;
+	BOOLEAN					bHoldForStackOperation;
+	u1Byte					bHoldPeriodCnt;
+	// this is for c2h hang work-around
+	u4Byte					c2hHangDetectCnt;
+}COEX_STA_8723A_2ANT, *PCOEX_STA_8723A_2ANT;
+
+//===========================================
+// The following is interface which will notify coex module.
+//===========================================
+VOID
+EXhalbtc8723a2ant_InitHwConfig(
+	IN	PBTC_COEXIST		pBtCoexist
+	);
+VOID
+EXhalbtc8723a2ant_InitCoexDm(
+	IN	PBTC_COEXIST		pBtCoexist
+	);
+VOID
+EXhalbtc8723a2ant_IpsNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			type
+	);
+VOID
+EXhalbtc8723a2ant_LpsNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			type
+	);
+VOID
+EXhalbtc8723a2ant_ScanNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			type
+	);
+VOID
+EXhalbtc8723a2ant_ConnectNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	u1Byte			type
+	);
+VOID
+EXhalbtc8723a2ant_MediaStatusNotify(
+	IN	PBTC_COEXIST			pBtCoexist,
+	IN	u1Byte				type
+	);
+VOID
+EXhalbtc8723a2ant_SpecialPacketNotify(
+	IN	PBTC_COEXIST			pBtCoexist,
+	IN	u1Byte				type
+	);
+VOID
+EXhalbtc8723a2ant_HaltNotify(
+	IN	PBTC_COEXIST			pBtCoexist
+	);
+VOID
+EXhalbtc8723a2ant_Periodical(
+	IN	PBTC_COEXIST			pBtCoexist
+	);
+VOID
+EXhalbtc8723a2ant_BtInfoNotify(
+	IN	PBTC_COEXIST		pBtCoexist,
+	IN	pu1Byte			tmpBuf,
+	IN	u1Byte			length
+	);
+VOID
+EXhalbtc8723a2ant_StackOperationNotify(
+	IN	PBTC_COEXIST			pBtCoexist,
+	IN	u1Byte				type
+	);
+VOID
+EXhalbtc8723a2ant_DisplayCoexInfo(
+	IN	PBTC_COEXIST		pBtCoexist
+	);
+
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtc8723b1ant.c b/drivers/staging/rtl8821ae/btcoexist/halbtc8723b1ant.c
new file mode 100644
index 000000000000..3414ba78cc43
--- /dev/null
+++ b/drivers/staging/rtl8821ae/btcoexist/halbtc8723b1ant.c
@@ -0,0 +1,4104 @@
+/***************************************************************
+ * Description:
+ *
+ * This file is for RTL8723B Co-exist mechanism
+ *
+ * History
+ * 2012/11/15 Cosa first check in.
+ *
+ ***************************************************************/
+
+
+/***************************************************************
+ * include files
+ ***************************************************************/
+#include "halbt_precomp.h"
+#if 1
+/***************************************************************
+ * Global variables, these are static variables
+ ***************************************************************/
+static struct coex_dm_8723b_1ant glcoex_dm_8723b_1ant;
+static struct coex_dm_8723b_1ant *coex_dm = &glcoex_dm_8723b_1ant;
+static struct coex_sta_8723b_1ant glcoex_sta_8723b_1ant;
+static struct coex_sta_8723b_1ant *coex_sta = &glcoex_sta_8723b_1ant;
+
+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;
+
+/***************************************************************
+ * local function proto type if needed
+ ***************************************************************/
+/***************************************************************
+ * local function start with halbtc8723b1ant_
+ ***************************************************************/
+u8 halbtc8723b1ant_bt_rssi_state(u8 level_num, u8 rssi_thresh, u8 rssi_thresh1)
+{
+	s32 bt_rssi=0;
+	u8 bt_rssi_state = coex_sta->pre_bt_rssi_state;
+
+	bt_rssi = coex_sta->bt_rssi;
+
+	if (level_num == 2){
+		if ((coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
+		    (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+			if (bt_rssi >= rssi_thresh +
+					BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT) {
+				bt_rssi_state = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state "
+					  "switch to High\n");
+			} else {
+				bt_rssi_state = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state "
+					  "stay at Low\n");
+			}
+		} else {
+			if (bt_rssi < rssi_thresh) {
+				bt_rssi_state = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state "
+					  "switch to Low\n");
+			} else {
+				bt_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state "
+					  "stay at High\n");
+			}
+		}
+	} else if (level_num == 3) {
+		if (rssi_thresh > rssi_thresh1) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+				  "[BTCoex], BT Rssi thresh error!!\n");
+			return coex_sta->pre_bt_rssi_state;
+		}
+
+		if ((coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
+		    (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+			if (bt_rssi >= rssi_thresh +
+					BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT) {
+				bt_rssi_state = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state "
+					  "switch to Medium\n");
+			} else {
+				bt_rssi_state = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state "
+					  "stay at Low\n");
+			}
+		} else if ((coex_sta->pre_bt_rssi_state ==
+					BTC_RSSI_STATE_MEDIUM) ||
+			  (coex_sta->pre_bt_rssi_state ==
+			  		BTC_RSSI_STATE_STAY_MEDIUM)) {
+			if (bt_rssi >= rssi_thresh1 +
+					BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT) {
+				bt_rssi_state = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state "
+					  "switch to High\n");
+			} else if (bt_rssi < rssi_thresh) {
+				bt_rssi_state = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state "
+					  "switch to Low\n");
+			} else {
+				bt_rssi_state = BTC_RSSI_STATE_STAY_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state "
+					  "stay at Medium\n");
+			}
+		} else {
+			if (bt_rssi < rssi_thresh1) {
+				bt_rssi_state = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state "
+					  "switch to Medium\n");
+			} else {
+				bt_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state "
+					  "stay at High\n");
+			}
+		}
+	}
+
+	coex_sta->pre_bt_rssi_state = bt_rssi_state;
+
+	return bt_rssi_state;
+}
+
+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;
+}
+
+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;
+}
+
+void halbtc8723b1ant_auto_rate_fallback_retry(struct btc_coexist *btcoexist,
+					      bool force_exec, u8 type)
+{
+	bool wifi_under_bmode = false;
+
+	coex_dm->cur_arfr_type = type;
+
+	if (force_exec || (coex_dm->pre_arfr_type != coex_dm->cur_arfr_type)) {
+		switch (coex_dm->cur_arfr_type) {
+		case 0:	/* normal mode */
+			btcoexist->btc_write_4byte(btcoexist, 0x430,
+						   coex_dm->backup_arfr_cnt1);
+			btcoexist->btc_write_4byte(btcoexist, 0x434,
+						   coex_dm->backup_arfr_cnt2);
+			break;
+		case 1:
+			btcoexist->btc_get(btcoexist,
+					   BTC_GET_BL_WIFI_UNDER_B_MODE,
+					   &wifi_under_bmode);
+			if (wifi_under_bmode) {
+				btcoexist->btc_write_4byte(btcoexist,
+							   0x430, 0x0);
+				btcoexist->btc_write_4byte(btcoexist,
+							   0x434, 0x01010101);
+			} else {
+				btcoexist->btc_write_4byte(btcoexist,
+							   0x430, 0x0);
+				btcoexist->btc_write_4byte(btcoexist,
+							   0x434, 0x04030201);
+			}
+			break;
+		default:
+			break;
+		}
+	}
+
+	coex_dm->pre_arfr_type = coex_dm->cur_arfr_type;
+}
+
+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;
+}
+
+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;
+}
+
+void halbtc8723b1ant_limited_tx(struct btc_coexist *btcoexist,
+				bool force_exec, u8 ra_maskType, u8 arfr_type,
+				u8 retry_limit_type, u8 ampdu_time_type)
+{
+	switch (ra_maskType) {
+	case 0:	/* normal mode */
+		halbtc8723b1ant_updatera_mask(btcoexist, force_exec, 0x0);
+		break;
+	case 1:	/* disable cck 1/2 */
+		halbtc8723b1ant_updatera_mask(btcoexist, force_exec,
+					      0x00000003);
+		break;
+	/* disable cck 1/2/5.5, ofdm 6/9/12/18/24, mcs 0/1/2/3/4*/
+	case 2:
+		halbtc8723b1ant_updatera_mask(btcoexist, force_exec,
+					      0x0001f1f7);
+		break;
+	default:
+		break;
+	}
+
+	halbtc8723b1ant_auto_rate_fallback_retry(btcoexist, force_exec,
+						 arfr_type);
+	halbtc8723b1ant_retry_limit(btcoexist, force_exec, retry_limit_type);
+	halbtc8723b1ant_ampdu_maxtime(btcoexist, force_exec, ampdu_time_type);
+}
+
+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);
+}
+
+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);
+}
+
+void halbtc8723b1ant_query_bt_info(struct btc_coexist *btcoexist)
+{
+	u8 h2c_parameter[1] = {0};
+
+	coex_sta->c2h_bt_info_req_sent = true;
+
+	h2c_parameter[0] |= BIT0;	/* trigger*/
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], Query Bt Info, FW write 0x61=0x%x\n",
+		  h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x61, 1, h2c_parameter);
+}
+
+bool halbtc8723b1ant_is_wifi_status_changed(struct btc_coexist *btcoexist)
+{
+	static bool pre_wifi_busy = false;
+	static bool pre_under_4way = false, pre_bt_hs_on = false;
+	bool wifi_busy = false, under_4way = false, bt_hs_on = false;
+	bool wifi_connected = false;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+			   &wifi_connected);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+ 	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS,
+			   &under_4way);
+
+	if (wifi_connected) {
+		if (wifi_busy != pre_wifi_busy) {
+			pre_wifi_busy = wifi_busy;
+			return true;
+		}
+		if (under_4way != pre_under_4way) {
+			pre_under_4way = under_4way;
+			return true;
+		}
+		if (bt_hs_on != pre_bt_hs_on) {
+			pre_bt_hs_on = bt_hs_on;
+			return true;
+		}
+	}
+
+	return false;
+}
+
+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;
+}
+
+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;
+}
+
+bool halbtc8723b1ant_need_to_dec_bt_pwr(struct btc_coexist *btcoexist)
+{
+	bool ret = false;
+	bool bt_hs_on = false, wifi_connected = false;
+	s32 bt_hs_rssi = 0;
+	u8 bt_rssi_state;
+
+	if (!btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on))
+		return false;
+	if (!btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+				&wifi_connected))
+		return false;
+	if (!btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi))
+		return false;
+
+	bt_rssi_state = halbtc8723b1ant_bt_rssi_state(2, 35, 0);
+
+	if (wifi_connected) {
+		if (bt_hs_on) {
+			if (bt_hs_rssi > 37)
+				ret = true;
+		} else {
+			if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+				ret = true;
+		}
+	}
+
+	return ret;
+}
+
+void halbtc8723b1ant_set_fw_dac_swing_level(struct btc_coexist *btcoexist,
+					    u8 dac_swing_lvl)
+{
+	u8 h2c_parameter[1] = {0};
+
+	/* There are several type of dacswing
+	 * 0x18/ 0x10/ 0xc/ 0x8/ 0x4/ 0x6 */
+	h2c_parameter[0] = dac_swing_lvl;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], Set Dac Swing Level=0x%x\n", dac_swing_lvl);
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], FW write 0x64=0x%x\n", h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x64, 1, h2c_parameter);
+}
+
+void halbtc8723b1ant_set_fw_dec_bt_pwr(struct btc_coexist *btcoexist,
+				       bool dec_bt_pwr)
+{
+	u8 h2c_parameter[1] = {0};
+
+	h2c_parameter[0] = 0;
+
+	if (dec_bt_pwr)
+		h2c_parameter[0] |= BIT1;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], decrease Bt Power : %s, FW write 0x62=0x%x\n",
+		  (dec_bt_pwr? "Yes!!":"No!!"),h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x62, 1, h2c_parameter);
+}
+
+void halbtc8723b1ant_dec_bt_pwr(struct btc_coexist *btcoexist,
+				bool force_exec, bool dec_bt_pwr)
+{
+	return;
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s Dec BT power = %s\n",
+		  (force_exec ? "force to" : ""), (dec_bt_pwr ? "ON" : "OFF"));
+	coex_dm->cur_dec_bt_pwr = dec_bt_pwr;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], bPreDecBtPwr=%d, bCurDecBtPwr=%d\n",
+			  coex_dm->pre_dec_bt_pwr, coex_dm->cur_dec_bt_pwr);
+
+		if (coex_dm->pre_dec_bt_pwr == coex_dm->cur_dec_bt_pwr)
+			return;
+	}
+	halbtc8723b1ant_set_fw_dec_bt_pwr(btcoexist, coex_dm->cur_dec_bt_pwr);
+
+	coex_dm->pre_dec_bt_pwr = coex_dm->cur_dec_bt_pwr;
+}
+
+void halbtc8723b1ant_set_bt_auto_report(struct btc_coexist *btcoexist,
+					bool enable_auto_report)
+{
+	u8 h2c_parameter[1] = {0};
+
+	h2c_parameter[0] = 0;
+
+	if (enable_auto_report)
+		h2c_parameter[0] |= BIT0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], BT FW auto report : %s, FW write 0x68=0x%x\n",
+		  (enable_auto_report? "Enabled!!":"Disabled!!"),
+		  h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x68, 1, h2c_parameter);
+}
+
+void halbtc8723b1ant_bt_auto_report(struct btc_coexist *btcoexist,
+				    bool force_exec, bool enable_auto_report)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s BT Auto report = %s\n",
+		  (force_exec? "force to":""),
+		  ((enable_auto_report)? "Enabled":"Disabled"));
+	coex_dm->cur_bt_auto_report = enable_auto_report;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], bPreBtAutoReport=%d, "
+			  "bCurBtAutoReport=%d\n",
+			  coex_dm->pre_bt_auto_report,
+			  coex_dm->cur_bt_auto_report);
+
+		if (coex_dm->pre_bt_auto_report == coex_dm->cur_bt_auto_report)
+			return;
+	}
+	halbtc8723b1ant_set_bt_auto_report(btcoexist,
+					   coex_dm->cur_bt_auto_report);
+
+	coex_dm->pre_bt_auto_report = coex_dm->cur_bt_auto_report;
+}
+
+void halbtc8723b1ant_fw_dac_swing_lvl(struct btc_coexist *btcoexist,
+				      bool force_exec, u8 fw_dac_swing_lvl)
+{
+	return;
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s set FW Dac Swing level = %d\n",
+		  (force_exec? "force to":""), fw_dac_swing_lvl);
+	coex_dm->cur_fw_dac_swing_lvl = fw_dac_swing_lvl;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], preFwDacSwingLvl=%d, "
+			  "curFwDacSwingLvl=%d\n",
+			  coex_dm->pre_fw_dac_swing_lvl,
+			  coex_dm->cur_fw_dac_swing_lvl);
+
+		if (coex_dm->pre_fw_dac_swing_lvl ==
+		    coex_dm->cur_fw_dac_swing_lvl)
+			return;
+	}
+
+	halbtc8723b1ant_set_fw_dac_swing_level(btcoexist,
+					       coex_dm->cur_fw_dac_swing_lvl);
+
+	coex_dm->pre_fw_dac_swing_lvl = coex_dm->cur_fw_dac_swing_lvl;
+}
+
+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(
+					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 halbtc8723b1ant_low_penalty_ra(struct btc_coexist *btcoexist,
+				    bool force_exec, bool low_penalty_ra)
+{
+	coex_dm->cur_low_penalty_ra = low_penalty_ra;
+
+	if (!force_exec) {
+		if (coex_dm->pre_low_penalty_ra == coex_dm->cur_low_penalty_ra)
+			return;
+	}
+	halbtc8723b1ant_set_sw_penalty_tx_rate_adaptive(btcoexist,
+						coex_dm->cur_low_penalty_ra);
+
+	coex_dm->pre_low_penalty_ra = coex_dm->cur_low_penalty_ra;
+}
+
+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,
+				    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);
+}
+
+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;
+}
+
+void halbtc8723b1ant_coex_table_with_type(struct btc_coexist *btcoexist,
+					  bool force_exec, u8 type)
+{
+	switch (type) {
+	case 0:
+		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x55555555,
+					   0x55555555, 0xffffff, 0x3);
+		break;
+	case 1:
+		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x55555555,
+					   0x5a5a5a5a, 0xffffff, 0x3);
+		break;
+	case 2:
+		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x5a5a5a5a,
+					   0x5a5a5a5a, 0xffffff, 0x3);
+		break;
+	case 3:
+		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x55555555,
+					   0xaaaaaaaa, 0xffffff, 0x3);
+		break;
+	case 4:
+		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x55555555,
+					   0x5aaa5aaa, 0xffffff, 0x3);
+		break;
+	case 5:
+		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x5a5a5a5a,
+					   0xaaaa5a5a, 0xffffff, 0x3);
+		break;
+	case 6:
+		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x55555555,
+					   0xaaaa5a5a, 0xffffff, 0x3);
+		break;
+	case 7:
+		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x5afa5afa,
+					   0x5afa5afa, 0xffffff, 0x3);
+		break;
+	default:
+		break;
+	}
+}
+
+void halbtc8723b1ant_SetFwIgnoreWlanAct(struct btc_coexist *btcoexist,
+					bool enable)
+{
+	u8 h2c_parameter[1] = {0};
+
+	if (enable)
+		h2c_parameter[0] |= BIT0;	/* function enable */
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], set FW for BT Ignore Wlan_Act,"
+		  " FW write 0x63=0x%x\n", h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x63, 1, h2c_parameter);
+}
+
+void halbtc8723b1ant_ignore_wlan_act(struct btc_coexist *btcoexist,
+				     bool force_exec, bool enable)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s turn Ignore WlanAct %s\n",
+		  (force_exec ? "force to" : ""), (enable ? "ON" : "OFF"));
+	coex_dm->cur_ignore_wlan_act = enable;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], bPreIgnoreWlanAct = %d, "
+			  "bCurIgnoreWlanAct = %d!!\n",
+			  coex_dm->pre_ignore_wlan_act,
+			  coex_dm->cur_ignore_wlan_act);
+
+		if (coex_dm->pre_ignore_wlan_act ==
+		    coex_dm->cur_ignore_wlan_act)
+			return;
+	}
+	halbtc8723b1ant_SetFwIgnoreWlanAct(btcoexist, enable);
+
+	coex_dm->pre_ignore_wlan_act = coex_dm->cur_ignore_wlan_act;
+}
+
+void halbtc8723b1ant_set_fw_ps_tdma(struct btc_coexist *btcoexist,
+				    u8 byte1, u8 byte2, u8 byte3,
+				    u8 byte4, u8 byte5)
+{
+	u8 h2c_parameter[5] = {0};
+	u8 real_byte1 = byte1, real_byte5 = byte5;
+	bool ap_enable = false;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_AP_MODE_ENABLE,
+			   &ap_enable);
+
+	if (ap_enable) {
+		if ((byte1 & BIT4) && !(byte1 & BIT5)) {
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+				  "[BTCoex], FW for 1Ant AP mode\n");
+			real_byte1 &= ~BIT4;
+			real_byte1 |= BIT5;
+
+			real_byte5 |= BIT5;
+			real_byte5 &= ~BIT6;
+		}
+	}
+
+	h2c_parameter[0] = real_byte1;
+	h2c_parameter[1] = byte2;
+	h2c_parameter[2] = byte3;
+	h2c_parameter[3] = byte4;
+	h2c_parameter[4] = real_byte5;
+
+	coex_dm->ps_tdma_para[0] = real_byte1;
+	coex_dm->ps_tdma_para[1] = byte2;
+	coex_dm->ps_tdma_para[2] = byte3;
+	coex_dm->ps_tdma_para[3] = byte4;
+	coex_dm->ps_tdma_para[4] = real_byte5;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], PS-TDMA H2C cmd =0x%x%08x\n",
+		  h2c_parameter[0],
+		  h2c_parameter[1] << 24 |
+		  h2c_parameter[2] << 16 |
+		  h2c_parameter[3] << 8 |
+		  h2c_parameter[4]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x60, 5, h2c_parameter);
+}
+
+void halbtc8723b1ant_SetLpsRpwm(struct btc_coexist *btcoexist,
+				u8 lps_val, u8 rpwm_val)
+{
+	u8 lps = lps_val;
+	u8 rpwm = rpwm_val;
+
+	btcoexist->btc_set(btcoexist, BTC_SET_U1_1ANT_LPS, &lps);
+	btcoexist->btc_set(btcoexist, BTC_SET_U1_1ANT_RPWM, &rpwm);
+}
+
+void halbtc8723b1ant_LpsRpwm(struct btc_coexist *btcoexist, bool force_exec,
+			     u8 lps_val, u8 rpwm_val)
+{
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s set lps/rpwm=0x%x/0x%x \n",
+		  (force_exec ? "force to" : ""), lps_val, rpwm_val);
+	coex_dm->cur_lps = lps_val;
+	coex_dm->cur_rpwm = rpwm_val;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], LPS-RxBeaconMode=0x%x , LPS-RPWM=0x%x!!\n",
+			  coex_dm->cur_lps, coex_dm->cur_rpwm);
+
+		if ((coex_dm->pre_lps == coex_dm->cur_lps) &&
+		    (coex_dm->pre_rpwm == coex_dm->cur_rpwm)) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+				  "[BTCoex], LPS-RPWM_Last=0x%x"
+				  " , LPS-RPWM_Now=0x%x!!\n",
+				  coex_dm->pre_rpwm, coex_dm->cur_rpwm);
+
+			return;
+		}
+	}
+	halbtc8723b1ant_SetLpsRpwm(btcoexist, lps_val, rpwm_val);
+
+	coex_dm->pre_lps = coex_dm->cur_lps;
+	coex_dm->pre_rpwm = coex_dm->cur_rpwm;
+}
+
+void halbtc8723b1ant_sw_mechanism1(struct btc_coexist *btcoexist,
+				   bool shrink_rx_lpf, bool low_penalty_ra,
+				   bool limited_dig, bool bt_lna_constrain)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+		  "[BTCoex], SM1[ShRf/ LpRA/ LimDig/ btLna] = %d %d %d %d\n",
+		  shrink_rx_lpf, low_penalty_ra, limited_dig, bt_lna_constrain);
+
+	halbtc8723b1ant_low_penalty_ra(btcoexist, NORMAL_EXEC, low_penalty_ra);
+}
+
+void halbtc8723b1ant_sw_mechanism2(struct btc_coexist *btcoexist,
+				   bool agc_table_shift, bool adc_backoff,
+				   bool sw_dac_swing, u32 dac_swing_lvl)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+		  "[BTCoex], SM2[AgcT/ AdcB/ SwDacSwing(lvl)] = %d %d %d\n",
+		  agc_table_shift, adc_backoff, sw_dac_swing);
+}
+
+void halbtc8723b1ant_SetAntPath(struct btc_coexist *btcoexist,
+				u8 ant_pos_type, bool init_hw_cfg,
+				bool wifi_off)
+{
+	struct btc_board_info *board_info = &btcoexist->board_info;
+	u32 fw_ver = 0, u32tmp = 0;
+	bool pg_ext_switch = false;
+	bool use_ext_switch = false;
+	u8 h2c_parameter[2] = {0};
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_EXT_SWITCH, &pg_ext_switch);
+	/* [31:16]=fw ver, [15:0]=fw sub ver */
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
+
+
+	if ((fw_ver < 0xc0000) || pg_ext_switch)
+		use_ext_switch = true;
+
+	if (init_hw_cfg){
+		/*BT select s0/s1 is controlled by WiFi */
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x67, 0x20, 0x1);
+
+		/*Force GNT_BT to Normal */
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x765, 0x18, 0x0);
+	} else if (wifi_off) {
+		/*Force GNT_BT to High */
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x765, 0x18, 0x3);
+		/*BT select s0/s1 is controlled by BT */
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x67, 0x20, 0x0);
+
+		/* 0x4c[24:23]=00, Set Antenna control by BT_RFE_CTRL
+		 * BT Vendor 0xac=0xf002 */
+		u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
+		u32tmp &= ~BIT23;
+		u32tmp &= ~BIT24;
+		btcoexist->btc_write_4byte(btcoexist, 0x4c, u32tmp);
+	}
+
+	if (use_ext_switch) {
+		if (init_hw_cfg) {
+			/* 0x4c[23]=0, 0x4c[24]=1  Antenna control by WL/BT */
+			u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
+			u32tmp &= ~BIT23;
+			u32tmp |= BIT24;
+			btcoexist->btc_write_4byte(btcoexist, 0x4c, u32tmp);
+
+			if (board_info->btdm_ant_pos ==
+			    BTC_ANTENNA_AT_MAIN_PORT) {
+				/* Main Ant to  BT for IPS case 0x4c[23]=1 */
+				btcoexist->btc_write_1byte_bitmask(btcoexist,
+								   0x64, 0x1,
+								   0x1);
+
+				/*tell firmware "no antenna inverse"*/
+				h2c_parameter[0] = 0;
+				h2c_parameter[1] = 1;  /*ext switch type*/
+				btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
+							h2c_parameter);
+			} else {
+				/*Aux Ant to  BT for IPS case 0x4c[23]=1 */
+				btcoexist->btc_write_1byte_bitmask(btcoexist,
+								   0x64, 0x1,
+								   0x0);
+
+				/*tell firmware "antenna inverse"*/
+				h2c_parameter[0] = 1;
+				h2c_parameter[1] = 1;  /*ext switch type*/
+				btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
+							h2c_parameter);
+			}
+		}
+
+		/* fixed internal switch first*/
+		/* fixed internal switch S1->WiFi, S0->BT*/
+		if (board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
+ 			btcoexist->btc_write_2byte(btcoexist, 0x948, 0x0);
+		else/* fixed internal switch S0->WiFi, S1->BT*/
+			btcoexist->btc_write_2byte(btcoexist, 0x948, 0x280);
+
+		/* ext switch setting */
+		switch (ant_pos_type) {
+		case BTC_ANT_PATH_WIFI:
+			if (board_info->btdm_ant_pos ==
+			    BTC_ANTENNA_AT_MAIN_PORT)
+				btcoexist->btc_write_1byte_bitmask(btcoexist,
+								   0x92c, 0x3,
+								   0x1);
+			else
+				btcoexist->btc_write_1byte_bitmask(btcoexist,
+								   0x92c, 0x3,
+								   0x2);
+			break;
+		case BTC_ANT_PATH_BT:
+			if (board_info->btdm_ant_pos ==
+			    BTC_ANTENNA_AT_MAIN_PORT)
+				btcoexist->btc_write_1byte_bitmask(btcoexist,
+							    	   0x92c, 0x3,
+							    	   0x2);
+			else
+				btcoexist->btc_write_1byte_bitmask(btcoexist,
+							    	   0x92c, 0x3,
+							    	   0x1);
+			break;
+		default:
+		case BTC_ANT_PATH_PTA:
+			if (board_info->btdm_ant_pos ==
+			    BTC_ANTENNA_AT_MAIN_PORT)
+				btcoexist->btc_write_1byte_bitmask(btcoexist,
+							    	   0x92c, 0x3,
+							    	   0x1);
+			else
+				btcoexist->btc_write_1byte_bitmask(btcoexist,
+							    	   0x92c, 0x3,
+							    	   0x2);
+			break;
+		}
+
+	} else {
+		if (init_hw_cfg) {
+			/* 0x4c[23]=1, 0x4c[24]=0  Antenna control by 0x64*/
+			u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
+			u32tmp |= BIT23;
+			u32tmp &= ~BIT24;
+			btcoexist->btc_write_4byte(btcoexist, 0x4c, u32tmp);
+
+			if (board_info->btdm_ant_pos ==
+			    BTC_ANTENNA_AT_MAIN_PORT) {
+				/*Main Ant to  WiFi for IPS case 0x4c[23]=1*/
+				btcoexist->btc_write_1byte_bitmask(btcoexist,
+								   0x64, 0x1,
+								   0x0);
+
+				/*tell firmware "no antenna inverse"*/
+				h2c_parameter[0] = 0;
+				h2c_parameter[1] = 0;  /*internal switch type*/
+				btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
+							h2c_parameter);
+			} else {
+				/*Aux Ant to  BT for IPS case 0x4c[23]=1*/
+				btcoexist->btc_write_1byte_bitmask(btcoexist,
+								   0x64, 0x1,
+								   0x1);
+
+				/*tell firmware "antenna inverse"*/
+				h2c_parameter[0] = 1;
+				h2c_parameter[1] = 0;  /*internal switch type*/
+				btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
+							h2c_parameter);
+			}
+		}
+
+		/* fixed external switch first*/
+		/*Main->WiFi, Aux->BT*/
+		if(board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x92c,
+							   0x3, 0x1);
+		else/*Main->BT, Aux->WiFi */
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x92c,
+							   0x3, 0x2);
+
+		/* internal switch setting*/
+		switch (ant_pos_type) {
+		case BTC_ANT_PATH_WIFI:
+			if(board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
+				btcoexist->btc_write_2byte(btcoexist, 0x948,
+							   0x0);
+			else
+				btcoexist->btc_write_2byte(btcoexist, 0x948,
+							   0x280);
+			break;
+		case BTC_ANT_PATH_BT:
+			if(board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
+				btcoexist->btc_write_2byte(btcoexist, 0x948,
+							   0x280);
+			else
+				btcoexist->btc_write_2byte(btcoexist, 0x948,
+							   0x0);
+			break;
+		default:
+		case BTC_ANT_PATH_PTA:
+			if(board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
+				btcoexist->btc_write_2byte(btcoexist, 0x948,
+							   0x200);
+			else
+				btcoexist->btc_write_2byte(btcoexist, 0x948,
+							   0x80);
+			break;
+		}
+	}
+}
+
+void halbtc8723b1ant_ps_tdma(struct btc_coexist *btcoexist, bool force_exec,
+			     bool turn_on, u8 type)
+{
+	bool wifi_busy = false;
+	u8 rssi_adjust_val = 0;
+
+	coex_dm->cur_ps_tdma_on = turn_on;
+	coex_dm->cur_ps_tdma = type;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+
+	if (!force_exec) {
+		if (coex_dm->cur_ps_tdma_on)
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+				  "[BTCoex], ******** TDMA(on, %d) *********\n",
+				  coex_dm->cur_ps_tdma);
+		else
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+				  "[BTCoex], ******** TDMA(off, %d) ********\n",
+				  coex_dm->cur_ps_tdma);
+
+
+		if ((coex_dm->pre_ps_tdma_on == coex_dm->cur_ps_tdma_on) &&
+		    (coex_dm->pre_ps_tdma == coex_dm->cur_ps_tdma))
+			return;
+	}
+	if (turn_on) {
+		switch (type) {
+		default:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x1a,
+						       0x1a, 0x0, 0x50);
+			break;
+		case 1:
+			if (wifi_busy)
+				halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51,
+							       0x3a, 0x03,
+							       0x10, 0x50);
+			else
+				halbtc8723b1ant_set_fw_ps_tdma(btcoexist,0x51,
+							       0x3a, 0x03,
+							       0x10, 0x51);
+
+			rssi_adjust_val = 11;
+			break;
+		case 2:
+			if (wifi_busy)
+				halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51,
+							       0x2b, 0x03,
+							       0x10, 0x50);
+			else
+				halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51,
+							       0x2b, 0x03,
+							       0x10, 0x51);
+			rssi_adjust_val = 14;
+			break;
+		case 3:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x1d,
+						       0x1d, 0x0, 0x52);
+			break;
+		case 4:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x93, 0x15,
+						       0x3, 0x14, 0x0);
+			rssi_adjust_val = 17;
+			break;
+		case 5:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x61, 0x15,
+						       0x3, 0x11, 0x10);
+			break;
+		case 6:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x61, 0x20,
+						       0x3, 0x11, 0x13);
+			break;
+		case 7:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x13, 0xc,
+						       0x5, 0x0, 0x0);
+			break;
+		case 8:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x93, 0x25,
+						       0x3, 0x10, 0x0);
+			break;
+		case 9:
+			if(wifi_busy)
+				halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51,
+							       0x21, 0x3,
+							       0x10, 0x50);
+			else
+				halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51,
+							       0x21, 0x3,
+							       0x10, 0x50);
+			rssi_adjust_val = 18;
+			break;
+		case 10:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x13, 0xa,
+						       0xa, 0x0, 0x40);
+			break;
+		case 11:
+			if (wifi_busy)
+				halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51,
+							       0x15, 0x03,
+							       0x10, 0x50);
+			else
+				halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51,
+							       0x15, 0x03,
+							       0x10, 0x50);
+			rssi_adjust_val = 20;
+			break;
+		case 12:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x0a,
+						       0x0a, 0x0, 0x50);
+			break;
+		case 13:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x15,
+						       0x15, 0x0, 0x50);
+			break;
+		case 14:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x21,
+						       0x3, 0x10, 0x52);
+			break;
+		case 15:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x13, 0xa,
+						       0x3, 0x8, 0x0);
+			break;
+		case 16:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x93, 0x15,
+						       0x3, 0x10, 0x0);
+			rssi_adjust_val = 18;
+			break;
+		case 18:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x93, 0x25,
+						       0x3, 0x10, 0x0);
+			rssi_adjust_val = 14;
+			break;
+		case 20:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x61, 0x35,
+						       0x03, 0x11, 0x10);
+			break;
+		case 21:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x61, 0x15,
+						       0x03, 0x11, 0x10);
+			break;
+		case 22:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x61, 0x25,
+						       0x03, 0x11, 0x10);
+			break;
+		case 23:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x25,
+						       0x3, 0x31, 0x18);
+			rssi_adjust_val = 22;
+			break;
+		case 24:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x15,
+						       0x3, 0x31, 0x18);
+			rssi_adjust_val = 22;
+			break;
+		case 25:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xe3, 0xa,
+						       0x3, 0x31, 0x18);
+			rssi_adjust_val = 22;
+			break;
+		case 26:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xe3, 0xa,
+						       0x3, 0x31, 0x18);
+			rssi_adjust_val = 22;
+			break;
+		case 27:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x25,
+						       0x3, 0x31, 0x98);
+			rssi_adjust_val = 22;
+			break;
+		case 28:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x69, 0x25,
+						       0x3, 0x31, 0x0);
+			break;
+		case 29:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xab, 0x1a,
+						       0x1a, 0x1, 0x10);
+			break;
+		case 30:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x14,
+						       0x3, 0x10, 0x50);
+			break;
+		case 31:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xd3, 0x1a,
+						       0x1a, 0, 0x58);
+			break;
+		case 32:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x61, 0xa,
+						       0x3, 0x10, 0x0);
+			break;
+		case 33:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xa3, 0x25,
+						       0x3, 0x30, 0x90);
+			break;
+		case 34:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x53, 0x1a,
+						       0x1a, 0x0, 0x10);
+			break;
+		case 35:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x63, 0x1a,
+						       0x1a, 0x0, 0x10);
+			break;
+		case 36:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xd3, 0x12,
+						       0x3, 0x14, 0x50);
+			break;
+		/* SoftAP only with no sta associated,BT disable ,
+		 * TDMA mode for power saving
+		 * here softap mode screen off will cost 70-80mA for phone */
+		case 40:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x23, 0x18,
+						       0x00, 0x10, 0x24);
+			break;
+		}
+	} else {
+		switch (type) {
+		case 8: /*PTA Control */
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x8, 0x0,
+						       0x0, 0x0, 0x0);
+			halbtc8723b1ant_SetAntPath(btcoexist, BTC_ANT_PATH_PTA,
+						   false, false);
+			break;
+		case 0:
+		default:  /*Software control, Antenna at BT side */
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x0, 0x0,
+						       0x0, 0x0, 0x0);
+			halbtc8723b1ant_SetAntPath(btcoexist, BTC_ANT_PATH_BT,
+						   false, false);
+			break;
+		case 9:   /*Software control, Antenna at WiFi side */
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x0, 0x0,
+						       0x0, 0x0, 0x0);
+			halbtc8723b1ant_SetAntPath(btcoexist, BTC_ANT_PATH_WIFI,
+						   false, false);
+			break;
+		}
+	}
+	rssi_adjust_val = 0;
+	btcoexist->btc_set(btcoexist,
+			   BTC_SET_U1_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE,
+			   &rssi_adjust_val);
+
+	/* update pre state */
+	coex_dm->pre_ps_tdma_on = coex_dm->cur_ps_tdma_on;
+	coex_dm->pre_ps_tdma = coex_dm->cur_ps_tdma;
+}
+
+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)
+{
+	bool commom = false, wifi_connected = false;
+	bool wifi_busy = false;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+			   &wifi_connected);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+
+	if (!wifi_connected &&
+	    BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE == coex_dm->bt_status) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Wifi non connected-idle + "
+			  "BT non connected-idle!!\n");
+		halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+ 		halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
+					      false, false);
+		halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+					      false, 0x18);
+
+		commom = true;
+	} else if (wifi_connected &&
+		   (BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE ==
+		    coex_dm->bt_status)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Wifi connected + "
+			  "BT non connected-idle!!\n");
+		halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+      		halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
+					      false, false);
+		halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+					      false, 0x18);
+
+		commom = true;
+	} else if (!wifi_connected &&
+		   (BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE ==
+		    coex_dm->bt_status)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Wifi non connected-idle + "
+			  "BT connected-idle!!\n");
+		halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+		halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
+					      false, false);
+		halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+					      false, 0x18);
+
+		commom = true;
+	} else if (wifi_connected &&
+		   (BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE ==
+		    coex_dm->bt_status)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Wifi connected + BT connected-idle!!\n");
+		halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+		halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
+					      false, false);
+		halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+					      false, 0x18);
+
+		commom = true;
+	} else if (!wifi_connected &&
+		   (BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE !=
+		    coex_dm->bt_status)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			("[BTCoex], Wifi non connected-idle + BT Busy!!\n"));
+		halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+		halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
+					      false, false);
+		halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+					      false, 0x18);
+
+		commom = true;
+	} else {
+		if (wifi_busy)
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Wifi Connected-Busy"
+				  " + BT Busy!!\n");
+		else
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Wifi Connected-Idle"
+				  " + BT Busy!!\n");
+
+		commom = false;
+	}
+
+	return commom;
+}
+
+
+void halbtc8723b1ant_tdma_duration_adjust_for_acl(struct btc_coexist *btcoexist,
+						  u8 wifi_status)
+{
+	static s32 up, dn, m, n, wait_count;
+	/* 0: no change, +1: increase WiFi duration,
+	 * -1: decrease WiFi duration */
+	s32 result;
+	u8 retry_count = 0, bt_info_ext;
+	static bool pre_wifi_busy = false;
+	bool wifi_busy = false;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], TdmaDurationAdjustForAcl()\n");
+
+	if (BT_8723B_1ANT_WIFI_STATUS_CONNECTED_BUSY == wifi_status)
+		wifi_busy = true;
+	else
+		wifi_busy = false;
+
+	if ((BT_8723B_1ANT_WIFI_STATUS_NON_CONNECTED_ASSO_AUTH_SCAN ==
+							 wifi_status) ||
+	    (BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SCAN == wifi_status) ||
+	    (BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SPECIAL_PKT == wifi_status)) {
+		if (coex_dm->cur_ps_tdma != 1 && coex_dm->cur_ps_tdma != 2 &&
+		    coex_dm->cur_ps_tdma != 3 && coex_dm->cur_ps_tdma != 9) {
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 9);
+			coex_dm->ps_tdma_du_adj_type = 9;
+
+			up = 0;
+			dn = 0;
+			m = 1;
+			n = 3;
+			result = 0;
+			wait_count = 0;
+		}
+		return;
+	}
+
+	if (!coex_dm->auto_tdma_adjust) {
+		coex_dm->auto_tdma_adjust = true;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], first run TdmaDurationAdjust()!!\n");
+
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 2);
+		coex_dm->ps_tdma_du_adj_type = 2;
+
+		up = 0;
+		dn = 0;
+		m = 1;
+		n = 3;
+		result = 0;
+		wait_count = 0;
+	} else {
+		/*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 */
+			/*if busy / idle change */
+			if (wifi_busy != pre_wifi_busy) {
+				pre_wifi_busy = wifi_busy;
+				halbtc8723b1ant_ps_tdma(btcoexist, FORCE_EXEC,
+						        true,
+						        coex_dm->cur_ps_tdma);
+			}
+
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+				  "[BTCoex],********* TDMA(on, %d) ********\n",
+				  coex_dm->cur_ps_tdma);
+		}
+
+		if (coex_dm->cur_ps_tdma != 1 && coex_dm->cur_ps_tdma != 2 &&
+		    coex_dm->cur_ps_tdma != 9 && coex_dm->cur_ps_tdma != 11) {
+			/* recover to previous adjust type */
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true,
+						coex_dm->ps_tdma_du_adj_type);
+		}
+	}
+}
+
+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)
+{
+	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. */
+		}
+	}
+}
+
+void halbtc8723b1ant_power_save_state(struct btc_coexist *btcoexist,
+				      u8 ps_type, u8 lps_val,
+				      u8 rpwm_val)
+{
+	bool low_pwr_disable = false;
+
+	switch (ps_type) {
+	case BTC_PS_WIFI_NATIVE:
+		/* recover to original 32k low power setting */
+		low_pwr_disable = false;
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
+				   &low_pwr_disable);
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_NORMAL_LPS, NULL);
+		break;
+	case BTC_PS_LPS_ON:
+		halbtc8723b1ant_PsTdmaCheckForPowerSaveState(btcoexist, true);
+		halbtc8723b1ant_LpsRpwm(btcoexist, NORMAL_EXEC, lps_val,
+					rpwm_val);
+		/* when coex force to enter LPS, do not enter 32k low power. */
+		low_pwr_disable = true;
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
+				   &low_pwr_disable);
+		/* power save must executed before psTdma.	 */
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_ENTER_LPS, NULL);
+		break;
+	case BTC_PS_LPS_OFF:
+		halbtc8723b1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
+		break;
+	default:
+		break;
+	}
+}
+
+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 bool pre_bt_disabled = false;
+	static u32 bt_disable_cnt = 0;
+	bool bt_active = true, bt_disabled = false;
+
+	/* This function check if bt is disabled */
+
+	if (coex_sta->high_priority_tx == 0 &&
+	    coex_sta->high_priority_rx == 0 &&
+	    coex_sta->low_priority_tx == 0 &&
+	    coex_sta->low_priority_rx == 0)
+		bt_active = false;
+
+	if (coex_sta->high_priority_tx == 0xffff &&
+	    coex_sta->high_priority_rx == 0xffff &&
+	    coex_sta->low_priority_tx == 0xffff &&
+	    coex_sta->low_priority_rx == 0xffff)
+		bt_active = false;
+
+	if (bt_active) {
+		bt_disable_cnt = 0;
+		bt_disabled = false;
+ 		btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_DISABLE,
+				   &bt_disabled);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+			  "[BTCoex], BT is enabled !!\n");
+	} else {
+		bt_disable_cnt++;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+			  "[BTCoex], bt all counters=0, %d times!!\n",
+			  bt_disable_cnt);
+		if (bt_disable_cnt >= 2) {
+			bt_disabled = true;
+			btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_DISABLE,
+					   &bt_disabled);
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+				  "[BTCoex], BT is disabled !!\n");
+			halbtc8723b1ant_action_wifi_only(btcoexist);
+		}
+	}
+	if (pre_bt_disabled != bt_disabled) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+			  "[BTCoex], BT is from %s to %s!!\n",
+			  (pre_bt_disabled ? "disabled" : "enabled"),
+			  (bt_disabled ? "disabled" : "enabled"));
+		pre_bt_disabled = bt_disabled;
+		if (!bt_disabled) {
+		} else {
+			btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS,
+					   NULL);
+			btcoexist->btc_set(btcoexist, BTC_SET_ACT_NORMAL_LPS,
+					   NULL);
+		}
+	}
+}
+
+/***************************************************
+ *
+ *	Software Coex Mechanism start
+ *
+ ***************************************************/
+/* SCO only or SCO+PAN(HS) */
+void halbtc8723b1ant_action_sco(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state =
+		halbtc8723b1ant_wifi_rssi_state(btcoexist, 0, 2, 25, 0);
+
+	halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 4);
+
+	if (halbtc8723b1ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+	else
+		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		/* sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
+			  			      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+			  			      false, 0x18);
+		} else {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
+			  			      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+			  			      false, 0x18);
+		}
+	} else {
+		/* sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+
+void halbtc8723b1ant_action_hid(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = halbtc8723b1ant_wifi_rssi_state(btcoexist,
+							  0, 2, 25, 0);
+	bt_rssi_state = halbtc8723b1ant_bt_rssi_state(2, 50, 0);
+
+	halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (halbtc8723b1ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+	else
+		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist,
+		BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		/* sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		/* sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+/*A2DP only / PAN(EDR) only/ A2DP+PAN(HS) */
+void halbtc8723b1ant_action_a2dp(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = halbtc8723b1ant_wifi_rssi_state(btcoexist,
+							  0, 2, 25, 0);
+	bt_rssi_state = halbtc8723b1ant_bt_rssi_state(2, 50, 0);
+
+	halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (halbtc8723b1ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+	else
+		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		/* sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		/* sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+void halbtc8723b1ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state, bt_info_ext;
+	u32 wifi_bw;
+
+	bt_info_ext = coex_sta->bt_info_ext;
+	wifi_rssi_state = halbtc8723b1ant_wifi_rssi_state(btcoexist,
+							  0, 2, 25, 0);
+	bt_rssi_state = halbtc8723b1ant_bt_rssi_state(2, 50, 0);
+
+	halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (halbtc8723b1ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+	else
+		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		/* sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+ 			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		/* sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+void halbtc8723b1ant_action_pan_edr(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = halbtc8723b1ant_wifi_rssi_state(btcoexist,
+							  0, 2, 25, 0);
+	bt_rssi_state = halbtc8723b1ant_bt_rssi_state(2, 50, 0);
+
+	halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (halbtc8723b1ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+	else
+		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		/* sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		/* sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+
+/* PAN(HS) only */
+void halbtc8723b1ant_action_pan_hs(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = halbtc8723b1ant_wifi_rssi_state(btcoexist,
+							  0, 2, 25, 0);
+	bt_rssi_state = halbtc8723b1ant_bt_rssi_state(2, 50, 0);
+
+	halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		/* fw mechanism */
+		if((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		   (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+			halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC,
+						   false);
+		else
+			halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC,
+						   false);
+
+		/* sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		/* fw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+			halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC,
+						   false);
+		else
+			halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC,
+						   false);
+
+		/* sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+/*PAN(EDR)+A2DP */
+void halbtc8723b1ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state, bt_info_ext;
+	u32 wifi_bw;
+
+	bt_info_ext = coex_sta->bt_info_ext;
+	wifi_rssi_state = halbtc8723b1ant_wifi_rssi_state(btcoexist,
+							  0, 2, 25, 0);
+	bt_rssi_state = halbtc8723b1ant_bt_rssi_state(2, 50, 0);
+
+	halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (halbtc8723b1ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+	else
+		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist,
+		BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		/* sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		/* sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+void halbtc8723b1ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = halbtc8723b1ant_wifi_rssi_state(btcoexist,
+							  0, 2, 25, 0);
+	bt_rssi_state = halbtc8723b1ant_bt_rssi_state(2, 50, 0);
+
+	halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (halbtc8723b1ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+	else
+		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		/* sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		/* sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+/* HID+A2DP+PAN(EDR) */
+void halbtc8723b1ant_action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state, bt_info_ext;
+	u32 wifi_bw;
+
+	bt_info_ext = coex_sta->bt_info_ext;
+	wifi_rssi_state = halbtc8723b1ant_wifi_rssi_state(btcoexist,
+							  0, 2, 25, 0);
+	bt_rssi_state = halbtc8723b1ant_bt_rssi_state(2, 50, 0);
+
+	halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (halbtc8723b1ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+	else
+		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		/* sw mechanism */
+		if((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		   (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		/* sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+void halbtc8723b1ant_action_hid_a2dp(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state, bt_info_ext;
+	u32 wifi_bw;
+
+	bt_info_ext = coex_sta->bt_info_ext;
+	wifi_rssi_state = halbtc8723b1ant_wifi_rssi_state(btcoexist,
+							  0, 2, 25, 0);
+	bt_rssi_state = halbtc8723b1ant_bt_rssi_state(2, 50, 0);
+
+	if (halbtc8723b1ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+	else
+		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		/* sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		/* sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+/*****************************************************
+ *
+ *	Non-Software Coex Mechanism start
+ *
+ *****************************************************/
+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)
+{
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	bool wifi_connected = false, ap_enable = false;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_AP_MODE_ENABLE,
+			   &ap_enable);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+			   &wifi_connected);
+
+	if (!wifi_connected) {
+		halbtc8723b1ant_power_save_state(btcoexist,
+						 BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+	} else if (bt_link_info->sco_exist || bt_link_info->hid_only) {
+		/* SCO/HID-only busy */
+		halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+						 0x0, 0x0);
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 32);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+	} else {
+		if (ap_enable)
+			halbtc8723b1ant_power_save_state(btcoexist,
+							 BTC_PS_WIFI_NATIVE,
+							 0x0, 0x0);
+		else
+			halbtc8723b1ant_power_save_state(btcoexist,
+							 BTC_PS_LPS_ON,
+							 0x50, 0x4);
+
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 30);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+	}
+}
+
+void halbtc8723b1ant_action_bt_sco_hid_only_busy(struct btc_coexist * btcoexist,
+						 u8 wifi_status)
+{
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	bool wifi_connected = false;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+			   &wifi_connected);
+
+	/* tdma and coex table */
+
+	if (bt_link_info->sco_exist) {
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+	} else { /* HID */
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 6);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 5);
+	}
+}
+
+void halbtc8723b1ant_action_wifi_connected_bt_acl_busy(
+					struct btc_coexist *btcoexist,
+					u8 wifi_status)
+{
+	u8 bt_rssi_state;
+
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	bt_rssi_state = halbtc8723b1ant_bt_rssi_state(2, 28, 0);
+
+	if (bt_link_info->hid_only) {  /*HID */
+		halbtc8723b1ant_action_bt_sco_hid_only_busy(btcoexist,
+							    wifi_status);
+		coex_dm->auto_tdma_adjust = false;
+		return;
+	} else if (bt_link_info->a2dp_only) { /*A2DP */
+		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+ 			 halbtc8723b1ant_tdma_duration_adjust_for_acl(btcoexist,
+			 					   wifi_status);
+		} else { /*for low BT RSSI */
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 11);
+			coex_dm->auto_tdma_adjust = false;
+		}
+
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+	} else if (bt_link_info->hid_exist &&
+			bt_link_info->a2dp_exist) { /*HID+A2DP */
+		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 14);
+			coex_dm->auto_tdma_adjust = false;
+		} else { /*for low BT RSSI*/
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 14);
+			coex_dm->auto_tdma_adjust = false;
+		}
+
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 6);
+	 /*PAN(OPP,FTP), HID+PAN(OPP,FTP) */
+	} else if (bt_link_info->pan_only ||
+		   (bt_link_info->hid_exist && bt_link_info->pan_exist)) {
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 3);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 6);
+		coex_dm->auto_tdma_adjust = false;
+	 /*A2DP+PAN(OPP,FTP), HID+A2DP+PAN(OPP,FTP)*/
+	} else if ((bt_link_info->a2dp_exist && bt_link_info->pan_exist) ||
+		   (bt_link_info->hid_exist && bt_link_info->a2dp_exist &&
+		    bt_link_info->pan_exist)) {
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 13);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		coex_dm->auto_tdma_adjust = false;
+	} else {
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 11);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		coex_dm->auto_tdma_adjust = false;
+	}
+}
+
+void halbtc8723b1ant_action_wifi_not_connected(struct btc_coexist *btcoexist)
+{
+	/* power save state */
+	halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+					 0x0, 0x0);
+
+	/* tdma and coex table */
+	halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
+	halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
+}
+
+void halbtc8723b1ant_action_wifi_not_connected_asso_auth_scan(
+						struct btc_coexist *btcoexist)
+{
+	halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+					 0x0, 0x0);
+
+	halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 22);
+	halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+}
+
+void halbtc8723b1ant_ActionWifiConnectedScan(struct btc_coexist *btcoexist)
+{
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+
+	halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+					 0x0, 0x0);
+
+	/* tdma and coex table */
+	if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
+		if (bt_link_info->a2dp_exist &&
+		    bt_link_info->pan_exist) {
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 22);
+			halbtc8723b1ant_coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 1);
+	 	} else {
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 20);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		}
+	} else if ((BT_8723B_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
+		   (BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY ==
+		    coex_dm->bt_status)) {
+		halbtc8723b1ant_action_bt_sco_hid_only_busy(btcoexist,
+				BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SCAN);
+	} else {
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 20);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+	}
+}
+
+void halbtc8723b1ant_action_wifi_connected_special_packet(
+						struct btc_coexist *btcoexist)
+{
+	bool hs_connecting = false;
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_CONNECTING, &hs_connecting);
+
+	halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+					 0x0, 0x0);
+
+	/* tdma and coex table */
+	if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
+		if (bt_link_info->a2dp_exist && bt_link_info->pan_exist) {
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 22);
+			halbtc8723b1ant_coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 1);
+	 	} else {
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 20);
+			halbtc8723b1ant_coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 1);
+	 	}
+	} else {
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 20);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+	}
+}
+
+void halbtc8723b1ant_action_wifi_connected(struct btc_coexist *btcoexist)
+{
+	bool wifi_busy = false;
+	bool scan = false, link = false, roam = false;
+	bool under_4way = false, ap_enable = false;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+		  "[BTCoex], CoexForWifiConnect()===>\n");
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS,
+			   &under_4way);
+	if (under_4way) {
+		halbtc8723b1ant_action_wifi_connected_special_packet(btcoexist);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], CoexForWifiConnect(), "
+			  "return for wifi is under 4way<===\n");
+		return;
+	}
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
+
+	if (scan || link || roam) {
+		halbtc8723b1ant_ActionWifiConnectedScan(btcoexist);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], CoexForWifiConnect(), "
+			  "return for wifi is under scan<===\n");
+		return;
+	}
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_AP_MODE_ENABLE,
+			   &ap_enable);
+	/* power save state */
+	if (!ap_enable &&
+	    BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status &&
+	    !btcoexist->bt_link_info.hid_only)
+		halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_LPS_ON,
+						 0x50, 0x4);
+	else
+		halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+						 0x0, 0x0);
+
+	/* tdma and coex table */
+	btcoexist->btc_get(btcoexist,
+		BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+	if (!wifi_busy) {
+		if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
+			halbtc8723b1ant_action_wifi_connected_bt_acl_busy(btcoexist,
+				      BT_8723B_1ANT_WIFI_STATUS_CONNECTED_IDLE);
+		} else if ((BT_8723B_1ANT_BT_STATUS_SCO_BUSY ==
+						coex_dm->bt_status) ||
+			   (BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY ==
+			   			coex_dm->bt_status)) {
+			halbtc8723b1ant_action_bt_sco_hid_only_busy(btcoexist,
+				     BT_8723B_1ANT_WIFI_STATUS_CONNECTED_IDLE);
+		} else {
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						false, 8);
+			halbtc8723b1ant_coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 2);
+		}
+	} else {
+		if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
+			halbtc8723b1ant_action_wifi_connected_bt_acl_busy(btcoexist,
+				    BT_8723B_1ANT_WIFI_STATUS_CONNECTED_BUSY);
+		} else if ((BT_8723B_1ANT_BT_STATUS_SCO_BUSY ==
+						coex_dm->bt_status) ||
+			   (BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY ==
+			   			coex_dm->bt_status)) {
+			halbtc8723b1ant_action_bt_sco_hid_only_busy(btcoexist,
+				    BT_8723B_1ANT_WIFI_STATUS_CONNECTED_BUSY);
+		} else {
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
+			halbtc8723b1ant_coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 2);
+		}
+	}
+}
+
+void halbtc8723b1ant_run_sw_coexist_mechanism(struct btc_coexist *btcoexist)
+{
+	u8 algorithm = 0;
+
+	algorithm = halbtc8723b1ant_action_algorithm(btcoexist);
+	coex_dm->cur_algorithm = algorithm;
+
+	if (halbtc8723b1ant_is_common_action(btcoexist)) {
+	} else {
+		switch (coex_dm->cur_algorithm) {
+		case BT_8723B_1ANT_COEX_ALGO_SCO:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = SCO.\n");
+			halbtc8723b1ant_action_sco(btcoexist);
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_HID:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = HID.\n");
+			halbtc8723b1ant_action_hid(btcoexist);
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = A2DP.\n");
+			halbtc8723b1ant_action_a2dp(btcoexist);
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_A2DP_PANHS:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = "
+				  "A2DP+PAN(HS).\n");
+			halbtc8723b1ant_action_a2dp_pan_hs(btcoexist);
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_PANEDR:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = PAN(EDR).\n");
+			halbtc8723b1ant_action_pan_edr(btcoexist);
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_PANHS:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = HS mode.\n");
+			halbtc8723b1ant_action_pan_hs(btcoexist);
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_PANEDR_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = PAN+A2DP.\n");
+			halbtc8723b1ant_action_pan_edr_a2dp(btcoexist);
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_PANEDR_HID:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = "
+				  "PAN(EDR)+HID.\n");
+			halbtc8723b1ant_action_pan_edr_hid(btcoexist);
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_HID_A2DP_PANEDR:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = "
+				  "HID+A2DP+PAN.\n");
+			halbtc8723b1ant_action_hid_a2dp_pan_edr(btcoexist);
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_HID_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = HID+A2DP.\n");
+			halbtc8723b1ant_action_hid_a2dp(btcoexist);
+			break;
+		default:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = "
+				  "coexist All Off!!\n");
+			break;
+		}
+		coex_dm->pre_algorithm = coex_dm->cur_algorithm;
+	}
+}
+
+void halbtc8723b1ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
+{
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	bool wifi_connected = false, bt_hs_on = false;
+	bool limited_dig = false, bIncreaseScanDevNum = false;
+	bool b_bt_ctrl_agg_buf_size = false;
+	u8 agg_buf_size = 5;
+	u8 wifi_rssi_state = BTC_RSSI_STATE_HIGH;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+		  "[BTCoex], RunCoexistMechanism()===>\n");
+
+	if (btcoexist->manual_control) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], RunCoexistMechanism(), "
+			  "return for Manual CTRL <===\n");
+		return;
+	}
+
+	if (btcoexist->stop_coex_dm) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], RunCoexistMechanism(), "
+			  "return for Stop Coex DM <===\n");
+		return;
+	}
+
+	if (coex_sta->under_ips) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], wifi is under IPS !!!\n");
+		return;
+	}
+
+	if ((BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) ||
+	    (BT_8723B_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
+	    (BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status)) {
+		limited_dig = true;
+		bIncreaseScanDevNum = true;
+	}
+
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_LIMITED_DIG, &limited_dig);
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_INC_SCAN_DEV_NUM,
+			   &bIncreaseScanDevNum);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+			   &wifi_connected);
+
+	if (!bt_link_info->sco_exist && !bt_link_info->hid_exist) {
+		halbtc8723b1ant_limited_tx(btcoexist, NORMAL_EXEC, 0, 0, 0, 0);
+	} else {
+		if (wifi_connected) {
+			wifi_rssi_state =
+				halbtc8723b1ant_wifi_rssi_state(btcoexist,
+								1, 2, 30, 0);
+			if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+				halbtc8723b1ant_limited_tx(btcoexist,
+							   NORMAL_EXEC,
+							   1, 1, 1, 1);
+			} else {
+				halbtc8723b1ant_limited_tx(btcoexist,
+							   NORMAL_EXEC,
+							   1, 1, 1, 1);
+			}
+		} else {
+			halbtc8723b1ant_limited_tx(btcoexist, NORMAL_EXEC,
+						   0, 0, 0, 0);
+		}
+	}
+
+	if (bt_link_info->sco_exist) {
+		b_bt_ctrl_agg_buf_size = true;
+		agg_buf_size = 0x3;
+	} else if (bt_link_info->hid_exist) {
+		b_bt_ctrl_agg_buf_size = true;
+		agg_buf_size = 0x5;
+ 	} else if (bt_link_info->a2dp_exist || bt_link_info->pan_exist) {
+		b_bt_ctrl_agg_buf_size = true;
+		agg_buf_size = 0x8;
+	}
+	halbtc8723b1ant_limited_rx(btcoexist, NORMAL_EXEC, false,
+				   b_bt_ctrl_agg_buf_size, agg_buf_size);
+
+	halbtc8723b1ant_run_sw_coexist_mechanism(btcoexist);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+
+	if (coex_sta->c2h_bt_inquiry_page) {
+		halbtc8723b1ant_action_bt_inquiry(btcoexist);
+		return;
+	} else if (bt_hs_on) {
+		halbtc8723b1ant_action_hs(btcoexist);
+		return;
+	}
+
+
+	if (!wifi_connected) {
+		bool scan = false, link = false, roam = false;
+
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], wifi is non connected-idle !!!\n");
+
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
+
+		if (scan || link || roam)
+			halbtc8723b1ant_action_wifi_not_connected_asso_auth_scan(btcoexist);
+		else
+			halbtc8723b1ant_action_wifi_not_connected(btcoexist);
+	} else { /* wifi LPS/Busy */
+		halbtc8723b1ant_action_wifi_connected(btcoexist);
+	}
+}
+
+void halbtc8723b1ant_init_coex_dm(struct btc_coexist *btcoexist)
+{
+	/* force to reset coex mechanism */
+	halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, FORCE_EXEC, 6);
+	halbtc8723b1ant_dec_bt_pwr(btcoexist, FORCE_EXEC, false);
+
+	/* sw all off */
+	halbtc8723b1ant_sw_mechanism1(btcoexist, false, false, false, false);
+	halbtc8723b1ant_sw_mechanism2(btcoexist,false, false, false, 0x18);
+
+	halbtc8723b1ant_ps_tdma(btcoexist, FORCE_EXEC, false, 8);
+	halbtc8723b1ant_coex_table_with_type(btcoexist, FORCE_EXEC, 0);
+}
+
+void halbtc8723b1ant_init_hw_config(struct btc_coexist *btcoexist, bool backup)
+{
+	u32 u32tmp = 0;
+	u8 u8tmp = 0;
+	u32 cnt_bt_cal_chk = 0;
+
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+		  "[BTCoex], 1Ant Init HW Config!!\n");
+
+	if (backup) {/* backup rf 0x1e value */
+		coex_dm->bt_rf0x1e_backup =
+			btcoexist->btc_get_rf_reg(btcoexist,
+						  BTC_RF_A, 0x1e, 0xfffff);
+
+		coex_dm->backup_arfr_cnt1 =
+			btcoexist->btc_read_4byte(btcoexist, 0x430);
+		coex_dm->backup_arfr_cnt2 =
+			btcoexist->btc_read_4byte(btcoexist, 0x434);
+		coex_dm->backup_retry_limit =
+			btcoexist->btc_read_2byte(btcoexist, 0x42a);
+		coex_dm->backup_ampdu_max_time =
+			btcoexist->btc_read_1byte(btcoexist, 0x456);
+	}
+
+	/* WiFi goto standby while GNT_BT 0-->1 */
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x780);
+	/* BT goto standby while GNT_BT 1-->0 */
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x2, 0xfffff, 0x500);
+
+	btcoexist->btc_write_1byte(btcoexist, 0x974, 0xff);
+	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x944, 0x3, 0x3);
+	btcoexist->btc_write_1byte(btcoexist, 0x930, 0x77);
+
+
+	/* BT calibration check */
+	while (cnt_bt_cal_chk <= 20) {
+		u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x49d);
+		cnt_bt_cal_chk++;
+		if (u32tmp & BIT0) {
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+				  "[BTCoex], ########### BT "
+				  "calibration(cnt=%d) ###########\n",
+				  cnt_bt_cal_chk);
+			mdelay(50);
+		} else {
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+				  "[BTCoex], ********** BT NOT "
+				  "calibration (cnt=%d)**********\n",
+				  cnt_bt_cal_chk);
+			break;
+		}
+	}
+
+	/* 0x790[5:0]=0x5 */
+	u8tmp = btcoexist->btc_read_1byte(btcoexist, 0x790);
+	u8tmp &= 0xc0;
+	u8tmp |= 0x5;
+	btcoexist->btc_write_1byte(btcoexist, 0x790, u8tmp);
+
+	/* Enable counter statistics */
+	/*0x76e[3] =1, WLAN_Act control by PTA */
+	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
+	btcoexist->btc_write_1byte(btcoexist, 0x778, 0x1);
+	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x40, 0x20, 0x1);
+
+	/*Antenna config */
+	halbtc8723b1ant_SetAntPath(btcoexist, BTC_ANT_PATH_PTA, true, false);
+	/* PTA parameter */
+	halbtc8723b1ant_coex_table_with_type(btcoexist, FORCE_EXEC, 0);
+
+}
+
+void halbtc8723b1ant_wifi_off_hw_cfg(struct btc_coexist *btcoexist)
+{
+	/* set wlan_act to low */
+	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0);
+}
+
+/**************************************************************
+ * work around function start with wa_halbtc8723b1ant_
+ **************************************************************/
+/**************************************************************
+ * extern function start with EXhalbtc8723b1ant_
+ **************************************************************/
+
+void ex_halbtc8723b1ant_init_hwconfig(struct btc_coexist *btcoexist)
+{
+	halbtc8723b1ant_init_hw_config(btcoexist, true);
+}
+
+void ex_halbtc8723b1ant_init_coex_dm(struct btc_coexist *btcoexist)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+		  "[BTCoex], Coex Mechanism Init!!\n");
+
+	btcoexist->stop_coex_dm = false;
+
+	halbtc8723b1ant_init_coex_dm(btcoexist);
+
+	halbtc8723b1ant_query_bt_info(btcoexist);
+}
+
+void ex_halbtc8723b1ant_display_coex_info(struct btc_coexist *btcoexist)
+{
+	struct btc_board_info *board_info = &btcoexist->board_info;
+	struct btc_stack_info *stack_info = &btcoexist->stack_info;
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	u8 *cli_buf = btcoexist->cli_buf;
+	u8 u8tmp[4], i, bt_info_ext, psTdmaCase=0;
+	u16 u16tmp[4];
+	u32 u32tmp[4];
+	bool roam = false, scan = false;
+	bool link = false, wifi_under_5g = false;
+	bool bt_hs_on = false, wifi_busy = false;
+	s32 wifi_rssi =0, bt_hs_rssi = 0;
+	u32 wifi_bw, wifi_traffic_dir, fa_ofdm, fa_cck;
+	u8 wifi_dot11_chnl, wifi_hs_chnl;
+	u32 fw_ver = 0, bt_patch_ver = 0;
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n ============[BT Coexist info]============");
+	CL_PRINTF(cli_buf);
+
+	if (btcoexist->manual_control) {
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\r\n ============[Under Manual Control]==========");
+		CL_PRINTF(cli_buf);
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\r\n ==========================================");
+		CL_PRINTF(cli_buf);
+	}
+	if (btcoexist->stop_coex_dm) {
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\r\n ============[Coex is STOPPED]============");
+		CL_PRINTF(cli_buf);
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\r\n ==========================================");
+		CL_PRINTF(cli_buf);
+	}
+
+	if (!board_info->bt_exist) {
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n BT not exists !!!");
+		CL_PRINTF(cli_buf);
+		return;
+	}
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d",
+		   "Ant PG Num/ Ant Mech/ Ant Pos:", \
+		   board_info->pg_ant_num, board_info->btdm_ant_num,
+		   board_info->btdm_ant_pos);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %d",
+		   "BT stack/ hci ext ver", \
+		   ((stack_info->profile_notified)? "Yes":"No"),
+		   stack_info->hci_version);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER, &bt_patch_ver);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n %-35s = %d_%x/ 0x%x/ 0x%x(%d)",
+		   "CoexVer/ FwVer/ PatchVer", \
+		   glcoex_ver_date_8723b_1ant, glcoex_ver_8723b_1ant,
+		   fw_ver, bt_patch_ver, bt_patch_ver);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_DOT11_CHNL,
+			   &wifi_dot11_chnl);
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifi_hs_chnl);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d(%d)",
+		   "Dot11 channel / HsChnl(HsMode)", \
+		   wifi_dot11_chnl, wifi_hs_chnl, bt_hs_on);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x ",
+		   "H2C Wifi inform bt chnl Info", \
+		   coex_dm->wifi_chnl_info[0], coex_dm->wifi_chnl_info[1],
+		   coex_dm->wifi_chnl_info[2]);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
+	btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
+		   "Wifi rssi/ HS rssi", wifi_rssi, bt_hs_rssi);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ",
+		"Wifi link/ roam/ scan", link, roam, scan);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist,BTC_GET_BL_WIFI_UNDER_5G,
+			   &wifi_under_5g);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION,
+			   &wifi_traffic_dir);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %s/ %s ",
+		   "Wifi status", (wifi_under_5g? "5G":"2.4G"),
+		   ((BTC_WIFI_BW_LEGACY==wifi_bw)? "Legacy":
+			(((BTC_WIFI_BW_HT40==wifi_bw)? "HT40":"HT20"))),
+		   ((!wifi_busy)? "idle":
+			((BTC_WIFI_TRAFFIC_TX==wifi_traffic_dir)?
+				"uplink":"downlink")));
+	CL_PRINTF(cli_buf);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = [%s/ %d/ %d] ",
+		"BT [status/ rssi/ retryCnt]",
+		((btcoexist->bt_info.bt_disabled)? ("disabled"):
+		  ((coex_sta->c2h_bt_inquiry_page)?("inquiry/page scan"):
+		    ((BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE == coex_dm->bt_status)?
+		      "non-connected idle":
+			((BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status)?
+			  "connected-idle":"busy")))),
+			    coex_sta->bt_rssi, coex_sta->bt_retry_cnt);
+	CL_PRINTF(cli_buf);
+
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d / %d / %d",
+		"SCO/HID/PAN/A2DP", bt_link_info->sco_exist,
+		bt_link_info->hid_exist, bt_link_info->pan_exist,
+		bt_link_info->a2dp_exist);
+	CL_PRINTF(cli_buf);
+	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_BT_LINK_INFO);
+
+	bt_info_ext = coex_sta->bt_info_ext;
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s",
+		   "BT Info A2DP rate",
+		   (bt_info_ext & BIT0) ? "Basic rate" : "EDR rate");
+	CL_PRINTF(cli_buf);
+
+	for (i = 0; i < BT_INFO_SRC_8723B_1ANT_MAX; i++) {
+		if (coex_sta->bt_info_c2h_cnt[i]) {
+			CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+				   "\r\n %-35s = %02x %02x %02x "
+				   "%02x %02x %02x %02x(%d)",
+				   GLBtInfoSrc8723b1Ant[i],
+				   coex_sta->bt_info_c2h[i][0],
+				   coex_sta->bt_info_c2h[i][1],
+				   coex_sta->bt_info_c2h[i][2],
+				   coex_sta->bt_info_c2h[i][3],
+				   coex_sta->bt_info_c2h[i][4],
+				   coex_sta->bt_info_c2h[i][5],
+				   coex_sta->bt_info_c2h[i][6],
+				   coex_sta->bt_info_c2h_cnt[i]);
+			CL_PRINTF(cli_buf);
+		}
+	}
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n %-35s = %s/%s, (0x%x/0x%x)",
+		   "PS state, IPS/LPS, (lps/rpwm)", \
+		   ((coex_sta->under_ips? "IPS ON":"IPS OFF")),
+		   ((coex_sta->under_lps? "LPS ON":"LPS OFF")),
+		   btcoexist->bt_info.lps_1ant,
+		   btcoexist->bt_info.rpwm_1ant);
+	CL_PRINTF(cli_buf);
+	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_FW_PWR_MODE_CMD);
+
+	if (!btcoexist->manual_control) {
+		/* Sw mechanism	*/
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
+			   "============[Sw mechanism]============");
+		CL_PRINTF(cli_buf);
+
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ",
+ 			   "SM1[ShRf/ LpRA/ LimDig]", \
+ 			   coex_dm->cur_rf_rx_lpf_shrink,
+			   coex_dm->cur_low_penalty_ra,
+			   btcoexist->bt_info.limited_dig);
+		CL_PRINTF(cli_buf);
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\r\n %-35s = %d/ %d/ %d(0x%x) ",
+			   "SM2[AgcT/ AdcB/ SwDacSwing(lvl)]", \
+			   coex_dm->cur_agc_table_en,
+			   coex_dm->cur_adc_backoff,
+			   coex_dm->cur_dac_swing_on,
+			   coex_dm->cur_dac_swing_lvl);
+		CL_PRINTF(cli_buf);
+
+
+		CL_PRINTF(cli_buf);
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x ",
+			   "Rate Mask", btcoexist->bt_info.ra_mask);
+		CL_PRINTF(cli_buf);
+
+		/* Fw mechanism	*/
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
+			   "============[Fw mechanism]============");
+		CL_PRINTF(cli_buf);
+
+		psTdmaCase = coex_dm->cur_ps_tdma;
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\r\n %-35s = %02x %02x %02x %02x %02x "
+			   "case-%d (auto:%d)",
+			   "PS TDMA", coex_dm->ps_tdma_para[0],
+			   coex_dm->ps_tdma_para[1], coex_dm->ps_tdma_para[2],
+			   coex_dm->ps_tdma_para[3], coex_dm->ps_tdma_para[4],
+			   psTdmaCase, coex_dm->auto_tdma_adjust);
+		CL_PRINTF(cli_buf);
+
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x ",
+			   "Latest error condition(should be 0)", \
+			   coex_dm->error_condition);
+		CL_PRINTF(cli_buf);
+
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ",
+			   "DecBtPwr/ IgnWlanAct", coex_dm->cur_dec_bt_pwr,
+			   coex_dm->cur_ignore_wlan_act);
+		CL_PRINTF(cli_buf);
+	}
+
+	/* Hw setting */
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
+		   "============[Hw setting]============");
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x",
+		   "RF-A, 0x1e initVal", coex_dm->bt_rf0x1e_backup);
+	CL_PRINTF(cli_buf);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/0x%x/0x%x/0x%x",
+		   "backup ARFR1/ARFR2/RL/AMaxTime", coex_dm->backup_arfr_cnt1,
+		   coex_dm->backup_arfr_cnt2, coex_dm->backup_retry_limit,
+		   coex_dm->backup_ampdu_max_time);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x430);
+	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x434);
+	u16tmp[0] = btcoexist->btc_read_2byte(btcoexist, 0x42a);
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x456);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/0x%x/0x%x/0x%x",
+		   "0x430/0x434/0x42a/0x456",
+		   u32tmp[0], u32tmp[1], u16tmp[0], u8tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x778);
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x6cc);
+	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x880);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
+		   "0x778/0x6cc/0x880[29:25]", u8tmp[0], u32tmp[0],
+		   (u32tmp[1] & 0x3e000000) >> 25);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x948);
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x67);
+	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x765);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
+		   "0x948/ 0x67[5] / 0x765",
+		   u32tmp[0], ((u8tmp[0] & 0x20)>> 5), u8tmp[1]);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x92c);
+	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x930);
+	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x944);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
+		   "0x92c[1:0]/ 0x930[7:0]/0x944[1:0]",
+		   u32tmp[0] & 0x3, u32tmp[1] & 0xff, u32tmp[2] & 0x3);
+	CL_PRINTF(cli_buf);
+
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x39);
+	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x40);
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x4c);
+	u8tmp[2] = btcoexist->btc_read_1byte(btcoexist, 0x64);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x",
+		   "0x38[11]/0x40/0x4c[24:23]/0x64[0]",
+		   ((u8tmp[0] & 0x8)>>3), u8tmp[1],
+		   ((u32tmp[0] & 0x01800000) >> 23), u8tmp[2] & 0x1);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x550);
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x522);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
+		   "0x550(bcn ctrl)/0x522", u32tmp[0], u8tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xc50);
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x49c);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
+		   "0xc50(dig)/0x49c(null-drop)", u32tmp[0] & 0xff, u8tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xda0);
+	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0xda4);
+	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0xda8);
+	u32tmp[3] = btcoexist->btc_read_4byte(btcoexist, 0xcf0);
+
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0xa5b);
+	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0xa5c);
+
+	fa_ofdm = ((u32tmp[0] & 0xffff0000) >> 16) +
+		  ((u32tmp[1] & 0xffff0000) >> 16) +
+		   (u32tmp[1] & 0xffff) +
+		   (u32tmp[2] & 0xffff) + \
+		  ((u32tmp[3] & 0xffff0000) >> 16) +
+		   (u32tmp[3] & 0xffff) ;
+	fa_cck = (u8tmp[0] << 8) + u8tmp[1];
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
+		   "OFDM-CCA/OFDM-FA/CCK-FA",
+		   u32tmp[0] & 0xffff, fa_ofdm, fa_cck);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x6c0);
+	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x6c4);
+	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x6c8);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
+		   "0x6c0/0x6c4/0x6c8(coexTable)",
+		   u32tmp[0], u32tmp[1], u32tmp[2]);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
+		   "0x770(high-pri rx/tx)", coex_sta->high_priority_rx,
+		   coex_sta->high_priority_tx);
+	CL_PRINTF(cli_buf);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
+		   "0x774(low-pri rx/tx)", coex_sta->low_priority_rx,
+		   coex_sta->low_priority_tx);
+	CL_PRINTF(cli_buf);
+#if(BT_AUTO_REPORT_ONLY_8723B_1ANT == 1)
+	halbtc8723b1ant_monitor_bt_ctr(btcoexist);
+#endif
+	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_COEX_STATISTICS);
+}
+
+
+void ex_halbtc8723b1ant_ips_notify(struct btc_coexist *btcoexist, u8 type)
+{
+
+	if (btcoexist->manual_control || btcoexist->stop_coex_dm)
+		return;
+
+	if (BTC_IPS_ENTER == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], IPS ENTER notify\n");
+		coex_sta->under_ips = true;
+
+		halbtc8723b1ant_SetAntPath(btcoexist, BTC_ANT_PATH_BT,
+					   false, true);
+                /* set PTA control */
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 0);
+		halbtc8723b1ant_coex_table_with_type(btcoexist,
+			NORMAL_EXEC, 0);
+	} else if (BTC_IPS_LEAVE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], IPS LEAVE notify\n");
+		coex_sta->under_ips = false;
+
+		halbtc8723b1ant_run_coexist_mechanism(btcoexist);
+	}
+}
+
+void ex_halbtc8723b1ant_lps_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	if (btcoexist->manual_control || btcoexist->stop_coex_dm)
+		return;
+
+	if (BTC_LPS_ENABLE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], LPS ENABLE notify\n");
+		coex_sta->under_lps = true;
+	} else if (BTC_LPS_DISABLE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], LPS DISABLE notify\n");
+		coex_sta->under_lps = false;
+	}
+}
+
+void ex_halbtc8723b1ant_scan_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	bool wifi_connected = false, bt_hs_on = false;
+
+	if (btcoexist->manual_control || btcoexist->stop_coex_dm ||
+	    btcoexist->bt_info.bt_disabled)
+		return;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+			   &wifi_connected);
+
+	halbtc8723b1ant_query_bt_info(btcoexist);
+
+	if (coex_sta->c2h_bt_inquiry_page) {
+		halbtc8723b1ant_action_bt_inquiry(btcoexist);
+		return;
+	} else if (bt_hs_on) {
+		halbtc8723b1ant_action_hs(btcoexist);
+		return;
+	}
+
+	if (BTC_SCAN_START == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], SCAN START notify\n");
+		if (!wifi_connected)	/* non-connected scan */
+			halbtc8723b1ant_action_wifi_not_connected_asso_auth_scan(btcoexist);
+		else	/* wifi is connected */
+			halbtc8723b1ant_ActionWifiConnectedScan(btcoexist);
+	} else if (BTC_SCAN_FINISH == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], SCAN FINISH notify\n");
+		if (!wifi_connected)	/* non-connected scan */
+			halbtc8723b1ant_action_wifi_not_connected(btcoexist);
+		else
+			halbtc8723b1ant_action_wifi_connected(btcoexist);
+	}
+}
+
+void ex_halbtc8723b1ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	bool wifi_connected = false, bt_hs_on = false;
+
+	if (btcoexist->manual_control || btcoexist->stop_coex_dm ||
+	    btcoexist->bt_info.bt_disabled)
+		return;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	if (coex_sta->c2h_bt_inquiry_page) {
+		halbtc8723b1ant_action_bt_inquiry(btcoexist);
+		return;
+	} else if (bt_hs_on) {
+		halbtc8723b1ant_action_hs(btcoexist);
+		return;
+	}
+
+	if (BTC_ASSOCIATE_START == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], CONNECT START notify\n");
+		halbtc8723b1ant_action_wifi_not_connected_asso_auth_scan(btcoexist);
+	} else if (BTC_ASSOCIATE_FINISH == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], CONNECT FINISH notify\n");
+
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+				   &wifi_connected);
+		if (!wifi_connected) /* non-connected scan */
+			halbtc8723b1ant_action_wifi_not_connected(btcoexist);
+		else
+			halbtc8723b1ant_action_wifi_connected(btcoexist);
+	}
+}
+
+void ex_halbtc8723b1ant_media_status_notify(struct btc_coexist *btcoexist,
+					    u8 type)
+{
+	u8 h2c_parameter[3] ={0};
+	u32 wifi_bw;
+	u8 wifiCentralChnl;
+
+	if (btcoexist->manual_control || btcoexist->stop_coex_dm ||
+	    btcoexist->bt_info.bt_disabled )
+		return;
+
+	if (BTC_MEDIA_CONNECT == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], MEDIA connect notify\n");
+	else
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], MEDIA disconnect notify\n");
+
+	/* only 2.4G we need to inform bt the chnl mask */
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_CENTRAL_CHNL,
+			   &wifiCentralChnl);
+
+	if ((BTC_MEDIA_CONNECT == type) &&
+	    (wifiCentralChnl <= 14)) {
+		h2c_parameter[0] = 0x0;
+		h2c_parameter[1] = wifiCentralChnl;
+		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+		if (BTC_WIFI_BW_HT40 == wifi_bw)
+			h2c_parameter[2] = 0x30;
+		else
+			h2c_parameter[2] = 0x20;
+	}
+
+	coex_dm->wifi_chnl_info[0] = h2c_parameter[0];
+	coex_dm->wifi_chnl_info[1] = h2c_parameter[1];
+	coex_dm->wifi_chnl_info[2] = h2c_parameter[2];
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], FW write 0x66=0x%x\n",
+		  h2c_parameter[0] << 16 | h2c_parameter[1] << 8 |
+		  h2c_parameter[2]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x66, 3, h2c_parameter);
+}
+
+void ex_halbtc8723b1ant_special_packet_notify(struct btc_coexist *btcoexist,
+					      u8 type)
+{
+	bool bt_hs_on = false;
+
+	if (btcoexist->manual_control || btcoexist->stop_coex_dm ||
+	    btcoexist->bt_info.bt_disabled)
+		return;
+
+	coex_sta->special_pkt_period_cnt = 0;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	if (coex_sta->c2h_bt_inquiry_page) {
+		halbtc8723b1ant_action_bt_inquiry(btcoexist);
+		return;
+	} else if (bt_hs_on) {
+		halbtc8723b1ant_action_hs(btcoexist);
+		return;
+	}
+
+	if (BTC_PACKET_DHCP == type ||
+		BTC_PACKET_EAPOL == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], special Packet(%d) notify\n", type);
+		halbtc8723b1ant_action_wifi_connected_special_packet(btcoexist);
+	}
+}
+
+void ex_halbtc8723b1ant_bt_info_notify(struct btc_coexist *btcoexist,
+				       u8 *tmp_buf, u8 length)
+{
+	u8 bt_info = 0;
+	u8 i, rsp_source = 0;
+	bool wifi_connected = false;
+	bool bt_busy = false;
+
+	coex_sta->c2h_bt_info_req_sent = false;
+
+	rsp_source = tmp_buf[0] & 0xf;
+	if (rsp_source >= BT_INFO_SRC_8723B_1ANT_MAX)
+		rsp_source = BT_INFO_SRC_8723B_1ANT_WIFI_FW;
+	coex_sta->bt_info_c2h_cnt[rsp_source]++;
+
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+		  "[BTCoex], Bt info[%d], length=%d, hex data=[",
+		  rsp_source, length);
+	for (i=0; i<length; i++) {
+		coex_sta->bt_info_c2h[rsp_source][i] = tmp_buf[i];
+		if (i == 1)
+			bt_info = tmp_buf[i];
+		if (i == length - 1)
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+				  "0x%02x]\n", tmp_buf[i]);
+		else
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+				  "0x%02x, ", tmp_buf[i]);
+	}
+
+	if (BT_INFO_SRC_8723B_1ANT_WIFI_FW != rsp_source) {
+		coex_sta->bt_retry_cnt =	/* [3:0] */
+			coex_sta->bt_info_c2h[rsp_source][2] & 0xf;
+
+		coex_sta->bt_rssi =
+			coex_sta->bt_info_c2h[rsp_source][3] * 2 + 10;
+
+		coex_sta->bt_info_ext =
+			coex_sta->bt_info_c2h[rsp_source][4];
+
+		/* Here we need to resend some wifi info to BT
+		 * because bt is reset and loss of the info.*/
+		if(coex_sta->bt_info_ext & BIT1)
+		{
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], BT ext info bit1 check, "
+				  "send wifi BW&Chnl to BT!!\n");
+			btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+					   &wifi_connected);
+			if(wifi_connected)
+				ex_halbtc8723b1ant_media_status_notify(btcoexist,
+							     BTC_MEDIA_CONNECT);
+			else
+				ex_halbtc8723b1ant_media_status_notify(btcoexist,
+							  BTC_MEDIA_DISCONNECT);
+		}
+
+		if (coex_sta->bt_info_ext & BIT3) {
+			if (!btcoexist->manual_control &&
+			    !btcoexist->stop_coex_dm) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], BT ext info bit3 check, "
+					  "set BT NOT ignore Wlan active!!\n");
+				halbtc8723b1ant_ignore_wlan_act(btcoexist,
+								FORCE_EXEC,
+								false);
+			}
+		} else {
+			/* BT already NOT ignore Wlan active, do nothing here.*/
+		}
+#if(BT_AUTO_REPORT_ONLY_8723B_1ANT == 0)
+		if (coex_sta->bt_info_ext & BIT4) {
+			/* BT auto report already enabled, do nothing */
+		} else {
+			halbtc8723b1ant_bt_auto_report(btcoexist, FORCE_EXEC,
+						       true);
+		}
+#endif
+	}
+
+	/* check BIT2 first ==> check if bt is under inquiry or page scan */
+	if (bt_info & BT_INFO_8723B_1ANT_B_INQ_PAGE)
+		coex_sta->c2h_bt_inquiry_page = true;
+	else
+		coex_sta->c2h_bt_inquiry_page = false;
+
+	/* set link exist status */
+	if (!(bt_info & BT_INFO_8723B_1ANT_B_CONNECTION)) {
+		coex_sta->bt_link_exist = false;
+		coex_sta->pan_exist = false;
+		coex_sta->a2dp_exist = false;
+		coex_sta->hid_exist = false;
+		coex_sta->sco_exist = false;
+	} else { /* connection exists */
+		coex_sta->bt_link_exist = true;
+		if (bt_info & BT_INFO_8723B_1ANT_B_FTP)
+			coex_sta->pan_exist = true;
+		else
+			coex_sta->pan_exist = false;
+		if (bt_info & BT_INFO_8723B_1ANT_B_A2DP)
+			coex_sta->a2dp_exist = true;
+		else
+			coex_sta->a2dp_exist = false;
+		if (bt_info & BT_INFO_8723B_1ANT_B_HID)
+			coex_sta->hid_exist = true;
+		else
+			coex_sta->hid_exist = false;
+		if (bt_info & BT_INFO_8723B_1ANT_B_SCO_ESCO)
+			coex_sta->sco_exist = true;
+		else
+			coex_sta->sco_exist = false;
+	}
+
+	halbtc8723b1ant_update_bt_link_info(btcoexist);
+
+	if (!(bt_info&BT_INFO_8723B_1ANT_B_CONNECTION)) {
+		coex_dm->bt_status = BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), "
+			  "BT Non-Connected idle!!!\n");
+	/* connection exists but no busy */
+	} else if (bt_info == BT_INFO_8723B_1ANT_B_CONNECTION) {
+		coex_dm->bt_status = BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), BT Connected-idle!!!\n");
+	} else if ((bt_info & BT_INFO_8723B_1ANT_B_SCO_ESCO) ||
+		(bt_info & BT_INFO_8723B_1ANT_B_SCO_BUSY)) {
+		coex_dm->bt_status = BT_8723B_1ANT_BT_STATUS_SCO_BUSY;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), "
+			  "BT SCO busy!!!\n");
+	} else if (bt_info & BT_INFO_8723B_1ANT_B_ACL_BUSY) {
+		if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY != coex_dm->bt_status)
+			coex_dm->auto_tdma_adjust = false;
+
+		coex_dm->bt_status = BT_8723B_1ANT_BT_STATUS_ACL_BUSY;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), BT ACL busy!!!\n");
+	} else {
+		coex_dm->bt_status =
+			BT_8723B_1ANT_BT_STATUS_MAX;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), BT Non-Defined state!!\n");
+	}
+
+	if ((BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) ||
+	    (BT_8723B_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
+	    (BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status))
+		bt_busy = true;
+	else
+		bt_busy = false;
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bt_busy);
+
+	halbtc8723b1ant_run_coexist_mechanism(btcoexist);
+}
+
+void ex_halbtc8723b1ant_halt_notify(struct btc_coexist *btcoexist)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, "[BTCoex], Halt notify\n");
+
+	btcoexist->stop_coex_dm = true;
+
+	halbtc8723b1ant_SetAntPath(btcoexist, BTC_ANT_PATH_BT, false, true);
+
+	halbtc8723b1ant_wifi_off_hw_cfg(btcoexist);
+	halbtc8723b1ant_ignore_wlan_act(btcoexist, FORCE_EXEC, true);
+
+	halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+					 0x0, 0x0);
+	halbtc8723b1ant_ps_tdma(btcoexist, FORCE_EXEC, false, 0);
+
+	ex_halbtc8723b1ant_media_status_notify(btcoexist, BTC_MEDIA_DISCONNECT);
+}
+
+void ex_halbtc8723b1ant_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, "[BTCoex], Pnp notify\n");
+
+	if (BTC_WIFI_PNP_SLEEP == pnp_state) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], Pnp notify to SLEEP\n");
+		btcoexist->stop_coex_dm = true;
+		halbtc8723b1ant_ignore_wlan_act(btcoexist, FORCE_EXEC, true);
+		halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+						 0x0, 0x0);
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 9);
+	} else if (BTC_WIFI_PNP_WAKE_UP == pnp_state) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], Pnp notify to WAKE UP\n");
+		btcoexist->stop_coex_dm = false;
+		halbtc8723b1ant_init_hw_config(btcoexist, false);
+		halbtc8723b1ant_init_coex_dm(btcoexist);
+		halbtc8723b1ant_query_bt_info(btcoexist);
+	}
+}
+
+void ex_halbtc8723b1ant_periodical(struct btc_coexist *btcoexist)
+{
+	struct btc_board_info *board_info = &btcoexist->board_info;
+	struct btc_stack_info *stack_info = &btcoexist->stack_info;
+	static u8 dis_ver_info_cnt = 0;
+	u32 fw_ver = 0, bt_patch_ver = 0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+		  "[BTCoex], =========================="
+		  "Periodical===========================\n");
+
+	if (dis_ver_info_cnt <= 5) {
+		dis_ver_info_cnt += 1;
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], *************************"
+			  "***************************************\n");
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], Ant PG Num/ Ant Mech/ "
+			  "Ant Pos = %d/ %d/ %d\n", \
+			  board_info->pg_ant_num, board_info->btdm_ant_num,
+			  board_info->btdm_ant_pos);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], BT stack/ hci ext ver = %s / %d\n", \
+			  ((stack_info->profile_notified)? "Yes":"No"),
+			  stack_info->hci_version);
+		btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER,
+				   &bt_patch_ver);
+		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], CoexVer/ FwVer/ PatchVer "
+			  "= %d_%x/ 0x%x/ 0x%x(%d)\n", \
+			  glcoex_ver_date_8723b_1ant,
+			  glcoex_ver_8723b_1ant, fw_ver,
+			  bt_patch_ver, bt_patch_ver);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], *****************************"
+			  "***********************************\n");
+	}
+
+#if(BT_AUTO_REPORT_ONLY_8723B_1ANT == 0)
+	halbtc8723b1ant_query_bt_info(btcoexist);
+	halbtc8723b1ant_monitor_bt_ctr(btcoexist);
+	halbtc8723b1ant_monitor_bt_enable_disable(btcoexist);
+#else
+	if (halbtc8723b1ant_is_wifi_status_changed(btcoexist) ||
+	    coex_dm->auto_tdma_adjust) {
+		if (coex_sta->special_pkt_period_cnt > 2)
+			halbtc8723b1ant_run_coexist_mechanism(btcoexist);
+	}
+
+	coex_sta->special_pkt_period_cnt++;
+#endif
+}
+
+
+#endif
+
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtc8723b1ant.h b/drivers/staging/rtl8821ae/btcoexist/halbtc8723b1ant.h
new file mode 100644
index 000000000000..5ce292f2e7c6
--- /dev/null
+++ b/drivers/staging/rtl8821ae/btcoexist/halbtc8723b1ant.h
@@ -0,0 +1,175 @@
+/**********************************************************************
+ * 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			BIT7
+#define	BT_INFO_8723B_1ANT_B_A2DP			BIT6
+#define	BT_INFO_8723B_1ANT_B_HID			BIT5
+#define	BT_INFO_8723B_1ANT_B_SCO_BUSY			BIT4
+#define	BT_INFO_8723B_1ANT_B_ACL_BUSY			BIT3
+#define	BT_INFO_8723B_1ANT_B_INQ_PAGE			BIT2
+#define	BT_INFO_8723B_1ANT_B_SCO_ESCO			BIT1
+#define	BT_INFO_8723B_1ANT_B_CONNECTION			BIT0
+
+#define	BT_INFO_8723B_1ANT_A2DP_BASIC_RATE(_BT_INFO_EXT_)	\
+		(((_BT_INFO_EXT_&BIT0))? true:false)
+
+#define	BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT		2
+
+typedef enum _BT_INFO_SRC_8723B_1ANT{
+	BT_INFO_SRC_8723B_1ANT_WIFI_FW			= 0x0,
+	BT_INFO_SRC_8723B_1ANT_BT_RSP			= 0x1,
+	BT_INFO_SRC_8723B_1ANT_BT_ACTIVE_SEND		= 0x2,
+	BT_INFO_SRC_8723B_1ANT_MAX
+}BT_INFO_SRC_8723B_1ANT,*PBT_INFO_SRC_8723B_1ANT;
+
+typedef enum _BT_8723B_1ANT_BT_STATUS{
+	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
+}BT_8723B_1ANT_BT_STATUS,*PBT_8723B_1ANT_BT_STATUS;
+
+typedef 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
+}BT_8723B_1ANT_WIFI_STATUS,*PBT_8723B_1ANT_WIFI_STATUS;
+
+typedef 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,
+}BT_8723B_1ANT_COEX_ALGO,*PBT_8723B_1ANT_COEX_ALGO;
+
+struct coex_dm_8723b_1ant{
+	/* fw mechanism */
+	bool pre_dec_bt_pwr;
+	bool cur_dec_bt_pwr;
+	u8 pre_fw_dac_swing_lvl;
+	u8 cur_fw_dac_swing_lvl;
+	bool cur_ignore_wlan_act;
+	bool pre_ignore_wlan_act;
+	u8 pre_ps_tdma;
+	u8 cur_ps_tdma;
+	u8 ps_tdma_para[5];
+	u8 ps_tdma_du_adj_type;
+	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_rf_rx_lpf_shrink;
+	bool cur_rf_rx_lpf_shrink;
+	u32 bt_rf0x1e_backup;
+	bool pre_low_penalty_ra;
+	bool cur_low_penalty_ra;
+	bool pre_dac_swing_on;
+	u32 pre_dac_swing_lvl;
+	bool cur_dac_swing_on;
+	u32 cur_dac_swing_lvl;
+	bool pre_adc_backoff;
+	bool cur_adc_backoff;
+	bool pre_agc_table_en;
+	bool cur_agc_table_en;
+	u32 pre_val0x6c0;
+	u32 cur_val0x6c0;
+	u32 pre_val0x6c4;
+	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 pnpState);
+void ex_halbtc8723b1ant_periodical(struct btc_coexist *btcoexist);
+void ex_halbtc8723b1ant_display_coex_info(struct btc_coexist *btcoexist);
+
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtc8723b2ant.c b/drivers/staging/rtl8821ae/btcoexist/halbtc8723b2ant.c
new file mode 100644
index 000000000000..83b1b4218333
--- /dev/null
+++ b/drivers/staging/rtl8821ae/btcoexist/halbtc8723b2ant.c
@@ -0,0 +1,4185 @@
+/***************************************************************
+ * Description:
+ *
+ * This file is for RTL8723B Co-exist mechanism
+ *
+ * History
+ * 2012/11/15 Cosa first check in.
+ *
+ **************************************************************/
+/**************************************************************
+ * include files
+ **************************************************************/
+#include "halbt_precomp.h"
+#if 1
+/**************************************************************
+ * Global variables, these are static variables
+ **************************************************************/
+static struct coex_dm_8723b_2ant glcoex_dm_8723b_2ant;
+static struct coex_dm_8723b_2ant *coex_dm = &glcoex_dm_8723b_2ant;
+static struct coex_sta_8723b_2ant glcoex_sta_8723b_2ant;
+static struct coex_sta_8723b_2ant *coex_sta = &glcoex_sta_8723b_2ant;
+
+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;
+
+/**************************************************************
+ * local function proto type if needed
+ **************************************************************/
+/**************************************************************
+ * local function start with halbtc8723b2ant_
+ **************************************************************/
+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;
+}
+
+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;
+}
+
+void halbtc8723b2ant_monitor_bt_enable_disable(struct btc_coexist *btcoexist)
+{
+	static bool pre_bt_disabled = false;
+	static u32 bt_disable_cnt = 0;
+	bool bt_active = true, bt_disabled = false;
+
+	/* This function check if bt is disabled */
+	if (coex_sta->high_priority_tx == 0 &&
+	    coex_sta->high_priority_rx == 0 &&
+	    coex_sta->low_priority_tx == 0 &&
+	    coex_sta->low_priority_rx == 0)
+		bt_active = false;
+
+	if (coex_sta->high_priority_tx == 0xffff &&
+	    coex_sta->high_priority_rx == 0xffff &&
+	    coex_sta->low_priority_tx == 0xffff &&
+	    coex_sta->low_priority_rx == 0xffff)
+		bt_active = true;
+
+	if (bt_active) {
+		bt_disable_cnt = 0;
+		bt_disabled = false;
+		btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_DISABLE,
+				   &bt_disabled);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+			  "[BTCoex], BT is enabled !!\n");
+	} else {
+		bt_disable_cnt++;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+			  "[BTCoex], bt all counters=0, %d times!!\n",
+			  bt_disable_cnt);
+		if (bt_disable_cnt >= 2) {
+			bt_disabled = true;
+			btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_DISABLE,
+					   &bt_disabled);
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+				  "[BTCoex], BT is disabled !!\n");
+		}
+	}
+
+	if (pre_bt_disabled != bt_disabled) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+			  "[BTCoex], BT is from %s to %s!!\n",
+			  (pre_bt_disabled ? "disabled":"enabled"),
+			  (bt_disabled ? "disabled":"enabled"));
+
+		pre_bt_disabled = bt_disabled;
+		if (!bt_disabled) {
+		} else {
+		}
+	}
+}
+
+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);
+}
+
+void halbtc8723b2ant_query_bt_info(struct btc_coexist *btcoexist)
+{
+	u8 h2c_parameter[1] ={0};
+
+	coex_sta->c2h_bt_info_req_sent = true;
+
+	h2c_parameter[0] |= BIT0;	/* trigger */
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], Query Bt Info, FW write 0x61=0x%x\n",
+		  h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x61, 1, h2c_parameter);
+}
+
+bool halbtc8723b2ant_is_wifi_status_changed(struct btc_coexist *btcoexist)
+{
+	static bool pre_wifi_busy = false;
+	static bool pre_under_4way = false;
+	static bool pre_bt_hs_on = false;
+	bool wifi_busy = false, under_4way = false, bt_hs_on = false;
+	bool wifi_connected = false;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+			   &wifi_connected);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS,
+			   &under_4way);
+
+	if (wifi_connected) {
+		if (wifi_busy != pre_wifi_busy) {
+			pre_wifi_busy = wifi_busy;
+			return true;
+		}
+
+		if (under_4way != pre_under_4way) {
+			pre_under_4way = under_4way;
+			return true;
+		}
+
+		if (bt_hs_on != pre_bt_hs_on) {
+			pre_bt_hs_on = bt_hs_on;
+			return true;
+		}
+	}
+
+	return false;
+}
+
+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;
+}
+
+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;
+}
+
+bool halbtc8723b2ant_need_to_dec_bt_pwr(struct btc_coexist *btcoexist)
+{
+	bool bRet = false;
+	bool bt_hs_on = false, wifi_connected = false;
+	s32 bt_hs_rssi=0;
+	u8 bt_rssi_state;
+
+	if (!btcoexist->btc_get(btcoexist,
+			BTC_GET_BL_HS_OPERATION, &bt_hs_on))
+		return false;
+	if (!btcoexist->btc_get(btcoexist,
+			BTC_GET_BL_WIFI_CONNECTED, &wifi_connected))
+		return false;
+	if (!btcoexist->btc_get(btcoexist,
+			BTC_GET_S4_HS_RSSI, &bt_hs_rssi))
+		return false;
+
+	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
+
+	if (wifi_connected) {
+		if (bt_hs_on) {
+			if (bt_hs_rssi > 37) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+					  "[BTCoex], Need to decrease bt "
+					  "power for HS mode!!\n");
+				bRet = true;
+			}
+		} else {
+			if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+					  "[BTCoex], Need to decrease bt "
+					  "power for Wifi is connected!!\n");
+				bRet = true;
+			}
+		}
+	}
+
+	return bRet;
+}
+
+void halbtc8723b2ant_set_fw_dac_swing_level(struct btc_coexist *btcoexist,
+					    u8 dac_swing_lvl)
+{
+	u8 h2c_parameter[1] ={0};
+
+	/* There are several type of dacswing
+	 * 0x18/ 0x10/ 0xc/ 0x8/ 0x4/ 0x6 */
+	h2c_parameter[0] = dac_swing_lvl;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], Set Dac Swing Level=0x%x\n", dac_swing_lvl);
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], FW write 0x64=0x%x\n", h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x64, 1, h2c_parameter);
+}
+
+void halbtc8723b2ant_set_fw_dec_bt_pwr(struct btc_coexist *btcoexist,
+				       bool dec_bt_pwr)
+{
+	u8 h2c_parameter[1] = {0};
+
+	h2c_parameter[0] = 0;
+
+	if (dec_bt_pwr)
+		h2c_parameter[0] |= BIT1;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], decrease Bt Power : %s, FW write 0x62=0x%x\n",
+		  (dec_bt_pwr? "Yes!!":"No!!"), h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x62, 1, h2c_parameter);
+}
+
+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;
+}
+
+void halbtc8723b2ant_set_bt_auto_report(struct btc_coexist *btcoexist,
+					bool enable_auto_report)
+{
+	u8 h2c_parameter[1] = {0};
+	h2c_parameter[0] = 0;
+
+	if (enable_auto_report)
+		h2c_parameter[0] |= BIT0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], BT FW auto report : %s, FW write 0x68=0x%x\n",
+		  (enable_auto_report? "Enabled!!":"Disabled!!"),
+		  h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x68, 1, h2c_parameter);
+}
+
+void halbtc8723b2ant_bt_auto_report(struct btc_coexist *btcoexist,
+				    bool force_exec, bool enable_auto_report)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s BT Auto report = %s\n",
+		  (force_exec? "force to":""),
+		  ((enable_auto_report)? "Enabled":"Disabled"));
+	coex_dm->cur_bt_auto_report = enable_auto_report;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], bPreBtAutoReport=%d, "
+			  "bCurBtAutoReport=%d\n",
+			  coex_dm->pre_bt_auto_report,
+			  coex_dm->cur_bt_auto_report);
+
+		if (coex_dm->pre_bt_auto_report == coex_dm->cur_bt_auto_report)
+			return;
+	}
+	halbtc8723b2ant_set_bt_auto_report(btcoexist,
+					   coex_dm->cur_bt_auto_report);
+
+	coex_dm->pre_bt_auto_report = coex_dm->cur_bt_auto_report;
+}
+
+void halbtc8723b2ant_fw_dac_swing_lvl(struct btc_coexist *btcoexist,
+				      bool force_exec, u8 fw_dac_swing_lvl)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s set FW Dac Swing level = %d\n",
+		  (force_exec? "force to":""), fw_dac_swing_lvl);
+	coex_dm->cur_fw_dac_swing_lvl = fw_dac_swing_lvl;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], preFwDacSwingLvl=%d, "
+			  "curFwDacSwingLvl=%d\n",
+			  coex_dm->pre_fw_dac_swing_lvl,
+			  coex_dm->cur_fw_dac_swing_lvl);
+
+		if(coex_dm->pre_fw_dac_swing_lvl ==
+		   coex_dm->cur_fw_dac_swing_lvl)
+			return;
+	}
+
+	halbtc8723b2ant_set_fw_dac_swing_level(btcoexist,
+					       coex_dm->cur_fw_dac_swing_lvl);
+	coex_dm->pre_fw_dac_swing_lvl = coex_dm->cur_fw_dac_swing_lvl;
+}
+
+void halbtc8723b2ant_set_sw_rf_rx_lpf_corner(struct btc_coexist *btcoexist,
+					     bool rx_rf_shrink_on)
+{
+	if (rx_rf_shrink_on) {
+		/* Shrink RF Rx LPF corner */
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+			  "[BTCoex], Shrink RF Rx LPF corner!!\n");
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1e,
+					  0xfffff, 0xffffc);
+	} else {
+		/* Resume RF Rx LPF corner */
+		/* After initialized, we can use coex_dm->btRf0x1eBackup */
+		if (btcoexist->initilized) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+				  "[BTCoex], Resume RF Rx LPF corner!!\n");
+			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1e,
+						  0xfffff,
+						  coex_dm->bt_rf0x1e_backup);
+		}
+	}
+}
+
+void halbtc8723b2ant_rf_shrink(struct btc_coexist *btcoexist,
+			       bool force_exec, bool rx_rf_shrink_on)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
+		  "[BTCoex], %s turn Rx RF Shrink = %s\n",
+		  (force_exec? "force to":""), (rx_rf_shrink_on? "ON":"OFF"));
+	coex_dm->cur_rf_rx_lpf_shrink = rx_rf_shrink_on;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "[BTCoex], bPreRfRxLpfShrink=%d, "
+			  "bCurRfRxLpfShrink=%d\n",
+			  coex_dm->pre_rf_rx_lpf_shrink,
+			  coex_dm->cur_rf_rx_lpf_shrink);
+
+		if (coex_dm->pre_rf_rx_lpf_shrink ==
+		    coex_dm->cur_rf_rx_lpf_shrink)
+			return;
+	}
+	halbtc8723b2ant_set_sw_rf_rx_lpf_corner(btcoexist,
+						coex_dm->cur_rf_rx_lpf_shrink);
+
+	coex_dm->pre_rf_rx_lpf_shrink = coex_dm->cur_rf_rx_lpf_shrink;
+}
+
+void halbtc8723b2ant_set_sw_penalty_txrate_adaptive(
+						struct btc_coexist *btcoexist,
+						bool low_penalty_ra)
+{
+	u8 h2c_parameter[6] ={0};
+
+	h2c_parameter[0] = 0x6;	/* opCode, 0x6= Retry_Penalty*/
+
+	if (low_penalty_ra) {
+		h2c_parameter[1] |= BIT0;
+		/*normal rate except MCS7/6/5, OFDM54/48/36*/
+		h2c_parameter[2] = 0x00;
+		h2c_parameter[3] = 0xf7;  /*MCS7 or OFDM54*/
+		h2c_parameter[4] = 0xf8;  /*MCS6 or OFDM48*/
+		h2c_parameter[5] = 0xf9;  /*MCS5 or OFDM36*/
+	}
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], set WiFi Low-Penalty Retry: %s",
+		  (low_penalty_ra? "ON!!":"OFF!!"));
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x69, 6, h2c_parameter);
+}
+
+void halbtc8723b2ant_low_penalty_ra(struct btc_coexist *btcoexist,
+				    bool force_exec, bool low_penalty_ra)
+{
+	/*return; */
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
+		  "[BTCoex], %s turn LowPenaltyRA = %s\n",
+		  (force_exec? "force to":""), (low_penalty_ra? "ON":"OFF"));
+	coex_dm->cur_low_penalty_ra = low_penalty_ra;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "[BTCoex], bPreLowPenaltyRa=%d, "
+			  "bCurLowPenaltyRa=%d\n",
+			  coex_dm->pre_low_penalty_ra,
+			  coex_dm->cur_low_penalty_ra);
+
+		if (coex_dm->pre_low_penalty_ra == coex_dm->cur_low_penalty_ra)
+			return;
+	}
+	halbtc8723b2ant_set_sw_penalty_txrate_adaptive(btcoexist,
+						coex_dm->cur_low_penalty_ra);
+
+	coex_dm->pre_low_penalty_ra = coex_dm->cur_low_penalty_ra;
+}
+
+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);
+}
+
+void halbtc8723b2ant_set_sw_fulltime_dac_swing(struct btc_coexist *btcoexist,
+					       bool sw_dac_swing_on,
+					       u32 sw_dac_swing_lvl)
+{
+	if(sw_dac_swing_on)
+		halbtc8723b2ant_set_dac_swing_reg(btcoexist, sw_dac_swing_lvl);
+	else
+		halbtc8723b2ant_set_dac_swing_reg(btcoexist, 0x18);
+}
+
+
+void halbtc8723b2ant_dac_swing(struct btc_coexist *btcoexist,
+			       bool force_exec, bool dac_swing_on,
+			       u32 dac_swing_lvl)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
+		  "[BTCoex], %s turn DacSwing=%s, dac_swing_lvl=0x%x\n",
+		  (force_exec? "force to":""),
+		  (dac_swing_on? "ON":"OFF"), dac_swing_lvl);
+	coex_dm->cur_dac_swing_on = dac_swing_on;
+	coex_dm->cur_dac_swing_lvl = dac_swing_lvl;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "[BTCoex], bPreDacSwingOn=%d, preDacSwingLvl=0x%x,"
+			  " bCurDacSwingOn=%d, curDacSwingLvl=0x%x\n",
+			  coex_dm->pre_dac_swing_on, coex_dm->pre_dac_swing_lvl,
+			  coex_dm->cur_dac_swing_on,
+			  coex_dm->cur_dac_swing_lvl);
+
+		if ((coex_dm->pre_dac_swing_on == coex_dm->cur_dac_swing_on) &&
+		    (coex_dm->pre_dac_swing_lvl == coex_dm->cur_dac_swing_lvl))
+			return;
+	}
+	mdelay(30);
+	halbtc8723b2ant_set_sw_fulltime_dac_swing(btcoexist, dac_swing_on,
+						  dac_swing_lvl);
+
+	coex_dm->pre_dac_swing_on = coex_dm->cur_dac_swing_on;
+	coex_dm->pre_dac_swing_lvl = coex_dm->cur_dac_swing_lvl;
+}
+
+void halbtc8723b2ant_set_adc_backoff(struct btc_coexist *btcoexist,
+				     bool adc_backoff)
+{
+	if (adc_backoff) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+			  "[BTCoex], BB BackOff Level On!\n");
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0xc05, 0x30, 0x3);
+	} else {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+			  "[BTCoex], BB BackOff Level Off!\n");
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0xc05, 0x30, 0x1);
+	}
+}
+
+void halbtc8723b2ant_adc_backoff(struct btc_coexist *btcoexist,
+				 bool force_exec, bool adc_backoff)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
+		  "[BTCoex], %s turn AdcBackOff = %s\n",
+		  (force_exec? "force to":""), (adc_backoff? "ON":"OFF"));
+	coex_dm->cur_adc_back_off = adc_backoff;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "[BTCoex], bPreAdcBackOff=%d, bCurAdcBackOff=%d\n",
+			  coex_dm->pre_adc_back_off,
+			  coex_dm->cur_adc_back_off);
+
+		if (coex_dm->pre_adc_back_off == coex_dm->cur_adc_back_off)
+			return;
+	}
+	halbtc8723b2ant_set_adc_backoff(btcoexist, coex_dm->cur_adc_back_off);
+
+	coex_dm->pre_adc_back_off = coex_dm->cur_adc_back_off;
+}
+
+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);
+}
+
+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;
+}
+
+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);
+}
+
+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;
+}
+
+void halbtc8723b2ant_coex_table_with_type(struct btc_coexist *btcoexist,
+					  bool force_exec, u8 type)
+{
+	switch (type) {
+	case 0:
+		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55555555,
+					   0x55555555, 0xffff, 0x3);
+		break;
+	case 1:
+		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55555555,
+					   0x5afa5afa, 0xffff, 0x3);
+		break;
+	case 2:
+		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x5a5a5a5a,
+					   0x5a5a5a5a, 0xffff, 0x3);
+		break;
+	case 3:
+		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0xaaaaaaaa,
+					   0xaaaaaaaa, 0xffff, 0x3);
+		break;
+	case 4:
+		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0xffffffff,
+					   0xffffffff, 0xffff, 0x3);
+		break;
+	case 5:
+		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x5fff5fff,
+					   0x5fff5fff, 0xffff, 0x3);
+		break;
+	case 6:
+		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
+					   0x5a5a5a5a, 0xffff, 0x3);
+		break;
+	case 7:
+		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
+					   0x5afa5afa, 0xffff, 0x3);
+		break;
+	case 8:
+		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x5aea5aea,
+					   0x5aea5aea, 0xffff, 0x3);
+		break;
+	case 9:
+		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
+					   0x5aea5aea, 0xffff, 0x3);
+		break;
+	case 10:
+		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
+					   0x5aff5aff, 0xffff, 0x3);
+		break;
+	case 11:
+		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
+					   0x5a5f5a5f, 0xffff, 0x3);
+		break;
+	case 12:
+		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
+					   0x5f5f5f5f, 0xffff, 0x3);
+		break;
+	default:
+		break;
+	}
+}
+
+void halbtc8723b2ant_set_fw_ignore_wlan_act(struct btc_coexist *btcoexist,
+					    bool enable)
+{
+	u8 h2c_parameter[1] ={0};
+
+	if (enable)
+		h2c_parameter[0] |= BIT0;/* function enable*/
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], set FW for BT Ignore Wlan_Act, "
+		  "FW write 0x63=0x%x\n", h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x63, 1, h2c_parameter);
+}
+
+void halbtc8723b2ant_ignore_wlan_act(struct btc_coexist *btcoexist,
+				     bool force_exec, bool enable)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s turn Ignore WlanAct %s\n",
+		  (force_exec? "force to":""), (enable? "ON":"OFF"));
+	coex_dm->cur_ignore_wlan_act = enable;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], bPreIgnoreWlanAct = %d, "
+			  "bCurIgnoreWlanAct = %d!!\n",
+			  coex_dm->pre_ignore_wlan_act,
+			  coex_dm->cur_ignore_wlan_act);
+
+		if (coex_dm->pre_ignore_wlan_act ==
+		    coex_dm->cur_ignore_wlan_act)
+			return;
+	}
+	halbtc8723b2ant_set_fw_ignore_wlan_act(btcoexist, enable);
+
+	coex_dm->pre_ignore_wlan_act = coex_dm->cur_ignore_wlan_act;
+}
+
+void halbtc8723b2ant_set_fw_ps_tdma(struct btc_coexist *btcoexist, u8 byte1,
+				    u8 byte2, u8 byte3, u8 byte4, u8 byte5)
+{
+	u8 h2c_parameter[5] ={0};
+
+	h2c_parameter[0] = byte1;
+	h2c_parameter[1] = byte2;
+	h2c_parameter[2] = byte3;
+	h2c_parameter[3] = byte4;
+	h2c_parameter[4] = byte5;
+
+	coex_dm->ps_tdma_para[0] = byte1;
+	coex_dm->ps_tdma_para[1] = byte2;
+	coex_dm->ps_tdma_para[2] = byte3;
+	coex_dm->ps_tdma_para[3] = byte4;
+	coex_dm->ps_tdma_para[4] = byte5;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], FW write 0x60(5bytes)=0x%x%08x\n",
+		  h2c_parameter[0],
+		  h2c_parameter[1] << 24 | h2c_parameter[2] << 16 |
+		  h2c_parameter[3] << 8 | h2c_parameter[4]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x60, 5, h2c_parameter);
+}
+
+void halbtc8723b2ant_sw_mechanism1(struct btc_coexist *btcoexist,
+				   bool shrink_rx_lpf, bool low_penalty_ra,
+				   bool limited_dig, bool bt_lna_constrain)
+{
+	/*
+	u32	wifi_bw;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if(BTC_WIFI_BW_HT40 != wifi_bw)  //only shrink RF Rx LPF for HT40
+	{
+		if (shrink_rx_lpf)
+			shrink_rx_lpf = false;
+	}
+	*/
+
+	halbtc8723b2ant_rf_shrink(btcoexist, NORMAL_EXEC, shrink_rx_lpf);
+	halbtc8723b2ant_low_penalty_ra(btcoexist, NORMAL_EXEC, low_penalty_ra);
+}
+
+void halbtc8723b2ant_sw_mechanism2(struct btc_coexist *btcoexist,
+				   bool agc_table_shift, bool adc_backoff,
+				   bool sw_dac_swing, u32 dac_swing_lvl)
+{
+	halbtc8723b2ant_agc_table(btcoexist, NORMAL_EXEC, agc_table_shift);
+	/*halbtc8723b2ant_adc_backoff(btcoexist, NORMAL_EXEC, adc_backoff);*/
+	halbtc8723b2ant_dac_swing(btcoexist, NORMAL_EXEC, sw_dac_swing,
+				  dac_swing_lvl);
+}
+
+void halbtc8723b2ant_set_ant_path(struct btc_coexist *btcoexist,
+				  u8 antpos_type, bool init_hwcfg,
+				  bool wifi_off)
+{
+	struct btc_board_info *board_info = &btcoexist->board_info;
+	u32 fw_ver = 0, u32tmp=0;
+	bool pg_ext_switch = false;
+	bool use_ext_switch = false;
+	u8 h2c_parameter[2] ={0};
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_EXT_SWITCH, &pg_ext_switch);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
+
+	if ((fw_ver<0xc0000) || pg_ext_switch)
+		use_ext_switch = true;
+
+	if (init_hwcfg) {
+		/* 0x4c[23]=0, 0x4c[24]=1  Antenna control by WL/BT */
+		u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
+		u32tmp &= ~BIT23;
+		u32tmp |= BIT24;
+		btcoexist->btc_write_4byte(btcoexist, 0x4c, u32tmp);
+
+		btcoexist->btc_write_1byte(btcoexist, 0x974, 0xff);
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x944, 0x3, 0x3);
+		btcoexist->btc_write_1byte(btcoexist, 0x930, 0x77);
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x67, 0x20, 0x1);
+
+		/* Force GNT_BT to low */
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x765, 0x18, 0x0);
+		btcoexist->btc_write_2byte(btcoexist, 0x948, 0x0);
+
+		if (board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT) {
+			/* tell firmware "no antenna inverse" */
+			h2c_parameter[0] = 0;
+			h2c_parameter[1] = 1;  /* ext switch type */
+			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
+						h2c_parameter);
+		} else {
+			/* tell firmware "antenna inverse" */
+			h2c_parameter[0] = 1;
+			h2c_parameter[1] = 1;  /* ext switch type */
+			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
+						h2c_parameter);
+		}
+	}
+
+	/* ext switch setting */
+	if (use_ext_switch) {
+		 /* fixed internal switch S1->WiFi, S0->BT */
+		btcoexist->btc_write_2byte(btcoexist, 0x948, 0x0);
+		switch (antpos_type) {
+		case BTC_ANT_WIFI_AT_MAIN:
+			/* ext switch main at wifi */
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x92c,
+							   0x3, 0x1);
+			break;
+		case BTC_ANT_WIFI_AT_AUX:
+			/* ext switch aux at wifi */
+			btcoexist->btc_write_1byte_bitmask(btcoexist,
+							   0x92c, 0x3, 0x2);
+			break;
+		}
+	} else {	/* internal switch */
+		/* fixed ext switch */
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x92c, 0x3, 0x1);
+		switch (antpos_type) {
+		case BTC_ANT_WIFI_AT_MAIN:
+			/* fixed internal switch S1->WiFi, S0->BT */
+			btcoexist->btc_write_2byte(btcoexist, 0x948, 0x0);
+			break;
+		case BTC_ANT_WIFI_AT_AUX:
+			/* fixed internal switch S0->WiFi, S1->BT */
+			btcoexist->btc_write_2byte(btcoexist, 0x948, 0x280);
+			break;
+		}
+	}
+}
+
+
+void halbtc8723b2ant_ps_tdma(struct btc_coexist *btcoexist, bool force_exec,
+			     bool turn_on, u8 type)
+{
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s turn %s PS TDMA, type=%d\n",
+		  (force_exec? "force to":""), (turn_on? "ON":"OFF"), type);
+	coex_dm->cur_ps_tdma_on = turn_on;
+	coex_dm->cur_ps_tdma = type;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], bPrePsTdmaOn = %d, bCurPsTdmaOn = %d!!\n",
+			  coex_dm->pre_ps_tdma_on, coex_dm->cur_ps_tdma_on);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], prePsTdma = %d, curPsTdma = %d!!\n",
+			  coex_dm->pre_ps_tdma, coex_dm->cur_ps_tdma);
+
+		if ((coex_dm->pre_ps_tdma_on == coex_dm->cur_ps_tdma_on) &&
+		    (coex_dm->pre_ps_tdma == coex_dm->cur_ps_tdma))
+			return;
+	}
+	if (turn_on) {
+		switch (type) {
+		case 1:
+		default:
+			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x1a,
+						       0x1a, 0xe1, 0x90);
+			break;
+		case 2:
+			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x12,
+						       0x12, 0xe1, 0x90);
+			break;
+		case 3:
+			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x1c,
+						       0x3, 0xf1, 0x90);
+			break;
+		case 4:
+			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x10,
+						       0x03, 0xf1, 0x90);
+			break;
+		case 5:
+			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x1a,
+						       0x1a, 0x60, 0x90);
+			break;
+		case 6:
+			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x12,
+						       0x12, 0x60, 0x90);
+			break;
+		case 7:
+			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x1c,
+						       0x3, 0x70, 0x90);
+			break;
+		case 8:
+			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xa3, 0x10,
+						       0x3, 0x70, 0x90);
+			break;
+		case 9:
+			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x1a,
+						       0x1a, 0xe1, 0x90);
+			break;
+		case 10:
+			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x12,
+						       0x12, 0xe1, 0x90);
+			break;
+		case 11:
+			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0xa,
+						       0xa, 0xe1, 0x90);
+			break;
+		case 12:
+			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x5,
+						       0x5, 0xe1, 0x90);
+			break;
+		case 13:
+			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x1a,
+						       0x1a, 0x60, 0x90);
+			break;
+		case 14:
+			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x12,
+						       0x12, 0x60, 0x90);
+			break;
+		case 15:
+			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0xa,
+						       0xa, 0x60, 0x90);
+			break;
+		case 16:
+			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x5,
+						       0x5, 0x60, 0x90);
+			break;
+		case 17:
+			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xa3, 0x2f,
+						       0x2f, 0x60, 0x90);
+			break;
+		case 18:
+			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x5,
+						       0x5, 0xe1, 0x90);
+			break;
+		case 19:
+			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x25,
+						       0x25, 0xe1, 0x90);
+			break;
+		case 20:
+			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x25,
+						       0x25, 0x60, 0x90);
+			break;
+		case 21:
+			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x15,
+						       0x03, 0x70, 0x90);
+			break;
+		case 71:
+			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x1a,
+						       0x1a, 0xe1, 0x90);
+			break;
+		}
+	} else {
+		/* disable PS tdma */
+		switch (type) {
+		case 0:
+			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0x0, 0x0, 0x0,
+						       0x40, 0x0);
+			break;
+		case 1:
+			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0x0, 0x0, 0x0,
+						       0x48, 0x0);
+			break;
+		default:
+			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0x0, 0x0, 0x0,
+						       0x40, 0x0);
+			break;
+		}
+	}
+
+	/* update pre state */
+	coex_dm->pre_ps_tdma_on = coex_dm->cur_ps_tdma_on;
+	coex_dm->pre_ps_tdma = coex_dm->cur_ps_tdma;
+}
+
+void halbtc8723b2ant_coex_alloff(struct btc_coexist *btcoexist)
+{
+	/* fw all off */
+	halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
+	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+	halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	/* sw all off */
+	halbtc8723b2ant_sw_mechanism1(btcoexist, false, false, false, false);
+	halbtc8723b2ant_sw_mechanism2(btcoexist, false, false, false, 0x18);
+
+	/* hw all off */
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+	halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
+}
+
+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);
+}
+
+void halbtc8723b2ant_action_bt_inquiry(struct btc_coexist *btcoexist)
+{
+	bool wifi_connected = false;
+	bool low_pwr_disable = true;
+
+	btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
+			   &low_pwr_disable);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+			   &wifi_connected);
+
+	if (wifi_connected) {
+		halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
+		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 3);
+	} else {
+		halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
+		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
+	}
+	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, FORCE_EXEC, 6);
+	halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	halbtc8723b2ant_sw_mechanism1(btcoexist, false, false, false, false);
+	halbtc8723b2ant_sw_mechanism2(btcoexist, false, false, false, 0x18);
+
+	coex_dm->need_recover_0x948 = true;
+	coex_dm->backup_0x948 = btcoexist->btc_read_2byte(btcoexist, 0x948);
+
+	halbtc8723b2ant_set_ant_path(btcoexist, BTC_ANT_WIFI_AT_AUX,
+				     false, false);
+}
+
+bool halbtc8723b2ant_is_common_action(struct btc_coexist *btcoexist)
+{
+	bool bCommon = false, wifi_connected = false;
+	bool wifi_busy = false;
+	bool bt_hs_on = false, low_pwr_disable = false;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+			   &wifi_connected);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+
+	if (!wifi_connected) {
+		low_pwr_disable = false;
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
+				   &low_pwr_disable);
+
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Wifi non-connected idle!!\n");
+
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff,
+					  0x0);
+		halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
+		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
+		halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+ 		halbtc8723b2ant_sw_mechanism1(btcoexist, false, false, false,
+					      false);
+		halbtc8723b2ant_sw_mechanism2(btcoexist, false, false, false,
+					      0x18);
+
+		bCommon = true;
+	} else {
+		if (BT_8723B_2ANT_BT_STATUS_NON_CONNECTED_IDLE ==
+		    coex_dm->bt_status) {
+			low_pwr_disable = false;
+			btcoexist->btc_set(btcoexist,
+					   BTC_SET_ACT_DISABLE_LOW_POWER,
+					   &low_pwr_disable);
+
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Wifi connected + "
+				  "BT non connected-idle!!\n");
+
+			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1,
+						  0xfffff, 0x0);
+			halbtc8723b2ant_coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 0);
+			halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false,
+						1);
+			halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC,
+							 0xb);
+			halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC,
+						   false);
+
+	      		halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+
+			bCommon = true;
+		} else if (BT_8723B_2ANT_BT_STATUS_CONNECTED_IDLE ==
+			   coex_dm->bt_status) {
+			low_pwr_disable = true;
+			btcoexist->btc_set(btcoexist,
+					   BTC_SET_ACT_DISABLE_LOW_POWER,
+					   &low_pwr_disable);
+
+			if(bt_hs_on)
+				return false;
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Wifi connected + "
+				  "BT connected-idle!!\n");
+
+			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1,
+						  0xfffff, 0x0);
+			halbtc8723b2ant_coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 0);
+			halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false,
+						1);
+			halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC,
+							 0xb);
+			halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC,
+						   false);
+
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+
+			bCommon = true;
+		} else {
+			low_pwr_disable = true;
+			btcoexist->btc_set(btcoexist,
+					   BTC_SET_ACT_DISABLE_LOW_POWER,
+					   &low_pwr_disable);
+
+			if (wifi_busy) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], Wifi Connected-Busy + "
+					  "BT Busy!!\n");
+				bCommon = false;
+			} else {
+				if(bt_hs_on)
+					return false;
+
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], Wifi Connected-Idle + "
+					  "BT Busy!!\n");
+
+				btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A,
+							  0x1, 0xfffff, 0x0);
+				halbtc8723b2ant_coex_table_with_type(btcoexist,
+								    NORMAL_EXEC,
+								    7);
+				halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 21);
+				halbtc8723b2ant_fw_dac_swing_lvl(btcoexist,
+								 NORMAL_EXEC,
+								 0xb);
+				if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
+					halbtc8723b2ant_dec_bt_pwr(btcoexist,
+								   NORMAL_EXEC,
+								   true);
+				else
+					halbtc8723b2ant_dec_bt_pwr(btcoexist,
+								   NORMAL_EXEC,
+								   false);
+				halbtc8723b2ant_sw_mechanism1(btcoexist, false,
+							      false, false,
+							      false);
+				halbtc8723b2ant_sw_mechanism2(btcoexist, false,
+							      false, false,
+							      0x18);
+				bCommon = true;
+			}
+		}
+	}
+
+	return bCommon;
+}
+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) */
+void halbtc8723b2ant_action_sco(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
+							  0, 2, 15, 0);
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 4);
+
+	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	/*for SCO quality at 11b/g mode*/
+	if (BTC_WIFI_BW_LEGACY == wifi_bw)
+		halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+	else  /*for SCO quality & wifi performance balance at 11n mode*/
+		halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 8);
+
+	/*for voice quality */
+	halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 0);
+
+	/* sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      true, 0x4);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      true, 0x4);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      true, 0x4);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      true, 0x4);
+		}
+	}
+}
+
+
+void halbtc8723b2ant_action_hid(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
+							  0, 2, 15, 0);
+	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if (BTC_WIFI_BW_LEGACY == wifi_bw) /*/for HID at 11b/g mode*/
+		halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
+	else  /*for HID quality & wifi performance balance at 11n mode*/
+		halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 9);
+
+	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
+	else
+		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 13);
+
+	/* sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+ 			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+ 			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+/*A2DP only / PAN(EDR) only/ A2DP+PAN(HS)*/
+void halbtc8723b2ant_action_a2dp(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, wifi_rssi_state1, bt_rssi_state;
+	u32 wifi_bw;
+	u8 ap_num = 0;
+
+	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
+							  0, 2, 15, 0);
+	wifi_rssi_state1 = halbtc8723b2ant_wifi_rssi_state(btcoexist,
+							   1, 2, 40, 0);
+	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_AP_NUM, &ap_num);
+
+	/* define the office environment */
+	/* driver don't know AP num in Linux, so we will never enter this if */
+	if (ap_num >= 10 && BTC_RSSI_HIGH(wifi_rssi_state1)) {
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff,
+					  0x0);
+		halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+		halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
+		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
+
+		/* sw mechanism */
+		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+		if (BTC_WIFI_BW_HT40 == wifi_bw) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      true, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      true, 0x18);
+		}
+		return;
+	}
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
+
+	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+		halbtc8723b2ant_tdma_duration_adjust(btcoexist,false, false, 1);
+	else
+		halbtc8723b2ant_tdma_duration_adjust(btcoexist,false, true, 1);
+
+	/* sw mechanism */
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+ 			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+void halbtc8723b2ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
+							  0, 2, 15, 0);
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
+
+	halbtc8723b2ant_tdma_duration_adjust(btcoexist, false, true, 2);
+
+	/* sw mechanism */
+	btcoexist->btc_get(btcoexist,
+		BTC_GET_U4_WIFI_BW, &wifi_bw);
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+ 			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false,0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+void halbtc8723b2ant_action_pan_edr(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
+							  0, 2, 15, 0);
+	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 10);
+
+	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 1);
+	else
+		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
+
+	/* sw mechanism */
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+
+/*PAN(HS) only*/
+void halbtc8723b2ant_action_pan_hs(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
+							  0, 2, 15, 0);
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+	    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
+
+	halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+/*PAN(EDR)+A2DP*/
+void halbtc8723b2ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
+							  0, 2, 15, 0);
+	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+		halbtc8723b2ant_coex_table_with_type(btcoexist,NORMAL_EXEC, 12);
+		if (BTC_WIFI_BW_HT40 == wifi_bw)
+			halbtc8723b2ant_tdma_duration_adjust(btcoexist, false,
+							     true, 3);
+		else
+			halbtc8723b2ant_tdma_duration_adjust(btcoexist, false,
+							     false, 3);
+	} else {
+		halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
+		halbtc8723b2ant_tdma_duration_adjust(btcoexist, false, true, 3);
+	}
+
+	/* sw mechanism	*/
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+void halbtc8723b2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
+							  0, 2, 15, 0);
+	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+		if (BTC_WIFI_BW_HT40 == wifi_bw) {
+			halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC,
+							 3);
+			halbtc8723b2ant_coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 11);
+			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1,
+						  0xfffff, 0x780);
+		} else {
+			halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC,
+							 6);
+			halbtc8723b2ant_coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 7);
+			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1,
+						  0xfffff, 0x0);
+		}
+		halbtc8723b2ant_tdma_duration_adjust(btcoexist, true, false, 2);
+	} else {
+		halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+		halbtc8723b2ant_coex_table_with_type(btcoexist,NORMAL_EXEC, 11);
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff,
+					  0x0);
+		halbtc8723b2ant_tdma_duration_adjust(btcoexist, true, true, 2);
+	}
+
+	/* sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+ 			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)){
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+/* HID+A2DP+PAN(EDR) */
+void halbtc8723b2ant_action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
+							  0, 2, 15, 0);
+	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
+
+	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+		if (BTC_WIFI_BW_HT40 == wifi_bw)
+			halbtc8723b2ant_tdma_duration_adjust(btcoexist, true,
+							     true, 2);
+		else
+			halbtc8723b2ant_tdma_duration_adjust(btcoexist, true,
+							     false, 3);
+	} else {
+		halbtc8723b2ant_tdma_duration_adjust(btcoexist, true, true, 3);
+	}
+
+	/* sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+void halbtc8723b2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
+							  0, 2, 15, 0);
+	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
+
+	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+		halbtc8723b2ant_tdma_duration_adjust(btcoexist, true, false, 2);
+	else
+		halbtc8723b2ant_tdma_duration_adjust(btcoexist, true, true, 2);
+
+	/* sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+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;
+	}
+}
+
+void halbtc8723b2ant_wifioff_hwcfg(struct btc_coexist *btcoexist)
+{
+	/* set wlan_act to low */
+	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0x4);
+	/* Force GNT_BT to High */
+	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x765, 0x18, 0x3);
+	/* BT select s0/s1 is controlled by BT */
+	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x67, 0x20, 0x0);
+}
+
+/*********************************************************************
+ *  work around function start with wa_halbtc8723b2ant_
+ *********************************************************************/
+/*********************************************************************
+ *  extern function start with EXhalbtc8723b2ant_
+ *********************************************************************/
+void ex_halbtc8723b2ant_init_hwconfig(struct btc_coexist *btcoexist)
+{
+	u8 u8tmp = 0;
+
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+		  "[BTCoex], 2Ant Init HW Config!!\n");
+	coex_dm->bt_rf0x1e_backup =
+		btcoexist->btc_get_rf_reg(btcoexist, BTC_RF_A, 0x1e, 0xfffff);
+
+	/* 0x790[5:0]=0x5 */
+	u8tmp = btcoexist->btc_read_1byte(btcoexist, 0x790);
+	u8tmp &= 0xc0;
+	u8tmp |= 0x5;
+	btcoexist->btc_write_1byte(btcoexist, 0x790, u8tmp);
+
+
+	/*Antenna config	*/
+	halbtc8723b2ant_set_ant_path(btcoexist, BTC_ANT_WIFI_AT_MAIN,
+				     true, false);
+
+
+
+
+	/* PTA parameter */
+	halbtc8723b2ant_coex_table_with_type(btcoexist, FORCE_EXEC, 0);
+
+	/* Enable counter statistics */
+	/*0x76e[3] =1, WLAN_Act control by PTA*/
+	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
+	btcoexist->btc_write_1byte(btcoexist, 0x778, 0x3);
+	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x40, 0x20, 0x1);
+}
+
+void ex_halbtc8723b2ant_init_coex_dm(struct btc_coexist *btcoexist)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+		  "[BTCoex], Coex Mechanism Init!!\n");
+	halbtc8723b2ant_init_coex_dm(btcoexist);
+}
+
+void ex_halbtc8723b2ant_display_coex_info(struct btc_coexist *btcoexist)
+{
+	struct btc_board_info *board_info = &btcoexist->board_info;
+	struct btc_stack_info *stack_info = &btcoexist->stack_info;
+	struct btc_bt_link_info* bt_link_info = &btcoexist->bt_link_info;
+	u8 *cli_buf = btcoexist->cli_buf;
+	u8 u8tmp[4], i, bt_info_ext, ps_tdma_case=0;
+	u32 u32tmp[4];
+	bool roam = false, scan = false;
+	bool link = false, wifi_under_5g = false;
+	bool bt_hs_on = false, wifi_busy = false;
+	s32 wifi_rssi = 0, bt_hs_rssi = 0;
+	u32 wifi_bw, wifi_traffic_dir, fa_ofdm, fa_cck;
+	u8 wifi_dot11_chnl, wifi_hs_chnl;
+	u32 fw_ver = 0, bt_patch_ver = 0;
+	u8 ap_num = 0;
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n ============[BT Coexist info]============");
+	CL_PRINTF(cli_buf);
+
+	if (btcoexist->manual_control) {
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\r\n ==========[Under Manual Control]============");
+		CL_PRINTF(cli_buf);
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\r\n ==========================================");
+		CL_PRINTF(cli_buf);
+	}
+
+	if (!board_info->bt_exist) {
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n BT not exists !!!");
+		CL_PRINTF(cli_buf);
+		return;
+	}
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ",
+		   "Ant PG number/ Ant mechanism:",
+		   board_info->pg_ant_num, board_info->btdm_ant_num);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %d",
+		   "BT stack/ hci ext ver",
+		   ((stack_info->profile_notified)? "Yes":"No"),
+		   stack_info->hci_version);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER, &bt_patch_ver);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n %-35s = %d_%x/ 0x%x/ 0x%x(%d)",
+		   "CoexVer/ fw_ver/ PatchVer",
+		   glcoex_ver_date_8723b_2ant, glcoex_ver_8723b_2ant,
+		   fw_ver, bt_patch_ver, bt_patch_ver);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_DOT11_CHNL,
+			   &wifi_dot11_chnl);
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifi_hs_chnl);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d(%d)",
+		   "Dot11 channel / HsChnl(HsMode)",
+		   wifi_dot11_chnl, wifi_hs_chnl, bt_hs_on);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x ",
+		   "H2C Wifi inform bt chnl Info", coex_dm->wifi_chnl_info[0],
+		   coex_dm->wifi_chnl_info[1], coex_dm->wifi_chnl_info[2]);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
+	btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi);
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_AP_NUM, &ap_num);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d",
+		   "Wifi rssi/ HS rssi/ AP#", wifi_rssi, bt_hs_rssi, ap_num);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ",
+		   "Wifi link/ roam/ scan", link, roam, scan);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under_5g);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION,
+			   &wifi_traffic_dir);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %s/ %s ",
+		   "Wifi status", (wifi_under_5g? "5G":"2.4G"),
+		   ((BTC_WIFI_BW_LEGACY == wifi_bw)? "Legacy":
+		   (((BTC_WIFI_BW_HT40 == wifi_bw)? "HT40":"HT20"))),
+		   ((!wifi_busy)? "idle":
+		   ((BTC_WIFI_TRAFFIC_TX ==wifi_traffic_dir)?\
+		   "uplink":"downlink")));
+	CL_PRINTF(cli_buf);
+
+
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d / %d / %d",
+		   "SCO/HID/PAN/A2DP",
+		   bt_link_info->sco_exist, bt_link_info->hid_exist,
+		   bt_link_info->pan_exist, bt_link_info->a2dp_exist);
+	CL_PRINTF(cli_buf);
+	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_BT_LINK_INFO);
+
+	bt_info_ext = coex_sta->bt_info_ext;
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s",
+		   "BT Info A2DP rate",
+		   (bt_info_ext&BIT0)? "Basic rate":"EDR rate");
+	CL_PRINTF(cli_buf);
+
+	for (i=0; i<BT_INFO_SRC_8723B_2ANT_MAX; i++) {
+		if (coex_sta->bt_info_c2h_cnt[i]) {
+			CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+				   "\r\n %-35s = %02x %02x %02x "
+				   "%02x %02x %02x %02x(%d)",
+				   glbt_info_src_8723b_2ant[i], \
+				   coex_sta->bt_info_c2h[i][0],
+				   coex_sta->bt_info_c2h[i][1],
+				   coex_sta->bt_info_c2h[i][2],
+				   coex_sta->bt_info_c2h[i][3],
+				   coex_sta->bt_info_c2h[i][4],
+				   coex_sta->bt_info_c2h[i][5],
+				   coex_sta->bt_info_c2h[i][6],
+				   coex_sta->bt_info_c2h_cnt[i]);
+			CL_PRINTF(cli_buf);
+		}
+	}
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s/%s",
+		   "PS state, IPS/LPS",
+		   ((coex_sta->under_ips? "IPS ON":"IPS OFF")),
+		   ((coex_sta->under_lps? "LPS ON":"LPS OFF")));
+	CL_PRINTF(cli_buf);
+	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_FW_PWR_MODE_CMD);
+
+	/* Sw mechanism	*/
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n %-35s", "============[Sw mechanism]============");
+	CL_PRINTF(cli_buf);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ",
+ 		   "SM1[ShRf/ LpRA/ LimDig]", coex_dm->cur_rf_rx_lpf_shrink,
+		   coex_dm->cur_low_penalty_ra, coex_dm->limited_dig);
+	CL_PRINTF(cli_buf);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d(0x%x) ",
+		   "SM2[AgcT/ AdcB/ SwDacSwing(lvl)]",
+		   coex_dm->cur_agc_table_en, coex_dm->cur_adc_back_off,
+		   coex_dm->cur_dac_swing_on, coex_dm->cur_dac_swing_lvl);
+	CL_PRINTF(cli_buf);
+
+	/* Fw mechanism	*/
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
+		   "============[Fw mechanism]============");
+	CL_PRINTF(cli_buf);
+
+	ps_tdma_case = coex_dm->cur_ps_tdma;
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n %-35s = %02x %02x %02x %02x %02x case-%d (auto:%d)",
+		   "PS TDMA", coex_dm->ps_tdma_para[0],
+		   coex_dm->ps_tdma_para[1], coex_dm->ps_tdma_para[2],
+		   coex_dm->ps_tdma_para[3], coex_dm->ps_tdma_para[4],
+		   ps_tdma_case, coex_dm->auto_tdma_adjust);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ",
+		   "DecBtPwr/ IgnWlanAct", coex_dm->cur_dec_bt_pwr,
+		   coex_dm->cur_ignore_wlan_act);
+	CL_PRINTF(cli_buf);
+
+	/* Hw setting */
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
+		   "============[Hw setting]============");
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x",
+		   "RF-A, 0x1e initVal", coex_dm->bt_rf0x1e_backup);
+	CL_PRINTF(cli_buf);
+
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x778);
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x880);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
+		   "0x778/0x880[29:25]", u8tmp[0],
+		   (u32tmp[0]&0x3e000000) >> 25);
+	CL_PRINTF(cli_buf);
+
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x948);
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x67);
+	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x765);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
+		   "0x948/ 0x67[5] / 0x765",
+		   u32tmp[0], ((u8tmp[0]&0x20)>> 5), u8tmp[1]);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x92c);
+	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x930);
+	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x944);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
+		   "0x92c[1:0]/ 0x930[7:0]/0x944[1:0]",
+		   u32tmp[0]&0x3, u32tmp[1]&0xff, u32tmp[2]&0x3);
+	CL_PRINTF(cli_buf);
+
+
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x39);
+	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x40);
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x4c);
+	u8tmp[2] = btcoexist->btc_read_1byte(btcoexist, 0x64);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x",
+		   "0x38[11]/0x40/0x4c[24:23]/0x64[0]",
+		   ((u8tmp[0] & 0x8)>>3), u8tmp[1],
+		   ((u32tmp[0]&0x01800000)>>23), u8tmp[2]&0x1);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x550);
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x522);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
+		   "0x550(bcn ctrl)/0x522", u32tmp[0], u8tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xc50);
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x49c);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
+		   "0xc50(dig)/0x49c(null-drop)", u32tmp[0]&0xff, u8tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xda0);
+	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0xda4);
+	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0xda8);
+	u32tmp[3] = btcoexist->btc_read_4byte(btcoexist, 0xcf0);
+
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0xa5b);
+	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0xa5c);
+
+	fa_ofdm = ((u32tmp[0]&0xffff0000) >> 16) +
+		  ((u32tmp[1]&0xffff0000) >> 16) +
+		   (u32tmp[1] & 0xffff) +
+		   (u32tmp[2] & 0xffff) +
+		  ((u32tmp[3]&0xffff0000) >> 16) +
+		   (u32tmp[3] & 0xffff) ;
+	fa_cck = (u8tmp[0] << 8) + u8tmp[1];
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
+		   "OFDM-CCA/OFDM-FA/CCK-FA", \
+		   u32tmp[0]&0xffff, fa_ofdm, fa_cck);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x6c0);
+	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x6c4);
+	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x6c8);
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x6cc);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x",
+		   "0x6c0/0x6c4/0x6c8/0x6cc(coexTable)", \
+		   u32tmp[0], u32tmp[1], u32tmp[2], u8tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
+		   "0x770(high-pri rx/tx)",
+		   coex_sta->high_priority_rx, coex_sta->high_priority_tx);
+	CL_PRINTF(cli_buf);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
+		   "0x774(low-pri rx/tx)", coex_sta->low_priority_rx,
+		   coex_sta->low_priority_tx);
+	CL_PRINTF(cli_buf);
+#if(BT_AUTO_REPORT_ONLY_8723B_2ANT == 1)
+	halbtc8723b2ant_monitor_bt_ctr(btcoexist);
+#endif
+	btcoexist->btc_disp_dbg_msg(btcoexist,
+	BTC_DBG_DISP_COEX_STATISTICS);
+}
+
+
+void ex_halbtc8723b2ant_ips_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	if (BTC_IPS_ENTER == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], IPS ENTER notify\n");
+		coex_sta->under_ips = true;
+		halbtc8723b2ant_wifioff_hwcfg(btcoexist);
+		halbtc8723b2ant_ignore_wlan_act(btcoexist, FORCE_EXEC, true);
+		halbtc8723b2ant_coex_alloff(btcoexist);
+	} else if (BTC_IPS_LEAVE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], IPS LEAVE notify\n");
+		coex_sta->under_ips = false;
+		ex_halbtc8723b2ant_init_hwconfig(btcoexist);
+		halbtc8723b2ant_init_coex_dm(btcoexist);
+		halbtc8723b2ant_query_bt_info(btcoexist);
+	}
+}
+
+void ex_halbtc8723b2ant_lps_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	if (BTC_LPS_ENABLE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], LPS ENABLE notify\n");
+		coex_sta->under_lps = true;
+	} else if (BTC_LPS_DISABLE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], LPS DISABLE notify\n");
+		coex_sta->under_lps = false;
+	}
+}
+
+void ex_halbtc8723b2ant_scan_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	if (BTC_SCAN_START == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], SCAN START notify\n");
+	else if (BTC_SCAN_FINISH == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], SCAN FINISH notify\n");
+}
+
+void ex_halbtc8723b2ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	if (BTC_ASSOCIATE_START == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], CONNECT START notify\n");
+	else if (BTC_ASSOCIATE_FINISH == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], CONNECT FINISH notify\n");
+}
+
+void ex_halbtc8723b2ant_media_status_notify(struct btc_coexist *btcoexist,
+					    u8 type)
+{
+	u8 h2c_parameter[3] ={0};
+	u32 wifi_bw;
+	u8 wifi_central_chnl;
+
+	if (BTC_MEDIA_CONNECT == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], MEDIA connect notify\n");
+	else
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], MEDIA disconnect notify\n");
+
+	/* only 2.4G we need to inform bt the chnl mask */
+	btcoexist->btc_get(btcoexist,
+		BTC_GET_U1_WIFI_CENTRAL_CHNL, &wifi_central_chnl);
+	if ((BTC_MEDIA_CONNECT == type) &&
+	    (wifi_central_chnl <= 14)) {
+		h2c_parameter[0] = 0x1;
+		h2c_parameter[1] = wifi_central_chnl;
+		btcoexist->btc_get(btcoexist,
+			BTC_GET_U4_WIFI_BW, &wifi_bw);
+		if (BTC_WIFI_BW_HT40 == wifi_bw)
+			h2c_parameter[2] = 0x30;
+		else
+			h2c_parameter[2] = 0x20;
+	}
+
+	coex_dm->wifi_chnl_info[0] = h2c_parameter[0];
+	coex_dm->wifi_chnl_info[1] = h2c_parameter[1];
+	coex_dm->wifi_chnl_info[2] = h2c_parameter[2];
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], FW write 0x66=0x%x\n",
+		  h2c_parameter[0] << 16 | h2c_parameter[1] << 8 |
+		  h2c_parameter[2]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x66, 3, h2c_parameter);
+}
+
+void ex_halbtc8723b2ant_special_packet_notify(struct btc_coexist *btcoexist,
+					      u8 type)
+{
+	if (type == BTC_PACKET_DHCP)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], DHCP Packet notify\n");
+}
+
+void ex_halbtc8723b2ant_bt_info_notify(struct btc_coexist *btcoexist,
+				       u8 *tmpbuf, u8 length)
+{
+	u8 btInfo = 0;
+	u8 i, rsp_source = 0;
+	bool bt_busy = false, limited_dig = false;
+	bool wifi_connected = false;
+
+	coex_sta->c2h_bt_info_req_sent = false;
+
+	rsp_source = tmpbuf[0]&0xf;
+	if(rsp_source >= BT_INFO_SRC_8723B_2ANT_MAX)
+		rsp_source = BT_INFO_SRC_8723B_2ANT_WIFI_FW;
+	coex_sta->bt_info_c2h_cnt[rsp_source]++;
+
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+		  "[BTCoex], Bt info[%d], length=%d, hex data=[",
+		  rsp_source, length);
+	for (i = 0; i < length; i++) {
+		coex_sta->bt_info_c2h[rsp_source][i] = tmpbuf[i];
+		if (i == 1)
+			btInfo = tmpbuf[i];
+		if (i == length-1)
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+				  "0x%02x]\n", tmpbuf[i]);
+		else
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+				  "0x%02x, ", tmpbuf[i]);
+	}
+
+	if (btcoexist->manual_control) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), "
+			  "return for Manual CTRL<===\n");
+		return;
+	}
+
+	if (BT_INFO_SRC_8723B_2ANT_WIFI_FW != rsp_source) {
+		coex_sta->bt_retry_cnt =	/* [3:0]*/
+			coex_sta->bt_info_c2h[rsp_source][2] & 0xf;
+
+		coex_sta->bt_rssi =
+			coex_sta->bt_info_c2h[rsp_source][3] * 2 + 10;
+
+		coex_sta->bt_info_ext =
+			coex_sta->bt_info_c2h[rsp_source][4];
+
+		/* Here we need to resend some wifi info to BT
+		     because bt is reset and loss of the info.*/
+		if ((coex_sta->bt_info_ext & BIT1)) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], BT ext info bit1 check,"
+				  " send wifi BW&Chnl to BT!!\n");
+			btcoexist->btc_get(btcoexist,BTC_GET_BL_WIFI_CONNECTED,
+					   &wifi_connected);
+			if (wifi_connected)
+				ex_halbtc8723b2ant_media_status_notify(
+							btcoexist,
+							BTC_MEDIA_CONNECT);
+			else
+				ex_halbtc8723b2ant_media_status_notify(
+							btcoexist,
+							BTC_MEDIA_DISCONNECT);
+		}
+
+		if ((coex_sta->bt_info_ext & BIT3)) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], BT ext info bit3 check, "
+				  "set BT NOT to ignore Wlan active!!\n");
+			halbtc8723b2ant_ignore_wlan_act(btcoexist, FORCE_EXEC,
+							false);
+		} else {
+			/* BT already NOT ignore Wlan active, do nothing here.*/
+		}
+#if(BT_AUTO_REPORT_ONLY_8723B_2ANT == 0)
+		if ((coex_sta->bt_info_ext & BIT4)) {
+			/* BT auto report already enabled, do nothing*/
+		} else {
+			halbtc8723b2ant_bt_auto_report(btcoexist, FORCE_EXEC,
+						       true);
+		}
+#endif
+	}
+
+	/* check BIT2 first ==> check if bt is under inquiry or page scan*/
+	if (btInfo & BT_INFO_8723B_2ANT_B_INQ_PAGE)
+		coex_sta->c2h_bt_inquiry_page = true;
+	else
+		coex_sta->c2h_bt_inquiry_page = false;
+
+	/* set link exist status*/
+	if (!(btInfo & BT_INFO_8723B_2ANT_B_CONNECTION)) {
+		coex_sta->bt_link_exist = false;
+		coex_sta->pan_exist = false;
+		coex_sta->a2dp_exist = false;
+		coex_sta->hid_exist = false;
+		coex_sta->sco_exist = false;
+	} else {// connection exists
+		coex_sta->bt_link_exist = true;
+		if (btInfo & BT_INFO_8723B_2ANT_B_FTP)
+			coex_sta->pan_exist = true;
+		else
+			coex_sta->pan_exist = false;
+		if (btInfo & BT_INFO_8723B_2ANT_B_A2DP)
+			coex_sta->a2dp_exist = true;
+		else
+			coex_sta->a2dp_exist = false;
+		if (btInfo & BT_INFO_8723B_2ANT_B_HID)
+			coex_sta->hid_exist = true;
+		else
+			coex_sta->hid_exist = false;
+		if (btInfo & BT_INFO_8723B_2ANT_B_SCO_ESCO)
+			coex_sta->sco_exist = true;
+		else
+			coex_sta->sco_exist = false;
+	}
+
+	halbtc8723b2ant_update_bt_link_info(btcoexist);
+
+	if (!(btInfo & BT_INFO_8723B_2ANT_B_CONNECTION)) {
+		coex_dm->bt_status = BT_8723B_2ANT_BT_STATUS_NON_CONNECTED_IDLE;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), "
+			  "BT Non-Connected idle!!!\n");
+	/* connection exists but no busy */
+	} else if (btInfo == BT_INFO_8723B_2ANT_B_CONNECTION) {
+		coex_dm->bt_status = BT_8723B_2ANT_BT_STATUS_CONNECTED_IDLE;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), BT Connected-idle!!!\n");
+	} else if ((btInfo & BT_INFO_8723B_2ANT_B_SCO_ESCO) ||
+		   (btInfo & BT_INFO_8723B_2ANT_B_SCO_BUSY)) {
+		coex_dm->bt_status =
+			BT_8723B_2ANT_BT_STATUS_SCO_BUSY;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), BT SCO busy!!!\n");
+	} else if (btInfo&BT_INFO_8723B_2ANT_B_ACL_BUSY) {
+		coex_dm->bt_status =
+			BT_8723B_2ANT_BT_STATUS_ACL_BUSY;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), BT ACL busy!!!\n");
+	} else {
+		coex_dm->bt_status = BT_8723B_2ANT_BT_STATUS_MAX;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), "
+			  "BT Non-Defined state!!!\n");
+	}
+
+	if ((BT_8723B_2ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) ||
+	    (BT_8723B_2ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
+	    (BT_8723B_2ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status)) {
+		bt_busy = true;
+		limited_dig = true;
+	} else {
+		bt_busy = false;
+		limited_dig = false;
+	}
+
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bt_busy);
+
+	coex_dm->limited_dig = limited_dig;
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_LIMITED_DIG, &limited_dig);
+
+	halbtc8723b2ant_run_coexist_mechanism(btcoexist);
+}
+
+void ex_halbtc8723b2ant_halt_notify(struct btc_coexist *btcoexist)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, "[BTCoex], Halt notify\n");
+
+	halbtc8723b2ant_wifioff_hwcfg(btcoexist);
+	halbtc8723b2ant_ignore_wlan_act(btcoexist, FORCE_EXEC, true);
+	ex_halbtc8723b2ant_media_status_notify(btcoexist, BTC_MEDIA_DISCONNECT);
+}
+
+void ex_halbtc8723b2ant_periodical(struct btc_coexist *btcoexist)
+{
+	struct btc_board_info *board_info = &btcoexist->board_info;
+	struct btc_stack_info *stack_info = &btcoexist->stack_info;
+	static u8 dis_ver_info_cnt = 0;
+	u32 fw_ver = 0, bt_patch_ver = 0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+		  "[BTCoex], =========================="
+		  "Periodical===========================\n");
+
+	if (dis_ver_info_cnt <= 5) {
+		dis_ver_info_cnt += 1;
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], ****************************"
+			  "************************************\n");
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], Ant PG Num/ Ant Mech/ "
+			  "Ant Pos = %d/ %d/ %d\n", board_info->pg_ant_num,
+			  board_info->btdm_ant_num, board_info->btdm_ant_pos);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], BT stack/ hci ext ver = %s / %d\n",
+			  ((stack_info->profile_notified)? "Yes":"No"),
+			  stack_info->hci_version);
+		btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER,
+				   &bt_patch_ver);
+		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], CoexVer/ fw_ver/ PatchVer = "
+			  "%d_%x/ 0x%x/ 0x%x(%d)\n",
+			  glcoex_ver_date_8723b_2ant, glcoex_ver_8723b_2ant,
+			  fw_ver, bt_patch_ver, bt_patch_ver);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], *****************************"
+			  "***********************************\n");
+	}
+
+#if(BT_AUTO_REPORT_ONLY_8723B_2ANT == 0)
+	halbtc8723b2ant_query_bt_info(btcoexist);
+	halbtc8723b2ant_monitor_bt_ctr(btcoexist);
+	halbtc8723b2ant_monitor_bt_enable_disable(btcoexist);
+#else
+	if (halbtc8723b2ant_is_wifi_status_changed(btcoexist) ||
+	    coex_dm->auto_tdma_adjust)
+		halbtc8723b2ant_run_coexist_mechanism(btcoexist);
+#endif
+}
+
+
+#endif
+
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtc8723b2ant.h b/drivers/staging/rtl8821ae/btcoexist/halbtc8723b2ant.h
new file mode 100644
index 000000000000..fa3784aa70cd
--- /dev/null
+++ b/drivers/staging/rtl8821ae/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			BIT7
+#define	BT_INFO_8723B_2ANT_B_A2DP			BIT6
+#define	BT_INFO_8723B_2ANT_B_HID			BIT5
+#define	BT_INFO_8723B_2ANT_B_SCO_BUSY			BIT4
+#define	BT_INFO_8723B_2ANT_B_ACL_BUSY			BIT3
+#define	BT_INFO_8723B_2ANT_B_INQ_PAGE			BIT2
+#define	BT_INFO_8723B_2ANT_B_SCO_ESCO			BIT1
+#define	BT_INFO_8723B_2ANT_B_CONNECTION			BIT0
+
+#define BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT		2
+
+typedef enum _BT_INFO_SRC_8723B_2ANT{
+	BT_INFO_SRC_8723B_2ANT_WIFI_FW			= 0x0,
+	BT_INFO_SRC_8723B_2ANT_BT_RSP			= 0x1,
+	BT_INFO_SRC_8723B_2ANT_BT_ACTIVE_SEND		= 0x2,
+	BT_INFO_SRC_8723B_2ANT_MAX
+}BT_INFO_SRC_8723B_2ANT,*PBT_INFO_SRC_8723B_2ANT;
+
+typedef enum _BT_8723B_2ANT_BT_STATUS{
+	BT_8723B_2ANT_BT_STATUS_NON_CONNECTED_IDLE	= 0x0,
+	BT_8723B_2ANT_BT_STATUS_CONNECTED_IDLE		= 0x1,
+	BT_8723B_2ANT_BT_STATUS_INQ_PAGE		= 0x2,
+	BT_8723B_2ANT_BT_STATUS_ACL_BUSY		= 0x3,
+	BT_8723B_2ANT_BT_STATUS_SCO_BUSY		= 0x4,
+	BT_8723B_2ANT_BT_STATUS_ACL_SCO_BUSY		= 0x5,
+	BT_8723B_2ANT_BT_STATUS_MAX
+}BT_8723B_2ANT_BT_STATUS,*PBT_8723B_2ANT_BT_STATUS;
+
+typedef enum _BT_8723B_2ANT_COEX_ALGO{
+	BT_8723B_2ANT_COEX_ALGO_UNDEFINED		= 0x0,
+	BT_8723B_2ANT_COEX_ALGO_SCO			= 0x1,
+	BT_8723B_2ANT_COEX_ALGO_HID			= 0x2,
+	BT_8723B_2ANT_COEX_ALGO_A2DP			= 0x3,
+	BT_8723B_2ANT_COEX_ALGO_A2DP_PANHS		= 0x4,
+	BT_8723B_2ANT_COEX_ALGO_PANEDR			= 0x5,
+	BT_8723B_2ANT_COEX_ALGO_PANHS			= 0x6,
+	BT_8723B_2ANT_COEX_ALGO_PANEDR_A2DP		= 0x7,
+	BT_8723B_2ANT_COEX_ALGO_PANEDR_HID		= 0x8,
+	BT_8723B_2ANT_COEX_ALGO_HID_A2DP_PANEDR		= 0x9,
+	BT_8723B_2ANT_COEX_ALGO_HID_A2DP		= 0xa,
+	BT_8723B_2ANT_COEX_ALGO_MAX			= 0xb,
+}BT_8723B_2ANT_COEX_ALGO,*PBT_8723B_2ANT_COEX_ALGO;
+
+struct coex_dm_8723b_2ant{
+	/* fw mechanism */
+	bool pre_dec_bt_pwr;
+	bool cur_dec_bt_pwr;
+	u8 pre_fw_dac_swing_lvl;
+	u8 cur_fw_dac_swing_lvl;
+	bool cur_ignore_wlan_act;
+	bool pre_ignore_wlan_act;
+	u8 pre_ps_tdma;
+	u8 cur_ps_tdma;
+	u8 ps_tdma_para[5];
+	u8 ps_tdma_du_adj_type;
+	bool reset_tdma_adjust;
+	bool auto_tdma_adjust;
+	bool pre_ps_tdma_on;
+	bool cur_ps_tdma_on;
+	bool pre_bt_auto_report;
+	bool cur_bt_auto_report;
+
+	/* sw mechanism */
+	bool pre_rf_rx_lpf_shrink;
+	bool cur_rf_rx_lpf_shrink;
+	u32 bt_rf0x1e_backup;
+	bool pre_low_penalty_ra;
+	bool cur_low_penalty_ra;
+	bool pre_dac_swing_on;
+	u32 pre_dac_swing_lvl;
+	bool cur_dac_swing_on;
+	u32 cur_dac_swing_lvl;
+	bool pre_adc_back_off;
+	bool cur_adc_back_off;
+	bool pre_agc_table_en;
+	bool cur_agc_table_en;
+	u32 pre_val0x6c0;
+	u32 cur_val0x6c0;
+	u32 pre_val0x6c4;
+	u32 cur_val0x6c4;
+	u32 pre_val0x6c8;
+	u32 cur_val0x6c8;
+	u8 pre_val0x6cc;
+	u8 cur_val0x6cc;
+	bool limited_dig;
+
+	/* algorithm related */
+	u8 pre_algorithm;
+	u8 cur_algorithm;
+	u8 bt_status;
+	u8 wifi_chnl_info[3];
+
+	bool need_recover_0x948;
+	u16 backup_0x948;
+};
+
+struct coex_sta_8723b_2ant{
+	bool bt_link_exist;
+	bool sco_exist;
+	bool a2dp_exist;
+	bool hid_exist;
+	bool pan_exist;
+
+	bool under_lps;
+	bool under_ips;
+	u32 high_priority_tx;
+	u32 high_priority_rx;
+	u32 low_priority_tx;
+	u32 low_priority_rx;
+	u8 bt_rssi;
+	u8 pre_bt_rssi_state;
+	u8 pre_wifi_rssi_state[4];
+	bool c2h_bt_info_req_sent;
+	u8 bt_info_c2h[BT_INFO_SRC_8723B_2ANT_MAX][10];
+	u32 bt_info_c2h_cnt[BT_INFO_SRC_8723B_2ANT_MAX];
+	bool c2h_bt_inquiry_page;
+	u8 bt_retry_cnt;
+	u8 bt_info_ext;
+};
+
+/*********************************************************************
+ * The following is interface which will notify coex module.
+ *********************************************************************/
+void ex_halbtc8723b2ant_init_hwconfig(struct btc_coexist *btcoexist);
+void ex_halbtc8723b2ant_init_coex_dm(struct btc_coexist *btcoexist);
+void ex_halbtc8723b2ant_ips_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_halbtc8723b2ant_lps_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_halbtc8723b2ant_scan_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_halbtc8723b2ant_connect_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_halbtc8723b2ant_media_status_notify(struct btc_coexist *btcoexist,
+					    u8 type);
+void ex_halbtc8723b2ant_special_packet_notify(struct btc_coexist *btcoexist,
+					      u8 type);
+void ex_halbtc8723b2ant_bt_info_notify(struct btc_coexist *btcoexist,
+				       u8 *tmpbuf, u8 length);
+void ex_halbtc8723b2ant_halt_notify(struct btc_coexist *btcoexist);
+void ex_halbtc8723b2ant_periodical(struct btc_coexist * btcoexist);
+void ex_halbtc8723b2ant_display_coex_info(struct btc_coexist *btcoexist);
+
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtcoutsrc.c b/drivers/staging/rtl8821ae/btcoexist/halbtcoutsrc.c
new file mode 100644
index 000000000000..9d9fa4d7575d
--- /dev/null
+++ b/drivers/staging/rtl8821ae/btcoexist/halbtcoutsrc.c
@@ -0,0 +1,1181 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2013 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+
+#include "halbt_precomp.h"
+
+/*#if(BT_30_SUPPORT == 1)*/
+#if 1
+/***********************************************
+ *		Global variables
+ ***********************************************/
+const char *const bt_profile_string[]={
+	"NONE",
+	"A2DP",
+	"PAN",
+	"HID",
+	"SCO",
+};
+
+const char *const bt_spec_string[]={
+	"1.0b",
+	"1.1",
+	"1.2",
+	"2.0+EDR",
+	"2.1+EDR",
+	"3.0+HS",
+	"4.0",
+};
+
+const char *const bt_link_role_string[]={
+	"Master",
+	"Slave",
+};
+
+const char *const h2c_state_string[]={
+	"successful",
+	"h2c busy",
+	"rf off",
+	"fw not read",
+};
+
+const char *const io_state_string[]={
+	"IO_STATUS_SUCCESS",
+	"IO_STATUS_FAIL_CANNOT_IO",
+	"IO_STATUS_FAIL_RF_OFF",
+	"IO_STATUS_FAIL_FW_READ_CLEAR_TIMEOUT",
+	"IO_STATUS_FAIL_WAIT_IO_EVENT_TIMEOUT",
+	"IO_STATUS_INVALID_LEN",
+	"IO_STATUS_IO_IDLE_QUEUE_EMPTY",
+	"IO_STATUS_IO_INSERT_WAIT_QUEUE_FAIL",
+	"IO_STATUS_UNKNOWN_FAIL",
+	"IO_STATUS_WRONG_LEVEL",
+	"IO_STATUS_H2C_STOPPED",
+};
+
+struct btc_coexist gl_bt_coexist;
+
+u32 btc_dbg_type[BTC_MSG_MAX];
+u8 btc_dbg_buf[100];
+
+/***************************************************
+ *		Debug related function
+ ***************************************************/
+bool halbtc_is_bt_coexist_available(struct btc_coexist *btcoexist)
+{
+	if (!btcoexist->binded || NULL == btcoexist->adapter)
+		return false;
+	
+	return true;
+}
+
+bool halbtc_is_wifi_busy(struct rtl_priv *rtlpriv)
+{
+
+	if (rtlpriv->link_info.b_busytraffic)
+		return true;
+	else
+		return false;
+}
+
+
+void halbtc_dbg_init(void)
+{
+	u8 i;
+
+	for (i = 0; i < BTC_MSG_MAX; i++)
+		btc_dbg_type[i] = 0;
+
+	btc_dbg_type[BTC_MSG_INTERFACE]			= 	\
+//			INTF_INIT				|
+//			INTF_NOTIFY				|
+			0;
+	
+	btc_dbg_type[BTC_MSG_ALGORITHM]			= 	\
+//			ALGO_BT_RSSI_STATE			|
+//			ALGO_WIFI_RSSI_STATE			|
+//			ALGO_BT_MONITOR				|
+//			ALGO_TRACE				|
+//			ALGO_TRACE_FW				|
+//			ALGO_TRACE_FW_DETAIL			|
+//			ALGO_TRACE_FW_EXEC			|
+//			ALGO_TRACE_SW				|
+//			ALGO_TRACE_SW_DETAIL			|
+//			ALGO_TRACE_SW_EXEC			|
+			0;
+}
+
+bool halbtc_is_hw_mailbox_exist(struct btc_coexist *btcoexist)
+{
+	return true;
+}
+
+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;
+}
+
+bool halbtc_legacy(struct rtl_priv *adapter)
+{
+	struct rtl_priv *rtlpriv = adapter;
+	struct rtl_mac *mac = rtl_mac(rtlpriv);
+	
+	bool is_legacy = false;
+
+	if ((mac->mode == WIRELESS_MODE_B) || (mac->mode == WIRELESS_MODE_B))
+		is_legacy = true;
+
+	return is_legacy;
+}
+
+bool halbtc_is_wifi_uplink(struct rtl_priv *adapter)
+{
+	struct rtl_priv *rtlpriv = adapter;
+	
+	if (rtlpriv->link_info.b_tx_busy_traffic)
+		return true;
+	else
+		return false;
+}
+
+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;
+}
+
+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;
+}
+
+void halbtc_leave_lps(struct btc_coexist *btcoexist)
+{
+	struct rtl_priv *rtlpriv;
+	struct rtl_ps_ctl *ppsc;
+	bool ap_enable = false;
+	
+	rtlpriv = btcoexist->adapter;
+	ppsc = rtl_psc(rtlpriv);
+	
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_AP_MODE_ENABLE,
+			   &ap_enable);
+	
+	if (ap_enable) {
+		printk("halbtc_leave_lps()<--dont leave lps under AP mode\n");
+		return;
+	}
+
+	btcoexist->bt_info.bt_ctrl_lps = true;
+	btcoexist->bt_info.bt_lps_on = false;
+}
+
+void halbtc_enter_lps(struct btc_coexist *btcoexist)
+{
+	struct rtl_priv *rtlpriv;
+	struct rtl_ps_ctl *ppsc;
+	bool ap_enable = false;
+	
+	rtlpriv = btcoexist->adapter;
+	ppsc = rtl_psc(rtlpriv);
+	
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_AP_MODE_ENABLE, 
+			   &ap_enable);
+	
+	if (ap_enable) {
+		printk("halbtc_enter_lps()<--dont enter lps under AP mode\n");
+		return;
+	}
+	
+	btcoexist->bt_info.bt_ctrl_lps = true;
+	btcoexist->bt_info.bt_lps_on = false;
+}
+
+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;
+	}
+		
+}
+
+void halbtc_leave_low_power(void)
+{
+}
+
+void halbtc_nomal_low_power(void)
+{
+}
+
+void halbtc_disable_low_power(void)
+{
+}
+
+void halbtc_aggregation_check(void)
+{
+}
+
+
+u32 halbtc_get_bt_patch_version(struct btc_coexist *btcoexist)
+{
+	return 0;
+}
+
+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;
+}
+
+bool halbtc_get(void *void_btcoexist, u8 get_type, void *out_buf)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)void_btcoexist;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_mac *mac = rtl_mac(rtlpriv);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	bool *bool_tmp = (bool*)out_buf;
+	int *s32_tmp = (int*)out_buf;
+	u32 *u32_tmp = (u32*)out_buf;
+	u8 *u8_tmp = (u8*)out_buf;
+	bool tmp = false;
+
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return false;
+	
+
+	switch (get_type){
+	case BTC_GET_BL_HS_OPERATION:
+		*bool_tmp = false;
+		break;
+	case BTC_GET_BL_HS_CONNECTING:
+		*bool_tmp = false;
+		break;
+	case BTC_GET_BL_WIFI_CONNECTED:
+		if (rtlpriv->mac80211.link_state >= MAC80211_LINKED)
+			tmp = true;
+				
+		*bool_tmp = tmp;
+		break;
+	case BTC_GET_BL_WIFI_BUSY:
+		if(halbtc_is_wifi_busy(rtlpriv))
+			*bool_tmp = true;
+		else
+			*bool_tmp = false;
+		break;
+	case BTC_GET_BL_WIFI_SCAN:
+		if (mac->act_scanning == true)
+			*bool_tmp = true;
+		else
+			*bool_tmp = false;
+		break;
+	case BTC_GET_BL_WIFI_LINK:
+		if (mac->link_state == MAC80211_LINKING)
+			*bool_tmp = true;
+		else
+			*bool_tmp = false;
+		break;
+	case BTC_GET_BL_WIFI_ROAM:	/*TODO*/
+		if (mac->link_state == MAC80211_LINKING)
+			*bool_tmp = true;
+		else
+			*bool_tmp = false;
+		break;
+	case BTC_GET_BL_WIFI_4_WAY_PROGRESS:	/*TODO*/
+			*bool_tmp = false;
+
+		break;
+	case BTC_GET_BL_WIFI_UNDER_5G:
+		*bool_tmp = false; /*TODO*/
+		
+	case BTC_GET_BL_WIFI_DHCP:	/*TODO*/
+		break;
+	case BTC_GET_BL_WIFI_SOFTAP_IDLE:
+		*bool_tmp = true;
+		break;
+	case BTC_GET_BL_WIFI_SOFTAP_LINKING:
+		*bool_tmp = false;
+		break;
+	case BTC_GET_BL_WIFI_IN_EARLY_SUSPEND:
+		*bool_tmp = false;
+		break;
+	case BTC_GET_BL_WIFI_AP_MODE_ENABLE:
+		*bool_tmp = false;
+		break;
+	case BTC_GET_BL_WIFI_ENABLE_ENCRYPTION:
+		if (NO_ENCRYPTION == rtlpriv->sec.pairwise_enc_algorithm)
+			*bool_tmp = false;
+		else
+			*bool_tmp = true;
+		break;
+	case BTC_GET_BL_WIFI_UNDER_B_MODE:
+		*bool_tmp = false; /*TODO*/
+		break;
+	case BTC_GET_BL_EXT_SWITCH:
+		*bool_tmp = false;
+		break;
+	case BTC_GET_S4_WIFI_RSSI:
+		*s32_tmp = halbtc_get_wifi_rssi(rtlpriv);
+		break;
+	case BTC_GET_S4_HS_RSSI:	/*TODO*/
+		*s32_tmp = halbtc_get_wifi_rssi(rtlpriv);
+		break;
+	case BTC_GET_U4_WIFI_BW:
+		*u32_tmp = halbtc_get_wifi_bw(btcoexist);
+		break;
+	case BTC_GET_U4_WIFI_TRAFFIC_DIRECTION:
+		if (halbtc_is_wifi_uplink(rtlpriv))
+			*u32_tmp = BTC_WIFI_TRAFFIC_TX;
+		else
+			*u32_tmp = BTC_WIFI_TRAFFIC_RX;
+		break;
+	case BTC_GET_U4_WIFI_FW_VER:
+		*u32_tmp = rtlhal->fw_version;
+		break;
+	case BTC_GET_U4_BT_PATCH_VER:
+		*u32_tmp = halbtc_get_bt_patch_version(btcoexist);
+		break;
+	case BTC_GET_U1_WIFI_DOT11_CHNL:
+		*u8_tmp = rtlphy->current_channel;
+		break;
+	case BTC_GET_U1_WIFI_CENTRAL_CHNL:
+		*u8_tmp = halbtc_get_wifi_central_chnl(btcoexist);
+		break;
+	case BTC_GET_U1_WIFI_HS_CHNL:
+		*u8_tmp = 1;/* BT_OperateChnl(rtlpriv); */
+		break;
+	case BTC_GET_U1_MAC_PHY_MODE:
+		*u8_tmp = BTC_MP_UNKNOWN;
+		break;
+	case BTC_GET_U1_AP_NUM:
+		/* driver don't know AP num in Linux, 
+		 * So, the return value here is not right */
+		*u8_tmp = 1;/* pDefMgntInfo->NumBssDesc4Query; */
+		break;
+
+	/************* 1Ant **************/
+	case BTC_GET_U1_LPS_MODE:
+		*u8_tmp = btcoexist->pwr_mode_val[0];
+		break;
+					
+	default:
+		break;
+	}
+	
+	return true;
+}
+
+bool halbtc_set(void *void_btcoexist, u8 set_type, void *in_buf)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)void_btcoexist;
+	bool *bool_tmp = (bool *)in_buf;
+	u8 *u8_tmp = (u8 *)in_buf;
+	u32 *u32_tmp = (u32 *)in_buf;
+	
+	
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return false;
+
+	switch (set_type) {
+	/* set some bool type variables. */
+	case BTC_SET_BL_BT_DISABLE:
+		btcoexist->bt_info.bt_disabled = *bool_tmp;
+		break;
+	case BTC_SET_BL_BT_TRAFFIC_BUSY:
+		btcoexist->bt_info.bt_busy = *bool_tmp;
+		break;
+	case BTC_SET_BL_BT_LIMITED_DIG:
+		btcoexist->bt_info.limited_dig = *bool_tmp;
+		break;
+	case BTC_SET_BL_FORCE_TO_ROAM:
+		btcoexist->bt_info.force_to_roam = *bool_tmp;
+		break;
+	case BTC_SET_BL_TO_REJ_AP_AGG_PKT:
+		btcoexist->bt_info.reject_agg_pkt = *bool_tmp;
+		break;
+	case BTC_SET_BL_BT_CTRL_AGG_SIZE:
+		btcoexist->bt_info.b_bt_ctrl_buf_size = *bool_tmp;
+		break;
+	case BTC_SET_BL_INC_SCAN_DEV_NUM:
+		btcoexist->bt_info.increase_scan_dev_num = *bool_tmp;
+		break;
+		/* set some u1Byte type variables. */
+	case BTC_SET_U1_RSSI_ADJ_VAL_FOR_AGC_TABLE_ON:
+		btcoexist->bt_info.rssi_adjust_for_agc_table_on = *u8_tmp;
+		break;
+	case BTC_SET_U1_AGG_BUF_SIZE:
+		btcoexist->bt_info.agg_buf_size = *u8_tmp;
+		break;
+		/* the following are some action which will be triggered */
+	case BTC_SET_ACT_GET_BT_RSSI:
+		/*BTHCI_SendGetBtRssiEvent(rtlpriv);*/
+		break;
+	case BTC_SET_ACT_AGGREGATE_CTRL:
+		halbtc_aggregation_check();
+		break;
+
+		/* 1Ant */
+	case BTC_SET_U1_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE:
+		btcoexist->bt_info.rssi_adjust_for_1ant_coex_type = *u8_tmp;
+		break;
+	case BTC_SET_UI_SCAN_SIG_COMPENSATION:
+	/*	rtlpriv->mlmepriv.scan_compensation = *u8_tmp;  */
+		break;
+	case BTC_SET_U1_1ANT_LPS:
+		btcoexist->bt_info.lps_1ant = *u8_tmp;
+		break;
+	case BTC_SET_U1_1ANT_RPWM:
+		btcoexist->bt_info.rpwm_1ant = *u8_tmp;
+		break;
+	/* the following are some action which will be triggered  */
+	case BTC_SET_ACT_LEAVE_LPS:
+		halbtc_leave_lps(btcoexist);
+		break;
+	case BTC_SET_ACT_ENTER_LPS:
+		halbtc_enter_lps(btcoexist);
+		break;
+	case BTC_SET_ACT_NORMAL_LPS:
+		halbtc_normal_lps(btcoexist);
+		break;
+	case BTC_SET_ACT_DISABLE_LOW_POWER:
+		halbtc_disable_low_power();
+		break;
+	case BTC_SET_ACT_UPDATE_ra_mask:
+		btcoexist->bt_info.ra_mask = *u32_tmp;
+		break;
+	case BTC_SET_ACT_SEND_MIMO_PS:
+		break;
+	case BTC_SET_ACT_INC_FORCE_EXEC_PWR_CMD_CNT:
+		btcoexist->bt_info.force_exec_pwr_cmd_cnt++;
+		break;
+	case BTC_SET_ACT_CTRL_BT_INFO: /*wait for 8812/8821*/
+		break;
+	case BTC_SET_ACT_CTRL_BT_COEX:
+		break;
+	default:
+		break;
+	}
+	
+	return true;
+}
+
+void halbtc_display_coex_statistics(struct btc_coexist *btcoexist)
+{
+}
+
+void halbtc_display_bt_link_info(struct btc_coexist *btcoexist)
+{
+}
+
+void halbtc_display_bt_fw_info(struct btc_coexist *btcoexist)
+{
+}
+
+void halbtc_display_fw_pwr_mode_cmd(struct btc_coexist *btcoexist)
+{
+}
+
+/************************************************************
+ *		IO related function
+ ************************************************************/
+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);
+}
+
+
+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);
+}
+
+
+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);
+}
+
+
+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);
+}
+
+void halbtc_bitmask_write_1byte(void *bt_context, u32 reg_addr, 
+				u8 bit_mask, u8 data)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	u8 original_value, bit_shift = 0;
+	u8 i;
+
+	if (bit_mask != MASKDWORD) {/*if not "double word" write*/
+		original_value = rtl_read_byte(rtlpriv, reg_addr);	
+		for (i=0; i<=7; i++) {
+			if((bit_mask>>i)&0x1)
+				break;
+		}
+		bit_shift = i;
+		data = (original_value & (~bit_mask)) | 
+			((data << bit_shift) & bit_mask);
+	}
+	rtl_write_byte(rtlpriv, reg_addr, data);
+}
+
+
+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);
+}
+
+
+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);
+}
+
+
+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)
+{
+	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_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);
+}
+
+
+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);
+}
+
+
+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);
+}
+
+
+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);
+}
+
+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;
+	}
+}
+
+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 exhalbtc_initlize_variables(struct rtl_priv *adapter)
+{
+	struct btc_coexist *btcoexist = &gl_bt_coexist;
+
+	btcoexist->statistics.cnt_bind++;
+	
+	halbtc_dbg_init();
+
+	if (btcoexist->binded)
+		return false;
+	else
+		btcoexist->binded = true;
+
+#if ( defined(CONFIG_PCI_HCI))
+	btcoexist->chip_interface = BTC_INTF_PCI;
+#elif ( defined(CONFIG_USB_HCI))
+	btcoexist->chip_interface = BTC_INTF_USB;
+#elif ( defined(CONFIG_SDIO_HCI))
+	btcoexist->chip_interface = BTC_INTF_SDIO;
+#elif ( defined(CONFIG_GSPI_HCI))
+	btcoexist->chip_interface = BTC_INTF_GSPI;
+#else
+	btcoexist->chip_interface = BTC_INTF_UNKNOWN;
+#endif
+
+	if (NULL == btcoexist->adapter)
+		btcoexist->adapter = adapter;
+
+	btcoexist->stack_info.profile_notified = false;
+
+	btcoexist->btc_read_1byte = halbtc_read_1byte;
+	btcoexist->btc_write_1byte = halbtc_write_1byte;
+	btcoexist->btc_write_1byte_bitmask = halbtc_bitmask_write_1byte;
+	btcoexist->btc_read_2byte = halbtc_read_2byte;
+	btcoexist->btc_write_2byte = halbtc_write_2byte;
+	btcoexist->btc_read_4byte = halbtc_read_4byte;
+	btcoexist->btc_write_4byte = halbtc_write_4byte;
+
+	btcoexist->btc_set_bb_reg = halbtc_set_bbreg;
+	btcoexist->btc_get_bb_reg = halbtc_get_bbreg;
+
+	btcoexist->btc_set_rf_reg = halbtc_set_rfreg;
+	btcoexist->btc_get_rf_reg = halbtc_get_rfreg;
+
+	btcoexist->btc_fill_h2c = halbtc_fill_h2c_cmd;
+	btcoexist->btc_disp_dbg_msg = halbtc_display_dbg_msg;
+	
+	btcoexist->btc_get = halbtc_get;
+	btcoexist->btc_set = halbtc_set;
+
+	btcoexist->cli_buf = &btc_dbg_buf[0];
+
+	btcoexist->bt_info.b_bt_ctrl_buf_size = false;
+	btcoexist->bt_info.agg_buf_size = 5;
+
+	btcoexist->bt_info.increase_scan_dev_num = false;
+	return true;
+}
+
+void exhalbtc_init_hw_config(struct btc_coexist *btcoexist)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+
+	btcoexist->statistics.cnt_init_hw_config++;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_halbtc8723b2ant_init_hwconfig(btcoexist);
+		else if(btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8723b1ant_init_hwconfig(btcoexist);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
+		ex_halbtc8192e2ant_init_hwconfig(btcoexist);
+	}
+	
+}
+
+void exhalbtc_init_coex_dm(struct btc_coexist *btcoexist)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+
+	btcoexist->statistics.cnt_init_coex_dm++;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_halbtc8723b2ant_init_coex_dm(btcoexist);		
+		else if(btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8723b1ant_init_coex_dm(btcoexist);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
+		ex_halbtc8192e2ant_init_coex_dm(btcoexist);
+	}
+	
+	btcoexist->initilized = true;
+}
+
+void exhalbtc_ips_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	u8 ips_type;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_ips_notify++;
+	if (btcoexist->manual_control)
+		return;
+
+	if (ERFOFF == type)
+		ips_type = BTC_IPS_ENTER;
+	else
+		ips_type = BTC_IPS_LEAVE;
+
+	halbtc_leave_low_power();
+	
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_halbtc8723b2ant_ips_notify(btcoexist, ips_type);
+		else if(btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8723b1ant_ips_notify(btcoexist, ips_type);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
+		ex_halbtc8192e2ant_ips_notify(btcoexist, ips_type);
+	}
+
+	halbtc_nomal_low_power();
+}
+
+void exhalbtc_lps_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	u8 lps_type;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_lps_notify++;
+	if (btcoexist->manual_control)
+		return;
+	
+	if (EACTIVE == type)
+		lps_type = BTC_LPS_DISABLE;
+	else
+		lps_type = BTC_LPS_ENABLE;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_halbtc8723b2ant_lps_notify(btcoexist, lps_type);
+		else if(btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8723b1ant_lps_notify(btcoexist, lps_type);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
+		ex_halbtc8192e2ant_lps_notify(btcoexist, lps_type);
+	}
+}
+
+void exhalbtc_scan_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	u8 scan_type;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_scan_notify++;
+	if (btcoexist->manual_control)
+		return;
+
+	if (type)
+		scan_type = BTC_SCAN_START;
+	else
+		scan_type = BTC_SCAN_FINISH;
+
+	halbtc_leave_low_power();
+	
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_halbtc8723b2ant_scan_notify(btcoexist, scan_type);
+		else if(btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8723b1ant_scan_notify(btcoexist, scan_type);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
+		ex_halbtc8192e2ant_scan_notify(btcoexist, scan_type);
+	}
+
+	halbtc_nomal_low_power();
+}
+
+void exhalbtc_connect_notify(struct btc_coexist *btcoexist, u8 action)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	u8 asso_type;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_connect_notify++;
+	if (btcoexist->manual_control)
+		return;
+
+	if (action)
+		asso_type = BTC_ASSOCIATE_START;
+	else
+		asso_type = BTC_ASSOCIATE_FINISH;
+
+	halbtc_leave_low_power();
+	
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_halbtc8723b2ant_connect_notify(btcoexist, asso_type);
+		else if(btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8723b1ant_connect_notify(btcoexist, asso_type);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
+		ex_halbtc8192e2ant_connect_notify(btcoexist, asso_type);
+	}
+
+	halbtc_nomal_low_power();
+}
+
+void exhalbtc_mediastatus_notify(struct btc_coexist *btcoexist, 
+				 enum rt_media_status media_status)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	u8 status;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_media_status_notify++;
+	if (btcoexist->manual_control)
+		return;
+
+	if (RT_MEDIA_CONNECT == media_status)
+		status = BTC_MEDIA_CONNECT;
+	else
+		status = BTC_MEDIA_DISCONNECT;
+
+	halbtc_leave_low_power();
+	
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_halbtc8723b2ant_media_status_notify(btcoexist, status);
+		else if(btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8723b1ant_media_status_notify(btcoexist, status);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
+		ex_halbtc8192e2ant_media_status_notify(btcoexist, status);
+	}
+
+	halbtc_nomal_low_power();
+}
+
+void exhalbtc_special_packet_notify(struct btc_coexist *btcoexist, u8 pkt_type)
+{
+	u8 packet_type;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_special_packet_notify++;
+	if (btcoexist->manual_control)
+		return;
+
+	/*if(PACKET_DHCP == pkt_type)*/
+		packet_type = BTC_PACKET_DHCP;
+	/*else if(PACKET_EAPOL == pkt_type)
+		packet_type = BTC_PACKET_EAPOL;
+	else
+		packet_type = BTC_PACKET_UNKNOWN;*/
+
+	halbtc_leave_low_power();
+
+	if (btcoexist->board_info.btdm_ant_num == 2)
+		ex_halbtc8723b2ant_special_packet_notify(btcoexist,
+							 packet_type);
+	else if (btcoexist->board_info.btdm_ant_num == 1)
+		ex_halbtc8723b1ant_special_packet_notify(btcoexist,
+							 packet_type);
+
+	halbtc_nomal_low_power();
+}
+
+void exhalbtc_bt_info_notify(struct btc_coexist *btcoexist, 
+			     u8 *tmp_buf, u8 length)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_bt_info_notify++;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_halbtc8723b2ant_bt_info_notify(btcoexist, tmp_buf, length);
+		else if(btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8723b1ant_bt_info_notify(btcoexist, tmp_buf, length);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
+	//	ex_halbtc8192e2ant_bt_info_notify(btcoexist, tmp_buf, length);
+	}
+}
+
+void exhalbtc_stack_operation_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	u8 stack_op_type;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_stack_operation_notify++;
+	if (btcoexist->manual_control)
+		return;
+
+	stack_op_type = BTC_STACK_OP_NONE;
+}
+
+void exhalbtc_halt_notify(struct btc_coexist *btcoexist)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_halbtc8723b2ant_halt_notify(btcoexist);
+		else if(btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8723b1ant_halt_notify(btcoexist);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
+		ex_halbtc8192e2ant_halt_notify(btcoexist);
+	}
+}
+
+void exhalbtc_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state)
+{
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+}
+
+void exhalbtc_periodical(struct btc_coexist *btcoexist)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_periodical++;
+
+	halbtc_leave_low_power();
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_halbtc8723b2ant_periodical(btcoexist);
+		else if(btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8723b1ant_periodical(btcoexist);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
+		ex_halbtc8192e2ant_periodical(btcoexist);
+	}
+
+	halbtc_nomal_low_power();
+}
+
+void exhalbtc_dbg_control(struct btc_coexist *btcoexist, 
+			  u8 code, u8 len, u8 *data)
+{
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_dbg_ctrl++;
+}
+
+void exhalbtc_stack_update_profile_info()
+{
+}
+
+void exhalbtc_update_min_bt_rssi(char bt_rssi)
+{
+	struct btc_coexist *btcoexist = &gl_bt_coexist;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+
+	btcoexist->stack_info.min_bt_rssi = bt_rssi;
+}
+
+
+void exhalbtc_set_hci_version(u16 hci_version)
+{
+	struct btc_coexist *btcoexist = &gl_bt_coexist;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+
+	btcoexist->stack_info.hci_version = hci_version;
+}
+
+void exhalbtc_set_bt_patch_version(u16 bt_hci_version, u16 bt_patch_version) 
+{
+	struct btc_coexist *btcoexist = &gl_bt_coexist;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+
+	btcoexist->bt_info.bt_real_fw_ver = bt_patch_version;
+	btcoexist->bt_info.bt_hci_ver = bt_hci_version;
+}
+
+void exhalbtc_set_bt_exist(bool bt_exist)
+{
+	gl_bt_coexist.board_info.bt_exist = bt_exist;
+}
+
+void exhalbtc_set_chip_type(u8 chip_type)
+{
+	switch (chip_type) {
+	default:
+	case BT_2WIRE:
+	case BT_ISSC_3WIRE:
+	case BT_ACCEL:
+	case BT_RTL8756:
+		gl_bt_coexist.board_info.bt_chip_type = BTC_CHIP_UNDEF;
+		break;
+	case BT_CSR_BC4:
+		gl_bt_coexist.board_info.bt_chip_type = BTC_CHIP_CSR_BC4;
+		break;
+	case BT_CSR_BC8:
+		gl_bt_coexist.board_info.bt_chip_type = BTC_CHIP_CSR_BC8;
+		break;
+	case BT_RTL8723A:
+		gl_bt_coexist.board_info.bt_chip_type = BTC_CHIP_RTL8723A;
+		break;
+	case BT_RTL8821A:
+		gl_bt_coexist.board_info.bt_chip_type = BTC_CHIP_RTL8821;
+		break;
+	case BT_RTL8723B:
+		gl_bt_coexist.board_info.bt_chip_type = BTC_CHIP_RTL8723B;
+		break;
+	}
+}
+
+void exhalbtc_set_ant_num(u8 type, u8 ant_num)
+{
+	if (BT_COEX_ANT_TYPE_PG == type) {
+		gl_bt_coexist.board_info.pg_ant_num = ant_num;
+		gl_bt_coexist.board_info.btdm_ant_num = ant_num;
+	} else if (BT_COEX_ANT_TYPE_ANTDIV == type) {
+		gl_bt_coexist.board_info.btdm_ant_num = ant_num;
+	}
+}
+
+void exhalbtc_display_bt_coex_info(struct btc_coexist *btcoexist)
+{
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	
+	if (btcoexist->board_info.btdm_ant_num == 2)
+		ex_halbtc8723b2ant_display_coex_info(btcoexist);
+	else if (btcoexist->board_info.btdm_ant_num == 1)
+		ex_halbtc8723b1ant_display_coex_info(btcoexist);
+}
+
+#endif
+
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtcoutsrc.h b/drivers/staging/rtl8821ae/btcoexist/halbtcoutsrc.h
new file mode 100644
index 000000000000..787798e76217
--- /dev/null
+++ b/drivers/staging/rtl8821ae/btcoexist/halbtcoutsrc.h
@@ -0,0 +1,549 @@
+#ifndef	__HALBTC_OUT_SRC_H__
+#define __HALBTC_OUT_SRC_H__
+
+#include	"../wifi.h"
+
+#define		NORMAL_EXEC				false
+#define		FORCE_EXEC				true
+
+#define		BTC_RF_A				RF90_PATH_A
+#define		BTC_RF_B				RF90_PATH_B
+#define		BTC_RF_C				RF90_PATH_C
+#define		BTC_RF_D				RF90_PATH_D
+
+#define		BTC_SMSP				SINGLEMAC_SINGLEPHY
+#define		BTC_DMDP				DUALMAC_DUALPHY
+#define		BTC_DMSP				DUALMAC_SINGLEPHY
+#define		BTC_MP_UNKNOWN				0xff
+
+#define 	IN
+#define 	OUT
+
+#define 	BT_TMP_BUF_SIZE 			100
+
+#define		BT_COEX_ANT_TYPE_PG			0
+#define		BT_COEX_ANT_TYPE_ANTDIV			1
+#define		BT_COEX_ANT_TYPE_DETECTED		2
+
+#define		BTC_MIMO_PS_STATIC			0
+#define		BTC_MIMO_PS_DYNAMIC			1
+
+#define		BTC_RATE_DISABLE			0
+#define		BTC_RATE_ENABLE				1
+
+/* single Antenna definition */
+#define		BTC_ANT_PATH_WIFI			0
+#define		BTC_ANT_PATH_BT				1
+#define		BTC_ANT_PATH_PTA			2
+/* dual Antenna definition */
+#define		BTC_ANT_WIFI_AT_MAIN			0
+#define		BTC_ANT_WIFI_AT_AUX			1
+/* coupler Antenna definition */
+#define		BTC_ANT_WIFI_AT_CPL_MAIN		0
+#define		BTC_ANT_WIFI_AT_CPL_AUX			1
+
+enum btc_chip_interface{
+	BTC_INTF_UNKNOWN	= 0,
+	BTC_INTF_PCI		= 1,
+	BTC_INTF_USB		= 2,
+	BTC_INTF_SDIO		= 3,
+	BTC_INTF_GSPI		= 4,
+	BTC_INTF_MAX
+};
+
+enum btc_chip_type{
+	BTC_CHIP_UNDEF		= 0,
+	BTC_CHIP_CSR_BC4	= 1,
+	BTC_CHIP_CSR_BC8	= 2,
+	BTC_CHIP_RTL8723A 	= 3,
+	BTC_CHIP_RTL8821	= 4,
+	BTC_CHIP_RTL8723B 	= 5,
+	BTC_CHIP_MAX
+};
+
+enum btc_msg_type{
+	BTC_MSG_INTERFACE	= 0x0,
+	BTC_MSG_ALGORITHM	= 0x1,
+	BTC_MSG_MAX
+};
+
+extern u32 btc_dbg_type[];
+
+/* following is for BTC_MSG_INTERFACE */
+#define		INTF_INIT				BIT0
+#define		INTF_NOTIFY				BIT2
+
+/* following is for BTC_ALGORITHM */
+#define		ALGO_BT_RSSI_STATE			BIT0
+#define		ALGO_WIFI_RSSI_STATE			BIT1
+#define		ALGO_BT_MONITOR				BIT2
+#define		ALGO_TRACE				BIT3
+#define		ALGO_TRACE_FW				BIT4
+#define		ALGO_TRACE_FW_DETAIL			BIT5
+#define		ALGO_TRACE_FW_EXEC			BIT6
+#define		ALGO_TRACE_SW				BIT7
+#define		ALGO_TRACE_SW_DETAIL			BIT8
+#define		ALGO_TRACE_SW_EXEC			BIT9
+
+
+
+#define	CL_SPRINTF	snprintf
+#define	CL_PRINTF	printk
+
+#define	BTC_PRINT(dbgtype, dbgflag, printstr, ...)		\
+	do { 							\
+		if (unlikely(btc_dbg_type[dbgtype] & dbgflag)) {\
+			printk(printstr, ##__VA_ARGS__);	\
+		}						\
+	} while(0)
+
+#define	BTC_PRINT_F(dbgtype, dbgflag, printstr, ...)		\
+	do {							\
+		if (unlikely(btc_dbg_type[dbgtype] & dbgflag)) {\
+			printk(KERN_DEBUG "%s: ", __func__);	\
+			printk(printstr, ##__VA_ARGS__);	\
+		}						\
+	} while(0)
+
+#define	BTC_PRINT_ADDR(dbgtype, dbgflag, printstr, _ptr)	\
+	do { 							\
+		if(unlikely(btc_dbg_type[dbgtype] & dbgflag)) {	\
+			int __i;				\
+			u8* __ptr = (u8*)_Ptr;			\
+			printk printstr;			\
+			for( __i = 0; __i < 6; __i++ )		\
+				printk("%02X%s", __ptr[__i], (__i==5)?"":"-");\
+			printk(KERN_DEBUG "\n");		\
+		}\
+	} while(0)
+
+#define BTC_PRINT_DATA(dbgtype, dbgflag, _titlestring, _hexdata, _hexdatalen) \
+	do {								\
+		if(unlikely(btc_dbg_type[dbgtype] & dbgflag) )	{ 	\
+			int __i;					\
+			u8 *__ptr = (u8*)_hexdata;			\
+			printk(_titlestring);				\
+			for( __i = 0; __i < (int)_hexdatalen; __i++ ) {	\
+				printk("%02X%s", __ptr[__i], (((__i + 1) % 4) \
+							== 0)?"  ":" ");\
+				if (((__i + 1) % 16) == 0)		\
+					printk("\n");			\
+			}						\
+			printk(KERN_DEBUG "\n");			\
+		} 							\
+	} while(0)
+
+
+#define	BTC_RSSI_HIGH(_rssi_) \
+	((_rssi_==BTC_RSSI_STATE_HIGH || _rssi_==BTC_RSSI_STATE_STAY_HIGH) ? \
+	true : false)
+
+#define	BTC_RSSI_MEDIUM(_rssi_)	\
+	((_rssi_==BTC_RSSI_STATE_MEDIUM || _rssi_==BTC_RSSI_STATE_STAY_MEDIUM) \
+	? true : false)
+
+#define	BTC_RSSI_LOW(_rssi_) \
+	((_rssi_==BTC_RSSI_STATE_LOW || _rssi_==BTC_RSSI_STATE_STAY_LOW) ? \
+	true : false)
+
+
+enum btc_power_save_type {
+	BTC_PS_WIFI_NATIVE = 0,
+	BTC_PS_LPS_ON = 1,
+	BTC_PS_LPS_OFF = 2,
+	BTC_PS_LPS_MAX
+};
+
+struct btc_board_info {
+	/* The following is some board information */
+	u8 bt_chip_type;
+	u8 pg_ant_num;	/* pg ant number */
+	u8 btdm_ant_num;	/* ant number for btdm */
+	u8 btdm_ant_pos;
+	bool bt_exist;
+};
+
+enum btc_dbg_opcode{
+	BTC_DBG_SET_COEX_NORMAL = 0x0,
+	BTC_DBG_SET_COEX_WIFI_ONLY = 0x1,
+	BTC_DBG_SET_COEX_BT_ONLY = 0x2,
+	BTC_DBG_MAX
+};
+
+enum btc_rssi_state{
+	BTC_RSSI_STATE_HIGH = 0x0,
+	BTC_RSSI_STATE_MEDIUM = 0x1,
+	BTC_RSSI_STATE_LOW = 0x2,
+	BTC_RSSI_STATE_STAY_HIGH = 0x3,
+	BTC_RSSI_STATE_STAY_MEDIUM = 0x4,
+	BTC_RSSI_STATE_STAY_LOW = 0x5,
+	BTC_RSSI_MAX
+};
+
+enum btc_wifi_role{
+	BTC_ROLE_STATION = 0x0,
+	BTC_ROLE_AP = 0x1,
+	BTC_ROLE_IBSS = 0x2,
+	BTC_ROLE_HS_MODE = 0x3,
+	BTC_ROLE_MAX
+};
+
+enum btc_wifi_bw_mode{
+	BTC_WIFI_BW_LEGACY = 0x0,
+	BTC_WIFI_BW_HT20 = 0x1,
+	BTC_WIFI_BW_HT40 = 0x2,
+	BTC_WIFI_BW_MAX
+};
+
+enum btc_wifi_traffic_dir{
+	BTC_WIFI_TRAFFIC_TX = 0x0,
+	BTC_WIFI_TRAFFIC_RX = 0x1,
+	BTC_WIFI_TRAFFIC_MAX
+};
+
+enum btc_wifi_pnp{
+	BTC_WIFI_PNP_WAKE_UP = 0x0,
+	BTC_WIFI_PNP_SLEEP = 0x1,
+	BTC_WIFI_PNP_MAX
+};
+
+
+enum btc_get_type{
+	/* type bool */
+	BTC_GET_BL_HS_OPERATION,
+	BTC_GET_BL_HS_CONNECTING,
+	BTC_GET_BL_WIFI_CONNECTED,
+	BTC_GET_BL_WIFI_BUSY,
+	BTC_GET_BL_WIFI_SCAN,
+	BTC_GET_BL_WIFI_LINK,
+	BTC_GET_BL_WIFI_DHCP,
+	BTC_GET_BL_WIFI_SOFTAP_IDLE,
+	BTC_GET_BL_WIFI_SOFTAP_LINKING,
+	BTC_GET_BL_WIFI_IN_EARLY_SUSPEND,
+	BTC_GET_BL_WIFI_ROAM,
+	BTC_GET_BL_WIFI_4_WAY_PROGRESS,
+	BTC_GET_BL_WIFI_UNDER_5G,
+	BTC_GET_BL_WIFI_AP_MODE_ENABLE,
+	BTC_GET_BL_WIFI_ENABLE_ENCRYPTION,
+	BTC_GET_BL_WIFI_UNDER_B_MODE,
+	BTC_GET_BL_EXT_SWITCH,
+
+	/* type s4Byte */
+	BTC_GET_S4_WIFI_RSSI,
+	BTC_GET_S4_HS_RSSI,
+
+	/* type u32 */
+	BTC_GET_U4_WIFI_BW,
+	BTC_GET_U4_WIFI_TRAFFIC_DIRECTION,
+	BTC_GET_U4_WIFI_FW_VER,
+	BTC_GET_U4_BT_PATCH_VER,
+
+	/* type u1Byte */
+	BTC_GET_U1_WIFI_DOT11_CHNL,
+	BTC_GET_U1_WIFI_CENTRAL_CHNL,
+	BTC_GET_U1_WIFI_HS_CHNL,
+	BTC_GET_U1_MAC_PHY_MODE,
+	BTC_GET_U1_AP_NUM,
+
+	/* for 1Ant */
+	BTC_GET_U1_LPS_MODE,
+	BTC_GET_BL_BT_SCO_BUSY,
+
+	/* for test mode */
+	BTC_GET_DRIVER_TEST_CFG,
+#if 0
+	BTC_GET_U1_LPS,
+	BTC_GET_U1_RPWM,
+#endif
+	BTC_GET_MAX
+};
+
+
+enum btc_set_type{
+	/* type bool */
+	BTC_SET_BL_BT_DISABLE,
+	BTC_SET_BL_BT_TRAFFIC_BUSY,
+	BTC_SET_BL_BT_LIMITED_DIG,
+	BTC_SET_BL_FORCE_TO_ROAM,
+	BTC_SET_BL_TO_REJ_AP_AGG_PKT,
+	BTC_SET_BL_BT_CTRL_AGG_SIZE,
+	BTC_SET_BL_INC_SCAN_DEV_NUM,
+
+	/* type u1Byte */
+	BTC_SET_U1_RSSI_ADJ_VAL_FOR_AGC_TABLE_ON,
+	BTC_SET_U1_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE,
+	BTC_SET_UI_SCAN_SIG_COMPENSATION,
+	BTC_SET_U1_AGG_BUF_SIZE,
+
+	/* type trigger some action */
+	BTC_SET_ACT_GET_BT_RSSI,
+	BTC_SET_ACT_AGGREGATE_CTRL,
+
+	/********* for 1Ant **********/
+	/* type bool */
+	BTC_SET_BL_BT_SCO_BUSY,
+	/* type u1Byte */
+	BTC_SET_U1_1ANT_LPS,
+	BTC_SET_U1_1ANT_RPWM,
+	/* type trigger some action */
+	BTC_SET_ACT_LEAVE_LPS,
+	BTC_SET_ACT_ENTER_LPS,
+	BTC_SET_ACT_NORMAL_LPS,
+	BTC_SET_ACT_INC_FORCE_EXEC_PWR_CMD_CNT,
+	BTC_SET_ACT_DISABLE_LOW_POWER,
+	BTC_SET_ACT_UPDATE_ra_mask,
+	BTC_SET_ACT_SEND_MIMO_PS,
+	/* BT Coex related */
+	BTC_SET_ACT_CTRL_BT_INFO,
+	BTC_SET_ACT_CTRL_BT_COEX,
+	/***************************/
+	BTC_SET_MAX
+};
+
+enum btc_dbg_disp_type{
+	BTC_DBG_DISP_COEX_STATISTICS = 0x0,
+	BTC_DBG_DISP_BT_LINK_INFO = 0x1,
+	BTC_DBG_DISP_BT_FW_VER = 0x2,
+	BTC_DBG_DISP_FW_PWR_MODE_CMD = 0x3,
+	BTC_DBG_DISP_MAX
+};
+
+enum btc_notify_type_ips{
+	BTC_IPS_LEAVE = 0x0,
+	BTC_IPS_ENTER = 0x1,
+	BTC_IPS_MAX
+};
+
+enum btc_notify_type_lps{
+	BTC_LPS_DISABLE = 0x0,
+	BTC_LPS_ENABLE = 0x1,
+	BTC_LPS_MAX
+};
+
+enum btc_notify_type_scan{
+	BTC_SCAN_FINISH = 0x0,
+	BTC_SCAN_START = 0x1,
+	BTC_SCAN_MAX
+};
+
+enum btc_notify_type_associate{
+	BTC_ASSOCIATE_FINISH = 0x0,
+	BTC_ASSOCIATE_START = 0x1,
+	BTC_ASSOCIATE_MAX
+};
+
+enum btc_notify_type_media_status{
+	BTC_MEDIA_DISCONNECT = 0x0,
+	BTC_MEDIA_CONNECT = 0x1,
+	BTC_MEDIA_MAX
+};
+
+enum btc_notify_type_special_packet{
+	BTC_PACKET_UNKNOWN = 0x0,
+	BTC_PACKET_DHCP = 0x1,
+	BTC_PACKET_ARP = 0x2,
+	BTC_PACKET_EAPOL = 0x3,
+	BTC_PACKET_MAX
+};
+
+enum btc_notify_type_stack_operation{
+	BTC_STACK_OP_NONE = 0x0,
+	BTC_STACK_OP_INQ_PAGE_PAIR_START = 0x1,
+	BTC_STACK_OP_INQ_PAGE_PAIR_FINISH = 0x2,
+	BTC_STACK_OP_MAX
+};
+
+
+typedef u8 (*bfp_btc_r1)(void *btc_context, u32 reg_addr);
+
+typedef u16 (*bfp_btc_r2)(void *btc_context, u32 reg_addr);
+
+typedef u32 (*bfp_btc_r4)(void *btc_context, u32 reg_addr);
+
+typedef void (*bfp_btc_w1)(void *btc_context, u32 reg_addr, u8 data);
+
+typedef void (*bfp_btc_w1_bit_mak)(void *btc_context, u32 reg_addr,
+				   u8 bit_mask, u8 data1b);
+
+typedef void (*bfp_btc_w2)(void *btc_context, u32 reg_addr, u16 data);
+
+typedef void (*bfp_btc_w4)(void *btc_context, u32 reg_addr, u32 data);
+
+typedef void (*bfp_btc_wr_1byte_bit_mask)(void *btc_context, u32 reg_addr,
+					  u8 bit_mask, u8 data);
+
+typedef void (*bfp_btc_set_bb_reg)(void *btc_context, u32 reg_addr,
+				   u32 bit_mask, u32 data);
+
+typedef u32 (*bfp_btc_get_bb_reg)(void *btc_context, u32 reg_addr,
+				  u32 bit_mask);
+
+typedef void (*bfp_btc_set_rf_reg)(void *btc_context, u8 rf_path, u32 reg_addr,
+				   u32 bit_mask, u32 data);
+
+typedef u32 (*bfp_btc_get_rf_reg)(void *btc_context, u8 rf_path,
+				  u32 reg_addr, u32 bit_mask);
+
+typedef void (*bfp_btc_fill_h2c)(void *btc_context, u8 element_id,
+				 u32 cmd_len, u8 *cmd_buffer);
+
+typedef	bool (*bfp_btc_get)(void *btcoexist, u8 get_type, void *out_buf);
+
+typedef	bool (*bfp_btc_set)(void *btcoexist, u8 set_type, void *in_buf);
+
+typedef void (*bfp_btc_disp_dbg_msg)(void *btcoexist, u8 disp_type);
+
+struct btc_bt_info {
+	bool bt_disabled;
+	u8 rssi_adjust_for_agc_table_on;
+	u8 rssi_adjust_for_1ant_coex_type;
+	bool bt_busy;
+	u8 agg_buf_size;
+	bool limited_dig;
+	bool reject_agg_pkt;
+	bool b_bt_ctrl_buf_size;
+	bool increase_scan_dev_num;
+	u16 bt_hci_ver;
+	u16 bt_real_fw_ver;
+	u8 bt_fw_ver;
+
+	/* the following is for 1Ant solution */
+	bool bt_ctrl_lps;
+	bool bt_pwr_save_mode;
+	bool bt_lps_on;
+	bool force_to_roam;
+	u8 force_exec_pwr_cmd_cnt;
+	u8 lps_1ant;
+	u8 rpwm_1ant;
+	u32 ra_mask;
+};
+
+struct btc_stack_info {
+	bool profile_notified;
+	u16 hci_version;	/* stack hci version */
+	u8 num_of_link;
+	bool bt_link_exist;
+	bool sco_exist;
+	bool acl_exist;
+	bool a2dp_exist;
+	bool hid_exist;
+	u8 num_of_hid;
+	bool pan_exist;
+	bool unknown_acl_exist;
+	char min_bt_rssi;
+};
+
+struct btc_statistics {
+	u32 cnt_bind;
+	u32 cnt_init_hw_config;
+	u32 cnt_init_coex_dm;
+	u32 cnt_ips_notify;
+	u32 cnt_lps_notify;
+	u32 cnt_scan_notify;
+	u32 cnt_connect_notify;
+	u32 cnt_media_status_notify;
+	u32 cnt_special_packet_notify;
+	u32 cnt_bt_info_notify;
+	u32 cnt_periodical;
+	u32 cnt_stack_operation_notify;
+	u32 cnt_dbg_ctrl;
+};
+
+struct btc_bt_link_info {
+	bool bt_link_exist;
+	bool sco_exist;
+	bool sco_only;
+	bool a2dp_exist;
+	bool a2dp_only;
+	bool hid_exist;
+	bool hid_only;
+	bool pan_exist;
+	bool pan_only;
+};
+
+enum btc_antenna_pos {
+	BTC_ANTENNA_AT_MAIN_PORT = 0x1,
+	BTC_ANTENNA_AT_AUX_PORT = 0x2,
+};
+
+struct btc_coexist {
+	/* make sure only one adapter can bind the data context  */
+	bool binded;
+	/* default adapter */
+	void *adapter;
+	struct btc_board_info board_info;
+	/* some bt info referenced by non-bt module */
+	struct btc_bt_info bt_info;
+	struct btc_stack_info stack_info;
+	enum btc_chip_interface	chip_interface;
+	struct btc_bt_link_info bt_link_info;
+
+	bool initilized;
+	bool stop_coex_dm;
+	bool manual_control;
+	u8 *cli_buf;
+	struct btc_statistics statistics;
+	u8 pwr_mode_val[10];
+
+	/* function pointers
+	 * io related */
+	bfp_btc_r1 btc_read_1byte;
+	bfp_btc_w1 btc_write_1byte;
+	bfp_btc_w1_bit_mak btc_write_1byte_bitmask;
+	bfp_btc_r2 btc_read_2byte;
+	bfp_btc_w2 btc_write_2byte;
+	bfp_btc_r4 btc_read_4byte;
+	bfp_btc_w4 btc_write_4byte;
+
+	bfp_btc_set_bb_reg btc_set_bb_reg;
+	bfp_btc_get_bb_reg btc_get_bb_reg;
+
+
+	bfp_btc_set_rf_reg btc_set_rf_reg;
+	bfp_btc_get_rf_reg btc_get_rf_reg;
+
+
+	bfp_btc_fill_h2c btc_fill_h2c;
+
+	bfp_btc_disp_dbg_msg btc_disp_dbg_msg;
+
+	bfp_btc_get btc_get;
+	bfp_btc_set btc_set;
+};
+
+bool halbtc_is_wifi_uplink(struct rtl_priv *adapter);
+
+
+extern struct btc_coexist gl_bt_coexist;
+
+bool exhalbtc_initlize_variables(struct rtl_priv* adapter);
+void exhalbtc_init_hw_config(struct btc_coexist *btcoexist);
+void exhalbtc_init_coex_dm(struct btc_coexist *btcoexist);
+void exhalbtc_ips_notify(struct btc_coexist *btcoexist, u8 type);
+void exhalbtc_lps_notify(struct btc_coexist *btcoexist, u8 type);
+void exhalbtc_scan_notify(struct btc_coexist *btcoexist, u8 type);
+void exhalbtc_connect_notify(struct btc_coexist *btcoexist, u8 action);
+void exhalbtc_mediastatus_notify(struct btc_coexist *btcoexist,
+				 enum rt_media_status media_status);
+void exhalbtc_special_packet_notify(struct btc_coexist *btcoexist, u8 pkt_type);
+void exhalbtc_bt_info_notify(struct btc_coexist *btcoexist, u8 *tmp_buf,
+			     u8 length);
+void exhalbtc_stack_operation_notify(struct btc_coexist *btcoexist, u8 type);
+void exhalbtc_halt_notify(struct btc_coexist *btcoexist);
+void exhalbtc_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state);
+void exhalbtc_periodical(struct btc_coexist *btcoexist);
+void exhalbtc_dbg_control(struct btc_coexist *btcoexist, u8 code, u8 len,
+			  u8 *data);
+void exhalbtc_stack_update_profile_info(void);
+void exhalbtc_set_hci_version(u16 hci_version);
+void exhalbtc_set_bt_patch_version(u16 bt_hci_version, u16 bt_patch_version);
+void exhalbtc_update_min_bt_rssi(char bt_rssi);
+void exhalbtc_set_bt_exist(bool bt_exist);
+void exhalbtc_set_chip_type(u8 chip_type);
+void exhalbtc_set_ant_num(u8 type, u8 ant_num);
+void exhalbtc_display_bt_coex_info(struct btc_coexist *btcoexist);
+void exhalbtc_signal_compensation(struct btc_coexist *btcoexist,
+				  u8 *rssi_wifi, u8 *rssi_bt);
+void exhalbtc_lps_leave(struct btc_coexist *btcoexist);
+void exhalbtc_low_wifi_traffic_notify(struct btc_coexist *btcoexist);
+#endif
diff --git a/drivers/staging/rtl8821ae/btcoexist/rtl_btc.c b/drivers/staging/rtl8821ae/btcoexist/rtl_btc.c
new file mode 100644
index 000000000000..6653f147757c
--- /dev/null
+++ b/drivers/staging/rtl8821ae/btcoexist/rtl_btc.c
@@ -0,0 +1,236 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+#include <linux/vmalloc.h>
+#include <linux/module.h>
+
+#include "rtl_btc.h"
+#include "halbt_precomp.h"
+
+struct rtl_btc_ops rtl_btc_operation ={
+	.btc_init_variables = rtl_btc_init_variables,
+	.btc_init_hal_vars = rtl_btc_init_hal_vars,
+	.btc_init_hw_config = rtl_btc_init_hw_config,
+	.btc_ips_notify = rtl_btc_ips_notify,
+	.btc_scan_notify = rtl_btc_scan_notify,
+	.btc_connect_notify = rtl_btc_connect_notify,
+	.btc_mediastatus_notify = rtl_btc_mediastatus_notify,
+	.btc_periodical = rtl_btc_periodical,
+	.btc_halt_notify = rtl_btc_halt_notify,
+	.btc_btinfo_notify = rtl_btc_btinfo_notify,
+	.btc_is_limited_dig = rtl_btc_is_limited_dig,
+	.btc_is_disable_edca_turbo = rtl_btc_is_disable_edca_turbo,
+	.btc_is_bt_disabled = rtl_btc_is_bt_disabled,
+};
+
+
+void rtl_btc_init_variables(struct rtl_priv *rtlpriv)
+{
+
+	exhalbtc_initlize_variables(rtlpriv);
+}
+
+void rtl_btc_init_hal_vars(struct rtl_priv *rtlpriv)
+{
+	u8 ant_num;
+	u8 bt_exist;
+	u8 bt_type;
+	ant_num = rtl_get_hwpg_ant_num(rtlpriv);
+	RT_TRACE(COMP_INIT, DBG_DMESG, ("%s, antNum is %d\n", __func__, ant_num));
+
+	bt_exist = rtl_get_hwpg_bt_exist(rtlpriv);
+	RT_TRACE(COMP_INIT, DBG_DMESG, ("%s, bt_exist is %d\n", __func__, bt_exist));
+	exhalbtc_set_bt_exist(bt_exist);
+
+	bt_type = rtl_get_hwpg_bt_type(rtlpriv);
+	RT_TRACE(COMP_INIT, DBG_DMESG, ("%s, bt_type is %d\n", __func__, bt_type));
+	exhalbtc_set_chip_type(bt_type);
+
+	exhalbtc_set_ant_num(BT_COEX_ANT_TYPE_PG, ant_num);
+
+}
+
+
+void rtl_btc_init_hw_config(struct rtl_priv *rtlpriv)
+{
+	exhalbtc_init_hw_config(&gl_bt_coexist);
+	exhalbtc_init_coex_dm(&gl_bt_coexist);
+}
+
+
+void rtl_btc_ips_notify(struct rtl_priv *rtlpriv, u8 type)
+{
+	exhalbtc_ips_notify(&gl_bt_coexist, type);
+}
+
+
+void rtl_btc_scan_notify(struct rtl_priv *rtlpriv, u8 scantype)
+{
+	exhalbtc_scan_notify(&gl_bt_coexist, scantype);
+}
+
+
+void rtl_btc_connect_notify(struct rtl_priv *rtlpriv, u8 action)
+{
+	exhalbtc_connect_notify(&gl_bt_coexist, action);
+}
+
+
+void rtl_btc_mediastatus_notify(struct rtl_priv *rtlpriv, enum rt_media_status mstatus)
+{
+	exhalbtc_mediastatus_notify(&gl_bt_coexist, mstatus);
+}
+
+void rtl_btc_periodical(struct rtl_priv *rtlpriv)
+{
+//	rtl_bt_dm_monitor();
+	exhalbtc_periodical(&gl_bt_coexist);
+}
+
+void rtl_btc_halt_notify(void)
+{
+	exhalbtc_halt_notify(&gl_bt_coexist);
+}
+
+void rtl_btc_btinfo_notify(struct rtl_priv *rtlpriv, u8 * tmp_buf, u8 length)
+{
+	exhalbtc_bt_info_notify(&gl_bt_coexist, tmp_buf, length);
+}
+
+bool rtl_btc_is_limited_dig(struct rtl_priv *rtlpriv)
+{
+	return gl_bt_coexist.bt_info.limited_dig;
+}
+
+bool rtl_btc_is_disable_edca_turbo(struct rtl_priv *rtlpriv)
+{
+	bool bt_change_edca = false;
+	u32 cur_edca_val;
+	u32 edca_bt_hs_uplink = 0x5ea42b, edca_bt_hs_downlink = 0x5ea42b;
+	u32 edca_hs;
+	u32 edca_addr = 0x504;
+
+	cur_edca_val = rtl_read_dword(rtlpriv, edca_addr);
+	if (halbtc_is_wifi_uplink(rtlpriv)){
+		if (cur_edca_val != edca_bt_hs_uplink){
+			edca_hs = edca_bt_hs_uplink;
+			bt_change_edca = true;
+		}
+	}else{
+		if (cur_edca_val != edca_bt_hs_downlink){
+			edca_hs = edca_bt_hs_downlink;
+			bt_change_edca = true;
+		}
+	}
+
+	if(bt_change_edca)
+		rtl_write_dword(rtlpriv, edca_addr, edca_hs);
+
+	return true;
+}
+
+bool rtl_btc_is_bt_disabled(struct rtl_priv *rtlpriv)
+{
+	if (gl_bt_coexist.bt_info.bt_disabled)
+		return true;
+	else
+		return false;
+}
+
+struct rtl_btc_ops *rtl_btc_get_ops_pointer(void)
+{
+	return &rtl_btc_operation;
+}
+//EXPORT_SYMBOL(rtl_btc_get_ops_pointer);
+
+u8 rtl_get_hwpg_ant_num(struct rtl_priv *rtlpriv)
+{
+	u8 num;
+
+	if (rtlpriv->btcoexist.btc_info.ant_num == ANT_X2)
+		num = 2;
+	else
+		num = 1;
+
+	return num;
+}
+
+#if 0
+enum rt_media_status mgnt_link_status_query(struct ieee80211_hw *hw)
+{
+    struct rtl_priv *rtlpriv = rtl_priv(hw);
+    struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+    enum rt_media_status    m_status = RT_MEDIA_DISCONNECT;
+
+    u8 bibss = (mac->opmode == NL80211_IFTYPE_ADHOC) ? 1 : 0;
+
+    if(bibss || rtlpriv->mac80211.link_state >= MAC80211_LINKED) {
+            m_status = RT_MEDIA_CONNECT;
+    }
+
+    return m_status;
+}
+#endif
+
+u8 rtl_get_hwpg_bt_exist(struct rtl_priv *rtlpriv)
+{
+	return rtlpriv->btcoexist.btc_info.btcoexist;
+}
+
+u8 rtl_get_hwpg_bt_type(struct rtl_priv *rtlpriv)
+{
+	return rtlpriv->btcoexist.btc_info.bt_type;
+}
+
+
+#if 0
+
+MODULE_AUTHOR("Page He	<page_he@realsil.com.cn>");
+MODULE_AUTHOR("Realtek WlanFAE	<wlanfae@realtek.com>");
+MODULE_AUTHOR("Larry Finger	<Larry.FInger@lwfinger.net>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Realtek 802.11n PCI wireless core");
+
+static int __init rtl_btcoexist_module_init(void)
+{
+
+	//printk("%s, rtlpriv->btc_ops.btc_init_variables addr is %p\n", __func__, rtlpriv->btc_ops.btc_init_variables);
+
+	return 0;
+}
+
+static void __exit rtl_btcoexist_module_exit(void)
+{
+	return;
+}
+
+module_init(rtl_btcoexist_module_init);
+module_exit(rtl_btcoexist_module_exit);
+
+#endif
+
diff --git a/drivers/staging/rtl8821ae/btcoexist/rtl_btc.h b/drivers/staging/rtl8821ae/btcoexist/rtl_btc.h
new file mode 100644
index 000000000000..452fbf1e6d1e
--- /dev/null
+++ b/drivers/staging/rtl8821ae/btcoexist/rtl_btc.h
@@ -0,0 +1,66 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL_BTC_H__
+#define __RTL_BTC_H__
+
+#include "halbt_precomp.h"
+
+
+
+void rtl_btc_init_variables(struct rtl_priv *rtlpriv);
+void rtl_btc_init_hal_vars(struct rtl_priv *rtlpriv);
+void rtl_btc_init_hw_config(struct rtl_priv *rtlpriv);
+void rtl_btc_ips_notify(struct rtl_priv *rtlpriv, u8 type);
+void rtl_btc_scan_notify(struct rtl_priv *rtlpriv, u8 scantype);
+void rtl_btc_connect_notify(struct rtl_priv *rtlpriv, u8 action);
+void rtl_btc_mediastatus_notify(struct rtl_priv *rtlpriv, enum rt_media_status mstatus);
+void rtl_btc_periodical(struct rtl_priv *rtlpriv);
+void rtl_btc_halt_notify(void);
+void rtl_btc_btinfo_notify(struct rtl_priv *rtlpriv, u8 * tmpBuf, u8 length);
+bool rtl_btc_is_limited_dig(struct rtl_priv *rtlpriv);
+bool rtl_btc_is_disable_edca_turbo(struct rtl_priv *rtlpriv);
+bool rtl_btc_is_bt_disabled(struct rtl_priv *rtlpriv);
+
+
+//extern struct rtl_btc_ops rtl_btc_operation;
+extern struct rtl_btc_ops *rtl_btc_get_ops_pointer(void);
+
+u8 rtl_get_hwpg_ant_num(struct rtl_priv *rtlpriv);
+u8 rtl_get_hwpg_bt_exist(struct rtl_priv *rtlpriv);
+u8 rtl_get_hwpg_bt_type(struct rtl_priv *rtlpriv);
+//enum rt_media_status mgnt_link_status_query(struct ieee80211_hw *hw);
+
+
+
+
+
+
+
+
+#endif
diff --git a/drivers/staging/rtl8821ae/cam.c b/drivers/staging/rtl8821ae/cam.c
new file mode 100644
index 000000000000..72743e78954b
--- /dev/null
+++ b/drivers/staging/rtl8821ae/cam.c
@@ -0,0 +1,354 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+#include "wifi.h"
+#include "cam.h"
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0))
+#include <linux/export.h>
+#endif
+
+void rtl_cam_reset_sec_info(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->sec.use_defaultkey = false;
+	rtlpriv->sec.pairwise_enc_algorithm = NO_ENCRYPTION;
+	rtlpriv->sec.group_enc_algorithm = NO_ENCRYPTION;
+	memset(rtlpriv->sec.key_buf, 0, KEY_BUF_SIZE * MAX_KEY_LEN);
+	memset(rtlpriv->sec.key_len, 0, KEY_BUF_SIZE);
+	rtlpriv->sec.pairwise_key = NULL;
+}
+
+static void rtl_cam_program_entry(struct ieee80211_hw *hw, u32 entry_no,
+			   u8 *mac_addr, u8 *key_cont_128, u16 us_config)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	u32 target_command;
+	u32 target_content = 0;
+	u8 entry_i;
+
+	RT_PRINT_DATA(rtlpriv, COMP_SEC, DBG_DMESG, "Key content :",
+			key_cont_128, 16);
+
+	for (entry_i = 0; entry_i < CAM_CONTENT_COUNT; entry_i++) {
+		target_command = entry_i + CAM_CONTENT_COUNT * entry_no;
+		target_command = target_command | BIT(31) | BIT(16);
+
+		if (entry_i == 0) {
+			target_content = (u32) (*(mac_addr + 0)) << 16 |
+			    (u32) (*(mac_addr + 1)) << 24 | (u32) us_config;
+
+			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[WCAMI],
+					target_content);
+			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM],
+					target_command);
+
+			RT_TRACE(COMP_SEC, DBG_LOUD,
+				 ("WRITE %x: %x \n",
+				  rtlpriv->cfg->maps[WCAMI], target_content));
+			RT_TRACE(COMP_SEC, DBG_LOUD,
+				 ("The Key ID is %d\n", entry_no));
+			RT_TRACE(COMP_SEC, DBG_LOUD,
+				 ("WRITE %x: %x \n",
+				  rtlpriv->cfg->maps[RWCAM], target_command));
+
+		} else if (entry_i == 1) {
+
+			target_content = (u32) (*(mac_addr + 5)) << 24 |
+			    (u32) (*(mac_addr + 4)) << 16 |
+			    (u32) (*(mac_addr + 3)) << 8 |
+			    (u32) (*(mac_addr + 2));
+
+			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[WCAMI],
+					target_content);
+			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM],
+					target_command);
+
+			RT_TRACE(COMP_SEC, DBG_LOUD,
+				 ("WRITE A4: %x \n", target_content));
+			RT_TRACE(COMP_SEC, DBG_LOUD,
+				 ("WRITE A0: %x \n", target_command));
+
+		} else {
+
+			target_content =
+			    (u32) (*(key_cont_128 + (entry_i * 4 - 8) + 3)) <<
+			    24 | (u32) (*(key_cont_128 + (entry_i * 4 - 8) + 2))
+			    << 16 |
+			    (u32) (*(key_cont_128 + (entry_i * 4 - 8) + 1)) << 8
+			    | (u32) (*(key_cont_128 + (entry_i * 4 - 8) + 0));
+
+			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[WCAMI],
+					target_content);
+			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM],
+					target_command);
+			udelay(100);
+
+			RT_TRACE(COMP_SEC, DBG_LOUD,
+				 ("WRITE A4: %x \n", target_content));
+			RT_TRACE(COMP_SEC, DBG_LOUD,
+				 ("WRITE A0: %x \n", target_command));
+		}
+	}
+
+	RT_TRACE(COMP_SEC, DBG_LOUD,
+		 ("after set key, usconfig:%x\n", us_config));
+}
+
+u8 rtl_cam_add_one_entry(struct ieee80211_hw *hw, u8 *mac_addr,
+			 u32 ul_key_id, u32 ul_entry_idx, u32 ul_enc_alg,
+			 u32 ul_default_key, u8 *key_content)
+{
+	u32 us_config;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	RT_TRACE(COMP_SEC, DBG_DMESG,
+		 ("EntryNo:%x, ulKeyId=%x, ulEncAlg=%x, "
+		  "ulUseDK=%x MacAddr %pM\n",
+		  ul_entry_idx, ul_key_id, ul_enc_alg,
+		  ul_default_key, mac_addr));
+
+	if (ul_key_id == TOTAL_CAM_ENTRY) {
+		RT_TRACE(COMP_ERR, DBG_WARNING,
+			 ("ulKeyId exceed!\n"));
+		return 0;
+	}
+
+	if (ul_default_key == 1) {
+		us_config = CFG_VALID | ((u16) (ul_enc_alg) << 2);
+	} else {
+		us_config = CFG_VALID | ((ul_enc_alg) << 2) | ul_key_id;
+	}
+
+	rtl_cam_program_entry(hw, ul_entry_idx, mac_addr,
+			      (u8 *) key_content, us_config);
+
+	RT_TRACE(COMP_SEC, DBG_DMESG, ("end \n"));
+
+	return 1;
+
+}
+//EXPORT_SYMBOL(rtl_cam_add_one_entry);
+
+int rtl_cam_delete_one_entry(struct ieee80211_hw *hw,
+			     u8 *mac_addr, u32 ul_key_id)
+{
+	u32 ul_command;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	RT_TRACE(COMP_SEC, DBG_DMESG, ("key_idx:%d\n", ul_key_id));
+
+	ul_command = ul_key_id * CAM_CONTENT_COUNT;
+	ul_command = ul_command | BIT(31) | BIT(16);
+
+	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[WCAMI], 0);
+	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM], ul_command);
+
+	RT_TRACE(COMP_SEC, DBG_DMESG,
+		 ("rtl_cam_delete_one_entry(): WRITE A4: %x \n", 0));
+	RT_TRACE(COMP_SEC, DBG_DMESG,
+		 ("rtl_cam_delete_one_entry(): WRITE A0: %x \n", ul_command));
+
+	return 0;
+
+}
+//EXPORT_SYMBOL(rtl_cam_delete_one_entry);
+
+void rtl_cam_reset_all_entry(struct ieee80211_hw *hw)
+{
+	u32 ul_command;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	ul_command = BIT(31) | BIT(30);
+	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM], ul_command);
+}
+//EXPORT_SYMBOL(rtl_cam_reset_all_entry);
+
+void rtl_cam_mark_invalid(struct ieee80211_hw *hw, u8 uc_index)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	u32 ul_command;
+	u32 ul_content;
+	u32 ul_enc_algo = rtlpriv->cfg->maps[SEC_CAM_AES];
+
+	switch (rtlpriv->sec.pairwise_enc_algorithm) {
+	case WEP40_ENCRYPTION:
+		ul_enc_algo = rtlpriv->cfg->maps[SEC_CAM_WEP40];
+		break;
+	case WEP104_ENCRYPTION:
+		ul_enc_algo = rtlpriv->cfg->maps[SEC_CAM_WEP104];
+		break;
+	case TKIP_ENCRYPTION:
+		ul_enc_algo = rtlpriv->cfg->maps[SEC_CAM_TKIP];
+		break;
+	case AESCCMP_ENCRYPTION:
+		ul_enc_algo = rtlpriv->cfg->maps[SEC_CAM_AES];
+		break;
+	default:
+		ul_enc_algo = rtlpriv->cfg->maps[SEC_CAM_AES];
+	}
+
+	ul_content = (uc_index & 3) | ((u16) (ul_enc_algo) << 2);
+
+	ul_content |= BIT(15);
+	ul_command = CAM_CONTENT_COUNT * uc_index;
+	ul_command = ul_command | BIT(31) | BIT(16);
+
+	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[WCAMI], ul_content);
+	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM], ul_command);
+
+	RT_TRACE(COMP_SEC, DBG_DMESG,
+		 ("rtl_cam_mark_invalid(): WRITE A4: %x \n", ul_content));
+	RT_TRACE(COMP_SEC, DBG_DMESG,
+		 ("rtl_cam_mark_invalid(): WRITE A0: %x \n", ul_command));
+}
+//EXPORT_SYMBOL(rtl_cam_mark_invalid);
+
+void rtl_cam_empty_entry(struct ieee80211_hw *hw, u8 uc_index)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	u32 ul_command;
+	u32 ul_content;
+	u32 ul_encalgo = rtlpriv->cfg->maps[SEC_CAM_AES];
+	u8 entry_i;
+
+	switch (rtlpriv->sec.pairwise_enc_algorithm) {
+	case WEP40_ENCRYPTION:
+		ul_encalgo = rtlpriv->cfg->maps[SEC_CAM_WEP40];
+		break;
+	case WEP104_ENCRYPTION:
+		ul_encalgo = rtlpriv->cfg->maps[SEC_CAM_WEP104];
+		break;
+	case TKIP_ENCRYPTION:
+		ul_encalgo = rtlpriv->cfg->maps[SEC_CAM_TKIP];
+		break;
+	case AESCCMP_ENCRYPTION:
+		ul_encalgo = rtlpriv->cfg->maps[SEC_CAM_AES];
+		break;
+	default:
+		ul_encalgo = rtlpriv->cfg->maps[SEC_CAM_AES];
+	}
+
+	for (entry_i = 0; entry_i < CAM_CONTENT_COUNT; entry_i++) {
+
+		if (entry_i == 0) {
+			ul_content =
+			    (uc_index & 0x03) | ((u16) (ul_encalgo) << 2);
+			ul_content |= BIT(15);
+
+		} else {
+			ul_content = 0;
+		}
+
+		ul_command = CAM_CONTENT_COUNT * uc_index + entry_i;
+		ul_command = ul_command | BIT(31) | BIT(16);
+
+		rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[WCAMI], ul_content);
+		rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM], ul_command);
+
+		RT_TRACE(COMP_SEC, DBG_LOUD,
+			 ("rtl_cam_empty_entry(): WRITE A4: %x \n",
+			  ul_content));
+		RT_TRACE(COMP_SEC, DBG_LOUD,
+			 ("rtl_cam_empty_entry(): WRITE A0: %x \n",
+			  ul_command));
+	}
+
+}
+//EXPORT_SYMBOL(rtl_cam_empty_entry);
+
+u8 rtl_cam_get_free_entry(struct ieee80211_hw *hw, u8 *sta_addr)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 bitmap = (rtlpriv->sec.hwsec_cam_bitmap) >> 4;
+	u8 entry_idx = 0;
+	u8 i, *addr;
+
+	if (NULL == sta_addr) {
+		RT_TRACE(COMP_SEC, DBG_EMERG,
+			("sta_addr is NULL.\n"));
+		return TOTAL_CAM_ENTRY;
+	}
+	/* Does STA already exist? */
+	for (i = 4; i < TOTAL_CAM_ENTRY; i++) {
+		addr = rtlpriv->sec.hwsec_cam_sta_addr[i];
+		if(memcmp(addr, sta_addr, ETH_ALEN) == 0)
+			return i;
+	}
+	/* Get a free CAM entry. */
+	for (entry_idx = 4; entry_idx < TOTAL_CAM_ENTRY; entry_idx++) {
+		if ((bitmap & BIT(0)) == 0) {
+			RT_TRACE(COMP_SEC, DBG_EMERG,
+				("-----hwsec_cam_bitmap: 0x%x entry_idx=%d\n",
+				 rtlpriv->sec.hwsec_cam_bitmap, entry_idx));
+			rtlpriv->sec.hwsec_cam_bitmap |= BIT(0) << entry_idx;
+			memcpy(rtlpriv->sec.hwsec_cam_sta_addr[entry_idx],
+			       sta_addr, ETH_ALEN);
+			return entry_idx;
+		}
+		bitmap = bitmap >>1;
+	}
+	return TOTAL_CAM_ENTRY;
+}
+//EXPORT_SYMBOL(rtl_cam_get_free_entry);
+
+void rtl_cam_del_entry(struct ieee80211_hw *hw, u8 *sta_addr)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 bitmap;
+	u8 i, *addr;
+
+	if (NULL == sta_addr) {
+		RT_TRACE(COMP_SEC, DBG_EMERG,
+			("sta_addr is NULL.\n"));
+	}
+
+	if ((sta_addr[0]|sta_addr[1]|sta_addr[2]|sta_addr[3]|\
+				sta_addr[4]|sta_addr[5]) == 0) {
+		RT_TRACE(COMP_SEC, DBG_EMERG,
+			("sta_addr is 00:00:00:00:00:00.\n"));
+		return;
+	}
+	/* Does STA already exist? */
+	for (i = 4; i < TOTAL_CAM_ENTRY; i++) {
+		addr = rtlpriv->sec.hwsec_cam_sta_addr[i];
+		bitmap = (rtlpriv->sec.hwsec_cam_bitmap) >> i;
+		if (((bitmap & BIT(0)) == BIT(0)) &&
+		    (memcmp(addr, sta_addr, ETH_ALEN) == 0)) {
+			/* Remove from HW Security CAM */
+			memset(rtlpriv->sec.hwsec_cam_sta_addr[i], 0, ETH_ALEN);
+			rtlpriv->sec.hwsec_cam_bitmap &= ~(BIT(0) << i);
+			printk("&&&&&&&&&del entry %d\n",i);
+		}
+	}
+	return;
+}
+//EXPORT_SYMBOL(rtl_cam_del_entry);
diff --git a/drivers/staging/rtl8821ae/cam.h b/drivers/staging/rtl8821ae/cam.h
new file mode 100644
index 000000000000..326fa6784ae5
--- /dev/null
+++ b/drivers/staging/rtl8821ae/cam.h
@@ -0,0 +1,56 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL_CAM_H_
+#define __RTL_CAM_H_
+
+#define CAM_CONTENT_COUNT 				8
+
+#define CFG_DEFAULT_KEY  				BIT(5)
+#define CFG_VALID        				BIT(15)
+
+#define PAIRWISE_KEYIDX					0
+#define CAM_PAIRWISE_KEY_POSITION			4
+
+#define	CAM_CONFIG_USEDK				1
+#define	CAM_CONFIG_NO_USEDK				0
+
+extern void rtl_cam_reset_all_entry(struct ieee80211_hw *hw);
+extern u8 rtl_cam_add_one_entry(struct ieee80211_hw *hw, u8 *mac_addr,
+				u32 ul_key_id, u32 ul_entry_idx, u32 ul_enc_alg,
+				u32 ul_default_key, u8 *key_content);
+int rtl_cam_delete_one_entry(struct ieee80211_hw *hw, u8 *mac_addr,
+			     u32 ul_key_id);
+void rtl_cam_mark_invalid(struct ieee80211_hw *hw, u8 uc_index);
+void rtl_cam_empty_entry(struct ieee80211_hw *hw, u8 uc_index);
+void rtl_cam_reset_sec_info(struct ieee80211_hw *hw);
+u8 rtl_cam_get_free_entry(struct ieee80211_hw *hw, u8 *sta_addr);
+void rtl_cam_del_entry(struct ieee80211_hw *hw, u8 *sta_addr);
+
+#endif
diff --git a/drivers/staging/rtl8821ae/compat.h b/drivers/staging/rtl8821ae/compat.h
new file mode 100644
index 000000000000..68269cc2d477
--- /dev/null
+++ b/drivers/staging/rtl8821ae/compat.h
@@ -0,0 +1,125 @@
+#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;
+	__le16 duration;
+	u8 da[6];
+	u8 sa[6];
+	u8 bssid[6];
+	__le16 seq_ctrl;
+	union {
+		struct {
+			u8 category;
+			union {
+				struct {
+					u8 action_code;
+					u8 dialog_token;
+					u8 status_code;
+					u8 variable[0];
+				} __attribute__ ((packed)) wme_action;
+				struct{
+					u8 action_code;
+					u8 dialog_token;
+					__le16 capab;
+					__le16 timeout;
+					__le16 start_seq_num;
+				} __attribute__((packed)) addba_req;
+				struct{
+					u8 action_code;
+					u8 dialog_token;
+					__le16 status;
+					__le16 capab;
+					__le16 timeout;
+				} __attribute__((packed)) addba_resp;
+				struct{
+					u8 action_code;
+					__le16 params;
+					__le16 reason_code;
+				} __attribute__((packed)) delba;
+				struct{
+					u8 action_code;
+					/* capab_info for open and confirm,
+					 * reason for close
+					 */
+					__le16 aux;
+					/* Followed in plink_confirm by status
+					 * code, AID and supported rates,
+					 * and directly by supported rates in
+					 * plink_open and plink_close
+					 */
+					u8 variable[0];
+				} __attribute__((packed)) plink_action;
+				struct{
+					u8 action_code;
+					u8 variable[0];
+				} __attribute__((packed)) mesh_action;
+				struct {
+					u8 action;
+					u8 smps_control;
+				} __attribute__ ((packed)) ht_smps;
+			} u;
+		} __attribute__ ((packed)) action;
+	} u;
+} __attribute__ ((packed));
+#endif
diff --git a/drivers/staging/rtl8821ae/core.c b/drivers/staging/rtl8821ae/core.c
new file mode 100644
index 000000000000..40de6089039e
--- /dev/null
+++ b/drivers/staging/rtl8821ae/core.c
@@ -0,0 +1,1464 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "wifi.h"
+#include "core.h"
+#include "cam.h"
+#include "base.h"
+#include "ps.h"
+
+#include "btcoexist/rtl_btc.h"
+
+/*mutex for start & stop is must here. */
+static int rtl_op_start(struct ieee80211_hw *hw)
+{
+	int err = 0;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	if (!is_hal_stop(rtlhal))
+		return 0;
+	if (!test_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status))
+		return 0;
+	mutex_lock(&rtlpriv->locks.conf_mutex);
+	err = rtlpriv->intf_ops->adapter_start(hw);
+	if (err)
+		goto out;
+	rtl_watch_dog_timer_callback((unsigned long)hw);
+
+out:
+	mutex_unlock(&rtlpriv->locks.conf_mutex);
+	return err;
+}
+
+static void rtl_op_stop(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+
+	if (is_hal_stop(rtlhal))
+		return;
+
+	/* here is must, because adhoc do stop and start,
+	 * but stop with RFOFF may cause something wrong,
+	 * like adhoc TP */
+	if (unlikely(ppsc->rfpwr_state == ERFOFF))
+		rtl_ips_nic_on(hw);
+
+	mutex_lock(&rtlpriv->locks.conf_mutex);
+
+	mac->link_state = MAC80211_NOLINK;
+	memset(mac->bssid, 0, 6);
+	mac->vendor = PEER_UNKNOWN;
+
+	/*reset sec info */
+	rtl_cam_reset_sec_info(hw);
+
+	rtl_deinit_deferred_work(hw);
+	rtlpriv->intf_ops->adapter_stop(hw);
+
+	mutex_unlock(&rtlpriv->locks.conf_mutex);
+}
+
+/*<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));
+	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;
+
+/*<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)
+{
+	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;
+	}
+
+/*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,
+				 ("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;
+/*<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"));
+
+		mac->link_state = MAC80211_LINKED;
+		rtlpriv->cfg->ops->set_bcn_reg(hw);
+		if (rtlpriv->rtlhal.current_bandtype == BAND_ON_2_4G)
+			mac->basic_rates = 0xfff;
+		else
+			mac->basic_rates = 0xff0;
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BASIC_RATE,
+					      (u8 *) (&mac->basic_rates));
+		break;
+	case NL80211_IFTYPE_MESH_POINT:
+		RT_TRACE(COMP_MAC80211, DBG_LOUD,
+			 ("NL80211_IFTYPE_MESH_POINT \n"));
+
+		mac->link_state = MAC80211_LINKED;
+		rtlpriv->cfg->ops->set_bcn_reg(hw);
+		if (rtlpriv->rtlhal.current_bandtype == BAND_ON_2_4G)
+			mac->basic_rates = 0xfff;
+		else
+			mac->basic_rates = 0xff0;
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BASIC_RATE,
+				(u8 *) (&mac->basic_rates));
+		break;
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("operation mode %d is not support!\n", vif->type));
+		err = -EOPNOTSUPP;
+		goto out;
+	}
+
+#ifdef VIF_TODO
+	if (!rtl_set_vif_info(hw, vif))
+		goto out;
+#endif
+
+	if (mac->p2p) {
+		RT_TRACE(COMP_MAC80211, DBG_LOUD,
+			 ("p2p role %x \n",vif->type));
+		mac->basic_rates = 0xff0;/*disable cck rate for p2p*/
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BASIC_RATE,
+				(u8 *) (&mac->basic_rates));
+	}
+	mac->vif = vif;
+	mac->opmode = vif->type;
+	rtlpriv->cfg->ops->set_network_type(hw, vif->type);
+	memcpy(mac->mac_addr, vif->addr, ETH_ALEN);
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ETHER_ADDR, mac->mac_addr);
+
+out:
+	mutex_unlock(&rtlpriv->locks.conf_mutex);
+	return err;
+}
+
+static void rtl_op_remove_interface(struct ieee80211_hw *hw,
+		struct ieee80211_vif *vif)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+
+	mutex_lock(&rtlpriv->locks.conf_mutex);
+
+	/* Free beacon resources */
+	if ((vif->type == NL80211_IFTYPE_AP) ||
+	    (vif->type == NL80211_IFTYPE_ADHOC) ||
+	    (vif->type == NL80211_IFTYPE_MESH_POINT)) {
+		if (mac->beacon_enabled == 1) {
+			mac->beacon_enabled = 0;
+			rtlpriv->cfg->ops->update_interrupt_mask(hw, 0,
+					rtlpriv->cfg->maps[RTL_IBSS_INT_MASKS]);
+		}
+	}
+
+	/*
+	 *Note: We assume NL80211_IFTYPE_UNSPECIFIED as
+	 *NO LINK for our hardware.
+	 */
+	mac->p2p = 0;
+	mac->vif = NULL;
+	mac->link_state = MAC80211_NOLINK;
+	memset(mac->bssid, 0, 6);
+	mac->vendor = PEER_UNKNOWN;
+	mac->opmode = NL80211_IFTYPE_UNSPECIFIED;
+	rtlpriv->cfg->ops->set_network_type(hw, mac->opmode);
+
+	mutex_unlock(&rtlpriv->locks.conf_mutex);
+}
+/*<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)
+{
+	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;
+}
+/*<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);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct ieee80211_conf *conf = &hw->conf;
+
+	if (mac->skip_scan)
+		return 1;
+
+
+	mutex_lock(&rtlpriv->locks.conf_mutex);
+	if (changed & IEEE80211_CONF_CHANGE_LISTEN_INTERVAL) {	/* BIT(2) */
+		RT_TRACE(COMP_MAC80211, DBG_LOUD,
+			 ("IEEE80211_CONF_CHANGE_LISTEN_INTERVAL\n"));
+	}
+
+	/*For IPS */
+	if (changed & IEEE80211_CONF_CHANGE_IDLE) {
+		if (hw->conf.flags & IEEE80211_CONF_IDLE)
+			rtl_ips_nic_off(hw);
+		else
+			rtl_ips_nic_on(hw);
+	} else {
+		/*
+		 *although rfoff may not cause by ips, but we will
+		 *check the reason in set_rf_power_state function
+		 */
+		if (unlikely(ppsc->rfpwr_state == ERFOFF))
+			rtl_ips_nic_on(hw);
+	}
+
+	/*For LPS */
+	if (changed & IEEE80211_CONF_CHANGE_PS) {
+		cancel_delayed_work(&rtlpriv->works.ps_work);
+		cancel_delayed_work(&rtlpriv->works.ps_rfon_wq);
+		if (conf->flags & IEEE80211_CONF_PS) {
+			rtlpriv->psc.sw_ps_enabled = true;
+			/* sleep here is must, or we may recv the beacon and
+			 * cause mac80211 into wrong ps state, this will cause
+			 * power save nullfunc send fail, and further cause
+			 * pkt loss, So sleep must quickly but not 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 {
+			rtl_swlps_rf_awake(hw);
+			rtlpriv->psc.sw_ps_enabled = false;
+		}
+	}
+
+	if (changed & IEEE80211_CONF_CHANGE_RETRY_LIMITS) {
+		RT_TRACE(COMP_MAC80211, DBG_LOUD,
+			 ("IEEE80211_CONF_CHANGE_RETRY_LIMITS %x\n",
+			  hw->conf.long_frame_max_tx_count));
+		mac->retry_long = hw->conf.long_frame_max_tx_count;
+		mac->retry_short = hw->conf.long_frame_max_tx_count;
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_RETRY_LIMIT,
+				(u8 *) (&hw->conf.long_frame_max_tx_count));
+	}
+
+	if (changed & IEEE80211_CONF_CHANGE_CHANNEL) {
+#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)
+			mac->n_channels++;
+
+		if (rtlpriv->dm.supp_phymode_switch &&
+			mac->link_state < MAC80211_LINKED &&
+			!mac->act_scanning) {
+			if (rtlpriv->cfg->ops->check_switch_to_dmdp)
+				rtlpriv->cfg->ops->check_switch_to_dmdp(hw);
+		}
+
+		/*
+		 *because we should back channel to
+		 *current_network.chan in in scanning,
+		 *So if set_chan == current_network.chan
+		 *we should set it.
+		 *because mac80211 tell us wrong bw40
+		 *info for cisco1253 bw20, so we modify
+		 *it here based on UPPER & LOWER
+		 */
+		switch (channel_type) {
+			case NL80211_CHAN_HT20:
+			case NL80211_CHAN_NO_HT:
+				/* SC */
+				mac->cur_40_prime_sc =
+					PRIME_CHNL_OFFSET_DONT_CARE;
+				rtlphy->current_chan_bw = HT_CHANNEL_WIDTH_20;
+				mac->bw_40 = false;
+				break;
+			case NL80211_CHAN_HT40MINUS:
+				/* SC */
+				mac->cur_40_prime_sc = PRIME_CHNL_OFFSET_UPPER;
+				rtlphy->current_chan_bw =
+					HT_CHANNEL_WIDTH_20_40;
+				mac->bw_40 = true;
+
+				/*wide channel */
+				wide_chan -= 2;
+
+				break;
+			case NL80211_CHAN_HT40PLUS:
+				/* SC */
+				mac->cur_40_prime_sc = PRIME_CHNL_OFFSET_LOWER;
+				rtlphy->current_chan_bw =
+					HT_CHANNEL_WIDTH_20_40;
+				mac->bw_40 = true;
+
+				/*wide channel */
+				wide_chan += 2;
+
+				break;
+			default:
+				mac->bw_40 = false;
+				RT_TRACE(COMP_ERR, DBG_EMERG,
+						("switch case not processed \n"));
+				break;
+		}
+
+		if (wide_chan <= 0)
+			wide_chan = 1;
+
+		/* in scanning, when before we offchannel we may send a ps=1
+		 * null to AP, and then we may send a ps = 0 null to AP quickly,
+		 * but first null have cause AP's put lots of packet to hw tx
+		 * buffer, these packet must be tx before off channel so we must
+		 * delay more time to let AP flush these packets before
+		 * offchannel, or dis-association or delete BA will happen by AP
+		 */
+		if (rtlpriv->mac80211.offchan_deley) {
+			rtlpriv->mac80211.offchan_deley = false;
+			mdelay(50);
+		}
+
+		rtlphy->current_channel = wide_chan;
+
+		rtlpriv->cfg->ops->switch_channel(hw);
+		rtlpriv->cfg->ops->set_channel_access(hw);
+		rtlpriv->cfg->ops->set_bw_mode(hw,
+			channel_type);
+	}
+
+	mutex_unlock(&rtlpriv->locks.conf_mutex);
+
+	return 0;
+}
+
+static void rtl_op_configure_filter(struct ieee80211_hw *hw,
+				    unsigned int changed_flags,
+				    unsigned int *new_flags, u64 multicast)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+
+	*new_flags &= RTL_SUPPORTED_FILTERS;
+	if (0 == changed_flags)
+		return;
+
+	/*TODO: we disable broadcase now, so enable here */
+	if (changed_flags & FIF_ALLMULTI) {
+		if (*new_flags & FIF_ALLMULTI) {
+			mac->rx_conf |= rtlpriv->cfg->maps[MAC_RCR_AM] |
+			    rtlpriv->cfg->maps[MAC_RCR_AB];
+			RT_TRACE(COMP_MAC80211, DBG_LOUD,
+				 ("Enable receive multicast frame.\n"));
+		} else {
+			mac->rx_conf &= ~(rtlpriv->cfg->maps[MAC_RCR_AM] |
+					  rtlpriv->cfg->maps[MAC_RCR_AB]);
+			RT_TRACE(COMP_MAC80211, DBG_LOUD,
+				 ("Disable receive multicast frame.\n"));
+		}
+	}
+
+	if (changed_flags & FIF_FCSFAIL) {
+		if (*new_flags & FIF_FCSFAIL) {
+			mac->rx_conf |= rtlpriv->cfg->maps[MAC_RCR_ACRC32];
+			RT_TRACE(COMP_MAC80211, DBG_LOUD,
+				 ("Enable receive FCS error frame.\n"));
+		} else {
+			mac->rx_conf &= ~rtlpriv->cfg->maps[MAC_RCR_ACRC32];
+			RT_TRACE(COMP_MAC80211, DBG_LOUD,
+				 ("Disable receive FCS error frame.\n"));
+		}
+	}
+
+	/* if ssid not set to hw don't check bssid
+	 * here just used for linked scanning, & linked
+	 * and nolink check bssid is set in set network_type */
+	if ((changed_flags & FIF_BCN_PRBRESP_PROMISC) &&
+		(mac->link_state >= MAC80211_LINKED)) {
+		if (mac->opmode != NL80211_IFTYPE_AP &&
+			mac->opmode != NL80211_IFTYPE_MESH_POINT) {
+			if (*new_flags & FIF_BCN_PRBRESP_PROMISC) {
+				rtlpriv->cfg->ops->set_chk_bssid(hw, false);
+			} else {
+				rtlpriv->cfg->ops->set_chk_bssid(hw, true);
+			}
+		}
+	}
+
+	if (changed_flags & FIF_CONTROL) {
+		if (*new_flags & FIF_CONTROL) {
+			mac->rx_conf |= rtlpriv->cfg->maps[MAC_RCR_ACF];
+
+			RT_TRACE(COMP_MAC80211, DBG_LOUD,
+				 ("Enable receive control frame.\n"));
+		} else {
+			mac->rx_conf &= ~rtlpriv->cfg->maps[MAC_RCR_ACF];
+			RT_TRACE(COMP_MAC80211, DBG_LOUD,
+				 ("Disable receive control frame.\n"));
+		}
+	}
+
+	if (changed_flags & FIF_OTHER_BSS) {
+		if (*new_flags & FIF_OTHER_BSS) {
+			mac->rx_conf |= rtlpriv->cfg->maps[MAC_RCR_AAP];
+			RT_TRACE(COMP_MAC80211, DBG_LOUD,
+				 ("Enable receive other BSS's frame.\n"));
+		} else {
+			mac->rx_conf &= ~rtlpriv->cfg->maps[MAC_RCR_AAP];
+			RT_TRACE(COMP_MAC80211, DBG_LOUD,
+				 ("Disable receive other BSS's frame.\n"));
+		}
+	}
+}
+static int rtl_op_sta_add(struct ieee80211_hw *hw,
+			 struct ieee80211_vif *vif,
+			 struct ieee80211_sta *sta)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal= rtl_hal(rtl_priv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_sta_info *sta_entry;
+
+	if (sta) {
+		sta_entry = (struct rtl_sta_info *) sta->drv_priv;
+		spin_lock_bh(&rtlpriv->locks.entry_list_lock);
+		list_add_tail(&sta_entry->list, &rtlpriv->entry_list);
+		spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
+		if (rtlhal->current_bandtype == BAND_ON_2_4G) {
+			sta_entry->wireless_mode = WIRELESS_MODE_G;
+			if (sta->supp_rates[0] <= 0xf)
+				sta_entry->wireless_mode = WIRELESS_MODE_B;
+			if (sta->ht_cap.ht_supported == true)
+				sta_entry->wireless_mode = WIRELESS_MODE_N_24G;
+
+			if (vif->type == NL80211_IFTYPE_ADHOC)
+				sta_entry->wireless_mode = WIRELESS_MODE_G;
+		} else if (rtlhal->current_bandtype == BAND_ON_5G) {
+			sta_entry->wireless_mode = WIRELESS_MODE_A;
+			if (sta->ht_cap.ht_supported == true)
+				sta_entry->wireless_mode = WIRELESS_MODE_N_24G;
+
+			if (vif->type == NL80211_IFTYPE_ADHOC)
+				sta_entry->wireless_mode = WIRELESS_MODE_A;
+		}
+		/*disable cck rate for p2p*/
+		if (mac->p2p)
+			sta->supp_rates[0] &= 0xfffffff0;
+
+		memcpy(sta_entry->mac_addr, sta->addr, ETH_ALEN);
+		RT_TRACE(COMP_MAC80211, DBG_DMESG,
+			("Add sta addr is %pM\n",sta->addr));
+		rtlpriv->cfg->ops->update_rate_tbl(hw, sta, 0);
+	}
+
+	return 0;
+}
+
+static int rtl_op_sta_remove(struct ieee80211_hw *hw,
+				struct ieee80211_vif *vif,
+				struct ieee80211_sta *sta)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_sta_info *sta_entry;
+	if (sta) {
+		RT_TRACE(COMP_MAC80211, DBG_DMESG,
+			("Remove sta addr is %pM\n",sta->addr));
+		sta_entry = (struct rtl_sta_info *) sta->drv_priv;
+		sta_entry->wireless_mode = 0;
+		sta_entry->ratr_index = 0;
+		spin_lock_bh(&rtlpriv->locks.entry_list_lock);
+		list_del(&sta_entry->list);
+		spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
+	}
+	return 0;
+}
+static int _rtl_get_hal_qnum(u16 queue)
+{
+	int qnum;
+
+	switch (queue) {
+	case 0:
+		qnum = AC3_VO;
+		break;
+	case 1:
+		qnum = AC2_VI;
+		break;
+	case 2:
+		qnum = AC0_BE;
+		break;
+	case 3:
+		qnum = AC1_BK;
+		break;
+	default:
+		qnum = AC0_BE;
+		break;
+	}
+	return qnum;
+}
+
+/*
+ *for mac80211 VO=0, VI=1, BE=2, BK=3
+ *for rtl819x  BE=0, BK=1, VI=2, VO=3
+ */
+#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));
+	int aci;
+
+	if (queue >= AC_MAX) {
+		RT_TRACE(COMP_ERR, DBG_WARNING,
+			 ("queue number %d is incorrect!\n", queue));
+		return -EINVAL;
+	}
+
+	aci = _rtl_get_hal_qnum(queue);
+	mac->ac[aci].aifs = param->aifs;
+	mac->ac[aci].cw_min = param->cw_min;
+	mac->ac[aci].cw_max = param->cw_max;
+	mac->ac[aci].tx_op = param->txop;
+	memcpy(&mac->edca_param[aci], param, sizeof(*param));
+	rtlpriv->cfg->ops->set_qos(hw, aci);
+	return 0;
+}
+
+static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
+				    struct ieee80211_vif *vif,
+				    struct ieee80211_bss_conf *bss_conf,
+				    u32 changed)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+
+	mutex_lock(&rtlpriv->locks.conf_mutex);
+	if ((vif->type == NL80211_IFTYPE_ADHOC) ||
+	    (vif->type == NL80211_IFTYPE_AP) ||
+	    (vif->type == NL80211_IFTYPE_MESH_POINT)) {
+		if ((changed & BSS_CHANGED_BEACON) ||
+		    (changed & BSS_CHANGED_BEACON_ENABLED &&
+		     bss_conf->enable_beacon)) {
+			if (mac->beacon_enabled == 0) {
+				RT_TRACE(COMP_MAC80211, DBG_DMESG,
+					 ("BSS_CHANGED_BEACON_ENABLED \n"));
+
+				/*start hw beacon interrupt. */
+				/*rtlpriv->cfg->ops->set_bcn_reg(hw); */
+				mac->beacon_enabled = 1;
+				rtlpriv->cfg->ops->update_interrupt_mask(hw,
+						rtlpriv->cfg->maps
+						[RTL_IBSS_INT_MASKS], 0);
+
+				if (rtlpriv->cfg->ops->linked_set_reg)
+					rtlpriv->cfg->ops->linked_set_reg(hw);
+			}
+		}
+		if ((changed & BSS_CHANGED_BEACON_ENABLED &&
+			!bss_conf->enable_beacon)){
+			if (mac->beacon_enabled == 1) {
+				RT_TRACE(COMP_MAC80211, DBG_DMESG,
+					 ("ADHOC DISABLE BEACON\n"));
+
+				mac->beacon_enabled = 0;
+				rtlpriv->cfg->ops->update_interrupt_mask(hw, 0,
+						rtlpriv->cfg->maps
+						[RTL_IBSS_INT_MASKS]);
+			}
+		}
+		if (changed & BSS_CHANGED_BEACON_INT) {
+			RT_TRACE(COMP_BEACON, DBG_TRACE,
+				 ("BSS_CHANGED_BEACON_INT\n"));
+			mac->beacon_interval = bss_conf->beacon_int;
+			rtlpriv->cfg->ops->set_bcn_intv(hw);
+		}
+	}
+
+	/*TODO: reference to enum ieee80211_bss_change */
+	if (changed & BSS_CHANGED_ASSOC) {
+		if (bss_conf->assoc) {
+			struct ieee80211_sta *sta = NULL;
+			/* we should reset all sec info & cam
+			 * before set cam after linked, we should not
+			 * reset in disassoc, that will cause tkip->wep
+			 * fail because some flag will be wrong */
+			/* reset sec info */
+			rtl_cam_reset_sec_info(hw);
+			/* reset cam to fix wep fail issue
+			 * when change from wpa to wep */
+			rtl_cam_reset_all_entry(hw);
+
+			mac->link_state = MAC80211_LINKED;
+			mac->cnt_after_linked = 0;
+			mac->assoc_id = bss_conf->aid;
+			memcpy(mac->bssid, bss_conf->bssid, 6);
+
+			if (rtlpriv->cfg->ops->linked_set_reg)
+				rtlpriv->cfg->ops->linked_set_reg(hw);
+
+			rcu_read_lock();
+			sta = ieee80211_find_sta(vif, (u8*)bss_conf->bssid);
+
+			if (vif->type == NL80211_IFTYPE_STATION && sta)
+				rtlpriv->cfg->ops->update_rate_tbl(hw, sta, 0);
+			RT_TRACE(COMP_EASY_CONCURRENT, DBG_LOUD,
+					("send PS STATIC frame \n"));
+			if (rtlpriv->dm.supp_phymode_switch) {
+				if (sta->ht_cap.ht_supported)
+					rtl_send_smps_action(hw, sta,
+							IEEE80211_SMPS_STATIC);
+			}
+			rcu_read_unlock();
+
+			RT_TRACE(COMP_MAC80211, DBG_DMESG,
+				 ("BSS_CHANGED_ASSOC\n"));
+		} else {
+			if (mac->link_state == MAC80211_LINKED)
+				rtl_lps_leave(hw);
+			if (ppsc->p2p_ps_info.p2p_ps_mode> P2P_PS_NONE)
+				rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);
+			mac->link_state = MAC80211_NOLINK;
+			memset(mac->bssid, 0, 6);
+			mac->vendor = PEER_UNKNOWN;
+
+			if (rtlpriv->dm.supp_phymode_switch) {
+				if (rtlpriv->cfg->ops->check_switch_to_dmdp)
+					rtlpriv->cfg->ops->check_switch_to_dmdp(hw);
+			}
+			RT_TRACE(COMP_MAC80211, DBG_DMESG,
+				 ("BSS_CHANGED_UN_ASSOC\n"));
+		}
+	}
+
+	if (changed & BSS_CHANGED_ERP_CTS_PROT) {
+		RT_TRACE(COMP_MAC80211, DBG_TRACE,
+			 ("BSS_CHANGED_ERP_CTS_PROT\n"));
+		mac->use_cts_protect = bss_conf->use_cts_prot;
+	}
+
+	if (changed & BSS_CHANGED_ERP_PREAMBLE) {
+		RT_TRACE(COMP_MAC80211, DBG_LOUD,
+			 ("BSS_CHANGED_ERP_PREAMBLE use short preamble:%x \n",
+			  bss_conf->use_short_preamble));
+
+		mac->short_preamble = bss_conf->use_short_preamble;
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ACK_PREAMBLE,
+					      (u8 *) (&mac->short_preamble));
+	}
+
+	if (changed & BSS_CHANGED_ERP_SLOT) {
+		RT_TRACE(COMP_MAC80211, DBG_TRACE,
+			 ("BSS_CHANGED_ERP_SLOT\n"));
+
+		if (bss_conf->use_short_slot)
+			mac->slot_time = RTL_SLOT_TIME_9;
+		else
+			mac->slot_time = RTL_SLOT_TIME_20;
+
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SLOT_TIME,
+					      (u8 *) (&mac->slot_time));
+	}
+
+	if (changed & BSS_CHANGED_HT) {
+		struct ieee80211_sta *sta = NULL;
+
+		RT_TRACE(COMP_MAC80211, DBG_TRACE,
+			 ("BSS_CHANGED_HT\n"));
+
+		rcu_read_lock();
+		sta = ieee80211_find_sta(vif, (u8*)bss_conf->bssid);
+		if (sta) {
+			if (sta->ht_cap.ampdu_density >
+			    mac->current_ampdu_density)
+				mac->current_ampdu_density =
+				    sta->ht_cap.ampdu_density;
+			if (sta->ht_cap.ampdu_factor <
+			    mac->current_ampdu_factor)
+				mac->current_ampdu_factor =
+				    sta->ht_cap.ampdu_factor;
+		}
+		rcu_read_unlock();
+
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SHORTGI_DENSITY,
+					      (u8 *) (&mac->max_mss_density));
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AMPDU_FACTOR,
+					      &mac->current_ampdu_factor);
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AMPDU_MIN_SPACE,
+					      &mac->current_ampdu_density);
+	}
+
+	if (changed & BSS_CHANGED_BSSID) {
+		u32 basic_rates;
+		struct ieee80211_sta *sta = NULL;
+
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BSSID,
+					      (u8 *) bss_conf->bssid);
+
+		RT_TRACE(COMP_MAC80211, DBG_DMESG,
+			 ("bssid: %pM\n", bss_conf->bssid));
+
+		mac->vendor = PEER_UNKNOWN;
+		memcpy(mac->bssid, bss_conf->bssid, 6);
+		rtlpriv->cfg->ops->set_network_type(hw, vif->type);
+
+		rcu_read_lock();
+		sta = ieee80211_find_sta(vif, (u8*)bss_conf->bssid);
+		if (!sta) {
+			rcu_read_unlock();
+			goto out;
+		}
+
+		if (rtlhal->current_bandtype == BAND_ON_5G) {
+			mac->mode = WIRELESS_MODE_A;
+		} else {
+			if (sta->supp_rates[0] <= 0xf)
+				mac->mode = WIRELESS_MODE_B;
+			else
+				mac->mode = WIRELESS_MODE_G;
+		}
+
+		if (sta->ht_cap.ht_supported) {
+			if (rtlhal->current_bandtype == BAND_ON_2_4G)
+				mac->mode = WIRELESS_MODE_N_24G;
+			else
+				mac->mode = WIRELESS_MODE_N_5G;
+		}
+
+		/* just station need it, because ibss & ap mode will
+		 * set in sta_add, and will be NULL here */
+		if (vif->type == NL80211_IFTYPE_STATION) {
+			struct rtl_sta_info *sta_entry;
+			sta_entry = (struct rtl_sta_info *) sta->drv_priv;
+			sta_entry->wireless_mode = mac->mode;
+		}
+
+		if (sta->ht_cap.ht_supported) {
+			mac->ht_enable = true;
+
+			/*
+			 * for cisco 1252 bw20 it's wrong
+			 * if (ht_cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40) {
+			 * 	mac->bw_40 = true;
+			 * }
+			 * */
+		}
+
+		if (changed & BSS_CHANGED_BASIC_RATES) {
+			/* for 5G must << RATE_6M_INDEX=4,
+			 * because 5G have no cck rate*/
+			if (rtlhal->current_bandtype == BAND_ON_5G)
+				basic_rates = sta->supp_rates[1] << 4;
+			else
+				basic_rates = sta->supp_rates[0];
+
+			mac->basic_rates = basic_rates;
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BASIC_RATE,
+					(u8 *) (&basic_rates));
+		}
+		rcu_read_unlock();
+	}
+
+	/*
+	 * For FW LPS and Keep Alive:
+	 * To tell firmware we have connected
+	 * to an AP. For 92SE/CE power save v2.
+	 */
+	if (changed & BSS_CHANGED_ASSOC) {
+		if (bss_conf->assoc) {
+			u8 keep_alive = 10;
+			u8 mstatus = RT_MEDIA_CONNECT;
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						      HW_VAR_KEEP_ALIVE,
+						      (u8 *) (&keep_alive));
+
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						      HW_VAR_H2C_FW_JOINBSSRPT,
+						      (u8 *) (&mstatus));
+			ppsc->report_linked = true;
+
+		} else {
+
+			u8 mstatus = RT_MEDIA_DISCONNECT;
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						      HW_VAR_H2C_FW_JOINBSSRPT,
+						      (u8 *) (&mstatus));
+			ppsc->report_linked = false;
+
+		}
+
+		if (rtlpriv->cfg->ops->get_btc_status()){
+			rtlpriv->btcoexist.btc_ops->btc_mediastatus_notify(
+						rtlpriv, ppsc->report_linked);
+		}
+	}
+
+out:
+	mutex_unlock(&rtlpriv->locks.conf_mutex);
+}
+
+#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;
+
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_CORRECT_TSF, (u8 *) (&tsf));
+	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));
+	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));
+}
+
+#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;
+
+	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
+/*<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);
+
+	switch (action) {
+	case IEEE80211_AMPDU_TX_START:
+		RT_TRACE(COMP_MAC80211, DBG_TRACE,
+			 ("IEEE80211_AMPDU_TX_START: TID:%d\n", tid));
+		return rtl_tx_agg_start(hw, vif, sta, tid, ssn);
+		break;
+#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);
+		break;
+	case IEEE80211_AMPDU_TX_OPERATIONAL:
+		RT_TRACE(COMP_MAC80211, DBG_TRACE,
+			 ("IEEE80211_AMPDU_TX_OPERATIONAL:TID:%d\n", tid));
+		rtl_tx_agg_oper(hw, sta, tid);
+		break;
+	case IEEE80211_AMPDU_RX_START:
+		RT_TRACE(COMP_MAC80211, DBG_TRACE,
+			 ("IEEE80211_AMPDU_RX_START:TID:%d\n", tid));
+		return rtl_rx_agg_start(hw, sta, tid);
+		break;
+	case IEEE80211_AMPDU_RX_STOP:
+		RT_TRACE(COMP_MAC80211, DBG_TRACE,
+			 ("IEEE80211_AMPDU_RX_STOP:TID:%d\n", tid));
+		return rtl_rx_agg_stop(hw, sta, tid);
+		break;
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("IEEE80211_AMPDU_ERR!!!!:\n"));
+		return -EOPNOTSUPP;
+	}
+	return 0;
+}
+
+static void rtl_op_sw_scan_start(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+
+	RT_TRACE(COMP_MAC80211, DBG_LOUD, ("\n"));
+	mac->act_scanning = true;
+	/*rtlpriv->btcops->btc_scan_notify(rtlpriv, 0); */
+	if (rtlpriv->link_info.b_higher_busytraffic) {
+		mac->skip_scan = true;
+		return;
+	}
+
+	if (rtlpriv->dm.supp_phymode_switch) {
+		if (rtlpriv->cfg->ops->check_switch_to_dmdp)
+			rtlpriv->cfg->ops->check_switch_to_dmdp(hw);
+	}
+
+	if (mac->link_state == MAC80211_LINKED) {
+		rtl_lps_leave(hw);
+		mac->link_state = MAC80211_LINKED_SCANNING;
+	} else {
+		rtl_ips_nic_on(hw);
+	}
+
+	/* Dul mac */
+	rtlpriv->rtlhal.b_load_imrandiqk_setting_for2g = false;
+
+	rtlpriv->cfg->ops->led_control(hw, LED_CTL_SITE_SURVEY);
+
+	rtlpriv->cfg->ops->scan_operation_backup(hw, SCAN_OPT_BACKUP_BAND0);
+
+}
+
+static void rtl_op_sw_scan_complete(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+
+	RT_TRACE(COMP_MAC80211, DBG_LOUD, ("\n"));
+	mac->act_scanning = false;
+	mac->skip_scan = false;
+	if (rtlpriv->link_info.b_higher_busytraffic) {
+		return;
+	}
+
+	/* p2p will use 1/6/11 to scan */
+	if (mac->n_channels == 3)
+		mac->p2p_in_use = true;
+	else
+		mac->p2p_in_use = false;
+	mac->n_channels = 0;
+	/* Dul mac */
+	rtlpriv->rtlhal.b_load_imrandiqk_setting_for2g = false;
+
+	if (mac->link_state == MAC80211_LINKED_SCANNING) {
+		mac->link_state = MAC80211_LINKED;
+		if (mac->opmode == NL80211_IFTYPE_STATION) {
+			/* fix fwlps issue */
+			rtlpriv->cfg->ops->set_network_type(hw, mac->opmode);
+		}
+	}
+
+	rtlpriv->cfg->ops->scan_operation_backup(hw, SCAN_OPT_RESTORE);
+	/* rtlpriv->btcops->btc_scan_notify(rtlpriv, 1); */
+}
+
+static int rtl_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
+			  struct ieee80211_vif *vif, struct ieee80211_sta *sta,
+			  struct ieee80211_key_conf *key)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 key_type = NO_ENCRYPTION;
+	u8 key_idx;
+	bool group_key = false;
+	bool wep_only = false;
+	int err = 0;
+	u8 mac_addr[ETH_ALEN];
+	u8 bcast_addr[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
+	u8 zero_addr[ETH_ALEN] = { 0 };
+
+	if (rtlpriv->cfg->mod_params->sw_crypto || rtlpriv->sec.use_sw_sec) {
+		RT_TRACE(COMP_ERR, DBG_WARNING,
+			 ("not open hw encryption\n"));
+		return -ENOSPC;	/*User disabled HW-crypto */
+	}
+	/* To support IBSS, use sw-crypto for GTK */
+	if(((vif->type == NL80211_IFTYPE_ADHOC) ||
+	    (vif->type == NL80211_IFTYPE_MESH_POINT)) &&
+	   !(key->flags & IEEE80211_KEY_FLAG_PAIRWISE))
+		return -ENOSPC;
+	RT_TRACE(COMP_SEC, DBG_DMESG,
+		 ("%s hardware based encryption for keyidx: %d, mac: %pM\n",
+		  cmd == SET_KEY ? "Using" : "Disabling", key->keyidx,
+		  sta ? sta->addr : bcast_addr));
+	rtlpriv->sec.being_setkey = true;
+	rtl_ips_nic_on(hw);
+	mutex_lock(&rtlpriv->locks.conf_mutex);
+	/* <1> get encryption alg */
+
+/*<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;
+		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 encrypiton\n"));
+		err = -EOPNOTSUPP;
+		goto out_unlock;
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("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 encrypiton, "
+			  "use software CMAC encrypiton\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)
+		rtlpriv->sec.use_defaultkey = true;
+
+	/* <2> get key_idx */
+	key_idx = (u8) (key->keyidx);
+	if (key_idx > 3)
+		goto out_unlock;
+	/* <3> if pairwise key enable_hw_sec */
+	group_key = !(key->flags & IEEE80211_KEY_FLAG_PAIRWISE);
+
+	/* wep always be group key, but there are two conditions:
+	 * 1) wep only: is just for wep enc, in this condition
+	 * rtlpriv->sec.pairwise_enc_algorithm == NO_ENCRYPTION
+	 * will be true & enable_hw_sec will be set when wep
+	 * ke setting.
+	 * 2) wep(group) + AES(pairwise): some AP like cisco
+	 * may use it, in this condition enable_hw_sec will not
+	 * be set when wep key setting */
+	/* we must reset sec_info after lingked before set key,
+	 * or some flag will be wrong*/
+	if (vif->type == NL80211_IFTYPE_AP ||
+		vif->type == NL80211_IFTYPE_MESH_POINT) {
+		if (!group_key || key_type == WEP40_ENCRYPTION ||
+			key_type == WEP104_ENCRYPTION) {
+			if (group_key) {
+				wep_only = true;
+			}
+			rtlpriv->cfg->ops->enable_hw_sec(hw);
+		}
+	} else {
+		if ((!group_key) || (vif->type == NL80211_IFTYPE_ADHOC) ||
+	    	    rtlpriv->sec.pairwise_enc_algorithm == NO_ENCRYPTION) {
+			if (rtlpriv->sec.pairwise_enc_algorithm ==
+			    NO_ENCRYPTION &&
+		    	   (key_type == WEP40_ENCRYPTION ||
+		    	    key_type == WEP104_ENCRYPTION))
+				wep_only = true;
+			rtlpriv->sec.pairwise_enc_algorithm = key_type;
+			RT_TRACE(COMP_SEC, DBG_DMESG,
+				 ("set enable_hw_sec, key_type:%x(OPEN:0 WEP40:"
+				  "1 TKIP:2 AES:4 WEP104:5)\n", key_type));
+			rtlpriv->cfg->ops->enable_hw_sec(hw);
+		}
+	}
+	/* <4> set key based on cmd */
+	switch (cmd) {
+	case SET_KEY:
+		if (wep_only) {
+			RT_TRACE(COMP_SEC, DBG_DMESG,
+				 ("set WEP(group/pairwise) key\n"));
+			/* Pairwise key with an assigned MAC address. */
+			rtlpriv->sec.pairwise_enc_algorithm = key_type;
+			rtlpriv->sec.group_enc_algorithm = key_type;
+			/*set local buf about wep key. */
+			memcpy(rtlpriv->sec.key_buf[key_idx],
+			       key->key, key->keylen);
+			rtlpriv->sec.key_len[key_idx] = key->keylen;
+			memcpy(mac_addr, zero_addr, ETH_ALEN);
+		} else if (group_key) {	/* group key */
+			RT_TRACE(COMP_SEC, DBG_DMESG,
+				 ("set group key\n"));
+			/* group key */
+			rtlpriv->sec.group_enc_algorithm = key_type;
+			/*set local buf about group key. */
+			memcpy(rtlpriv->sec.key_buf[key_idx],
+			       key->key, key->keylen);
+			rtlpriv->sec.key_len[key_idx] = key->keylen;
+			memcpy(mac_addr, bcast_addr, ETH_ALEN);
+		} else {	/* pairwise key */
+			RT_TRACE(COMP_SEC, DBG_DMESG,
+				 ("set pairwise key\n"));
+			if (!sta) {
+				RT_ASSERT(false, ("pairwise key withnot"
+						  "mac_addr\n"));
+
+				err = -EOPNOTSUPP;
+				goto out_unlock;
+			}
+			/* Pairwise key with an assigned MAC address. */
+			rtlpriv->sec.pairwise_enc_algorithm = key_type;
+			/*set local buf about pairwise key. */
+			memcpy(rtlpriv->sec.key_buf[PAIRWISE_KEYIDX],
+			       key->key, key->keylen);
+			rtlpriv->sec.key_len[PAIRWISE_KEYIDX] = key->keylen;
+			rtlpriv->sec.pairwise_key =
+			    rtlpriv->sec.key_buf[PAIRWISE_KEYIDX];
+			memcpy(mac_addr, sta->addr, ETH_ALEN);
+		}
+		rtlpriv->cfg->ops->set_key(hw, key_idx, mac_addr,
+					   group_key, key_type, wep_only,
+					   false);
+		/* <5> tell mac80211 do something: */
+		/*must use sw generate IV, or can not work !!!!. */
+		key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
+		key->hw_key_idx = key_idx;
+		if (key_type == TKIP_ENCRYPTION)
+			key->flags |= IEEE80211_KEY_FLAG_GENERATE_MMIC;
+		/*use software CCMP encryption for management frames (MFP) */
+		if (key_type == AESCCMP_ENCRYPTION)
+			key->flags |= IEEE80211_KEY_FLAG_SW_MGMT;
+		break;
+	case DISABLE_KEY:
+		RT_TRACE(COMP_SEC, DBG_DMESG,
+			 ("disable key delete one entry\n"));
+		/*set local buf about wep key. */
+		if (vif->type == NL80211_IFTYPE_AP ||
+			vif->type == NL80211_IFTYPE_MESH_POINT) {
+			if (sta)
+				rtl_cam_del_entry(hw, sta->addr);
+		}
+		memset(rtlpriv->sec.key_buf[key_idx], 0, key->keylen);
+		rtlpriv->sec.key_len[key_idx] = 0;
+		memcpy(mac_addr, zero_addr, ETH_ALEN);
+		/*
+		 *mac80211 will delete entrys one by one,
+		 *so don't use rtl_cam_reset_all_entry
+		 *or clear all entry here.
+		 */
+		rtl_cam_delete_one_entry(hw, mac_addr, key_idx);
+		break;
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("cmd_err:%x!!!!:\n", cmd));
+	}
+out_unlock:
+	mutex_unlock(&rtlpriv->locks.conf_mutex);
+	rtlpriv->sec.being_setkey = false;
+	return err;
+}
+
+static void rtl_op_rfkill_poll(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	bool radio_state;
+	bool blocked;
+	u8 valid = 0;
+
+	if (!test_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status))
+		return;
+
+	mutex_lock(&rtlpriv->locks.conf_mutex);
+
+	/*if Radio On return true here */
+	radio_state = rtlpriv->cfg->ops->radio_onoff_checking(hw, &valid);
+
+	if (valid) {
+		if (unlikely(radio_state != rtlpriv->rfkill.rfkill_state)) {
+			rtlpriv->rfkill.rfkill_state = radio_state;
+
+			RT_TRACE(COMP_RF, DBG_DMESG,
+				 (KERN_INFO "wireless radio switch turned %s\n",
+				  radio_state ? "on" : "off"));
+
+			blocked = (rtlpriv->rfkill.rfkill_state == 1) ? 0 : 1;
+			wiphy_rfkill_set_hw_state(hw->wiphy, blocked);
+		}
+	}
+
+	mutex_unlock(&rtlpriv->locks.conf_mutex);
+}
+
+/* this function is called by mac80211 to flush tx buffer
+ * before switch channle 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);
+
+	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,
+	.stop = rtl_op_stop,
+	.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,
+	.conf_tx = rtl_op_conf_tx,
+	.bss_info_changed = rtl_op_bss_info_changed,
+	.get_tsf = rtl_op_get_tsf,
+	.set_tsf = rtl_op_set_tsf,
+	.reset_tsf = rtl_op_reset_tsf,
+	.sta_notify = rtl_op_sta_notify,
+	.ampdu_action = rtl_op_ampdu_action,
+	.sw_scan_start = rtl_op_sw_scan_start,
+	.sw_scan_complete = rtl_op_sw_scan_complete,
+	.rfkill_poll = rtl_op_rfkill_poll,
+	.sta_add = rtl_op_sta_add,
+	.sta_remove = rtl_op_sta_remove,
+	.flush = rtl_op_flush,
+};
diff --git a/drivers/staging/rtl8821ae/core.h b/drivers/staging/rtl8821ae/core.h
new file mode 100644
index 000000000000..4b247db2861d
--- /dev/null
+++ b/drivers/staging/rtl8821ae/core.h
@@ -0,0 +1,43 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * tmis program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * Tme full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL_CORE_H__
+#define __RTL_CORE_H__
+
+#define RTL_SUPPORTED_FILTERS		\
+	(FIF_PROMISC_IN_BSS | \
+	FIF_ALLMULTI | FIF_CONTROL | \
+	FIF_OTHER_BSS | \
+	FIF_FCSFAIL | \
+	FIF_BCN_PRBRESP_PROMISC)
+
+#define RTL_SUPPORTED_CTRL_FILTER	0xFF
+
+extern const struct ieee80211_ops rtl_ops;
+#endif
diff --git a/drivers/staging/rtl8821ae/debug.c b/drivers/staging/rtl8821ae/debug.c
new file mode 100644
index 000000000000..cb051223c684
--- /dev/null
+++ b/drivers/staging/rtl8821ae/debug.c
@@ -0,0 +1,988 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * tmis program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * Tme full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * 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"
+
+#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)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 i;
+
+	rtlpriv->dbg.global_debuglevel = DBG_DMESG;
+
+	rtlpriv->dbg.global_debugcomponents =
+		COMP_ERR |
+		COMP_FW |
+		COMP_INIT |
+		COMP_RECV |
+		COMP_SEND |
+	  	COMP_MLME |
+	  	COMP_SCAN |
+	  	COMP_INTR |
+	  	COMP_LED |
+	  	COMP_SEC |
+	  	COMP_BEACON |
+	  	COMP_RATE |
+	  	COMP_RXDESC |
+	  	COMP_DIG |
+	  	COMP_TXAGC |
+		COMP_POWER |
+	  	COMP_POWER_TRACKING |
+	  	COMP_BB_POWERSAVING |
+	  	COMP_SWAS |
+	  	COMP_RF |
+	  	COMP_TURBO |
+	  	COMP_RATR |
+	  	COMP_CMD |
+	  	COMP_EASY_CONCURRENT |
+	  	COMP_EFUSE |
+	  	COMP_QOS | COMP_MAC80211 | COMP_REGD |
+		COMP_CHAN |
+		COMP_BT_COEXIST |
+		COMP_IQK |
+		0;
+
+	for (i = 0; i < DBGP_TYPE_MAX; i++)
+		rtlpriv->dbg.dbgp_type[i] = 0;
+
+	/*Init Debug flag enable condition */
+}
+
+struct proc_dir_entry *proc_topdir;
+static int rtl_proc_get_mac_0(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int i, n, page;
+	int max = 0xff;
+	page = 0x000;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_read_dword(rtlpriv, (page | n)));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_mac_0(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_mac_0, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_mac_0 = {
+	.open = dl_proc_open_mac_0,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_mac_1(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int i, n, page;
+	int max = 0xff;
+	page = 0x100;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_read_dword(rtlpriv, (page | n)));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_mac_1(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_mac_1, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_mac_1 = {
+	.open = dl_proc_open_mac_1,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_mac_2(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int i, n, page;
+	int max = 0xff;
+	page = 0x200;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_read_dword(rtlpriv, (page | n)));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_mac_2(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_mac_2, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_mac_2 = {
+	.open = dl_proc_open_mac_2,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_mac_3(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int i, n, page;
+	int max = 0xff;
+	page = 0x300;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_read_dword(rtlpriv, (page | n)));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_mac_3(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_mac_3, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_mac_3 = {
+	.open = dl_proc_open_mac_3,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_mac_4(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int i, n, page;
+	int max = 0xff;
+	page = 0x400;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_read_dword(rtlpriv, (page | n)));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_mac_4(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_mac_4, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_mac_4 = {
+	.open = dl_proc_open_mac_4,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_mac_5(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int i, n, page;
+	int max = 0xff;
+	page = 0x500;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_read_dword(rtlpriv, (page | n)));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_mac_5(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_mac_5, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_mac_5 = {
+	.open = dl_proc_open_mac_5,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_mac_6(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int i, n, page;
+	int max = 0xff;
+	page = 0x600;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_read_dword(rtlpriv, (page | n)));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_mac_6(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_mac_6, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_mac_6 = {
+	.open = dl_proc_open_mac_6,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_mac_7(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int i, n, page;
+	int max = 0xff;
+	page = 0x700;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_read_dword(rtlpriv, (page | n)));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_mac_7(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_mac_7, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_mac_7 = {
+	.open = dl_proc_open_mac_7,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_bb_8(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	int i, n, page;
+	int max = 0xff;
+	page = 0x800;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_bb_8(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_bb_8, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_bb_8 = {
+	.open = dl_proc_open_bb_8,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_bb_9(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	int i, n, page;
+	int max = 0xff;
+	page = 0x900;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_bb_9(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_bb_9, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_bb_9 = {
+	.open = dl_proc_open_bb_9,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_bb_a(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	int i, n, page;
+	int max = 0xff;
+	page = 0xa00;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_bb_a(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_bb_a, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_bb_a = {
+	.open = dl_proc_open_bb_a,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_bb_b(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	int i, n, page;
+	int max = 0xff;
+	page = 0xb00;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_bb_b(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_bb_b, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_bb_b = {
+	.open = dl_proc_open_bb_b,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_bb_c(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	int i, n, page;
+	int max = 0xff;
+	page = 0xc00;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_bb_c(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_bb_c, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_bb_c = {
+	.open = dl_proc_open_bb_c,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_bb_d(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	int i, n, page;
+	int max = 0xff;
+	page = 0xd00;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_bb_d(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_bb_d, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_bb_d = {
+	.open = dl_proc_open_bb_d,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_bb_e(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	int i, n, page;
+	int max = 0xff;
+	page = 0xe00;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_bb_e(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_bb_e, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_bb_e = {
+	.open = dl_proc_open_bb_e,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_bb_f(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	int i, n, page;
+	int max = 0xff;
+	page = 0xf00;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_bb_f(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_bb_f, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_bb_f = {
+	.open = dl_proc_open_bb_f,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_reg_rf_a(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	int i, n;
+	int max = 0x40;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n);
+		for (i = 0; i < 4 && n <= max; n += 1, i++)
+			seq_printf(m, "%8.8x    ",
+				   rtl_get_rfreg(hw, RF90_PATH_A, n, 0xffffffff));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_rf_a(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_reg_rf_a, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_rf_a = {
+	.open = dl_proc_open_rf_a,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_reg_rf_b(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	int i, n;
+	int max = 0x40;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n);
+		for (i = 0; i < 4 && n <= max; n += 1, i++)
+			seq_printf(m, "%8.8x    ",
+				   rtl_get_rfreg(hw, RF90_PATH_B, n,
+				   		 0xffffffff));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_rf_b(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_reg_rf_b, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_rf_b = {
+	.open = dl_proc_open_rf_b,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_cam_register_1(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 target_cmd = 0;
+	u32 target_val=0;
+	u8 entry_i=0;
+	u32 ulstatus;
+	int i = 100, j = 0;
+
+	/* This dump the current register page */
+	seq_puts(m,
+	    "\n#################### SECURITY CAM (0-10) ##################\n ");
+
+	for (j = 0; j < 11; j++) {
+		seq_printf(m, "\nD:  %2x > ", j);
+	 	for (entry_i = 0; entry_i < CAM_CONTENT_COUNT; entry_i++) {
+	   		/* polling bit, and No Write enable, and address  */
+			target_cmd = entry_i + CAM_CONTENT_COUNT * j;
+			target_cmd = target_cmd | BIT(31);
+
+			/* Check polling bit is clear */
+			while ((i--) >= 0) {
+				ulstatus = rtl_read_dword(rtlpriv,
+						rtlpriv->cfg->maps[RWCAM]);
+				if (ulstatus & BIT(31)) {
+					continue;
+				} else {
+					break;
+				}
+			}
+
+	  		rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM],
+					target_cmd);
+	  	 	target_val = rtl_read_dword(rtlpriv,
+						    rtlpriv->cfg->maps[RCAMO]);
+			seq_printf(m, "%8.8x ", target_val);
+	 	}
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_cam_1(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_cam_register_1,
+			   GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_cam_1 = {
+	.open = dl_proc_open_cam_1,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_cam_register_2(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 target_cmd = 0;
+	u32 target_val = 0;
+	u8 entry_i = 0;
+	u32 ulstatus;
+	int i = 100, j = 0;
+
+	/* This dump the current register page */
+	seq_puts(m,
+	    "\n################### SECURITY CAM (11-21) ##################\n ");
+
+	for (j = 11; j < 22; j++) {
+		seq_printf(m, "\nD:  %2x > ", j);
+	 	for (entry_i = 0; entry_i < CAM_CONTENT_COUNT; entry_i++) {
+			target_cmd = entry_i + CAM_CONTENT_COUNT * j;
+			target_cmd = target_cmd | BIT(31);
+
+			while ((i--) >= 0) {
+				ulstatus = rtl_read_dword(rtlpriv,
+						rtlpriv->cfg->maps[RWCAM]);
+				if (ulstatus & BIT(31)) {
+					continue;
+				} else {
+					break;
+				}
+			}
+
+	  		rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM],
+					target_cmd);
+	  	 	target_val = rtl_read_dword(rtlpriv,
+						    rtlpriv->cfg->maps[RCAMO]);
+			seq_printf(m, "%8.8x ", target_val);
+	 	}
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_cam_2(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_cam_register_2,
+			   GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_cam_2 = {
+	.open = dl_proc_open_cam_2,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_cam_register_3(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 target_cmd = 0;
+	u32 target_val = 0;
+	u8 entry_i = 0;
+	u32 ulstatus;
+	int i = 100, j = 0;
+
+	/* This dump the current register page */
+	seq_puts(m,
+	    "\n################### SECURITY CAM (22-31) ##################\n ");
+
+	for (j = 22; j < TOTAL_CAM_ENTRY; j++) {
+		seq_printf(m, "\nD:  %2x > ", j);
+	 	for (entry_i = 0; entry_i < CAM_CONTENT_COUNT; entry_i++) {
+			target_cmd = entry_i+CAM_CONTENT_COUNT*j;
+			target_cmd = target_cmd | BIT(31);
+
+			while ((i--) >= 0) {
+				ulstatus = rtl_read_dword(rtlpriv,
+						rtlpriv->cfg->maps[RWCAM]);
+				if (ulstatus & BIT(31)) {
+					continue;
+				} else {
+					break;
+				}
+			}
+
+	  		rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM],
+					target_cmd);
+	  	 	target_val = rtl_read_dword(rtlpriv,
+						    rtlpriv->cfg->maps[RCAMO]);
+			seq_printf(m, "%8.8x ", target_val);
+	 	}
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_cam_3(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_cam_register_3,
+			   GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_cam_3 = {
+	.open = dl_proc_open_cam_3,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+void rtl_proc_add_one(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct proc_dir_entry *entry;
+
+	snprintf(rtlpriv->dbg.proc_name, 18, "%x-%x-%x-%x-%x-%x",
+		rtlefuse->dev_addr[0], rtlefuse->dev_addr[1],
+		rtlefuse->dev_addr[2], rtlefuse->dev_addr[3],
+		rtlefuse->dev_addr[4], rtlefuse->dev_addr[5]);
+
+	rtlpriv->dbg.proc_dir = proc_mkdir(rtlpriv->dbg.proc_name, proc_topdir);
+	if (!rtlpriv->dbg.proc_dir) {
+		RT_TRACE(COMP_INIT, DBG_EMERG, ("Unable to init "
+			"/proc/net/%s/%s\n", rtlpriv->cfg->name,
+			rtlpriv->dbg.proc_name));
+		return;
+	}
+
+	entry = proc_create_data("mac-0", S_IFREG | S_IRUGO,
+				  rtlpriv->dbg.proc_dir, &file_ops_mac_0, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, DBG_EMERG,
+			 ("Unable to initialize /proc/net/%s/%s/mac-0\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("mac-1", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_mac_1, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/mac-1\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("mac-2", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_mac_2, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/mac-2\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("mac-3", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_mac_3, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/mac-3\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("mac-4", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_mac_4, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/mac-4\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("mac-5", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_mac_5, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/mac-5\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("mac-6", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_mac_6, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/mac-6\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("mac-7", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_mac_7, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/mac-7\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("bb-8", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_bb_8, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/bb-8\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("bb-9", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_bb_9, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/bb-9\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("bb-a", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_bb_a, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/bb-a\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("bb-b", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_bb_b, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/bb-b\n",
+		      rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("bb-c", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_bb_c, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/bb-c\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("bb-d", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_bb_d, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/bb-d\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("bb-e", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_bb_e, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/bb-e\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("bb-f", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_bb_f, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/bb-f\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("rf-a", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_rf_a, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/rf-a\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("rf-b", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_rf_b, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/rf-b\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("cam-1", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_cam_1, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/cam-1\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("cam-2", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_cam_2, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/cam-2\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("cam-3", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_cam_3, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/cam-3\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+}
+
+void rtl_proc_remove_one(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (rtlpriv->dbg.proc_dir) {
+		remove_proc_entry("mac-0", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("mac-1", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("mac-2", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("mac-3", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("mac-4", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("mac-5", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("mac-6", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("mac-7", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("bb-8", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("bb-9", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("bb-a", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("bb-b", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("bb-c", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("bb-d", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("bb-e", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("bb-f", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("rf-a", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("rf-b", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("cam-1", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("cam-2", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("cam-3", rtlpriv->dbg.proc_dir);
+
+		remove_proc_entry(rtlpriv->dbg.proc_name, proc_topdir);
+
+		rtlpriv->dbg.proc_dir = NULL;
+	}
+}
+
+void rtl_proc_add_topdir(void)
+{
+	proc_topdir = proc_mkdir("rtlwifi", init_net.proc_net);
+}
+
+void rtl_proc_remove_topdir(void)
+{
+	if (proc_topdir)
+		remove_proc_entry("rtlwifi", init_net.proc_net);
+}
\ No newline at end of file
diff --git a/drivers/staging/rtl8821ae/debug.h b/drivers/staging/rtl8821ae/debug.h
new file mode 100644
index 000000000000..5eb6251b89da
--- /dev/null
+++ b/drivers/staging/rtl8821ae/debug.h
@@ -0,0 +1,227 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * tmis program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * Tme full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL_DEBUG_H__
+#define __RTL_DEBUG_H__
+
+/*--------------------------------------------------------------
+			Debug level
+--------------------------------------------------------------*/
+/*
+ *Fatal bug.
+ *For example, Tx/Rx/IO locked up,
+ *memory access violation,
+ *resource allocation failed,
+ *unexpected HW behavior, HW BUG
+ *and so on.
+ */
+#define DBG_EMERG			0
+
+/*
+ *Abnormal, rare, or 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))	{			\
+			printk(KERN_DEBUG "%s:%s(): ", KBUILD_MODNAME, \
+			__func__);	\
+			printk fmt;			\
+		} \
+	} while(0);
+
+#define	RT_DISP(dbgtype, dbgflag, printstr)
+
+#define RT_TRACE(comp, level, fmt)\
+	do { \
+		if(unlikely(((comp) & rtlpriv->dbg.global_debugcomponents) && \
+			((level) <= rtlpriv->dbg.global_debuglevel))) {\
+			printk(KERN_DEBUG "%s-%d:%s():<%lx-%x> ", \
+			KBUILD_MODNAME, \
+			rtlpriv->rtlhal.interfaceindex, __func__, \
+			in_interrupt(), in_atomic());	\
+			printk fmt; 			\
+		}\
+	} while(0);
+
+#define RTPRINT(rtlpriv, dbgtype, dbgflag, printstr)	\
+	do {						\
+		if (unlikely(rtlpriv->dbg.dbgp_type[dbgtype] & dbgflag)) { \
+			printk(KERN_DEBUG "%s: ", KBUILD_MODNAME);	\
+			printk printstr;		\
+		}					\
+	} while(0);
+
+#define RT_PRINT_DATA(rtlpriv, _comp, _level, _titlestring, _hexdata, \
+		_hexdatalen) \
+	do {\
+		if(unlikely(((_comp) & rtlpriv->dbg.global_debugcomponents ) &&\
+			(_level <= rtlpriv->dbg.global_debuglevel )))	{ \
+			int __i;					\
+			u8*	ptr = (u8*)_hexdata;			\
+			printk(KERN_DEBUG "%s: ", KBUILD_MODNAME);	\
+			printk(KERN_DEBUG "In process \"%s\" (pid %i):", \
+					current->comm, 	\
+					current->pid); \
+			printk(_titlestring);		\
+			for( __i=0; __i<(int)_hexdatalen; __i++ ) {	\
+				printk("%02X%s", ptr[__i], (((__i + 1) % 4) \
+							== 0)?"  ":" ");\
+				if (((__i + 1) % 16) == 0)	\
+					printk("\n");	\
+			}				\
+			printk(KERN_DEBUG "\n");			\
+		} \
+	} while(0);
+
+void rtl_dbgp_flag_init(struct ieee80211_hw *hw);
+void rtl_proc_add_one(struct ieee80211_hw *hw);
+void rtl_proc_remove_one(struct ieee80211_hw *hw);
+void rtl_proc_add_topdir(void);
+void rtl_proc_remove_topdir(void);
+#endif
diff --git a/drivers/staging/rtl8821ae/efuse.c b/drivers/staging/rtl8821ae/efuse.c
new file mode 100644
index 000000000000..74c19ecc95a9
--- /dev/null
+++ b/drivers/staging/rtl8821ae/efuse.c
@@ -0,0 +1,1285 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * tmis program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * Tme full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+#include "wifi.h"
+#include "efuse.h"
+#include "btcoexist/halbt_precomp.h"
+#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;
+static const int EFUSE_MAX_SIZE = 512;
+
+static const struct efuse_map RTL8712_SDIO_EFUSE_TABLE[] = {
+	{0, 0, 0, 2},
+	{0, 1, 0, 2},
+	{0, 2, 0, 2},
+	{1, 0, 0, 1},
+	{1, 0, 1, 1},
+	{1, 1, 0, 1},
+	{1, 1, 1, 3},
+	{1, 3, 0, 17},
+	{3, 3, 1, 48},
+	{10, 0, 0, 6},
+	{10, 3, 0, 1},
+	{10, 3, 1, 1},
+	{11, 0, 0, 28}
+};
+
+static void efuse_shadow_read_1byte(struct ieee80211_hw *hw, u16 offset,
+				    u8 * value);
+static void efuse_shadow_read_2byte(struct ieee80211_hw *hw, u16 offset,
+				    u16 * value);
+static void efuse_shadow_read_4byte(struct ieee80211_hw *hw, u16 offset,
+				    u32 * value);
+static void efuse_shadow_write_1byte(struct ieee80211_hw *hw, u16 offset,
+				     u8 value);
+static void efuse_shadow_write_2byte(struct ieee80211_hw *hw, u16 offset,
+				     u16 value);
+static void efuse_shadow_write_4byte(struct ieee80211_hw *hw, u16 offset,
+				     u32 value);
+static int efuse_one_byte_write(struct ieee80211_hw *hw, u16 addr,
+				u8 data);
+static void efuse_read_all_map(struct ieee80211_hw *hw, u8 * efuse);
+static int efuse_pg_packet_read(struct ieee80211_hw *hw, u8 offset,
+				u8 *data);
+static int efuse_pg_packet_write(struct ieee80211_hw *hw, u8 offset,
+				 u8 word_en, u8 * data);
+static void efuse_word_enable_data_read(u8 word_en, u8 * sourdata,
+					u8 * targetdata);
+static u8 efuse_word_enable_data_write(struct ieee80211_hw *hw,
+				       u16 efuse_addr, u8 word_en, u8 * data);
+static void efuse_power_switch(struct ieee80211_hw *hw, u8 bwrite,
+					u8 pwrstate);
+static u16 efuse_get_current_size(struct ieee80211_hw *hw);
+static u8 efuse_calculate_word_cnts(u8 word_en);
+
+void efuse_initialize(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 bytetemp;
+	u8 temp;
+
+	bytetemp = rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[SYS_FUNC_EN] + 1);
+	temp = bytetemp | 0x20;
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[SYS_FUNC_EN] + 1, temp);
+
+	bytetemp = rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[SYS_ISO_CTRL] + 1);
+	temp = bytetemp & 0xFE;
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[SYS_ISO_CTRL] + 1, temp);
+
+	bytetemp = rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_TEST] + 3);
+	temp = bytetemp | 0x80;
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_TEST] + 3, temp);
+
+	rtl_write_byte(rtlpriv, 0x2F8, 0x3);
+
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 3, 0x72);
+
+}
+
+u8 efuse_read_1byte(struct ieee80211_hw *hw, u16 address)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 data;
+	u8 bytetemp;
+	u8 temp;
+	u32 k = 0;
+	const u32 efuse_real_content_len =
+		rtlpriv->cfg->maps[EFUSE_REAL_CONTENT_SIZE];
+
+	if (address < efuse_real_content_len) {
+		temp = address & 0xFF;
+		rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 1,
+			       temp);
+		bytetemp = rtl_read_byte(rtlpriv,
+					 rtlpriv->cfg->maps[EFUSE_CTRL] + 2);
+		temp = ((address >> 8) & 0x03) | (bytetemp & 0xFC);
+		rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 2,
+			       temp);
+
+		bytetemp = rtl_read_byte(rtlpriv,
+					 rtlpriv->cfg->maps[EFUSE_CTRL] + 3);
+		temp = bytetemp & 0x7F;
+		rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 3,
+			       temp);
+
+		bytetemp = rtl_read_byte(rtlpriv,
+					 rtlpriv->cfg->maps[EFUSE_CTRL] + 3);
+		while (!(bytetemp & 0x80)) {
+			bytetemp = rtl_read_byte(rtlpriv,
+						 rtlpriv->cfg->
+						 maps[EFUSE_CTRL] + 3);
+			k++;
+			if (k == 1000) {
+				k = 0;
+				break;
+			}
+		}
+		data = rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL]);
+		return data;
+	} else
+		return 0xFF;
+
+}
+//EXPORT_SYMBOL(efuse_read_1byte);
+
+void efuse_write_1byte(struct ieee80211_hw *hw, u16 address, u8 value)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 bytetemp;
+	u8 temp;
+	u32 k = 0;
+	const u32 efuse_real_content_len =
+		rtlpriv->cfg->maps[EFUSE_REAL_CONTENT_SIZE];
+
+	RT_TRACE(COMP_EFUSE, DBG_LOUD,
+		 ("Addr=%x Data =%x\n", address, value));
+
+	if (address < efuse_real_content_len) {
+		rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL], value);
+
+		temp = address & 0xFF;
+		rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 1,
+			       temp);
+		bytetemp = rtl_read_byte(rtlpriv,
+					 rtlpriv->cfg->maps[EFUSE_CTRL] + 2);
+
+		temp = ((address >> 8) & 0x03) | (bytetemp & 0xFC);
+		rtl_write_byte(rtlpriv,
+			       rtlpriv->cfg->maps[EFUSE_CTRL] + 2, temp);
+
+		bytetemp = rtl_read_byte(rtlpriv,
+					 rtlpriv->cfg->maps[EFUSE_CTRL] + 3);
+		temp = bytetemp | 0x80;
+		rtl_write_byte(rtlpriv,
+			       rtlpriv->cfg->maps[EFUSE_CTRL] + 3, temp);
+
+		bytetemp = rtl_read_byte(rtlpriv,
+					 rtlpriv->cfg->maps[EFUSE_CTRL] + 3);
+
+		while (bytetemp & 0x80) {
+			bytetemp = rtl_read_byte(rtlpriv,
+						 rtlpriv->cfg->
+						 maps[EFUSE_CTRL] + 3);
+			k++;
+			if (k == 100) {
+				k = 0;
+				break;
+			}
+		}
+	}
+
+}
+
+void read_efuse_byte(struct ieee80211_hw *hw, u16 _offset, u8 *pbuf)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 value32;
+	u8 readbyte;
+	u16 retry;
+
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 1,
+		       (_offset & 0xff));
+	readbyte = rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 2);
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 2,
+		       ((_offset >> 8) & 0x03) | (readbyte & 0xfc));
+
+	readbyte = rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 3);
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 3,
+		       (readbyte & 0x7f));
+
+	retry = 0;
+	value32 = rtl_read_dword(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL]);
+	while (!(((value32 >> 24) & 0xff) & 0x80) && (retry < 10000)) {
+		value32 = rtl_read_dword(rtlpriv,
+					 rtlpriv->cfg->maps[EFUSE_CTRL]);
+		retry++;
+	}
+
+	udelay(50);
+	value32 = rtl_read_dword(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL]);
+
+	*pbuf = (u8) (value32 & 0xff);
+}
+
+void read_efuse(struct ieee80211_hw *hw, u16 _offset, u16 _size_byte, u8 *pbuf)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u8 efuse_tbl[rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE]];
+	u8 rtemp8[1];
+	u16 efuse_addr = 0;
+	u8 offset, wren;
+	u8 u1temp = 0;
+	u16 i;
+	u16 j;
+	const u16 efuse_max_section =
+		rtlpriv->cfg->maps[EFUSE_MAX_SECTION_MAP];
+	const u32 efuse_real_content_len =
+		rtlpriv->cfg->maps[EFUSE_REAL_CONTENT_SIZE];
+	u16 efuse_word[efuse_max_section][EFUSE_MAX_WORD_UNIT];
+	u16 efuse_utilized = 0;
+	u8 efuse_usage;
+
+	if ((_offset + _size_byte) > rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE]) {
+		RT_TRACE(COMP_EFUSE, DBG_LOUD,
+			 ("read_efuse(): Invalid offset(%#x) with read "
+			  "bytes(%#x)!!\n", _offset, _size_byte));
+		return;
+	}
+
+	for (i = 0; i < efuse_max_section; i++)
+		for (j = 0; j < EFUSE_MAX_WORD_UNIT; j++)
+			efuse_word[i][j] = 0xFFFF;
+
+	read_efuse_byte(hw, efuse_addr, rtemp8);
+	if (*rtemp8 != 0xFF) {
+		efuse_utilized++;
+		RTPRINT(rtlpriv, FEEPROM, EFUSE_READ_ALL,
+			("Addr=%d\n", efuse_addr));
+		efuse_addr++;
+	}
+
+	while ((*rtemp8 != 0xFF) && (efuse_addr < efuse_real_content_len)) {
+		/*  Check PG header for section num.  */
+		if((*rtemp8 & 0x1F ) == 0x0F) {/* extended header */
+			u1temp =( (*rtemp8 & 0xE0) >> 5);
+			read_efuse_byte(hw, efuse_addr, rtemp8);
+
+			if((*rtemp8 & 0x0F) == 0x0F) {
+				efuse_addr++;
+				read_efuse_byte(hw, efuse_addr, rtemp8);
+
+				if (*rtemp8 != 0xFF &&
+				    (efuse_addr < efuse_real_content_len)) {
+					efuse_addr++;
+				}
+				continue;
+			} else {
+				offset = ((*rtemp8 & 0xF0) >> 1) | u1temp;
+				wren = (*rtemp8 & 0x0F);
+				efuse_addr++;
+			}
+		} else {
+			offset = ((*rtemp8 >> 4) & 0x0f);
+			wren = (*rtemp8 & 0x0f);
+		}
+
+		if (offset < efuse_max_section) {
+			RTPRINT(rtlpriv, FEEPROM, EFUSE_READ_ALL,
+				("offset-%d Worden=%x\n", offset, wren));
+
+			for (i = 0; i < EFUSE_MAX_WORD_UNIT; i++) {
+				if (!(wren & 0x01)) {
+					RTPRINT(rtlpriv, FEEPROM,
+						EFUSE_READ_ALL, ("Addr=%d\n",
+								 efuse_addr));
+
+					read_efuse_byte(hw, efuse_addr, rtemp8);
+					efuse_addr++;
+					efuse_utilized++;
+					efuse_word[offset][i] = (*rtemp8 &
+								 0xff);
+
+					if (efuse_addr >=
+					    efuse_real_content_len)
+						break;
+
+					RTPRINT(rtlpriv, FEEPROM,
+						EFUSE_READ_ALL, ("Addr=%d\n",
+								 efuse_addr));
+
+					read_efuse_byte(hw, efuse_addr, rtemp8);
+					efuse_addr++;
+					efuse_utilized++;
+					efuse_word[offset][i] |=
+					    (((u16) * rtemp8 << 8) & 0xff00);
+
+					if (efuse_addr >= efuse_real_content_len)
+						break;
+				}
+
+				wren >>= 1;
+			}
+		}
+
+		RTPRINT(rtlpriv, FEEPROM, EFUSE_READ_ALL,
+			("Addr=%d\n", efuse_addr));
+		read_efuse_byte(hw, efuse_addr, rtemp8);
+		if (*rtemp8 != 0xFF && (efuse_addr < efuse_real_content_len)) {
+			efuse_utilized++;
+			efuse_addr++;
+		}
+	}
+
+	for (i = 0; i < efuse_max_section; i++) {
+		for (j = 0; j < EFUSE_MAX_WORD_UNIT; j++) {
+			efuse_tbl[(i * 8) + (j * 2)] =
+			    (efuse_word[i][j] & 0xff);
+			efuse_tbl[(i * 8) + ((j * 2) + 1)] =
+			    ((efuse_word[i][j] >> 8) & 0xff);
+		}
+	}
+
+	for (i = 0; i < _size_byte; i++)
+		pbuf[i] = efuse_tbl[_offset + i];
+
+	rtlefuse->efuse_usedbytes = efuse_utilized;
+	efuse_usage = (u8) ((efuse_utilized * 100) / efuse_real_content_len);
+	rtlefuse->efuse_usedpercentage = efuse_usage;
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_EFUSE_BYTES,
+				      (u8 *) & efuse_utilized);
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_EFUSE_USAGE,
+				      (u8 *) & efuse_usage);
+}
+
+bool efuse_shadow_update_chk(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u8 section_idx, i, Base;
+	u16 words_need = 0, hdr_num = 0, totalbytes, efuse_used;
+	bool bwordchanged, bresult = true;
+
+	for (section_idx = 0; section_idx < 16; section_idx++) {
+		Base = section_idx * 8;
+		bwordchanged = false;
+
+		for (i = 0; i < 8; i = i + 2) {
+			if ((rtlefuse->efuse_map[EFUSE_INIT_MAP][Base + i] !=
+			     rtlefuse->efuse_map[EFUSE_MODIFY_MAP][Base + i]) ||
+			    (rtlefuse->efuse_map[EFUSE_INIT_MAP][Base + i + 1] !=
+			     rtlefuse->efuse_map[EFUSE_MODIFY_MAP][Base + i +
+								   1])) {
+				words_need++;
+				bwordchanged = true;
+			}
+		}
+
+		if (bwordchanged == true)
+			hdr_num++;
+	}
+
+	totalbytes = hdr_num + words_need * 2;
+	efuse_used = rtlefuse->efuse_usedbytes;
+
+	if ((totalbytes + efuse_used) >= (EFUSE_MAX_SIZE -
+		rtlpriv->cfg->maps[EFUSE_OOB_PROTECT_BYTES_LEN]))
+		bresult = false;
+
+	RT_TRACE(COMP_EFUSE, DBG_LOUD,
+		 ("efuse_shadow_update_chk(): totalbytes(%#x), "
+		  "hdr_num(%#x), words_need(%#x), efuse_used(%d)\n",
+		  totalbytes, hdr_num, words_need, efuse_used));
+
+	return bresult;
+}
+
+void efuse_shadow_read(struct ieee80211_hw *hw, u8 type,
+		       u16 offset, u32 *value)
+{
+	if (type == 1)
+		efuse_shadow_read_1byte(hw, offset, (u8 *) value);
+	else if (type == 2)
+		efuse_shadow_read_2byte(hw, offset, (u16 *) value);
+	else if (type == 4)
+		efuse_shadow_read_4byte(hw, offset, (u32 *) value);
+
+}
+
+void efuse_shadow_write(struct ieee80211_hw *hw, u8 type, u16 offset,
+				u32 value)
+{
+	if (type == 1)
+		efuse_shadow_write_1byte(hw, offset, (u8) value);
+	else if (type == 2)
+		efuse_shadow_write_2byte(hw, offset, (u16) value);
+	else if (type == 4)
+		efuse_shadow_write_4byte(hw, offset, (u32) value);
+
+}
+
+bool efuse_shadow_update(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u16 i, offset, base;
+	u8 word_en = 0x0F;
+	u8 first_pg = false;
+
+	RT_TRACE(COMP_EFUSE, DBG_LOUD, ("\n"));
+
+	if (!efuse_shadow_update_chk(hw)) {
+		efuse_read_all_map(hw, &rtlefuse->efuse_map[EFUSE_INIT_MAP][0]);
+		memcpy(&rtlefuse->efuse_map[EFUSE_MODIFY_MAP][0],
+		       &rtlefuse->efuse_map[EFUSE_INIT_MAP][0],
+		       rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE]);
+
+		RT_TRACE(COMP_EFUSE, DBG_LOUD,
+			 ("efuse out of capacity!!\n"));
+		return false;
+	}
+	efuse_power_switch(hw, true, true);
+
+	for (offset = 0; offset < 16; offset++) {
+
+		word_en = 0x0F;
+		base = offset * 8;
+
+		for (i = 0; i < 8; i++) {
+			if (first_pg == true) {
+
+				word_en &= ~(BIT(i / 2));
+
+				rtlefuse->efuse_map[EFUSE_INIT_MAP][base + i] =
+				    rtlefuse->efuse_map[EFUSE_MODIFY_MAP][base + i];
+			} else {
+
+				if (rtlefuse->efuse_map[EFUSE_INIT_MAP][base + i] !=
+				    rtlefuse->efuse_map[EFUSE_MODIFY_MAP][base + i]) {
+					word_en &= ~(BIT(i / 2));
+
+					rtlefuse->efuse_map[EFUSE_INIT_MAP][base + i] =
+					    rtlefuse->efuse_map[EFUSE_MODIFY_MAP][base + i];
+				}
+			}
+		}
+
+		if (word_en != 0x0F) {
+			u8 tmpdata[8];
+			memcpy(tmpdata, (&rtlefuse->efuse_map[EFUSE_MODIFY_MAP][base]), 8);
+			RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_LOUD,
+				      ("U-efuse\n"), tmpdata, 8);
+
+			if (!efuse_pg_packet_write(hw, (u8) offset, word_en,
+						   tmpdata)) {
+				RT_TRACE(COMP_ERR, DBG_WARNING,
+					 ("PG section(%#x) fail!!\n", offset));
+				break;
+			}
+		}
+
+	}
+
+	efuse_power_switch(hw, true, false);
+	efuse_read_all_map(hw, &rtlefuse->efuse_map[EFUSE_INIT_MAP][0]);
+
+	memcpy(&rtlefuse->efuse_map[EFUSE_MODIFY_MAP][0],
+	       &rtlefuse->efuse_map[EFUSE_INIT_MAP][0],
+	       rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE]);
+
+	RT_TRACE(COMP_EFUSE, DBG_LOUD, ("\n"));
+	return true;
+}
+
+void rtl_efuse_shadow_map_update(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+
+	if (rtlefuse->autoload_failflag == true) {
+		memset((&rtlefuse->efuse_map[EFUSE_INIT_MAP][0]),
+			0xFF, rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE]);
+	} else {
+		efuse_read_all_map(hw, &rtlefuse->efuse_map[EFUSE_INIT_MAP][0]);
+	}
+
+	memcpy(&rtlefuse->efuse_map[EFUSE_MODIFY_MAP][0],
+			&rtlefuse->efuse_map[EFUSE_INIT_MAP][0],
+			rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE]);
+
+}
+//EXPORT_SYMBOL(rtl_efuse_shadow_map_update);
+
+void efuse_force_write_vendor_Id(struct ieee80211_hw *hw)
+{
+	u8 tmpdata[8] = { 0xFF, 0xFF, 0xEC, 0x10, 0xFF, 0xFF, 0xFF, 0xFF };
+
+	efuse_power_switch(hw, true, true);
+
+	efuse_pg_packet_write(hw, 1, 0xD, tmpdata);
+
+	efuse_power_switch(hw, true, false);
+
+}
+
+void efuse_re_pg_section(struct ieee80211_hw *hw, u8 section_idx)
+{
+}
+
+static void efuse_shadow_read_1byte(struct ieee80211_hw *hw,
+				    u16 offset, u8 *value)
+{
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	*value = rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset];
+}
+
+static void efuse_shadow_read_2byte(struct ieee80211_hw *hw,
+				    u16 offset, u16 *value)
+{
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+
+	*value = rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset];
+	*value |= rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 1] << 8;
+
+}
+
+static void efuse_shadow_read_4byte(struct ieee80211_hw *hw,
+				    u16 offset, u32 *value)
+{
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+
+	*value = rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset];
+	*value |= rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 1] << 8;
+	*value |= rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 2] << 16;
+	*value |= rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 3] << 24;
+}
+
+static void efuse_shadow_write_1byte(struct ieee80211_hw *hw,
+				     u16 offset, u8 value)
+{
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+
+	rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset] = value;
+}
+
+static void efuse_shadow_write_2byte(struct ieee80211_hw *hw,
+				     u16 offset, u16 value)
+{
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+
+	rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset] = value & 0x00FF;
+	rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 1] = value >> 8;
+
+}
+
+static void efuse_shadow_write_4byte(struct ieee80211_hw *hw,
+				     u16 offset, u32 value)
+{
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+
+	rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset] =
+	    (u8) (value & 0x000000FF);
+	rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 1] =
+	    (u8) ((value >> 8) & 0x0000FF);
+	rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 2] =
+	    (u8) ((value >> 16) & 0x00FF);
+	rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 3] =
+	    (u8) ((value >> 24) & 0xFF);
+
+}
+
+int efuse_one_byte_read(struct ieee80211_hw *hw, u16 addr, u8 *data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmpidx = 0;
+	int bresult;
+
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 1,
+		       (u8) (addr & 0xff));
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 2,
+		       ((u8) ((addr >> 8) & 0x03)) |
+		       (rtl_read_byte(rtlpriv,
+				      rtlpriv->cfg->maps[EFUSE_CTRL] + 2) &
+			0xFC));
+
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 3, 0x72);
+
+	while (!(0x80 & rtl_read_byte(rtlpriv,
+				      rtlpriv->cfg->maps[EFUSE_CTRL] + 3))
+	       && (tmpidx < 100)) {
+		tmpidx++;
+	}
+
+	if (tmpidx < 100) {
+		*data = rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL]);
+		bresult = true;
+	} else {
+		*data = 0xff;
+		bresult = false;
+	}
+	return bresult;
+}
+//EXPORT_SYMBOL(efuse_one_byte_read);
+
+static int efuse_one_byte_write(struct ieee80211_hw *hw, u16 addr, u8 data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmpidx = 0;
+	bool bresult;
+
+	RT_TRACE(COMP_EFUSE, DBG_LOUD,
+		 ("Addr = %x Data=%x\n", addr, data));
+
+	rtl_write_byte(rtlpriv,
+		       rtlpriv->cfg->maps[EFUSE_CTRL] + 1, (u8) (addr & 0xff));
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 2,
+		       (rtl_read_byte(rtlpriv,
+			 rtlpriv->cfg->maps[EFUSE_CTRL] +
+			 2) & 0xFC) | (u8) ((addr >> 8) & 0x03));
+
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL], data);
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 3, 0xF2);
+
+	while ((0x80 & rtl_read_byte(rtlpriv,
+				     rtlpriv->cfg->maps[EFUSE_CTRL] + 3))
+	       && (tmpidx < 100)) {
+		tmpidx++;
+	}
+
+	if (tmpidx < 100)
+		bresult = true;
+	else
+		bresult = false;
+
+	return bresult;
+}
+
+static void efuse_read_all_map(struct ieee80211_hw *hw, u8 * efuse)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	efuse_power_switch(hw, false, true);
+	read_efuse(hw, 0, rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE], efuse);
+	efuse_power_switch(hw, false, false);
+}
+
+static void efuse_read_data_case1(struct ieee80211_hw *hw, u16 *efuse_addr,
+				u8 efuse_data, u8 offset, u8 *tmpdata,
+				u8 *readstate)
+{
+	bool bdataempty = true;
+	u8 hoffset;
+	u8 tmpidx;
+	u8 hworden;
+	u8 word_cnts;
+
+	hoffset = (efuse_data >> 4) & 0x0F;
+	hworden = efuse_data & 0x0F;
+	word_cnts = efuse_calculate_word_cnts(hworden);
+
+	if (hoffset == offset) {
+		for (tmpidx = 0; tmpidx < word_cnts * 2; tmpidx++) {
+			if (efuse_one_byte_read(hw, *efuse_addr + 1 + tmpidx,
+						&efuse_data)) {
+				tmpdata[tmpidx] = efuse_data;
+				if (efuse_data != 0xff)
+					bdataempty = true;
+			}
+		}
+
+		if (bdataempty == true) {
+			*readstate = PG_STATE_DATA;
+		} else {
+			*efuse_addr = *efuse_addr + (word_cnts * 2) + 1;
+			*readstate = PG_STATE_HEADER;
+		}
+
+	} else {
+		*efuse_addr = *efuse_addr + (word_cnts * 2) + 1;
+		*readstate = PG_STATE_HEADER;
+	}
+}
+
+static int efuse_pg_packet_read(struct ieee80211_hw *hw, u8 offset, u8 *data)
+{
+	u8 readstate = PG_STATE_HEADER;
+
+	bool bcontinual = true;
+
+	u8 efuse_data, word_cnts = 0;
+	u16 efuse_addr = 0;
+	u8 hworden = 0;
+	u8 tmpdata[8];
+
+	if (data == NULL)
+		return false;
+	if (offset > 15)
+		return false;
+
+	memset(data, 0xff, PGPKT_DATA_SIZE * sizeof(u8));
+	memset(tmpdata, 0xff, PGPKT_DATA_SIZE * sizeof(u8));
+
+	while (bcontinual && (efuse_addr < EFUSE_MAX_SIZE)) {
+		if (readstate & PG_STATE_HEADER) {
+			if (efuse_one_byte_read(hw, efuse_addr, &efuse_data)
+			    && (efuse_data != 0xFF))
+				efuse_read_data_case1(hw, &efuse_addr, efuse_data, offset,
+							tmpdata, &readstate);
+			else
+				bcontinual = false;
+		} else if (readstate & PG_STATE_DATA) {
+			efuse_word_enable_data_read(hworden, tmpdata, data);
+			efuse_addr = efuse_addr + (word_cnts * 2) + 1;
+			readstate = PG_STATE_HEADER;
+		}
+
+	}
+
+	if ((data[0] == 0xff) && (data[1] == 0xff) &&
+	    (data[2] == 0xff) && (data[3] == 0xff) &&
+	    (data[4] == 0xff) && (data[5] == 0xff) &&
+	    (data[6] == 0xff) && (data[7] == 0xff))
+		return false;
+	else
+		return true;
+
+}
+
+static void efuse_write_data_case1(struct ieee80211_hw *hw, u16 *efuse_addr,
+				u8 efuse_data, u8 offset, int *bcontinual,
+				u8 *write_state, struct pgpkt_struct *target_pkt,
+				int *repeat_times, int *bresult, u8 word_en)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct pgpkt_struct tmp_pkt;
+	int bdataempty = true;
+	u8 originaldata[8 * sizeof(u8)];
+	u8 badworden = 0x0F;
+	u8 match_word_en, tmp_word_en;
+	u8 tmpindex;
+	u8 tmp_header = efuse_data;
+	u8 tmp_word_cnts;
+
+	tmp_pkt.offset = (tmp_header >> 4) & 0x0F;
+	tmp_pkt.word_en = tmp_header & 0x0F;
+	tmp_word_cnts = efuse_calculate_word_cnts(tmp_pkt.word_en);
+
+	if (tmp_pkt.offset != target_pkt->offset) {
+		*efuse_addr = *efuse_addr + (tmp_word_cnts * 2) + 1;
+		*write_state = PG_STATE_HEADER;
+	} else {
+		for (tmpindex = 0; tmpindex < (tmp_word_cnts * 2); tmpindex++) {
+			if (efuse_one_byte_read(hw,
+						(*efuse_addr + 1 + tmpindex),
+						&efuse_data) && (efuse_data != 0xFF))
+				bdataempty = false;
+		}
+
+		if (bdataempty == false) {
+			*efuse_addr = *efuse_addr + (tmp_word_cnts * 2) + 1;
+			*write_state = PG_STATE_HEADER;
+		} else {
+			match_word_en = 0x0F;
+			if (!((target_pkt->word_en & BIT(0)) |
+			    (tmp_pkt.word_en & BIT(0))))
+				match_word_en &= (~BIT(0));
+
+			if (!((target_pkt->word_en & BIT(1)) |
+			    (tmp_pkt.word_en & BIT(1))))
+				match_word_en &= (~BIT(1));
+
+			if (!((target_pkt->word_en & BIT(2)) |
+			    (tmp_pkt.word_en & BIT(2))))
+				match_word_en &= (~BIT(2));
+
+			if (!((target_pkt->word_en & BIT(3)) |
+			    (tmp_pkt.word_en & BIT(3))))
+				match_word_en &= (~BIT(3));
+
+			if ((match_word_en & 0x0F) != 0x0F) {
+				badworden = efuse_word_enable_data_write(hw,
+							*efuse_addr + 1,
+							tmp_pkt.word_en,
+							target_pkt->data);
+
+				if (0x0F != (badworden & 0x0F))	{
+					u8 reorg_offset = offset;
+					u8 reorg_worden = badworden;
+					efuse_pg_packet_write(hw, reorg_offset,
+							      reorg_worden,
+							      originaldata);
+				}
+
+				tmp_word_en = 0x0F;
+				if ((target_pkt->word_en & BIT(0)) ^
+				    (match_word_en & BIT(0)))
+					tmp_word_en &= (~BIT(0));
+
+				if ((target_pkt->word_en & BIT(1)) ^
+				    (match_word_en & BIT(1)))
+					tmp_word_en &= (~BIT(1));
+
+				if ((target_pkt->word_en & BIT(2)) ^
+				    (match_word_en & BIT(2)))
+					tmp_word_en &= (~BIT(2));
+
+				if ((target_pkt->word_en & BIT(3)) ^
+				    (match_word_en & BIT(3)))
+					tmp_word_en &= (~BIT(3));
+
+				if ((tmp_word_en & 0x0F) != 0x0F) {
+					*efuse_addr = efuse_get_current_size(hw);
+					target_pkt->offset = offset;
+					target_pkt->word_en = tmp_word_en;
+				} else {
+					*bcontinual = false;
+				}
+				*write_state = PG_STATE_HEADER;
+				*repeat_times += 1;
+				if (*repeat_times > EFUSE_REPEAT_THRESHOLD_) {
+					*bcontinual = false;
+					*bresult = false;
+				}
+			} else {
+				*efuse_addr += (2 * tmp_word_cnts) + 1;
+				target_pkt->offset = offset;
+				target_pkt->word_en = word_en;
+				*write_state = PG_STATE_HEADER;
+			}
+		}
+	}
+	RTPRINT(rtlpriv, FEEPROM, EFUSE_PG, ("efuse PG_STATE_HEADER-1\n"));
+}
+
+static void efuse_write_data_case2(struct ieee80211_hw *hw, u16 *efuse_addr,
+				   int *bcontinual, u8 *write_state,
+				   struct pgpkt_struct target_pkt,
+				   int *repeat_times, int *bresult)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct pgpkt_struct tmp_pkt;
+	u8 pg_header;
+	u8 tmp_header;
+	u8 originaldata[8 * sizeof(u8)];
+	u8 tmp_word_cnts;
+	u8 badworden = 0x0F;
+
+	pg_header = ((target_pkt.offset << 4) & 0xf0) | target_pkt.word_en;
+	efuse_one_byte_write(hw, *efuse_addr, pg_header);
+	efuse_one_byte_read(hw, *efuse_addr, &tmp_header);
+
+	if (tmp_header == pg_header) {
+		*write_state = PG_STATE_DATA;
+	} else if (tmp_header == 0xFF) {
+		*write_state = PG_STATE_HEADER;
+		*repeat_times += 1;
+		if (*repeat_times > EFUSE_REPEAT_THRESHOLD_) {
+			*bcontinual = false;
+			*bresult = false;
+		}
+	} else {
+		tmp_pkt.offset = (tmp_header >> 4) & 0x0F;
+		tmp_pkt.word_en = tmp_header & 0x0F;
+
+		tmp_word_cnts = efuse_calculate_word_cnts(tmp_pkt.word_en);
+
+		memset(originaldata, 0xff,  8 * sizeof(u8));
+
+		if (efuse_pg_packet_read(hw, tmp_pkt.offset, originaldata)) {
+			badworden = efuse_word_enable_data_write(hw,
+								*efuse_addr + 1,
+								tmp_pkt.word_en,
+								originaldata);
+
+			if (0x0F != (badworden & 0x0F)) {
+				u8 reorg_offset = tmp_pkt.offset;
+				u8 reorg_worden = badworden;
+				efuse_pg_packet_write(hw, reorg_offset,
+						      reorg_worden,
+						      originaldata);
+				*efuse_addr = efuse_get_current_size(hw);
+			} else {
+				*efuse_addr = *efuse_addr +
+					      (tmp_word_cnts * 2) + 1;
+			}
+		} else {
+			*efuse_addr = *efuse_addr + (tmp_word_cnts * 2) + 1;
+		}
+
+		*write_state = PG_STATE_HEADER;
+		*repeat_times += 1;
+		if (*repeat_times > EFUSE_REPEAT_THRESHOLD_) {
+			*bcontinual = false;
+			*bresult = false;
+		}
+
+		RTPRINT(rtlpriv, FEEPROM, EFUSE_PG,
+			("efuse PG_STATE_HEADER-2\n"));
+	}
+}
+
+static int efuse_pg_packet_write(struct ieee80211_hw *hw,
+				 u8 offset, u8 word_en, u8 *data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct pgpkt_struct target_pkt;
+	u8 write_state = PG_STATE_HEADER;
+	int bcontinual = true, bdataempty = true, bresult = true;
+	u16 efuse_addr = 0;
+	u8 efuse_data;
+	u8 target_word_cnts = 0;
+	u8 badworden = 0x0F;
+	static int repeat_times = 0;
+
+	if (efuse_get_current_size(hw) >= (EFUSE_MAX_SIZE -
+		rtlpriv->cfg->maps[EFUSE_OOB_PROTECT_BYTES_LEN])) {
+		RTPRINT(rtlpriv, FEEPROM, EFUSE_PG,
+			("efuse_pg_packet_write error \n"));
+		return false;
+	}
+
+	target_pkt.offset = offset;
+	target_pkt.word_en = word_en;
+
+	memset(target_pkt.data, 0xFF,  8 * sizeof(u8));
+
+	efuse_word_enable_data_read(word_en, data, target_pkt.data);
+	target_word_cnts = efuse_calculate_word_cnts(target_pkt.word_en);
+
+	RTPRINT(rtlpriv, FEEPROM, EFUSE_PG, ("efuse Power ON\n"));
+
+	while (bcontinual && (efuse_addr < (EFUSE_MAX_SIZE -
+		rtlpriv->cfg->maps[EFUSE_OOB_PROTECT_BYTES_LEN]))) {
+
+		if (write_state == PG_STATE_HEADER) {
+			bdataempty = true;
+			badworden = 0x0F;
+			RTPRINT(rtlpriv, FEEPROM, EFUSE_PG,
+				("efuse PG_STATE_HEADER\n"));
+
+			if (efuse_one_byte_read(hw, efuse_addr, &efuse_data) &&
+			    (efuse_data != 0xFF))
+				efuse_write_data_case1(hw, &efuse_addr,
+						       efuse_data, offset,
+						       &bcontinual,
+						       &write_state,
+						       &target_pkt,
+						       &repeat_times, &bresult,
+						       word_en);
+			else
+				efuse_write_data_case2(hw, &efuse_addr,
+						       &bcontinual,
+						       &write_state,
+						       target_pkt,
+						       &repeat_times,
+						       &bresult);
+
+		} else if (write_state == PG_STATE_DATA) {
+			RTPRINT(rtlpriv, FEEPROM, EFUSE_PG,
+				("efuse PG_STATE_DATA\n"));
+			badworden = 0x0f;
+			badworden =
+			    efuse_word_enable_data_write(hw, efuse_addr + 1,
+							 target_pkt.word_en,
+							 target_pkt.data);
+
+			if ((badworden & 0x0F) == 0x0F) {
+				bcontinual = false;
+			} else {
+				efuse_addr =
+				    efuse_addr + (2 * target_word_cnts) + 1;
+
+				target_pkt.offset = offset;
+				target_pkt.word_en = badworden;
+				target_word_cnts =
+				    efuse_calculate_word_cnts(target_pkt.
+							      word_en);
+				write_state = PG_STATE_HEADER;
+				repeat_times++;
+				if (repeat_times > EFUSE_REPEAT_THRESHOLD_) {
+					bcontinual = false;
+					bresult = false;
+				}
+				RTPRINT(rtlpriv, FEEPROM, EFUSE_PG,
+					("efuse PG_STATE_HEADER-3\n"));
+			}
+		}
+	}
+
+	if (efuse_addr >= (EFUSE_MAX_SIZE -
+		rtlpriv->cfg->maps[EFUSE_OOB_PROTECT_BYTES_LEN])) {
+		RT_TRACE(COMP_EFUSE, DBG_LOUD,
+			 ("efuse_addr(%#x) Out of size!!\n", efuse_addr));
+	}
+
+	return true;
+}
+
+static void efuse_word_enable_data_read(u8 word_en, u8 * sourdata,
+					u8 *targetdata)
+{
+	if (!(word_en & BIT(0))) {
+		targetdata[0] = sourdata[0];
+		targetdata[1] = sourdata[1];
+	}
+
+	if (!(word_en & BIT(1))) {
+		targetdata[2] = sourdata[2];
+		targetdata[3] = sourdata[3];
+	}
+
+	if (!(word_en & BIT(2))) {
+		targetdata[4] = sourdata[4];
+		targetdata[5] = sourdata[5];
+	}
+
+	if (!(word_en & BIT(3))) {
+		targetdata[6] = sourdata[6];
+		targetdata[7] = sourdata[7];
+	}
+}
+
+static u8 efuse_word_enable_data_write(struct ieee80211_hw *hw,
+				       u16 efuse_addr, u8 word_en, u8 *data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u16 tmpaddr;
+	u16 start_addr = efuse_addr;
+	u8 badworden = 0x0F;
+	u8 tmpdata[8];
+
+	memset(tmpdata, 0xff, PGPKT_DATA_SIZE);
+	RT_TRACE(COMP_EFUSE, DBG_LOUD,
+		 ("word_en = %x efuse_addr=%x\n", word_en, efuse_addr));
+
+	if (!(word_en & BIT(0))) {
+		tmpaddr = start_addr;
+		efuse_one_byte_write(hw, start_addr++, data[0]);
+		efuse_one_byte_write(hw, start_addr++, data[1]);
+
+		efuse_one_byte_read(hw, tmpaddr, &tmpdata[0]);
+		efuse_one_byte_read(hw, tmpaddr + 1, &tmpdata[1]);
+		if ((data[0] != tmpdata[0]) || (data[1] != tmpdata[1]))
+			badworden &= (~BIT(0));
+	}
+
+	if (!(word_en & BIT(1))) {
+		tmpaddr = start_addr;
+		efuse_one_byte_write(hw, start_addr++, data[2]);
+		efuse_one_byte_write(hw, start_addr++, data[3]);
+
+		efuse_one_byte_read(hw, tmpaddr, &tmpdata[2]);
+		efuse_one_byte_read(hw, tmpaddr + 1, &tmpdata[3]);
+		if ((data[2] != tmpdata[2]) || (data[3] != tmpdata[3]))
+			badworden &= (~BIT(1));
+	}
+
+	if (!(word_en & BIT(2))) {
+		tmpaddr = start_addr;
+		efuse_one_byte_write(hw, start_addr++, data[4]);
+		efuse_one_byte_write(hw, start_addr++, data[5]);
+
+		efuse_one_byte_read(hw, tmpaddr, &tmpdata[4]);
+		efuse_one_byte_read(hw, tmpaddr + 1, &tmpdata[5]);
+		if ((data[4] != tmpdata[4]) || (data[5] != tmpdata[5]))
+			badworden &= (~BIT(2));
+	}
+
+	if (!(word_en & BIT(3))) {
+		tmpaddr = start_addr;
+		efuse_one_byte_write(hw, start_addr++, data[6]);
+		efuse_one_byte_write(hw, start_addr++, data[7]);
+
+		efuse_one_byte_read(hw, tmpaddr, &tmpdata[6]);
+		efuse_one_byte_read(hw, tmpaddr + 1, &tmpdata[7]);
+		if ((data[6] != tmpdata[6]) || (data[7] != tmpdata[7]))
+			badworden &= (~BIT(3));
+	}
+
+	return badworden;
+}
+
+static void efuse_power_switch(struct ieee80211_hw *hw, u8 bwrite, u8 pwrstate)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 tempval;
+	u16 tmpV16;
+
+	if(rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+	{
+		if (pwrstate == true)
+		{
+			rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_ACCESS], 0x69);
+
+			// 1.2V Power: From VDDON with Power Cut(0x0000h[15]), defualt valid
+			tmpV16 = rtl_read_word(rtlpriv,
+					       rtlpriv->cfg->maps[SYS_ISO_CTRL]);
+
+			printk("SYS_ISO_CTRL=%04x.\n",tmpV16);
+			if( ! (tmpV16 & PWC_EV12V ) ){
+				tmpV16 |= PWC_EV12V ;
+				 //PlatformEFIOWrite2Byte(pAdapter,REG_SYS_ISO_CTRL,tmpV16);
+			}
+			// Reset: 0x0000h[28], default valid
+ 			tmpV16 = rtl_read_word(rtlpriv,  rtlpriv->cfg->maps[SYS_FUNC_EN]);
+			printk("SYS_FUNC_EN=%04x.\n",tmpV16);
+			if( !(tmpV16 & FEN_ELDR) ){
+				tmpV16 |= FEN_ELDR ;
+				rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[SYS_FUNC_EN], tmpV16);
+			}
+
+			// Clock: Gated(0x0008h[5]) 8M(0x0008h[1]) clock from ANA, default valid
+			tmpV16 = rtl_read_word(rtlpriv,  rtlpriv->cfg->maps[SYS_CLK] );
+			printk("SYS_CLK=%04x.\n",tmpV16);
+			if( (!(tmpV16 & LOADER_CLK_EN) )  ||(!(tmpV16 & ANA8M) ) )
+			{
+				tmpV16 |= (LOADER_CLK_EN |ANA8M ) ;
+				rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[SYS_CLK], tmpV16);
+			}
+
+			if(bwrite == true)
+			{
+				// Enable LDO 2.5V before read/write action
+				tempval = rtl_read_word(rtlpriv,  rtlpriv->cfg->maps[EFUSE_TEST] + 3);
+				printk("EFUSE_TEST=%04x.\n",tmpV16);
+				tempval &= ~(BIT(3) | BIT(4) |BIT(5) | BIT(6));
+				tempval |= (VOLTAGE_V25 << 3);
+				tempval |= BIT(7);
+				rtl_write_byte(rtlpriv,  rtlpriv->cfg->maps[EFUSE_TEST] + 3, tempval);
+			}
+		}
+		else
+		{
+			rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_ACCESS], 0x00);
+			if(bwrite == true){
+				// Disable LDO 2.5V after read/write action
+				tempval = rtl_read_word(rtlpriv,  rtlpriv->cfg->maps[EFUSE_TEST] + 3);
+				rtl_write_byte(rtlpriv,  rtlpriv->cfg->maps[EFUSE_TEST] + 3, (tempval & 0x7F));
+			}
+		}
+	}
+	else
+	{
+		if (pwrstate == true && (rtlhal->hw_type !=
+			HARDWARE_TYPE_RTL8192SE)) {
+
+			if(rtlhal->hw_type == HARDWARE_TYPE_RTL8188EE)
+				rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_ACCESS],
+						0x69);
+
+			tmpV16 = rtl_read_word(rtlpriv,
+					       rtlpriv->cfg->maps[SYS_ISO_CTRL]);
+			if (!(tmpV16 & rtlpriv->cfg->maps[EFUSE_PWC_EV12V])) {
+				tmpV16 |= rtlpriv->cfg->maps[EFUSE_PWC_EV12V];
+				rtl_write_word(rtlpriv,
+					       rtlpriv->cfg->maps[SYS_ISO_CTRL],
+					       tmpV16);
+			}
+
+			tmpV16 = rtl_read_word(rtlpriv,
+					       rtlpriv->cfg->maps[SYS_FUNC_EN]);
+			if (!(tmpV16 & rtlpriv->cfg->maps[EFUSE_FEN_ELDR])) {
+				tmpV16 |= rtlpriv->cfg->maps[EFUSE_FEN_ELDR];
+				rtl_write_word(rtlpriv,
+					       rtlpriv->cfg->maps[SYS_FUNC_EN], tmpV16);
+			}
+
+			tmpV16 = rtl_read_word(rtlpriv, rtlpriv->cfg->maps[SYS_CLK]);
+			if ((!(tmpV16 & rtlpriv->cfg->maps[EFUSE_LOADER_CLK_EN])) ||
+			    (!(tmpV16 & rtlpriv->cfg->maps[EFUSE_ANA8M]))) {
+				tmpV16 |= (rtlpriv->cfg->maps[EFUSE_LOADER_CLK_EN] |
+					   rtlpriv->cfg->maps[EFUSE_ANA8M]);
+				rtl_write_word(rtlpriv,
+					       rtlpriv->cfg->maps[SYS_CLK], tmpV16);
+			}
+		}
+
+		if (pwrstate == true) {
+			if (bwrite == true) {
+				tempval = rtl_read_byte(rtlpriv,
+							rtlpriv->cfg->maps[EFUSE_TEST] +
+							3);
+
+				if (rtlhal->hw_type != HARDWARE_TYPE_RTL8192SE) {
+					tempval &= 0x0F;
+					tempval |= (VOLTAGE_V25 << 4);
+				}
+
+				rtl_write_byte(rtlpriv,
+					       rtlpriv->cfg->maps[EFUSE_TEST] + 3,
+					       (tempval | 0x80));
+			}
+
+			if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192SE) {
+				rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CLK],
+							0x03);
+			}
+
+		} else {
+			if(rtlhal->hw_type == HARDWARE_TYPE_RTL8188EE)
+				rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_ACCESS], 0);
+
+			if (bwrite == true) {
+				tempval = rtl_read_byte(rtlpriv,
+							rtlpriv->cfg->maps[EFUSE_TEST] +
+							3);
+				rtl_write_byte(rtlpriv,
+					       rtlpriv->cfg->maps[EFUSE_TEST] + 3,
+					       (tempval & 0x7F));
+			}
+
+			if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192SE) {
+				rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CLK],
+							0x02);
+			}
+
+		}
+	}
+
+}
+
+static u16 efuse_get_current_size(struct ieee80211_hw *hw)
+{
+	int bcontinual = true;
+	u16 efuse_addr = 0;
+	u8 hoffset, hworden;
+	u8 efuse_data, word_cnts;
+
+	while (bcontinual && efuse_one_byte_read(hw, efuse_addr, &efuse_data)
+	       && (efuse_addr < EFUSE_MAX_SIZE)) {
+		if (efuse_data != 0xFF) {
+			hoffset = (efuse_data >> 4) & 0x0F;
+			hworden = efuse_data & 0x0F;
+			word_cnts = efuse_calculate_word_cnts(hworden);
+			efuse_addr = efuse_addr + (word_cnts * 2) + 1;
+		} else {
+			bcontinual = false;
+		}
+	}
+
+	return efuse_addr;
+}
+
+static u8 efuse_calculate_word_cnts(u8 word_en)
+{
+	u8 word_cnts = 0;
+	if (!(word_en & BIT(0)))
+		word_cnts++;
+	if (!(word_en & BIT(1)))
+		word_cnts++;
+	if (!(word_en & BIT(2)))
+		word_cnts++;
+	if (!(word_en & BIT(3)))
+		word_cnts++;
+	return word_cnts;
+}
+
diff --git a/drivers/staging/rtl8821ae/efuse.h b/drivers/staging/rtl8821ae/efuse.h
new file mode 100644
index 000000000000..a9fcbe05cf9a
--- /dev/null
+++ b/drivers/staging/rtl8821ae/efuse.h
@@ -0,0 +1,130 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL_EFUSE_H_
+#define __RTL_EFUSE_H_
+
+#define EFUSE_IC_ID_OFFSET		506
+
+/*
+#define EFUSE_REAL_CONTENT_LEN	512
+#define EFUSE_MAP_LEN			128
+#define EFUSE_MAX_SECTION		16
+#define EFUSE_MAX_WORD_UNIT		4
+#define EFUSE_IC_ID_OFFSET		506
+*/
+
+#define EFUSE_MAX_WORD_UNIT		4
+
+#define EFUSE_INIT_MAP			0
+#define EFUSE_MODIFY_MAP		1
+
+#define PG_STATE_HEADER 		0x01
+#define PG_STATE_WORD_0			0x02
+#define PG_STATE_WORD_1			0x04
+#define PG_STATE_WORD_2			0x08
+#define PG_STATE_WORD_3			0x10
+#define PG_STATE_DATA			0x20
+
+#define PG_SWBYTE_H			0x01
+#define PG_SWBYTE_L			0x02
+
+#define _POWERON_DELAY_
+#define _PRE_EXECUTE_READ_CMD_
+
+#define EFUSE_REPEAT_THRESHOLD_		3
+#define EFUSE_ERROE_HANDLE		1
+
+struct efuse_map {
+	u8 offset;
+	u8 word_start;
+	u8 byte_start;
+	u8 byte_cnts;
+};
+
+struct pgpkt_struct {
+	u8 offset;
+	u8 word_en;
+	u8 data[8];
+};
+
+enum efuse_data_item {
+	EFUSE_CHIP_ID = 0,
+	EFUSE_LDO_SETTING,
+	EFUSE_CLK_SETTING,
+	EFUSE_SDIO_SETTING,
+	EFUSE_CCCR,
+	EFUSE_SDIO_MODE,
+	EFUSE_OCR,
+	EFUSE_F0CIS,
+	EFUSE_F1CIS,
+	EFUSE_MAC_ADDR,
+	EFUSE_EEPROM_VER,
+	EFUSE_CHAN_PLAN,
+	EFUSE_TXPW_TAB
+};
+
+enum {
+	VOLTAGE_V25 = 0x03,
+	LDOE25_SHIFT = 28,
+};
+
+struct efuse_priv {
+	u8 id[2];
+	u8 ldo_setting[2];
+	u8 clk_setting[2];
+	u8 cccr;
+	u8 sdio_mode;
+	u8 ocr[3];
+	u8 cis0[17];
+	u8 cis1[48];
+	u8 mac_addr[6];
+	u8 eeprom_verno;
+	u8 channel_plan;
+	u8 tx_power_b[14];
+	u8 tx_power_g[14];
+};
+
+extern void read_efuse_byte(struct ieee80211_hw *hw, u16 _offset, u8 *pbuf);
+extern void efuse_initialize(struct ieee80211_hw *hw);
+extern u8 efuse_read_1byte(struct ieee80211_hw *hw, u16 address);
+extern int efuse_one_byte_read(struct ieee80211_hw *hw, u16 addr, u8 *data);
+extern void efuse_write_1byte(struct ieee80211_hw *hw, u16 address, u8 value);
+extern void read_efuse(struct ieee80211_hw *hw, u16 _offset,
+		       u16 _size_byte, u8 * pbuf);
+extern void efuse_shadow_read(struct ieee80211_hw *hw, u8 type,
+			      u16 offset, u32 * value);
+extern void efuse_shadow_write(struct ieee80211_hw *hw, u8 type,
+			       u16 offset, u32 value);
+extern bool efuse_shadow_update(struct ieee80211_hw *hw);
+extern bool efuse_shadow_update_chk(struct ieee80211_hw *hw);
+extern void rtl_efuse_shadow_map_update(struct ieee80211_hw *hw);
+extern void efuse_force_write_vendor_Id(struct ieee80211_hw *hw);
+extern void efuse_re_pg_section(struct ieee80211_hw *hw, u8 section_idx);
+#endif
diff --git a/drivers/staging/rtl8821ae/pci.c b/drivers/staging/rtl8821ae/pci.c
new file mode 100644
index 000000000000..cfa651edd238
--- /dev/null
+++ b/drivers/staging/rtl8821ae/pci.c
@@ -0,0 +1,2549 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "core.h"
+#include "wifi.h"
+#include "pci.h"
+#include "base.h"
+#include "ps.h"
+#include "efuse.h"
+#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,
+	ATI_VENDOR_ID,
+	AMD_VENDOR_ID,
+	SIS_VENDOR_ID
+};
+
+static const u8 ac_to_hwq[] = {
+	VO_QUEUE,
+	VI_QUEUE,
+	BE_QUEUE,
+	BK_QUEUE
+};
+
+u8 _rtl_mac_to_hwqueue(struct ieee80211_hw *hw,
+		struct sk_buff *skb)
+{
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u16 fc = rtl_get_fc(skb);
+	u8 queue_index = skb_get_queue_mapping(skb);
+
+	if (unlikely(ieee80211_is_beacon(fc)))
+		return BEACON_QUEUE;
+	if (ieee80211_is_mgmt(fc) || ieee80211_is_ctl(fc))
+		return MGNT_QUEUE;
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192SE)
+		if (ieee80211_is_nullfunc(fc))
+			return HIGH_QUEUE;
+
+	return ac_to_hwq[queue_index];
+}
+
+/* Update PCI dependent default settings*/
+static void _rtl_pci_update_default_setting(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u8 pcibridge_vendor = pcipriv->ndis_adapter.pcibridge_vendor;
+	u8 init_aspm;
+
+	ppsc->reg_rfps_level = 0;
+	ppsc->b_support_aspm = 0;
+
+	/*Update PCI ASPM setting */
+	ppsc->const_amdpci_aspm = rtlpci->const_amdpci_aspm;
+	switch (rtlpci->const_pci_aspm) {
+	case 0:
+		/*No ASPM */
+		break;
+
+	case 1:
+		/*ASPM dynamically enabled/disable. */
+		ppsc->reg_rfps_level |= RT_RF_LPS_LEVEL_ASPM;
+		break;
+
+	case 2:
+		/*ASPM with Clock Req dynamically enabled/disable. */
+		ppsc->reg_rfps_level |= (RT_RF_LPS_LEVEL_ASPM |
+					 RT_RF_OFF_LEVL_CLK_REQ);
+		break;
+
+	case 3:
+		/*
+		 * Always enable ASPM and Clock Req
+		 * from initialization to halt.
+		 * */
+		ppsc->reg_rfps_level &= ~(RT_RF_LPS_LEVEL_ASPM);
+		ppsc->reg_rfps_level |= (RT_RF_PS_LEVEL_ALWAYS_ASPM |
+					 RT_RF_OFF_LEVL_CLK_REQ);
+		break;
+
+	case 4:
+		/*
+		 * Always enable ASPM without Clock Req
+		 * from initialization to halt.
+		 * */
+		ppsc->reg_rfps_level &= ~(RT_RF_LPS_LEVEL_ASPM |
+					  RT_RF_OFF_LEVL_CLK_REQ);
+		ppsc->reg_rfps_level |= RT_RF_PS_LEVEL_ALWAYS_ASPM;
+		break;
+	}
+
+	ppsc->reg_rfps_level |= RT_RF_OFF_LEVL_HALT_NIC;
+
+	/*Update Radio OFF setting */
+	switch (rtlpci->const_hwsw_rfoff_d3) {
+	case 1:
+		if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM)
+			ppsc->reg_rfps_level |= RT_RF_OFF_LEVL_ASPM;
+		break;
+
+	case 2:
+		if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM)
+			ppsc->reg_rfps_level |= RT_RF_OFF_LEVL_ASPM;
+		ppsc->reg_rfps_level |= RT_RF_OFF_LEVL_HALT_NIC;
+		break;
+
+	case 3:
+		ppsc->reg_rfps_level |= RT_RF_OFF_LEVL_PCI_D3;
+		break;
+	}
+
+	/*Set HW definition to determine if it supports ASPM. */
+	switch (rtlpci->const_support_pciaspm) {
+	case 0:{
+			/*Not support ASPM. */
+			bool b_support_aspm = false;
+			ppsc->b_support_aspm = b_support_aspm;
+			break;
+		}
+	case 1:{
+			/*Support ASPM. */
+			bool b_support_aspm = true;
+			bool b_support_backdoor = true;
+			ppsc->b_support_aspm = b_support_aspm;
+
+			/*if(priv->oem_id == RT_CID_TOSHIBA &&
+			   !priv->ndis_adapter.amd_l1_patch)
+			   b_support_backdoor = false; */
+
+			ppsc->b_support_backdoor = b_support_backdoor;
+
+			break;
+		}
+	case 2:
+		/*ASPM value set by chipset. */
+		if (pcibridge_vendor == PCI_BRIDGE_VENDOR_INTEL) {
+			bool b_support_aspm = true;
+			ppsc->b_support_aspm = b_support_aspm;
+		}
+		break;
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("switch case not process \n"));
+		break;
+	}
+
+	/* toshiba aspm issue, toshiba will set aspm selfly
+	 * so we should not set aspm in driver */
+	pci_read_config_byte(rtlpci->pdev, 0x80, &init_aspm);
+	if (rtlpriv->rtlhal.hw_type == HARDWARE_TYPE_RTL8192SE &&
+		init_aspm == 0x43)
+		ppsc->b_support_aspm = false;
+}
+
+static bool _rtl_pci_platform_switch_device_pci_aspm(struct ieee80211_hw *hw,
+						     u8 value)
+{
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool bresult = false;
+
+	if (rtlhal->hw_type != HARDWARE_TYPE_RTL8192SE)
+		value |= 0x40;
+
+	pci_write_config_byte(rtlpci->pdev, 0x80, value);
+
+	return bresult;
+}
+
+/*When we set 0x01 to enable clk request. Set 0x0 to disable clk req.*/
+static bool _rtl_pci_switch_clk_req(struct ieee80211_hw *hw, u8 value)
+{
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool bresult = false;
+
+	pci_write_config_byte(rtlpci->pdev, 0x81, value);
+	bresult = true;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192SE)
+		udelay(100);
+
+	return bresult;
+}
+
+/*Disable RTL8192SE ASPM & Disable Pci Bridge ASPM*/
+static void rtl_pci_disable_aspm(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u8 pcibridge_vendor = pcipriv->ndis_adapter.pcibridge_vendor;
+	u32 pcicfg_addrport = pcipriv->ndis_adapter.pcicfg_addrport;
+	u8 num4bytes = pcipriv->ndis_adapter.num4bytes;
+	/*Retrieve original configuration settings. */
+	u8 linkctrl_reg = pcipriv->ndis_adapter.linkctrl_reg;
+	u16 pcibridge_linkctrlreg = pcipriv->ndis_adapter.
+				pcibridge_linkctrlreg;
+	u16 aspmlevel = 0;
+
+	if (!ppsc->b_support_aspm)
+		return;
+
+	if (pcibridge_vendor == PCI_BRIDGE_VENDOR_UNKNOWN) {
+		RT_TRACE(COMP_POWER, DBG_TRACE,
+			 ("PCI(Bridge) UNKNOWN.\n"));
+
+		return;
+	}
+
+	if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_CLK_REQ) {
+		RT_CLEAR_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_CLK_REQ);
+		_rtl_pci_switch_clk_req(hw, 0x0);
+	}
+
+	if (1) {
+		/*for promising device will in L0 state after an I/O. */
+		u8 tmp_u1b;
+		pci_read_config_byte(rtlpci->pdev, 0x80, &tmp_u1b);
+	}
+
+	/*Set corresponding value. */
+	aspmlevel |= BIT(0) | BIT(1);
+	linkctrl_reg &= ~aspmlevel;
+	pcibridge_linkctrlreg &= ~(BIT(0) | BIT(1));
+
+	_rtl_pci_platform_switch_device_pci_aspm(hw, linkctrl_reg);
+	udelay(50);
+
+	/*4 Disable Pci Bridge ASPM */
+	rtl_pci_raw_write_port_ulong(PCI_CONF_ADDRESS,
+				     pcicfg_addrport + (num4bytes << 2));
+	rtl_pci_raw_write_port_uchar(PCI_CONF_DATA, pcibridge_linkctrlreg);
+
+	udelay(50);
+
+}
+
+/*
+ *Enable RTL8192SE ASPM & Enable Pci Bridge ASPM for
+ *power saving We should follow the sequence to enable
+ *RTL8192SE first then enable Pci Bridge ASPM
+ *or the system will show bluescreen.
+ */
+static void rtl_pci_enable_aspm(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u8 pcibridge_vendor = pcipriv->ndis_adapter.pcibridge_vendor;
+	u32 pcicfg_addrport = pcipriv->ndis_adapter.pcicfg_addrport;
+	u8 num4bytes = pcipriv->ndis_adapter.num4bytes;
+	u16 aspmlevel;
+	u8 u_pcibridge_aspmsetting;
+	u8 u_device_aspmsetting;
+
+	if (!ppsc->b_support_aspm)
+		return;
+
+	if (pcibridge_vendor == PCI_BRIDGE_VENDOR_UNKNOWN) {
+		RT_TRACE(COMP_POWER, DBG_TRACE,
+			 ("PCI(Bridge) UNKNOWN.\n"));
+		return;
+	}
+
+	/*4 Enable Pci Bridge ASPM */
+	rtl_pci_raw_write_port_ulong(PCI_CONF_ADDRESS,
+				     pcicfg_addrport + (num4bytes << 2));
+
+	u_pcibridge_aspmsetting =
+	    pcipriv->ndis_adapter.pcibridge_linkctrlreg |
+	    rtlpci->const_hostpci_aspm_setting;
+
+	if (pcibridge_vendor == PCI_BRIDGE_VENDOR_INTEL)
+		u_pcibridge_aspmsetting &= ~BIT(0);
+
+	rtl_pci_raw_write_port_uchar(PCI_CONF_DATA, u_pcibridge_aspmsetting);
+
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("PlatformEnableASPM(): Write reg[%x] = %x\n",
+		  (pcipriv->ndis_adapter.pcibridge_pciehdr_offset + 0x10),
+		  u_pcibridge_aspmsetting));
+
+	udelay(50);
+
+	/*Get ASPM level (with/without Clock Req) */
+	aspmlevel = rtlpci->const_devicepci_aspm_setting;
+	u_device_aspmsetting = pcipriv->ndis_adapter.linkctrl_reg;
+
+	/*_rtl_pci_platform_switch_device_pci_aspm(dev,*/
+	/*(priv->ndis_adapter.linkctrl_reg | ASPMLevel)); */
+
+	u_device_aspmsetting |= aspmlevel;
+
+	_rtl_pci_platform_switch_device_pci_aspm(hw, u_device_aspmsetting);
+
+	if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_CLK_REQ) {
+		_rtl_pci_switch_clk_req(hw, (ppsc->reg_rfps_level &
+					     RT_RF_OFF_LEVL_CLK_REQ) ? 1 : 0);
+		RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_CLK_REQ);
+	}
+	udelay(100);
+}
+
+static bool rtl_pci_get_amd_l1_patch(struct ieee80211_hw *hw)
+{
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	u32 pcicfg_addrport = pcipriv->ndis_adapter.pcicfg_addrport;
+
+	bool status = false;
+	u8 offset_e0;
+	unsigned offset_e4;
+
+	rtl_pci_raw_write_port_ulong(PCI_CONF_ADDRESS,
+			pcicfg_addrport + 0xE0);
+	rtl_pci_raw_write_port_uchar(PCI_CONF_DATA, 0xA0);
+
+	rtl_pci_raw_write_port_ulong(PCI_CONF_ADDRESS,
+			pcicfg_addrport + 0xE0);
+	rtl_pci_raw_read_port_uchar(PCI_CONF_DATA, &offset_e0);
+
+	if (offset_e0 == 0xA0) {
+		rtl_pci_raw_write_port_ulong(PCI_CONF_ADDRESS,
+					     pcicfg_addrport + 0xE4);
+		rtl_pci_raw_read_port_ulong(PCI_CONF_DATA, &offset_e4);
+		if (offset_e4 & BIT(23))
+			status = true;
+	}
+
+	return status;
+}
+
+/*<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)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	bool b_find_buddy_priv = false;
+	struct rtl_priv *temp_priv = NULL;
+	struct rtl_pci_priv *temp_pcipriv = NULL;
+
+	if (!list_empty(&rtlpriv->glb_var->glb_priv_list)) {
+		list_for_each_entry(temp_priv, &rtlpriv->glb_var->glb_priv_list,
+			list) {
+			if (temp_priv) {
+				temp_pcipriv = 
+					(struct rtl_pci_priv *)temp_priv->priv;
+				RT_TRACE(COMP_INIT, DBG_LOUD,
+					(("pcipriv->ndis_adapter.funcnumber %x \n"),
+					pcipriv->ndis_adapter.funcnumber));
+				RT_TRACE(COMP_INIT, DBG_LOUD,
+					(("temp_pcipriv->ndis_adapter.funcnumber %x \n"),
+					temp_pcipriv->ndis_adapter.funcnumber));
+
+				if ((pcipriv->ndis_adapter.busnumber ==
+					temp_pcipriv->ndis_adapter.busnumber) &&
+				    (pcipriv->ndis_adapter.devnumber ==
+				    temp_pcipriv->ndis_adapter.devnumber) &&
+				    (pcipriv->ndis_adapter.funcnumber !=
+				    temp_pcipriv->ndis_adapter.funcnumber)) {
+					b_find_buddy_priv = true;
+					break;
+				}
+			}
+		}
+	}
+
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		(("b_find_buddy_priv %d \n"), b_find_buddy_priv));
+
+	if (b_find_buddy_priv)
+		*buddy_priv = temp_priv;
+
+	return b_find_buddy_priv;
+}
+
+void rtl_pci_get_linkcontrol_field(struct ieee80211_hw *hw)
+{
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	u8 capabilityoffset = pcipriv->ndis_adapter.pcibridge_pciehdr_offset;
+	u32 pcicfg_addrport = pcipriv->ndis_adapter.pcicfg_addrport;
+	u8 linkctrl_reg;
+	u8 num4bbytes;
+
+	num4bbytes = (capabilityoffset + 0x10) / 4;
+
+	/*Read  Link Control Register */
+	rtl_pci_raw_write_port_ulong(PCI_CONF_ADDRESS,
+				     pcicfg_addrport + (num4bbytes << 2));
+	rtl_pci_raw_read_port_uchar(PCI_CONF_DATA, &linkctrl_reg);
+
+	pcipriv->ndis_adapter.pcibridge_linkctrlreg = linkctrl_reg;
+}
+
+static void rtl_pci_parse_configuration(struct pci_dev *pdev,
+					struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+
+	u8 tmp;
+	int pos;
+	u8 linkctrl_reg;
+
+	/*Link Control Register */
+	pos = pci_find_capability(pdev, PCI_CAP_ID_EXP);
+	pci_read_config_byte(pdev, pos + PCI_EXP_LNKCTL, &linkctrl_reg);
+	pcipriv->ndis_adapter.linkctrl_reg = linkctrl_reg;
+
+	RT_TRACE(COMP_INIT, DBG_TRACE,
+		 ("Link Control Register =%x\n",
+		  pcipriv->ndis_adapter.linkctrl_reg));
+
+	pci_read_config_byte(pdev, 0x98, &tmp);
+	tmp |= BIT(4);
+	pci_write_config_byte(pdev, 0x98, tmp);
+
+	tmp = 0x17;
+	pci_write_config_byte(pdev, 0x70f, tmp);
+}
+
+static void rtl_pci_init_aspm(struct ieee80211_hw *hw)
+{
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+
+	_rtl_pci_update_default_setting(hw);
+
+	if (ppsc->reg_rfps_level & RT_RF_PS_LEVEL_ALWAYS_ASPM) {
+		/*Always enable ASPM & Clock Req. */
+		rtl_pci_enable_aspm(hw);
+		RT_SET_PS_LEVEL(ppsc, RT_RF_PS_LEVEL_ALWAYS_ASPM);
+	}
+
+}
+
+static void _rtl_pci_io_handler_init(struct device *dev,
+				     struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->io.dev = dev;
+
+	rtlpriv->io.write8_async = pci_write8_async;
+	rtlpriv->io.write16_async = pci_write16_async;
+	rtlpriv->io.write32_async = pci_write32_async;
+
+	rtlpriv->io.read8_sync = pci_read8_sync;
+	rtlpriv->io.read16_sync = pci_read16_sync;
+	rtlpriv->io.read32_sync = pci_read32_sync;
+
+}
+
+static bool _rtl_pci_update_earlymode_info(struct ieee80211_hw *hw,
+					   struct sk_buff *skb, 
+					   struct rtl_tcb_desc *tcb_desc, 
+					   u8 tid)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 additionlen = FCS_LEN;
+	struct sk_buff *next_skb;
+		
+	/* here open is 4, wep/tkip is 8, aes is 12*/
+	if (info->control.hw_key)
+		additionlen += info->control.hw_key->icv_len;
+
+	/* The most skb num is 6 */
+	tcb_desc->empkt_num = 0;
+	spin_lock_bh(&rtlpriv->locks.waitq_lock);
+	skb_queue_walk(&rtlpriv->mac80211.skb_waitq[tid], next_skb) {
+		struct ieee80211_tx_info *next_info = 
+					IEEE80211_SKB_CB(next_skb);
+		if (next_info->flags & IEEE80211_TX_CTL_AMPDU) {
+			tcb_desc->empkt_len[tcb_desc->empkt_num] =
+				next_skb->len + additionlen;
+			tcb_desc->empkt_num++;
+		} else {
+			break;
+		}
+
+		if (skb_queue_is_last(&rtlpriv->mac80211.skb_waitq[tid], 
+				      next_skb))
+			break;
+
+		if (tcb_desc->empkt_num >= rtlhal->max_earlymode_num)
+			break;
+	}
+	spin_unlock_bh(&rtlpriv->locks.waitq_lock);
+	return true;
+}
+
+/* just for early mode now */
+static void _rtl_pci_tx_chk_waitq(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct sk_buff *skb = NULL;
+	struct ieee80211_tx_info *info = NULL;
+	int tid; /* should be int */
+	
+	if (!rtlpriv->rtlhal.b_earlymode_enable)
+		return;	
+	if (rtlpriv->dm.supp_phymode_switch &&
+		(rtlpriv->easy_concurrent_ctl.bswitch_in_process ||
+		(rtlpriv->buddy_priv && 
+		 rtlpriv->buddy_priv->easy_concurrent_ctl.bswitch_in_process)))
+		return;
+	/* we juse use em for BE/BK/VI/VO */
+	for (tid = 7; tid >= 0; tid--) {
+		u8 hw_queue = ac_to_hwq[rtl_tid_to_ac(hw, tid)];
+		struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[hw_queue];
+		while (!mac->act_scanning && 
+		       rtlpriv->psc.rfpwr_state == ERFON) {
+			struct rtl_tcb_desc tcb_desc;
+			memset(&tcb_desc, 0, sizeof(struct rtl_tcb_desc));			
+
+			spin_lock_bh(&rtlpriv->locks.waitq_lock);
+			if (!skb_queue_empty(&mac->skb_waitq[tid]) &&
+			    (ring->entries - skb_queue_len(&ring->queue) > 
+			     rtlhal->max_earlymode_num)) {
+				skb = skb_dequeue(&mac->skb_waitq[tid]);
+			} else {
+				spin_unlock_bh(&rtlpriv->locks.waitq_lock);
+				break;
+			}
+			spin_unlock_bh(&rtlpriv->locks.waitq_lock);
+
+			/* Some macaddr can't do early mode. like
+			 * multicast/broadcast/no_qos data */
+			info = IEEE80211_SKB_CB(skb);
+			if (info->flags & IEEE80211_TX_CTL_AMPDU)
+				_rtl_pci_update_earlymode_info(hw, skb, 
+							       &tcb_desc, tid);
+
+/*<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>*/
+		}
+	}
+}
+
+static void _rtl_pci_tx_isr(struct ieee80211_hw *hw, int prio)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[prio];
+
+	while (skb_queue_len(&ring->queue)) {
+		struct sk_buff *skb;
+		struct ieee80211_tx_info *info;
+		u16 fc;
+		u8 tid;
+		u8 *entry;
+
+		
+		if (rtlpriv->use_new_trx_flow)
+			entry = (u8 *)(&ring->buffer_desc[ring->idx]);
+		else
+			entry = (u8 *)(&ring->desc[ring->idx]);
+
+		if (!rtlpriv->cfg->ops->is_tx_desc_closed(hw, prio, ring->idx))
+			return;
+			
+		ring->idx = (ring->idx + 1) % ring->entries;
+		
+		skb = __skb_dequeue(&ring->queue);
+		
+		pci_unmap_single(rtlpci->pdev,
+				 le32_to_cpu(rtlpriv->cfg->ops->
+					     get_desc((u8 *) entry, true,
+						      HW_DESC_TXBUFF_ADDR)),
+				 skb->len, PCI_DMA_TODEVICE);
+
+		/* remove early mode header */
+		if(rtlpriv->rtlhal.b_earlymode_enable)
+			skb_pull(skb, EM_HDR_LEN);
+
+		RT_TRACE((COMP_INTR | COMP_SEND), DBG_TRACE,
+			 ("new ring->idx:%d, "
+			  "free: skb_queue_len:%d, free: seq:%d\n",
+			  ring->idx,
+			  skb_queue_len(&ring->queue),
+			  *(u16 *) (skb->data + 22)));
+
+		if(prio == TXCMD_QUEUE) {
+			dev_kfree_skb(skb);
+			goto tx_status_ok;
+
+		}
+
+		/* for sw LPS, just after NULL skb send out, we can
+		 * sure AP kown we are sleeped, our we should not let
+		 * rf to sleep*/
+		fc = rtl_get_fc(skb);
+		if (ieee80211_is_nullfunc(fc)) {
+			if(ieee80211_has_pm(fc)) {
+				rtlpriv->mac80211.offchan_deley = true;
+				rtlpriv->psc.state_inap = 1;
+			} else {
+				rtlpriv->psc.state_inap = 0;
+			}
+		}
+		if (ieee80211_is_action(fc)) {
+			struct ieee80211_mgmt_compat *action_frame =
+				(struct ieee80211_mgmt_compat *)skb->data;
+			if (action_frame->u.action.u.ht_smps.action ==
+				WLAN_HT_ACTION_SMPS) {
+				dev_kfree_skb(skb);
+				goto tx_status_ok;
+			}
+		}
+
+		/* update tid tx pkt num */
+		tid = rtl_get_tid(skb);
+		if (tid <= 7)
+			rtlpriv->link_info.tidtx_inperiod[tid]++;
+
+		info = IEEE80211_SKB_CB(skb);
+		ieee80211_tx_info_clear_status(info);
+
+		info->flags |= IEEE80211_TX_STAT_ACK;
+		/*info->status.rates[0].count = 1; */
+
+		ieee80211_tx_status_irqsafe(hw, skb);
+
+		if ((ring->entries - skb_queue_len(&ring->queue))
+				== 2) {
+
+			RT_TRACE(COMP_ERR, DBG_LOUD,
+					("more desc left, wake"
+					 "skb_queue@%d,ring->idx = %d,"
+					 "skb_queue_len = 0x%d\n",
+					 prio, ring->idx,
+					 skb_queue_len(&ring->queue)));
+
+			ieee80211_wake_queue(hw,
+					skb_get_queue_mapping
+					(skb));
+		}
+tx_status_ok:
+		skb = NULL;
+	}
+
+	if (((rtlpriv->link_info.num_rx_inperiod +
+		rtlpriv->link_info.num_tx_inperiod) > 8) ||
+		(rtlpriv->link_info.num_rx_inperiod > 2)) {
+		rtl_lps_leave(hw);
+	}
+}
+
+static int _rtl_pci_init_one_rxdesc(struct ieee80211_hw *hw,
+	u8 *entry, int rxring_idx, int desc_idx)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u32 bufferaddress;
+	u8 tmp_one = 1;
+	struct sk_buff *skb;
+
+	skb = dev_alloc_skb(rtlpci->rxbuffersize);	
+	if (!skb)
+		return 0;
+	rtlpci->rx_ring[rxring_idx].rx_buf[desc_idx] = skb;
+
+	/* just set skb->cb to mapping addr
+	 * for pci_unmap_single use */
+	*((dma_addr_t *) skb->cb) = pci_map_single(rtlpci->pdev,
+				skb_tail_pointer(skb), rtlpci->rxbuffersize,
+				PCI_DMA_FROMDEVICE);
+	bufferaddress = cpu_to_le32(*((dma_addr_t *) skb->cb));
+	if (pci_dma_mapping_error(rtlpci->pdev, bufferaddress))
+		return 0;
+	if (rtlpriv->use_new_trx_flow) {
+		rtlpriv->cfg->ops->set_desc(hw, (u8 *) entry, false, 
+					    HW_DESC_RX_PREPARE,
+					    (u8 *) & bufferaddress);
+	} else {	
+		rtlpriv->cfg->ops->set_desc(hw, (u8 *) entry, false, 
+					    HW_DESC_RXBUFF_ADDR,
+					    (u8 *) & bufferaddress);
+		rtlpriv->cfg->ops->set_desc(hw, (u8 *) entry, false, 
+					    HW_DESC_RXPKT_LEN,
+					    (u8 *) & rtlpci->rxbuffersize);
+		rtlpriv->cfg->ops->set_desc(hw, (u8 *) entry, false, 
+					    HW_DESC_RXOWN,
+					    (u8 *) & tmp_one);
+	}
+	
+	return 1;
+}
+
+/* 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(!rtl_action_proc(hw, skb, false))) {
+		dev_kfree_skb_any(skb);
+	} else {
+		struct sk_buff *uskb = NULL;
+		u8 *pdata;
+		
+		uskb = dev_alloc_skb(skb->len + 128);
+		if (likely(uskb)) {
+			memcpy(IEEE80211_SKB_RXCB(uskb), &rx_status,
+					sizeof(rx_status));
+			pdata = (u8 *)skb_put(uskb, skb->len);
+			memcpy(pdata, skb->data, skb->len);
+			dev_kfree_skb_any(skb);
+
+			ieee80211_rx_irqsafe(hw, uskb);
+		} else {
+			ieee80211_rx_irqsafe(hw, skb);
+		}	
+	}
+}
+
+/*hsisr interrupt handler*/
+static void _rtl_pci_hs_interrupt(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[MAC_HSISR], 
+		       rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[MAC_HSISR]) | 
+		       rtlpci->sys_irq_mask);
+
+	
+}
+static void _rtl_pci_rx_interrupt(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	int rxring_idx = RTL_PCI_RX_MPDU_QUEUE;
+
+	struct ieee80211_rx_status rx_status = { 0 };
+	unsigned int count = rtlpci->rxringcount;
+	bool unicast = false;
+	u8 hw_queue = 0;
+	unsigned int rx_remained_cnt;
+	u8 own;
+	u8 tmp_one;
+
+	struct rtl_stats status = {
+		.signal = 0,
+		.noise = -98,
+		.rate = 0,
+	};
+	
+	/*RX NORMAL PKT */
+	while (count--) {
+		struct ieee80211_hdr *hdr;
+		u16 fc;
+		u16 len;
+		/*rx buffer descriptor */
+		struct rtl_rx_buffer_desc *buffer_desc = NULL;
+		/*if use new trx flow, it means wifi info */
+		struct rtl_rx_desc *pdesc = NULL;
+		/*rx pkt */
+		struct sk_buff *skb = rtlpci->rx_ring[rxring_idx].rx_buf[
+					rtlpci->rx_ring[rxring_idx].idx];
+		
+		if (rtlpriv->use_new_trx_flow) {
+			rx_remained_cnt = 
+				rtlpriv->cfg->ops->rx_desc_buff_remained_cnt(hw,
+								      hw_queue);
+			if (rx_remained_cnt < 1) 
+				return;
+			
+		} else {	/* rx descriptor */
+			pdesc = &rtlpci->rx_ring[rxring_idx].desc[
+				rtlpci->rx_ring[rxring_idx].idx];
+			
+			own = (u8) rtlpriv->cfg->ops->get_desc((u8 *) pdesc,
+							       false, 
+							       HW_DESC_OWN);
+			if (own) /* wait data to be filled by hardware */
+				return;
+		}
+		
+		/* Get here means: data is filled already*/
+		/* AAAAAAttention !!!
+		 * We can NOT access 'skb' before 'pci_unmap_single' */
+		pci_unmap_single(rtlpci->pdev, *((dma_addr_t *) skb->cb),
+			 	 rtlpci->rxbuffersize, PCI_DMA_FROMDEVICE);
+		
+		if (rtlpriv->use_new_trx_flow) {
+			buffer_desc = &rtlpci->rx_ring[rxring_idx].buffer_desc[
+				rtlpci->rx_ring[rxring_idx].idx];
+			/*means rx wifi info*/
+			pdesc = (struct rtl_rx_desc *)skb->data;
+		}
+		
+		rtlpriv->cfg->ops->query_rx_desc(hw, &status,
+						 &rx_status, (u8 *) pdesc, skb);
+			
+		if (rtlpriv->use_new_trx_flow)
+			rtlpriv->cfg->ops->rx_check_dma_ok(hw, 
+							   (u8 *)buffer_desc, 
+							   hw_queue);
+
+			
+		len = rtlpriv->cfg->ops->get_desc((u8 *)pdesc, false, 
+						  HW_DESC_RXPKT_LEN);
+			
+		if (skb->end - skb->tail > len) {
+			skb_put(skb, len);
+			if (rtlpriv->use_new_trx_flow)	
+				skb_reserve(skb, status.rx_drvinfo_size + 
+						 status.rx_bufshift + 24);
+			else
+				skb_reserve(skb, status.rx_drvinfo_size + 
+						 status.rx_bufshift);
+
+		} else {
+			printk("skb->end - skb->tail = %d, len is %d\n", 
+			       skb->end - skb->tail, len);
+			break;
+		}
+			
+		rtlpriv->cfg->ops->rx_command_packet_handler(hw, status, skb);
+
+		/*
+		 *NOTICE This can not be use for mac80211,
+		 *this is done in mac80211 code,
+		 *if you done here sec DHCP will fail
+		 *skb_trim(skb, skb->len - 4);
+		 */
+
+		hdr = rtl_get_hdr(skb);
+		fc = rtl_get_fc(skb);
+		
+		if (!status.b_crc && !status.b_hwerror) {
+			memcpy(IEEE80211_SKB_RXCB(skb), &rx_status, 
+			       sizeof(rx_status));
+
+			if (is_broadcast_ether_addr(hdr->addr1)) {
+				;/*TODO*/
+			} else if (is_multicast_ether_addr(hdr->addr1)) {
+			 	;/*TODO*/
+			} else {
+				unicast = true;
+				rtlpriv->stats.rxbytesunicast += skb->len;
+			}
+
+			rtl_is_special_data(hw, skb, false);
+
+			if (ieee80211_is_data(fc)) {
+				rtlpriv->cfg->ops->led_control(hw, LED_CTL_RX);
+
+				if (unicast)
+					rtlpriv->link_info.num_rx_inperiod++;
+			}
+
+			/* static bcn for roaming */
+			rtl_beacon_statistic(hw, skb);
+			rtl_p2p_info(hw, (void*)skb->data, skb->len);	
+			/* for sw lps */
+			rtl_swlps_beacon(hw, (void*)skb->data, skb->len);
+			rtl_recognize_peer(hw, (void*)skb->data, skb->len);
+			if ((rtlpriv->mac80211.opmode == NL80211_IFTYPE_AP) &&
+			    (rtlpriv->rtlhal.current_bandtype == BAND_ON_2_4G)&&
+			    (ieee80211_is_beacon(fc) || 
+			     ieee80211_is_probe_resp(fc))) {
+				dev_kfree_skb_any(skb);
+			} else {
+				_rtl_pci_rx_to_mac80211(hw, skb, rx_status);
+			}
+		} else {
+			dev_kfree_skb_any(skb);
+		}
+		if (rtlpriv->use_new_trx_flow) {
+			rtlpci->rx_ring[hw_queue].next_rx_rp += 1;
+			rtlpci->rx_ring[hw_queue].next_rx_rp %= 
+							RTL_PCI_MAX_RX_COUNT;
+
+
+			rx_remained_cnt--;
+			if (1/*rx_remained_cnt == 0*/) {
+				rtl_write_word(rtlpriv, 0x3B4, 
+					rtlpci->rx_ring[hw_queue].next_rx_rp);
+			}
+		}
+		if (((rtlpriv->link_info.num_rx_inperiod +
+		      rtlpriv->link_info.num_tx_inperiod) > 8) ||
+		    (rtlpriv->link_info.num_rx_inperiod > 2)) {
+			rtl_lps_leave(hw);
+		}
+
+		if (rtlpriv->use_new_trx_flow) {
+			_rtl_pci_init_one_rxdesc(hw, (u8 *)buffer_desc, 
+						 rxring_idx,
+					       rtlpci->rx_ring[rxring_idx].idx);			
+		} else {
+			_rtl_pci_init_one_rxdesc(hw, (u8 *)pdesc, rxring_idx,
+					       rtlpci->rx_ring[rxring_idx].idx);			
+
+			if (rtlpci->rx_ring[rxring_idx].idx == 
+			    rtlpci->rxringcount - 1)
+				rtlpriv->cfg->ops->set_desc(hw, (u8 *) pdesc, 
+							    false, 
+							    HW_DESC_RXERO,
+							    (u8 *) & tmp_one);
+		}
+		rtlpci->rx_ring[rxring_idx].idx = 
+				(rtlpci->rx_ring[rxring_idx].idx + 1) % 
+				rtlpci->rxringcount;
+	}
+}
+
+static irqreturn_t _rtl_pci_interrupt(int irq, void *dev_id)
+{
+	struct ieee80211_hw *hw = dev_id;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	unsigned long flags;
+	u32 inta = 0;
+	u32 intb = 0;
+
+	
+
+	if (rtlpci->irq_enabled == 0)
+		return IRQ_HANDLED;
+
+	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock,flags);
+
+
+	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[MAC_HIMR], 0x0);
+	
+
+	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[MAC_HIMRE], 0x0);
+
+
+	/*read ISR: 4/8bytes */
+	rtlpriv->cfg->ops->interrupt_recognized(hw, &inta, &intb);
+
+
+	/*Shared IRQ or HW 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_write_byte(rtlpriv, 0xb4, BIT(1) );
+		_rtl_pci_rx_interrupt(hw);
+	}
+
+	if (unlikely(intb & rtlpriv->cfg->maps[RTL_IMR_RXFOVW])) {
+		RT_TRACE(COMP_ERR, DBG_WARNING, ("rx overflow !\n"));
+		_rtl_pci_rx_interrupt(hw);
+	}
+
+	/*<4> fw related*/
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723AE) {
+		if (inta & rtlpriv->cfg->maps[RTL_IMR_C2HCMD]) {
+			RT_TRACE(COMP_INTR, DBG_TRACE, 
+				 ("firmware interrupt!\n"));
+			queue_delayed_work(rtlpriv->works.rtl_wq,
+					   &rtlpriv->works.fwevt_wq, 0);
+		}
+	}
+
+	/*<5> hsisr related*/
+	/* Only 8188EE & 8723BE Supported.
+	 * If Other ICs Come in, System will corrupt,
+	 * because maps[RTL_IMR_HSISR_IND] & maps[MAC_HSISR]
+	 * are not initialized*/
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8188EE ||
+	    rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
+		if (unlikely(inta & rtlpriv->cfg->maps[RTL_IMR_HSISR_IND])) {
+			RT_TRACE(COMP_INTR, DBG_TRACE, 
+					 ("hsisr interrupt!\n"));
+			_rtl_pci_hs_interrupt(hw);
+		}
+	}
+	
+
+	if(rtlpriv->rtlhal.b_earlymode_enable)
+		tasklet_schedule(&rtlpriv->works.irq_tasklet);
+
+	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[MAC_HIMR],
+			rtlpci->irq_mask[0]);
+	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[MAC_HIMRE],
+			rtlpci->irq_mask[1]);
+	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
+	
+	return IRQ_HANDLED;
+
+done:
+	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
+	return IRQ_HANDLED;
+}
+
+static void _rtl_pci_irq_tasklet(struct ieee80211_hw *hw)
+{
+	_rtl_pci_tx_chk_waitq(hw);
+}
+
+static void _rtl_pci_prepare_bcn_tasklet(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl8192_tx_ring *ring = NULL;
+	struct ieee80211_hdr *hdr = NULL;
+	struct ieee80211_tx_info *info = NULL;
+	struct sk_buff *pskb = NULL;
+	struct rtl_tx_desc *pdesc = NULL;
+	struct rtl_tcb_desc tcb_desc;
+	/*This is for new trx flow*/
+	struct rtl_tx_buffer_desc *pbuffer_desc = NULL;
+	u8 temp_one = 1;
+
+	memset(&tcb_desc, 0, sizeof(struct rtl_tcb_desc));
+	ring = &rtlpci->tx_ring[BEACON_QUEUE];
+	pskb = __skb_dequeue(&ring->queue);
+	if (pskb)
+		kfree_skb(pskb);
+
+	/*NB: the beacon data buffer must be 32-bit aligned. */
+	pskb = ieee80211_beacon_get(hw, mac->vif);
+	if (pskb == NULL)
+		return;
+	hdr = rtl_get_hdr(pskb);
+	info = IEEE80211_SKB_CB(pskb);
+	pdesc = &ring->desc[0];
+	if (rtlpriv->use_new_trx_flow)
+		pbuffer_desc = &ring->buffer_desc[0];
+	
+/*<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);
+
+	rtlpriv->cfg->ops->set_desc(hw, (u8 *) pdesc, true, HW_DESC_OWN,
+				    (u8 *) & temp_one);
+
+	return;
+}
+
+static void _rtl_pci_init_trx_var(struct ieee80211_hw *hw)
+{
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	u8 i;
+	u16 desc_num;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE)
+		desc_num = TX_DESC_NUM_92E;
+	else
+		desc_num = RT_TXDESC_NUM;
+	
+	for (i = 0; i < RTL_PCI_MAX_TX_QUEUE_COUNT; i++) {
+		rtlpci->txringcount[i] = desc_num;
+	}
+	/*
+	 *we just alloc 2 desc for beacon queue,
+	 *because we just need first desc in hw beacon.
+	 */
+	rtlpci->txringcount[BEACON_QUEUE] = 2;
+
+	/*
+	 *BE queue need more descriptor for performance
+	 *consideration or, No more tx desc will happen,
+	 *and may cause mac80211 mem leakage.
+	 */
+	if (rtl_priv(hw)->use_new_trx_flow == false)
+		rtlpci->txringcount[BE_QUEUE] = RT_TXDESC_NUM_BE_QUEUE;
+
+	rtlpci->rxbuffersize = 9100;	/*2048/1024; */
+	rtlpci->rxringcount = RTL_PCI_MAX_RX_COUNT;	/*64; */
+}
+
+static void _rtl_pci_init_struct(struct ieee80211_hw *hw,
+		struct pci_dev *pdev)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	rtlpriv->rtlhal.up_first_time = true;
+	rtlpriv->rtlhal.being_init_adapter = false;
+
+	rtlhal->hw = hw;
+	rtlpci->pdev = pdev;
+
+	/*Tx/Rx related var */
+	_rtl_pci_init_trx_var(hw);
+
+	/*IBSS*/ mac->beacon_interval = 100;
+
+	/*AMPDU*/
+	mac->min_space_cfg = 0;
+	mac->max_mss_density = 0;
+	/*set sane AMPDU defaults */
+	mac->current_ampdu_density = 7;
+	mac->current_ampdu_factor = 3;
+
+	/*QOS*/
+	rtlpci->acm_method = eAcmWay2_SW;
+
+	/*task */
+	tasklet_init(&rtlpriv->works.irq_tasklet,
+		     (void (*)(unsigned long))_rtl_pci_irq_tasklet,
+		     (unsigned long)hw);
+	tasklet_init(&rtlpriv->works.irq_prepare_bcn_tasklet,
+		     (void (*)(unsigned long))_rtl_pci_prepare_bcn_tasklet,
+		     (unsigned long)hw);
+}
+
+static int _rtl_pci_init_tx_ring(struct ieee80211_hw *hw,
+				 unsigned int prio, unsigned int entries)
+{
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_tx_buffer_desc *buffer_desc;
+	struct rtl_tx_desc *desc;
+	dma_addr_t buffer_desc_dma, desc_dma;
+	u32 nextdescaddress;
+	int i;
+
+	/* alloc tx buffer desc for new trx flow*/
+	if (rtlpriv->use_new_trx_flow) {
+		buffer_desc = pci_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 == false) {
+		for (i = 0; i < entries; i++) {
+			nextdescaddress = cpu_to_le32((u32) desc_dma +
+						      ((i +	1) % entries) *
+						      sizeof(*desc));
+
+			rtlpriv->cfg->ops->set_desc(hw, (u8 *) & (desc[i]),
+						    true, 
+						    HW_DESC_TX_NEXTDESC_ADDR,
+						    (u8 *) & nextdescaddress);
+		}
+	}
+	return 0;
+}
+
+static int _rtl_pci_init_rx_ring(struct ieee80211_hw *hw, int rxring_idx)
+{
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	int i;
+	
+	if (rtlpriv->use_new_trx_flow) {
+		struct rtl_rx_buffer_desc *entry = NULL;
+		/* alloc dma for this ring */
+		rtlpci->rx_ring[rxring_idx].buffer_desc = 
+		    pci_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)) {
+		u8 *entry;
+		struct sk_buff *skb = __skb_dequeue(&ring->queue);
+		if (rtlpriv->use_new_trx_flow)
+			entry = (u8 *)(&ring->buffer_desc[ring->idx]);
+		else
+			entry = (u8 *)(&ring->desc[ring->idx]);
+
+		pci_unmap_single(rtlpci->pdev,
+				 le32_to_cpu(rtlpriv->cfg->ops->get_desc(
+				 (u8 *) entry, true, HW_DESC_TXBUFF_ADDR)),
+				 skb->len, PCI_DMA_TODEVICE);
+		kfree_skb(skb);
+		ring->idx = (ring->idx + 1) % ring->entries;
+	}
+
+	/* free dma of this ring */
+	pci_free_consistent(rtlpci->pdev,
+			    sizeof(*ring->desc) * ring->entries,
+			    ring->desc, ring->dma);
+	ring->desc = NULL;
+	if (rtlpriv->use_new_trx_flow) {
+		pci_free_consistent(rtlpci->pdev,
+				    sizeof(*ring->buffer_desc) * ring->entries,
+				    ring->buffer_desc, ring->buffer_desc_dma);
+		ring->buffer_desc = NULL;
+	}	
+}
+
+static void _rtl_pci_free_rx_ring(struct ieee80211_hw *hw, int rxring_idx)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	int i;
+
+	/* free every desc in this ring */
+	for (i = 0; i < rtlpci->rxringcount; i++) {
+		struct sk_buff *skb = rtlpci->rx_ring[rxring_idx].rx_buf[i];
+		if (!skb)
+			continue;
+
+		pci_unmap_single(rtlpci->pdev, *((dma_addr_t *) skb->cb),
+				 rtlpci->rxbuffersize, PCI_DMA_FROMDEVICE);
+		kfree_skb(skb);
+	}
+
+	/* free dma of this ring */
+	if (rtlpriv->use_new_trx_flow) {
+		pci_free_consistent(rtlpci->pdev,
+				    sizeof(*rtlpci->rx_ring[rxring_idx].
+				           buffer_desc) * rtlpci->rxringcount,
+				    rtlpci->rx_ring[rxring_idx].buffer_desc,
+				    rtlpci->rx_ring[rxring_idx].dma);
+		rtlpci->rx_ring[rxring_idx].buffer_desc = NULL;
+	} else {
+		pci_free_consistent(rtlpci->pdev,
+				    sizeof(*rtlpci->rx_ring[rxring_idx].desc) *
+				    rtlpci->rxringcount,
+				    rtlpci->rx_ring[rxring_idx].desc,
+				    rtlpci->rx_ring[rxring_idx].dma);
+		rtlpci->rx_ring[rxring_idx].desc = NULL;
+	}
+}
+
+static int _rtl_pci_init_trx_ring(struct ieee80211_hw *hw)
+{
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	int ret;
+	int i, rxring_idx;
+
+	/* rxring_idx 0:RX_MPDU_QUEUE
+	 * rxring_idx 1:RX_CMD_QUEUE */
+	for (rxring_idx = 0; rxring_idx < RTL_PCI_MAX_RX_QUEUE; rxring_idx++) {
+		ret = _rtl_pci_init_rx_ring(hw, rxring_idx);
+		if (ret)
+			return ret;
+	}
+
+	for (i = 0; i < RTL_PCI_MAX_TX_QUEUE_COUNT; i++) {
+		ret = _rtl_pci_init_tx_ring(hw, i, rtlpci->txringcount[i]);
+		if (ret)
+			goto err_free_rings;
+	}
+
+	return 0;
+
+err_free_rings:
+	for (rxring_idx = 0; rxring_idx < RTL_PCI_MAX_RX_QUEUE; rxring_idx++)
+		_rtl_pci_free_rx_ring(hw, rxring_idx);
+
+	for (i = 0; i < RTL_PCI_MAX_TX_QUEUE_COUNT; i++)
+		if (rtlpci->tx_ring[i].desc || 
+		    rtlpci->tx_ring[i].buffer_desc)
+			_rtl_pci_free_tx_ring(hw, i);
+
+	return 1;
+}
+
+static int _rtl_pci_deinit_trx_ring(struct ieee80211_hw *hw)
+{
+	u32 i, rxring_idx;
+
+	/*free rx rings */
+	for (rxring_idx = 0; rxring_idx < RTL_PCI_MAX_RX_QUEUE; rxring_idx++)
+		_rtl_pci_free_rx_ring(hw, rxring_idx);
+
+	/*free tx rings */
+	for (i = 0; i < RTL_PCI_MAX_TX_QUEUE_COUNT; i++)
+		_rtl_pci_free_tx_ring(hw, i);
+
+	return 0;
+}
+
+int rtl_pci_reset_trx_ring(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	int i, rxring_idx;
+	unsigned long flags;
+	u8 tmp_one = 1;
+	/* rxring_idx 0:RX_MPDU_QUEUE */
+	/* rxring_idx 1:RX_CMD_QUEUE */
+	for (rxring_idx = 0; rxring_idx < RTL_PCI_MAX_RX_QUEUE; rxring_idx++) {
+		/* force the rx_ring[RX_MPDU_QUEUE/
+		 * RX_CMD_QUEUE].idx to the first one */
+		/*new trx flow, do nothing*/
+		if ((rtlpriv->use_new_trx_flow == false) && 
+		     rtlpci->rx_ring[rxring_idx].desc) {
+			struct rtl_rx_desc *entry = NULL;
+
+			for (i = 0; i < rtlpci->rxringcount; i++) {
+				entry = &rtlpci->rx_ring[rxring_idx].desc[i];			
+				rtlpriv->cfg->ops->set_desc(hw, (u8 *) entry, 
+							    false,
+							    HW_DESC_RXOWN, 
+							    (u8 *) & tmp_one);		
+			}
+		}
+		rtlpci->rx_ring[rxring_idx].idx = 0;	}
+
+	/* after reset, release previous pending packet,
+	 * and force the  tx idx to the first one */
+	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
+	for (i = 0; i < RTL_PCI_MAX_TX_QUEUE_COUNT; i++) {
+		if (rtlpci->tx_ring[i].desc || 
+			rtlpci->tx_ring[i].buffer_desc) {
+			struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[i];
+
+			while (skb_queue_len(&ring->queue)) {
+				u8 *entry;
+				struct sk_buff *skb = 
+					__skb_dequeue(&ring->queue);
+				if (rtlpriv->use_new_trx_flow)
+					entry = (u8 *)(&ring->buffer_desc
+								[ring->idx]);
+				else
+					entry = (u8 *)(&ring->desc[ring->idx]);
+
+				pci_unmap_single(rtlpci->pdev,
+					le32_to_cpu(rtlpriv->cfg->ops->get_desc(
+							(u8 *)entry, true,
+							HW_DESC_TXBUFF_ADDR)),
+					skb->len, PCI_DMA_TODEVICE);
+				kfree_skb(skb);
+				ring->idx = (ring->idx + 1) % ring->entries;
+			}
+			ring->idx = 0;
+		}
+	}
+
+	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
+
+	return 0;
+}
+
+/*<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);
+
+	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;
+}
+
+/*<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;
+	u16 idx;
+	u8 own;
+	u8 temp_one = 1;
+	u8 hw_queue = _rtl_mac_to_hwqueue(hw, skb);
+	unsigned long flags;
+	struct ieee80211_hdr *hdr = rtl_get_hdr(skb);
+	u16 fc = rtl_get_fc(skb);
+	u8 *pda_addr = hdr->addr1;
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	/*ssn */
+	u8 tid = 0;
+	u16 seq_number = 0;
+
+
+	if (ieee80211_is_mgmt(fc))
+		rtl_tx_mgmt_proc(hw, skb);
+
+	if (rtlpriv->psc.sw_ps_enabled) {
+		if (ieee80211_is_data(fc) && !ieee80211_is_nullfunc(fc) &&
+		    !ieee80211_has_pm(fc))
+			hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_PM);
+	}
+
+	rtl_action_proc(hw, skb, true);
+
+	if (is_multicast_ether_addr(pda_addr))
+		rtlpriv->stats.txbytesmulticast += skb->len;
+	else if (is_broadcast_ether_addr(pda_addr))
+		rtlpriv->stats.txbytesbroadcast += skb->len;
+	else
+		rtlpriv->stats.txbytesunicast += skb->len;
+
+	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
+	ring = &rtlpci->tx_ring[hw_queue];
+	if (hw_queue != BEACON_QUEUE) {
+		if (rtlpriv->use_new_trx_flow)
+			idx = ring->cur_tx_wp;
+		else
+			idx = (ring->idx + skb_queue_len(&ring->queue)) %
+			      ring->entries;
+	} else {
+		idx = 0;
+	}
+
+	pdesc = &ring->desc[idx];
+	
+	if (rtlpriv->use_new_trx_flow) {
+		ptx_bd_desc = &ring->buffer_desc[idx];
+	} else {	
+		own = (u8) rtlpriv->cfg->ops->get_desc((u8 *) pdesc,
+				true, HW_DESC_OWN);
+
+		if ((own == 1) && (hw_queue != BEACON_QUEUE)) {
+			RT_TRACE(COMP_ERR, DBG_WARNING,
+				 ("No more TX desc@%d, ring->idx = %d,"
+				  "idx = %d, skb_queue_len = 0x%d\n",
+				  hw_queue, ring->idx, idx,
+				  skb_queue_len(&ring->queue)));
+
+			spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, 
+					       flags);
+			return skb->len;
+		}
+	}
+	
+	if (ieee80211_is_data_qos(fc)) {
+		tid = rtl_get_tid(skb);
+		if (sta) {
+			sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+			seq_number = (le16_to_cpu(hdr->seq_ctrl) & 
+				      IEEE80211_SCTL_SEQ) >> 4;
+			seq_number += 1;
+
+			if (!ieee80211_has_morefrags(hdr->frame_control))
+				sta_entry->tids[tid].seq_number = seq_number;
+		}
+	}
+
+	if (ieee80211_is_data(fc))
+		rtlpriv->cfg->ops->led_control(hw, LED_CTL_TX);
+
+/*<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) {
+		rtlpriv->cfg->ops->set_desc(hw, (u8 *) pdesc, true,
+					    HW_DESC_OWN, (u8 *) & hw_queue);
+	} else {
+		rtlpriv->cfg->ops->set_desc(hw, (u8 *) pdesc, true,
+					    HW_DESC_OWN, (u8 *) & temp_one);
+	}
+
+	if ((ring->entries - skb_queue_len(&ring->queue)) < 2 &&
+	    hw_queue != BEACON_QUEUE) {
+
+		RT_TRACE(COMP_ERR, DBG_LOUD,
+			 ("less desc left, stop skb_queue@%d, "
+			  "ring->idx = %d,"
+			  "idx = %d, skb_queue_len = 0x%d\n",
+			  hw_queue, ring->idx, idx,
+			  skb_queue_len(&ring->queue)));
+
+		ieee80211_stop_queue(hw, skb_get_queue_mapping(skb));
+	}
+
+	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
+
+	rtlpriv->cfg->ops->tx_polling(hw, hw_queue);
+
+	return 0;
+}
+#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);
+	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 (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 ||
+			queue_id == TXCMD_QUEUE) {
+			queue_id--;
+			continue;
+		} else {
+			msleep(5);
+			i++;
+		}
+
+		/* we just wait 1s for all queues */
+		if (rtlpriv->psc.rfpwr_state == ERFOFF ||
+			is_hal_stop(rtlhal) || i >= 200)
+			return;
+	}
+}
+
+void rtl_pci_deinit(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	_rtl_pci_deinit_trx_ring(hw);
+
+	synchronize_irq(rtlpci->pdev->irq);
+	tasklet_kill(&rtlpriv->works.irq_tasklet);
+
+	flush_workqueue(rtlpriv->works.rtl_wq);
+	destroy_workqueue(rtlpriv->works.rtl_wq);
+
+}
+
+int rtl_pci_init(struct ieee80211_hw *hw, struct pci_dev *pdev)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int err;
+
+	_rtl_pci_init_struct(hw, pdev);
+
+	err = _rtl_pci_init_trx_ring(hw);
+	if (err) {
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("tx ring initialization failed"));
+		return err;
+	}
+
+	return 1;
+}
+
+int rtl_pci_start(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+
+	int err = 0;
+	RT_TRACE(COMP_INIT, DBG_DMESG, (" rtl_pci_start \n"));
+	rtl_pci_reset_trx_ring(hw);
+
+	rtlpriv->rtlhal.driver_is_goingto_unload = false;
+	err = rtlpriv->cfg->ops->hw_init(hw);
+	if (err) {
+		RT_TRACE(COMP_INIT, DBG_DMESG,
+			 ("Failed to config hardware err %x!\n",err));
+		return err;
+	}
+
+	rtlpriv->cfg->ops->enable_interrupt(hw);
+	RT_TRACE(COMP_INIT, DBG_LOUD, ("enable_interrupt OK\n"));
+
+	rtl_init_rx_config(hw);
+
+	/*should after adapter start and interrupt enable. */
+	set_hal_start(rtlhal);
+
+	RT_CLEAR_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
+
+	rtlpriv->rtlhal.up_first_time = false;
+
+	RT_TRACE(COMP_INIT, DBG_DMESG, ("rtl_pci_start OK\n"));
+	return 0;
+}
+
+void rtl_pci_stop(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 RFInProgressTimeOut = 0;
+
+	/*
+	 *should before disable 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);
+/*<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;
+
+		rtl_pci_get_linkcontrol_field(hw);
+
+		if (pcipriv->ndis_adapter.pcibridge_vendor ==
+		    PCI_BRIDGE_VENDOR_AMD) {
+			pcipriv->ndis_adapter.amd_l1_patch =
+			    rtl_pci_get_amd_l1_patch(hw);
+		}
+	}
+
+	RT_TRACE(COMP_INIT, DBG_DMESG,
+		 ("pcidev busnumber:devnumber:funcnumber:"
+		  "vendor:link_ctl %d:%d:%d:%x:%x\n",
+		  pcipriv->ndis_adapter.busnumber,
+		  pcipriv->ndis_adapter.devnumber,
+		  pcipriv->ndis_adapter.funcnumber,
+		  pdev->vendor, pcipriv->ndis_adapter.linkctrl_reg));
+
+	RT_TRACE(COMP_INIT, DBG_DMESG,
+		 ("pci_bridge busnumber:devnumber:funcnumber:vendor:"
+		  "pcie_cap:link_ctl_reg:amd %d:%d:%d:%x:%x:%x:%x\n",
+		  pcipriv->ndis_adapter.pcibridge_busnum,
+		  pcipriv->ndis_adapter.pcibridge_devnum,
+		  pcipriv->ndis_adapter.pcibridge_funcnum,
+		  pcibridge_vendors[pcipriv->ndis_adapter.pcibridge_vendor],
+		  pcipriv->ndis_adapter.pcibridge_pciehdr_offset,
+		  pcipriv->ndis_adapter.pcibridge_linkctrlreg,
+		  pcipriv->ndis_adapter.amd_l1_patch));
+
+	rtl_pci_parse_configuration(pdev, hw);
+	list_add_tail(&rtlpriv->list, &rtlpriv->glb_var->glb_priv_list);
+	return true;
+}
+
+static int rtl_pci_intr_mode_msi(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(pcipriv);
+	int ret;
+	ret = pci_enable_msi(rtlpci->pdev);
+	if (ret < 0)
+		return ret;
+
+	ret = request_irq(rtlpci->pdev->irq, &_rtl_pci_interrupt,
+			  IRQF_SHARED, KBUILD_MODNAME, hw);
+	if (ret < 0) {
+		pci_disable_msi(rtlpci->pdev);
+		return ret;
+	}
+
+	rtlpci->using_msi = true;
+	
+	RT_TRACE(COMP_INIT|COMP_INTR, DBG_DMESG, ("MSI Interrupt Mode!\n"));
+	return 0;
+}
+
+static int rtl_pci_intr_mode_legacy(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(pcipriv);
+	int ret;
+	
+	ret = request_irq(rtlpci->pdev->irq, &_rtl_pci_interrupt,
+			  IRQF_SHARED, KBUILD_MODNAME, hw);
+	if (ret < 0) {
+		return ret;
+	}
+
+	rtlpci->using_msi = false;
+	RT_TRACE(COMP_INIT|COMP_INTR, DBG_DMESG, 
+		 ("Pin-based Interrupt Mode!\n"));
+	return 0;
+}
+
+static int rtl_pci_intr_mode_decide(struct ieee80211_hw *hw)
+{
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(pcipriv);
+	int ret;
+	if (rtlpci->msi_support == true) {
+		ret = rtl_pci_intr_mode_msi(hw);
+		if (ret < 0)
+			ret = rtl_pci_intr_mode_legacy(hw);
+	} else {
+		ret = rtl_pci_intr_mode_legacy(hw);
+	}
+	return ret;
+}
+
+/* this is used for other modules get
+ * hw pointer in rtl_pci_get_hw_pointer */
+struct ieee80211_hw *hw_export = NULL;
+
+#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;
+
+	struct rtl_priv *rtlpriv = NULL;
+	struct rtl_pci_priv *pcipriv = NULL;
+	struct rtl_pci *rtlpci;
+	unsigned long pmem_start, pmem_len, pmem_flags;
+	int err;
+
+
+	err = pci_enable_device(pdev);
+	if (err) {
+		RT_ASSERT(false,
+			  ("%s : Cannot enable new PCI device\n",
+			   pci_name(pdev)));
+		return err;
+	}
+
+	if (!pci_set_dma_mask(pdev, DMA_BIT_MASK(32))) {
+		if (pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32))) {
+			RT_ASSERT(false, ("Unable to obtain 32bit DMA "
+					  "for consistent allocations\n"));
+			pci_disable_device(pdev);
+			return -ENOMEM;
+		}
+	}
+
+	pci_set_master(pdev);
+
+	hw = ieee80211_alloc_hw(sizeof(struct rtl_pci_priv) +
+				sizeof(struct rtl_priv), &rtl_ops);
+	if (!hw) {
+		RT_ASSERT(false,
+			  ("%s : ieee80211 alloc failed\n", pci_name(pdev)));
+		err = -ENOMEM;
+		goto fail1;
+	}
+	hw_export = hw;
+
+	SET_IEEE80211_DEV(hw, &pdev->dev);
+	pci_set_drvdata(pdev, hw);
+
+	rtlpriv = hw->priv;
+	pcipriv = (void *)rtlpriv->priv;
+	pcipriv->dev.pdev = pdev;
+
+	/* init cfg & intf_ops */
+	rtlpriv->rtlhal.interface = INTF_PCI;
+	rtlpriv->cfg = (struct rtl_hal_cfg *)(id->driver_data);
+	rtlpriv->intf_ops = &rtl_pci_ops;
+	rtlpriv->glb_var = &global_var;
+
+	/*
+	 *init dbgp flags before all
+	 *other functions, because we will
+	 *use it in other funtions like
+	 *RT_TRACE/RT_PRINT/RTL_PRINT_DATA
+	 *you can not use these macro
+	 *before this
+	 */
+	rtl_dbgp_flag_init(hw);
+
+	/* MEM map */
+	err = pci_request_regions(pdev, KBUILD_MODNAME);
+	if (err) {
+		RT_ASSERT(false, ("Can't obtain PCI resources\n"));
+		return err;
+	}
+
+	pmem_start = pci_resource_start(pdev, rtlpriv->cfg->bar_id);
+	pmem_len = pci_resource_len(pdev, rtlpriv->cfg->bar_id);
+	pmem_flags = pci_resource_flags(pdev, rtlpriv->cfg->bar_id);
+
+	/*shared mem start */
+	rtlpriv->io.pci_mem_start =
+			(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);
+
+	/* find adapter */
+	/* if chip not support, will return false */
+	if(!_rtl_pci_find_adapter(pdev, hw))
+		goto fail3;
+
+	/* Init IO handler */
+	_rtl_pci_io_handler_init(&pdev->dev, hw);
+
+	/*like read eeprom and so on */
+	rtlpriv->cfg->ops->read_eeprom_info(hw);
+
+	if (rtlpriv->cfg->ops->init_sw_vars(hw)) {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("Can't init_sw_vars.\n"));
+		goto fail3;
+	}
+
+	rtlpriv->cfg->ops->init_sw_leds(hw);
+
+	/*aspm */
+	rtl_pci_init_aspm(hw);
+
+	/* Init mac80211 sw */
+	err = rtl_init_core(hw);
+	if (err) {
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("Can't allocate sw for mac80211.\n"));
+		goto fail3;
+	}
+
+	/* Init PCI sw */
+	err = !rtl_pci_init(hw, pdev);
+	if (err) {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("Failed to init PCI.\n"));
+		goto fail3;
+	}
+
+	err = ieee80211_register_hw(hw);
+	if (err) {
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("Can't register mac80211 hw.\n"));
+		goto fail3;
+	} else {
+		rtlpriv->mac80211.mac80211_registered = 1;
+	}
+	/* the wiphy must have been registed to 
+	 * cfg80211 prior to regulatory_hint */
+	if (regulatory_hint(hw->wiphy, rtlpriv->regd.alpha2)) {
+		RT_TRACE(COMP_ERR, DBG_WARNING, ("regulatory_hint fail\n"));
+	}
+
+	err = sysfs_create_group(&pdev->dev.kobj, &rtl_attribute_group);
+	if (err) {
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("failed to create sysfs device attributes\n"));
+		goto fail3;
+	}
+	/* add for prov */
+	rtl_proc_add_one(hw);
+
+	/*init rfkill */
+	rtl_init_rfkill(hw);
+
+	rtlpci = rtl_pcidev(pcipriv);
+
+	err = rtl_pci_intr_mode_decide(hw);
+	if (err) {
+		RT_TRACE(COMP_INIT, DBG_DMESG,
+			 ("%s: failed to register IRQ handler\n",
+			  wiphy_name(hw->wiphy)));
+		goto fail3;
+	} else {
+		rtlpci->irq_alloc = 1;
+	}
+
+	set_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status);
+	return 0;
+
+fail3:
+	pci_set_drvdata(pdev, NULL);
+	rtl_deinit_core(hw);
+	ieee80211_free_hw(hw);
+
+	if (rtlpriv->io.pci_mem_start != 0)
+		pci_iounmap(pdev, (void *)rtlpriv->io.pci_mem_start);
+
+fail2:
+	pci_release_regions(pdev);
+
+fail1:
+
+	pci_disable_device(pdev);
+
+	return -ENODEV;
+
+}
+//EXPORT_SYMBOL(rtl_pci_probe);
+
+struct ieee80211_hw *rtl_pci_get_hw_pointer(void)
+{
+	return hw_export;
+}
+//EXPORT_SYMBOL(rtl_pci_get_hw_pointer);
+
+void rtl_pci_disconnect(struct pci_dev *pdev)
+{
+	struct ieee80211_hw *hw = pci_get_drvdata(pdev);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(pcipriv);
+	struct rtl_mac *rtlmac = rtl_mac(rtlpriv);
+
+	clear_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status);
+
+	sysfs_remove_group(&pdev->dev.kobj, &rtl_attribute_group);
+
+	/* add for prov */
+	rtl_proc_remove_one(hw);
+	
+
+	/*ieee80211_unregister_hw will call ops_stop */
+	if (rtlmac->mac80211_registered == 1) {
+		ieee80211_unregister_hw(hw);
+		rtlmac->mac80211_registered = 0;
+	} else {
+		rtl_deinit_deferred_work(hw);
+		rtlpriv->intf_ops->adapter_stop(hw);
+	}
+
+	/*deinit rfkill */
+	rtl_deinit_rfkill(hw);
+
+	rtl_pci_deinit(hw);
+	rtl_deinit_core(hw);
+	rtlpriv->cfg->ops->deinit_sw_vars(hw);
+
+	if (rtlpci->irq_alloc) {
+		synchronize_irq(rtlpci->pdev->irq);
+		free_irq(rtlpci->pdev->irq, hw);
+		rtlpci->irq_alloc = 0;
+	}
+
+	if (rtlpci->using_msi == true)
+		pci_disable_msi(rtlpci->pdev);
+
+	list_del(&rtlpriv->list);
+	if (rtlpriv->io.pci_mem_start != 0) {
+		pci_iounmap(pdev, (void *)rtlpriv->io.pci_mem_start);
+		pci_release_regions(pdev);
+	}
+
+	pci_disable_device(pdev);
+
+	rtl_pci_disable_aspm(hw);
+
+	pci_set_drvdata(pdev, NULL);
+
+	ieee80211_free_hw(hw);
+}
+//EXPORT_SYMBOL(rtl_pci_disconnect);
+
+/***************************************
+kernel pci power state define:
+PCI_D0         ((pci_power_t __force) 0)
+PCI_D1         ((pci_power_t __force) 1)
+PCI_D2         ((pci_power_t __force) 2)
+PCI_D3hot      ((pci_power_t __force) 3)
+PCI_D3cold     ((pci_power_t __force) 4)
+PCI_UNKNOWN    ((pci_power_t __force) 5)
+
+This function is called when system
+goes into suspend state mac80211 will
+call rtl_mac_stop() from the mac80211
+suspend function first, So there is
+no need to call hw_disable here.
+****************************************/
+int rtl_pci_suspend(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct ieee80211_hw *hw = pci_get_drvdata(pdev);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->cfg->ops->hw_suspend(hw);
+	rtl_deinit_rfkill(hw);
+
+	return 0;
+}
+//EXPORT_SYMBOL(rtl_pci_suspend);
+
+int rtl_pci_resume(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct ieee80211_hw *hw = pci_get_drvdata(pdev);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->cfg->ops->hw_resume(hw);
+	rtl_init_rfkill(hw);
+	
+	return 0;
+}
+//EXPORT_SYMBOL(rtl_pci_resume);
+
+struct rtl_intf_ops rtl_pci_ops = {
+	.read_efuse_byte = read_efuse_byte,
+	.adapter_start = rtl_pci_start,
+	.adapter_stop = rtl_pci_stop,
+	.check_buddy_priv = rtl_pci_check_buddy_priv,
+	.adapter_tx = rtl_pci_tx,
+	.flush = rtl_pci_flush,
+	.reset_trx_ring = rtl_pci_reset_trx_ring,
+	.waitq_insert = rtl_pci_tx_chk_waitq_insert,
+
+	.disable_aspm = rtl_pci_disable_aspm,
+	.enable_aspm = rtl_pci_enable_aspm,
+};
diff --git a/drivers/staging/rtl8821ae/pci.h b/drivers/staging/rtl8821ae/pci.h
new file mode 100644
index 000000000000..9f206550a657
--- /dev/null
+++ b/drivers/staging/rtl8821ae/pci.h
@@ -0,0 +1,353 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL_PCI_H__
+#define __RTL_PCI_H__
+
+#include <linux/pci.h>
+/*
+1: MSDU packet queue,
+2: Rx Command Queue
+*/
+#define RTL_PCI_RX_MPDU_QUEUE			0
+#define RTL_PCI_RX_CMD_QUEUE			1
+#define RTL_PCI_MAX_RX_QUEUE			2
+
+#define RTL_PCI_MAX_RX_COUNT			512//64
+#define RTL_PCI_MAX_TX_QUEUE_COUNT		9
+
+#define RT_TXDESC_NUM				128
+#define TX_DESC_NUM_92E				512
+#define RT_TXDESC_NUM_BE_QUEUE			256
+
+#define BK_QUEUE				0
+#define BE_QUEUE				1
+#define VI_QUEUE				2
+#define VO_QUEUE				3
+#define BEACON_QUEUE				4
+#define TXCMD_QUEUE				5
+#define MGNT_QUEUE				6
+#define HIGH_QUEUE				7
+#define HCCA_QUEUE				8
+
+#define RTL_PCI_DEVICE(vend, dev, cfg)  \
+	.vendor = (vend), \
+	.device = (dev), \
+	.subvendor = PCI_ANY_ID, \
+	.subdevice = PCI_ANY_ID,\
+	.driver_data = (kernel_ulong_t)&(cfg)
+
+#define INTEL_VENDOR_ID				0x8086
+#define SIS_VENDOR_ID				0x1039
+#define ATI_VENDOR_ID				0x1002
+#define ATI_DEVICE_ID				0x7914
+#define AMD_VENDOR_ID				0x1022
+
+#define PCI_MAX_BRIDGE_NUMBER			255
+#define PCI_MAX_DEVICES				32
+#define PCI_MAX_FUNCTION			8
+
+#define PCI_CONF_ADDRESS   	0x0CF8	/*PCI Configuration Space Address */
+#define PCI_CONF_DATA		0x0CFC	/*PCI Configuration Space Data */
+
+#define PCI_CLASS_BRIDGE_DEV		0x06
+#define PCI_SUBCLASS_BR_PCI_TO_PCI	0x04
+#define PCI_CAPABILITY_ID_PCI_EXPRESS	0x10
+#define PCI_CAP_ID_EXP			0x10
+
+#define U1DONTCARE 			0xFF
+#define U2DONTCARE 			0xFFFF
+#define U4DONTCARE 			0xFFFFFFFF
+
+#define RTL_PCI_8192_DID	0x8192	/*8192 PCI-E */
+#define RTL_PCI_8192SE_DID	0x8192	/*8192 SE */
+#define RTL_PCI_8174_DID	0x8174	/*8192 SE */
+#define RTL_PCI_8173_DID	0x8173	/*8191 SE Crab */
+#define RTL_PCI_8172_DID	0x8172	/*8191 SE RE */
+#define RTL_PCI_8171_DID	0x8171	/*8191 SE Unicron */
+#define RTL_PCI_0045_DID	0x0045	/*8190 PCI for Ceraga */
+#define RTL_PCI_0046_DID	0x0046	/*8190 Cardbus for Ceraga */
+#define RTL_PCI_0044_DID	0x0044	/*8192e PCIE for Ceraga */
+#define RTL_PCI_0047_DID	0x0047	/*8192e Express Card for Ceraga */
+#define RTL_PCI_700F_DID	0x700F
+#define RTL_PCI_701F_DID	0x701F
+#define RTL_PCI_DLINK_DID	0x3304
+#define RTL_PCI_8723AE_DID	0x8723	/*8723e */
+#define RTL_PCI_8192CET_DID	0x8191	/*8192ce */
+#define RTL_PCI_8192CE_DID	0x8178	/*8192ce */
+#define RTL_PCI_8191CE_DID	0x8177	/*8192ce */
+#define RTL_PCI_8188CE_DID	0x8176	/*8192ce */
+#define RTL_PCI_8192CU_DID	0x8191	/*8192ce */
+#define RTL_PCI_8192DE_DID	0x8193	/*8192de */
+#define RTL_PCI_8192DE_DID2	0x002B	/*92DE*/
+#define RTL_PCI_8188EE_DID	0x8179  /*8188ee*/
+#define RTL_PCI_8723BE_DID	0xB723  /*8723be*/
+#define RTL_PCI_8192EE_DID	0x818B	/*8192ee*/
+#define RTL_PCI_8821AE_DID	0x8821	/*8821ae*/
+#define RTL_PCI_8812AE_DID	0x8812	/*8812ae*/
+
+/*8192 support 16 pages of IO registers*/
+#define RTL_MEM_MAPPED_IO_RANGE_8190PCI 	0x1000
+#define RTL_MEM_MAPPED_IO_RANGE_8192PCIE	0x4000
+#define RTL_MEM_MAPPED_IO_RANGE_8192SE		0x4000
+#define RTL_MEM_MAPPED_IO_RANGE_8192CE		0x4000
+#define RTL_MEM_MAPPED_IO_RANGE_8192DE		0x4000
+
+#define RTL_PCI_REVISION_ID_8190PCI		0x00
+#define RTL_PCI_REVISION_ID_8192PCIE		0x01
+#define RTL_PCI_REVISION_ID_8192SE		0x10
+#define RTL_PCI_REVISION_ID_8192CE		0x1
+#define RTL_PCI_REVISION_ID_8192DE		0x0
+
+#define PCI_VENDOR_ID_REALTEK		0x10ec
+
+#define RTL_DEFAULT_HARDWARE_TYPE	HARDWARE_TYPE_RTL8192CE
+
+enum pci_bridge_vendor {
+	PCI_BRIDGE_VENDOR_INTEL = 0x0,	/*0b'0000,0001 */
+	PCI_BRIDGE_VENDOR_ATI,		/*0b'0000,0010*/
+	PCI_BRIDGE_VENDOR_AMD,		/*0b'0000,0100*/
+	PCI_BRIDGE_VENDOR_SIS,		/*0b'0000,1000*/
+	PCI_BRIDGE_VENDOR_UNKNOWN,	/*0b'0100,0000*/
+	PCI_BRIDGE_VENDOR_MAX,	
+};
+
+struct rtl_pci_capabilities_header {
+    u8 capability_id;
+    u8 next;
+};
+
+/* In new TRX flow, Buffer_desc is new concept 
+  * But TX wifi info == TX descriptor in old flow
+  * RX wifi info == RX descriptor in old flow */
+struct rtl_tx_buffer_desc {
+#if (RTL8192EE_SEG_NUM == 2)
+	u32 dword[2*(DMA_IS_64BIT + 1)*8]; //seg = 8
+#elif (RTL8192EE_SEG_NUM == 1)
+	u32 dword[2*(DMA_IS_64BIT + 1)*4]; //seg = 4
+#elif (RTL8192EE_SEG_NUM == 0)
+	u32 dword[2*(DMA_IS_64BIT + 1)*2]; //seg = 2
+#endif
+} __packed;
+
+struct rtl_tx_desc {/*old: tx desc*//*new: tx wifi info*/
+	u32 dword[16];
+} __packed;
+
+struct rtl_rx_buffer_desc { /*rx buffer desc*/
+	u32 dword[2];
+} __packed;
+
+struct rtl_rx_desc { /*old: rx desc*//*new: rx wifi info*/
+	u32 dword[8];
+} __packed;
+
+struct rtl_tx_cmd_desc {
+	u32 dword[16];
+} __packed;
+
+struct rtl8192_tx_ring {
+	struct rtl_tx_desc *desc; /*tx desc / tx wifi info*/
+	dma_addr_t dma; /*tx desc dma memory / tx wifi info dma memory*/
+	unsigned int idx;
+	unsigned int entries;
+	struct sk_buff_head queue;
+	/*add for new trx flow*/
+	struct rtl_tx_buffer_desc *buffer_desc; /*tx buffer descriptor*/
+	dma_addr_t buffer_desc_dma; /*tx bufferd desc dma memory*/
+	u16 avl_desc; /* available_desc_to_write */
+	u16 cur_tx_wp; /* current_tx_write_point */
+	u16 cur_tx_rp; /* current_tx_read_point */
+};
+
+struct rtl8192_rx_ring {
+	struct rtl_rx_desc *desc;/*for old trx flow, not 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 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);
+
+static inline u8 pci_read8_sync(struct rtl_priv *rtlpriv, u32 addr)
+{
+	return 0xff & readb((u8 __iomem *) rtlpriv->io.pci_mem_start + addr);
+}
+
+static inline u16 pci_read16_sync(struct rtl_priv *rtlpriv, u32 addr)
+{
+	return readw((u8 __iomem *) rtlpriv->io.pci_mem_start + addr);
+}
+
+static inline u32 pci_read32_sync(struct rtl_priv *rtlpriv, u32 addr)
+{
+	return readl((u8 __iomem *) rtlpriv->io.pci_mem_start + addr);
+}
+
+static inline void pci_write8_async(struct rtl_priv *rtlpriv, u32 addr, u8 val)
+{
+	writeb(val, (u8 __iomem *) rtlpriv->io.pci_mem_start + addr);
+}
+
+static inline void pci_write16_async(struct rtl_priv *rtlpriv,
+				     u32 addr, u16 val)
+{
+	writew(val, (u8 __iomem *) rtlpriv->io.pci_mem_start + addr);
+}
+
+static inline void pci_write32_async(struct rtl_priv *rtlpriv,
+				     u32 addr, u32 val)
+{
+	writel(val, (u8 __iomem *) rtlpriv->io.pci_mem_start + addr);
+}
+
+static inline void rtl_pci_raw_write_port_ulong(u32 port, u32 val)
+{
+	outl(val, port);
+}
+
+static inline void rtl_pci_raw_write_port_uchar(u32 port, u8 val)
+{
+	outb(val, port);
+}
+
+static inline void rtl_pci_raw_read_port_uchar(u32 port, u8 * pval)
+{
+	*pval = inb(port);
+}
+
+static inline void rtl_pci_raw_read_port_ushort(u32 port, u16 * pval)
+{
+	*pval = inw(port);
+}
+
+static inline void rtl_pci_raw_read_port_ulong(u32 port, u32 * pval)
+{
+	*pval = inl(port);
+}
+
+#endif
diff --git a/drivers/staging/rtl8821ae/ps.c b/drivers/staging/rtl8821ae/ps.c
new file mode 100644
index 000000000000..f12ffa83c58d
--- /dev/null
+++ b/drivers/staging/rtl8821ae/ps.c
@@ -0,0 +1,1025 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "wifi.h"
+#include "base.h"
+#include "ps.h"
+#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)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool init_status = true;
+
+	/*<1> reset trx ring */
+	if (rtlhal->interface == INTF_PCI)
+		rtlpriv->intf_ops->reset_trx_ring(hw);
+
+	if (is_hal_stop(rtlhal))
+		RT_TRACE(COMP_ERR, DBG_WARNING, ("Driver is already down!\n"));
+
+	/*<2> Enable Adapter */
+	rtlpriv->cfg->ops->hw_init(hw);
+	RT_CLEAR_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
+	/*init_status = false; */
+
+	/*<3> Enable Interrupt */
+	rtlpriv->cfg->ops->enable_interrupt(hw);
+
+	/*<enable timer> */
+	rtl_watch_dog_timer_callback((unsigned long)hw);
+
+	return init_status;
+}
+//EXPORT_SYMBOL(rtl_ps_enable_nic);
+
+bool rtl_ps_disable_nic(struct ieee80211_hw *hw)
+{
+	bool status = true;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	/*<1> Stop all timer */
+	rtl_deinit_deferred_work(hw);
+
+	/*<2> Disable Interrupt */
+	rtlpriv->cfg->ops->disable_interrupt(hw);
+
+	/*<3> Disable Adapter */
+	rtlpriv->cfg->ops->hw_disable(hw);
+
+	return status;
+}
+//EXPORT_SYMBOL(rtl_ps_disable_nic);
+
+bool rtl_ps_set_rf_state(struct ieee80211_hw *hw,
+			 enum rf_pwrstate state_toset,
+			 u32 changesource, bool protect_or_not)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	enum rf_pwrstate rtstate;
+	bool b_actionallowed = false;
+	u16 rfwait_cnt = 0;
+
+	/*protect_or_not = true; */
+
+	if (protect_or_not)
+		goto no_protect;
+
+	/*
+	 *Only one thread can change
+	 *the RF state at one time, and others
+	 *should wait to be executed.
+	 */
+	while (true) {
+		spin_lock(&rtlpriv->locks.rf_ps_lock);
+		if (ppsc->rfchange_inprogress) {
+			spin_unlock(&rtlpriv->locks.rf_ps_lock);
+
+			RT_TRACE(COMP_ERR, DBG_WARNING,
+				 ("RF Change in progress!"
+				  "Wait to set..state_toset(%d).\n",
+				  state_toset));
+
+			/* Set RF after the previous action is done.  */
+			while (ppsc->rfchange_inprogress) {
+				rfwait_cnt++;
+				mdelay(1);
+				/*
+				 *Wait too long, return false to avoid
+				 *to be stuck here.
+				 */
+				if (rfwait_cnt > 100)
+					return false;
+			}
+		} else {
+			ppsc->rfchange_inprogress = true;
+			spin_unlock(&rtlpriv->locks.rf_ps_lock);
+			break;
+		}
+	}
+
+no_protect:
+	rtstate = ppsc->rfpwr_state;
+
+	switch (state_toset) {
+	case ERFON:
+		ppsc->rfoff_reason &= (~changesource);
+
+		if ((changesource == RF_CHANGE_BY_HW) &&
+		    (ppsc->b_hwradiooff == true)) {
+			ppsc->b_hwradiooff = false;
+		}
+
+		if (!ppsc->rfoff_reason) {
+			ppsc->rfoff_reason = 0;
+			b_actionallowed = true;
+		}
+
+		break;
+
+	case ERFOFF:
+
+		if ((changesource == RF_CHANGE_BY_HW) &&
+		    (ppsc->b_hwradiooff == false)) {
+			ppsc->b_hwradiooff = true;
+		}
+
+		ppsc->rfoff_reason |= changesource;
+		b_actionallowed = true;
+		break;
+
+	case ERFSLEEP:
+		ppsc->rfoff_reason |= changesource;
+		b_actionallowed = true;
+		break;
+
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("switch case not process \n"));
+		break;
+	}
+
+	if (b_actionallowed)
+		rtlpriv->cfg->ops->set_rf_power_state(hw, state_toset);
+
+	if (!protect_or_not) {
+		spin_lock(&rtlpriv->locks.rf_ps_lock);
+		ppsc->rfchange_inprogress = false;
+		spin_unlock(&rtlpriv->locks.rf_ps_lock);
+	}
+
+	return b_actionallowed;
+}
+//EXPORT_SYMBOL(rtl_ps_set_rf_state);
+
+static void _rtl_ps_inactive_ps(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+
+	ppsc->b_swrf_processing = true;
+
+	if (ppsc->inactive_pwrstate == ERFON && rtlhal->interface == INTF_PCI) {
+		if ((ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM) &&
+		    RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM) &&
+		    rtlhal->interface == INTF_PCI) {
+			rtlpriv->intf_ops->disable_aspm(hw);
+			RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
+		}
+	}
+
+	if (rtlpriv->cfg->ops->get_btc_status()){
+		rtlpriv->btcoexist.btc_ops->btc_ips_notify(rtlpriv,
+						ppsc->inactive_pwrstate);
+	}
+	rtl_ps_set_rf_state(hw, ppsc->inactive_pwrstate,
+			    RF_CHANGE_BY_IPS, false);
+
+	if (ppsc->inactive_pwrstate == ERFOFF &&
+	    rtlhal->interface == INTF_PCI) {
+		if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM &&
+		    !RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
+			rtlpriv->intf_ops->enable_aspm(hw);
+			RT_SET_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
+		}
+	}
+
+	ppsc->b_swrf_processing = false;
+}
+
+void rtl_ips_nic_off_wq_callback(void *data)
+{
+	struct rtl_works *rtlworks =
+	    container_of_dwork_rtl(data, struct rtl_works, ips_nic_off_wq);
+	struct ieee80211_hw *hw = rtlworks->hw;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	enum rf_pwrstate rtstate;
+
+	if (mac->opmode != NL80211_IFTYPE_STATION) {
+		RT_TRACE(COMP_ERR, DBG_WARNING, ("not station return\n"));
+		return;
+	}
+
+	if (mac->p2p_in_use)
+		return;
+
+	if (mac->link_state > MAC80211_NOLINK)
+		return;
+
+	if (is_hal_stop(rtlhal))
+		return;
+
+	if (rtlpriv->sec.being_setkey)
+		return;
+
+	if(rtlpriv->cfg->ops->bt_turn_off_bt_coexist_before_enter_lps)
+		rtlpriv->cfg->ops->bt_turn_off_bt_coexist_before_enter_lps(hw);
+
+	if (ppsc->b_inactiveps) {
+		rtstate = ppsc->rfpwr_state;
+
+		/*
+		 *Do not enter IPS in the following conditions:
+		 *(1) RF is already OFF or Sleep
+		 *(2) b_swrf_processing (indicates the IPS is still under going)
+		 *(3) 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;
+
+			/*rtl_pci_reset_trx_ring(hw); */
+			_rtl_ps_inactive_ps(hw);
+		}
+	}
+}
+
+void rtl_ips_nic_off(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	/*
+	 *because when link with ap, mac80211 will ask us
+	 *to disable nic quickly after scan before linking,
+	 *this will cause link failed, so we delay 100ms here
+	 */
+	queue_delayed_work(rtlpriv->works.rtl_wq,
+			   &rtlpriv->works.ips_nic_off_wq, MSECS(100));
+}
+
+/* NOTICE: any opmode should exc nic_on, or disable without
+ * nic_on may something wrong, like adhoc TP*/
+void rtl_ips_nic_on(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	enum rf_pwrstate rtstate;
+
+	cancel_delayed_work(&rtlpriv->works.ips_nic_off_wq);
+
+	spin_lock(&rtlpriv->locks.ips_lock);
+	if (ppsc->b_inactiveps) {
+		rtstate = ppsc->rfpwr_state;
+
+		if (rtstate != ERFON &&
+		    !ppsc->b_swrf_processing &&
+		    ppsc->rfoff_reason <= RF_CHANGE_BY_IPS) {
+
+			ppsc->inactive_pwrstate = ERFON;
+			ppsc->b_in_powersavemode = false;
+			_rtl_ps_inactive_ps(hw);
+		}
+	}
+	spin_unlock(&rtlpriv->locks.ips_lock);
+}
+
+/*for FW LPS*/
+
+/*
+ *Determine if we can set Fw into PS mode
+ *in current condition.Return true if it
+ *can enter PS mode.
+ */
+static bool rtl_get_fwlps_doze(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	u32 ps_timediff;
+
+	ps_timediff = jiffies_to_msecs(jiffies -
+				       ppsc->last_delaylps_stamp_jiffies);
+
+	if (ps_timediff < 2000) {
+		RT_TRACE(COMP_POWER, DBG_LOUD,
+			 ("Delay enter Fw LPS for DHCP, ARP,"
+			  " or EAPOL exchanging state.\n"));
+		return false;
+	}
+
+	if (mac->link_state != MAC80211_LINKED)
+		return false;
+
+	if (mac->opmode == NL80211_IFTYPE_ADHOC)
+		return false;
+
+	return true;
+}
+
+/* Change current and default preamble mode.*/
+void rtl_lps_set_psmode(struct ieee80211_hw *hw, u8 rt_psmode)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	bool enter_fwlps;
+
+	if (mac->opmode == NL80211_IFTYPE_ADHOC)
+		return;
+
+	if (mac->link_state != MAC80211_LINKED)
+		return;
+
+	if (ppsc->dot11_psmode == rt_psmode)
+		return;
+
+	/* Update power save mode configured. */
+	ppsc->dot11_psmode = rt_psmode;
+
+	/*
+	 *<FW control LPS>
+	 *1. Enter PS mode
+	 *   Set RPWM to Fw to turn RF off and send H2C fw_pwrmode
+	 *   cmd to set Fw into PS mode.
+	 *2. Leave PS mode
+	 *   Send H2C fw_pwrmode cmd to Fw to set Fw into Active
+	 *   mode and set RPWM to turn RF on.
+	 */
+
+	if ((ppsc->b_fwctrl_lps) && ppsc->report_linked) {
+		if (ppsc->dot11_psmode == EACTIVE) {
+			RT_TRACE(COMP_RF, DBG_DMESG,
+				 ("FW LPS leave ps_mode:%x\n",
+				  FW_PS_ACTIVE_MODE));
+			enter_fwlps = false;
+			ppsc->pwr_mode = FW_PS_ACTIVE_MODE;
+			ppsc->smart_ps = 0;
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_LPS_ACTION,
+						      (u8 *)(&enter_fwlps));
+			if (ppsc->p2p_ps_info.opp_ps)
+				rtl_p2p_ps_cmd(hw,P2P_PS_ENABLE);
+
+		} else {
+			if (rtl_get_fwlps_doze(hw)) {
+				RT_TRACE(COMP_RF, DBG_DMESG,
+					 ("FW LPS enter ps_mode:%x\n",
+					 ppsc->fwctrl_psmode));
+				enter_fwlps = true;
+				ppsc->pwr_mode = ppsc->fwctrl_psmode;
+				ppsc->smart_ps = 2;
+				rtlpriv->cfg->ops->set_hw_reg(hw,
+							HW_VAR_FW_LPS_ACTION,
+							(u8 *)(&enter_fwlps));
+
+			} else {
+				/* Reset the power save related parameters. */
+				ppsc->dot11_psmode = EACTIVE;
+			}
+		}
+	}
+}
+
+/*Enter the leisure power save mode.*/
+void rtl_lps_enter(struct ieee80211_hw *hw)
+{
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	unsigned long flag;
+
+	if (!ppsc->b_fwctrl_lps)
+		return;
+
+	if (rtlpriv->sec.being_setkey)
+		return;
+
+	if (rtlpriv->link_info.b_busytraffic)
+		return;
+
+	/*sleep after linked 10s, to let DHCP and 4-way handshake ok enough!! */
+	if (mac->cnt_after_linked < 5)
+		return;
+
+	if (mac->opmode == NL80211_IFTYPE_ADHOC)
+		return;
+
+	if (mac->link_state != MAC80211_LINKED)
+		return;
+
+	spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag);
+
+	/* Idle for a while if we connect to AP a while ago. */
+	if (mac->cnt_after_linked >= 2) {
+		if (ppsc->dot11_psmode == EACTIVE) {
+			RT_TRACE(COMP_POWER, DBG_LOUD,
+				 ("Enter 802.11 power save mode...\n"));
+
+			rtl_lps_set_psmode(hw, EAUTOPS);
+		}
+	}
+
+	spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag);
+}
+
+/*Leave the leisure power save mode.*/
+void rtl_lps_leave(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	unsigned long flag;
+
+	spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag);
+
+	if (ppsc->b_fwctrl_lps) {
+		if (ppsc->dot11_psmode != EACTIVE) {
+
+			/*FIX ME */
+			rtlpriv->cfg->ops->enable_interrupt(hw);
+
+			if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM &&
+			    RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM) &&
+			    rtlhal->interface == INTF_PCI) {
+				rtlpriv->intf_ops->disable_aspm(hw);
+				RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
+			}
+
+			RT_TRACE(COMP_POWER, DBG_LOUD,
+				 ("Busy Traffic,Leave 802.11 power save..\n"));
+
+			rtl_lps_set_psmode(hw, EACTIVE);
+		}
+	}
+	spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag);
+}
+
+/* For sw LPS*/
+void rtl_swlps_beacon(struct ieee80211_hw *hw, void *data, unsigned int len)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct ieee80211_hdr *hdr = (void *) data;
+	struct ieee80211_tim_ie *tim_ie;
+	u8 *tim;
+	u8 tim_len;
+	bool u_buffed;
+	bool m_buffed;
+
+	if (mac->opmode != NL80211_IFTYPE_STATION)
+		return;
+
+	if (!rtlpriv->psc.b_swctrl_lps)
+		return;
+
+	if (rtlpriv->mac80211.link_state != MAC80211_LINKED)
+		return;
+
+	if (!rtlpriv->psc.sw_ps_enabled)
+		return;
+
+	if (rtlpriv->psc.b_fwctrl_lps)
+		return;
+
+	if (likely(!(hw->conf.flags & IEEE80211_CONF_PS)))
+		return;
+
+	/* check if this really is a beacon */
+	if (!ieee80211_is_beacon(hdr->frame_control))
+		return;
+
+	/* min. beacon length + FCS_LEN */
+	if (len <= 40 + FCS_LEN)
+		return;
+
+	/* and only beacons from the associated BSSID, please */
+	if (ether_addr_equal(hdr->addr3, rtlpriv->mac80211.bssid))
+		return;
+
+	rtlpriv->psc.last_beacon = jiffies;
+
+	tim = rtl_find_ie(data, len - FCS_LEN, WLAN_EID_TIM);
+	if (!tim)
+		return;
+
+	if (tim[1] < sizeof(*tim_ie))
+		return;
+
+	tim_len = tim[1];
+	tim_ie = (struct ieee80211_tim_ie *) &tim[2];
+
+/*<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. */
+
+	/* 1. What about buffered unicast traffic for our AID? */
+	u_buffed = ieee80211_check_tim(tim_ie, tim_len,
+				       rtlpriv->mac80211.assoc_id);
+
+	/* 2. Maybe the AP wants to send multicast/broadcast data? */
+	m_buffed = tim_ie->bitmap_ctrl & 0x01;
+	rtlpriv->psc.multi_buffered = m_buffed;
+
+	/* unicast will process by mac80211 through
+	 * set ~IEEE80211_CONF_PS, So we just check
+	 * multicast frames here */
+	if (!m_buffed ) {//&&) {// !rtlpriv->psc.tx_doing) {
+		/* back to low-power land. and delay is
+		 * prevent null power save frame tx fail */
+		queue_delayed_work(rtlpriv->works.rtl_wq,
+				   &rtlpriv->works.ps_work, MSECS(5));
+	} else {
+		RT_TRACE(COMP_POWER, DBG_DMESG,
+			 ("u_bufferd: %x, m_buffered: %x\n",
+			  u_buffed, m_buffed));
+	}
+}
+
+void rtl_swlps_rf_awake(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	unsigned long flag;
+
+	if (!rtlpriv->psc.b_swctrl_lps)
+		return;
+	if (mac->link_state != MAC80211_LINKED)
+		return;
+
+	if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM &&
+	    RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
+		rtlpriv->intf_ops->disable_aspm(hw);
+		RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
+	}
+
+	spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag);
+	rtl_ps_set_rf_state(hw, ERFON, RF_CHANGE_BY_PS, false);
+	spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag);
+}
+
+void rtl_swlps_rfon_wq_callback(void *data)
+{
+	struct rtl_works *rtlworks =
+	    container_of_dwork_rtl(data, struct rtl_works, ps_rfon_wq);
+	struct ieee80211_hw *hw = rtlworks->hw;
+
+	rtl_swlps_rf_awake(hw);
+}
+
+void rtl_swlps_rf_sleep(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	unsigned long flag;
+	u8 sleep_intv;
+
+	if (!rtlpriv->psc.sw_ps_enabled)
+		return;
+
+	if ((rtlpriv->sec.being_setkey) ||
+	    (mac->opmode == NL80211_IFTYPE_ADHOC))
+		return;
+
+	/*sleep after linked 10s, to let DHCP and 4-way handshake ok enough!! */
+	if ((mac->link_state != MAC80211_LINKED) || (mac->cnt_after_linked < 5))
+		return;
+
+	if (rtlpriv->link_info.b_busytraffic)
+		return;
+
+	spin_lock(&rtlpriv->locks.rf_ps_lock);
+	if (rtlpriv->psc.rfchange_inprogress) {
+		spin_unlock(&rtlpriv->locks.rf_ps_lock);
+		return;
+	}
+	spin_unlock(&rtlpriv->locks.rf_ps_lock);
+
+	spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag);
+	rtl_ps_set_rf_state(hw, ERFSLEEP, RF_CHANGE_BY_PS,false);
+	spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag);
+
+	if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM &&
+	    !RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
+		rtlpriv->intf_ops->enable_aspm(hw);
+		RT_SET_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
+	}
+
+	/* here is power save alg, when this beacon is DTIM
+	 * we will set sleep time to dtim_period * n;
+	 * when this beacon is not DTIM, we will set sleep
+	 * time to sleep_intv = rtlpriv->psc.dtim_counter or
+	 * MAX_SW_LPS_SLEEP_INTV(default set to 5) */
+
+/*<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;
+		else
+			sleep_intv = hw->conf.ps_dtim_period;
+	} 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;
+
+	/* this print should always be dtim_conter = 0 &
+	 * sleep  = dtim_period, that meaons, we should
+	 * awake before every dtim */
+	RT_TRACE(COMP_POWER, DBG_DMESG,
+		 ("dtim_counter:%x will sleep :%d beacon_intv\n",
+		  rtlpriv->psc.dtim_counter, sleep_intv));
+
+	/* we tested that 40ms is enough for sw & hw sw delay */
+	queue_delayed_work(rtlpriv->works.rtl_wq, &rtlpriv->works.ps_rfon_wq,
+			MSECS(sleep_intv * mac->vif->bss_conf.beacon_int - 40));
+}
+
+
+void rtl_swlps_wq_callback(void *data)
+{
+	struct rtl_works *rtlworks =
+		container_of_dwork_rtl(data, struct rtl_works, ps_work);
+	struct ieee80211_hw *hw = rtlworks->hw;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	bool ps = false;
+
+	ps = (hw->conf.flags & IEEE80211_CONF_PS);
+
+	/* we can sleep after ps null send ok */
+	if (rtlpriv->psc.state_inap) {
+		rtl_swlps_rf_sleep(hw);
+
+		if (rtlpriv->psc.state && !ps) {
+			rtlpriv->psc.sleep_ms =
+				jiffies_to_msecs(jiffies -
+						 rtlpriv->psc.last_action);
+		}
+
+		if (ps)
+			rtlpriv->psc.last_slept = jiffies;
+
+		rtlpriv->psc.last_action = jiffies;
+		rtlpriv->psc.state = ps;
+	}
+}
+
+
+void rtl_p2p_noa_ie(struct ieee80211_hw *hw, void *data, unsigned int len)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct ieee80211_mgmt *mgmt = (void *)data;
+	struct rtl_p2p_ps_info *p2pinfo = &(rtlpriv->psc.p2p_ps_info);
+	u8 *pos, *end, *ie;
+	u16 noa_len;
+	static u8 p2p_oui_ie_type[4] = {0x50, 0x6f, 0x9a, 0x09};
+	u8 noa_num, index,i, noa_index = 0;
+	bool find_p2p_ie = false , find_p2p_ps_ie = false;
+	pos = (u8 *)mgmt->u.beacon.variable;
+	end = data + len;
+	ie = NULL;
+
+	while (pos + 1 < end) {
+
+		if (pos + 2 + pos[1] > end)
+			return;
+
+		if (pos[0] == 221 && pos[1] > 4) {
+			if (memcmp(&pos[2], p2p_oui_ie_type, 4) == 0) {
+				ie = pos + 2+4;
+				break;
+			}
+		}
+		pos += 2 + pos[1];
+	}
+
+	if (ie == NULL)
+		return;
+	find_p2p_ie = true;
+	/*to find noa ie*/
+	while (ie + 1 < end) {
+		noa_len = READEF2BYTE(&ie[1]);
+		if (ie + 3 + ie[1] > end)
+			return;
+
+		if (ie[0] == 12) {
+			find_p2p_ps_ie = true;
+			if ( (noa_len - 2) % 13 != 0){
+				RT_TRACE(COMP_INIT, DBG_LOUD,
+					 ("P2P notice of absence: "
+					  "invalid length.%d\n",noa_len));
+				return;
+			} else {
+				noa_num = (noa_len - 2) / 13;
+			}
+			noa_index = ie[3];
+			if (rtlpriv->psc.p2p_ps_info.p2p_ps_mode == P2P_PS_NONE
+			    || noa_index != p2pinfo->noa_index) {
+				RT_TRACE(COMP_FW, DBG_LOUD,
+					 ("update NOA ie.\n"));
+				p2pinfo->noa_index = noa_index;
+				p2pinfo->opp_ps= (ie[4] >> 7);
+				p2pinfo->ctwindow = ie[4] & 0x7F;
+				p2pinfo->noa_num = noa_num;
+				index = 5;
+				for (i = 0; i< noa_num; i++){
+					p2pinfo->noa_count_type[i] =
+							READEF1BYTE(ie+index);
+					index += 1;
+					p2pinfo->noa_duration[i] =
+							READEF4BYTE(ie+index);
+					index += 4;
+					p2pinfo->noa_interval[i] =
+							READEF4BYTE(ie+index);
+					index += 4;
+					p2pinfo->noa_start_time[i] =
+							READEF4BYTE(ie+index);
+					index += 4;
+				}
+
+				if (p2pinfo->opp_ps == 1) {
+					p2pinfo->p2p_ps_mode = P2P_PS_CTWINDOW;
+					/* Driver should wait LPS
+					 * entering CTWindow*/
+					if (rtlpriv->psc.b_fw_current_inpsmode){
+						rtl_p2p_ps_cmd(hw,
+							       P2P_PS_ENABLE);
+					}
+				} else if (p2pinfo->noa_num > 0) {
+					p2pinfo->p2p_ps_mode = P2P_PS_NOA;
+					rtl_p2p_ps_cmd(hw, P2P_PS_ENABLE);
+				} else if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
+					rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);
+				}
+			}
+
+		break;
+		}
+		ie += 3 + noa_len;
+	}
+
+	if (find_p2p_ie == true) {
+		if ((p2pinfo->p2p_ps_mode > P2P_PS_NONE) &&
+		    (find_p2p_ps_ie == false))
+			rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);
+	}
+}
+
+void rtl_p2p_action_ie(struct ieee80211_hw *hw, void *data, unsigned int len)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct ieee80211_mgmt *mgmt = (void *)data;
+	struct rtl_p2p_ps_info *p2pinfo = &(rtlpriv->psc.p2p_ps_info);
+	bool find_p2p_ie = false , find_p2p_ps_ie = false;
+	u8 noa_num, index,i, noa_index = 0;
+	u8 *pos, *end, *ie;
+	u16 noa_len;
+	static u8 p2p_oui_ie_type[4] = {0x50, 0x6f, 0x9a, 0x09};
+
+	pos = (u8 *) &mgmt->u.action.category;
+	end = data + len;
+	ie = NULL;
+
+	if (pos[0] == 0x7f ) {
+		if (memcmp(&pos[1], p2p_oui_ie_type, 4) == 0) {
+			ie = pos + 3+4;
+		}
+	}
+
+	if (ie == NULL)
+		return;
+	find_p2p_ie = true;
+
+	RT_TRACE(COMP_FW, DBG_LOUD, ("action frame find P2P IE.\n"));
+	/*to find noa ie*/
+	while (ie + 1 < end) {
+		noa_len = READEF2BYTE(&ie[1]);
+		if (ie + 3 + ie[1] > end)
+			return;
+
+		if (ie[0] == 12) {
+			RT_TRACE(COMP_FW, DBG_LOUD, ("find NOA IE.\n"));
+			RT_PRINT_DATA(rtlpriv, COMP_FW, DBG_LOUD, ("noa ie "),
+				      ie, noa_len);
+			find_p2p_ps_ie = true;
+			if ( (noa_len - 2) % 13 != 0){
+				RT_TRACE(COMP_FW, DBG_LOUD,
+					 ("P2P notice of absence: "
+					  "invalid length.%d\n",noa_len));
+				return;
+			} else {
+				noa_num = (noa_len - 2) / 13;
+			}
+			noa_index = ie[3];
+			if (rtlpriv->psc.p2p_ps_info.p2p_ps_mode == P2P_PS_NONE
+			    || noa_index != p2pinfo->noa_index) {
+				p2pinfo->noa_index = noa_index;
+				p2pinfo->opp_ps= (ie[4] >> 7);
+				p2pinfo->ctwindow = ie[4] & 0x7F;
+				p2pinfo->noa_num = noa_num;
+				index = 5;
+				for (i = 0; i< noa_num; i++){
+					p2pinfo->noa_count_type[i] =
+							READEF1BYTE(ie+index);
+					index += 1;
+					p2pinfo->noa_duration[i] =
+							READEF4BYTE(ie+index);
+					index += 4;
+					p2pinfo->noa_interval[i] =
+							READEF4BYTE(ie+index);
+					index += 4;
+					p2pinfo->noa_start_time[i] =
+							READEF4BYTE(ie+index);
+					index += 4;
+				}
+
+				if (p2pinfo->opp_ps == 1) {
+					p2pinfo->p2p_ps_mode = P2P_PS_CTWINDOW;
+					/* Driver should wait LPS
+					 * entering CTWindow */
+					if (rtlpriv->psc.b_fw_current_inpsmode){
+						rtl_p2p_ps_cmd(hw,
+							       P2P_PS_ENABLE);
+					}
+				} else if (p2pinfo->noa_num > 0) {
+					p2pinfo->p2p_ps_mode = P2P_PS_NOA;
+					rtl_p2p_ps_cmd(hw, P2P_PS_ENABLE);
+				} else if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
+					rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);
+				}
+			}
+
+		break;
+		}
+		ie += 3 + noa_len;
+	}
+
+
+}
+
+
+void rtl_p2p_ps_cmd(struct ieee80211_hw *hw,u8 p2p_ps_state)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *rtlps = rtl_psc(rtl_priv(hw));
+	struct rtl_p2p_ps_info  *p2pinfo = &(rtlpriv->psc.p2p_ps_info);
+
+	RT_TRACE(COMP_FW, DBG_LOUD, (" p2p state %x\n",p2p_ps_state));
+	switch (p2p_ps_state) {
+		case P2P_PS_DISABLE:
+			p2pinfo->p2p_ps_state = p2p_ps_state;
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						   HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
+						   (u8 *)(&p2p_ps_state));
+
+			p2pinfo->noa_index = 0;
+			p2pinfo->ctwindow = 0;
+			p2pinfo->opp_ps = 0;
+			p2pinfo->noa_num = 0;
+			p2pinfo->p2p_ps_mode = P2P_PS_NONE;
+			if (rtlps->b_fw_current_inpsmode == true) {
+				if (rtlps->smart_ps == 0) {
+					rtlps->smart_ps = 2;
+					rtlpriv->cfg->ops->set_hw_reg(hw,
+						    HW_VAR_H2C_FW_PWRMODE,
+						    (u8 *)(&rtlps->pwr_mode));
+				}
+
+			}
+			break;
+		case P2P_PS_ENABLE:
+			if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
+				p2pinfo->p2p_ps_state = p2p_ps_state;
+
+				if (p2pinfo->ctwindow > 0) {
+					if (rtlps->smart_ps != 0){
+						rtlps->smart_ps = 0;
+						rtlpriv->cfg->ops->set_hw_reg(
+						    hw, HW_VAR_H2C_FW_PWRMODE,
+						    (u8 *)(&rtlps->pwr_mode));
+					}
+				}
+				rtlpriv->cfg->ops->set_hw_reg(hw,
+						HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
+						(u8 *)(&p2p_ps_state));
+
+			}
+			break;
+		case P2P_PS_SCAN:
+		case P2P_PS_SCAN_DONE:
+		case P2P_PS_ALLSTASLEEP:
+			if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
+				p2pinfo->p2p_ps_state = p2p_ps_state;
+				rtlpriv->cfg->ops->set_hw_reg(hw,
+						HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
+						(u8 *)(&p2p_ps_state));
+			}
+			break;
+		default:
+			break;
+
+	}
+	RT_TRACE(COMP_FW, DBG_LOUD, (" ctwindow %x oppps %x \n",
+		 		     p2pinfo->ctwindow,p2pinfo->opp_ps));
+	RT_TRACE(COMP_FW, DBG_LOUD, ("count %x duration %x index %x interval %x"
+				     " start time %x noa num %x\n",
+				     p2pinfo->noa_count_type[0],
+				     p2pinfo->noa_duration[0],
+				     p2pinfo->noa_index,
+				     p2pinfo->noa_interval[0],
+				     p2pinfo->noa_start_time[0],
+				     p2pinfo->noa_num));
+	RT_TRACE(COMP_FW, DBG_LOUD, ("end\n"));
+}
+
+void rtl_p2p_info(struct ieee80211_hw *hw, void *data, unsigned int len)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct ieee80211_hdr *hdr = (void *) data;
+
+	if (!mac->p2p)
+		return;
+	if (mac->link_state != MAC80211_LINKED)
+		return;
+	/* min. beacon length + FCS_LEN */
+	if (len <= 40 + FCS_LEN)
+		return;
+
+	/* and only beacons from the associated BSSID, please */
+	if (ether_addr_equal(hdr->addr3, rtlpriv->mac80211.bssid))
+		return;
+
+	/* check if this really is a beacon */
+	if (!(ieee80211_is_beacon(hdr->frame_control) ||
+	      ieee80211_is_probe_resp(hdr->frame_control) ||
+	      ieee80211_is_action(hdr->frame_control)))
+		return;
+
+	if (ieee80211_is_action(hdr->frame_control)) {
+		rtl_p2p_action_ie(hw,data,len - FCS_LEN);
+	} else {
+		rtl_p2p_noa_ie(hw,data,len - FCS_LEN);
+	}
+
+}
diff --git a/drivers/staging/rtl8821ae/ps.h b/drivers/staging/rtl8821ae/ps.h
new file mode 100644
index 000000000000..374ed77c4126
--- /dev/null
+++ b/drivers/staging/rtl8821ae/ps.h
@@ -0,0 +1,55 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __REALTEK_RTL_PCI_PS_H__
+#define __REALTEK_RTL_PCI_PS_H__
+
+#define MAX_SW_LPS_SLEEP_INTV	5
+
+bool rtl_ps_set_rf_state(struct ieee80211_hw *hw,
+			 enum rf_pwrstate state_toset, u32 changesource,
+			 bool protect_or_not);
+bool rtl_ps_enable_nic(struct ieee80211_hw *hw);
+bool rtl_ps_disable_nic(struct ieee80211_hw *hw);
+void rtl_ips_nic_off(struct ieee80211_hw *hw);
+void rtl_ips_nic_on(struct ieee80211_hw *hw);
+void rtl_ips_nic_off_wq_callback(void *data);
+void rtl_lps_enter(struct ieee80211_hw *hw);
+void rtl_lps_leave(struct ieee80211_hw *hw);
+
+void rtl_lps_set_psmode(struct ieee80211_hw *hw, u8 rt_psmode);
+
+void rtl_swlps_beacon(struct ieee80211_hw *hw, void *data, unsigned int len);
+void rtl_swlps_wq_callback(void *data);
+void rtl_swlps_rfon_wq_callback(void *data);
+void rtl_swlps_rf_awake(struct ieee80211_hw *hw);
+void rtl_swlps_rf_sleep(struct ieee80211_hw *hw);
+void rtl_p2p_ps_cmd(struct ieee80211_hw *hw,u8 p2p_ps_state);
+void rtl_p2p_info(struct ieee80211_hw *hw, void *data, unsigned int len);
+#endif
diff --git a/drivers/staging/rtl8821ae/rc.c b/drivers/staging/rtl8821ae/rc.c
new file mode 100644
index 000000000000..0cc32c60ddee
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rc.c
@@ -0,0 +1,308 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "wifi.h"
+#include "base.h"
+#include "rc.h"
+
+/*
+ *Finds the highest rate index we can use
+ *if skb is special data like DHCP/EAPOL, we set should
+ *it to lowest rate CCK_1M, otherwise we set rate to
+ *highest rate based on wireless mode used for iwconfig
+ *show Tx rate.
+ */
+static u8 _rtl_rc_get_highest_rix(struct rtl_priv *rtlpriv,
+				  struct ieee80211_sta *sta,
+				  struct sk_buff *skb, bool not_data)
+{
+	struct rtl_mac *rtlmac = rtl_mac(rtlpriv);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_sta_info *sta_entry = NULL;
+	u8 wireless_mode = 0;
+
+	/*
+	 *this rate is no use for true rate, firmware
+	 *will control rate at all it just used for
+	 *1.show in iwconfig in B/G mode
+	 *2.in rtl_get_tcb_desc when we check rate is
+	 *      1M we will not use FW rate but user rate.
+	 */
+	if (rtlmac->opmode == NL80211_IFTYPE_AP ||
+	    rtlmac->opmode == NL80211_IFTYPE_ADHOC ||
+	    rtlmac->opmode == NL80211_IFTYPE_MESH_POINT) {
+		if (sta) {
+			sta_entry = (struct rtl_sta_info *) sta->drv_priv;
+			wireless_mode = sta_entry->wireless_mode;
+		} else {
+			return 0;
+		}
+	} else {
+		wireless_mode = rtlmac->mode;
+	}
+
+	if (rtl_is_special_data(rtlpriv->mac80211.hw, skb, true) || not_data) {
+		return 0;
+	} else {
+		if (rtlhal->current_bandtype == BAND_ON_2_4G) {
+			if (wireless_mode == WIRELESS_MODE_B) {
+				return B_MODE_MAX_RIX;
+			} else if (wireless_mode == WIRELESS_MODE_G) {
+				return G_MODE_MAX_RIX;
+			} else {
+				if (get_rf_type(rtlphy) != RF_2T2R)
+					return N_MODE_MCS7_RIX;
+				else
+					return N_MODE_MCS15_RIX;
+			}
+		} else {
+			if (wireless_mode == WIRELESS_MODE_A) {
+				return A_MODE_MAX_RIX;
+			} else {
+				if (get_rf_type(rtlphy) != RF_2T2R)
+					return N_MODE_MCS7_RIX;
+				else
+					return N_MODE_MCS15_RIX;
+			}
+		}
+	}
+}
+
+static void _rtl_rc_rate_set_series(struct rtl_priv *rtlpriv,
+				    struct ieee80211_sta *sta,
+				    struct ieee80211_tx_rate *rate,
+				    struct ieee80211_tx_rate_control *txrc,
+				    u8 tries, char rix, int rtsctsenable,
+				    bool not_data)
+{
+	struct rtl_mac *mac = rtl_mac(rtlpriv);
+	u8 sgi_20 = 0, sgi_40 = 0;
+
+	if (sta) {
+		sgi_20 = sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20;
+		sgi_40 = sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40;
+	}
+	rate->count = tries;
+	rate->idx = rix >= 0x00 ? rix : 0x00;
+
+	if (!not_data) {
+		if (txrc->short_preamble)
+			rate->flags |= IEEE80211_TX_RC_USE_SHORT_PREAMBLE;
+		if (mac->opmode == NL80211_IFTYPE_AP ||
+			mac->opmode == NL80211_IFTYPE_ADHOC) {
+			if (sta && (sta->ht_cap.cap &
+				    IEEE80211_HT_CAP_SUP_WIDTH_20_40))
+				rate->flags |= IEEE80211_TX_RC_40_MHZ_WIDTH;
+		} else {
+			if (mac->bw_40)
+				rate->flags |= IEEE80211_TX_RC_40_MHZ_WIDTH;
+		}
+		if (sgi_20 || sgi_40)
+			rate->flags |= IEEE80211_TX_RC_SHORT_GI;
+		if (sta && sta->ht_cap.ht_supported)
+			rate->flags |= IEEE80211_TX_RC_MCS;
+	}
+}
+
+static void rtl_get_rate(void *ppriv, struct ieee80211_sta *sta,
+			 void *priv_sta,
+			 struct ieee80211_tx_rate_control *txrc)
+{
+	struct rtl_priv *rtlpriv = ppriv;
+	struct sk_buff *skb = txrc->skb;
+	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
+	struct ieee80211_tx_rate *rates = tx_info->control.rates;
+	__le16 fc = rtl_get_fc(skb);
+	u8 try_per_rate, i, rix;
+	bool not_data = !ieee80211_is_data(fc);
+
+	if (rate_control_send_low(sta, priv_sta, txrc))
+		return;
+
+	rix = _rtl_rc_get_highest_rix(rtlpriv, sta, skb, not_data);
+	try_per_rate = 1;
+	_rtl_rc_rate_set_series(rtlpriv, sta, &rates[0], txrc,
+				try_per_rate, rix, 1, not_data);
+
+	if (!not_data) {
+		for (i = 1; i < 4; i++)
+			_rtl_rc_rate_set_series(rtlpriv, sta, &rates[i],
+						txrc, i, (rix - i), 1,
+						not_data);
+	}
+}
+
+static bool _rtl_tx_aggr_check(struct rtl_priv *rtlpriv,
+			       struct rtl_sta_info *sta_entry, u16 tid)
+{
+	struct rtl_mac *mac = rtl_mac(rtlpriv);
+
+	if (mac->act_scanning)
+		return false;
+
+	if (mac->opmode == NL80211_IFTYPE_STATION &&
+	    mac->cnt_after_linked < 3)
+		return false;
+
+	if (sta_entry->tids[tid].agg.agg_state == RTL_AGG_STOP)
+		return true;
+
+	return false;
+}
+
+/*mac80211 Rate Control callbacks*/
+static void rtl_tx_status(void *ppriv,
+			  struct ieee80211_supported_band *sband,
+			  struct ieee80211_sta *sta, void *priv_sta,
+			  struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = ppriv;
+	struct rtl_mac *mac = rtl_mac(rtlpriv);
+	struct ieee80211_hdr *hdr = rtl_get_hdr(skb);
+	__le16 fc = rtl_get_fc(skb);
+	struct rtl_sta_info *sta_entry;
+
+	if (!priv_sta || !ieee80211_is_data(fc))
+		return;
+
+	if (rtl_is_special_data(mac->hw, skb, true))
+		return;
+
+	if (is_multicast_ether_addr(ieee80211_get_DA(hdr)) ||
+	    is_broadcast_ether_addr(ieee80211_get_DA(hdr)))
+		return;
+
+	if (sta) {
+		/* Check if aggregation has to be enabled for this tid */
+		sta_entry = (struct rtl_sta_info *) sta->drv_priv;
+		if ((sta->ht_cap.ht_supported == true) &&
+				!(skb->protocol == cpu_to_be16(ETH_P_PAE))) {
+			if (ieee80211_is_data_qos(fc)) {
+				u8 tid = rtl_get_tid(skb);
+				if (_rtl_tx_aggr_check(rtlpriv, sta_entry,
+						       tid)) {
+					sta_entry->tids[tid].agg.agg_state =
+						RTL_AGG_PROGRESS;
+					/*<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>*/
+				}
+			}
+		}
+	}
+}
+
+static void rtl_rate_init(void *ppriv,
+			  struct ieee80211_supported_band *sband,
+			  struct cfg80211_chan_def *chandef,
+			  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,
+			    struct ieee80211_sta *sta, void *priv_sta,
+			    u32 changed)
+{
+}
+#endif
+static void *rtl_rate_alloc(struct ieee80211_hw *hw, struct dentry *debugfsdir)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	return rtlpriv;
+}
+
+static void rtl_rate_free(void *rtlpriv)
+{
+	return;
+}
+
+static void *rtl_rate_alloc_sta(void *ppriv,
+				struct ieee80211_sta *sta, gfp_t gfp)
+{
+	struct rtl_priv *rtlpriv = ppriv;
+	struct rtl_rate_priv *rate_priv;
+
+	rate_priv = kzalloc(sizeof(struct rtl_rate_priv), gfp);
+	if (!rate_priv) {
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("Unable to allocate private rc structure\n"));
+		return NULL;
+	}
+
+	rtlpriv->rate_priv = rate_priv;
+
+	return rate_priv;
+}
+
+static void rtl_rate_free_sta(void *rtlpriv,
+			      struct ieee80211_sta *sta, void *priv_sta)
+{
+	struct rtl_rate_priv *rate_priv = priv_sta;
+	kfree(rate_priv);
+}
+
+static struct rate_control_ops rtl_rate_ops = {
+	.name = "rtl_rc",
+	.alloc = rtl_rate_alloc,
+	.free = rtl_rate_free,
+	.alloc_sta = rtl_rate_alloc_sta,
+	.free_sta = rtl_rate_free_sta,
+	.rate_init = rtl_rate_init,
+	.rate_update = rtl_rate_update,
+	.tx_status = rtl_tx_status,
+	.get_rate = rtl_get_rate,
+};
+
+int rtl_rate_control_register(void)
+{
+	return ieee80211_rate_control_register(&rtl_rate_ops);
+}
+
+void rtl_rate_control_unregister(void)
+{
+	ieee80211_rate_control_unregister(&rtl_rate_ops);
+}
diff --git a/drivers/staging/rtl8821ae/rc.h b/drivers/staging/rtl8821ae/rc.h
new file mode 100644
index 000000000000..4afa2c20adcf
--- /dev/null
+++ b/drivers/staging/rtl8821ae/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.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL_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
+
+struct rtl_rate_priv {
+	u8 ht_cap;
+};
+
+int rtl_rate_control_register(void);
+void rtl_rate_control_unregister(void);
+#endif
diff --git a/drivers/staging/rtl8821ae/regd.c b/drivers/staging/rtl8821ae/regd.c
new file mode 100644
index 000000000000..d89f15cb8089
--- /dev/null
+++ b/drivers/staging/rtl8821ae/regd.c
@@ -0,0 +1,503 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "wifi.h"
+#include "regd.h"
+
+static struct country_code_to_enum_rd allCountries[] = {
+	{COUNTRY_CODE_FCC, "US"},
+	{COUNTRY_CODE_IC, "US"},
+	{COUNTRY_CODE_ETSI, "EC"},
+	{COUNTRY_CODE_SPAIN, "EC"},
+	{COUNTRY_CODE_FRANCE, "EC"},
+	{COUNTRY_CODE_MKK, "JP"},
+	{COUNTRY_CODE_MKK1, "JP"},
+	{COUNTRY_CODE_ISRAEL, "EC"},
+	{COUNTRY_CODE_TELEC, "JP"},
+	{COUNTRY_CODE_MIC, "JP"},
+	{COUNTRY_CODE_GLOBAL_DOMAIN, "JP"},
+	{COUNTRY_CODE_WORLD_WIDE_13, "EC"},
+	{COUNTRY_CODE_TELEC_NETGEAR, "EC"},
+};
+
+/*
+ *Only these channels all allow active
+ *scan on all world regulatory domains
+ */
+#define RTL819x_2GHZ_CH01_11	\
+	REG_RULE(2412-10, 2462+10, 40, 0, 20, 0)
+
+/*
+ *We enable active scan on these a case
+ *by case basis by regulatory domain
+ */
+#define RTL819x_2GHZ_CH12_13	\
+	REG_RULE(2467-10, 2472+10, 40, 0, 20,\
+	NL80211_RRF_PASSIVE_SCAN)
+
+#define RTL819x_2GHZ_CH14	\
+	REG_RULE(2484-10, 2484+10, 40, 0, 20, \
+	NL80211_RRF_PASSIVE_SCAN | \
+	NL80211_RRF_NO_OFDM)
+
+/* 5G chan 36 - chan 64*/
+#define RTL819x_5GHZ_5150_5350	\
+	REG_RULE(5150-10, 5350+10, 40, 0, 30, \
+	NL80211_RRF_PASSIVE_SCAN | \
+	NL80211_RRF_NO_IBSS)
+
+/* 5G chan 100 - chan 165*/
+#define RTL819x_5GHZ_5470_5850	\
+	REG_RULE(5470-10, 5850+10, 40, 0, 30, \
+	NL80211_RRF_PASSIVE_SCAN | \
+	NL80211_RRF_NO_IBSS)
+
+/* 5G chan 149 - chan 165*/
+#define RTL819x_5GHZ_5725_5850	\
+	REG_RULE(5725-10, 5850+10, 40, 0, 30, \
+	NL80211_RRF_PASSIVE_SCAN | \
+	NL80211_RRF_NO_IBSS)
+
+#define RTL819x_5GHZ_ALL	\
+	RTL819x_5GHZ_5150_5350, RTL819x_5GHZ_5470_5850
+
+static const struct ieee80211_regdomain rtl_regdom_11 = {
+	.n_reg_rules = 1,
+	.alpha2 = "99",
+	.reg_rules = {
+		      RTL819x_2GHZ_CH01_11,
+		      }
+};
+
+static const struct ieee80211_regdomain rtl_regdom_12_13 = {
+	.n_reg_rules = 2,
+	.alpha2 = "99",
+	.reg_rules = {
+		      RTL819x_2GHZ_CH01_11,
+			  RTL819x_2GHZ_CH12_13,
+		      }
+};
+
+static const struct ieee80211_regdomain rtl_regdom_no_midband = {
+	.n_reg_rules = 3,
+	.alpha2 = "99",
+	.reg_rules = {
+		      RTL819x_2GHZ_CH01_11,
+			  RTL819x_5GHZ_5150_5350,
+			  RTL819x_5GHZ_5725_5850,
+		      }
+};
+
+static const struct ieee80211_regdomain rtl_regdom_60_64 = {
+	.n_reg_rules = 3,
+	.alpha2 = "99",
+	.reg_rules = {
+		      RTL819x_2GHZ_CH01_11,
+			  RTL819x_2GHZ_CH12_13,
+			  RTL819x_5GHZ_5725_5850,
+		      }
+};
+
+static const struct ieee80211_regdomain rtl_regdom_14_60_64 = {
+	.n_reg_rules = 4,
+	.alpha2 = "99",
+	.reg_rules = {
+		      RTL819x_2GHZ_CH01_11,
+			  RTL819x_2GHZ_CH12_13,
+			  RTL819x_2GHZ_CH14,
+			  RTL819x_5GHZ_5725_5850,
+		      }
+};
+
+static const struct ieee80211_regdomain rtl_regdom_14 = {
+	.n_reg_rules = 3,
+	.alpha2 = "99",
+	.reg_rules = {
+		      RTL819x_2GHZ_CH01_11,
+			  RTL819x_2GHZ_CH12_13,
+			  RTL819x_2GHZ_CH14,
+		      }
+};
+
+static bool _rtl_is_radar_freq(u16 center_freq)
+{
+	return (center_freq >= 5260 && center_freq <= 5700);
+}
+
+static void _rtl_reg_apply_beaconing_flags(struct wiphy *wiphy,
+					   enum nl80211_reg_initiator initiator)
+{
+	enum ieee80211_band band;
+	struct ieee80211_supported_band *sband;
+	const struct ieee80211_reg_rule *reg_rule;
+	struct ieee80211_channel *ch;
+	unsigned int i;
+#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])
+			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) {
+#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
+				 *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 (LINUX_VERSION_CODE < KERNEL_VERSION(3,9,0))
+	u32 bandwidth = 0;
+	int r;
+#endif
+
+	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 */
+#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;
+	}
+}
+
+/*
+ *Always apply Radar/DFS rules on
+ *freq range 5260 MHz - 5700 MHz
+ */
+static void _rtl_reg_apply_radar_flags(struct wiphy *wiphy)
+{
+	struct ieee80211_supported_band *sband;
+	struct ieee80211_channel *ch;
+	unsigned int i;
+
+	if (!wiphy->bands[IEEE80211_BAND_5GHZ])
+		return;
+
+	sband = wiphy->bands[IEEE80211_BAND_5GHZ];
+
+	for (i = 0; i < sband->n_channels; i++) {
+		ch = &sband->channels[i];
+		if (!_rtl_is_radar_freq(ch->center_freq))
+			continue;
+
+		/*
+		 *We always enable radar detection/DFS on this
+		 *frequency range. Additionally we also apply on
+		 *this frequency range:
+		 *- If STA mode does not yet have DFS supports disable
+		 * active scanning
+		 *- If adhoc mode does not support DFS yet then disable
+		 * adhoc in the frequency.
+		 *- If AP mode does not yet support radar detection/DFS
+		 *do not allow AP mode
+		 */
+		if (!(ch->flags & IEEE80211_CHAN_DISABLED))
+			ch->flags |= IEEE80211_CHAN_RADAR |
+			    IEEE80211_CHAN_NO_IBSS |
+			    IEEE80211_CHAN_PASSIVE_SCAN;
+	}
+}
+
+static void _rtl_reg_apply_world_flags(struct wiphy *wiphy,
+				       enum nl80211_reg_initiator initiator,
+				       struct rtl_regulatory *reg)
+{
+	_rtl_reg_apply_beaconing_flags(wiphy, initiator);
+	_rtl_reg_apply_active_scan_flags(wiphy, initiator);
+	return;
+}
+
+static void _rtl_dump_channel_map(struct wiphy *wiphy)
+{
+	enum ieee80211_band band;
+	struct ieee80211_supported_band *sband;
+	struct ieee80211_channel *ch;
+	unsigned int i;
+
+	for (band = 0; band < IEEE80211_NUM_BANDS; band++) {
+		if (!wiphy->bands[band])
+			continue;
+		sband = wiphy->bands[band];
+		for (i = 0; i < sband->n_channels; i++)
+			ch = &sband->channels[i];
+	}
+}
+
+static int _rtl_reg_notifier_apply(struct wiphy *wiphy,
+				   struct regulatory_request *request,
+				   struct rtl_regulatory *reg)
+{
+	/* We always apply this */
+	_rtl_reg_apply_radar_flags(wiphy);
+
+	switch (request->initiator) {
+	case NL80211_REGDOM_SET_BY_DRIVER:
+	case NL80211_REGDOM_SET_BY_CORE:
+	case NL80211_REGDOM_SET_BY_USER:
+		break;
+	case NL80211_REGDOM_SET_BY_COUNTRY_IE:
+		_rtl_reg_apply_world_flags(wiphy, request->initiator, reg);
+		break;
+	}
+
+	_rtl_dump_channel_map(wiphy);
+
+	return 0;
+}
+
+static const struct ieee80211_regdomain *_rtl_regdomain_select(
+						struct rtl_regulatory *reg)
+{
+	switch (reg->country_code) {
+	case COUNTRY_CODE_FCC:
+		return &rtl_regdom_no_midband;
+	case COUNTRY_CODE_IC:
+		return &rtl_regdom_11;
+	case COUNTRY_CODE_ETSI:
+	case COUNTRY_CODE_TELEC_NETGEAR:
+		return &rtl_regdom_60_64;
+	case COUNTRY_CODE_SPAIN:
+	case COUNTRY_CODE_FRANCE:
+	case COUNTRY_CODE_ISRAEL:
+	case COUNTRY_CODE_WORLD_WIDE_13:
+		return &rtl_regdom_12_13;
+	case COUNTRY_CODE_MKK:
+	case COUNTRY_CODE_MKK1:
+	case COUNTRY_CODE_TELEC:
+	case COUNTRY_CODE_MIC:
+		return &rtl_regdom_14_60_64;
+	case COUNTRY_CODE_GLOBAL_DOMAIN:
+		return &rtl_regdom_14;
+	default:
+		return &rtl_regdom_no_midband;
+	}
+}
+
+#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;
+
+	wiphy->reg_notifier = reg_notifier;
+
+	wiphy->flags |= WIPHY_FLAG_CUSTOM_REGULATORY;
+	wiphy->flags &= ~WIPHY_FLAG_STRICT_REGULATORY;
+	wiphy->flags &= ~WIPHY_FLAG_DISABLE_BEACON_HINTS;
+
+	regd = _rtl_regdomain_select(reg);
+	wiphy_apply_custom_regulatory(wiphy, regd);
+	_rtl_reg_apply_radar_flags(wiphy);
+	_rtl_reg_apply_world_flags(wiphy, NL80211_REGDOM_SET_BY_DRIVER, reg);
+	return 0;
+}
+
+static struct country_code_to_enum_rd *_rtl_regd_find_country(u16 countrycode)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(allCountries); i++) {
+		if (allCountries[i].countrycode == countrycode)
+			return &allCountries[i];
+	}
+	return NULL;
+}
+
+#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;
+	struct country_code_to_enum_rd *country = NULL;
+
+	if (wiphy == NULL || &rtlpriv->regd == NULL)
+		return -EINVAL;
+
+	/* init country_code from efuse channel plan */
+	rtlpriv->regd.country_code = rtlpriv->efuse.channel_plan;
+
+	RT_TRACE(COMP_REGD, DBG_TRACE,
+		 (KERN_DEBUG "rtl: EEPROM regdomain: 0x%0x\n",
+		  rtlpriv->regd.country_code));
+
+	if (rtlpriv->regd.country_code >= COUNTRY_CODE_MAX) {
+		RT_TRACE(COMP_REGD, DBG_DMESG,
+			 (KERN_DEBUG "rtl: EEPROM indicates invalid 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]));
+
+	_rtl_regd_init_wiphy(&rtlpriv->regd, wiphy, reg_notifier);
+
+	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);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	RT_TRACE(COMP_REGD, DBG_LOUD, ("\n"));
+
+	_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
new file mode 100644
index 000000000000..abc60ab8165c
--- /dev/null
+++ b/drivers/staging/rtl8821ae/regd.h
@@ -0,0 +1,75 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL_REGD_H__
+#define __RTL_REGD_H__
+
+#define IEEE80211_CHAN_NO_IBSS		1<<2
+#define IEEE80211_CHAN_PASSIVE_SCAN	1<<1
+#define WIPHY_FLAG_CUSTOM_REGULATORY	BIT(0)
+#define WIPHY_FLAG_STRICT_REGULATORY	BIT(1)
+#define WIPHY_FLAG_DISABLE_BEACON_HINTS	BIT(2)
+
+struct country_code_to_enum_rd {
+	u16 countrycode;
+	const char *iso_name;
+};
+
+enum country_code_type_t {
+	COUNTRY_CODE_FCC = 0,
+	COUNTRY_CODE_IC = 1,
+	COUNTRY_CODE_ETSI = 2,
+	COUNTRY_CODE_SPAIN = 3,
+	COUNTRY_CODE_FRANCE = 4,
+	COUNTRY_CODE_MKK = 5,
+	COUNTRY_CODE_MKK1 = 6,
+	COUNTRY_CODE_ISRAEL = 7,
+	COUNTRY_CODE_TELEC = 8,
+	COUNTRY_CODE_MIC = 9,
+	COUNTRY_CODE_GLOBAL_DOMAIN = 10,
+	COUNTRY_CODE_WORLD_WIDE_13 = 11,
+	COUNTRY_CODE_TELEC_NETGEAR = 12,
+
+	/*add new channel plan above this line */
+	COUNTRY_CODE_MAX
+};
+
+#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/btc.h b/drivers/staging/rtl8821ae/rtl8821ae/btc.h
new file mode 100644
index 000000000000..74ac189e3a88
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/btc.h
@@ -0,0 +1,87 @@
+
+/******************************************************************************
+ **
+ ** Copyright(c) 2009-2010  Realtek Corporation.
+ **
+ ** This program is free software; you can redistribute it and/or modify it
+ ** under the terms of version 2 of the GNU General Public License as
+ ** published by the Free Software Foundation.
+ **
+ ** This program is distributed in the hope that it will be useful, but WITHOUT
+ ** ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ ** FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ ** more details.
+ **
+ ** You should have received a copy of the GNU General Public License along with
+ ** this program; if not, write to the Free Software Foundation, Inc.,
+ ** 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ **
+ ** The full GNU General Public License is included in this distribution in the
+ ** file called LICENSE.
+ **
+ ** Contact Information:
+ ** wlanfae <wlanfae@realtek.com>
+ ** Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ ** Hsinchu 300, Taiwan.
+ ** Larry Finger <Larry.Finger@lwfinger.net>
+ **
+ ******************************************************************************/
+
+#ifndef __RTL8821AE_BTC_H__
+#define __RTL8821AE_BTC_H__
+
+#include "../wifi.h"
+#include "hal_bt_coexist.h"
+
+struct bt_coexist_c2h_info {
+	u8 no_parse_c2h;
+	u8 has_c2h;
+};
+
+struct btdm_8821ae {
+	bool b_all_off;
+	bool b_agc_table_en;
+	bool b_adc_back_off_on;
+	bool b2_ant_hid_en;
+	bool b_low_penalty_rate_adaptive;
+	bool b_rf_rx_lpf_shrink;
+	bool b_reject_aggre_pkt;
+	bool b_tra_tdma_on;
+	u8 tra_tdma_nav;
+	u8 tra_tdma_ant;
+	bool b_tdma_on;
+	u8 tdma_ant;
+	u8 tdma_nav;
+	u8 tdma_dac_swing;
+	u8 fw_dac_swing_lvl;
+	bool b_ps_tdma_on;
+	u8 ps_tdma_byte[5];
+	bool b_pta_on;
+	u32 val_0x6c0;
+	u32 val_0x6c8;
+	u32 val_0x6cc;
+	bool b_sw_dac_swing_on;
+	u32 sw_dac_swing_lvl;
+	u32 wlan_act_hi;
+	u32 wlan_act_lo;
+	u32 bt_retry_index;
+	bool b_dec_bt_pwr;
+	bool b_ignore_wlan_act;
+};
+
+struct bt_coexist_8821ae {
+	u32 high_priority_tx;
+	u32 high_priority_rx;
+	u32 low_priority_tx;
+	u32 low_priority_rx;
+	u8 c2h_bt_info;
+	bool b_c2h_bt_info_req_sent;
+	bool b_c2h_bt_inquiry_page;
+	u32 bt_inq_page_start_time;
+	u8 bt_retry_cnt;
+	u8 c2h_bt_info_original;
+	u8 bt_inquiry_page_cnt;
+	struct btdm_8821ae btdm;
+};
+
+#endif
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/def.h b/drivers/staging/rtl8821ae/rtl8821ae/def.h
new file mode 100644
index 000000000000..72ebdeaa6e7d
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/def.h
@@ -0,0 +1,442 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL8821AE_DEF_H__
+#define __RTL8821AE_DEF_H__
+
+/*--------------------------Define -------------------------------------------*/
+/* BIT 7 HT Rate*/
+/*TxHT = 0*/
+#define	MGN_1M				0x02
+#define	MGN_2M				0x04
+#define	MGN_5_5M			0x0b
+#define	MGN_11M				0x16
+
+#define	MGN_6M				0x0c
+#define	MGN_9M				0x12
+#define	MGN_12M				0x18
+#define	MGN_18M				0x24
+#define	MGN_24M				0x30
+#define	MGN_36M				0x48
+#define	MGN_48M				0x60
+#define	MGN_54M				0x6c
+
+// TxHT = 1
+#define	MGN_MCS0			0x80
+#define	MGN_MCS1			0x81
+#define	MGN_MCS2			0x82
+#define	MGN_MCS3			0x83
+#define	MGN_MCS4			0x84
+#define	MGN_MCS5			0x85
+#define	MGN_MCS6			0x86
+#define	MGN_MCS7			0x87
+#define	MGN_MCS8			0x88
+#define	MGN_MCS9			0x89
+#define	MGN_MCS10			0x8a
+#define	MGN_MCS11			0x8b
+#define	MGN_MCS12			0x8c
+#define	MGN_MCS13			0x8d
+#define	MGN_MCS14			0x8e
+#define	MGN_MCS15			0x8f
+//VHT rate
+#define	MGN_VHT1SS_MCS0		0x90
+#define	MGN_VHT1SS_MCS1		0x91
+#define	MGN_VHT1SS_MCS2		0x92
+#define	MGN_VHT1SS_MCS3		0x93
+#define	MGN_VHT1SS_MCS4		0x94
+#define	MGN_VHT1SS_MCS5		0x95
+#define	MGN_VHT1SS_MCS6		0x96
+#define	MGN_VHT1SS_MCS7		0x97
+#define	MGN_VHT1SS_MCS8		0x98
+#define	MGN_VHT1SS_MCS9		0x99
+#define	MGN_VHT2SS_MCS0		0x9a
+#define	MGN_VHT2SS_MCS1		0x9b
+#define	MGN_VHT2SS_MCS2		0x9c
+#define	MGN_VHT2SS_MCS3		0x9d
+#define	MGN_VHT2SS_MCS4		0x9e
+#define	MGN_VHT2SS_MCS5		0x9f
+#define	MGN_VHT2SS_MCS6		0xa0
+#define	MGN_VHT2SS_MCS7		0xa1
+#define	MGN_VHT2SS_MCS8		0xa2
+#define	MGN_VHT2SS_MCS9		0xa3
+
+#define	MGN_VHT3SS_MCS0		0xa4
+#define	MGN_VHT3SS_MCS1		0xa5
+#define	MGN_VHT3SS_MCS2		0xa6
+#define	MGN_VHT3SS_MCS3		0xa7
+#define	MGN_VHT3SS_MCS4		0xa8
+#define	MGN_VHT3SS_MCS5		0xa9
+#define	MGN_VHT3SS_MCS6		0xaa
+#define	MGN_VHT3SS_MCS7		0xab
+#define	MGN_VHT3SS_MCS8		0xac
+#define	MGN_VHT3SS_MCS9		0xad
+
+#define	MGN_MCS0_SG			0xc0
+#define	MGN_MCS1_SG			0xc1
+#define	MGN_MCS2_SG			0xc2
+#define	MGN_MCS3_SG			0xc3
+#define	MGN_MCS4_SG			0xc4
+#define	MGN_MCS5_SG			0xc5
+#define	MGN_MCS6_SG			0xc6
+#define	MGN_MCS7_SG			0xc7
+#define	MGN_MCS8_SG			0xc8
+#define	MGN_MCS9_SG			0xc9
+#define	MGN_MCS10_SG		0xca
+#define	MGN_MCS11_SG		0xcb
+#define	MGN_MCS12_SG		0xcc
+#define	MGN_MCS13_SG		0xcd
+#define	MGN_MCS14_SG		0xce
+#define	MGN_MCS15_SG		0xcf
+
+#define	MGN_UNKNOWN			0xff
+
+
+/* 30 ms */
+#define	WIFI_NAV_UPPER_US				30000
+#define HAL_92C_NAV_UPPER_UNIT			128
+
+#define HAL_RETRY_LIMIT_INFRA				48
+#define HAL_RETRY_LIMIT_AP_ADHOC			7
+
+#define RESET_DELAY_8185					20
+
+#define RT_IBSS_INT_MASKS	(IMR_BCNINT | IMR_TBDOK | IMR_TBDER)
+#define RT_AC_INT_MASKS		(IMR_VIDOK | IMR_VODOK | IMR_BEDOK|IMR_BKDOK)
+
+#define NUM_OF_FIRMWARE_QUEUE				10
+#define NUM_OF_PAGES_IN_FW					0x100
+#define NUM_OF_PAGE_IN_FW_QUEUE_BK			0x07
+#define NUM_OF_PAGE_IN_FW_QUEUE_BE			0x07
+#define NUM_OF_PAGE_IN_FW_QUEUE_VI			0x07
+#define NUM_OF_PAGE_IN_FW_QUEUE_VO			0x07
+#define NUM_OF_PAGE_IN_FW_QUEUE_HCCA		0x0
+#define NUM_OF_PAGE_IN_FW_QUEUE_CMD			0x0
+#define NUM_OF_PAGE_IN_FW_QUEUE_MGNT		0x02
+#define NUM_OF_PAGE_IN_FW_QUEUE_HIGH		0x02
+#define NUM_OF_PAGE_IN_FW_QUEUE_BCN			0x2
+#define NUM_OF_PAGE_IN_FW_QUEUE_PUB			0xA1
+
+#define NUM_OF_PAGE_IN_FW_QUEUE_BK_DTM		0x026
+#define NUM_OF_PAGE_IN_FW_QUEUE_BE_DTM		0x048
+#define NUM_OF_PAGE_IN_FW_QUEUE_VI_DTM		0x048
+#define NUM_OF_PAGE_IN_FW_QUEUE_VO_DTM		0x026
+#define NUM_OF_PAGE_IN_FW_QUEUE_PUB_DTM		0x00
+
+#define MAX_RX_DMA_BUFFER_SIZE				0x3E80
+
+
+#define MAX_LINES_HWCONFIG_TXT				1000
+#define MAX_BYTES_LINE_HWCONFIG_TXT			256
+
+#define SW_THREE_WIRE						0
+#define HW_THREE_WIRE						2
+
+#define BT_DEMO_BOARD						0
+#define BT_QA_BOARD							1
+#define BT_FPGA								2
+
+#define HAL_PRIME_CHNL_OFFSET_DONT_CARE		0
+#define HAL_PRIME_CHNL_OFFSET_LOWER			1
+#define HAL_PRIME_CHNL_OFFSET_UPPER			2
+
+#define MAX_H2C_QUEUE_NUM					10
+
+#define RX_MPDU_QUEUE						0
+#define RX_CMD_QUEUE						1
+#define RX_MAX_QUEUE						2
+#define AC2QUEUEID(_AC)						(_AC)
+
+#define	C2H_RX_CMD_HDR_LEN					8
+#define	GET_C2H_CMD_CMD_LEN(__prxhdr)		\
+	LE_BITS_TO_4BYTE((__prxhdr), 0, 16)
+#define	GET_C2H_CMD_ELEMENT_ID(__prxhdr)	\
+	LE_BITS_TO_4BYTE((__prxhdr), 16, 8)
+#define	GET_C2H_CMD_CMD_SEQ(__prxhdr)		\
+	LE_BITS_TO_4BYTE((__prxhdr), 24, 7)
+#define	GET_C2H_CMD_CONTINUE(__prxhdr)		\
+	LE_BITS_TO_4BYTE((__prxhdr), 31, 1)
+#define	GET_C2H_CMD_CONTENT(__prxhdr)		\
+	((u8*)(__prxhdr) + C2H_RX_CMD_HDR_LEN)
+
+#define	GET_C2H_CMD_FEEDBACK_ELEMENT_ID(__pcmdfbhdr)	\
+	LE_BITS_TO_4BYTE((__pcmdfbhdr), 0, 8)
+#define	GET_C2H_CMD_FEEDBACK_CCX_LEN(__pcmdfbhdr)		\
+	LE_BITS_TO_4BYTE((__pcmdfbhdr), 8, 8)
+#define	GET_C2H_CMD_FEEDBACK_CCX_CMD_CNT(__pcmdfbhdr)	\
+	LE_BITS_TO_4BYTE((__pcmdfbhdr), 16, 16)
+#define	GET_C2H_CMD_FEEDBACK_CCX_MAC_ID(__pcmdfbhdr)	\
+	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 0, 5)
+#define	GET_C2H_CMD_FEEDBACK_CCX_VALID(__pcmdfbhdr)		\
+	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 7, 1)
+#define	GET_C2H_CMD_FEEDBACK_CCX_RETRY_CNT(__pcmdfbhdr)	\
+	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 8, 5)
+#define	GET_C2H_CMD_FEEDBACK_CCX_TOK(__pcmdfbhdr)		\
+	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 15, 1)
+#define	GET_C2H_CMD_FEEDBACK_CCX_QSEL(__pcmdfbhdr)		\
+	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 16, 4)
+#define	GET_C2H_CMD_FEEDBACK_CCX_SEQ(__pcmdfbhdr)		\
+	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 20, 12)
+
+#define CHIP_BONDING_IDENTIFIER(_value)	(((_value)>>22)&0x3)
+
+#define CHIP_8812				BIT(2)
+#define CHIP_8821				(BIT(0)|BIT(2))
+
+#define CHIP_8821A						(BIT(0)|BIT(2))
+#define NORMAL_CHIP  					BIT(3)
+#define RF_TYPE_1T1R					(~(BIT(4)|BIT(5)|BIT(6)))
+#define RF_TYPE_1T2R					BIT(4)
+#define RF_TYPE_2T2R					BIT(5)
+#define CHIP_VENDOR_UMC					BIT(7)
+#define B_CUT_VERSION					BIT(12)
+#define C_CUT_VERSION					BIT(13)
+#define D_CUT_VERSION					((BIT(12)|BIT(13)))
+#define E_CUT_VERSION					BIT(14)
+#define	RF_RL_ID						(BIT(31)|BIT(30)|BIT(29)|BIT(28))
+
+
+
+enum version_8821ae {
+	VERSION_TEST_CHIP_1T1R_8812 = 0x0004,
+	VERSION_TEST_CHIP_2T2R_8812 = 0x0024,
+	VERSION_NORMAL_TSMC_CHIP_1T1R_8812 = 0x100c,
+	VERSION_NORMAL_TSMC_CHIP_2T2R_8812 = 0x102c,
+	VERSION_NORMAL_TSMC_CHIP_1T1R_8812_C_CUT = 0x200c,
+	VERSION_NORMAL_TSMC_CHIP_2T2R_8812_C_CUT = 0x202c,
+	VERSION_TEST_CHIP_8821 = 0x0005,
+	VERSION_NORMAL_TSMC_CHIP_8821 = 0x000d,
+	VERSION_NORMAL_TSMC_CHIP_8821_B_CUT = 0x100d,
+	VERSION_UNKNOWN = 0xFF,
+};
+
+enum vht_data_sc{
+	VHT_DATA_SC_DONOT_CARE = 0,
+	VHT_DATA_SC_20_UPPER_OF_80MHZ = 1,
+	VHT_DATA_SC_20_LOWER_OF_80MHZ = 2,
+	VHT_DATA_SC_20_UPPERST_OF_80MHZ = 3,
+	VHT_DATA_SC_20_LOWEST_OF_80MHZ = 4,
+	VHT_DATA_SC_20_RECV1 = 5,
+	VHT_DATA_SC_20_RECV2 = 6,
+	VHT_DATA_SC_20_RECV3 = 7,
+	VHT_DATA_SC_20_RECV4 = 8,
+	VHT_DATA_SC_40_UPPER_OF_80MHZ = 9,
+	VHT_DATA_SC_40_LOWER_OF_80MHZ = 10,
+};
+
+
+/* MASK */
+#define IC_TYPE_MASK					(BIT(0)|BIT(1)|BIT(2))
+#define CHIP_TYPE_MASK 					BIT(3)
+#define RF_TYPE_MASK					(BIT(4)|BIT(5)|BIT(6))
+#define MANUFACTUER_MASK				BIT(7)
+#define ROM_VERSION_MASK				(BIT(11)|BIT(10)|BIT(9)|BIT(8))
+#define CUT_VERSION_MASK				(BIT(15)|BIT(14)|BIT(13)|BIT(12))
+
+/* Get element */
+#define GET_CVID_IC_TYPE(version)			((version) & IC_TYPE_MASK)
+#define GET_CVID_CHIP_TYPE(version)			((version) & CHIP_TYPE_MASK)
+#define GET_CVID_RF_TYPE(version)			((version) & RF_TYPE_MASK)
+#define GET_CVID_MANUFACTUER(version)		((version) & MANUFACTUER_MASK)
+#define GET_CVID_ROM_VERSION(version)		((version) & ROM_VERSION_MASK)
+#define GET_CVID_CUT_VERSION(version)		((version) & CUT_VERSION_MASK)
+
+#define IS_1T1R(version)			((GET_CVID_RF_TYPE(version))? false : true)
+#define IS_1T2R(version)			((GET_CVID_RF_TYPE(version) == RF_TYPE_1T2R)\
+									? true : false)
+#define IS_2T2R(version)			((GET_CVID_RF_TYPE(version) == RF_TYPE_2T2R)\
+									? true : false)
+
+#define IS_8812_SERIES(version)			((GET_CVID_IC_TYPE(version) == CHIP_8812)? \
+										true : false)
+#define IS_8821_SERIES(version)			((GET_CVID_IC_TYPE(version) == CHIP_8821)? \
+										true : false)
+
+#define IS_VENDOR_8812A_TEST_CHIP(version)	((IS_8812_SERIES(version)) ? \
+												((IS_NORMAL_CHIP(version)) ? \
+												false : true) : false)
+#define IS_VENDOR_8812A_MP_CHIP(version)		((IS_8812_SERIES(version)) ? \
+												((IS_NORMAL_CHIP(version)) ? \
+												true : false) : false)
+#define IS_VENDOR_8812A_C_CUT(version)		((IS_8812_SERIES(version)) ? \
+											((GET_CVID_CUT_VERSION(version) == C_CUT_VERSION) ? \
+											true : false) : false)
+
+#define IS_VENDOR_8821A_TEST_CHIP(version)	((IS_8821_SERIES(version)) ? \
+												((IS_NORMAL_CHIP(version)) ? \
+												false : true) : false)
+#define IS_VENDOR_8821A_MP_CHIP(version)		((IS_8821_SERIES(version)) ? \
+												((IS_NORMAL_CHIP(version)) ? \
+												true : false) : false)
+#define IS_VENDOR_8821A_B_CUT(version)		((IS_8821_SERIES(version)) ? \
+												((GET_CVID_CUT_VERSION(version) == B_CUT_VERSION) ? \
+												true : false) : false)
+
+
+enum rf_optype {
+	RF_OP_BY_SW_3WIRE = 0,
+	RF_OP_BY_FW,
+	RF_OP_MAX
+};
+
+enum rf_power_state {
+	RF_ON,
+	RF_OFF,
+	RF_SLEEP,
+	RF_SHUT_DOWN,
+};
+
+enum power_save_mode {
+	POWER_SAVE_MODE_ACTIVE,
+	POWER_SAVE_MODE_SAVE,
+};
+
+enum power_polocy_config {
+	POWERCFG_MAX_POWER_SAVINGS,
+	POWERCFG_GLOBAL_POWER_SAVINGS,
+	POWERCFG_LOCAL_POWER_SAVINGS,
+	POWERCFG_LENOVO,
+};
+
+enum interface_select_pci {
+	INTF_SEL1_MINICARD = 0,
+	INTF_SEL0_PCIE = 1,
+	INTF_SEL2_RSV = 2,
+	INTF_SEL3_RSV = 3,
+};
+
+enum hal_fw_c2h_cmd_id {
+	HAL_FW_C2H_CMD_Read_MACREG = 0,
+	HAL_FW_C2H_CMD_Read_BBREG = 1,
+	HAL_FW_C2H_CMD_Read_RFREG = 2,
+	HAL_FW_C2H_CMD_Read_EEPROM = 3,
+	HAL_FW_C2H_CMD_Read_EFUSE = 4,
+	HAL_FW_C2H_CMD_Read_CAM = 5,
+	HAL_FW_C2H_CMD_Get_BasicRate = 6,
+	HAL_FW_C2H_CMD_Get_DataRate = 7,
+	HAL_FW_C2H_CMD_Survey = 8,
+	HAL_FW_C2H_CMD_SurveyDone = 9,
+	HAL_FW_C2H_CMD_JoinBss = 10,
+	HAL_FW_C2H_CMD_AddSTA = 11,
+	HAL_FW_C2H_CMD_DelSTA = 12,
+	HAL_FW_C2H_CMD_AtimDone = 13,
+	HAL_FW_C2H_CMD_TX_Report = 14,
+	HAL_FW_C2H_CMD_CCX_Report = 15,
+	HAL_FW_C2H_CMD_DTM_Report = 16,
+	HAL_FW_C2H_CMD_TX_Rate_Statistics = 17,
+	HAL_FW_C2H_CMD_C2HLBK = 18,
+	HAL_FW_C2H_CMD_C2HDBG = 19,
+	HAL_FW_C2H_CMD_C2HFEEDBACK = 20,
+	HAL_FW_C2H_CMD_MAX
+};
+
+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_desc8821ae_rate {
+	DESC_RATE1M = 0x00,
+	DESC_RATE2M = 0x01,
+	DESC_RATE5_5M = 0x02,
+	DESC_RATE11M = 0x03,
+
+	DESC_RATE6M = 0x04,
+	DESC_RATE9M = 0x05,
+	DESC_RATE12M = 0x06,
+	DESC_RATE18M = 0x07,
+	DESC_RATE24M = 0x08,
+	DESC_RATE36M = 0x09,
+	DESC_RATE48M = 0x0a,
+	DESC_RATE54M = 0x0b,
+
+	DESC_RATEMCS0 = 0x0c,
+	DESC_RATEMCS1 = 0x0d,
+	DESC_RATEMCS2 = 0x0e,
+	DESC_RATEMCS3 = 0x0f,
+	DESC_RATEMCS4 = 0x10,
+	DESC_RATEMCS5 = 0x11,
+	DESC_RATEMCS6 = 0x12,
+	DESC_RATEMCS7 = 0x13,
+	DESC_RATEMCS8 = 0x14,
+	DESC_RATEMCS9 = 0x15,
+	DESC_RATEMCS10 = 0x16,
+	DESC_RATEMCS11 = 0x17,
+	DESC_RATEMCS12 = 0x18,
+	DESC_RATEMCS13 = 0x19,
+	DESC_RATEMCS14 = 0x1a,
+	DESC_RATEMCS15 = 0x1b,
+	DESC_RATEVHT1SS_MCS0 = 0x1c,
+	DESC_RATEVHT1SS_MCS1 = 0x1d,
+	DESC_RATEVHT1SS_MCS2 = 0x1e,
+	DESC_RATEVHT1SS_MCS3 = 0x1f,
+	DESC_RATEVHT1SS_MCS4 = 0x20,
+	DESC_RATEVHT1SS_MCS5 = 0x21,
+	DESC_RATEVHT1SS_MCS6 = 0x22,
+	DESC_RATEVHT1SS_MCS7 = 0x23,
+	DESC_RATEVHT1SS_MCS8 = 0x24,
+	DESC_RATEVHT1SS_MCS9 = 0x25,
+	DESC_RATEVHT2SS_MCS0 = 0x26,
+	DESC_RATEVHT2SS_MCS1 = 0x27,
+	DESC_RATEVHT2SS_MCS2 = 0x28,
+	DESC_RATEVHT2SS_MCS3 = 0x29,
+	DESC_RATEVHT2SS_MCS4 = 0x2a,
+	DESC_RATEVHT2SS_MCS5 = 0x2b,
+	DESC_RATEVHT2SS_MCS6 = 0x2c,
+	DESC_RATEVHT2SS_MCS7 = 0x2d,
+	DESC_RATEVHT2SS_MCS8 = 0x2e,
+	DESC_RATEVHT2SS_MCS9 = 0x2f,
+};
+
+enum rx_packet_type{
+	NORMAL_RX,
+	TX_REPORT1,
+	TX_REPORT2,
+	HIS_REPORT,
+	C2H_PACKET,
+};
+
+struct phy_sts_cck_8821ae_t {
+	u8 adc_pwdb_X[4];
+	u8 sq_rpt;
+	u8 cck_agc_rpt;
+};
+
+struct h2c_cmd_8821ae {
+	u8 element_id;
+	u32 cmd_len;
+	u8 *p_cmdbuffer;
+};
+
+#endif
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/dm.c b/drivers/staging/rtl8821ae/rtl8821ae/dm.c
new file mode 100644
index 000000000000..8634206b8929
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/dm.c
@@ -0,0 +1,3045 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../base.h"
+#include "../pci.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "dm.h"
+#include "fw.h"
+#include "trx.h"
+#include "../btcoexist/rtl_btc.h"
+
+struct dig_t dm_digtable;
+static struct ps_t dm_pstable;
+
+static const u32 rtl8812ae_txscaling_table[TXSCALE_TABLE_SIZE] =
+{
+	0x081, // 0,  -12.0dB
+	0x088, // 1,  -11.5dB
+	0x090, // 2,  -11.0dB
+	0x099, // 3,  -10.5dB
+	0x0A2, // 4,  -10.0dB
+	0x0AC, // 5,  -9.5dB
+	0x0B6, // 6,  -9.0dB
+	0x0C0, // 7,  -8.5dB
+	0x0CC, // 8,  -8.0dB
+	0x0D8, // 9,  -7.5dB
+	0x0E5, // 10, -7.0dB
+	0x0F2, // 11, -6.5dB
+	0x101, // 12, -6.0dB
+	0x110, // 13, -5.5dB
+	0x120, // 14, -5.0dB
+	0x131, // 15, -4.5dB
+	0x143, // 16, -4.0dB
+	0x156, // 17, -3.5dB
+	0x16A, // 18, -3.0dB
+	0x180, // 19, -2.5dB
+	0x197, // 20, -2.0dB
+	0x1AF, // 21, -1.5dB
+	0x1C8, // 22, -1.0dB
+	0x1E3, // 23, -0.5dB
+	0x200, // 24, +0  dB
+	0x21E, // 25, +0.5dB
+	0x23E, // 26, +1.0dB
+	0x261, // 27, +1.5dB
+	0x285, // 28, +2.0dB
+	0x2AB, // 29, +2.5dB
+	0x2D3, // 30, +3.0dB
+	0x2FE, // 31, +3.5dB
+	0x32B, // 32, +4.0dB
+	0x35C, // 33, +4.5dB
+	0x38E, // 34, +5.0dB
+	0x3C4, // 35, +5.5dB
+	0x3FE  // 36, +6.0dB
+};
+
+static const u32 rtl8821ae_txscaling_table[TXSCALE_TABLE_SIZE] = {
+	0x081, // 0,  -12.0dB
+	0x088, // 1,  -11.5dB
+	0x090, // 2,  -11.0dB
+	0x099, // 3,  -10.5dB
+	0x0A2, // 4,  -10.0dB
+	0x0AC, // 5,  -9.5dB
+	0x0B6, // 6,  -9.0dB
+	0x0C0, // 7,  -8.5dB
+	0x0CC, // 8,  -8.0dB
+	0x0D8, // 9,  -7.5dB
+	0x0E5, // 10, -7.0dB
+	0x0F2, // 11, -6.5dB
+	0x101, // 12, -6.0dB
+	0x110, // 13, -5.5dB
+	0x120, // 14, -5.0dB
+	0x131, // 15, -4.5dB
+	0x143, // 16, -4.0dB
+	0x156, // 17, -3.5dB
+	0x16A, // 18, -3.0dB
+	0x180, // 19, -2.5dB
+	0x197, // 20, -2.0dB
+	0x1AF, // 21, -1.5dB
+	0x1C8, // 22, -1.0dB
+	0x1E3, // 23, -0.5dB
+	0x200, // 24, +0  dB
+	0x21E, // 25, +0.5dB
+	0x23E, // 26, +1.0dB
+	0x261, // 27, +1.5dB
+	0x285, // 28, +2.0dB
+	0x2AB, // 29, +2.5dB
+	0x2D3, // 30, +3.0dB
+	0x2FE, // 31, +3.5dB
+	0x32B, // 32, +4.0dB
+	0x35C, // 33, +4.5dB
+	0x38E, // 34, +5.0dB
+	0x3C4, // 35, +5.5dB
+	0x3FE  // 36, +6.0dB
+};
+
+static const u32 ofdmswing_table[] = {
+	0x0b40002d, // 0,  -15.0dB
+	0x0c000030, // 1,  -14.5dB
+	0x0cc00033, // 2,  -14.0dB
+	0x0d800036, // 3,  -13.5dB
+	0x0e400039, // 4,  -13.0dB
+	0x0f00003c, // 5,  -12.5dB
+	0x10000040, // 6,  -12.0dB
+	0x11000044, // 7,  -11.5dB
+	0x12000048, // 8,  -11.0dB
+	0x1300004c, // 9,  -10.5dB
+	0x14400051, // 10, -10.0dB
+	0x15800056, // 11, -9.5dB
+	0x16c0005b, // 12, -9.0dB
+	0x18000060, // 13, -8.5dB
+	0x19800066, // 14, -8.0dB
+	0x1b00006c, // 15, -7.5dB
+	0x1c800072, // 16, -7.0dB
+	0x1e400079, // 17, -6.5dB
+	0x20000080, // 18, -6.0dB
+	0x22000088, // 19, -5.5dB
+	0x24000090, // 20, -5.0dB
+	0x26000098, // 21, -4.5dB
+	0x288000a2, // 22, -4.0dB
+	0x2ac000ab, // 23, -3.5dB
+	0x2d4000b5, // 24, -3.0dB
+	0x300000c0, // 25, -2.5dB
+	0x32c000cb, // 26, -2.0dB
+	0x35c000d7, // 27, -1.5dB
+	0x390000e4, // 28, -1.0dB
+	0x3c8000f2, // 29, -0.5dB
+	0x40000100, // 30, +0dB
+	0x43c0010f, // 31, +0.5dB
+	0x47c0011f, // 32, +1.0dB
+	0x4c000130, // 33, +1.5dB
+	0x50800142, // 34, +2.0dB
+	0x55400155, // 35, +2.5dB
+	0x5a400169, // 36, +3.0dB
+	0x5fc0017f, // 37, +3.5dB
+	0x65400195, // 38, +4.0dB
+	0x6b8001ae, // 39, +4.5dB
+	0x71c001c7, // 40, +5.0dB
+	0x788001e2, // 41, +5.5dB
+	0x7f8001fe  // 42, +6.0dB
+};
+
+static const u8 cckswing_table_ch1ch13[CCK_TABLE_SIZE][8] = {
+	{0x09, 0x08, 0x07, 0x06, 0x04, 0x03, 0x01, 0x01},	//  0, -16.0dB
+	{0x09, 0x09, 0x08, 0x06, 0x05, 0x03, 0x01, 0x01},	//  1, -15.5dB
+	{0x0a, 0x09, 0x08, 0x07, 0x05, 0x03, 0x02, 0x01},	//  2, -15.0dB
+	{0x0a, 0x0a, 0x09, 0x07, 0x05, 0x03, 0x02, 0x01},	//  3, -14.5dB
+	{0x0b, 0x0a, 0x09, 0x08, 0x06, 0x04, 0x02, 0x01},	//  4, -14.0dB
+	{0x0b, 0x0b, 0x0a, 0x08, 0x06, 0x04, 0x02, 0x01},	//  5, -13.5dB
+	{0x0c, 0x0c, 0x0a, 0x09, 0x06, 0x04, 0x02, 0x01},	//  6, -13.0dB
+	{0x0d, 0x0c, 0x0b, 0x09, 0x07, 0x04, 0x02, 0x01},	//  7, -12.5dB
+	{0x0d, 0x0d, 0x0c, 0x0a, 0x07, 0x05, 0x02, 0x01},	//  8, -12.0dB
+	{0x0e, 0x0e, 0x0c, 0x0a, 0x08, 0x05, 0x02, 0x01},	//  9, -11.5dB
+	{0x0f, 0x0f, 0x0d, 0x0b, 0x08, 0x05, 0x03, 0x01},	// 10, -11.0dB
+	{0x10, 0x10, 0x0e, 0x0b, 0x08, 0x05, 0x03, 0x01},	// 11, -10.5dB
+	{0x11, 0x11, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01},	// 12, -10.0dB
+	{0x12, 0x12, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01},	// 13, -9.5dB
+	{0x13, 0x13, 0x10, 0x0d, 0x0a, 0x06, 0x03, 0x01},	// 14, -9.0dB
+	{0x14, 0x14, 0x11, 0x0e, 0x0b, 0x07, 0x03, 0x02},	// 15, -8.5dB
+	{0x16, 0x15, 0x12, 0x0f, 0x0b, 0x07, 0x04, 0x01},	// 16, -8.0dB
+	{0x17, 0x16, 0x13, 0x10, 0x0c, 0x08, 0x04, 0x02},	// 17, -7.5dB
+	{0x18, 0x17, 0x15, 0x11, 0x0c, 0x08, 0x04, 0x02},	// 18, -7.0dB
+	{0x1a, 0x19, 0x16, 0x12, 0x0d, 0x09, 0x04, 0x02},	// 19, -6.5dB
+    {0x1b, 0x1a, 0x17, 0x13, 0x0e, 0x09, 0x04, 0x02},	// 20, -6.0dB
+	{0x1d, 0x1c, 0x18, 0x14, 0x0f, 0x0a, 0x05, 0x02},	// 21, -5.5dB
+	{0x1f, 0x1e, 0x1a, 0x15, 0x10, 0x0a, 0x05, 0x02},	// 22, -5.0dB
+	{0x20, 0x20, 0x1b, 0x16, 0x11, 0x08, 0x05, 0x02},	// 23, -4.5dB
+	{0x22, 0x21, 0x1d, 0x18, 0x11, 0x0b, 0x06, 0x02},	// 24, -4.0dB
+	{0x24, 0x23, 0x1f, 0x19, 0x13, 0x0c, 0x06, 0x03},	// 25, -3.5dB
+	{0x26, 0x25, 0x21, 0x1b, 0x14, 0x0d, 0x06, 0x03},	// 26, -3.0dB
+	{0x28, 0x28, 0x22, 0x1c, 0x15, 0x0d, 0x07, 0x03},	// 27, -2.5dB
+	{0x2b, 0x2a, 0x25, 0x1e, 0x16, 0x0e, 0x07, 0x03},	// 28, -2.0dB
+	{0x2d, 0x2d, 0x27, 0x1f, 0x18, 0x0f, 0x08, 0x03},	// 29, -1.5dB
+	{0x30, 0x2f, 0x29, 0x21, 0x19, 0x10, 0x08, 0x03},	// 30, -1.0dB
+	{0x33, 0x32, 0x2b, 0x23, 0x1a, 0x11, 0x08, 0x04},	// 31, -0.5dB
+	{0x36, 0x35, 0x2e, 0x25, 0x1c, 0x12, 0x09, 0x04} 	// 32, +0dB
+};
+
+static const u8 cckswing_table_ch14[CCK_TABLE_SIZE][8]= {
+	{0x09, 0x08, 0x07, 0x04, 0x00, 0x00, 0x00, 0x00},	//  0, -16.0dB
+	{0x09, 0x09, 0x08, 0x05, 0x00, 0x00, 0x00, 0x00},	//  1, -15.5dB
+	{0x0a, 0x09, 0x08, 0x05, 0x00, 0x00, 0x00, 0x00},	//  2, -15.0dB
+	{0x0a, 0x0a, 0x09, 0x05, 0x00, 0x00, 0x00, 0x00},	//  3, -14.5dB
+	{0x0b, 0x0a, 0x09, 0x05, 0x00, 0x00, 0x00, 0x00},	//  4, -14.0dB
+	{0x0b, 0x0b, 0x0a, 0x06, 0x00, 0x00, 0x00, 0x00},	//  5, -13.5dB
+	{0x0c, 0x0c, 0x0a, 0x06, 0x00, 0x00, 0x00, 0x00},	//  6, -13.0dB
+	{0x0d, 0x0c, 0x0b, 0x06, 0x00, 0x00, 0x00, 0x00},	//  7, -12.5dB
+	{0x0d, 0x0d, 0x0c, 0x07, 0x00, 0x00, 0x00, 0x00},	//  8, -12.0dB
+	{0x0e, 0x0e, 0x0c, 0x07, 0x00, 0x00, 0x00, 0x00},	//  9, -11.5dB
+	{0x0f, 0x0f, 0x0d, 0x08, 0x00, 0x00, 0x00, 0x00},	// 10, -11.0dB
+	{0x10, 0x10, 0x0e, 0x08, 0x00, 0x00, 0x00, 0x00},	// 11, -10.5dB
+	{0x11, 0x11, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00},	// 12, -10.0dB
+	{0x12, 0x12, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00},	// 13, -9.5dB
+	{0x13, 0x13, 0x10, 0x0a, 0x00, 0x00, 0x00, 0x00},	// 14, -9.0dB
+	{0x14, 0x14, 0x11, 0x0a, 0x00, 0x00, 0x00, 0x00},	// 15, -8.5dB
+	{0x16, 0x15, 0x12, 0x0b, 0x00, 0x00, 0x00, 0x00},	// 16, -8.0dB
+	{0x17, 0x16, 0x13, 0x0b, 0x00, 0x00, 0x00, 0x00},	// 17, -7.5dB
+	{0x18, 0x17, 0x15, 0x0c, 0x00, 0x00, 0x00, 0x00},	// 18, -7.0dB
+	{0x1a, 0x19, 0x16, 0x0d, 0x00, 0x00, 0x00, 0x00},	// 19, -6.5dB
+	{0x1b, 0x1a, 0x17, 0x0e, 0x00, 0x00, 0x00, 0x00},	// 20, -6.0dB
+	{0x1d, 0x1c, 0x18, 0x0e, 0x00, 0x00, 0x00, 0x00},	// 21, -5.5dB
+	{0x1f, 0x1e, 0x1a, 0x0f, 0x00, 0x00, 0x00, 0x00},	// 22, -5.0dB
+	{0x20, 0x20, 0x1b, 0x10, 0x00, 0x00, 0x00, 0x00},	// 23, -4.5dB
+	{0x22, 0x21, 0x1d, 0x11, 0x00, 0x00, 0x00, 0x00},	// 24, -4.0dB
+	{0x24, 0x23, 0x1f, 0x12, 0x00, 0x00, 0x00, 0x00},	// 25, -3.5dB
+	{0x26, 0x25, 0x21, 0x13, 0x00, 0x00, 0x00, 0x00},	// 26, -3.0dB
+	{0x28, 0x28, 0x24, 0x14, 0x00, 0x00, 0x00, 0x00},	// 27, -2.5dB
+	{0x2b, 0x2a, 0x25, 0x15, 0x00, 0x00, 0x00, 0x00},	// 28, -2.0dB
+	{0x2d, 0x2d, 0x17, 0x17, 0x00, 0x00, 0x00, 0x00},	// 29, -1.5dB
+	{0x30, 0x2f, 0x29, 0x18, 0x00, 0x00, 0x00, 0x00},	// 30, -1.0dB
+	{0x33, 0x32, 0x2b, 0x19, 0x00, 0x00, 0x00, 0x00},	// 31, -0.5dB
+	{0x36, 0x35, 0x2e, 0x1b, 0x00, 0x00, 0x00, 0x00} 	// 32, +0dB
+};
+
+static const u32 edca_setting_dl[PEER_MAX] = {
+	0xa44f, 	/* 0 UNKNOWN */
+ 	0x5ea44f,	/* 1 REALTEK_90 */
+	0x5e4322,	/* 2 REALTEK_92SE */
+	0x5ea42b,		/* 3 BROAD	*/
+	0xa44f,		/* 4 RAL */
+	0xa630,		/* 5 ATH */
+	0x5ea630,		/* 6 CISCO */
+	0x5ea42b,		/* 7 MARVELL */
+};
+
+static const u32 edca_setting_ul[PEER_MAX] = {
+	0x5e4322, 	/* 0 UNKNOWN */
+	0xa44f,		/* 1 REALTEK_90 */
+	0x5ea44f,	/* 2 REALTEK_92SE */
+	0x5ea32b, 	/* 3 BROAD */
+	0x5ea422,	/* 4 RAL */
+	0x5ea322, 	/* 5 ATH */
+	0x3ea430,	/* 6 CISCO */
+	0x5ea44f,	/* 7 MARV */
+};
+
+static u8 rtl8818e_delta_swing_table_idx_24gb_p_txpwrtrack[] =
+	{0, 0, 0, 0, 1, 1, 2, 2, 3, 3, 4, 4, 4,  4,  4,  4,  4,  4,  5,  5,  7,  7,  8,  8,  8,  9,  9,  9,  9,  9};
+static u8 rtl8818e_delta_swing_table_idx_24gb_n_txpwrtrack[] =
+	{0, 0, 0, 2, 2, 3, 3, 4, 4, 4, 4, 5, 5,  6,  6,  7,  7,  7,  7,  8,  8,  9,  9, 10, 10, 10, 11, 11, 11, 11};
+
+
+u8 rtl8812ae_delta_swing_table_idx_24gb_n_txpwrtrack[]    =
+	{0, 1, 1, 1, 2, 2, 2, 3, 3,  3,  4,  4,  5,  5,  5,  6,  6,  6,  7,  8,  9,  9,  9,  9, 10, 10, 10, 10, 11, 11};
+u8 rtl8812ae_delta_swing_table_idx_24gb_p_txpwrtrack[]    =
+	{0, 0, 1, 1, 2, 2, 2, 2, 3,  3,  3,  4,  4,  5,  5,  6,  6,  6,  7,  7,  7,  8,  8,  8,  9,  9,  9,  9,  9,  9};
+u8 rtl8812ae_delta_swing_table_idx_24ga_n_txpwrtrack[]    =
+	{0, 1, 1, 1, 2, 2, 2, 3, 3,  3,  4,  4,  5,  5,  5,  6,  6,  6,  7,  8,  8,  9,  9,  9, 10, 10, 10, 10, 11, 11};
+u8 rtl8812ae_delta_swing_table_idx_24ga_p_txpwrtrack[]    =
+	{0, 0, 1, 1, 2, 2, 2, 2, 3,  3,  3,  4,  4,  5,  5,  6,  6,  6,  7,  7,  7,  8,  8,  8,  9,  9,  9,  9,  9,  9};
+u8 rtl8812ae_delta_swing_table_idx_24gcckb_n_txpwrtrack[] =
+	{0, 1, 1, 1, 2, 2, 2, 3, 3,  3,  4,  4,  5,  5,  5,  6,  6,  6,  7,  8,  9,  9,  9,  9, 10, 10, 10, 10, 11, 11};
+u8 rtl8812ae_delta_swing_table_idx_24gcckb_p_txpwrtrack[] =
+	{0, 0, 1, 1, 2, 2, 2, 2, 3,  3,  3,  4,  4,  5,  5,  6,  6,  6,  7,  7,  7,  8,  8,  8,  9,  9,  9,  9,  9,  9};
+u8 rtl8812ae_delta_swing_table_idx_24gccka_n_txpwrtrack[] =
+	{0, 1, 1, 1, 2, 2, 2, 3, 3,  3,  4,  4,  5,  5,  5,  6,  6,  6,  7,  8,  8,  9,  9,  9, 10, 10, 10, 10, 11, 11};
+u8 rtl8812ae_delta_swing_table_idx_24gccka_p_txpwrtrack[] =
+	{0, 0, 1, 1, 2, 2, 2, 2, 3,  3,  3,  4,  4,  5,  5,  6,  6,  6,  7,  7,  7,  8,  8,  8,  9,  9,  9,  9,  9,  9};
+
+u8 rtl8812ae_delta_swing_table_idx_5gb_n_txpwrtrack[][DELTA_SWINGIDX_SIZE] = {
+	{0, 1, 1, 2, 2, 2, 3, 3, 4,  4,  5,  5,  6,  6,  6,  7,  7,  7,  8,  8,  9,  9,  9, 10, 10, 11, 11, 12, 12, 13},
+	{0, 1, 1, 2, 2, 2, 3, 3, 4,  4,  4,  5,  5,  6,  6,  7,  7,  8,  8,  9,  9, 10, 10, 11, 11, 12, 12, 12, 13, 13},
+	{0, 1, 1, 2, 3, 3, 4, 4, 5,  6,  6,  7,  8,  9, 10, 11, 12, 12, 13, 14, 14, 14, 15, 16, 17, 17, 17, 18, 18, 18},
+};
+u8 rtl8812ae_delta_swing_table_idx_5gb_p_txpwrtrack[][DELTA_SWINGIDX_SIZE] = {
+	{0, 1, 1, 2, 2, 3, 3, 4, 4,  5,  5,  6,  6,  6,  7,  7,  8,  8,  9,  9, 10, 10, 11, 11, 11, 11, 11, 11, 11, 11},
+	{0, 1, 1, 2, 2, 3, 3, 4, 4,  4,  5,  5,  6,  6,  7,  7,  8,  8,  9,  9, 10, 10, 11, 11, 11, 11, 11, 11, 11, 11},
+	{0, 1, 1, 2, 2, 3, 3, 4, 4,  5,  5,  6,  7,  7,  8,  8,  9,  9, 10, 10, 11, 11, 11, 11, 11, 11, 11, 11, 11, 11},
+};
+u8 rtl8812ae_delta_swing_table_idx_5ga_n_txpwrtrack[][DELTA_SWINGIDX_SIZE] = {
+	{0, 1, 1, 2, 2, 3, 3, 4, 4,  4,  5,  5,  6,  6,  7,  7,  8,  8,  9,  9, 10, 10, 11, 11, 12, 12, 12, 13, 13, 13},
+	{0, 1, 1, 2, 2, 2, 3, 3, 4,  4,  5,  5,  6,  6,  7,  8,  9,  9, 10, 10, 11, 11, 11, 12, 12, 12, 12, 12, 13, 13},
+	{0, 1, 1, 2, 2, 3, 3, 4, 5,  6,  7,  8,  8,  9, 10, 11, 12, 13, 14, 14, 15, 15, 15, 16, 16, 16, 17, 17, 18, 18},
+};
+u8 rtl8812ae_delta_swing_table_idx_5ga_p_txpwrtrack[][DELTA_SWINGIDX_SIZE] = {
+	{0, 1, 1, 2, 2, 3, 3, 4, 4,  4,  4,  5,  5,  6,  7,  7,  8,  8,  9,  9, 10, 10, 11, 11, 11, 11, 11, 11, 11, 11},
+	{0, 1, 1, 2, 2, 3, 3, 4, 4,  4,  5,  5,  6,  6,  7,  7,  8,  9,  9, 10, 11, 11, 11, 11, 11, 11, 11, 11, 11, 11},
+	{0, 1, 1, 2, 3, 3, 4, 4, 5,  6,  6,  7,  7,  8,  9,  9, 10, 10, 11, 11, 11, 11, 11, 11, 11, 11, 11, 11, 11, 11},
+};
+
+u8 rtl8821ae_delta_swing_table_idx_24gb_n_txpwrtrack[] =
+	{0, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 4, 5, 5, 5, 6, 6, 6, 7, 7, 7, 8, 8, 8, 9, 9, 9, 10, 10, 10};
+u8 rtl8821ae_delta_swing_table_idx_24gb_p_txpwrtrack[]  =
+	{0, 0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 7, 8, 8, 9, 9, 10, 10, 11, 11, 12, 12, 12, 12, 12, 12};
+u8 rtl8821ae_delta_swing_table_idx_24ga_n_txpwrtrack[]  =
+	{0, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 4, 5, 5, 5, 6, 6, 6, 7, 7, 7, 8, 8, 8, 9, 9, 9, 10, 10, 10};
+u8 rtl8821ae_delta_swing_table_idx_24ga_p_txpwrtrack[] =
+	{0, 0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 7, 8, 8, 9, 9, 10, 10, 11, 11, 12, 12, 12, 12, 12, 12};
+u8 rtl8821ae_delta_swing_table_idx_24gcckb_n_txpwrtrack[] =
+	{0, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 4, 5, 5, 5, 6, 6, 6, 7, 7, 7, 8, 8, 8, 9, 9, 9, 10, 10, 10};
+u8 rtl8821ae_delta_swing_table_idx_24gcckb_p_txpwrtrack[] =
+	{0, 0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 7, 8, 8, 9, 9, 10, 10, 11, 11, 12, 12, 12, 12, 12, 12};
+u8 rtl8821ae_delta_swing_table_idx_24gccka_n_txpwrtrack[] =
+	{0, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 4, 5, 5, 5, 6, 6, 6, 7, 7, 7, 8, 8, 8, 9, 9, 9, 10, 10, 10};
+u8 rtl8821ae_delta_swing_table_idx_24gccka_p_txpwrtrack[] =
+	{0, 0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 7, 8, 8, 9, 9, 10, 10, 11, 11, 12, 12, 12, 12, 12, 12};
+
+u8 rtl8821ae_delta_swing_table_idx_5gb_n_txpwrtrack[][DELTA_SWINGIDX_SIZE] = {
+	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11, 12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
+	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11, 12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
+	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11, 12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
+};
+
+u8 rtl8821ae_delta_swing_table_idx_5gb_p_txpwrtrack[][DELTA_SWINGIDX_SIZE] = {
+	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11, 12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
+	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11, 12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
+	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11, 12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
+};
+
+u8 rtl8821ae_delta_swing_table_idx_5ga_n_txpwrtrack[][DELTA_SWINGIDX_SIZE] = {
+	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11, 12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
+	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11, 12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
+	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11, 12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
+};
+
+u8 rtl8821ae_delta_swing_table_idx_5ga_p_txpwrtrack[][DELTA_SWINGIDX_SIZE] = {
+	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11, 12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
+	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11, 12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
+	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11, 12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
+};
+
+void rtl8812ae_dm_read_and_config_txpower_track(
+ 	struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+		("===> rtl8821ae_dm_read_and_config_txpower_track\n"));
+
+
+	memcpy(rtldm->delta_swing_table_idx_24ga_p,
+		rtl8812ae_delta_swing_table_idx_24ga_p_txpwrtrack, DELTA_SWINGIDX_SIZE);
+	memcpy(rtldm->delta_swing_table_idx_24ga_n,
+		rtl8812ae_delta_swing_table_idx_24ga_n_txpwrtrack, DELTA_SWINGIDX_SIZE);
+	memcpy(rtldm->delta_swing_table_idx_24gb_p,
+		rtl8812ae_delta_swing_table_idx_24gb_p_txpwrtrack, DELTA_SWINGIDX_SIZE);
+	memcpy(rtldm->delta_swing_table_idx_24gb_n,
+		rtl8812ae_delta_swing_table_idx_24gb_n_txpwrtrack, DELTA_SWINGIDX_SIZE);
+
+	memcpy(rtldm->delta_swing_table_idx_24gccka_p,
+		rtl8812ae_delta_swing_table_idx_24gccka_p_txpwrtrack, DELTA_SWINGIDX_SIZE);
+	memcpy(rtldm->delta_swing_table_idx_24gccka_n,
+		rtl8812ae_delta_swing_table_idx_24gccka_n_txpwrtrack, DELTA_SWINGIDX_SIZE);
+	memcpy(rtldm->delta_swing_table_idx_24gcckb_p,
+		rtl8812ae_delta_swing_table_idx_24gcckb_p_txpwrtrack, DELTA_SWINGIDX_SIZE);
+	memcpy(rtldm->delta_swing_table_idx_24gcckb_n,
+		rtl8812ae_delta_swing_table_idx_24gcckb_n_txpwrtrack, DELTA_SWINGIDX_SIZE);
+
+	memcpy(rtldm->delta_swing_table_idx_5ga_p,
+		rtl8812ae_delta_swing_table_idx_5ga_p_txpwrtrack, DELTA_SWINGIDX_SIZE*3);
+	memcpy(rtldm->delta_swing_table_idx_5ga_n,
+		rtl8812ae_delta_swing_table_idx_5ga_n_txpwrtrack, DELTA_SWINGIDX_SIZE*3);
+	memcpy(rtldm->delta_swing_table_idx_5gb_p,
+		rtl8812ae_delta_swing_table_idx_5gb_p_txpwrtrack, DELTA_SWINGIDX_SIZE*3);
+	memcpy(rtldm->delta_swing_table_idx_5gb_n,
+		rtl8812ae_delta_swing_table_idx_5gb_n_txpwrtrack, DELTA_SWINGIDX_SIZE*3);
+}
+
+void rtl8821ae_dm_read_and_config_txpower_track(
+ 	struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+		("===> rtl8821ae_dm_read_and_config_txpower_track\n"));
+
+
+	memcpy(rtldm->delta_swing_table_idx_24ga_p,
+		rtl8821ae_delta_swing_table_idx_24ga_p_txpwrtrack, DELTA_SWINGIDX_SIZE);
+	memcpy(rtldm->delta_swing_table_idx_24ga_n,
+		rtl8821ae_delta_swing_table_idx_24ga_n_txpwrtrack, DELTA_SWINGIDX_SIZE);
+	memcpy(rtldm->delta_swing_table_idx_24gb_p,
+		rtl8821ae_delta_swing_table_idx_24gb_p_txpwrtrack, DELTA_SWINGIDX_SIZE);
+	memcpy(rtldm->delta_swing_table_idx_24gb_n,
+		rtl8821ae_delta_swing_table_idx_24gb_n_txpwrtrack, DELTA_SWINGIDX_SIZE);
+
+	memcpy(rtldm->delta_swing_table_idx_24gccka_p,
+		rtl8821ae_delta_swing_table_idx_24gccka_p_txpwrtrack, DELTA_SWINGIDX_SIZE);
+	memcpy(rtldm->delta_swing_table_idx_24gccka_n,
+		rtl8821ae_delta_swing_table_idx_24gccka_n_txpwrtrack, DELTA_SWINGIDX_SIZE);
+	memcpy(rtldm->delta_swing_table_idx_24gcckb_p,
+		rtl8821ae_delta_swing_table_idx_24gcckb_p_txpwrtrack, DELTA_SWINGIDX_SIZE);
+	memcpy(rtldm->delta_swing_table_idx_24gcckb_n,
+		rtl8821ae_delta_swing_table_idx_24gcckb_n_txpwrtrack, DELTA_SWINGIDX_SIZE);
+
+	memcpy(rtldm->delta_swing_table_idx_5ga_p,
+		rtl8821ae_delta_swing_table_idx_5ga_p_txpwrtrack, DELTA_SWINGIDX_SIZE*3);
+	memcpy(rtldm->delta_swing_table_idx_5ga_n,
+		rtl8821ae_delta_swing_table_idx_5ga_n_txpwrtrack, DELTA_SWINGIDX_SIZE*3);
+	memcpy(rtldm->delta_swing_table_idx_5gb_p,
+		rtl8821ae_delta_swing_table_idx_5gb_p_txpwrtrack, DELTA_SWINGIDX_SIZE*3);
+	memcpy(rtldm->delta_swing_table_idx_5gb_n,
+		rtl8821ae_delta_swing_table_idx_5gb_n_txpwrtrack, DELTA_SWINGIDX_SIZE*3);
+}
+
+
+
+#define 	CALCULATE_SWINGTALBE_OFFSET(_offset, _direction, _size, _deltaThermal) \
+					do {\
+						for(_offset = 0; _offset < _size; _offset++)\
+						{\
+							if(_deltaThermal < thermal_threshold[_direction][_offset])\
+							{\
+								if(_offset != 0)\
+									_offset--;\
+								break;\
+							}\
+						}			\
+						if(_offset >= _size)\
+							_offset = _size-1;\
+					} while(0)
+
+
+void rtl8821ae_dm_txpower_track_adjust(struct ieee80211_hw *hw,
+												   u8 type,u8 *pdirection,
+												   u32 *poutwrite_val)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	u8 pwr_val = 0;
+
+	if (type == 0){
+		if (rtlpriv->dm.bb_swing_idx_ofdm[RF90_PATH_A] <=
+			rtlpriv->dm.bb_swing_idx_ofdm_base[RF90_PATH_A]) {
+			*pdirection = 1;
+			pwr_val = rtldm->bb_swing_idx_ofdm_base[RF90_PATH_A] - rtldm->bb_swing_idx_ofdm[RF90_PATH_A];
+		} else {
+			*pdirection = 2;
+			pwr_val = rtldm->bb_swing_idx_ofdm[RF90_PATH_A] - rtldm->bb_swing_idx_ofdm_base[RF90_PATH_A];
+		}
+	} else if (type ==1) {
+		if (rtldm->bb_swing_idx_cck <= rtldm->bb_swing_idx_cck_base) {
+			*pdirection = 1;
+			pwr_val = rtldm->bb_swing_idx_cck_base - rtldm->bb_swing_idx_cck;
+		} else {
+			*pdirection = 2;
+			pwr_val = rtldm->bb_swing_idx_cck - rtldm->bb_swing_idx_cck_base;
+		}
+	}
+
+	if (pwr_val >= TXPWRTRACK_MAX_IDX && (*pdirection == 1))
+		pwr_val = TXPWRTRACK_MAX_IDX;
+
+	*poutwrite_val = pwr_val |(pwr_val << 8)|(pwr_val << 16) | (pwr_val << 24);
+}
+
+void rtl8821ae_dm_clear_txpower_tracking_state(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_dm *rtldm = rtl_dm(rtlpriv);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtlpriv);
+	u8 p = 0;
+	rtldm->bb_swing_idx_cck_base = rtldm->default_cck_index;
+	rtldm->bb_swing_idx_cck = rtldm->default_cck_index;
+	rtldm->cck_index = 0;
+
+	for (p = RF90_PATH_A; p < MAX_RF_PATH; ++p) {
+		rtldm->bb_swing_idx_ofdm_base[p] = rtldm->default_ofdm_index;
+		rtldm->bb_swing_idx_ofdm[p] = rtldm->default_ofdm_index;
+		rtldm->ofdm_index[p] = rtldm->default_ofdm_index;
+
+		rtldm->power_index_offset[p] = 0;
+		rtldm->delta_power_index[p] = 0;
+		rtldm->delta_power_index_last[p] = 0;
+
+		rtldm->aboslute_ofdm_swing_idx[p] = 0;    /*Initial Mix mode power tracking*/
+		rtldm->remnant_ofdm_swing_idx[p] = 0;
+	}
+
+	rtldm->modify_txagc_flag_path_a = false;       /*Initial at Modify Tx Scaling Mode*/
+	rtldm->modify_txagc_flag_path_b = false;       /*Initial at Modify Tx Scaling Mode*/
+	rtldm->remnant_cck_idx = 0;
+	rtldm->thermalvalue = rtlefuse->eeprom_thermalmeter;
+	rtldm->thermalvalue_iqk = rtlefuse->eeprom_thermalmeter;
+	rtldm->thermalvalue_lck = rtlefuse->eeprom_thermalmeter;
+}
+
+u8  rtl8821ae_dm_get_swing_index(struct ieee80211_hw *hw)
+{
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 i = 0;
+	u32  bb_swing;
+
+	bb_swing =rtl8821ae_phy_query_bb_reg(hw, rtlhal->current_bandtype, RF90_PATH_A);
+
+	for (i = 0; i < TXSCALE_TABLE_SIZE; ++i)
+		if ( bb_swing == rtl8821ae_txscaling_table[i])
+			break;
+
+	return i;
+}
+
+void rtl8821ae_dm_initialize_txpower_tracking_thermalmeter(
+				struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_dm *rtldm = rtl_dm(rtlpriv);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtlpriv);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 default_swing_index  = 0;
+	u8 p = 0;
+
+	rtlpriv->dm.txpower_track_control = true;
+	rtldm->thermalvalue = rtlefuse->eeprom_thermalmeter;
+	rtldm->thermalvalue_iqk = rtlefuse->eeprom_thermalmeter;
+	rtldm->thermalvalue_lck = rtlefuse->eeprom_thermalmeter;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+		rtl8812ae_dm_read_and_config_txpower_track(hw);
+	else
+		rtl8821ae_dm_read_and_config_txpower_track(hw);
+
+	default_swing_index = rtl8821ae_dm_get_swing_index(hw);
+
+	rtldm->default_ofdm_index = (default_swing_index == TXSCALE_TABLE_SIZE) ? 24 : default_swing_index;
+	rtldm->default_cck_index = 24;
+
+	rtldm->bb_swing_idx_cck_base = rtldm->default_cck_index;
+	rtldm->cck_index = rtldm->default_cck_index;
+
+	for (p = RF90_PATH_A; p < MAX_RF_PATH; ++p)
+	{
+		rtldm->bb_swing_idx_ofdm_base[p] = rtldm->default_ofdm_index;
+	   	rtldm->ofdm_index[p] = rtldm->default_ofdm_index;
+		rtldm->delta_power_index[p] = 0;
+		rtldm->power_index_offset[p] = 0;
+		rtldm->delta_power_index_last[p] = 0;
+	}
+}
+
+static void rtl8821ae_dm_init_dynamic_bb_powersaving(struct ieee80211_hw *hw)
+{
+	dm_pstable.pre_ccastate = CCA_MAX;
+	dm_pstable.cur_ccasate = CCA_MAX;
+	dm_pstable.pre_rfstate = RF_MAX;
+	dm_pstable.cur_rfstate = RF_MAX;
+	dm_pstable.rssi_val_min = 0;
+	dm_pstable.initialize = 0;
+}
+
+
+static void rtl8821ae_dm_diginit(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	//dm_digtable.dig_enable_flag = true;
+	dm_digtable.cur_igvalue = rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, 0x7f);
+	/*dm_digtable.pre_igvalue = 0;
+	dm_digtable.cursta_connectctate = DIG_STA_DISCONNECT;
+	dm_digtable.presta_connectstate = DIG_STA_DISCONNECT;
+	dm_digtable.curmultista_connectstate = DIG_MULTISTA_DISCONNECT;*/
+	dm_digtable.rssi_lowthresh = DM_DIG_THRESH_LOW;
+	dm_digtable.rssi_highthresh = DM_DIG_THRESH_HIGH;
+	dm_digtable.fa_lowthresh = DM_FALSEALARM_THRESH_LOW;
+	dm_digtable.fa_highthresh = DM_FALSEALARM_THRESH_HIGH;
+	dm_digtable.rx_gain_range_max = DM_DIG_MAX;
+	dm_digtable.rx_gain_range_min = DM_DIG_MIN;
+	dm_digtable.backoff_val = DM_DIG_BACKOFF_DEFAULT;
+	dm_digtable.backoff_val_range_max = DM_DIG_BACKOFF_MAX;
+	dm_digtable.backoff_val_range_min = DM_DIG_BACKOFF_MIN;
+	dm_digtable.pre_cck_cca_thres = 0xff;
+	dm_digtable.cur_cck_cca_thres = 0x83;
+	dm_digtable.forbidden_igi = DM_DIG_MIN;
+	dm_digtable.large_fa_hit = 0;
+	dm_digtable.recover_cnt = 0;
+	dm_digtable.dig_dynamic_min_0 = DM_DIG_MIN;
+	dm_digtable.dig_dynamic_min_1 = DM_DIG_MIN;
+	dm_digtable.b_media_connect_0 = false;
+	dm_digtable.b_media_connect_1 = false;
+	rtlpriv->dm.b_dm_initialgain_enable = true;
+	dm_digtable.bt30_cur_igi = 0x32;
+}
+
+static void rtl8821ae_dm_init_dynamic_txpower(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->dm.bdynamic_txpower_enable = false;
+
+	rtlpriv->dm.last_dtp_lvl = TXHIGHPWRLEVEL_NORMAL;
+	rtlpriv->dm.dynamic_txhighpower_lvl = TXHIGHPWRLEVEL_NORMAL;
+}
+
+
+void rtl8821ae_dm_init_edca_turbo(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	rtlpriv->dm.bcurrent_turbo_edca = false;
+	rtlpriv->dm.bis_any_nonbepkts = false;
+	rtlpriv->dm.bis_cur_rdlstate = false;
+}
+
+
+void rtl8821ae_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rate_adaptive *p_ra = &(rtlpriv->ra);
+
+	p_ra->ratr_state = DM_RATR_STA_INIT;
+	p_ra->pre_ratr_state = DM_RATR_STA_INIT;
+
+	rtlpriv->dm.dm_type = DM_TYPE_BYDRIVER;
+	if (rtlpriv->dm.dm_type == DM_TYPE_BYDRIVER)
+		rtlpriv->dm.b_useramask = true;
+	else
+		rtlpriv->dm.b_useramask = false;
+
+	p_ra->high_rssi_thresh_for_ra = 50;
+	p_ra->low_rssi_thresh_for_ra = 20;
+}
+
+
+static void rtl8821ae_dm_init_txpower_tracking(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->dm.btxpower_tracking = true;
+	rtlpriv->dm.btxpower_trackinginit = false;
+	rtlpriv->dm.txpowercount = 0;
+	rtlpriv->dm.txpower_track_control = true;
+	rtlpriv->dm.thermalvalue = 0;
+
+	rtlpriv->dm.ofdm_index[0] = 30;
+	rtlpriv->dm.cck_index = 20;
+
+	rtlpriv->dm.bb_swing_idx_cck_base = rtlpriv->dm.cck_index;
+
+
+	rtlpriv->dm.bb_swing_idx_ofdm[RF90_PATH_A] = rtlpriv->dm.ofdm_index[0];
+	rtlpriv->dm.bb_swing_idx_ofdm[RF90_PATH_B] = rtlpriv->dm.ofdm_index[0];
+	rtlpriv->dm.delta_power_index[0] = 0;
+	rtlpriv->dm.delta_power_index_last[0] = 0;
+	rtlpriv->dm.power_index_offset[0] = 0;
+
+	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+		 ("  rtlpriv->dm.btxpower_tracking = %d\n",
+		  rtlpriv->dm.btxpower_tracking));
+}
+
+
+void rtl8821ae_dm_init_dynamic_atc_switch(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->dm.crystal_cap = rtlpriv->efuse.crystalcap;
+
+	rtlpriv->dm.atc_status = rtl_get_bbreg(hw, ROFDM1_CFOTRACKING, BIT(11));
+	rtlpriv->dm.cfo_threshold = CFO_THRESHOLD_XTAL;
+}
+
+
+void rtl8821ae_dm_init(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	spin_lock(&rtlpriv->locks.iqk_lock);
+	rtlphy->b_iqk_in_progress = false;
+	spin_unlock(&rtlpriv->locks.iqk_lock);
+
+	rtlpriv->dm.dm_type = DM_TYPE_BYDRIVER;
+	rtl8821ae_dm_diginit(hw);
+	rtl8821ae_dm_init_rate_adaptive_mask(hw);
+	rtl8812ae_dm_path_diversity_init(hw);
+	rtl8821ae_dm_init_edca_turbo(hw);
+	rtl8821ae_dm_initialize_txpower_tracking_thermalmeter(hw);
+#if 1
+	rtl8821ae_dm_init_dynamic_bb_powersaving(hw);
+	rtl8821ae_dm_init_dynamic_txpower(hw);
+	rtl8821ae_dm_init_txpower_tracking(hw);
+#endif
+	rtl8821ae_dm_init_dynamic_atc_switch(hw);
+}
+
+void rtl8821ae_dm_find_minimum_rssi(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_dig *rtl_dm_dig = &(rtlpriv->dm.dm_digtable);
+	struct rtl_mac *mac = rtl_mac(rtlpriv);
+
+	/* Determine the minimum RSSI  */
+	if ((mac->link_state < MAC80211_LINKED) &&
+	    (rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb == 0)) {
+		rtl_dm_dig->min_undecorated_pwdb_for_dm = 0;
+		RT_TRACE(COMP_BB_POWERSAVING, DBG_LOUD,
+			 ("Not connected to any \n"));
+	}
+	if (mac->link_state >= MAC80211_LINKED) {
+		if (mac->opmode == NL80211_IFTYPE_AP ||
+			mac->opmode == NL80211_IFTYPE_ADHOC) {
+			rtl_dm_dig->min_undecorated_pwdb_for_dm =
+			    rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb;
+			RT_TRACE(COMP_BB_POWERSAVING, DBG_LOUD,
+				 ("AP Client PWDB = 0x%lx \n",
+				  rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb));
+		} else {
+			rtl_dm_dig->min_undecorated_pwdb_for_dm =
+			    rtlpriv->dm.undecorated_smoothed_pwdb;
+			RT_TRACE(COMP_BB_POWERSAVING, DBG_LOUD,
+				 ("STA Default Port PWDB = 0x%x \n",
+				  rtl_dm_dig->min_undecorated_pwdb_for_dm));
+		}
+	} else {
+		rtl_dm_dig->min_undecorated_pwdb_for_dm =
+		    rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb;
+		RT_TRACE(COMP_BB_POWERSAVING, DBG_LOUD,
+			 ("AP Ext Port or 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));
+}
+
+#if 0
+void  rtl8812ae_dm_rssi_dump_to_register(
+	struct ieee80211_hw *hw
+	)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtl_write_byte(rtlpriv, RA_RSSI_DUMP, Adapter->RxStats.RxRSSIPercentage[0]);
+	rtl_write_byte(rtlpriv, RB_RSSI_DUMP, Adapter->RxStats.RxRSSIPercentage[1]);
+
+	/* Rx EVM*/
+	rtl_write_byte(rtlpriv, RS1_RX_EVM_DUMP, Adapter->RxStats.RxEVMdbm[0]);
+	rtl_write_byte(rtlpriv, RS2_RX_EVM_DUMP, Adapter->RxStats.RxEVMdbm[1]);
+
+	/*Rx SNR*/
+	rtl_write_byte(rtlpriv, RA_RX_SNR_DUMP, (u1Byte)(Adapter->RxStats.RxSNRdB[0]));
+	rtl_write_byte(rtlpriv, RB_RX_SNR_DUMP, (u1Byte)(Adapter->RxStats.RxSNRdB[1]));
+
+	/*Rx Cfo_Short*/
+	rtl_write_word(rtlpriv, RA_CFO_SHORT_DUMP, Adapter->RxStats.RxCfoShort[0]);
+	rtl_write_word(rtlpriv, RB_CFO_SHORT_DUMP, Adapter->RxStats.RxCfoShort[1]);
+
+	/*Rx Cfo_Tail*/
+	rtl_write_word(rtlpriv, RA_CFO_LONG_DUMP, Adapter->RxStats.RxCfoTail[0]);
+	rtl_write_word(rtlpriv, RB_CFO_LONG_DUMP, Adapter->RxStats.RxCfoTail[1]);
+
+}
+#endif
+
+static void rtl8821ae_dm_check_rssi_monitor(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_sta_info *drv_priv;
+	u8 h2c_parameter[3] = { 0 };
+	long tmp_entry_max_pwdb = 0, tmp_entry_min_pwdb = 0xff;
+
+
+	/* AP & ADHOC & MESH */
+	spin_lock_bh(&rtlpriv->locks.entry_list_lock);
+	list_for_each_entry(drv_priv, &rtlpriv->entry_list, list) {
+		if(drv_priv->rssi_stat.undecorated_smoothed_pwdb < tmp_entry_min_pwdb)
+			tmp_entry_min_pwdb = drv_priv->rssi_stat.undecorated_smoothed_pwdb;
+		if(drv_priv->rssi_stat.undecorated_smoothed_pwdb > tmp_entry_max_pwdb)
+			tmp_entry_max_pwdb = drv_priv->rssi_stat.undecorated_smoothed_pwdb;
+
+		/*h2c_parameter[2] = (u8) (rtlpriv->dm.undecorated_smoothed_pwdb & 0xFF);
+		h2c_parameter[1] = 0x20;
+		h2c_parameter[0] =  drv_priv->rssi_stat;
+		rtl8821ae_fill_h2c_cmd(hw, H2C_RSSI_REPORT, 3, h2c_parameter);*/
+	}
+	spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
+
+	/* If associated entry is found */
+	if (tmp_entry_max_pwdb != 0) {
+		rtlpriv->dm.entry_max_undecoratedsmoothed_pwdb = tmp_entry_max_pwdb;
+		RTPRINT(rtlpriv, FDM, DM_PWDB, ("EntryMaxPWDB = 0x%lx(%ld)\n",
+			tmp_entry_max_pwdb, tmp_entry_max_pwdb));
+	} else {
+		rtlpriv->dm.entry_max_undecoratedsmoothed_pwdb = 0;
+	}
+	/* If associated entry is found */
+	if (tmp_entry_min_pwdb != 0xff) {
+		rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb = tmp_entry_min_pwdb;
+		RTPRINT(rtlpriv, FDM, DM_PWDB, ("EntryMinPWDB = 0x%lx(%ld)\n",
+					tmp_entry_min_pwdb, tmp_entry_min_pwdb));
+	} else {
+		rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb = 0;
+	}
+	/* Indicate Rx signal strength to FW. */
+	if (rtlpriv->dm.b_useramask) {
+		h2c_parameter[2] = (u8) (rtlpriv->dm.undecorated_smoothed_pwdb & 0xFF);
+		h2c_parameter[1] = 0x20;
+		h2c_parameter[0] = 0;
+		rtl8821ae_fill_h2c_cmd(hw, H2C_RSSI_REPORT, 3, h2c_parameter);
+	} else {
+		rtl_write_byte(rtlpriv, 0x4fe, rtlpriv->dm.undecorated_smoothed_pwdb);
+	}
+	rtl8821ae_dm_find_minimum_rssi(hw);
+	dm_digtable.rssi_val_min = rtlpriv->dm.dm_digtable.min_undecorated_pwdb_for_dm;
+}
+
+void rtl8821ae_dm_write_cck_cca_thres(struct ieee80211_hw *hw, u8 current_cca)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (dm_digtable.cur_cck_cca_thres != current_cca)
+		rtl_write_byte(rtlpriv, DM_REG_CCK_CCA_11AC, current_cca);
+
+	dm_digtable.pre_cck_cca_thres = dm_digtable.cur_cck_cca_thres;
+	dm_digtable.cur_cck_cca_thres = current_cca;
+}
+
+void rtl8821ae_dm_write_dig(struct ieee80211_hw *hw, u8 current_igi)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	if(dm_digtable.stop_dig)
+		return;
+
+	if (dm_digtable.cur_igvalue != current_igi){
+		rtl_set_bbreg(hw, DM_REG_IGI_A_11AC, DM_BIT_IGI_11AC, current_igi);
+		if (rtlpriv->phy.rf_type != RF_1T1R)
+			rtl_set_bbreg(hw, DM_REG_IGI_B_11AC, DM_BIT_IGI_11AC, current_igi);
+	}
+	//dm_digtable.pre_igvalue = dm_digtable.cur_igvalue;
+	dm_digtable.cur_igvalue = current_igi;
+}
+
+static void rtl8821ae_dm_dig(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 dig_dynamic_min;
+	u8 dig_max_of_min;
+	bool first_connect, first_disconnect;
+	u8 dm_dig_max, dm_dig_min, offset;
+	u8 current_igi =dm_digtable.cur_igvalue;
+
+
+	RT_TRACE(COMP_DIG, DBG_LOUD,("rtl8821ae_dm_dig()==>\n"));
+
+
+	if (mac->act_scanning == true) {
+		RT_TRACE(COMP_DIG, DBG_LOUD,("rtl8821ae_dm_dig() Return: In Scan Progress \n"));
+	    	return;
+	}
+
+	/*add by Neil Chen to avoid PSD is processing*/
+	dig_dynamic_min = dm_digtable.dig_dynamic_min_0;
+	first_connect = (mac->link_state >= MAC80211_LINKED) &&
+			(dm_digtable.b_media_connect_0 == false);
+	first_disconnect = (mac->link_state < MAC80211_LINKED) &&
+			(dm_digtable.b_media_connect_0 == true);
+
+	/*1 Boundary Decision*/
+
+
+	dm_dig_max = 0x5A;
+
+	if (rtlhal->hw_type != HARDWARE_TYPE_RTL8821AE)
+		dm_dig_min = DM_DIG_MIN;
+	else
+		dm_dig_min = 0x1C;
+
+	dig_max_of_min = DM_DIG_MAX_AP;
+
+	if (mac->link_state >= MAC80211_LINKED) {
+		if (rtlhal->hw_type != HARDWARE_TYPE_RTL8821AE)
+			offset = 20;
+		else
+			offset = 10;
+
+		if ((dm_digtable.rssi_val_min + offset) > dm_dig_max)
+			dm_digtable.rx_gain_range_max = dm_dig_max;
+		else if ((dm_digtable.rssi_val_min + offset) < dm_dig_min)
+			dm_digtable.rx_gain_range_max = dm_dig_min;
+		else
+			dm_digtable.rx_gain_range_max = dm_digtable.rssi_val_min + offset;
+
+		if(rtlpriv->dm.b_one_entry_only){
+			offset = 0;
+
+			if (dm_digtable.rssi_val_min - offset < dm_dig_min)
+				dig_dynamic_min = dm_dig_min;
+			else if (dm_digtable.rssi_val_min - offset > dig_max_of_min)
+				dig_dynamic_min = dig_max_of_min;
+			else
+				dig_dynamic_min = dm_digtable.rssi_val_min - offset;
+
+			RT_TRACE(COMP_DIG, DBG_LOUD,
+				("rtl8821ae_dm_dig() : bOneEntryOnly=TRUE,  dig_dynamic_min=0x%x\n",
+				dig_dynamic_min));
+			RT_TRACE(COMP_DIG, DBG_LOUD,
+				("rtl8821ae_dm_dig() : dm_digtable.rssi_val_min=%d",dm_digtable.
+				rssi_val_min));
+		} else {
+			dig_dynamic_min = dm_dig_min;
+		}
+	} else {
+		dm_digtable.rx_gain_range_max = dm_dig_max;
+		dig_dynamic_min = dm_dig_min;
+		RT_TRACE(COMP_DIG, DBG_LOUD,
+			("rtl8821ae_dm_dig() : No Link\n"));
+	}
+
+	if (rtlpriv->falsealm_cnt.cnt_all > 10000) {
+		RT_TRACE(COMP_DIG, DBG_LOUD,
+			("rtl8821ae_dm_dig(): Abnornally false alarm case. \n"));
+
+		if (dm_digtable.large_fa_hit != 3)
+		        dm_digtable.large_fa_hit++;
+		if (dm_digtable.forbidden_igi < current_igi) {
+			dm_digtable.forbidden_igi = current_igi;
+			dm_digtable.large_fa_hit = 1;
+		}
+
+		if (dm_digtable.large_fa_hit >= 3) {
+			if((dm_digtable.forbidden_igi + 1) > dm_digtable.rx_gain_range_max)
+				dm_digtable.rx_gain_range_min = dm_digtable.rx_gain_range_max;
+			else
+				dm_digtable.rx_gain_range_min = (dm_digtable.forbidden_igi + 1);
+			dm_digtable.recover_cnt = 3600;
+		}
+
+	} else {
+		/*Recovery mechanism for IGI lower bound*/
+		if (dm_digtable.recover_cnt != 0)
+			dm_digtable.recover_cnt --;
+		else {
+			if (dm_digtable.large_fa_hit < 3) {
+				if ((dm_digtable.forbidden_igi -1) < dig_dynamic_min) {
+					dm_digtable.forbidden_igi = dig_dynamic_min;
+					dm_digtable.rx_gain_range_min = dig_dynamic_min;
+					RT_TRACE(COMP_DIG, DBG_LOUD,
+						("rtl8821ae_dm_dig(): Normal Case: At Lower Bound\n"));
+				} else {
+					dm_digtable.forbidden_igi --;
+					dm_digtable.rx_gain_range_min = (dm_digtable.forbidden_igi + 1);
+					RT_TRACE(COMP_DIG, DBG_LOUD,
+						("rtl8821ae_dm_dig(): Normal Case: Approach Lower Bound\n"));
+				}
+			} else {
+				dm_digtable.large_fa_hit = 0;
+			}
+		}
+	}
+	RT_TRACE(COMP_DIG, DBG_LOUD,
+		("rtl8821ae_dm_dig(): pDM_DigTable->LargeFAHit=%d\n",
+		dm_digtable.large_fa_hit));
+
+	if (rtlpriv->dm.dbginfo.num_qry_beacon_pkt < 10)
+		dm_digtable.rx_gain_range_min = dm_dig_min;
+
+	if (dm_digtable.rx_gain_range_min > dm_digtable.rx_gain_range_max)
+		dm_digtable.rx_gain_range_min = dm_digtable.rx_gain_range_max;
+
+	/*Adjust initial gain by false alarm*/
+	if (mac->link_state >= MAC80211_LINKED) {
+		RT_TRACE(COMP_DIG, DBG_LOUD,
+			("rtl8821ae_dm_dig(): DIG AfterLink\n"));
+		if (first_connect) {
+			if (dm_digtable.rssi_val_min <= dig_max_of_min)
+				current_igi = dm_digtable.rssi_val_min;
+			else
+				current_igi = dig_max_of_min;
+			RT_TRACE(COMP_DIG, DBG_LOUD,
+				("rtl8821ae_dm_dig: First Connect\n"));
+		} else {
+			if(rtlpriv->falsealm_cnt.cnt_all > DM_DIG_FA_TH2)
+				current_igi = current_igi + 4;
+			else if (rtlpriv->falsealm_cnt.cnt_all > DM_DIG_FA_TH1)
+				current_igi = current_igi + 2;
+			else if(rtlpriv->falsealm_cnt.cnt_all < DM_DIG_FA_TH0)
+				current_igi = current_igi - 2;
+
+			if((rtlpriv->dm.dbginfo.num_qry_beacon_pkt < 10)
+				&&(rtlpriv->falsealm_cnt.cnt_all < DM_DIG_FA_TH1)) {
+				current_igi = dm_digtable.rx_gain_range_min;
+				RT_TRACE(COMP_DIG, DBG_LOUD,
+					("rtl8821ae_dm_dig(): Beacon is less than 10 and FA is less than 768, IGI GOES TO 0x1E!!!!!!!!!!!!\n"));
+			}
+		}
+	}  else{
+		RT_TRACE(COMP_DIG, DBG_LOUD,
+			("rtl8821ae_dm_dig(): DIG BeforeLink\n"));
+		if (first_disconnect){
+			current_igi = dm_digtable.rx_gain_range_min;
+			RT_TRACE(COMP_DIG, DBG_LOUD,
+				("rtl8821ae_dm_dig(): First DisConnect \n"));
+		} else {
+			/*2012.03.30 LukeLee: enable DIG before link but with very high thresholds*/
+	       	if (rtlpriv->falsealm_cnt.cnt_all > 2000)
+				current_igi = current_igi + 4;
+			else if (rtlpriv->falsealm_cnt.cnt_all > 600)
+				current_igi = current_igi + 2;
+			else if(rtlpriv->falsealm_cnt.cnt_all < 300)
+				current_igi = current_igi - 2;
+			if (current_igi >= 0x3e)
+				current_igi = 0x3e;
+			RT_TRACE(COMP_DIG, DBG_LOUD,("rtl8821ae_dm_dig(): England DIG \n"));
+		}
+	}
+	RT_TRACE(COMP_DIG, DBG_LOUD,
+		("rtl8821ae_dm_dig(): DIG End Adjust IGI\n"));
+	/* Check initial gain by upper/lower bound*/
+
+	if (current_igi > dm_digtable.rx_gain_range_max)
+		current_igi = dm_digtable.rx_gain_range_max;
+	if (current_igi < dm_digtable.rx_gain_range_min)
+		current_igi = dm_digtable.rx_gain_range_min;
+
+	RT_TRACE(COMP_DIG, DBG_LOUD,
+		("rtl8821ae_dm_dig(): rx_gain_range_max=0x%x, rx_gain_range_min=0x%x\n",
+		dm_digtable.rx_gain_range_max, dm_digtable.rx_gain_range_min));
+	RT_TRACE(COMP_DIG, DBG_LOUD,
+		("rtl8821ae_dm_dig(): TotalFA=%d\n", rtlpriv->falsealm_cnt.cnt_all));
+	RT_TRACE(COMP_DIG, DBG_LOUD,
+		("rtl8821ae_dm_dig(): CurIGValue=0x%x\n", current_igi));
+
+	rtl8821ae_dm_write_dig(hw, current_igi);
+	dm_digtable.b_media_connect_0= ((mac->link_state >= MAC80211_LINKED) ? true :false);
+	dm_digtable.dig_dynamic_min_0 = dig_dynamic_min;
+}
+
+static void rtl8821ae_dm_common_info_self_update(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 cnt = 0;
+	struct rtl_sta_info *drv_priv;
+
+	rtlpriv->dm.b_one_entry_only = false;
+
+	if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_STATION &&
+		rtlpriv->mac80211.link_state >= MAC80211_LINKED) {
+		rtlpriv->dm.b_one_entry_only = true;
+		return;
+	}
+
+	if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_AP ||
+		rtlpriv->mac80211.opmode == NL80211_IFTYPE_ADHOC ||
+		rtlpriv->mac80211.opmode == NL80211_IFTYPE_MESH_POINT) {
+		spin_lock_bh(&rtlpriv->locks.entry_list_lock);
+		list_for_each_entry(drv_priv, &rtlpriv->entry_list, list) {
+			cnt ++;
+		}
+		spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
+
+		if (cnt == 1)
+			rtlpriv->dm.b_one_entry_only = true;
+	}
+}
+
+
+static void rtl8821ae_dm_false_alarm_counter_statistics(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct false_alarm_statistics *falsealm_cnt = &(rtlpriv->falsealm_cnt);
+	u32 cck_enable =0;
+
+	/*read OFDM FA counter*/
+	falsealm_cnt->cnt_ofdm_fail = rtl_get_bbreg(hw, ODM_REG_OFDM_FA_11AC, BMASKLWORD);
+	falsealm_cnt->cnt_cck_fail = rtl_get_bbreg(hw, ODM_REG_CCK_FA_11AC, BMASKLWORD);
+
+	cck_enable =  rtl_get_bbreg(hw, ODM_REG_BB_RX_PATH_11AC, BIT(28));
+	if (cck_enable)  /*if(pDM_Odm->pBandType == ODM_BAND_2_4G)*/
+		falsealm_cnt->cnt_all = falsealm_cnt->cnt_ofdm_fail + falsealm_cnt->cnt_cck_fail;
+	else
+		falsealm_cnt->cnt_all = falsealm_cnt->cnt_ofdm_fail;
+
+	/*reset OFDM FA coutner*/
+	rtl_set_bbreg(hw, ODM_REG_OFDM_FA_RST_11AC, BIT(17), 1);
+	rtl_set_bbreg(hw, ODM_REG_OFDM_FA_RST_11AC, BIT(17), 0);
+	/* reset CCK FA counter*/
+	rtl_set_bbreg(hw,  ODM_REG_CCK_FA_RST_11AC, BIT(15), 0);
+	rtl_set_bbreg(hw,  ODM_REG_CCK_FA_RST_11AC, BIT(15), 1);
+
+	RT_TRACE(COMP_DIG, DBG_LOUD, ("Cnt_Cck_fail=%d\n",
+			falsealm_cnt->cnt_cck_fail));
+	RT_TRACE(COMP_DIG, DBG_LOUD, ("cnt_ofdm_fail=%d\n",
+			falsealm_cnt->cnt_ofdm_fail));
+	RT_TRACE(COMP_DIG, DBG_LOUD, ("Total False Alarm=%d\n",
+			falsealm_cnt->cnt_all));
+}
+
+void rtl8812ae_dm_check_txpower_tracking_thermalmeter(
+		struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	static u8 tm_trigger = 0;
+
+	if (!rtlpriv->dm.btxpower_tracking)
+		return;
+
+	if (!tm_trigger) {
+		rtl_set_rfreg(hw, RF90_PATH_A, RF_T_METER_88E, BIT(17)|BIT(16), 0x03);
+		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+			 ("Trigger 8812 Thermal Meter!!\n"));
+		tm_trigger = 1;
+		return;
+	} else {
+		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+			 ("Schedule TxPowerTracking direct call!!\n"));
+		rtl8812ae_dm_txpower_tracking_callback_thermalmeter(hw);
+		tm_trigger = 0;
+	}
+}
+
+static void rtl8821ae_dm_iq_calibrate(struct ieee80211_hw *hw)
+{
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	if (mac->link_state >= MAC80211_LINKED) {
+		/*if ((*rtldm->p_channel != rtldm->pre_channel )
+			&& (!mac->act_scanning)) {
+			rtldm->pre_channel = *rtldm->p_channel;
+			rtldm->linked_interval = 0;
+		}*/
+
+		if(rtldm->linked_interval < 3)
+			rtldm->linked_interval ++;
+
+		if(rtldm->linked_interval == 2)
+		{
+			if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+				rtl8812ae_phy_iq_calibrate(hw, false);
+			else
+				rtl8821ae_phy_iq_calibrate(hw, false);
+		}
+	} else {
+		rtldm->linked_interval = 0;
+	}
+}
+
+
+void rtl8812ae_get_delta_swing_table(
+	struct ieee80211_hw *hw,
+	u8 **temperature_up_a,
+	u8 **temperature_down_a,
+	u8 **temperature_up_b,
+	u8 **temperature_down_b
+	)
+{
+   	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	u8 channel = rtlphy->current_channel;
+	u8 rate = rtldm->tx_rate;
+
+
+	if ( 1 <= channel && channel <= 14) {
+		if (RX_HAL_IS_CCK_RATE(rate)) {
+		        *temperature_up_a = rtldm->delta_swing_table_idx_24gccka_p;
+		        *temperature_down_a = rtldm->delta_swing_table_idx_24gccka_n;
+		        *temperature_up_b = rtldm->delta_swing_table_idx_24gcckb_p;
+		        *temperature_down_b = rtldm->delta_swing_table_idx_24gcckb_n;
+		} else {
+		        *temperature_up_a = rtldm->delta_swing_table_idx_24ga_p;
+		        *temperature_down_a = rtldm->delta_swing_table_idx_24ga_n;
+		        *temperature_up_b = rtldm->delta_swing_table_idx_24gb_p;
+		        *temperature_down_b = rtldm->delta_swing_table_idx_24gb_n;
+		}
+ 	} else if ( 36 <= channel && channel <= 64) {
+	        *temperature_up_a = rtldm->delta_swing_table_idx_5ga_p[0];
+	        *temperature_down_a = rtldm->delta_swing_table_idx_5ga_n[0];
+	        *temperature_up_b = rtldm->delta_swing_table_idx_5gb_p[0];
+	        *temperature_down_b = rtldm->delta_swing_table_idx_5gb_n[0];
+    	} else if ( 100 <= channel && channel <= 140) {
+		*temperature_up_a = rtldm->delta_swing_table_idx_5ga_p[1];
+		*temperature_down_a = rtldm->delta_swing_table_idx_5ga_n[1];
+		*temperature_up_b = rtldm->delta_swing_table_idx_5gb_p[1];
+		*temperature_down_b = rtldm->delta_swing_table_idx_5gb_n[1];
+    	} else if ( 149 <= channel && channel <= 173) {
+		*temperature_up_a = rtldm->delta_swing_table_idx_5ga_p[2];
+		*temperature_down_a = rtldm->delta_swing_table_idx_5ga_n[2];
+		*temperature_up_b = rtldm->delta_swing_table_idx_5gb_p[2];
+		*temperature_down_b = rtldm->delta_swing_table_idx_5gb_n[2];
+    	} else {
+	    *temperature_up_a = (u8*)rtl8818e_delta_swing_table_idx_24gb_p_txpwrtrack;
+	    *temperature_down_a =(u8*)rtl8818e_delta_swing_table_idx_24gb_n_txpwrtrack;
+	    *temperature_up_b = (u8*)rtl8818e_delta_swing_table_idx_24gb_p_txpwrtrack;
+	    *temperature_down_b = (u8*)rtl8818e_delta_swing_table_idx_24gb_n_txpwrtrack;
+    	}
+
+	return;
+}
+
+void rtl8812ae_phy_lccalibrate(
+	struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD, ("===> rtl8812ae_phy_lccalibrate\n"));
+
+	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD, ("<=== rtl8812ae_phy_lccalibrate\n"));
+
+}
+
+void rtl8812ae_dm_update_init_rate(
+	struct ieee80211_hw *hw,
+	u8 rate
+	)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_dm	*rtldm = rtl_dm(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 p = 0;
+
+	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+		("Get C2H Command! Rate=0x%x\n", rate));
+
+	rtldm->tx_rate = rate;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE){
+		rtl8821ae_dm_txpwr_track_set_pwr(hw, MIX_MODE, RF90_PATH_A, 0);
+	}
+	else
+	{
+		for (p = RF90_PATH_A; p < MAX_PATH_NUM_8812A; p++)
+		{
+			rtl8812ae_dm_txpwr_track_set_pwr(hw, BBSWING, p, 0);
+		}
+	}
+
+}
+
+u8 rtl8812ae_hw_rate_to_mrate(
+	struct ieee80211_hw *hw,
+	u8 rate
+	)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 ret_rate = MGN_1M;
+
+
+	switch(rate)
+	{
+		case DESC_RATE1M:		ret_rate = MGN_1M;		break;
+		case DESC_RATE2M:		ret_rate = MGN_2M;		break;
+		case DESC_RATE5_5M:		ret_rate = MGN_5_5M;		break;
+		case DESC_RATE11M:		ret_rate = MGN_11M;		break;
+		case DESC_RATE6M:		ret_rate = MGN_6M;		break;
+		case DESC_RATE9M:		ret_rate = MGN_9M;		break;
+		case DESC_RATE12M:		ret_rate = MGN_12M;		break;
+		case DESC_RATE18M:		ret_rate = MGN_18M;		break;
+		case DESC_RATE24M:		ret_rate = MGN_24M;		break;
+		case DESC_RATE36M:		ret_rate = MGN_36M;		break;
+		case DESC_RATE48M:		ret_rate = MGN_48M;		break;
+		case DESC_RATE54M:		ret_rate = MGN_54M;		break;
+		case DESC_RATEMCS0:	ret_rate = MGN_MCS0;		break;
+		case DESC_RATEMCS1:	ret_rate = MGN_MCS1;		break;
+		case DESC_RATEMCS2:	ret_rate = MGN_MCS2;		break;
+		case DESC_RATEMCS3:	ret_rate = MGN_MCS3;		break;
+		case DESC_RATEMCS4:	ret_rate = MGN_MCS4;		break;
+		case DESC_RATEMCS5:	ret_rate = MGN_MCS5;		break;
+		case DESC_RATEMCS6:	ret_rate = MGN_MCS6;		break;
+		case DESC_RATEMCS7:	ret_rate = MGN_MCS7;		break;
+		case DESC_RATEMCS8:	ret_rate = MGN_MCS8;		break;
+		case DESC_RATEMCS9:	ret_rate = MGN_MCS9;		break;
+		case DESC_RATEMCS10:	ret_rate = MGN_MCS10;	break;
+		case DESC_RATEMCS11:	ret_rate = MGN_MCS11;	break;
+		case DESC_RATEMCS12:	ret_rate = MGN_MCS12;	break;
+		case DESC_RATEMCS13:	ret_rate = MGN_MCS13;	break;
+		case DESC_RATEMCS14:	ret_rate = MGN_MCS14;	break;
+		case DESC_RATEMCS15:	ret_rate = MGN_MCS15;	break;
+		case DESC_RATEVHT1SS_MCS0:	ret_rate = MGN_VHT1SS_MCS0;		break;
+		case DESC_RATEVHT1SS_MCS1:	ret_rate = MGN_VHT1SS_MCS1;		break;
+		case DESC_RATEVHT1SS_MCS2:	ret_rate = MGN_VHT1SS_MCS2;		break;
+		case DESC_RATEVHT1SS_MCS3:	ret_rate = MGN_VHT1SS_MCS3;		break;
+		case DESC_RATEVHT1SS_MCS4:	ret_rate = MGN_VHT1SS_MCS4;		break;
+		case DESC_RATEVHT1SS_MCS5:	ret_rate = MGN_VHT1SS_MCS5;		break;
+		case DESC_RATEVHT1SS_MCS6:	ret_rate = MGN_VHT1SS_MCS6;		break;
+		case DESC_RATEVHT1SS_MCS7:	ret_rate = MGN_VHT1SS_MCS7;		break;
+		case DESC_RATEVHT1SS_MCS8:	ret_rate = MGN_VHT1SS_MCS8;		break;
+		case DESC_RATEVHT1SS_MCS9:	ret_rate = MGN_VHT1SS_MCS9;		break;
+		case DESC_RATEVHT2SS_MCS0:	ret_rate = MGN_VHT2SS_MCS0;		break;
+		case DESC_RATEVHT2SS_MCS1:	ret_rate = MGN_VHT2SS_MCS1;		break;
+		case DESC_RATEVHT2SS_MCS2:	ret_rate = MGN_VHT2SS_MCS2;		break;
+		case DESC_RATEVHT2SS_MCS3:	ret_rate = MGN_VHT2SS_MCS3;		break;
+		case DESC_RATEVHT2SS_MCS4:	ret_rate = MGN_VHT2SS_MCS4;		break;
+		case DESC_RATEVHT2SS_MCS5:	ret_rate = MGN_VHT2SS_MCS5;		break;
+		case DESC_RATEVHT2SS_MCS6:	ret_rate = MGN_VHT2SS_MCS6;		break;
+		case DESC_RATEVHT2SS_MCS7:	ret_rate = MGN_VHT2SS_MCS7;		break;
+		case DESC_RATEVHT2SS_MCS8:	ret_rate = MGN_VHT2SS_MCS8;		break;
+		case DESC_RATEVHT2SS_MCS9:	ret_rate = MGN_VHT2SS_MCS9;		break;
+
+		default:
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+				("HwRateToMRate8812(): Non supported Rate [%x]!!!\n",rate ));
+			break;
+	}
+	return ret_rate;
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	odm_TxPwrTrackSetPwr88E()
+ *
+ * Overview:	88E change all channel tx power accordign to flag.
+ *				OFDM & CCK are all different.
+ *
+ * Input:		NONE
+ *
+ * Output:		NONE
+ *
+ * Return:		NONE
+ *
+ * Revised History:
+ *	When		Who		Remark
+ *	04/23/2012	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+void rtl8812ae_dm_txpwr_track_set_pwr(struct ieee80211_hw *hw,
+	enum pwr_track_control_method method, u8 rf_path, u8 channel_mapped_index)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_dm	*rtldm = rtl_dm(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u32 final_bb_swing_idx[2];
+	u8 pwr_tracking_limit = 26; /*+1.0dB*/
+	u8 tx_rate = 0xFF;
+	s8 final_ofdm_swing_index = 0;
+
+	if(rtldm->tx_rate != 0xFF)
+		tx_rate = rtl8812ae_hw_rate_to_mrate(hw, rtldm->tx_rate);
+
+
+	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+		("===>rtl8812ae_dm_txpwr_track_set_pwr\n"));
+
+	if(tx_rate != 0xFF) { /*20130429 Mimic Modify High Rate BBSwing Limit.*/
+		/*CCK*/
+		if((tx_rate >= MGN_1M) && (tx_rate <= MGN_11M))
+			pwr_tracking_limit = 32; /*+4dB*/
+		/*OFDM*/
+		else if((tx_rate >= MGN_6M) && (tx_rate <= MGN_48M))
+			pwr_tracking_limit = 30; /*+3dB*/
+		else if(tx_rate == MGN_54M)
+			pwr_tracking_limit = 28; /*+2dB*/
+		/*HT*/
+		else if((tx_rate >= MGN_MCS0) && (tx_rate <= MGN_MCS2)) /*QPSK/BPSK*/
+			pwr_tracking_limit = 34; /*+5dB*/
+		else if((tx_rate >= MGN_MCS3) && (tx_rate <= MGN_MCS4)) /*16QAM*/
+			pwr_tracking_limit = 30; /*+3dB*/
+		else if((tx_rate >= MGN_MCS5) && (tx_rate <= MGN_MCS7)) /*64QAM*/
+			pwr_tracking_limit = 28; /*+2dB*/
+
+		else if((tx_rate >= MGN_MCS8) && (tx_rate <= MGN_MCS10)) /*QPSK/BPSK*/
+			pwr_tracking_limit = 34; /*+5dB*/
+		else if((tx_rate >= MGN_MCS11) && (tx_rate <= MGN_MCS12)) /*16QAM*/
+			pwr_tracking_limit = 30; /*+3dB*/
+		else if((tx_rate >= MGN_MCS13) && (tx_rate <= MGN_MCS15)) /*64QAM*/
+			pwr_tracking_limit = 28; /*+2dB*/
+
+		/*2 VHT*/
+		else if((tx_rate >= MGN_VHT1SS_MCS0) && (tx_rate <= MGN_VHT1SS_MCS2)) /*QPSK/BPSK*/
+			pwr_tracking_limit = 34; /*+5dB*/
+		else if((tx_rate >= MGN_VHT1SS_MCS3) && (tx_rate <= MGN_VHT1SS_MCS4)) /*16QAM*/
+			pwr_tracking_limit = 30; /*+3dB*/
+		else if((tx_rate >= MGN_VHT1SS_MCS5)&&(tx_rate <= MGN_VHT1SS_MCS6)) /*64QAM*/
+			pwr_tracking_limit = 28; /*+2dB*/
+		else if(tx_rate == MGN_VHT1SS_MCS7) /*64QAM*/
+			pwr_tracking_limit = 26; /*+1dB*/
+		else if(tx_rate == MGN_VHT1SS_MCS8) /*256QAM*/
+			pwr_tracking_limit = 24; /*+0dB*/
+		else if(tx_rate == MGN_VHT1SS_MCS9) /*256QAM*/
+			pwr_tracking_limit = 22; /*-1dB*/
+
+		else if((tx_rate >= MGN_VHT2SS_MCS0)&&(tx_rate <= MGN_VHT2SS_MCS2)) /*QPSK/BPSK*/
+			pwr_tracking_limit = 34; /*+5dB*/
+		else if((tx_rate >= MGN_VHT2SS_MCS3)&&(tx_rate <= MGN_VHT2SS_MCS4)) /*16QAM*/
+			pwr_tracking_limit = 30; /*+3dB*/
+		else if((tx_rate >= MGN_VHT2SS_MCS5)&&(tx_rate <= MGN_VHT2SS_MCS6)) /*64QAM*/
+			pwr_tracking_limit = 28; /*+2dB*/
+		else if(tx_rate == MGN_VHT2SS_MCS7) /*64QAM*/
+			pwr_tracking_limit = 26; /*+1dB*/
+		else if(tx_rate == MGN_VHT2SS_MCS8) /*256QAM*/
+			pwr_tracking_limit = 24; /*+0dB*/
+		else if(tx_rate == MGN_VHT2SS_MCS9) /*256QAM*/
+			pwr_tracking_limit = 22; /*-1dB*/
+		else
+			pwr_tracking_limit = 24;
+	}
+	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+		("TxRate=0x%x, PwrTrackingLimit=%d\n", tx_rate, pwr_tracking_limit));
+
+
+	if (method == BBSWING) {
+		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+			("===>rtl8812ae_dm_txpwr_track_set_pwr\n"));
+
+		if (rf_path == RF90_PATH_A) {
+			final_bb_swing_idx[RF90_PATH_A] =
+				(rtldm->ofdm_index[RF90_PATH_A] > pwr_tracking_limit) ?
+				pwr_tracking_limit : rtldm->ofdm_index[RF90_PATH_A];
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+				("pDM_Odm->RFCalibrateInfo.OFDM_index[ODM_RF_PATH_A]=%d, \
+				pDM_Odm->RealBbSwingIdx[ODM_RF_PATH_A]=%d\n",
+				rtldm->ofdm_index[RF90_PATH_A], final_bb_swing_idx[RF90_PATH_A]));
+
+			rtl_set_bbreg(hw, RA_TXSCALE, 0xFFE00000, rtl8812ae_txscaling_table[final_bb_swing_idx[RF90_PATH_A]]);
+		} else {
+			final_bb_swing_idx[RF90_PATH_B] =
+				rtldm->ofdm_index[RF90_PATH_B] > pwr_tracking_limit ? \
+				pwr_tracking_limit : rtldm->ofdm_index[RF90_PATH_B];
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+				("pDM_Odm->RFCalibrateInfo.OFDM_index[ODM_RF_PATH_B]=%d, \
+				pDM_Odm->RealBbSwingIdx[ODM_RF_PATH_B]=%d\n",
+				rtldm->ofdm_index[RF90_PATH_B], final_bb_swing_idx[RF90_PATH_B]));
+
+			rtl_set_bbreg(hw, RB_TXSCALE, 0xFFE00000, rtl8812ae_txscaling_table[final_bb_swing_idx[RF90_PATH_B]]);
+		}
+	} else if (method == MIX_MODE) {
+		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+			("pDM_Odm->DefaultOfdmIndex=%d, \
+			pDM_Odm->Aboslute_OFDMSwingIdx[RFPath]=%d, RF_Path = %d\n",
+			rtldm->default_ofdm_index, rtldm->aboslute_ofdm_swing_idx[rf_path],
+			rf_path ));
+
+
+		final_ofdm_swing_index = rtldm->default_ofdm_index + rtldm->aboslute_ofdm_swing_idx[rf_path];
+
+		if (rf_path == RF90_PATH_A) {
+			if(final_ofdm_swing_index > pwr_tracking_limit) {     /*BBSwing higher then Limit*/
+
+				rtldm->remnant_cck_idx = final_ofdm_swing_index - pwr_tracking_limit;
+				/* CCK Follow the same compensate value as Path A*/
+				rtldm->remnant_ofdm_swing_idx[rf_path] = final_ofdm_swing_index - pwr_tracking_limit;
+
+				rtl_set_bbreg(hw, RA_TXSCALE, 0xFFE00000, rtl8812ae_txscaling_table[pwr_tracking_limit]);
+
+				rtldm->modify_txagc_flag_path_a = true;
+
+				/*Set TxAGC Page C{};*/
+				rtl8821ae_phy_set_txpower_level_by_path(hw, rtlphy->current_channel, RF90_PATH_A);
+
+				RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+					("******Path_A Over BBSwing Limit , PwrTrackingLimit = %d , Remnant TxAGC Value = %d \n",
+					pwr_tracking_limit, rtldm->remnant_ofdm_swing_idx[rf_path]));
+			} else if (final_ofdm_swing_index < 0) {
+				rtldm->remnant_cck_idx = final_ofdm_swing_index;
+				/* CCK Follow the same compensate value as Path A*/
+				rtldm->remnant_ofdm_swing_idx[rf_path] = final_ofdm_swing_index;
+
+				rtl_set_bbreg(hw, RA_TXSCALE, 0xFFE00000, rtl8812ae_txscaling_table[0]);
+
+				rtldm->modify_txagc_flag_path_a = true;
+
+				/*Set TxAGC Page C{};*/
+				rtl8821ae_phy_set_txpower_level_by_path(hw, rtlphy->current_channel, RF90_PATH_A);
+
+				RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+					("******Path_A Lower then BBSwing lower bound  0 , Remnant TxAGC Value = %d \n",
+					 rtldm->remnant_ofdm_swing_idx[rf_path]));
+			} else {
+				rtl_set_bbreg(hw, RA_TXSCALE, 0xFFE00000, rtl8812ae_txscaling_table[final_ofdm_swing_index]);
+
+				RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+					("******Path_A Compensate with BBSwing , Final_OFDM_Swing_Index = %d \n",
+					final_ofdm_swing_index));
+
+				if(rtldm->modify_txagc_flag_path_a) { /*If TxAGC has changed, reset TxAGC again*/
+					rtldm->remnant_cck_idx = 0;
+					rtldm->remnant_ofdm_swing_idx[rf_path] = 0;
+
+					/*Set TxAGC Page C{};*/
+					rtl8821ae_phy_set_txpower_level_by_path(hw, rtlphy->current_channel, RF90_PATH_A);
+
+					rtldm->modify_txagc_flag_path_a = false;
+
+					RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+						("******Path_A pDM_Odm->Modify_TxAGC_Flag = FALSE \n"));
+				}
+			}
+		}
+
+		if (rf_path == RF90_PATH_B) {
+			if(final_ofdm_swing_index > pwr_tracking_limit) {    /*BBSwing higher then Limit*/
+				rtldm->remnant_ofdm_swing_idx[rf_path] = final_ofdm_swing_index - pwr_tracking_limit;
+
+				rtl_set_bbreg(hw, RB_TXSCALE, 0xFFE00000, rtl8812ae_txscaling_table[pwr_tracking_limit]);
+
+				rtldm->modify_txagc_flag_path_b = true;
+
+				/*Set TxAGC Page E{};*/
+				rtl8821ae_phy_set_txpower_level_by_path(hw, rtlphy->current_channel, RF90_PATH_B);
+
+				RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+					("******Path_B Over BBSwing Limit , PwrTrackingLimit = %d , Remnant TxAGC Value = %d \n",
+					pwr_tracking_limit, rtldm->remnant_ofdm_swing_idx[rf_path]));
+			} else if (final_ofdm_swing_index < 0) {
+				rtldm->remnant_ofdm_swing_idx[rf_path] = final_ofdm_swing_index;
+
+				rtl_set_bbreg(hw, RB_TXSCALE, 0xFFE00000, rtl8812ae_txscaling_table[0]);
+
+				rtldm->modify_txagc_flag_path_b = true;
+
+				/*Set TxAGC Page E{};*/
+				rtl8821ae_phy_set_txpower_level_by_path(hw, rtlphy->current_channel, RF90_PATH_B);
+
+				RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+					("******Path_B Lower then BBSwing lower bound  0 , Remnant TxAGC Value = %d \n",
+					rtldm->remnant_ofdm_swing_idx[rf_path] ));
+			} else {
+				rtl_set_bbreg(hw, RB_TXSCALE, 0xFFE00000, rtl8812ae_txscaling_table[final_ofdm_swing_index]);
+
+				RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+					("******Path_B Compensate with BBSwing , Final_OFDM_Swing_Index = %d \n",
+					final_ofdm_swing_index));
+
+				if(rtldm->modify_txagc_flag_path_b) { /*If TxAGC has changed, reset TxAGC again*/
+					rtldm->remnant_ofdm_swing_idx[rf_path] = 0;
+
+					/*Set TxAGC Page E{};*/
+					rtl8821ae_phy_set_txpower_level_by_path(hw, rtlphy->current_channel, RF90_PATH_B);
+
+					rtldm->modify_txagc_flag_path_b = false;
+
+					RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+						("******Path_B pDM_Odm->Modify_TxAGC_Flag = FALSE \n"));
+				}
+			}
+		}
+
+	} else {
+		return;
+	}
+}
+
+void rtl8812ae_dm_txpower_tracking_callback_thermalmeter
+	(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_dm	*rtldm = rtl_dm(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	u8 thermal_value = 0, delta, delta_lck, delta_iqk, p = 0, i = 0;
+	u8 thermal_value_avg_count = 0;
+	u32 thermal_value_avg = 0;
+
+	u8 ofdm_min_index = 6;  /*OFDM BB Swing should be less than +3.0dB, which is required by Arthur*/
+	u8 index_for_channel = 0; /* GetRightChnlPlaceforIQK(pHalData->CurrentChannel)*/
+
+	/* 1. The following TWO tables decide the final index of OFDM/CCK swing table.*/
+	u8 *delta_swing_table_idx_tup_a;
+	u8 *delta_swing_table_idx_tdown_a;
+	u8 *delta_swing_table_idx_tup_b;
+	u8 *delta_swing_table_idx_tdown_b;
+
+	/*2. Initilization ( 7 steps in total )*/
+	rtl8812ae_get_delta_swing_table(hw, (u8**)&delta_swing_table_idx_tup_a,
+									(u8**)&delta_swing_table_idx_tdown_a,
+									  (u8**)&delta_swing_table_idx_tup_b,
+									  (u8**)&delta_swing_table_idx_tdown_b);
+
+	rtldm->btxpower_trackinginit = true;
+
+	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+		("===>rtl8812ae_dm_txpower_tracking_callback_thermalmeter, \
+		 \n pDM_Odm->BbSwingIdxCckBase: %d, pDM_Odm->BbSwingIdxOfdmBase[A]:\
+		 %d, pDM_Odm->DefaultOfdmIndex: %d\n",
+		rtldm->bb_swing_idx_cck_base,
+		rtldm->bb_swing_idx_ofdm_base[RF90_PATH_A],
+		rtldm->default_ofdm_index));
+
+	thermal_value = (u8)rtl_get_rfreg(hw, RF90_PATH_A, RF_T_METER_8812A, 0xfc00);	/*0x42: RF Reg[15:10] 88E*/
+	if( ! rtldm->txpower_track_control || rtlefuse->eeprom_thermalmeter == 0 ||
+		rtlefuse->eeprom_thermalmeter == 0xFF)
+        	return;
+
+
+	/* 3. Initialize ThermalValues of RFCalibrateInfo*/
+
+	if(rtlhal->reloadtxpowerindex)
+	{
+		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+			("reload ofdm index for band switch\n"));
+	}
+
+	/*4. Calculate average thermal meter*/
+	rtldm->thermalvalue_avg[rtldm->thermalvalue_avg_index] = thermal_value;
+	rtldm->thermalvalue_avg_index++;
+	if(rtldm->thermalvalue_avg_index == AVG_THERMAL_NUM_8812A)
+		/*Average times =  c.AverageThermalNum*/
+		rtldm->thermalvalue_avg_index = 0;
+
+	for(i = 0; i < AVG_THERMAL_NUM_8812A; i++)
+	{
+		if(rtldm->thermalvalue_avg[i])
+		{
+			thermal_value_avg += rtldm->thermalvalue_avg[i];
+			thermal_value_avg_count++;
+		}
+	}
+
+	if(thermal_value_avg_count) /*Calculate Average ThermalValue after average enough times*/
+	{
+		thermal_value = (u8)(thermal_value_avg / thermal_value_avg_count);
+		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+			("AVG Thermal Meter = 0x%X, EFUSE Thermal Base = 0x%X\n",
+			thermal_value, rtlefuse->eeprom_thermalmeter));
+	}
+
+	/*5. Calculate delta, delta_LCK, delta_IQK.*/
+	/*"delta" here is used to determine whether thermal value changes or not.*/
+	delta = (thermal_value > rtldm->thermalvalue) ? \
+		(thermal_value - rtldm->thermalvalue): \
+		(rtldm->thermalvalue - thermal_value);
+	delta_lck = (thermal_value > rtldm->thermalvalue_lck) ? \
+		(thermal_value - rtldm->thermalvalue_lck) : \
+		(rtldm->thermalvalue_lck - thermal_value);
+	delta_iqk = (thermal_value > rtldm->thermalvalue_iqk) ? \
+		(thermal_value - rtldm->thermalvalue_iqk) : \
+		(rtldm->thermalvalue_iqk - thermal_value);
+
+	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+		("(delta, delta_LCK, delta_IQK) = (%d, %d, %d)\n",
+		delta, delta_lck, delta_iqk));
+
+	/* 6. If necessary, do LCK.	*/
+
+	if (delta_lck >= IQK_THRESHOLD) /*Delta temperature is equal to or larger than 20 centigrade.*/
+	{
+		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+			("delta_LCK(%d) >= Threshold_IQK(%d)\n",
+			delta_lck, IQK_THRESHOLD));
+		rtldm->thermalvalue_lck = thermal_value;
+		rtl8812ae_phy_lccalibrate(hw);
+	}
+
+	/*7. If necessary, move the index of swing table to adjust Tx power.*/
+
+	if (delta > 0 && rtldm->txpower_track_control)
+	{
+		/*"delta" here is used to record the absolute value of differrence.*/
+	    	delta = thermal_value > rtlefuse->eeprom_thermalmeter ? \
+		    	(thermal_value - rtlefuse->eeprom_thermalmeter) : \
+		    	(rtlefuse->eeprom_thermalmeter - thermal_value);
+
+		if (delta >= TXPWR_TRACK_TABLE_SIZE)
+			delta = TXPWR_TRACK_TABLE_SIZE - 1;
+
+		/*7.1 The Final Power Index = BaseIndex + PowerIndexOffset*/
+
+		if(thermal_value > rtlefuse->eeprom_thermalmeter) {
+
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+			    		("delta_swing_table_idx_tup_a[%d] = %d\n",
+			    		delta, delta_swing_table_idx_tup_a[delta]));
+			rtldm->delta_power_index_last[RF90_PATH_A] = rtldm->delta_power_index[RF90_PATH_A];
+			rtldm->delta_power_index[RF90_PATH_A] = delta_swing_table_idx_tup_a[delta];
+
+			rtldm->aboslute_ofdm_swing_idx[RF90_PATH_A] =  delta_swing_table_idx_tup_a[delta];
+			/*Record delta swing for mix mode power tracking*/
+
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+			("******Temp is higher and pDM_Odm->Aboslute_OFDMSwingIdx[ODM_RF_PATH_A] = %d\n",
+			rtldm->aboslute_ofdm_swing_idx[RF90_PATH_A]));
+
+
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+	                                     ("delta_swing_table_idx_tup_b[%d] = %d\n",
+	                                     delta, delta_swing_table_idx_tup_b[delta]));
+			rtldm->delta_power_index_last[RF90_PATH_B] = rtldm->delta_power_index[RF90_PATH_B];
+			rtldm->delta_power_index[RF90_PATH_B] = delta_swing_table_idx_tup_b[delta];
+
+			rtldm->aboslute_ofdm_swing_idx[RF90_PATH_B] =  delta_swing_table_idx_tup_b[delta];
+			/*Record delta swing for mix mode power tracking*/
+
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+			("******Temp is higher and pDM_Odm->Aboslute_OFDMSwingIdx[ODM_RF_PATH_B] = %d\n",
+			rtldm->aboslute_ofdm_swing_idx[RF90_PATH_B]));
+
+        	} else {
+        		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+               	 	("delta_swing_table_idx_tdown_a[%d] = %d\n",
+               	 	delta, delta_swing_table_idx_tdown_a[delta]));
+
+			rtldm->delta_power_index_last[RF90_PATH_A] = rtldm->delta_power_index[RF90_PATH_A];
+			rtldm->delta_power_index[RF90_PATH_A] = -1 * delta_swing_table_idx_tdown_a[delta];
+
+			rtldm->aboslute_ofdm_swing_idx[RF90_PATH_A] =  -1 * delta_swing_table_idx_tdown_a[delta];
+			/* Record delta swing for mix mode power tracking*/
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+			("******Temp is lower and pDM_Odm->Aboslute_OFDMSwingIdx[ODM_RF_PATH_A] = %d\n",
+			rtldm->aboslute_ofdm_swing_idx[RF90_PATH_A]));
+
+
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+                		("deltaSwingTableIdx_TDOWN_B[%d] = %d\n",
+                		delta, delta_swing_table_idx_tdown_b[delta]));
+
+			rtldm->delta_power_index_last[RF90_PATH_B] = rtldm->delta_power_index[RF90_PATH_B];
+			rtldm->delta_power_index[RF90_PATH_B] = -1 * delta_swing_table_idx_tdown_b[delta];
+
+			rtldm->aboslute_ofdm_swing_idx[RF90_PATH_B] =  -1 * delta_swing_table_idx_tdown_b[delta];
+			/*Record delta swing for mix mode power tracking*/
+
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+			("******Temp is lower and pDM_Odm->Aboslute_OFDMSwingIdx[ODM_RF_PATH_B] = %d\n",
+			rtldm->aboslute_ofdm_swing_idx[RF90_PATH_B]));
+ 		}
+
+		for (p = RF90_PATH_A; p < MAX_PATH_NUM_8812A; p++)
+        	{
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+		                ("\n\n================================ [Path-%c] \
+		                Calculating PowerIndexOffset ================================\n",
+		                (p == RF90_PATH_A ? 'A' : 'B')));
+
+		    	if (rtldm->delta_power_index[p] == rtldm->delta_power_index_last[p])
+				/*If Thermal value changes but lookup table value still the same*/
+		    		rtldm->power_index_offset[p] = 0;
+		    	else
+		    		rtldm->power_index_offset[p] =
+		    			rtldm->delta_power_index[p] - rtldm->delta_power_index_last[p];
+				/*Power Index Diff between 2 times Power Tracking*/
+
+		    	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+		    		("[Path-%c] PowerIndexOffset(%d) = DeltaPowerIndex(%d) - DeltaPowerIndexLast(%d)\n",
+                		(p == RF90_PATH_A ? 'A' : 'B'),
+                		rtldm->power_index_offset[p],
+                		rtldm->delta_power_index[p] ,
+                		rtldm->delta_power_index_last[p]));
+
+	    		rtldm->ofdm_index[p] =
+					rtldm->bb_swing_idx_ofdm_base[p] + rtldm->power_index_offset[p];
+		    	rtldm->cck_index =
+					rtldm->bb_swing_idx_cck_base + rtldm->power_index_offset[p];
+
+		    	rtldm->bb_swing_idx_cck = rtldm->cck_index;
+		   	rtldm->bb_swing_idx_ofdm[p] = rtldm->ofdm_index[p];
+
+	           	/*************Print BB Swing Base and Index Offset*************/
+
+		    	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+		    		("The 'CCK' final index(%d) = BaseIndex(%d) + PowerIndexOffset(%d)\n",
+		    		rtldm->bb_swing_idx_cck,
+		    		rtldm->bb_swing_idx_cck_base,
+		    		rtldm->power_index_offset[p]));
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+				("The 'OFDM' final index(%d) = BaseIndex[%c](%d) + PowerIndexOffset(%d)\n",
+		   		rtldm->bb_swing_idx_ofdm[p],
+		   		(p == RF90_PATH_A ? 'A' : 'B'),
+		   		rtldm->bb_swing_idx_ofdm_base[p],
+		   		rtldm->power_index_offset[p]));
+
+		    	/*7.1 Handle boundary conditions of index.*/
+
+
+			if(rtldm->ofdm_index[p] > TXSCALE_TABLE_SIZE -1)
+			{
+				rtldm->ofdm_index[p] = TXSCALE_TABLE_SIZE -1;
+			}
+			else if (rtldm->ofdm_index[p] < ofdm_min_index)
+			{
+				rtldm->ofdm_index[p] = ofdm_min_index;
+			}
+		}
+		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+            		("\n\n======================================================\
+            		==================================================\n"));
+		if(rtldm->cck_index > TXSCALE_TABLE_SIZE -1)
+			rtldm->cck_index = TXSCALE_TABLE_SIZE -1;
+		else if (rtldm->cck_index < 0)
+			rtldm->cck_index = 0;
+	} else {
+		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+			("The thermal meter is unchanged or TxPowerTracking OFF(%d): \
+			ThermalValue: %d , pDM_Odm->RFCalibrateInfo.ThermalValue: %d\n",
+			rtldm->txpower_track_control,
+			thermal_value,
+			rtldm->thermalvalue));
+
+	    	for (p = RF90_PATH_A; p < MAX_PATH_NUM_8812A; p++)
+		    	rtldm->power_index_offset[p] = 0;
+	}
+	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+		("TxPowerTracking: [CCK] Swing Current Index: %d, Swing Base Index: %d\n",
+		rtldm->cck_index, rtldm->bb_swing_idx_cck_base));       /*Print Swing base & current*/
+	for (p = RF90_PATH_A; p < MAX_PATH_NUM_8812A; p++)
+	{
+		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+			("TxPowerTracking: [OFDM] Swing Current Index: %d, Swing Base Index[%c]: %d\n",
+			rtldm->ofdm_index[p],
+			(p == RF90_PATH_A ? 'A' : 'B'),
+			rtldm->bb_swing_idx_ofdm_base[p]));
+	}
+
+	if ((rtldm->power_index_offset[RF90_PATH_A] != 0 ||
+		rtldm->power_index_offset[RF90_PATH_B] != 0 ) &&
+     	 	rtldm->txpower_track_control)
+	{
+		/*7.2 Configure the Swing Table to adjust Tx Power.*/
+		/*Always TRUE after Tx Power is adjusted by power tracking.*/
+		/*
+		  2012/04/23 MH According to Luke's suggestion, we can not write BB digital
+		  to increase TX power. Otherwise, EVM will be bad.
+
+		  2012/04/25 MH Add for tx power tracking to set tx power in tx agc for 88E.
+		*/
+		if (thermal_value > rtldm->thermalvalue)
+		{
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+				("Temperature Increasing(A): delta_pi: %d , delta_t: %d, Now_t: %d, EFUSE_t: %d, Last_t: %d\n",
+				rtldm->power_index_offset[RF90_PATH_A],
+				delta, thermal_value,
+				rtlefuse->eeprom_thermalmeter,
+				rtldm->thermalvalue));
+
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+				("Temperature Increasing(B): delta_pi: %d , delta_t: %d, Now_t: %d, EFUSE_t: %d, Last_t: %d\n",
+				rtldm->power_index_offset[RF90_PATH_B],
+				delta, thermal_value,
+				rtlefuse->eeprom_thermalmeter,
+				rtldm->thermalvalue));
+
+		} else if (thermal_value < rtldm->thermalvalue) { /*Low temperature*/
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+				("Temperature Decreasing(A): delta_pi: %d , delta_t: %d, Now_t: %d, EFUSE_t: %d, Last_t: %d\n",
+				rtldm->power_index_offset[RF90_PATH_A],
+				delta, thermal_value,
+				rtlefuse->eeprom_thermalmeter,
+				rtldm->thermalvalue));
+
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+				("Temperature Decreasing(B): delta_pi: %d , delta_t: %d, Now_t: %d, EFUSE_t: %d, Last_t: %d\n",
+				rtldm->power_index_offset[RF90_PATH_B],
+				delta, thermal_value,
+				rtlefuse->eeprom_thermalmeter,
+				rtldm->thermalvalue));
+		}
+
+		if (thermal_value > rtlefuse->eeprom_thermalmeter) {
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+				("Temperature(%d) higher than PG value(%d)\n",
+				thermal_value, rtlefuse->eeprom_thermalmeter));
+
+
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+				("**********Enter POWER Tracking MIX_MODE**********\n"));
+			for (p = RF90_PATH_A; p < MAX_PATH_NUM_8812A; p++)
+					rtl8812ae_dm_txpwr_track_set_pwr(hw, MIX_MODE, p, 0);
+
+		} else {
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+				("Temperature(%d) lower than PG value(%d)\n",
+				thermal_value, rtlefuse->eeprom_thermalmeter));
+
+
+	            	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+				("**********Enter POWER Tracking MIX_MODE**********\n"));
+			for (p = RF90_PATH_A; p < MAX_PATH_NUM_8812A; p++)
+				rtl8812ae_dm_txpwr_track_set_pwr(hw, MIX_MODE, p, index_for_channel);
+
+		}
+
+		rtldm->bb_swing_idx_cck_base = rtldm->bb_swing_idx_cck;   /*Record last time Power Tracking result as base.*/
+		for (p = RF90_PATH_A; p < MAX_PATH_NUM_8812A; p++)
+				rtldm->bb_swing_idx_ofdm_base[p] = rtldm->bb_swing_idx_ofdm[p];
+
+	 		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+					("pDM_Odm->RFCalibrateInfo.ThermalValue = %d ThermalValue= %d\n",
+					rtldm->thermalvalue, thermal_value));
+
+		rtldm->thermalvalue = thermal_value;         /*Record last Power Tracking Thermal Value*/
+
+	}
+	/*Delta temperature is equal to or larger than 20 centigrade (When threshold is 8).*/
+	if ((delta_iqk >= IQK_THRESHOLD)) {
+
+		if ( !rtlphy->b_iqk_in_progress) {
+
+			spin_lock(&rtlpriv->locks.iqk_lock);
+			rtlphy->b_iqk_in_progress = true;
+			spin_unlock(&rtlpriv->locks.iqk_lock);
+
+			rtl8812ae_do_iqk(hw, delta_iqk, thermal_value, 8);
+
+			spin_lock(&rtlpriv->locks.iqk_lock);
+			rtlphy->b_iqk_in_progress = false;
+			spin_unlock(&rtlpriv->locks.iqk_lock);
+		}
+	}
+
+	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+		("<===rtl8812ae_dm_txpower_tracking_callback_thermalmeter\n"));
+}
+
+
+void rtl8821ae_get_delta_swing_table(
+	struct ieee80211_hw *hw,
+	u8 **temperature_up_a,
+	u8 **temperature_down_a,
+	u8 **temperature_up_b,
+	u8 **temperature_down_b
+	)
+{
+   	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	u8 channel = rtlphy->current_channel;
+	u8 rate = rtldm->tx_rate;
+
+
+	if ( 1 <= channel && channel <= 14) {
+		if (RX_HAL_IS_CCK_RATE(rate)) {
+		        *temperature_up_a = rtldm->delta_swing_table_idx_24gccka_p;
+		        *temperature_down_a = rtldm->delta_swing_table_idx_24gccka_n;
+		        *temperature_up_b = rtldm->delta_swing_table_idx_24gcckb_p;
+		        *temperature_down_b = rtldm->delta_swing_table_idx_24gcckb_n;
+		} else {
+		        *temperature_up_a = rtldm->delta_swing_table_idx_24ga_p;
+		        *temperature_down_a = rtldm->delta_swing_table_idx_24ga_n;
+		        *temperature_up_b = rtldm->delta_swing_table_idx_24gb_p;
+		        *temperature_down_b = rtldm->delta_swing_table_idx_24gb_n;
+		}
+ 	} else if ( 36 <= channel && channel <= 64) {
+	        *temperature_up_a = rtldm->delta_swing_table_idx_5ga_p[0];
+	        *temperature_down_a = rtldm->delta_swing_table_idx_5ga_n[0];
+	        *temperature_up_b = rtldm->delta_swing_table_idx_5gb_p[0];
+	        *temperature_down_b = rtldm->delta_swing_table_idx_5gb_n[0];
+    	} else if ( 100 <= channel && channel <= 140) {
+		*temperature_up_a = rtldm->delta_swing_table_idx_5ga_p[1];
+		*temperature_down_a = rtldm->delta_swing_table_idx_5ga_n[1];
+		*temperature_up_b = rtldm->delta_swing_table_idx_5gb_p[1];
+		*temperature_down_b = rtldm->delta_swing_table_idx_5gb_n[1];
+    	} else if ( 149 <= channel && channel <= 173) {
+		*temperature_up_a = rtldm->delta_swing_table_idx_5ga_p[2];
+		*temperature_down_a = rtldm->delta_swing_table_idx_5ga_n[2];
+		*temperature_up_b = rtldm->delta_swing_table_idx_5gb_p[2];
+		*temperature_down_b = rtldm->delta_swing_table_idx_5gb_n[2];
+    	} else {
+	    *temperature_up_a = (u8*)rtl8818e_delta_swing_table_idx_24gb_p_txpwrtrack;
+	    *temperature_down_a =(u8*)rtl8818e_delta_swing_table_idx_24gb_n_txpwrtrack;
+	    *temperature_up_b = (u8*)rtl8818e_delta_swing_table_idx_24gb_p_txpwrtrack;
+	    *temperature_down_b = (u8*)rtl8818e_delta_swing_table_idx_24gb_n_txpwrtrack;
+    	}
+
+	return;
+}
+
+void rtl8821ae_phy_lccalibrate(
+	struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD, ("===> rtl8812ae_phy_lccalibrate\n"));
+
+	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD, ("<=== rtl8812ae_phy_lccalibrate\n"));
+
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	odm_TxPwrTrackSetPwr88E()
+ *
+ * Overview:	88E change all channel tx power accordign to flag.
+ *				OFDM & CCK are all different.
+ *
+ * Input:		NONE
+ *
+ * Output:		NONE
+ *
+ * Return:		NONE
+ *
+ * Revised History:
+ *	When		Who		Remark
+ *	04/23/2012	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+void rtl8821ae_dm_txpwr_track_set_pwr(struct ieee80211_hw *hw,
+	enum pwr_track_control_method method, u8 rf_path, u8 channel_mapped_index)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_dm	*rtldm = rtl_dm(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u32 final_bb_swing_idx[1];
+	u8 pwr_tracking_limit = 26; /*+1.0dB*/
+	u8 tx_rate = 0xFF;
+	s8 final_ofdm_swing_index = 0;
+
+	if(rtldm->tx_rate != 0xFF)
+		tx_rate = rtl8812ae_hw_rate_to_mrate(hw, rtldm->tx_rate);
+
+
+	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+		("===>rtl8812ae_dm_txpwr_track_set_pwr\n"));
+
+	if(tx_rate != 0xFF) { /*20130429 Mimic Modify High Rate BBSwing Limit.*/
+		/*CCK*/
+		if((tx_rate >= MGN_1M) && (tx_rate <= MGN_11M))
+			pwr_tracking_limit = 32; /*+4dB*/
+		/*OFDM*/
+		else if((tx_rate >= MGN_6M) && (tx_rate <= MGN_48M))
+			pwr_tracking_limit = 30; /*+3dB*/
+		else if(tx_rate == MGN_54M)
+			pwr_tracking_limit = 28; /*+2dB*/
+		/*HT*/
+		else if((tx_rate >= MGN_MCS0) && (tx_rate <= MGN_MCS2)) /*QPSK/BPSK*/
+			pwr_tracking_limit = 34; /*+5dB*/
+		else if((tx_rate >= MGN_MCS3) && (tx_rate <= MGN_MCS4)) /*16QAM*/
+			pwr_tracking_limit = 30; /*+3dB*/
+		else if((tx_rate >= MGN_MCS5) && (tx_rate <= MGN_MCS7)) /*64QAM*/
+			pwr_tracking_limit = 28; /*+2dB*/
+#if 0
+		else if((tx_rate >= MGN_MCS8) && (tx_rate <= MGN_MCS10)) /*QPSK/BPSK*/
+			pwr_tracking_limit = 34; /*+5dB*/
+		else if((tx_rate >= MGN_MCS11) && (tx_rate <= MGN_MCS12)) /*16QAM*/
+			pwr_tracking_limit = 30; /*+3dB*/
+		else if((tx_rate >= MGN_MCS13) && (tx_rate <= MGN_MCS15)) /*64QAM*/
+			pwr_tracking_limit = 28; /*+2dB*/
+#endif
+		/*2 VHT*/
+		else if((tx_rate >= MGN_VHT1SS_MCS0) && (tx_rate <= MGN_VHT1SS_MCS2)) /*QPSK/BPSK*/
+			pwr_tracking_limit = 34; /*+5dB*/
+		else if((tx_rate >= MGN_VHT1SS_MCS3) && (tx_rate <= MGN_VHT1SS_MCS4)) /*16QAM*/
+			pwr_tracking_limit = 30; /*+3dB*/
+		else if((tx_rate >= MGN_VHT1SS_MCS5)&&(tx_rate <= MGN_VHT1SS_MCS6)) /*64QAM*/
+			pwr_tracking_limit = 28; /*+2dB*/
+		else if(tx_rate == MGN_VHT1SS_MCS7) /*64QAM*/
+			pwr_tracking_limit = 26; /*+1dB*/
+		else if(tx_rate == MGN_VHT1SS_MCS8) /*256QAM*/
+			pwr_tracking_limit = 24; /*+0dB*/
+		else if(tx_rate == MGN_VHT1SS_MCS9) /*256QAM*/
+			pwr_tracking_limit = 22; /*-1dB*/
+		else
+			pwr_tracking_limit = 24;
+	}
+	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+		("TxRate=0x%x, PwrTrackingLimit=%d\n", tx_rate, pwr_tracking_limit));
+
+
+	if (method == BBSWING) {
+		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+			("===>rtl8812ae_dm_txpwr_track_set_pwr\n"));
+
+		if (rf_path == RF90_PATH_A) {
+			final_bb_swing_idx[RF90_PATH_A] =
+				(rtldm->ofdm_index[RF90_PATH_A] > pwr_tracking_limit) ?
+				pwr_tracking_limit : rtldm->ofdm_index[RF90_PATH_A];
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+				("pDM_Odm->RFCalibrateInfo.OFDM_index[ODM_RF_PATH_A]=%d, \
+				pDM_Odm->RealBbSwingIdx[ODM_RF_PATH_A]=%d\n",
+				rtldm->ofdm_index[RF90_PATH_A], final_bb_swing_idx[RF90_PATH_A]));
+
+			rtl_set_bbreg(hw, RA_TXSCALE, 0xFFE00000, rtl8812ae_txscaling_table[final_bb_swing_idx[RF90_PATH_A]]);
+		}
+	} else if (method == MIX_MODE) {
+		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+			("pDM_Odm->DefaultOfdmIndex=%d, \
+			pDM_Odm->Aboslute_OFDMSwingIdx[RFPath]=%d, RF_Path = %d\n",
+			rtldm->default_ofdm_index, rtldm->aboslute_ofdm_swing_idx[rf_path],
+			rf_path ));
+
+
+		final_ofdm_swing_index = rtldm->default_ofdm_index + rtldm->aboslute_ofdm_swing_idx[rf_path];
+
+		if (rf_path == RF90_PATH_A) {
+			if(final_ofdm_swing_index > pwr_tracking_limit) {     /*BBSwing higher then Limit*/
+
+				rtldm->remnant_cck_idx = final_ofdm_swing_index - pwr_tracking_limit;
+				/* CCK Follow the same compensate value as Path A*/
+				rtldm->remnant_ofdm_swing_idx[rf_path] = final_ofdm_swing_index - pwr_tracking_limit;
+
+				rtl_set_bbreg(hw, RA_TXSCALE, 0xFFE00000, rtl8812ae_txscaling_table[pwr_tracking_limit]);
+
+				rtldm->modify_txagc_flag_path_a = true;
+
+				/*Set TxAGC Page C{};*/
+				rtl8821ae_phy_set_txpower_level_by_path(hw, rtlphy->current_channel, RF90_PATH_A);
+
+				RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+					("******Path_A Over BBSwing Limit , PwrTrackingLimit = %d , Remnant TxAGC Value = %d \n",
+					pwr_tracking_limit, rtldm->remnant_ofdm_swing_idx[rf_path]));
+			} else if (final_ofdm_swing_index < 0) {
+				rtldm->remnant_cck_idx = final_ofdm_swing_index;
+				/* CCK Follow the same compensate value as Path A*/
+				rtldm->remnant_ofdm_swing_idx[rf_path] = final_ofdm_swing_index;
+
+				rtl_set_bbreg(hw, RA_TXSCALE, 0xFFE00000, rtl8812ae_txscaling_table[0]);
+
+				rtldm->modify_txagc_flag_path_a = true;
+
+				/*Set TxAGC Page C{};*/
+				rtl8821ae_phy_set_txpower_level_by_path(hw, rtlphy->current_channel, RF90_PATH_A);
+
+				RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+					("******Path_A Lower then BBSwing lower bound  0 , Remnant TxAGC Value = %d \n",
+					 rtldm->remnant_ofdm_swing_idx[rf_path]));
+			} else {
+				rtl_set_bbreg(hw, RA_TXSCALE, 0xFFE00000, rtl8812ae_txscaling_table[final_ofdm_swing_index]);
+
+				RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+					("******Path_A Compensate with BBSwing , Final_OFDM_Swing_Index = %d \n",
+					final_ofdm_swing_index));
+
+				if(rtldm->modify_txagc_flag_path_a) { /*If TxAGC has changed, reset TxAGC again*/
+					rtldm->remnant_cck_idx = 0;
+					rtldm->remnant_ofdm_swing_idx[rf_path] = 0;
+
+					/*Set TxAGC Page C{};*/
+					rtl8821ae_phy_set_txpower_level_by_path(hw, rtlphy->current_channel, RF90_PATH_A);
+
+					rtldm->modify_txagc_flag_path_a = false;
+
+					RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+						("******Path_A pDM_Odm->Modify_TxAGC_Flag = FALSE \n"));
+				}
+			}
+		}
+
+	} else {
+		return;
+	}
+}
+
+
+void rtl8821ae_dm_txpower_tracking_callback_thermalmeter
+	(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_dm	*rtldm = rtl_dm(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	u8 thermal_value = 0, delta, delta_lck, delta_iqk, p = 0, i = 0;
+	u8 thermal_value_avg_count = 0;
+	u32 thermal_value_avg = 0;
+
+	u8 ofdm_min_index = 6;  /*OFDM BB Swing should be less than +3.0dB, which is required by Arthur*/
+	u8 index_for_channel = 0; /* GetRightChnlPlaceforIQK(pHalData->CurrentChannel)*/
+
+	/* 1. The following TWO tables decide the final index of OFDM/CCK swing table.*/
+	u8 *delta_swing_table_idx_tup_a;
+	u8 *delta_swing_table_idx_tdown_a;
+	u8 *delta_swing_table_idx_tup_b;
+	u8 *delta_swing_table_idx_tdown_b;
+
+	/*2. Initilization ( 7 steps in total )*/
+	rtl8821ae_get_delta_swing_table(hw, (u8**)&delta_swing_table_idx_tup_a,
+									(u8**)&delta_swing_table_idx_tdown_a,
+									  (u8**)&delta_swing_table_idx_tup_b,
+									  (u8**)&delta_swing_table_idx_tdown_b);
+
+	rtldm->btxpower_trackinginit = true;
+
+	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+		("===>rtl8812ae_dm_txpower_tracking_callback_thermalmeter, \
+		 \n pDM_Odm->BbSwingIdxCckBase: %d, pDM_Odm->BbSwingIdxOfdmBase[A]:\
+		 %d, pDM_Odm->DefaultOfdmIndex: %d\n",
+		rtldm->bb_swing_idx_cck_base,
+		rtldm->bb_swing_idx_ofdm_base[RF90_PATH_A],
+		rtldm->default_ofdm_index));
+
+	thermal_value = (u8)rtl_get_rfreg(hw, RF90_PATH_A, RF_T_METER_8812A, 0xfc00);	/*0x42: RF Reg[15:10] 88E*/
+	if( ! rtldm->txpower_track_control || rtlefuse->eeprom_thermalmeter == 0 ||
+		rtlefuse->eeprom_thermalmeter == 0xFF)
+        	return;
+
+
+	/* 3. Initialize ThermalValues of RFCalibrateInfo*/
+
+	if(rtlhal->reloadtxpowerindex)
+	{
+		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+			("reload ofdm index for band switch\n"));
+	}
+
+	/*4. Calculate average thermal meter*/
+	rtldm->thermalvalue_avg[rtldm->thermalvalue_avg_index] = thermal_value;
+	rtldm->thermalvalue_avg_index++;
+	if(rtldm->thermalvalue_avg_index == AVG_THERMAL_NUM_8812A)
+		/*Average times =  c.AverageThermalNum*/
+		rtldm->thermalvalue_avg_index = 0;
+
+	for(i = 0; i < AVG_THERMAL_NUM_8812A; i++)
+	{
+		if(rtldm->thermalvalue_avg[i])
+		{
+			thermal_value_avg += rtldm->thermalvalue_avg[i];
+			thermal_value_avg_count++;
+		}
+	}
+
+	if(thermal_value_avg_count) /*Calculate Average ThermalValue after average enough times*/
+	{
+		thermal_value = (u8)(thermal_value_avg / thermal_value_avg_count);
+		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+			("AVG Thermal Meter = 0x%X, EFUSE Thermal Base = 0x%X\n",
+			thermal_value, rtlefuse->eeprom_thermalmeter));
+	}
+
+	/*5. Calculate delta, delta_LCK, delta_IQK.*/
+	/*"delta" here is used to determine whether thermal value changes or not.*/
+	delta = (thermal_value > rtldm->thermalvalue) ? \
+		(thermal_value - rtldm->thermalvalue): \
+		(rtldm->thermalvalue - thermal_value);
+	delta_lck = (thermal_value > rtldm->thermalvalue_lck) ? \
+		(thermal_value - rtldm->thermalvalue_lck) : \
+		(rtldm->thermalvalue_lck - thermal_value);
+	delta_iqk = (thermal_value > rtldm->thermalvalue_iqk) ? \
+		(thermal_value - rtldm->thermalvalue_iqk) : \
+		(rtldm->thermalvalue_iqk - thermal_value);
+
+	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+		("(delta, delta_LCK, delta_IQK) = (%d, %d, %d)\n",
+		delta, delta_lck, delta_iqk));
+
+	/* 6. If necessary, do LCK.	*/
+
+	if (delta_lck >= IQK_THRESHOLD) /*Delta temperature is equal to or larger than 20 centigrade.*/
+	{
+		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+			("delta_LCK(%d) >= Threshold_IQK(%d)\n",
+			delta_lck, IQK_THRESHOLD));
+		rtldm->thermalvalue_lck = thermal_value;
+		rtl8821ae_phy_lccalibrate(hw);
+	}
+
+	/*7. If necessary, move the index of swing table to adjust Tx power.*/
+
+	if (delta > 0 && rtldm->txpower_track_control)
+	{
+		/*"delta" here is used to record the absolute value of differrence.*/
+	    	delta = thermal_value > rtlefuse->eeprom_thermalmeter ? \
+		    	(thermal_value - rtlefuse->eeprom_thermalmeter) : \
+		    	(rtlefuse->eeprom_thermalmeter - thermal_value);
+
+		if (delta >= TXSCALE_TABLE_SIZE)
+			delta = TXSCALE_TABLE_SIZE - 1;
+
+		/*7.1 The Final Power Index = BaseIndex + PowerIndexOffset*/
+
+		if(thermal_value > rtlefuse->eeprom_thermalmeter) {
+
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+			    		("delta_swing_table_idx_tup_a[%d] = %d\n",
+			    		delta, delta_swing_table_idx_tup_a[delta]));
+			rtldm->delta_power_index_last[RF90_PATH_A] = rtldm->delta_power_index[RF90_PATH_A];
+			rtldm->delta_power_index[RF90_PATH_A] = delta_swing_table_idx_tup_a[delta];
+
+			rtldm->aboslute_ofdm_swing_idx[RF90_PATH_A] =  delta_swing_table_idx_tup_a[delta];
+			/*Record delta swing for mix mode power tracking*/
+
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+			("******Temp is higher and pDM_Odm->Aboslute_OFDMSwingIdx[ODM_RF_PATH_A] = %d\n",
+			rtldm->aboslute_ofdm_swing_idx[RF90_PATH_A]));
+
+        	} else {
+        		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+               	 	("delta_swing_table_idx_tdown_a[%d] = %d\n",
+               	 	delta, delta_swing_table_idx_tdown_a[delta]));
+
+			rtldm->delta_power_index_last[RF90_PATH_A] = rtldm->delta_power_index[RF90_PATH_A];
+			rtldm->delta_power_index[RF90_PATH_A] = -1 * delta_swing_table_idx_tdown_a[delta];
+
+			rtldm->aboslute_ofdm_swing_idx[RF90_PATH_A] =  -1 * delta_swing_table_idx_tdown_a[delta];
+			/* Record delta swing for mix mode power tracking*/
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+			("******Temp is lower and pDM_Odm->Aboslute_OFDMSwingIdx[ODM_RF_PATH_A] = %d\n",
+			rtldm->aboslute_ofdm_swing_idx[RF90_PATH_A]));
+ 		}
+
+		for (p = RF90_PATH_A; p < MAX_PATH_NUM_8821A; p++)
+        	{
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+		                ("\n\n================================ [Path-%c] \
+		                Calculating PowerIndexOffset ================================\n",
+		                (p == RF90_PATH_A ? 'A' : 'B')));
+
+		    	if (rtldm->delta_power_index[p] == rtldm->delta_power_index_last[p])
+				/*If Thermal value changes but lookup table value still the same*/
+		    		rtldm->power_index_offset[p] = 0;
+		    	else
+		    		rtldm->power_index_offset[p] =
+		    			rtldm->delta_power_index[p] - rtldm->delta_power_index_last[p];
+				/*Power Index Diff between 2 times Power Tracking*/
+
+		    	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+		    		("[Path-%c] PowerIndexOffset(%d) = DeltaPowerIndex(%d) - DeltaPowerIndexLast(%d)\n",
+                		(p == RF90_PATH_A ? 'A' : 'B'),
+                		rtldm->power_index_offset[p],
+                		rtldm->delta_power_index[p] ,
+                		rtldm->delta_power_index_last[p]));
+
+	    		rtldm->ofdm_index[p] =
+					rtldm->bb_swing_idx_ofdm_base[p] + rtldm->power_index_offset[p];
+		    	rtldm->cck_index =
+					rtldm->bb_swing_idx_cck_base + rtldm->power_index_offset[p];
+
+		    	rtldm->bb_swing_idx_cck = rtldm->cck_index;
+		   	rtldm->bb_swing_idx_ofdm[p] = rtldm->ofdm_index[p];
+
+	           	/*************Print BB Swing Base and Index Offset*************/
+
+		    	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+		    		("The 'CCK' final index(%d) = BaseIndex(%d) + PowerIndexOffset(%d)\n",
+		    		rtldm->bb_swing_idx_cck,
+		    		rtldm->bb_swing_idx_cck_base,
+		    		rtldm->power_index_offset[p]));
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+				("The 'OFDM' final index(%d) = BaseIndex[%c](%d) + PowerIndexOffset(%d)\n",
+		   		rtldm->bb_swing_idx_ofdm[p],
+		   		(p == RF90_PATH_A ? 'A' : 'B'),
+		   		rtldm->bb_swing_idx_ofdm_base[p],
+		   		rtldm->power_index_offset[p]));
+
+		    	/*7.1 Handle boundary conditions of index.*/
+
+
+			if(rtldm->ofdm_index[p] > TXSCALE_TABLE_SIZE -1)
+			{
+				rtldm->ofdm_index[p] = TXSCALE_TABLE_SIZE -1;
+			}
+			else if (rtldm->ofdm_index[p] < ofdm_min_index)
+			{
+				rtldm->ofdm_index[p] = ofdm_min_index;
+			}
+		}
+		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+            		("\n\n======================================================\
+            		==================================================\n"));
+		if(rtldm->cck_index > TXSCALE_TABLE_SIZE -1)
+			rtldm->cck_index = TXSCALE_TABLE_SIZE -1;
+		else if (rtldm->cck_index < 0)
+			rtldm->cck_index = 0;
+	} else {
+		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+			("The thermal meter is unchanged or TxPowerTracking OFF(%d): \
+			ThermalValue: %d , pDM_Odm->RFCalibrateInfo.ThermalValue: %d\n",
+			rtldm->txpower_track_control,
+			thermal_value,
+			rtldm->thermalvalue));
+
+	    	for (p = RF90_PATH_A; p < MAX_PATH_NUM_8821A; p++)
+		    	rtldm->power_index_offset[p] = 0;
+	}
+	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+		("TxPowerTracking: [CCK] Swing Current Index: %d, Swing Base Index: %d\n",
+		rtldm->cck_index, rtldm->bb_swing_idx_cck_base));       /*Print Swing base & current*/
+	for (p = RF90_PATH_A; p < MAX_PATH_NUM_8821A; p++)
+	{
+		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+			("TxPowerTracking: [OFDM] Swing Current Index: %d, Swing Base Index[%c]: %d\n",
+			rtldm->ofdm_index[p],
+			(p == RF90_PATH_A ? 'A' : 'B'),
+			rtldm->bb_swing_idx_ofdm_base[p]));
+	}
+
+	if ((rtldm->power_index_offset[RF90_PATH_A] != 0 ||
+		rtldm->power_index_offset[RF90_PATH_B] != 0 ) &&
+     	 	rtldm->txpower_track_control)
+	{
+		/*7.2 Configure the Swing Table to adjust Tx Power.*/
+		/*Always TRUE after Tx Power is adjusted by power tracking.*/
+		/*
+		  2012/04/23 MH According to Luke's suggestion, we can not write BB digital
+		  to increase TX power. Otherwise, EVM will be bad.
+
+		  2012/04/25 MH Add for tx power tracking to set tx power in tx agc for 88E.
+		*/
+		if (thermal_value > rtldm->thermalvalue)
+		{
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+				("Temperature Increasing(A): delta_pi: %d , delta_t: %d, Now_t: %d, EFUSE_t: %d, Last_t: %d\n",
+				rtldm->power_index_offset[RF90_PATH_A],
+				delta, thermal_value,
+				rtlefuse->eeprom_thermalmeter,
+				rtldm->thermalvalue));
+		} else if (thermal_value < rtldm->thermalvalue) { /*Low temperature*/
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+				("Temperature Decreasing(A): delta_pi: %d , delta_t: %d, Now_t: %d, EFUSE_t: %d, Last_t: %d\n",
+				rtldm->power_index_offset[RF90_PATH_A],
+				delta, thermal_value,
+				rtlefuse->eeprom_thermalmeter,
+				rtldm->thermalvalue));
+		}
+
+		if (thermal_value > rtlefuse->eeprom_thermalmeter) {
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+				("Temperature(%d) higher than PG value(%d)\n",
+				thermal_value, rtlefuse->eeprom_thermalmeter));
+
+
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+				("**********Enter POWER Tracking MIX_MODE**********\n"));
+			for (p = RF90_PATH_A; p < MAX_PATH_NUM_8821A; p++)
+					rtl8821ae_dm_txpwr_track_set_pwr(hw, MIX_MODE, p, index_for_channel);
+
+		} else {
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+				("Temperature(%d) lower than PG value(%d)\n",
+				thermal_value, rtlefuse->eeprom_thermalmeter));
+
+
+	            	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+				("**********Enter POWER Tracking MIX_MODE**********\n"));
+			for (p = RF90_PATH_A; p < MAX_PATH_NUM_8821A; p++)
+				rtl8812ae_dm_txpwr_track_set_pwr(hw, MIX_MODE, p, index_for_channel);
+
+		}
+
+		rtldm->bb_swing_idx_cck_base = rtldm->bb_swing_idx_cck;   /*Record last time Power Tracking result as base.*/
+		for (p = RF90_PATH_A; p < MAX_PATH_NUM_8821A; p++)
+				rtldm->bb_swing_idx_ofdm_base[p] = rtldm->bb_swing_idx_ofdm[p];
+
+	 		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+					("pDM_Odm->RFCalibrateInfo.ThermalValue = %d ThermalValue= %d\n",
+					rtldm->thermalvalue, thermal_value));
+
+		rtldm->thermalvalue = thermal_value;         /*Record last Power Tracking Thermal Value*/
+
+	}
+	/*Delta temperature is equal to or larger than 20 centigrade (When threshold is 8).*/
+	if ((delta_iqk >= IQK_THRESHOLD)) {
+
+		if ( !rtlphy->b_iqk_in_progress) {
+
+			spin_lock(&rtlpriv->locks.iqk_lock);
+			rtlphy->b_iqk_in_progress = true;
+			spin_unlock(&rtlpriv->locks.iqk_lock);
+
+			rtl8821ae_do_iqk(hw, delta_iqk, thermal_value, 8);
+
+			spin_lock(&rtlpriv->locks.iqk_lock);
+			rtlphy->b_iqk_in_progress = false;
+			spin_unlock(&rtlpriv->locks.iqk_lock);
+		}
+	}
+
+	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+		("<===rtl8812ae_dm_txpower_tracking_callback_thermalmeter\n"));
+}
+
+
+void rtl8821ae_dm_check_txpower_tracking_thermalmeter(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	static u8 tm_trigger = 0;
+
+	//if (!rtlpriv->dm.btxpower_tracking)
+	//	return;
+
+	if (!tm_trigger) {
+		rtl_set_rfreg(hw, RF90_PATH_A, RF_T_METER_88E, BIT(17)|BIT(16),
+			      0x03);
+		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+			 ("Trigger 8821ae Thermal Meter!!\n"));
+		tm_trigger = 1;
+		return;
+	} else {
+		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
+			 ("Schedule TxPowerTracking !!\n"));
+
+		rtl8821ae_dm_txpower_tracking_callback_thermalmeter(hw);
+		tm_trigger = 0;
+	}
+}
+
+
+void rtl8821ae_dm_refresh_rate_adaptive_mask(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rate_adaptive *p_ra = &(rtlpriv->ra);
+	u32 low_rssithresh_for_ra = p_ra->low2high_rssi_thresh_for_ra;
+	u32 high_rssithresh_for_ra = p_ra->high_rssi_thresh_for_ra;
+	u8 go_up_gap = 5;
+	struct ieee80211_sta *sta = NULL;
+
+	if (is_hal_stop(rtlhal)) {
+		RT_TRACE(COMP_RATE, DBG_LOUD,
+			 ("driver is going to unload\n"));
+		return;
+	}
+
+	if (!rtlpriv->dm.b_useramask) {
+		RT_TRACE(COMP_RATE, DBG_LOUD,
+			 ("driver does not control rate adaptive mask\n"));
+		return;
+	}
+
+	if (mac->link_state == MAC80211_LINKED &&
+		mac->opmode == NL80211_IFTYPE_STATION) {
+
+		switch (p_ra->pre_ratr_state) {
+			case DM_RATR_STA_MIDDLE:
+				high_rssithresh_for_ra += go_up_gap;
+				break;
+			case DM_RATR_STA_LOW:
+				high_rssithresh_for_ra += go_up_gap;
+				low_rssithresh_for_ra += go_up_gap;
+				break;
+			default:
+				break;
+		}
+
+		if (rtlpriv->dm.undecorated_smoothed_pwdb >
+		    (long)high_rssithresh_for_ra)
+			p_ra->ratr_state = DM_RATR_STA_HIGH;
+		else if (rtlpriv->dm.undecorated_smoothed_pwdb >
+			 (long)low_rssithresh_for_ra)
+			p_ra->ratr_state = DM_RATR_STA_MIDDLE;
+		else
+			p_ra->ratr_state = DM_RATR_STA_LOW;
+
+		if (p_ra->pre_ratr_state != p_ra->ratr_state ) {
+			RT_TRACE(COMP_RATE, DBG_LOUD,
+				 ("RSSI = %ld\n",
+				  rtlpriv->dm.undecorated_smoothed_pwdb));
+			RT_TRACE(COMP_RATE, DBG_LOUD,
+				 ("RSSI_LEVEL = %d\n", p_ra->ratr_state));
+			RT_TRACE(COMP_RATE, DBG_LOUD,
+				 ("PreState = %d, CurState = %d\n",
+				  p_ra->pre_ratr_state, p_ra->ratr_state));
+
+			rcu_read_lock();
+			sta = rtl_find_sta(hw, mac->bssid);
+			if (sta)
+			rtlpriv->cfg->ops->update_rate_tbl(hw, sta, p_ra->ratr_state);
+			rcu_read_unlock();
+
+			p_ra->pre_ratr_state = p_ra->ratr_state;
+		}
+	}
+}
+
+bool rtl8821ae_dm_is_edca_turbo_disable(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (rtlpriv->btcoexist.btc_ops->btc_is_disable_edca_turbo(rtlpriv))
+		return true;
+	if (rtlpriv->mac80211.mode == WIRELESS_MODE_B)
+		return true;
+
+	return false;
+}
+
+void rtl8821ae_dm_edca_choose_traffic_idx(
+	struct ieee80211_hw *hw, u64 cur_tx_bytes, u64 cur_rx_bytes, bool b_bias_on_rx,
+	bool *pb_is_cur_rdl_state)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if(b_bias_on_rx)
+	{
+		if (cur_tx_bytes > (cur_rx_bytes*4)) {
+			*pb_is_cur_rdl_state = false;
+			RT_TRACE(COMP_TURBO, DBG_LOUD,
+				("Uplink Traffic\n "));
+		} else {
+			*pb_is_cur_rdl_state = true;
+			RT_TRACE(COMP_TURBO, DBG_LOUD,
+				("Balance Traffic\n"));
+		}
+	} else {
+		if (cur_rx_bytes > (cur_tx_bytes*4)) {
+			*pb_is_cur_rdl_state = true;
+			RT_TRACE(COMP_TURBO, DBG_LOUD,
+				("Downlink	Traffic\n"));
+		} else {
+			*pb_is_cur_rdl_state = false;
+			RT_TRACE(COMP_TURBO, DBG_LOUD,
+				("Balance Traffic\n"));
+		}
+	}
+	return ;
+}
+
+static void rtl8821ae_dm_check_edca_turbo(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_dm *rtldm =  rtl_dm(rtl_priv(hw));
+
+	/*Keep past Tx/Rx packet count for RT-to-RT EDCA turbo.*/
+	unsigned long cur_tx_ok_cnt = 0;
+	unsigned long cur_rx_ok_cnt = 0;
+	u32 edca_be_ul = 0x5ea42b;
+	u32 edca_be_dl = 0x5ea42b;
+	u32 edca_be = 0x5ea42b;
+	u8 iot_peer = 0;
+	bool *pb_is_cur_rdl_state = NULL;
+	bool b_last_is_cur_rdl_state = false;
+	bool b_bias_on_rx = false;
+	bool b_edca_turbo_on = false;
+
+	RT_TRACE(COMP_TURBO, DBG_LOUD,
+		("rtl8821ae_dm_check_edca_turbo=====>"));
+	RT_TRACE(COMP_TURBO, DBG_LOUD,
+		("Orginial BE PARAM: 0x%x\n",
+		rtl_read_dword(rtlpriv, DM_REG_EDCA_BE_11N)));
+
+	/*===============================
+	list paramter for different platform
+	===============================*/
+	b_last_is_cur_rdl_state = rtlpriv->dm.bis_cur_rdlstate;
+	pb_is_cur_rdl_state = &( rtlpriv->dm.bis_cur_rdlstate);
+
+	cur_tx_ok_cnt = rtlpriv->stats.txbytesunicast - rtldm->last_tx_ok_cnt;
+	cur_rx_ok_cnt = rtlpriv->stats.rxbytesunicast - rtldm->last_rx_ok_cnt;
+
+	rtldm->last_tx_ok_cnt = rtlpriv->stats.txbytesunicast;
+	rtldm->last_rx_ok_cnt = rtlpriv->stats.rxbytesunicast;
+
+	iot_peer = rtlpriv->mac80211.vendor;
+	b_bias_on_rx = (iot_peer == PEER_RAL || iot_peer == PEER_ATH) ?
+		       true : false;
+	b_edca_turbo_on = ((!rtlpriv->dm.bis_any_nonbepkts) &&
+			   (!rtlpriv->dm.b_disable_framebursting)) ?
+			   true : false;
+
+	/*if (rtl8821ae_dm_is_edca_turbo_disable(hw))
+		goto dm_CheckEdcaTurbo_EXIT;*/
+
+	if ((iot_peer == PEER_CISCO) && (mac->mode == WIRELESS_MODE_N_24G))
+	{
+		edca_be_dl = edca_setting_dl[iot_peer];
+		edca_be_ul = edca_setting_ul[iot_peer];
+	}
+
+	RT_TRACE(COMP_TURBO, DBG_LOUD,
+		("bIsAnyNonBEPkts : 0x%x  bDisableFrameBursting : 0x%x  \n",
+		rtlpriv->dm.bis_any_nonbepkts, rtlpriv->dm.b_disable_framebursting));
+
+	RT_TRACE(COMP_TURBO, DBG_LOUD,
+			("bEdcaTurboOn : 0x%x bBiasOnRx : 0x%x\n",
+			b_edca_turbo_on, b_bias_on_rx));
+
+	if (b_edca_turbo_on) {
+		RT_TRACE(COMP_TURBO, DBG_LOUD,
+			("curTxOkCnt : 0x%lx \n",cur_tx_ok_cnt));
+		RT_TRACE(COMP_TURBO, DBG_LOUD,
+			("curRxOkCnt : 0x%lx \n",cur_rx_ok_cnt));
+		if(b_bias_on_rx)
+			rtl8821ae_dm_edca_choose_traffic_idx(hw, cur_tx_ok_cnt,
+				cur_rx_ok_cnt, true, pb_is_cur_rdl_state);
+		else
+			rtl8821ae_dm_edca_choose_traffic_idx(hw, cur_tx_ok_cnt,
+				cur_rx_ok_cnt, false, pb_is_cur_rdl_state);
+
+		edca_be = ((*pb_is_cur_rdl_state) == true) ? edca_be_dl : edca_be_ul;
+
+		rtl_write_dword(rtlpriv, DM_REG_EDCA_BE_11N, edca_be);
+
+		RT_TRACE(COMP_TURBO, DBG_LOUD,
+			("EDCA Turbo on: EDCA_BE:0x%x\n", edca_be));
+
+		rtlpriv->dm.bcurrent_turbo_edca = true;
+
+		RT_TRACE(COMP_TURBO, DBG_LOUD,
+			("EDCA_BE_DL : 0x%x  EDCA_BE_UL : 0x%x  EDCA_BE : 0x%x  \n",
+			edca_be_dl, edca_be_ul, edca_be));
+	} else {
+		if (rtlpriv->dm.bcurrent_turbo_edca) {
+			u8 tmp = AC0_BE;
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AC_PARAM,
+						      (u8 *) (&tmp));
+		}
+		rtlpriv->dm.bcurrent_turbo_edca = false;
+	}
+
+/* dm_CheckEdcaTurbo_EXIT: */
+	rtlpriv->dm.bis_any_nonbepkts = false;
+	rtldm->last_tx_ok_cnt = rtlpriv->stats.txbytesunicast;
+	rtldm->last_rx_ok_cnt = rtlpriv->stats.rxbytesunicast;
+}
+
+static void rtl8821ae_dm_cck_packet_detection_thresh(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 cur_cck_cca_thresh;
+
+	if (rtlpriv->mac80211.link_state >= MAC80211_LINKED) {
+		/*dm_digtable.rssi_val_min = rtl8821ae_dm_initial_gain_min_pwdb(hw);*/
+		if (dm_digtable.rssi_val_min > 25)
+			cur_cck_cca_thresh = 0xcd;
+		else if ((dm_digtable.rssi_val_min <= 25) && (dm_digtable.rssi_val_min > 10))
+			cur_cck_cca_thresh = 0x83;
+		else {
+			if (rtlpriv->falsealm_cnt.cnt_cck_fail > 1000)
+				cur_cck_cca_thresh = 0x83;
+			else
+				cur_cck_cca_thresh = 0x40;
+		}
+
+	} else {
+		if (rtlpriv->falsealm_cnt.cnt_cck_fail > 1000)
+			cur_cck_cca_thresh = 0x83;
+		else
+			cur_cck_cca_thresh = 0x40;
+	}
+
+	if (dm_digtable.cur_cck_cca_thres != cur_cck_cca_thresh) {
+		rtl_set_bbreg(hw, RCCK0_CCA, MASKBYTE2, cur_cck_cca_thresh);
+	}
+
+	dm_digtable.pre_cck_cca_thres = dm_digtable.cur_cck_cca_thres;
+	dm_digtable.cur_cck_cca_thres = cur_cck_cca_thresh;
+	RT_TRACE(COMP_DIG, DBG_TRACE,
+		 ("CCK cca thresh hold =%x\n", dm_digtable.cur_cck_cca_thres));
+
+}
+
+void rtl8821ae_dm_dynamic_edcca(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	bool b_fw_current_in_ps_mode = false;
+
+	rtlpriv->cfg->ops->get_hw_reg(hw,HW_VAR_FW_PSMODE_STATUS, \
+		(u8*)(&b_fw_current_in_ps_mode));
+	if (b_fw_current_in_ps_mode)
+		return;
+}
+
+void rtl8812ae_dm_update_txpath(struct ieee80211_hw *hw, u8 path)
+{
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (rtldm->resp_tx_path != path) {
+		RT_TRACE(COMP_DIG, DBG_LOUD, \
+			("Need to Update Tx Path\n"));
+		if (path == RF90_PATH_A) {
+			/*Tx by Reg*/
+			rtl_set_bbreg(hw, 0x80c, 0xFFF0, 0x111);
+			 /*Resp Tx by Txinfo*/
+			rtl_set_bbreg(hw, 0x6d8, BIT(7) | BIT(6), 1);
+		} else {
+			/*Tx by Reg*/
+			rtl_set_bbreg(hw, 0x80c, 0xFFF0, 0x222);
+			 /*Resp Tx by Txinfo*/
+			rtl_set_bbreg(hw, 0x6d8, BIT(7) |BIT(6), 2);
+		}
+	}
+	rtldm->resp_tx_path = path;
+	RT_TRACE(COMP_DIG, DBG_LOUD, \
+		("Path=%s\n",(path == RF90_PATH_A) ?  \
+		"RF90_PATH_A":"RF90_PATH_A"));
+}
+
+void rtl8812ae_dm_path_diversity_init(struct ieee80211_hw *hw)
+{
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+
+	//rtl_set_bbreg(hw, 0x80c , BIT(29), 1); /*Tx path from Reg*/
+	rtl_set_bbreg(hw, 0x80c , 0xFFF0, 0x111); /*Tx by Reg*/
+	rtl_set_bbreg(hw, 0x6d8 , BIT(7) | BIT(6), 1); /*Resp Tx by Txinfo*/
+	rtl8812ae_dm_update_txpath(hw, RF90_PATH_A);
+
+	rtldm->path_sel = 1; /* TxInfo default at path-A*/
+}
+
+void rtl812ae_dm_set_txpath_by_txinfo(struct ieee80211_hw *hw,
+	u8 *pdesc)
+{
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+
+	SET_TX_DESC_TX_ANT(pdesc, rtldm->path_sel);
+}
+
+void rtl8812ae_dm_path_statistics(struct ieee80211_hw *hw,
+	u32 rssi_a, u32 rssi_b)
+{
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+
+	rtldm->patha_sum += rssi_a;
+	rtldm->patha_cnt ++;
+
+	rtldm->pathb_sum += rssi_b;
+	rtldm->pathb_cnt ++;
+}
+
+void rtl8812ae_dm_path_diversity(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u32	rssi_avg_a = 0;
+	u32 rssi_avg_b = 0;
+	u32 local_min_rssi = 0;
+	u32 min_rssi = 0xFF;
+	u8 tx_resp_path=0, target_path;
+	struct ieee80211_sta *sta = NULL;
+
+	sta = rtl_find_sta(hw, mac->bssid);
+	if (sta) {
+		/*Caculate RSSI per Path*/
+		rssi_avg_a = (rtldm->patha_cnt != 0) ? \
+			(rtldm->patha_sum / rtldm->patha_cnt) : 0;
+		rssi_avg_b = (rtldm->pathb_cnt != 0) ? \
+			(rtldm->pathb_sum / rtldm->pathb_cnt) : 0;
+
+		target_path = (rssi_avg_a == rssi_avg_b) ? rtldm->resp_tx_path : \
+			((rssi_avg_a>=rssi_avg_b) ? RF90_PATH_A : RF90_PATH_B);
+
+		RT_TRACE(COMP_DIG, DBG_TRACE, \
+			("assoc_id=%d, PathA_Sum=%d, PathA_Cnt=%d\n", \
+			mac->assoc_id, rtldm->patha_sum, rtldm->patha_cnt));
+		RT_TRACE(COMP_DIG, DBG_TRACE, \
+			("assoc_id=%d, PathB_Sum=%d, PathB_Cnt=%d\n", \
+			mac->assoc_id, rtldm->pathb_sum, rtldm->pathb_cnt));
+		RT_TRACE(COMP_DIG, DBG_TRACE, \
+			("assoc_id=%d, RssiAvgA= %d, RssiAvgB= %d\n", \
+			mac->assoc_id, rssi_avg_a, rssi_avg_b));
+
+		/*Select Resp Tx Path*/
+		local_min_rssi = (rssi_avg_a > rssi_avg_b) ?  rssi_avg_b : rssi_avg_a;
+		if(local_min_rssi  < min_rssi)
+		{
+			min_rssi = local_min_rssi;
+			tx_resp_path = target_path;
+		}
+
+		/*Select Tx DESC*/
+		if(target_path == RF90_PATH_A)
+			rtldm->path_sel = 1;
+		else
+			rtldm->path_sel = 2;
+
+		RT_TRACE(COMP_DIG, DBG_TRACE, \
+			("Tx from TxInfo, TargetPath=%s\n", \
+			(target_path==RF90_PATH_A) ? \
+			"ODM_RF_PATH_A":"ODM_RF_PATH_B"));
+		RT_TRACE(COMP_DIG, DBG_TRACE, \
+			("pDM_PathDiv->PathSel= %d\n", \
+			rtldm->path_sel));
+	}
+	rtldm->patha_cnt = 0;
+	rtldm->patha_sum = 0;
+	rtldm->pathb_cnt = 0;
+	rtldm->pathb_sum = 0;
+}
+
+void rtl8821ae_dm_dynamic_atc_switch(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	u8 crystal_cap;
+	u32 packet_count;
+	int cfo_khz_a,cfo_khz_b,cfo_ave = 0, adjust_xtal = 0;
+	int cfo_ave_diff;
+
+	if (rtlpriv->mac80211.link_state < MAC80211_LINKED){
+		/*1.Enable ATC*/
+		if (rtldm->atc_status == ATC_STATUS_OFF)
+		{
+			rtl_set_bbreg(hw, RFC_AREA, BIT(14), ATC_STATUS_ON);
+			rtldm->atc_status = ATC_STATUS_ON;
+		}
+
+		RT_TRACE(COMP_DIG, DBG_LOUD, \
+			("rtl8821ae_dm_dynamic_atc_switch(): No link!!\n"));
+		RT_TRACE(COMP_DIG, DBG_LOUD, \
+			("rtl8821ae_dm_dynamic_atc_switch(): atc_status = %d\n", \
+			rtldm->atc_status));
+
+		if (rtldm->crystal_cap != rtlpriv->efuse.crystalcap)
+		{
+			rtldm->crystal_cap = rtlpriv->efuse.crystalcap;
+			crystal_cap = rtldm->crystal_cap & 0x3f;
+			crystal_cap = crystal_cap & 0x3f;
+			rtl_set_bbreg(hw, REG_MAC_PHY_CTRL, \
+				0x7ff80000, (crystal_cap | (crystal_cap << 6)));
+		}
+		RT_TRACE(COMP_DIG, DBG_LOUD, \
+			("rtl8821ae_dm_dynamic_atc_switch(): crystal_cap = 0x%x\n", \
+			rtldm->crystal_cap));
+	}else{
+		/*1. Calculate CFO for path-A & path-B*/
+		cfo_khz_a = (int)(rtldm->cfo_tail[0] * 3125) / 1280;
+		cfo_khz_b = (int)(rtldm->cfo_tail[1] * 3125) / 1280;
+		packet_count = rtldm->packet_count;
+
+		/*2.No new packet*/
+		if (packet_count == rtldm->packet_count_pre) {
+			RT_TRACE(COMP_DIG, DBG_LOUD, \
+				("rtl8821ae_dm_dynamic_atc_switch(): packet counter doesn't change\n"));
+			return;
+		}
+
+		rtldm->packet_count_pre = packet_count;
+		RT_TRACE(COMP_DIG, DBG_LOUD, \
+			("rtl8821ae_dm_dynamic_atc_switch(): packet counter = %d\n", \
+			rtldm->packet_count));
+
+		/*3.Average CFO*/
+		if (rtlpriv->phy.rf_type == RF_1T1R)
+			cfo_ave = cfo_khz_a;
+		else
+			cfo_ave = (cfo_khz_a + cfo_khz_b) >> 1;
+
+		RT_TRACE(COMP_DIG, DBG_LOUD, \
+			("rtl8821ae_dm_dynamic_atc_switch():"
+			"cfo_khz_a = %dkHz, cfo_khz_b = %dkHz, cfo_ave = %dkHz\n",
+			cfo_khz_a, cfo_khz_b, cfo_ave));
+
+		/*4.Avoid abnormal large CFO*/
+		cfo_ave_diff = (rtldm->cfo_ave_pre >= cfo_ave)?
+						(rtldm->cfo_ave_pre - cfo_ave):
+						(cfo_ave - rtldm->cfo_ave_pre);
+
+		if (cfo_ave_diff > 20 && rtldm->large_cfo_hit == 0){
+			RT_TRACE(COMP_DIG, DBG_LOUD, \
+				("rtl8821ae_dm_dynamic_atc_switch(): first large CFO hit\n"));
+			rtldm->large_cfo_hit = 1;
+			return;
+		}
+		else
+			rtldm->large_cfo_hit = 0;
+
+		rtldm->cfo_ave_pre = cfo_ave;
+
+		/*CFO tracking by adjusting Xtal cap.*/
+
+		/*1.Dynamic Xtal threshold*/
+		if (cfo_ave >= -rtldm->cfo_threshold &&
+			cfo_ave <= rtldm->cfo_threshold &&
+			rtldm->is_freeze == 0){
+			if (rtldm->cfo_threshold == CFO_THRESHOLD_XTAL){
+				rtldm->cfo_threshold = CFO_THRESHOLD_XTAL + 10;
+				rtldm->is_freeze = 1;
+			}
+			else
+				rtldm->cfo_threshold = CFO_THRESHOLD_XTAL;
+		}
+		RT_TRACE(COMP_DIG, DBG_LOUD, \
+			("rtl8821ae_dm_dynamic_atc_switch(): Dynamic threshold = %d\n", \
+			rtldm->cfo_threshold));
+
+		/* 2.Calculate Xtal offset*/
+		if (cfo_ave > rtldm->cfo_threshold && rtldm->crystal_cap < 0x3f)
+			adjust_xtal = ((cfo_ave - CFO_THRESHOLD_XTAL) >> 2) + 1;
+		else if ((cfo_ave < -rtlpriv->dm.cfo_threshold) && rtlpriv->dm.crystal_cap > 0)
+			adjust_xtal = ((cfo_ave + CFO_THRESHOLD_XTAL) >> 2) - 1;
+		RT_TRACE(COMP_DIG, DBG_LOUD, \
+			("rtl8821ae_dm_dynamic_atc_switch(): "
+			"Crystal cap = 0x%x, Crystal cap offset = %d\n",
+			rtldm->crystal_cap, adjust_xtal));
+
+		/*3.Adjudt Crystal Cap.*/
+		if (adjust_xtal != 0){
+			rtldm->is_freeze = 0;
+			rtldm->crystal_cap += adjust_xtal;
+
+			if (rtldm->crystal_cap > 0x3f)
+				rtldm->crystal_cap = 0x3f;
+			else if (rtldm->crystal_cap < 0)
+				rtldm->crystal_cap = 0;
+
+			crystal_cap = rtldm->crystal_cap & 0x3f;
+			crystal_cap = crystal_cap & 0x3f;
+			rtl_set_bbreg(hw, REG_MAC_PHY_CTRL, \
+				0x7ff80000, (crystal_cap | (crystal_cap << 6)));
+			RT_TRACE(COMP_DIG, DBG_LOUD, \
+				("rtl8821ae_dm_dynamic_atc_switch(): New crystal cap = 0x%x \n", \
+				rtldm->crystal_cap));
+		}
+	}
+
+}
+
+void rtl8821ae_dm_watchdog(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool b_fw_current_inpsmode = false;
+	bool b_fw_ps_awake = true;
+
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
+				      (u8 *) (&b_fw_current_inpsmode));
+
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FWLPS_RF_ON,
+				      (u8 *) (&b_fw_ps_awake));
+
+	if(ppsc->p2p_ps_info.p2p_ps_mode)
+		b_fw_ps_awake = false;
+
+	if((ppsc->rfpwr_state == ERFON) &&
+		((!b_fw_current_inpsmode) && b_fw_ps_awake) &&
+		(!ppsc->rfchange_inprogress)) {
+		rtl8821ae_dm_common_info_self_update(hw);
+		rtl8821ae_dm_false_alarm_counter_statistics(hw);
+		rtl8821ae_dm_check_rssi_monitor(hw);
+		rtl8821ae_dm_dig(hw);
+		rtl8821ae_dm_dynamic_edcca(hw);
+		rtl8821ae_dm_cck_packet_detection_thresh(hw);
+		rtl8821ae_dm_refresh_rate_adaptive_mask(hw);
+		rtl8821ae_dm_check_edca_turbo(hw);
+		rtl8821ae_dm_dynamic_atc_switch(hw);
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+			rtl8812ae_dm_check_txpower_tracking_thermalmeter(hw);
+		else
+			rtl8821ae_dm_check_txpower_tracking_thermalmeter(hw);
+		rtl8821ae_dm_iq_calibrate(hw);
+		if (rtlpriv->cfg->ops->get_btc_status()){
+			rtlpriv->btcoexist.btc_ops->btc_periodical(rtlpriv);
+		}
+	}
+
+	rtlpriv->dm.dbginfo.num_qry_beacon_pkt = 0;
+}
+
+void rtl8821ae_dm_set_tx_ant_by_tx_info(struct ieee80211_hw *hw,
+												   u8 *pdesc, u32 mac_id)
+{
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	struct fast_ant_trainning *pfat_table= &(rtldm->fat_table);
+
+	if (rtlhal->hw_type != HARDWARE_TYPE_RTL8812AE)
+		return;
+
+	if ((rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV) ||
+		(rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV)){
+		SET_TX_DESC_TX_ANT(pdesc, pfat_table->antsel_a[mac_id]);
+	}
+}
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/dm.h b/drivers/staging/rtl8821ae/rtl8821ae/dm.h
new file mode 100644
index 000000000000..ebbff9b6cacf
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/dm.h
@@ -0,0 +1,426 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef	__RTL8821AE_DM_H__
+#define __RTL8821AE_DM_H__
+
+#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
+
+#define	TXSCALE_TABLE_SIZE 37
+
+/*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_88E_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_CCA_11AC			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_IGI_A_11AC				0xC50
+#define	DM_REG_ANTDIV_PARA2_11N		0xC54
+#define	DM_REG_IGI_B_11N					0xC58
+#define	DM_REG_IGI_B_11AC					0xE50
+#define	DM_REG_ANTDIV_PARA3_11N		0xC5C
+#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	DM_BIT_IGI_11AC					0xFFFFFFFF
+
+
+
+#define HAL_DM_DIG_DISABLE			BIT(0)
+#define HAL_DM_HIPWR_DISABLE		BIT(1)
+
+#define OFDM_TABLE_LENGTH 			43
+#define CCK_TABLE_LENGTH 			33
+
+#define OFDM_TABLE_SIZE 			37
+#define CCK_TABLE_SIZE				33
+
+#define BW_AUTO_SWITCH_HIGH_LOW		25
+#define BW_AUTO_SWITCH_LOW_HIGH		30
+
+#define DM_DIG_THRESH_HIGH			40
+#define DM_DIG_THRESH_LOW			35
+
+#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 */
+
+#define AVG_THERMAL_NUM_8812A	4
+#define TXPWR_TRACK_TABLE_SIZE 	30
+#define MAX_PATH_NUM_8812A		2
+#define MAX_PATH_NUM_8821A		1
+
+
+struct ps_t {
+	u8 pre_ccastate;
+	u8 cur_ccasate;
+	u8 pre_rfstate;
+	u8 cur_rfstate;
+	u8 initialize;
+	long rssi_val_min;
+
+};
+
+struct dig_t {
+	u8 dig_enable_flag;
+	u8 dig_ext_port_stage;
+	u32 rssi_lowthresh;
+	u32 rssi_highthresh;
+
+	u32 fa_lowthresh;
+	u32 fa_highthresh;
+
+	u8 cursta_connectctate;
+	u8 presta_connectstate;
+	u8 curmultista_connectstate;
+
+	u8 pre_igvalue;
+	u8 cur_igvalue;
+	u8 bt30_cur_igi;
+	u8 backup_igvalue;
+	u8 stop_dig;
+
+	char backoff_val;
+	char backoff_val_range_max;
+	char backoff_val_range_min;
+	u8 rx_gain_range_max;
+	u8 rx_gain_range_min;
+	u8 rssi_val_min;
+
+	u8 pre_cck_cca_thres;
+	u8 cur_cck_cca_thres;
+	u8 pre_cck_pd_state;
+	u8 cur_cck_pd_state;
+
+	u8 large_fa_hit;
+	u8 forbidden_igi;
+	u32 recover_cnt;
+
+	u8 dig_dynamic_min_0;
+	u8 dig_dynamic_min_1;
+	bool b_media_connect_0;
+	bool b_media_connect_1;
+
+	u32 antdiv_rssi_max;
+	u32 rssi_max;
+};
+
+
+enum FAT_STATE {
+	FAT_NORMAL_STATE	= 0,
+	FAT_TRAINING_STATE = 1,
+};
+
+enum tag_dynamic_init_gain_operation_type_definition {
+	DIG_TYPE_THRESH_HIGH = 0,
+	DIG_TYPE_THRESH_LOW = 1,
+	DIG_TYPE_BACKOFF = 2,
+	DIG_TYPE_RX_GAIN_MIN = 3,
+	DIG_TYPE_RX_GAIN_MAX = 4,
+	DIG_TYPE_ENABLE = 5,
+	DIG_TYPE_DISABLE = 6,
+	DIG_OP_TYPE_MAX
+};
+
+enum tag_cck_packet_detection_threshold_type_definition {
+	CCK_PD_STAGE_LowRssi = 0,
+	CCK_PD_STAGE_HighRssi = 1,
+	CCK_FA_STAGE_Low = 2,
+	CCK_FA_STAGE_High = 3,
+	CCK_PD_STAGE_MAX = 4,
+};
+
+enum dm_1r_cca_e {
+	CCA_1R = 0,
+	CCA_2R = 1,
+	CCA_MAX = 2,
+};
+
+enum dm_rf_e {
+	RF_SAVE = 0,
+	RF_NORMAL = 1,
+	RF_MAX = 2,
+};
+
+enum dm_sw_ant_switch_e {
+	ANS_ANTENNA_B = 1,
+	ANS_ANTENNA_A = 2,
+	ANS_ANTENNA_MAX = 3,
+};
+
+enum dm_dig_ext_port_alg_e {
+	DIG_EXT_PORT_STAGE_0 = 0,
+	DIG_EXT_PORT_STAGE_1 = 1,
+	DIG_EXT_PORT_STAGE_2 = 2,
+	DIG_EXT_PORT_STAGE_3 = 3,
+	DIG_EXT_PORT_STAGE_MAX = 4,
+};
+
+enum dm_dig_connect_e {
+	DIG_STA_DISCONNECT = 0,
+	DIG_STA_CONNECT = 1,
+	DIG_STA_BEFORE_CONNECT = 2,
+	DIG_MULTISTA_DISCONNECT = 3,
+	DIG_MULTISTA_CONNECT = 4,
+	DIG_CONNECT_MAX
+};
+
+enum pwr_track_control_method {
+	BBSWING,
+	TXAGC,
+	MIX_MODE
+};
+
+#define BT_RSSI_STATE_NORMAL_POWER      BIT_OFFSET_LEN_MASK_32(0, 1)
+#define BT_RSSI_STATE_AMDPU_OFF         BIT_OFFSET_LEN_MASK_32(1, 1)
+#define BT_RSSI_STATE_SPECIAL_LOW       BIT_OFFSET_LEN_MASK_32(2, 1)
+#define BT_RSSI_STATE_BG_EDCA_LOW       BIT_OFFSET_LEN_MASK_32(3, 1)
+#define BT_RSSI_STATE_TXPOWER_LOW       BIT_OFFSET_LEN_MASK_32(4, 1)
+#define GET_UNDECORATED_AVERAGE_RSSI(_priv)     \
+        (((struct rtl_priv *)(_priv))->mac80211.opmode == NL80211_IFTYPE_ADHOC)?  \
+        (((struct rtl_priv *)(_priv))->dm.entry_min_undecoratedsmoothed_pwdb):  \
+        (((struct rtl_priv *)(_priv))->dm.undecorated_smoothed_pwdb)
+
+extern struct dig_t dm_digtable;
+void rtl8821ae_dm_set_tx_ant_by_tx_info(struct ieee80211_hw *hw,
+												   u8 *pdesc, u32 mac_id);
+void rtl8821ae_dm_ant_sel_statistics(struct ieee80211_hw *hw,
+											  u8 antsel_tr_mux, u32 mac_id,
+											  u32 rx_pwdb_all);
+void rtl8821ae_dm_fast_antenna_trainning_callback(unsigned long data);
+void rtl8821ae_dm_init(struct ieee80211_hw *hw);
+void rtl8821ae_dm_watchdog(struct ieee80211_hw *hw);
+void rtl8821ae_dm_write_dig(struct ieee80211_hw *hw, u8 current_igi);
+void rtl8821ae_dm_init_edca_turbo(struct ieee80211_hw *hw);
+void rtl8821ae_dm_check_txpower_tracking_thermalmeter(struct ieee80211_hw *hw);
+void rtl8821ae_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw);
+void rtl8821ae_dm_txpower_track_adjust(struct ieee80211_hw *hw,
+												   u8 type,u8 *pdirection,
+												   u32 *poutwrite_val);
+void rtl8821ae_dm_clear_txpower_tracking_state(struct ieee80211_hw *hw);
+void rtl8821ae_dm_write_cck_cca_thres(struct ieee80211_hw *hw, u8 current_cca);
+void rtl8821ae_dm_initialize_txpower_tracking_thermalmeter(struct ieee80211_hw *hw);
+void rtl8812ae_dm_path_diversity(struct ieee80211_hw *hw);
+void rtl8812ae_dm_path_diversity_init(struct ieee80211_hw *hw);
+void rtl8812ae_dm_path_statistics(struct ieee80211_hw *hw,
+	u32 rssi_a, u32 rssi_b);
+void rtl812ae_dm_set_txpath_by_txinfo(struct ieee80211_hw *hw,
+	u8 *pdesc);
+void rtl8812ae_dm_txpwr_track_set_pwr(struct ieee80211_hw *hw,
+												enum pwr_track_control_method method,
+												u8 rf_path,
+												u8 channel_mapped_index);
+void rtl8821ae_dm_txpwr_track_set_pwr(struct ieee80211_hw *hw,
+	enum pwr_track_control_method method, u8 rf_path, u8 channel_mapped_index);
+
+void rtl8812ae_dm_update_init_rate(struct ieee80211_hw *hw, u8 rate);
+u8 rtl8812ae_hw_rate_to_mrate(struct ieee80211_hw *hw, u8 rate);
+void rtl8812ae_dm_txpower_tracking_callback_thermalmeter(struct ieee80211_hw *hw);
+void rtl8821ae_dm_txpower_tracking_callback_thermalmeter(struct ieee80211_hw *hw);
+#endif
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/fw.c b/drivers/staging/rtl8821ae/rtl8821ae/fw.c
new file mode 100644
index 000000000000..4083cab926a3
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/fw.c
@@ -0,0 +1,1349 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../pci.h"
+#include "../base.h"
+#include "reg.h"
+#include "def.h"
+#include "fw.h"
+#include "dm.h"
+
+static void _rtl8821ae_enable_fw_download(struct ieee80211_hw *hw, bool enable)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmp;
+
+	if (enable) {
+		rtl_write_byte(rtlpriv, REG_MCUFWDL, 0x05);
+
+		tmp = rtl_read_byte(rtlpriv, REG_MCUFWDL + 2);
+		rtl_write_byte(rtlpriv, REG_MCUFWDL + 2, tmp & 0xf7);
+
+		tmp = rtl_read_byte(rtlpriv, REG_MCUFWDL);
+		//printk("0x80=%02x.\n",tmp);
+	} else {
+		tmp = rtl_read_byte(rtlpriv, REG_MCUFWDL);
+		rtl_write_byte(rtlpriv, REG_MCUFWDL, tmp & 0xfe);
+		tmp = rtl_read_byte(rtlpriv, REG_MCUFWDL);
+		//printk("0x80=%02x.\n",tmp);
+	}
+
+}
+
+static void _rtl8821ae_fw_block_write(struct ieee80211_hw *hw,
+				   const u8 *buffer, u32 size)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 blockSize = sizeof(u32);
+	u8 *bufferPtr = (u8 *) buffer;
+	u32 *pu4BytePtr = (u32 *) buffer;
+	u32 i, offset, blockCount, remainSize;
+
+	blockCount = size / blockSize;
+	remainSize = size % blockSize;
+
+	for (i = 0; i < blockCount; i++) {
+		offset = i * blockSize;
+		rtl_write_dword(rtlpriv, (FW_8821AE_START_ADDRESS + offset),
+				*(pu4BytePtr + i));
+	}
+
+	if (remainSize) {
+		offset = blockCount * blockSize;
+		bufferPtr += offset;
+		for (i = 0; i < remainSize; i++) {
+			rtl_write_byte(rtlpriv, (FW_8821AE_START_ADDRESS +
+						 offset + i), *(bufferPtr + i));
+		}
+	}
+}
+
+static void _rtl8821ae_fw_page_write(struct ieee80211_hw *hw,
+				  u32 page, const u8 *buffer, u32 size)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 value8;
+	u8 u8page = (u8) (page & 0x07);
+
+	value8 = (rtl_read_byte(rtlpriv, REG_MCUFWDL + 2) & 0xF8) | u8page;
+
+	rtl_write_byte(rtlpriv, (REG_MCUFWDL + 2), value8);
+	_rtl8821ae_fw_block_write(hw, buffer, size);
+}
+
+static void _rtl8821ae_fill_dummy(u8 *pfwbuf, u32 *pfwlen)
+{
+	u32 fwlen = *pfwlen;
+	u8 remain = (u8) (fwlen % 4);
+
+	remain = (remain == 0) ? 0 : (4 - remain);
+
+	while (remain > 0) {
+		pfwbuf[fwlen] = 0;
+		fwlen++;
+		remain--;
+	}
+
+	*pfwlen = fwlen;
+}
+
+static void _rtl8821ae_write_fw(struct ieee80211_hw *hw,
+			     					  enum version_8821ae version,
+			     					  u8 *buffer, u32 size)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 *bufferPtr = (u8 *) buffer;
+	u32 pageNums, remainSize;
+	u32 page, offset;
+
+	RT_TRACE(COMP_FW, DBG_LOUD, ("FW size is %d bytes,\n", size));
+
+	_rtl8821ae_fill_dummy(bufferPtr, &size);
+
+	pageNums = size / FW_8821AE_PAGE_SIZE;
+	remainSize = size % FW_8821AE_PAGE_SIZE;
+
+	if (pageNums > 8) {
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("Page numbers should not greater then 8\n"));
+	}
+
+	for (page = 0; page < pageNums; page++) {
+		offset = page * FW_8821AE_PAGE_SIZE;
+		_rtl8821ae_fw_page_write(hw, page, (bufferPtr + offset),
+				      FW_8821AE_PAGE_SIZE);
+	}
+
+	if (remainSize) {
+		offset = pageNums * FW_8821AE_PAGE_SIZE;
+		page = pageNums;
+		_rtl8821ae_fw_page_write(hw, page, (bufferPtr + offset),
+				      remainSize);
+	}
+
+}
+
+static int _rtl8821ae_fw_free_to_go(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int err = -EIO;
+	u32 counter = 0;
+	u32 value32;
+
+	do {
+		value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
+	} while ((counter++ < FW_8821AE_POLLING_TIMEOUT_COUNT) &&
+		 (!(value32 & FWDL_CHKSUM_RPT)));
+
+	if (counter >= FW_8821AE_POLLING_TIMEOUT_COUNT) {
+		RT_TRACE(COMP_ERR, DBG_LOUD,
+			 ("chksum report faill ! REG_MCUFWDL:0x%08x .\n",
+			  value32));
+		goto exit;
+	}
+
+	RT_TRACE(COMP_FW, DBG_EMERG,
+		 ("Checksum report OK ! REG_MCUFWDL:0x%08x .\n", value32));
+
+	value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
+	value32 |= MCUFWDL_RDY;
+	value32 &= ~WINTINI_RDY;
+	rtl_write_dword(rtlpriv, REG_MCUFWDL, value32);
+
+	rtl8821ae_firmware_selfreset(hw);
+
+	counter = 0;
+	do {
+		value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
+		if (value32 & WINTINI_RDY) {
+			RT_TRACE(COMP_FW, DBG_LOUD,
+				 ("Polling FW ready success!! REG_MCUFWDL:0x%08x .\n",
+				  value32));
+			err = 0;
+			goto exit;
+		}
+
+		udelay(FW_8821AE_POLLING_DELAY);
+
+	} while (counter++ < FW_8821AE_POLLING_TIMEOUT_COUNT);
+
+	RT_TRACE(COMP_ERR, DBG_EMERG,
+		 ("Polling FW ready fail!! REG_MCUFWDL:0x%08x .\n", value32));
+
+exit:
+	return err;
+}
+
+int rtl8821ae_download_fw(struct ieee80211_hw *hw,
+	bool buse_wake_on_wlan_fw
+	)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl8821a_firmware_header *pfwheader;
+	u8 *pfwdata;
+	u32 fwsize;
+	int err;
+	enum version_8821ae version = rtlhal->version;
+
+	if(!rtlhal->pfirmware)
+		return 1;
+
+	pfwheader = (struct rtl8821a_firmware_header *)rtlhal->pfirmware;
+	pfwdata = (u8 *) rtlhal->pfirmware;
+	fwsize = rtlhal->fwsize;
+	RT_TRACE(COMP_FW, DBG_DMESG,
+		 ("normal Firmware SIZE %d \n",fwsize));
+
+	if (IS_FW_HEADER_EXIST_8812(pfwheader) || IS_FW_HEADER_EXIST_8821(pfwheader)) {
+		RT_TRACE(COMP_FW, DBG_DMESG,
+			 ("Firmware Version(%d), Signature(%#x),Size(%d)\n",
+			  pfwheader->version, pfwheader->signature,
+			  (int)sizeof(struct rtl8821a_firmware_header)));
+
+		pfwdata = pfwdata + sizeof(struct rtl8821a_firmware_header);
+		fwsize = fwsize - sizeof(struct rtl8821a_firmware_header);
+	}
+
+	if(rtl_read_byte(rtlpriv, REG_MCUFWDL) & BIT(7)){
+		rtl_write_byte(rtlpriv, REG_MCUFWDL, 0x00);
+		rtl8821ae_firmware_selfreset(hw);
+	}
+	_rtl8821ae_enable_fw_download(hw, true);
+	_rtl8821ae_write_fw(hw, version, pfwdata, fwsize);
+	_rtl8821ae_enable_fw_download(hw, false);
+
+	err = _rtl8821ae_fw_free_to_go(hw);
+	if (err) {
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("Firmware is not ready to run!\n"));
+	} else {
+		RT_TRACE(COMP_FW, DBG_LOUD,
+			 ("Firmware is ready to run!\n"));
+	}
+
+	return 0;
+}
+
+static bool _rtl8821ae_check_fw_read_last_h2c(struct ieee80211_hw *hw, u8 boxnum)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 val_hmetfr;
+	bool result = false;
+
+	val_hmetfr = rtl_read_byte(rtlpriv, REG_HMETFR);
+	if (((val_hmetfr >> boxnum) & BIT(0)) == 0)
+		result = true;
+	return result;
+}
+
+static void _rtl8821ae_fill_h2c_command(struct ieee80211_hw *hw,
+			      u8 element_id, u32 cmd_len, u8 *p_cmdbuffer)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 boxnum =0;
+	u16 box_reg = 0, box_extreg = 0;
+	u8 u1b_tmp = 0;
+	bool isfw_read = false;
+	u8 buf_index = 0;
+	bool bwrite_sucess = false;
+	u8 wait_h2c_limmit = 100;
+	/*u8 wait_writeh2c_limmit = 100;*/
+	u8 boxcontent[4], boxextcontent[4];
+	u32 h2c_waitcounter = 0;
+	unsigned long flag =0;
+	u8 idx =0;
+
+	RT_TRACE(COMP_CMD, DBG_LOUD, ("come in\n"));
+
+	while (true) {
+		spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
+		if (rtlhal->b_h2c_setinprogress) {
+			RT_TRACE(COMP_CMD, DBG_LOUD,
+				 ("H2C set in progress! Wait to set.."
+				  "element_id(%d).\n", element_id));
+
+			while (rtlhal->b_h2c_setinprogress) {
+				spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock,
+						       flag);
+				h2c_waitcounter++;
+				RT_TRACE(COMP_CMD, DBG_LOUD,
+					 ("Wait 100 us (%d times)...\n",
+					  h2c_waitcounter));
+				udelay(100);
+
+				if (h2c_waitcounter > 1000)
+					return;
+				spin_lock_irqsave(&rtlpriv->locks.h2c_lock,
+						  flag);
+			}
+			spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
+		} else {
+			rtlhal->b_h2c_setinprogress = true;
+			spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
+			break;
+		}
+	}
+
+	while (!bwrite_sucess) {
+	/*cosa remove this because never reach this.*/
+#if 0
+		wait_writeh2c_limmit--;
+		if (wait_writeh2c_limmit == 0) {
+			RT_TRACE(COMP_ERR, DBG_EMERG,
+				 ("Write H2C fail because no trigger "
+				  "for FW INT!\n"));
+			break;
+		}
+#endif
+
+		boxnum = rtlhal->last_hmeboxnum;
+		switch (boxnum) {
+		case 0:
+			box_reg = REG_HMEBOX_0;
+			box_extreg = REG_HMEBOX_EXT_0;
+			break;
+		case 1:
+			box_reg = REG_HMEBOX_1;
+			box_extreg = REG_HMEBOX_EXT_1;
+			break;
+		case 2:
+			box_reg = REG_HMEBOX_2;
+			box_extreg = REG_HMEBOX_EXT_2;
+			break;
+		case 3:
+			box_reg = REG_HMEBOX_3;
+			box_extreg = REG_HMEBOX_EXT_3;
+			break;
+		default:
+			RT_TRACE(COMP_ERR, DBG_EMERG,
+				 ("switch case not process \n"));
+			break;
+		}
+
+		isfw_read = false;
+		u1b_tmp = rtl_read_byte(rtlpriv, REG_CR);
+
+		if (u1b_tmp != 0xEA)
+			isfw_read = true;
+		else {
+			if( rtl_read_byte(rtlpriv, REG_TXDMA_STATUS) == 0xEA ||
+				rtl_read_byte(rtlpriv, REG_TXPKT_EMPTY) == 0xEA)
+				rtl_write_byte(rtlpriv, REG_SYS_CFG1 + 3, 0xFF);
+		}
+
+		if (isfw_read == true) {
+			wait_h2c_limmit = 100;
+			isfw_read = _rtl8821ae_check_fw_read_last_h2c(hw, boxnum);
+			while (!isfw_read) {
+				/*wait until Fw read*/
+				wait_h2c_limmit--;
+				if (wait_h2c_limmit == 0) {
+					RT_TRACE(COMP_CMD, DBG_LOUD,
+						 ("Wating too long for FW read "
+						  "clear HMEBox(%d)!\n", boxnum));
+					break;
+				}
+
+				udelay(10);
+
+				isfw_read = _rtl8821ae_check_fw_read_last_h2c(hw, boxnum);
+				u1b_tmp = rtl_read_byte(rtlpriv, 0x130);
+				RT_TRACE(COMP_CMD, DBG_LOUD,
+					 ("Wating for FW read clear HMEBox(%d)!!! "
+					  "0x130 = %2x\n", boxnum, u1b_tmp));
+			}
+		}
+
+		if (!isfw_read) {
+			RT_TRACE(COMP_CMD, DBG_LOUD,
+				 ("Write H2C register BOX[%d] fail!!!!! "
+				  "Fw do not read. \n", boxnum));
+			break;
+		}
+
+		memset(boxcontent, 0, sizeof(boxcontent));
+		memset(boxextcontent, 0, sizeof(boxextcontent));
+		boxcontent[0] = element_id;
+		RT_TRACE(COMP_CMD, DBG_LOUD,
+			 ("Write element_id box_reg(%4x) = %2x \n",
+			  box_reg, element_id));
+
+		switch (cmd_len) {
+		case 1:
+		case 2:
+		case 3:
+			/*boxcontent[0] &= ~(BIT(7));*/
+			memcpy((u8 *) (boxcontent) + 1,
+			       p_cmdbuffer + buf_index, cmd_len);
+
+			for (idx = 0; idx < 4; idx++) {
+				rtl_write_byte(rtlpriv, box_reg + idx,
+					       boxcontent[idx]);
+			}
+			break;
+		case 4:
+		case 5:
+		case 6:
+		case 7:
+			/*boxcontent[0] |= (BIT(7));*/
+			memcpy((u8 *) (boxextcontent),
+			       p_cmdbuffer + buf_index+3, cmd_len-3);
+			memcpy((u8 *) (boxcontent) + 1,
+			       p_cmdbuffer + buf_index, 3);
+
+			for (idx = 0; idx < 4; idx++) {
+				rtl_write_byte(rtlpriv, box_extreg + idx,
+					       boxextcontent[idx]);
+			}
+
+			for (idx = 0; idx < 4; idx++) {
+				rtl_write_byte(rtlpriv, box_reg + idx,
+					       boxcontent[idx]);
+			}
+			break;
+		default:
+			RT_TRACE(COMP_ERR, DBG_EMERG,
+				 ("switch case not process \n"));
+			break;
+		}
+
+		bwrite_sucess = true;
+
+		rtlhal->last_hmeboxnum = boxnum + 1;
+		if (rtlhal->last_hmeboxnum == 4)
+			rtlhal->last_hmeboxnum = 0;
+
+		RT_TRACE(COMP_CMD, DBG_LOUD,
+			 ("pHalData->last_hmeboxnum  = %d\n",
+			  rtlhal->last_hmeboxnum));
+	}
+
+	spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
+	rtlhal->b_h2c_setinprogress = false;
+	spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
+
+	RT_TRACE(COMP_CMD, DBG_LOUD, ("go out\n"));
+}
+
+void rtl8821ae_fill_h2c_cmd(struct ieee80211_hw *hw,
+			 u8 element_id, u32 cmd_len, u8 *p_cmdbuffer)
+{
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u32 tmp_cmdbuf[2];
+
+	if (rtlhal->bfw_ready == false) {
+		RT_ASSERT(false, ("return H2C cmd because of Fw "
+				  "download fail!!!\n"));
+		return;
+	}
+
+	memset(tmp_cmdbuf, 0, 8);
+	memcpy(tmp_cmdbuf, p_cmdbuffer, cmd_len);
+	_rtl8821ae_fill_h2c_command(hw, element_id, cmd_len, (u8 *) & tmp_cmdbuf);
+
+	return;
+}
+
+void rtl8821ae_firmware_selfreset(struct ieee80211_hw *hw)
+{
+	u8 u1b_tmp;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	if(rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+	{
+		u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL+1);
+		rtl_write_byte(rtlpriv, REG_RSV_CTRL+1, (u1b_tmp & (~BIT(3))));
+	}else {
+		u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL+1);
+		rtl_write_byte(rtlpriv, REG_RSV_CTRL+1, (u1b_tmp & (~BIT(0))));
+	}
+
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN+1);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN+1, (u1b_tmp & (~BIT(2))));
+	udelay(50);
+
+	if(rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+	{
+		u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL+1);
+		rtl_write_byte(rtlpriv, REG_RSV_CTRL+1, (u1b_tmp | BIT(3)));
+	}else {
+		u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL+1);
+		rtl_write_byte(rtlpriv, REG_RSV_CTRL+1, (u1b_tmp | BIT(0)));
+	}
+
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN+1);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN+1, (u1b_tmp | BIT(2)));
+
+	RT_TRACE(COMP_INIT, DBG_LOUD, ("  _8051Reset8812ae(): 8051 reset success .\n"));
+
+}
+
+void rtl8821ae_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 u1_h2c_set_pwrmode[H2C_8821AE_PWEMODE_LENGTH] = { 0 };
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	u8 rlbm,power_state = 0;
+	RT_TRACE(COMP_POWER, DBG_LOUD, ("FW LPS mode = %d\n", mode));
+
+	SET_H2CCMD_PWRMODE_PARM_MODE(u1_h2c_set_pwrmode, ((mode) ? 1 : 0));
+	rlbm = 0;/*YJ,temp,120316. FW now not support RLBM=2.*/
+	SET_H2CCMD_PWRMODE_PARM_RLBM(u1_h2c_set_pwrmode, rlbm);
+	SET_H2CCMD_PWRMODE_PARM_SMART_PS(u1_h2c_set_pwrmode, (rtlpriv->mac80211.p2p) ? ppsc->smart_ps : 1);
+	SET_H2CCMD_PWRMODE_PARM_AWAKE_INTERVAL(u1_h2c_set_pwrmode, ppsc->reg_max_lps_awakeintvl);
+	SET_H2CCMD_PWRMODE_PARM_ALL_QUEUE_UAPSD(u1_h2c_set_pwrmode, 0);
+	if(mode == FW_PS_ACTIVE_MODE)
+	{
+		power_state |= FW_PWR_STATE_ACTIVE;
+	}
+	else
+	{
+		power_state |= FW_PWR_STATE_RF_OFF;
+	}
+	SET_H2CCMD_PWRMODE_PARM_PWR_STATE(u1_h2c_set_pwrmode, power_state);
+
+	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
+		      "rtl92c_set_fw_pwrmode(): u1_h2c_set_pwrmode \n",
+		      u1_h2c_set_pwrmode, H2C_8821AE_PWEMODE_LENGTH);
+	rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_SETPWRMODE, H2C_8821AE_PWEMODE_LENGTH, u1_h2c_set_pwrmode);
+
+}
+
+void rtl8821ae_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus)
+{
+	u8 u1_joinbssrpt_parm[1] = { 0 };
+
+	SET_H2CCMD_JOINBSSRPT_PARM_OPMODE(u1_joinbssrpt_parm, mstatus);
+
+	rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_JOINBSSRPT, 1, u1_joinbssrpt_parm);
+}
+
+void rtl8821ae_set_fw_ap_off_load_cmd(struct ieee80211_hw *hw,  u8 ap_offload_enable)
+{
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u8 u1_apoffload_parm[H2C_8821AE_AP_OFFLOAD_LENGTH] = { 0 };
+
+	SET_H2CCMD_AP_OFFLOAD_ON(u1_apoffload_parm, ap_offload_enable);
+	SET_H2CCMD_AP_OFFLOAD_HIDDEN(u1_apoffload_parm, mac->bhiddenssid);
+	SET_H2CCMD_AP_OFFLOAD_DENYANY(u1_apoffload_parm, 0);
+
+	rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_AP_OFFLOAD, H2C_8821AE_AP_OFFLOAD_LENGTH, u1_apoffload_parm);
+
+}
+
+static bool _rtl8821ae_cmd_send_packet(struct ieee80211_hw *hw,
+				struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl8192_tx_ring *ring;
+	struct rtl_tx_desc *pdesc;
+	u8 own;
+	unsigned long flags;
+	struct sk_buff *pskb = NULL;
+
+	ring = &rtlpci->tx_ring[BEACON_QUEUE];
+
+	pskb = __skb_dequeue(&ring->queue);
+	if (pskb)
+		kfree_skb(pskb);
+
+	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
+
+	pdesc = &ring->desc[0];
+	own = (u8) rtlpriv->cfg->ops->get_desc((u8 *) pdesc, true, HW_DESC_OWN);
+
+	rtlpriv->cfg->ops->fill_tx_cmddesc(hw, (u8 *) pdesc, 1, 1, skb);
+
+	__skb_queue_tail(&ring->queue, skb);
+
+	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
+
+	rtlpriv->cfg->ops->tx_polling(hw, BEACON_QUEUE);
+
+	return true;
+}
+
+#define BEACON_PG		0 /* ->1 */
+#define PSPOLL_PG		2
+#define NULL_PG			3
+#define PROBERSP_PG		4 /* ->5 */
+
+#define BEACON_PG_8812		0
+#define PSPOLL_PG_8812		1
+#define NULL_PG_8812			2
+#define PROBERSP_PG_8812		3
+
+#define BEACON_PG_8821		0
+#define PSPOLL_PG_8821		1
+#define NULL_PG_8821			2
+#define PROBERSP_PG_8821		3
+
+#define TOTAL_RESERVED_PKT_LEN_8812	2048
+#define TOTAL_RESERVED_PKT_LEN_8821	1024
+
+
+static u8 reserved_page_packet_8821[TOTAL_RESERVED_PKT_LEN_8821] = {
+	/* page 0 */
+	0x80, 0x00, 0x00, 0x00,  0xff, 0xff, 0xff, 0xff,
+	0xff, 0xff, 0x00, 0xe0,  0x4c, 0x02, 0xe2, 0x64,
+	0x40, 0x16, 0x9f, 0x23,  0xd4, 0x46, 0x20, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x64, 0x00, 0x20, 0x04,  0x00, 0x06, 0x64, 0x6c,
+	0x69, 0x6e, 0x6b, 0x31,  0x01, 0x08, 0x82, 0x84,
+	0x8b, 0x96, 0x0c, 0x18,  0x30, 0x48, 0x03, 0x01,
+	0x0b, 0x06, 0x02, 0x00,  0x00, 0x2a, 0x01, 0x8b,
+	0x32, 0x04, 0x12, 0x24,  0x60, 0x6c, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x10, 0x00, 0x28, 0x8c,  0x00, 0x12, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x81, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	/* page 1 */
+	0xa4, 0x10, 0x01, 0xc0,  0x40, 0x16, 0x9f, 0x23,
+	0xd4, 0x46, 0x00, 0xe0,  0x4c, 0x02, 0xe2, 0x64,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x18, 0x00, 0x28, 0x8c,  0x00, 0x12, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x01, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x80, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	/* page 2 */
+	0x48, 0x01, 0x00, 0x00,  0x40, 0x16, 0x9f, 0x23,
+	0xd4, 0x46, 0x00, 0xe0,  0x4c, 0x02, 0xe2, 0x64,
+	0x40, 0x16, 0x9f, 0x23,  0xd4, 0x46, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x1a, 0x00, 0x28, 0x8c,  0x00, 0x12, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x01, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x80, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	/* page 3 */
+	0xc8, 0x01, 0x00, 0x00,  0x40, 0x16, 0x9f, 0x23,
+	0xd4, 0x46, 0x00, 0xe0,  0x4c, 0x02, 0xe2, 0x64,
+	0x40, 0x16, 0x9f, 0x23,  0xd4, 0x46, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+};
+
+
+static u8 reserved_page_packet_8812[TOTAL_RESERVED_PKT_LEN_8812] = {
+ 	0x80, 0x00, 0x00, 0x00,  0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0x00, 0xE0,  0x4C, 0x02, 0x53, 0xE5,
+	0xE0, 0x46, 0x9A, 0x57,  0x71, 0x30, 0x20, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x64, 0x00, 0x30, 0x04,  0x00, 0x0C, 0x4E, 0x45,
+	0x54, 0x47, 0x45, 0x41,  0x52, 0x5F, 0x31, 0x35,
+	0x30, 0x4E, 0x01, 0x08,  0x82, 0x84, 0x8B, 0x96,
+	0x0C, 0x12, 0x18, 0x24,  0x03, 0x01, 0x03, 0x06,
+	0x02, 0x00, 0x00, 0x2A,  0x01, 0x8A, 0x32, 0x04,
+	0x30, 0x48, 0x60, 0x6C,  0xDD, 0x18, 0x00, 0x50,
+	0xF2, 0x01, 0x01, 0x00,  0x00, 0x50, 0xF2, 0x02,
+	0x01, 0x00, 0x00, 0x50,  0xF2, 0x02, 0x01, 0x00,
+	0x00, 0x50, 0xF2, 0x02,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x10, 0x00, 0x28, 0x8C,  0x00, 0x12, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x81, 0x00, 0x00,
+	0x04, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+
+	0xA4, 0x10, 0x02, 0xC0,  0xE0, 0x46, 0x9A, 0x57,
+	0x71, 0x30, 0x00, 0xE0,  0x4C, 0x02, 0x53, 0xE5,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x18, 0x00, 0x28, 0x8C,  0x00, 0x12, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x01, 0x00, 0x00,
+	0x04, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x80, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+
+	0x48, 0x01, 0x00, 0x00,  0xE0, 0x46, 0x9A, 0x57,
+	0x71, 0x30, 0x00, 0xE0,  0x4C, 0x02, 0x53, 0xE5,
+	0xE0, 0x46, 0x9A, 0x57,  0x71, 0x30, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x1A, 0x00, 0x28, 0x8C,  0x00, 0x12, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x01, 0x00, 0x00,
+	0x04, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x80, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+
+	0xC8, 0x01, 0x00, 0x00,  0xE0, 0x46, 0x9A, 0x57,
+	0x71, 0x30, 0x00, 0xE0,  0x4C, 0x02, 0x53, 0xE5,
+	0xE0, 0x46, 0x9A, 0x57,  0x71, 0x30, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+};
+
+void rtl8812ae_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct sk_buff *skb = NULL;
+
+	u32 totalpacketlen;
+	bool rtstatus;
+	u8 u1RsvdPageLoc[5] = { 0 };
+	bool b_dlok = false;
+
+	u8* beacon;
+	u8* p_pspoll;
+	u8* nullfunc;
+	u8* p_probersp;
+	/*---------------------------------------------------------
+				(1) beacon
+	---------------------------------------------------------*/
+	beacon = &reserved_page_packet_8812[BEACON_PG_8812 * 512];
+	SET_80211_HDR_ADDRESS2(beacon, mac->mac_addr);
+	SET_80211_HDR_ADDRESS3(beacon, mac->bssid);
+
+	/*-------------------------------------------------------
+				(2) ps-poll
+	--------------------------------------------------------*/
+	p_pspoll = &reserved_page_packet_8812[PSPOLL_PG_8812 * 512];
+	SET_80211_PS_POLL_AID(p_pspoll, (mac->assoc_id | 0xc000));
+	SET_80211_PS_POLL_BSSID(p_pspoll, mac->bssid);
+	SET_80211_PS_POLL_TA(p_pspoll, mac->mac_addr);
+
+	SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(u1RsvdPageLoc, PSPOLL_PG_8812);
+
+	/*--------------------------------------------------------
+				(3) null data
+	---------------------------------------------------------*/
+	nullfunc = &reserved_page_packet_8812[NULL_PG_8812* 512];
+	SET_80211_HDR_ADDRESS1(nullfunc, mac->bssid);
+	SET_80211_HDR_ADDRESS2(nullfunc, mac->mac_addr);
+	SET_80211_HDR_ADDRESS3(nullfunc, mac->bssid);
+
+	SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(u1RsvdPageLoc, NULL_PG_8812);
+
+	/*---------------------------------------------------------
+				(4) probe response
+	----------------------------------------------------------*/
+	p_probersp = &reserved_page_packet_8812[PROBERSP_PG_8812 * 512];
+	SET_80211_HDR_ADDRESS1(p_probersp, mac->bssid);
+	SET_80211_HDR_ADDRESS2(p_probersp, mac->mac_addr);
+	SET_80211_HDR_ADDRESS3(p_probersp, mac->bssid);
+
+	SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(u1RsvdPageLoc, PROBERSP_PG_8812);
+
+	totalpacketlen = TOTAL_RESERVED_PKT_LEN_8812;
+
+	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD,
+		      "rtl8821ae_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL \n",
+		      &reserved_page_packet_8812[0], totalpacketlen);
+	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
+		      "rtl8821ae_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL \n",
+		      u1RsvdPageLoc, 3);
+
+
+	skb = dev_alloc_skb(totalpacketlen);
+	memcpy((u8 *) skb_put(skb, totalpacketlen),
+	       &reserved_page_packet_8812, totalpacketlen);
+
+	rtstatus = _rtl8821ae_cmd_send_packet(hw, skb);
+
+	if (rtstatus)
+		b_dlok = true;
+
+	if (b_dlok) {
+		RT_TRACE(COMP_POWER, DBG_LOUD,
+			 ("Set RSVD page location to Fw.\n"));
+		RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
+				"H2C_RSVDPAGE:\n",
+				u1RsvdPageLoc, 3);
+		rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_RSVDPAGE,
+				    sizeof(u1RsvdPageLoc), u1RsvdPageLoc);
+	} else
+		RT_TRACE(COMP_ERR, DBG_WARNING,
+			 ("Set RSVD page location to Fw FAIL!!!!!!.\n"));
+}
+
+void rtl8821ae_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct sk_buff *skb = NULL;
+
+	u32 totalpacketlen;
+	bool rtstatus;
+	u8 u1RsvdPageLoc[5] = { 0 };
+	bool b_dlok = false;
+
+	u8* beacon;
+	u8* p_pspoll;
+	u8* nullfunc;
+	u8* p_probersp;
+	/*---------------------------------------------------------
+				(1) beacon
+	---------------------------------------------------------*/
+	beacon = &reserved_page_packet_8821[BEACON_PG_8821 * 256];
+	SET_80211_HDR_ADDRESS2(beacon, mac->mac_addr);
+	SET_80211_HDR_ADDRESS3(beacon, mac->bssid);
+
+	/*-------------------------------------------------------
+				(2) ps-poll
+	--------------------------------------------------------*/
+	p_pspoll = &reserved_page_packet_8821[PSPOLL_PG_8821 * 256];
+	SET_80211_PS_POLL_AID(p_pspoll, (mac->assoc_id | 0xc000));
+	SET_80211_PS_POLL_BSSID(p_pspoll, mac->bssid);
+	SET_80211_PS_POLL_TA(p_pspoll, mac->mac_addr);
+
+	SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(u1RsvdPageLoc, PSPOLL_PG_8821);
+
+	/*--------------------------------------------------------
+				(3) null data
+	---------------------------------------------------------*/
+	nullfunc = &reserved_page_packet_8821[NULL_PG_8821 * 256];
+	SET_80211_HDR_ADDRESS1(nullfunc, mac->bssid);
+	SET_80211_HDR_ADDRESS2(nullfunc, mac->mac_addr);
+	SET_80211_HDR_ADDRESS3(nullfunc, mac->bssid);
+
+	SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(u1RsvdPageLoc, NULL_PG_8821);
+
+	/*---------------------------------------------------------
+				(4) probe response
+	----------------------------------------------------------*/
+	p_probersp = &reserved_page_packet_8821[PROBERSP_PG_8821 * 256];
+	SET_80211_HDR_ADDRESS1(p_probersp, mac->bssid);
+	SET_80211_HDR_ADDRESS2(p_probersp, mac->mac_addr);
+	SET_80211_HDR_ADDRESS3(p_probersp, mac->bssid);
+
+	SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(u1RsvdPageLoc, PROBERSP_PG_8821);
+
+	totalpacketlen = TOTAL_RESERVED_PKT_LEN_8821;
+
+	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD,
+		      "rtl8821ae_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL \n",
+		      &reserved_page_packet_8821[0], totalpacketlen);
+	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
+		      "rtl8821ae_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL \n",
+		      u1RsvdPageLoc, 3);
+
+
+	skb = dev_alloc_skb(totalpacketlen);
+	memcpy((u8 *) skb_put(skb, totalpacketlen),
+	       &reserved_page_packet_8821, totalpacketlen);
+
+	rtstatus = _rtl8821ae_cmd_send_packet(hw, skb);
+
+	if (rtstatus)
+		b_dlok = true;
+
+	if (b_dlok) {
+		RT_TRACE(COMP_POWER, DBG_LOUD,
+			 ("Set RSVD page location to Fw.\n"));
+		RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
+				"H2C_RSVDPAGE:\n",
+				u1RsvdPageLoc, 3);
+		rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_RSVDPAGE,
+				    sizeof(u1RsvdPageLoc), u1RsvdPageLoc);
+	} else
+		RT_TRACE(COMP_ERR, DBG_WARNING,
+			 ("Set RSVD page location to Fw FAIL!!!!!!.\n"));
+}
+
+/*Shoud check FW support p2p or not.*/
+void rtl8821ae_set_p2p_ctw_period_cmd(struct ieee80211_hw *hw, u8 ctwindow)
+{
+	u8 u1_ctwindow_period[1] ={ ctwindow};
+
+	rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_P2P_PS_CTW_CMD, 1, u1_ctwindow_period);
+
+}
+
+void rtl8821ae_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *rtlps = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_p2p_ps_info *p2pinfo = &(rtlps->p2p_ps_info);
+	struct p2p_ps_offload_t *p2p_ps_offload = &rtlhal->p2p_ps_offload;
+	u8	i;
+	u16	ctwindow;
+	u32	start_time, tsf_low;
+
+	switch(p2p_ps_state)
+	{
+		case P2P_PS_DISABLE:
+			RT_TRACE(COMP_FW, DBG_LOUD,("P2P_PS_DISABLE \n"));
+			memset(p2p_ps_offload, 0, 1);
+			break;
+		case P2P_PS_ENABLE:
+			RT_TRACE(COMP_FW, DBG_LOUD,("P2P_PS_ENABLE \n"));
+			/* update CTWindow value. */
+			if( p2pinfo->ctwindow > 0 )
+			{
+				p2p_ps_offload->CTWindow_En = 1;
+				ctwindow = p2pinfo->ctwindow;
+				rtl8821ae_set_p2p_ctw_period_cmd(hw, ctwindow);
+			}
+
+			/* hw only support 2 set of NoA */
+			for( i=0 ; i<p2pinfo->noa_num ; i++)
+			{
+				/* To control the register setting for which NOA*/
+				rtl_write_byte(rtlpriv, 0x5cf, (i << 4));
+				if(i == 0)
+					p2p_ps_offload->NoA0_En = 1;
+				else
+					p2p_ps_offload->NoA1_En = 1;
+
+				/* config P2P NoA Descriptor Register */
+				rtl_write_dword(rtlpriv, 0x5E0, p2pinfo->noa_duration[i]);
+				rtl_write_dword(rtlpriv, 0x5E4, p2pinfo->noa_interval[i]);
+
+				/*Get Current TSF value */
+				tsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
+
+				start_time = p2pinfo->noa_start_time[i];
+				if(p2pinfo->noa_count_type[i] != 1)
+				{
+					while( start_time <= (tsf_low+(50*1024) ) ) {
+						start_time += p2pinfo->noa_interval[i];
+						if(p2pinfo->noa_count_type[i] != 255)
+							p2pinfo->noa_count_type[i]--;
+					}
+				}
+				rtl_write_dword(rtlpriv, 0x5E8, start_time);
+				rtl_write_dword(rtlpriv, 0x5EC, p2pinfo->noa_count_type[i] );
+
+			}
+
+			if( (p2pinfo->opp_ps == 1) || (p2pinfo->noa_num > 0) )
+			{
+				/* rst p2p circuit */
+				rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, BIT(4));
+
+				p2p_ps_offload->Offload_En = 1;
+
+				if(P2P_ROLE_GO == rtlpriv->mac80211.p2p)
+				{
+					p2p_ps_offload->role= 1;
+					p2p_ps_offload->AllStaSleep = 0;
+				}
+				else
+				{
+					p2p_ps_offload->role= 0;
+				}
+
+				p2p_ps_offload->discovery = 0;
+			}
+			break;
+		case P2P_PS_SCAN:
+			RT_TRACE(COMP_FW, DBG_LOUD,("P2P_PS_SCAN \n"));
+			p2p_ps_offload->discovery = 1;
+			break;
+		case P2P_PS_SCAN_DONE:
+			RT_TRACE(COMP_FW, DBG_LOUD,("P2P_PS_SCAN_DONE \n"));
+			p2p_ps_offload->discovery = 0;
+			p2pinfo->p2p_ps_state = P2P_PS_ENABLE;
+			break;
+		default:
+			break;
+	}
+
+	rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_P2P_PS_OFFLOAD, 1, (u8 *)p2p_ps_offload);
+
+}
+
+void rtl8812ae_c2h_ra_report_handler(
+	struct ieee80211_hw *hw,
+	u8 *cmd_buf,
+	u8 cmd_len
+)
+{
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 rate = cmd_buf[0] & 0x3F;
+
+	rtlhal->current_ra_rate= rtl8812ae_hw_rate_to_mrate(hw, rate);
+
+	rtl8812ae_dm_update_init_rate(hw, rate);
+}
+
+
+void _rtl8812ae_c2h_content_parsing(
+	struct ieee80211_hw *hw,
+	u8 c2h_cmd_id,
+	u8 c2h_cmd_len,
+	u8 *tmp_buf
+)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	switch (c2h_cmd_id) {
+	case C2H_8812_DBG:
+		RT_TRACE(COMP_FW, DBG_LOUD,("[C2H], C2H_8812_DBG!!\n"));
+		break;
+
+	case C2H_8812_RA_RPT:
+		rtl8812ae_c2h_ra_report_handler(hw, tmp_buf, c2h_cmd_len);
+		break;
+
+	default:
+		break;
+	}
+
+}
+
+void rtl8812ae_c2h_packet_handler(
+	struct ieee80211_hw *hw,
+	u8 *buffer,
+	u8 length
+	)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 c2h_cmd_id=0, c2h_cmd_seq=0, c2h_cmd_len=0;
+	u8 *tmp_buf=NULL;
+
+	c2h_cmd_id = buffer[0];
+	c2h_cmd_seq = buffer[1];
+	c2h_cmd_len = length -2;
+	tmp_buf = buffer + 2;
+
+	RT_TRACE(COMP_FW, DBG_LOUD,
+		("[C2H packet], c2hCmdId=0x%x, c2hCmdSeq=0x%x, c2hCmdLen=%d\n",
+		c2h_cmd_id, c2h_cmd_seq, c2h_cmd_len));
+
+	RT_PRINT_DATA(rtlpriv, COMP_FW, DBG_LOUD,
+		"[C2H packet], Content Hex:\n", tmp_buf, c2h_cmd_len);
+	_rtl8812ae_c2h_content_parsing(hw, c2h_cmd_id, c2h_cmd_len, tmp_buf);
+}
+
+
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/fw.h b/drivers/staging/rtl8821ae/rtl8821ae/fw.h
new file mode 100644
index 000000000000..30eec880026c
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/fw.h
@@ -0,0 +1,321 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL8821AE__FW__H__
+#define __RTL8821AE__FW__H__
+
+#define FW_8821AE_SIZE					0x8000
+#define FW_8821AE_START_ADDRESS			0x1000
+#define FW_8821AE_END_ADDRESS			0x5FFF
+#define FW_8821AE_PAGE_SIZE				4096
+#define FW_8821AE_POLLING_DELAY			5
+#define FW_8821AE_POLLING_TIMEOUT_COUNT	6000
+
+#define IS_FW_HEADER_EXIST_8812(_pfwhdr)	\
+	((_pfwhdr->signature&0xFFF0) == 0x9500 )
+
+#define IS_FW_HEADER_EXIST_8821(_pfwhdr)	\
+	((_pfwhdr->signature&0xFFF0) == 0x2100 )
+
+#define USE_OLD_WOWLAN_DEBUG_FW 0
+
+#define H2C_8821AE_RSVDPAGE_LOC_LEN		5
+#define H2C_8821AE_PWEMODE_LENGTH			5
+#define H2C_8821AE_JOINBSSRPT_LENGTH		1
+#define H2C_8821AE_AP_OFFLOAD_LENGTH		3
+#define H2C_8821AE_WOWLAN_LENGTH			3
+#define H2C_8821AE_KEEP_ALIVE_CTRL_LENGTH	3
+#if(USE_OLD_WOWLAN_DEBUG_FW == 0)
+#define H2C_8821AE_REMOTE_WAKE_CTRL_LEN	1
+#else
+#define H2C_8821AE_REMOTE_WAKE_CTRL_LEN	3
+#endif
+#define H2C_8821AE_AOAC_GLOBAL_INFO_LEN	2
+#define H2C_8821AE_AOAC_RSVDPAGE_LOC_LEN	7
+
+
+/* Fw PS state for RPWM.
+*BIT[2:0] = HW state
+*BIT[3] = Protocol PS state,   1: register active state , 0: register sleep state
+*BIT[4] = sub-state
+*/
+#define	FW_PS_GO_ON			BIT(0)
+#define	FW_PS_TX_NULL			BIT(1)
+#define	FW_PS_RF_ON			BIT(2)
+#define	FW_PS_REGISTER_ACTIVE	BIT(3)
+
+#define	FW_PS_DPS    		BIT(0)
+#define	FW_PS_LCLK   		(FW_PS_DPS)
+#define	FW_PS_RF_OFF   		BIT(1)
+#define	FW_PS_ALL_ON   		BIT(2)
+#define	FW_PS_ST_ACTIVE  	BIT(3)
+#define	FW_PS_ISR_ENABLE    	BIT(4)
+#define	FW_PS_IMR_ENABLE	BIT(5)
+
+
+#define	FW_PS_ACK    		BIT(6)
+#define	FW_PS_TOGGLE   		BIT(7)
+
+ /* 8821AE RPWM value*/
+ /* BIT[0] = 1: 32k, 0: 40M*/
+#define	FW_PS_CLOCK_OFF		BIT(0)		/* 32k*/
+#define	FW_PS_CLOCK_ON		0		/*40M*/
+
+#define	FW_PS_STATE_MASK  		(0x0F)
+#define	FW_PS_STATE_HW_MASK 	(0x07)
+#define	FW_PS_STATE_INT_MASK 	(0x3F)	/*ISR_ENABLE, IMR_ENABLE, and PS mode should be inherited.*/
+
+#define	FW_PS_STATE(x)   		(FW_PS_STATE_MASK & (x))
+#define	FW_PS_STATE_HW(x) 		(FW_PS_STATE_HW_MASK & (x))
+#define	FW_PS_STATE_INT(x)   	(FW_PS_STATE_INT_MASK & (x))
+#define	FW_PS_ISR_VAL(x)		((x) & 0x70)
+#define	FW_PS_IMR_MASK(x)   	((x) & 0xDF)
+#define	FW_PS_KEEP_IMR(x)		((x) & 0x20)
+
+
+#define	FW_PS_STATE_S0		(FW_PS_DPS)
+#define	FW_PS_STATE_S1		(FW_PS_LCLK)
+#define	FW_PS_STATE_S2		(FW_PS_RF_OFF)
+#define	FW_PS_STATE_S3		(FW_PS_ALL_ON)
+#define	FW_PS_STATE_S4		((FW_PS_ST_ACTIVE) | (FW_PS_ALL_ON))
+
+#define	FW_PS_STATE_ALL_ON_8821AE	(FW_PS_CLOCK_ON) /* ((FW_PS_RF_ON) | (FW_PS_REGISTER_ACTIVE))*/
+#define	FW_PS_STATE_RF_ON_8821AE	(FW_PS_CLOCK_ON) /* (FW_PS_RF_ON)*/
+#define	FW_PS_STATE_RF_OFF_8821AE	(FW_PS_CLOCK_ON) /* 0x0*/
+#define	FW_PS_STATE_RF_OFF_LOW_PWR_8821AE	(FW_PS_CLOCK_OFF) /* (FW_PS_STATE_RF_OFF)*/
+
+#define	FW_PS_STATE_ALL_ON_92C	(FW_PS_STATE_S4)
+#define	FW_PS_STATE_RF_ON_92C		(FW_PS_STATE_S3)
+#define	FW_PS_STATE_RF_OFF_92C	(FW_PS_STATE_S2)
+#define	FW_PS_STATE_RF_OFF_LOW_PWR_92C	(FW_PS_STATE_S1)
+
+
+/* For 8821AE H2C PwrMode Cmd ID 5.*/
+#define	FW_PWR_STATE_ACTIVE	((FW_PS_RF_ON) | (FW_PS_REGISTER_ACTIVE))
+#define	FW_PWR_STATE_RF_OFF	0
+
+#define	FW_PS_IS_ACK(x)  		((x) & FW_PS_ACK )
+#define	FW_PS_IS_CLK_ON(x) 		((x) & (FW_PS_RF_OFF |FW_PS_ALL_ON ))
+#define	FW_PS_IS_RF_ON(x)  		((x) & (FW_PS_ALL_ON))
+#define	FW_PS_IS_ACTIVE(x)  		((x) & (FW_PS_ST_ACTIVE))
+#define	FW_PS_IS_CPWM_INT(x)	((x) & 0x40)
+
+#define	FW_CLR_PS_STATE(x) 		((x) = ((x) & (0xF0)))
+
+#define	IS_IN_LOW_POWER_STATE_8821AE(FwPSState)		\
+			(FW_PS_STATE(FwPSState) == FW_PS_CLOCK_OFF)
+
+#define	FW_PWR_STATE_ACTIVE	((FW_PS_RF_ON) | (FW_PS_REGISTER_ACTIVE))
+#define	FW_PWR_STATE_RF_OFF	0
+
+struct rtl8821a_firmware_header {
+	u16 signature;
+	u8 category;
+	u8 function;
+	u16 version;
+	u8 subversion;
+	u8 rsvd1;
+	u8 month;
+	u8 date;
+	u8 hour;
+	u8 minute;
+	u16 ramcodeSize;
+	u16 rsvd2;
+	u32 svnindex;
+	u32 rsvd3;
+	u32 rsvd4;
+	u32 rsvd5;
+};
+
+enum rtl8812_c2h_evt{
+	C2H_8812_DBG = 0,
+	C2H_8812_LB = 1,
+	C2H_8812_TXBF = 2,
+	C2H_8812_TX_REPORT = 3,
+	C2H_8812_BT_INFO = 9,
+	C2H_8812_BT_MP = 11,
+	C2H_8812_RA_RPT=12,
+
+	C2H_8812_FW_SWCHNL = 0x10,
+	C2H_8812_IQK_FINISH = 0x11,
+	MAX_8812_C2HEVENT
+};
+
+enum rtl8821a_h2c_cmd {
+	H2C_8821AE_RSVDPAGE = 0,
+	H2C_8821AE_JOINBSSRPT = 1,
+	H2C_8821AE_SCAN = 2,
+	H2C_8821AE_KEEP_ALIVE_CTRL = 3,
+	H2C_8821AE_DISCONNECT_DECISION = 4,
+#if(USE_OLD_WOWLAN_DEBUG_FW == 1)
+	H2C_8821AE_WO_WLAN = 5,
+#endif
+	H2C_8821AE_INIT_OFFLOAD = 6,
+#if(USE_OLD_WOWLAN_DEBUG_FW == 1)
+	H2C_8821AE_REMOTE_WAKE_CTRL = 7,
+#endif
+	H2C_8821AE_AP_OFFLOAD = 8,
+	H2C_8821AE_BCN_RSVDPAGE = 9,
+	H2C_8821AE_PROBERSP_RSVDPAGE = 10,
+
+	H2C_8821AE_SETPWRMODE = 0x20,
+	H2C_8821AE_PS_TUNING_PARA = 0x21,
+	H2C_8821AE_PS_TUNING_PARA2 = 0x22,
+	H2C_8821AE_PS_LPS_PARA = 0x23,
+	H2C_8821AE_P2P_PS_OFFLOAD = 024,
+
+#if(USE_OLD_WOWLAN_DEBUG_FW == 0)
+	H2C_8821AE_WO_WLAN = 0x80,
+	H2C_8821AE_REMOTE_WAKE_CTRL = 0x81,
+	H2C_8821AE_AOAC_GLOBAL_INFO = 0x82,
+	H2C_8821AE_AOAC_RSVDPAGE = 0x83,
+#endif
+	H2C_RSSI_REPORT = 0x42,
+	H2C_8821AE_RA_MASK = 0x40,
+	H2C_8821AE_SELECTIVE_SUSPEND_ROF_CMD,
+	H2C_8821AE_P2P_PS_MODE,
+	H2C_8821AE_PSD_RESULT,
+	/*Not defined CTW CMD for P2P yet*/
+	H2C_8821AE_P2P_PS_CTW_CMD,
+	MAX_8821AE_H2CCMD
+};
+
+#define pagenum_128(_len)		(u32)(((_len)>>7) + ((_len)&0x7F ? 1:0))
+
+#define SET_8821AE_H2CCMD_WOWLAN_FUNC_ENABLE(__pH2CCmd, __Value)		\
+	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 0, 1, __Value)
+#define SET_8821AE_H2CCMD_WOWLAN_PATTERN_MATCH_ENABLE(__pH2CCmd, __Value)		\
+	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 1, 1, __Value)
+#define SET_8821AE_H2CCMD_WOWLAN_MAGIC_PKT_ENABLE(__pH2CCmd, __Value)		\
+	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 2, 1, __Value)
+#define SET_8821AE_H2CCMD_WOWLAN_UNICAST_PKT_ENABLE(__pH2CCmd, __Value)		\
+	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 3, 1, __Value)
+#define SET_8821AE_H2CCMD_WOWLAN_ALL_PKT_DROP(__pH2CCmd, __Value)		\
+	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 4, 1, __Value)
+#define SET_8821AE_H2CCMD_WOWLAN_GPIO_ACTIVE(__pH2CCmd, __Value)				\
+	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 5, 1, __Value)
+#define SET_8821AE_H2CCMD_WOWLAN_REKEY_WAKE_UP(__pH2CCmd, __Value)		\
+	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 6, 1, __Value)
+#define SET_8821AE_H2CCMD_WOWLAN_DISCONNECT_WAKE_UP(__pH2CCmd, __Value)		\
+	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 7, 1, __Value)
+#define SET_8821AE_H2CCMD_WOWLAN_GPIONUM(__pH2CCmd, __Value)					\
+	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+1, 0, 8, __Value)
+#define SET_8821AE_H2CCMD_WOWLAN_GPIO_DURATION(__pH2CCmd, __Value)			\
+	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+2, 0, 8, __Value)
+
+
+#define SET_H2CCMD_PWRMODE_PARM_MODE(__ph2ccmd, __val)			\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 8, __val)
+#define SET_H2CCMD_PWRMODE_PARM_RLBM(__pH2CCmd, __Value)		\
+	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+1, 0, 4, __Value)
+#define SET_H2CCMD_PWRMODE_PARM_SMART_PS(__pH2CCmd, __Value)	\
+	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+1, 4, 4, __Value)
+#define SET_H2CCMD_PWRMODE_PARM_AWAKE_INTERVAL(__pH2CCmd, __Value)	\
+	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+2, 0, 8, __Value)
+#define SET_H2CCMD_PWRMODE_PARM_ALL_QUEUE_UAPSD(__pH2CCmd, __Value)		\
+	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+3, 0, 8, __Value)
+#define SET_H2CCMD_PWRMODE_PARM_PWR_STATE(__pH2CCmd, __Value)	\
+	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+4, 0, 8, __Value)
+#define GET_8821AE_H2CCMD_PWRMODE_PARM_MODE(__pH2CCmd)		\
+	LE_BITS_TO_1BYTE(__pH2CCmd, 0, 8)
+
+#define SET_H2CCMD_JOINBSSRPT_PARM_OPMODE(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 8, __val)
+#define SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 8, __val)
+#define SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+1, 0, 8, __val)
+#define SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+2, 0, 8, __val)
+
+/* AP_OFFLOAD */
+#define SET_H2CCMD_AP_OFFLOAD_ON(__pH2CCmd, __Value)	\
+	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 0, 8, __Value)
+#define SET_H2CCMD_AP_OFFLOAD_HIDDEN(__pH2CCmd, __Value)		\
+	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+1, 0, 8, __Value)
+#define SET_H2CCMD_AP_OFFLOAD_DENYANY(__pH2CCmd, __Value)	\
+	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+2, 0, 8, __Value)
+#define SET_H2CCMD_AP_OFFLOAD_WAKEUP_EVT_RPT(__pH2CCmd, __Value)		\
+	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+3, 0, 8, __Value)
+
+/* Keep Alive Control*/
+#define SET_8821AE_H2CCMD_KEEP_ALIVE_ENABLE(__pH2CCmd, __Value)				\
+	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 0, 1, __Value)
+#define SET_8821AE_H2CCMD_KEEP_ALIVE_ACCPEPT_USER_DEFINED(__pH2CCmd, __Value)	\
+	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 1, 1, __Value)
+#define SET_8821AE_H2CCMD_KEEP_ALIVE_PERIOD(__pH2CCmd, __Value)				\
+	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+1, 0, 8, __Value)
+
+/*REMOTE_WAKE_CTRL */
+#define SET_8821AE_H2CCMD_REMOTE_WAKE_CTRL_EN(__pH2CCmd, __Value)				\
+	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 0, 1, __Value)
+#if(USE_OLD_WOWLAN_DEBUG_FW == 0)
+#define SET_8821AE_H2CCMD_REMOTE_WAKE_CTRL_ARP_OFFLOAD_EN(__pH2CCmd, __Value)				\
+	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 1, 1, __Value)
+#define SET_8821AE_H2CCMD_REMOTE_WAKE_CTRL_NDP_OFFLOAD_EN(__pH2CCmd, __Value)				\
+	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 2, 1, __Value)
+#define SET_8821AE_H2CCMD_REMOTE_WAKE_CTRL_GTK_OFFLOAD_EN(__pH2CCmd, __Value)				\
+	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 3, 1, __Value)
+#else
+#define SET_8821AE_H2CCMD_REMOTE_WAKE_CTRL_PAIRWISE_ENC_ALG(__pH2CCmd, __Value)		\
+	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+1, 0, 8, __Value)
+#define SET_8821AE_H2CCMD_REMOTE_WAKE_CTRL_GROUP_ENC_ALG(__pH2CCmd, __Value)		\
+	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+2, 0, 8, __Value)
+#endif
+
+/* GTK_OFFLOAD */
+#define SET_8821AE_H2CCMD_AOAC_GLOBAL_INFO_PAIRWISE_ENC_ALG(__pH2CCmd, __Value)		\
+	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 0, 8, __Value)
+#define SET_8821AE_H2CCMD_AOAC_GLOBAL_INFO_GROUP_ENC_ALG(__pH2CCmd, __Value)		\
+	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+1, 0, 8, __Value)
+
+/* AOAC_RSVDPAGE_LOC */
+#define SET_8821AE_H2CCMD_AOAC_RSVDPAGE_LOC_REMOTE_WAKE_CTRL_INFO(__pH2CCmd, __Value)		\
+	SET_BITS_TO_LE_1BYTE((__pH2CCmd), 0, 8, __Value)
+#define SET_8821AE_H2CCMD_AOAC_RSVDPAGE_LOC_ARP_RSP(__pH2CCmd, __Value)		\
+	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+1, 0, 8, __Value)
+#define SET_8821AE_H2CCMD_AOAC_RSVDPAGE_LOC_NEIGHBOR_ADV(__pH2CCmd, __Value)		\
+	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+2, 0, 8, __Value)
+#define SET_8821AE_H2CCMD_AOAC_RSVDPAGE_LOC_GTK_RSP(__pH2CCmd, __Value)		\
+	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+3, 0, 8, __Value)
+#define SET_8821AE_H2CCMD_AOAC_RSVDPAGE_LOC_GTK_INFO(__pH2CCmd, __Value)			\
+	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+4, 0, 8, __Value)
+
+int rtl8821ae_download_fw(struct ieee80211_hw *hw,
+				bool buse_wake_on_wlan_fw);
+void rtl8821ae_fill_h2c_cmd(struct ieee80211_hw *hw, u8 element_id,
+			 u32 cmd_len, u8 *p_cmdbuffer);
+void rtl8821ae_firmware_selfreset(struct ieee80211_hw *hw);
+void rtl8821ae_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode);
+void rtl8821ae_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus);
+void rtl8821ae_set_fw_ap_off_load_cmd(struct ieee80211_hw *hw,  u8 ap_offload_enable);
+void rtl8821ae_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished);
+void rtl8812ae_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished);
+void rtl8821ae_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state);
+void rtl8812ae_c2h_packet_handler(struct ieee80211_hw *hw, u8 *buffer, u8 length);
+#endif
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/hal_bt_coexist.c b/drivers/staging/rtl8821ae/rtl8821ae/hal_bt_coexist.c
new file mode 100644
index 000000000000..8bee772d766f
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/hal_bt_coexist.c
@@ -0,0 +1,519 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "hal_bt_coexist.h"
+#include "../pci.h"
+#include "dm.h"
+#include "fw.h"
+#include "phy.h"
+#include "reg.h"
+#include "hal_btc.h"
+
+static bool bt_operation_on = false;
+
+void rtl8821ae_dm_bt_reject_ap_aggregated_packet(struct ieee80211_hw *hw, bool b_reject)
+{
+#if 0
+	struct rtl_priv rtlpriv = rtl_priv(hw);
+	PRX_TS_RECORD			pRxTs = NULL;
+
+	if(b_reject){
+		// Do not allow receiving A-MPDU aggregation.
+		if (rtlpriv->mac80211.vendor == PEER_CISCO) {
+				if (pHTInfo->bAcceptAddbaReq) {
+					RTPRINT(FBT, BT_TRACE, ("BT_Disallow AMPDU \n"));
+					pHTInfo->bAcceptAddbaReq = FALSE;
+					if(GetTs(Adapter, (PTS_COMMON_INFO*)(&pRxTs), pMgntInfo->Bssid, 0, RX_DIR, FALSE))
+						TsInitDelBA(Adapter, (PTS_COMMON_INFO)pRxTs, RX_DIR);
+				}
+			} else {
+				if (!pHTInfo->bAcceptAddbaReq) {
+					RTPRINT(FBT, BT_TRACE, ("BT_Allow AMPDU BT Idle\n"));
+					pHTInfo->bAcceptAddbaReq = TRUE;
+				}
+			}
+		} else {
+			if(rtlpriv->mac80211.vendor == PEER_CISCO) {
+				if (!pHTInfo->bAcceptAddbaReq) {
+					RTPRINT(FBT, BT_TRACE, ("BT_Allow AMPDU \n"));
+					pHTInfo->bAcceptAddbaReq = TRUE;
+				}
+			}
+		}
+#endif
+}
+
+void _rtl8821ae_dm_bt_check_wifi_state(struct ieee80211_hw *hw)
+{
+struct rtl_priv *rtlpriv = rtl_priv(hw);
+struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+if (rtlpriv->link_info.b_busytraffic) {
+	rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_IDLE;
+
+	if(rtlpriv->link_info.b_tx_busy_traffic) {
+		rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_WIFI_UPLINK;
+	} else {
+		rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_UPLINK;
+	}
+
+	if(rtlpriv->link_info.b_rx_busy_traffic) {
+		rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_WIFI_DOWNLINK;
+	} else {
+		rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_DOWNLINK;
+	}
+} else {
+	rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_WIFI_IDLE;
+	rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_UPLINK;
+	rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_DOWNLINK;
+}
+
+if (rtlpriv->mac80211.mode == WIRELESS_MODE_G
+	|| rtlpriv->mac80211.mode == WIRELESS_MODE_B) {
+	rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_WIFI_LEGACY;
+	rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_HT20;
+	rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_HT40;
+} else {
+	rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_LEGACY;
+	if(rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40) {
+		rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_WIFI_HT40;
+		rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_HT20;
+	} else {
+		rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_WIFI_HT20;
+		rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_HT40;
+	}
+}
+
+if (bt_operation_on) {
+	rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_BT30;
+} else {
+	rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_BT30;
+}
+}
+
+
+u8 rtl8821ae_dm_bt_check_coex_rssi_state1(struct ieee80211_hw *hw,
+						u8	level_num, u8	rssi_thresh, u8 rssi_thresh1)
+
+{
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	long undecoratedsmoothed_pwdb = 0;
+	u8 bt_rssi_state = 0;
+
+	undecoratedsmoothed_pwdb =  rtl8821ae_dm_bt_get_rx_ss(hw);
+
+	if(level_num == 2) {
+		rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_RSSI_1_MEDIUM;
+
+		if( (rtlpcipriv->btcoexist.bt_pre_rssi_state == BT_RSSI_STATE_LOW) ||
+			(rtlpcipriv->btcoexist.bt_pre_rssi_state == BT_RSSI_STATE_STAY_LOW)) {
+			if(undecoratedsmoothed_pwdb >= (rssi_thresh + BT_FW_COEX_THRESH_TOL)) {
+				bt_rssi_state = BT_RSSI_STATE_HIGH;
+				rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_WIFI_RSSI_1_HIGH;
+				rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_RSSI_1_LOW;
+				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[DM][BT], RSSI_1 state switch to High\n"));
+			} else {
+				bt_rssi_state = BT_RSSI_STATE_STAY_LOW;
+				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[DM][BT], RSSI_1 state stay at Low\n"));
+			}
+		} else {
+			if(undecoratedsmoothed_pwdb < rssi_thresh) {
+				bt_rssi_state = BT_RSSI_STATE_LOW;
+				rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_WIFI_RSSI_1_LOW;
+				rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_RSSI_1_HIGH;
+				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[DM][BT], RSSI_1 state switch to Low\n"));
+			} else {
+				bt_rssi_state = BT_RSSI_STATE_STAY_HIGH;
+				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[DM][BT], RSSI_1 state stay at High\n"));
+			}
+		}
+	} else if(level_num == 3) {
+		if(rssi_thresh > rssi_thresh1) {
+			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[DM][BT], RSSI_1 thresh error!!\n"));
+			return rtlpcipriv->btcoexist.bt_pre_rssi_state;
+		}
+
+		if( (rtlpcipriv->btcoexist.bt_pre_rssi_state == BT_RSSI_STATE_LOW) ||
+			(rtlpcipriv->btcoexist.bt_pre_rssi_state == BT_RSSI_STATE_STAY_LOW)) {
+			if(undecoratedsmoothed_pwdb >= (rssi_thresh+BT_FW_COEX_THRESH_TOL)) {
+				bt_rssi_state = BT_RSSI_STATE_MEDIUM;
+				rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_WIFI_RSSI_1_MEDIUM;
+				rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_RSSI_1_LOW;
+				rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_RSSI_1_HIGH;
+				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[DM][BT], RSSI_1 state switch to Medium\n"));
+			} else {
+				bt_rssi_state = BT_RSSI_STATE_STAY_LOW;
+				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[DM][BT], RSSI_1 state stay at Low\n"));
+			}
+		} else if( (rtlpcipriv->btcoexist.bt_pre_rssi_state == BT_RSSI_STATE_MEDIUM) ||
+			(rtlpcipriv->btcoexist.bt_pre_rssi_state == BT_RSSI_STATE_STAY_MEDIUM)) {
+			if(undecoratedsmoothed_pwdb >= (rssi_thresh1 + BT_FW_COEX_THRESH_TOL)) {
+				bt_rssi_state = BT_RSSI_STATE_HIGH;
+				rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_WIFI_RSSI_1_HIGH;
+				rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_RSSI_1_LOW;
+				rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_RSSI_1_MEDIUM;
+				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[DM][BT], RSSI_1 state switch to High\n"));
+			} else if(undecoratedsmoothed_pwdb < rssi_thresh) {
+				bt_rssi_state = BT_RSSI_STATE_LOW;
+				rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_WIFI_RSSI_1_LOW;
+				rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_RSSI_1_HIGH;
+				rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_RSSI_1_MEDIUM;
+				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[DM][BT], RSSI_1 state switch to Low\n"));
+			} else {
+				bt_rssi_state = BT_RSSI_STATE_STAY_MEDIUM;
+				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[DM][BT], RSSI_1 state stay at Medium\n"));
+			}
+		} else {
+			if(undecoratedsmoothed_pwdb < rssi_thresh1) {
+				bt_rssi_state = BT_RSSI_STATE_MEDIUM;
+				rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_WIFI_RSSI_1_MEDIUM;
+				rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_RSSI_1_HIGH;
+				rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_RSSI_1_LOW;
+				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,("[DM][BT], RSSI_1 state switch to Medium\n"));
+			} else {
+				bt_rssi_state = BT_RSSI_STATE_STAY_HIGH;
+				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[DM][BT], RSSI_1 state stay at High\n"));
+			}
+		}
+	}
+
+	rtlpcipriv->btcoexist.bt_pre_rssi_state1 = bt_rssi_state;
+
+	return bt_rssi_state;
+}
+
+u8 rtl8821ae_dm_bt_check_coex_rssi_state(struct ieee80211_hw *hw,
+						u8	level_num, u8	rssi_thresh, u8 rssi_thresh1)
+{
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	long undecoratedsmoothed_pwdb = 0;
+	u8 bt_rssi_state = 0;
+
+	undecoratedsmoothed_pwdb = rtl8821ae_dm_bt_get_rx_ss(hw);
+
+	if (level_num == 2) {
+		rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_RSSI_MEDIUM;
+
+		if ((rtlpcipriv->btcoexist.bt_pre_rssi_state == BT_RSSI_STATE_LOW) ||
+			(rtlpcipriv->btcoexist.bt_pre_rssi_state == BT_RSSI_STATE_STAY_LOW)){
+			if (undecoratedsmoothed_pwdb
+				>= (rssi_thresh + BT_FW_COEX_THRESH_TOL)) {
+				bt_rssi_state = BT_RSSI_STATE_HIGH;
+				rtlpcipriv->btcoexist.current_state
+					|= BT_COEX_STATE_WIFI_RSSI_HIGH;
+				rtlpcipriv->btcoexist.current_state
+					&= ~BT_COEX_STATE_WIFI_RSSI_LOW;
+				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+					("[DM][BT], RSSI state switch to High\n"));
+			} else {
+				bt_rssi_state = BT_RSSI_STATE_STAY_LOW;
+				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+					("[DM][BT], RSSI state stay at Low\n"));
+			}
+		} else {
+			if (undecoratedsmoothed_pwdb < rssi_thresh) {
+				bt_rssi_state = BT_RSSI_STATE_LOW;
+				rtlpcipriv->btcoexist.current_state
+					|= BT_COEX_STATE_WIFI_RSSI_LOW;
+				rtlpcipriv->btcoexist.current_state
+					&= ~BT_COEX_STATE_WIFI_RSSI_HIGH;
+				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+					("[DM][BT], RSSI state switch to Low\n"));
+			} else {
+				bt_rssi_state = BT_RSSI_STATE_STAY_HIGH;
+				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+					("[DM][BT], RSSI state stay at High\n"));
+			}
+		}
+	}
+	else if (level_num == 3) {
+		if (rssi_thresh > rssi_thresh1) {
+			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+				("[DM][BT], RSSI thresh error!!\n"));
+			return rtlpcipriv->btcoexist.bt_pre_rssi_state;
+		}
+		if ((rtlpcipriv->btcoexist.bt_pre_rssi_state == BT_RSSI_STATE_LOW) ||
+			(rtlpcipriv->btcoexist.bt_pre_rssi_state == BT_RSSI_STATE_STAY_LOW)) {
+			if(undecoratedsmoothed_pwdb
+				>= (rssi_thresh + BT_FW_COEX_THRESH_TOL)) {
+				bt_rssi_state = BT_RSSI_STATE_MEDIUM;
+				rtlpcipriv->btcoexist.current_state
+					|= BT_COEX_STATE_WIFI_RSSI_MEDIUM;
+				rtlpcipriv->btcoexist.current_state
+					&= ~BT_COEX_STATE_WIFI_RSSI_LOW;
+				rtlpcipriv->btcoexist.current_state
+					&= ~BT_COEX_STATE_WIFI_RSSI_HIGH;
+				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+					("[DM][BT], RSSI state switch to Medium\n"));
+			} else {
+				bt_rssi_state = BT_RSSI_STATE_STAY_LOW;
+				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+					("[DM][BT], RSSI state stay at Low\n"));
+			}
+		} else if ((rtlpcipriv->btcoexist.bt_pre_rssi_state == BT_RSSI_STATE_MEDIUM) ||
+			(rtlpcipriv->btcoexist.bt_pre_rssi_state == BT_RSSI_STATE_STAY_MEDIUM)) {
+			if (undecoratedsmoothed_pwdb
+				>= (rssi_thresh1 + BT_FW_COEX_THRESH_TOL)) {
+				bt_rssi_state = BT_RSSI_STATE_HIGH;
+				rtlpcipriv->btcoexist.current_state
+					|= BT_COEX_STATE_WIFI_RSSI_HIGH;
+				rtlpcipriv->btcoexist.current_state
+					&= ~BT_COEX_STATE_WIFI_RSSI_LOW;
+				rtlpcipriv->btcoexist.current_state
+					&= ~BT_COEX_STATE_WIFI_RSSI_MEDIUM;
+				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+					("[DM][BT], RSSI state switch to High\n"));
+			} else if(undecoratedsmoothed_pwdb < rssi_thresh)
+			{
+				bt_rssi_state = BT_RSSI_STATE_LOW;
+				rtlpcipriv->btcoexist.current_state
+					|= BT_COEX_STATE_WIFI_RSSI_LOW;
+				rtlpcipriv->btcoexist.current_state
+					&= ~BT_COEX_STATE_WIFI_RSSI_HIGH;
+				rtlpcipriv->btcoexist.current_state
+					&= ~BT_COEX_STATE_WIFI_RSSI_MEDIUM;
+				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+					("[DM][BT], RSSI state switch to Low\n"));
+			} else {
+				bt_rssi_state = BT_RSSI_STATE_STAY_MEDIUM;
+				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+					("[DM][BT], RSSI state stay at Medium\n"));
+			}
+		} else {
+			if(undecoratedsmoothed_pwdb < rssi_thresh1) {
+				bt_rssi_state = BT_RSSI_STATE_MEDIUM;
+				rtlpcipriv->btcoexist.current_state
+					|= BT_COEX_STATE_WIFI_RSSI_MEDIUM;
+				rtlpcipriv->btcoexist.current_state
+					&= ~BT_COEX_STATE_WIFI_RSSI_HIGH;
+				rtlpcipriv->btcoexist.current_state
+					&= ~BT_COEX_STATE_WIFI_RSSI_LOW;
+				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+					("[DM][BT], RSSI state switch to Medium\n"));
+			} else {
+			bt_rssi_state = BT_RSSI_STATE_STAY_HIGH;
+			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+				("[DM][BT], RSSI state stay at High\n"));
+			}
+		}
+	}
+
+	rtlpcipriv->btcoexist.bt_pre_rssi_state = bt_rssi_state;
+	return bt_rssi_state;
+}
+long rtl8821ae_dm_bt_get_rx_ss(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	long undecoratedsmoothed_pwdb = 0;
+
+	if (rtlpriv->mac80211.link_state >= MAC80211_LINKED) {
+		undecoratedsmoothed_pwdb = GET_UNDECORATED_AVERAGE_RSSI(rtlpriv);
+	} else {
+		undecoratedsmoothed_pwdb
+			= rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb;
+	}
+	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+		("rtl8821ae_dm_bt_get_rx_ss() = %ld\n", undecoratedsmoothed_pwdb));
+
+	return undecoratedsmoothed_pwdb;
+}
+
+void rtl8821ae_dm_bt_balance(struct ieee80211_hw *hw,
+							bool b_balance_on, u8 ms0, u8 ms1)
+{
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 h2c_parameter[3] ={0};
+
+	if (b_balance_on) {
+		h2c_parameter[2] = 1;
+		h2c_parameter[1] = ms1;
+		h2c_parameter[0] = ms0;
+		rtlpcipriv->btcoexist.b_fw_coexist_all_off = false;
+	} else {
+		h2c_parameter[2] = 0;
+		h2c_parameter[1] = 0;
+		h2c_parameter[0] = 0;
+	}
+	rtlpcipriv->btcoexist.b_balance_on = b_balance_on;
+
+	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+		("[DM][BT], Balance=[%s:%dms:%dms], write 0xc=0x%x\n",
+		b_balance_on?"ON":"OFF", ms0, ms1,
+		h2c_parameter[0]<<16 | h2c_parameter[1]<<8 | h2c_parameter[2]));
+
+	rtl8821ae_fill_h2c_cmd(hw, 0xc, 3, h2c_parameter);
+}
+
+
+void rtl8821ae_dm_bt_agc_table(struct ieee80211_hw *hw, u8 type)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+
+	if (type == BT_AGCTABLE_OFF) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BT]AGCTable Off!\n"));
+		rtl_write_dword(rtlpriv, 0xc78,0x641c0001);
+		rtl_write_dword(rtlpriv, 0xc78,0x631d0001);
+		rtl_write_dword(rtlpriv, 0xc78,0x621e0001);
+		rtl_write_dword(rtlpriv, 0xc78,0x611f0001);
+		rtl_write_dword(rtlpriv, 0xc78,0x60200001);
+
+		rtl8821ae_phy_set_rf_reg(hw, RF90_PATH_A,
+					RF_RX_AGC_HP, 0xfffff, 0x32000);
+		rtl8821ae_phy_set_rf_reg(hw, RF90_PATH_A,
+					RF_RX_AGC_HP, 0xfffff, 0x71000);
+		rtl8821ae_phy_set_rf_reg(hw, RF90_PATH_A,
+					RF_RX_AGC_HP, 0xfffff, 0xb0000);
+		rtl8821ae_phy_set_rf_reg(hw, RF90_PATH_A,
+					RF_RX_AGC_HP, 0xfffff, 0xfc000);
+		rtl8821ae_phy_set_rf_reg(hw, RF90_PATH_A,
+					RF_RX_G1, 0xfffff, 0x30355);
+	} else if (type == BT_AGCTABLE_ON) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BT]AGCTable On!\n"));
+		rtl_write_dword(rtlpriv, 0xc78,0x4e1c0001);
+		rtl_write_dword(rtlpriv, 0xc78,0x4d1d0001);
+		rtl_write_dword(rtlpriv, 0xc78,0x4c1e0001);
+		rtl_write_dword(rtlpriv, 0xc78,0x4b1f0001);
+		rtl_write_dword(rtlpriv, 0xc78,0x4a200001);
+
+		rtl8821ae_phy_set_rf_reg(hw, RF90_PATH_A,
+					RF_RX_AGC_HP, 0xfffff, 0xdc000);
+		rtl8821ae_phy_set_rf_reg(hw, RF90_PATH_A,
+					RF_RX_AGC_HP, 0xfffff, 0x90000);
+		rtl8821ae_phy_set_rf_reg(hw, RF90_PATH_A,
+					RF_RX_AGC_HP, 0xfffff, 0x51000);
+		rtl8821ae_phy_set_rf_reg(hw, RF90_PATH_A,
+					RF_RX_AGC_HP, 0xfffff, 0x12000);
+		rtl8821ae_phy_set_rf_reg(hw, RF90_PATH_A,
+					RF_RX_G1, 0xfffff, 0x00355);
+
+		rtlpcipriv->btcoexist.b_sw_coexist_all_off = false;
+	}
+}
+
+void rtl8821ae_dm_bt_bb_back_off_level(struct ieee80211_hw *hw, u8 type)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+
+	if (type == BT_BB_BACKOFF_OFF) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BT]BBBackOffLevel Off!\n"));
+		rtl_write_dword(rtlpriv, 0xc04,0x3a05611);
+	} else if (type == BT_BB_BACKOFF_ON) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BT]BBBackOffLevel On!\n"));
+		rtl_write_dword(rtlpriv, 0xc04,0x3a07611);
+		rtlpcipriv->btcoexist.b_sw_coexist_all_off = false;
+	}
+}
+
+void rtl8821ae_dm_bt_fw_coex_all_off(struct ieee80211_hw *hw)
+{
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+		("rtl8821ae_dm_bt_fw_coex_all_off()\n"));
+
+	if(rtlpcipriv->btcoexist.b_fw_coexist_all_off)
+		return;
+
+	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+		("rtl8821ae_dm_bt_fw_coex_all_off(), real Do\n"));
+	rtl8821ae_dm_bt_fw_coex_all_off_8723a(hw);
+	rtlpcipriv->btcoexist.b_fw_coexist_all_off = true;
+}
+
+void rtl8821ae_dm_bt_sw_coex_all_off(struct ieee80211_hw *hw)
+{
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+		("rtl8821ae_dm_bt_sw_coex_all_off()\n"));
+
+	if(rtlpcipriv->btcoexist.b_sw_coexist_all_off)
+		return;
+
+	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+		("rtl8821ae_dm_bt_sw_coex_all_off(), real Do\n"));
+	rtl8821ae_dm_bt_sw_coex_all_off_8723a(hw);
+	rtlpcipriv->btcoexist.b_sw_coexist_all_off = true;
+}
+
+void rtl8821ae_dm_bt_hw_coex_all_off(struct ieee80211_hw *hw)
+{
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+		("rtl8821ae_dm_bt_hw_coex_all_off()\n"));
+
+	if(rtlpcipriv->btcoexist.b_hw_coexist_all_off)
+		return;
+	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+		("rtl8821ae_dm_bt_hw_coex_all_off(), real Do\n"));
+
+	rtl8821ae_dm_bt_hw_coex_all_off_8723a(hw);
+
+	rtlpcipriv->btcoexist.b_hw_coexist_all_off = true;
+}
+
+void rtl8821ae_btdm_coex_all_off(struct ieee80211_hw *hw)
+{
+	rtl8821ae_dm_bt_fw_coex_all_off(hw);
+	rtl8821ae_dm_bt_sw_coex_all_off(hw);
+	rtl8821ae_dm_bt_hw_coex_all_off(hw);
+}
+
+bool rtl8821ae_dm_bt_is_coexist_state_changed(struct ieee80211_hw *hw)
+{
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+
+	if((rtlpcipriv->btcoexist.previous_state
+		== rtlpcipriv->btcoexist.current_state)
+		&& (rtlpcipriv->btcoexist.previous_state_h
+		== rtlpcipriv->btcoexist.current_state_h))
+		return false;
+	else
+		return true;
+}
+
+bool rtl8821ae_dm_bt_is_wifi_up_link(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (rtlpriv->link_info.b_tx_busy_traffic)
+		return true;
+	else
+		return false;
+}
+
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/hal_bt_coexist.h b/drivers/staging/rtl8821ae/rtl8821ae/hal_bt_coexist.h
new file mode 100644
index 000000000000..799cc6f95cc1
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/hal_bt_coexist.h
@@ -0,0 +1,169 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL8821AE_HAL_BT_COEXIST_H__
+#define __RTL8821AE_HAL_BT_COEXIST_H__
+
+#include "../wifi.h"
+
+/* The reg define is for 8723 */
+#define	REG_HIGH_PRIORITY_TXRX			0x770
+#define	REG_LOW_PRIORITY_TXRX			0x774
+
+#define BT_FW_COEX_THRESH_TOL			6
+#define BT_FW_COEX_THRESH_20			20
+#define BT_FW_COEX_THRESH_23			23
+#define BT_FW_COEX_THRESH_25			25
+#define BT_FW_COEX_THRESH_30			30
+#define BT_FW_COEX_THRESH_35			35
+#define BT_FW_COEX_THRESH_40			40
+#define BT_FW_COEX_THRESH_45			45
+#define BT_FW_COEX_THRESH_47			47
+#define BT_FW_COEX_THRESH_50			50
+#define BT_FW_COEX_THRESH_55			55
+
+#define BT_COEX_STATE_BT30				BIT(0)
+#define BT_COEX_STATE_WIFI_HT20			BIT(1)
+#define BT_COEX_STATE_WIFI_HT40			BIT(2)
+#define BT_COEX_STATE_WIFI_LEGACY		BIT(3)
+
+#define BT_COEX_STATE_WIFI_RSSI_LOW		BIT(4)
+#define BT_COEX_STATE_WIFI_RSSI_MEDIUM	BIT(5)
+#define BT_COEX_STATE_WIFI_RSSI_HIGH	BIT(6)
+#define BT_COEX_STATE_DEC_BT_POWER		BIT(7)
+
+#define BT_COEX_STATE_WIFI_IDLE			BIT(8)
+#define BT_COEX_STATE_WIFI_UPLINK		BIT(9)
+#define BT_COEX_STATE_WIFI_DOWNLINK		BIT(10)
+
+#define BT_COEX_STATE_BT_INQ_PAGE			BIT(11)
+#define BT_COEX_STATE_BT_IDLE			BIT(12)
+#define BT_COEX_STATE_BT_UPLINK			BIT(13)
+#define BT_COEX_STATE_BT_DOWNLINK		BIT(14)
+
+#define BT_COEX_STATE_HOLD_FOR_BT_OPERATION	BIT(15)
+#define BT_COEX_STATE_BT_RSSI_LOW		BIT(19)
+
+#define BT_COEX_STATE_PROFILE_HID		BIT(20)
+#define BT_COEX_STATE_PROFILE_A2DP		BIT(21)
+#define BT_COEX_STATE_PROFILE_PAN		BIT(22)
+#define BT_COEX_STATE_PROFILE_SCO		BIT(23)
+
+#define BT_COEX_STATE_WIFI_RSSI_1_LOW		BIT(24)
+#define BT_COEX_STATE_WIFI_RSSI_1_MEDIUM	BIT(25)
+#define BT_COEX_STATE_WIFI_RSSI_1_HIGH		BIT(26)
+
+#define BT_COEX_STATE_BTINFO_COMMON			BIT(30)
+#define BT_COEX_STATE_BTINFO_B_HID_SCOESCO	BIT(31)
+#define BT_COEX_STATE_BTINFO_B_FTP_A2DP		BIT(29)
+
+#define BT_COEX_STATE_BT_CNT_LEVEL_0		BIT(0)
+#define BT_COEX_STATE_BT_CNT_LEVEL_1		BIT(1)
+#define BT_COEX_STATE_BT_CNT_LEVEL_2		BIT(2)
+#define BT_COEX_STATE_BT_CNT_LEVEL_3		BIT(3)
+
+#define BT_RSSI_STATE_HIGH			0
+#define BT_RSSI_STATE_MEDIUM		1
+#define BT_RSSI_STATE_LOW			2
+#define BT_RSSI_STATE_STAY_HIGH		3
+#define BT_RSSI_STATE_STAY_MEDIUM	4
+#define BT_RSSI_STATE_STAY_LOW		5
+
+#define	BT_AGCTABLE_OFF				0
+#define	BT_AGCTABLE_ON				1
+#define	BT_BB_BACKOFF_OFF			0
+#define	BT_BB_BACKOFF_ON			1
+#define	BT_FW_NAV_OFF				0
+#define	BT_FW_NAV_ON				1
+
+#define	BT_COEX_MECH_NONE			0
+#define	BT_COEX_MECH_SCO			1
+#define	BT_COEX_MECH_HID			2
+#define	BT_COEX_MECH_A2DP			3
+#define	BT_COEX_MECH_PAN			4
+#define	BT_COEX_MECH_HID_A2DP		5
+#define	BT_COEX_MECH_HID_PAN		6
+#define	BT_COEX_MECH_PAN_A2DP		7
+#define	BT_COEX_MECH_HID_SCO_ESCO	8
+#define	BT_COEX_MECH_FTP_A2DP		9
+#define	BT_COEX_MECH_COMMON			10
+#define	BT_COEX_MECH_MAX			11
+
+#define	BT_DBG_PROFILE_NONE			0
+#define	BT_DBG_PROFILE_SCO			1
+#define	BT_DBG_PROFILE_HID			2
+#define	BT_DBG_PROFILE_A2DP			3
+#define	BT_DBG_PROFILE_PAN			4
+#define	BT_DBG_PROFILE_HID_A2DP		5
+#define	BT_DBG_PROFILE_HID_PAN		6
+#define	BT_DBG_PROFILE_PAN_A2DP		7
+#define	BT_DBG_PROFILE_MAX			9
+
+#define	BTINFO_B_FTP					BIT(7)
+#define	BTINFO_B_A2DP					BIT(6)
+#define	BTINFO_B_HID					BIT(5)
+#define	BTINFO_B_SCO_BUSY				BIT(4)
+#define	BTINFO_B_ACL_BUSY				BIT(3)
+#define	BTINFO_B_INQ_PAGE				BIT(2)
+#define	BTINFO_B_SCO_ESCO				BIT(1)
+#define	BTINFO_B_CONNECTION				BIT(0)
+
+
+void rtl8821ae_btdm_coex_all_off(struct ieee80211_hw *hw);
+void rtl8821ae_dm_bt_fw_coex_all_off(struct ieee80211_hw *hw);
+
+void rtl8821ae_dm_bt_sw_coex_all_off(struct ieee80211_hw *hw);
+void rtl8821ae_dm_bt_hw_coex_all_off(struct ieee80211_hw *hw);
+long rtl8821ae_dm_bt_get_rx_ss(struct ieee80211_hw *hw);
+void rtl8821ae_dm_bt_balance(struct ieee80211_hw *hw,
+			bool b_balance_on, u8 ms0, u8 ms1);
+void rtl8821ae_dm_bt_agc_table(struct ieee80211_hw *hw, u8 tyep);
+void rtl8821ae_dm_bt_bb_back_off_level(struct ieee80211_hw *hw, u8 type);
+u8 rtl8821ae_dm_bt_check_coex_rssi_state(struct ieee80211_hw *hw,
+						u8	level_num, u8	rssi_thresh, u8 rssi_thresh1);
+u8 rtl8821ae_dm_bt_check_coex_rssi_state1(struct ieee80211_hw *hw,
+		                        u8  level_num, u8   rssi_thresh, u8 rssi_thresh1);
+void _rtl8821ae_dm_bt_check_wifi_state(struct ieee80211_hw *hw);
+void rtl8821ae_dm_bt_reject_ap_aggregated_packet(struct ieee80211_hw *hw,
+			bool b_reject);
+
+#if 0
+VOID
+BTDM_PWDBMonitor(
+	 	PADAPTER	Adapter
+	);
+
+BOOLEAN
+BTDM_DIGByBTRSSI(
+	 		PADAPTER	Adapter
+	);
+#endif
+bool rtl8821ae_dm_bt_is_coexist_state_changed(struct ieee80211_hw *hw);
+bool rtl8821ae_dm_bt_is_wifi_up_link(struct ieee80211_hw *hw);
+#endif
+
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/hal_btc.c b/drivers/staging/rtl8821ae/rtl8821ae/hal_btc.c
new file mode 100644
index 000000000000..79386ee142f9
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/hal_btc.c
@@ -0,0 +1,2069 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+#include "hal_btc.h"
+#include "../pci.h"
+#include "phy.h"
+#include "fw.h"
+#include "reg.h"
+#include "def.h"
+#include "../btcoexist/rtl_btc.h"
+
+static struct bt_coexist_8821ae hal_coex_8821ae;
+
+void rtl8821ae_dm_bt_turn_off_bt_coexist_before_enter_lps(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+    struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+
+	if(!rtlpcipriv->btcoexist.bt_coexistence)
+		return;
+
+	if(ppsc->b_inactiveps) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,("[BT][DM], Before enter IPS, turn off all Coexist DM\n"));
+		rtlpcipriv->btcoexist.current_state = 0;
+		rtlpcipriv->btcoexist.previous_state = 0;
+		rtlpcipriv->btcoexist.current_state_h = 0;
+		rtlpcipriv->btcoexist.previous_state_h = 0;
+		rtl8821ae_btdm_coex_all_off(hw);
+	}
+}
+
+
+enum rt_media_status mgnt_link_status_query(struct ieee80211_hw *hw)
+{
+    struct rtl_priv *rtlpriv = rtl_priv(hw);
+    struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+    enum rt_media_status    m_status = RT_MEDIA_DISCONNECT;
+
+    u8 bibss = (mac->opmode == NL80211_IFTYPE_ADHOC) ? 1 : 0;
+
+    if(bibss || rtlpriv->mac80211.link_state >= MAC80211_LINKED) {
+            m_status = RT_MEDIA_CONNECT;
+    }
+
+    return m_status;
+}
+
+void rtl_8821ae_bt_wifi_media_status_notify(struct ieee80211_hw *hw, bool mstatus)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 h2c_parameter[3] ={0};
+	u8 chnl;
+
+	if(!rtlpcipriv->btcoexist.bt_coexistence)
+		return;
+
+	if(RT_MEDIA_CONNECT == mstatus)
+		h2c_parameter[0] = 0x1; // 0: disconnected, 1:connected
+	else
+		h2c_parameter[0] = 0x0;
+
+	if(mgnt_link_status_query(hw))	{
+		chnl = rtlphy->current_channel;
+		h2c_parameter[1] = chnl;
+	}
+
+	if(rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40){
+		h2c_parameter[2] = 0x30;
+	} else {
+		h2c_parameter[2] = 0x20;
+	}
+
+	RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,("[BTCoex], FW write 0x19=0x%x\n",
+		h2c_parameter[0]<<16|h2c_parameter[1]<<8|h2c_parameter[2]));
+
+	rtl8821ae_fill_h2c_cmd(hw, 0x19, 3, h2c_parameter);
+
+}
+
+
+bool rtl8821ae_dm_bt_is_wifi_busy(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	if(rtlpriv->link_info.b_busytraffic ||
+		rtlpriv->link_info.b_rx_busy_traffic ||
+		rtlpriv->link_info.b_tx_busy_traffic)
+		return true;
+	else
+		return false;
+}
+void rtl8821ae_dm_bt_set_fw_3a(struct ieee80211_hw *hw,
+						u8 byte1, u8 byte2, u8 byte3, u8 byte4, u8 byte5)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 h2c_parameter[5] ={0};
+	h2c_parameter[0] = byte1;
+	h2c_parameter[1] = byte2;
+	h2c_parameter[2] = byte3;
+	h2c_parameter[3] = byte4;
+	h2c_parameter[4] = byte5;
+	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], FW write 0x3a(4bytes)=0x%x%8x\n",
+		h2c_parameter[0], h2c_parameter[1]<<24 | h2c_parameter[2]<<16 | h2c_parameter[3]<<8 | h2c_parameter[4]));
+	rtl8821ae_fill_h2c_cmd(hw, 0x3a, 5, h2c_parameter);
+}
+
+bool rtl8821ae_dm_bt_need_to_dec_bt_pwr(struct ieee80211_hw *hw)
+{
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (mgnt_link_status_query(hw) == RT_MEDIA_CONNECT) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("Need to decrease bt power\n"));
+		rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_DEC_BT_POWER;
+			return true;
+	}
+
+	rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_DEC_BT_POWER;
+	return false;
+}
+
+
+bool rtl8821ae_dm_bt_is_same_coexist_state(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+
+	if ((rtlpcipriv->btcoexist.previous_state
+		== rtlpcipriv->btcoexist.current_state)
+		&&(rtlpcipriv->btcoexist.previous_state_h
+		== rtlpcipriv->btcoexist.current_state_h)) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+				("[DM][BT], Coexist state do not chang!!\n"));
+		return true;
+	} else {
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+				("[DM][BT], Coexist state changed!!\n"));
+		return false;
+	}
+}
+
+void rtl8821ae_dm_bt_set_coex_table(struct ieee80211_hw *hw,
+						u32 val_0x6c0, u32 val_0x6c8, u32 val_0x6cc)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("set coex table, set 0x6c0=0x%x\n", val_0x6c0));
+	rtl_write_dword(rtlpriv, 0x6c0, val_0x6c0);
+
+	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("set coex table, set 0x6c8=0x%x\n", val_0x6c8));
+	rtl_write_dword(rtlpriv, 0x6c8, val_0x6c8);
+
+	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("set coex table, set 0x6cc=0x%x\n", val_0x6cc));
+	rtl_write_byte(rtlpriv, 0x6cc, val_0x6cc);
+}
+
+void rtl8821ae_dm_bt_set_hw_pta_mode(struct ieee80211_hw *hw, bool b_mode)
+{
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (BT_PTA_MODE_ON == b_mode) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("PTA mode on, "));
+		/*  Enable GPIO 0/1/2/3/8 pins for bt */
+		rtl_write_byte(rtlpriv, 0x40, 0x20);
+		rtlpcipriv->btcoexist.b_hw_coexist_all_off = false;
+	} else {
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("PTA mode off\n"));
+		rtl_write_byte(rtlpriv, 0x40, 0x0);
+	}
+}
+
+void rtl8821ae_dm_bt_set_sw_rf_rx_lpf_corner(struct ieee80211_hw *hw, u8 type)
+{
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (BT_RF_RX_LPF_CORNER_SHRINK == type) {
+		/* Shrink RF Rx LPF corner, 0x1e[7:4]=1111 ==> [11:4] by Jenyu */
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("Shrink RF Rx LPF corner!!\n"));
+		/* PHY_SetRFReg(Adapter, (RF_RADIO_PATH_E)PathA, 0x1e, 0xf0, 0xf); */
+		rtl8821ae_phy_set_rf_reg(hw, RF90_PATH_A, 0x1e, 0xfffff, 0xf0ff7);
+		rtlpcipriv->btcoexist.b_sw_coexist_all_off = false;
+	} else if(BT_RF_RX_LPF_CORNER_RESUME == type) {
+		/*Resume RF Rx LPF corner*/
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("Resume RF Rx LPF corner!!\n"));
+		/* PHY_SetRFReg(Adapter, (RF_RADIO_PATH_E)PathA, 0x1e, 0xf0,
+		 * pHalData->btcoexist.BtRfRegOrigin1E); */
+		rtl8821ae_phy_set_rf_reg(hw, RF90_PATH_A, 0x1e, 0xfffff,
+			rtlpcipriv->btcoexist.bt_rfreg_origin_1e);
+	}
+}
+
+void rtl8821ae_dm_bt_set_sw_penalty_tx_rate_adaptive(struct ieee80211_hw *hw,
+																	u8 ra_type)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	u8 tmp_u1;
+
+	tmp_u1 = rtl_read_byte(rtlpriv, 0x4fd);
+	tmp_u1 |= BIT(0);
+	if (BT_TX_RATE_ADAPTIVE_LOW_PENALTY == ra_type) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("Tx rate adaptive, set low penalty!!\n"));
+		tmp_u1 &= ~BIT(2);
+		rtlpcipriv->btcoexist.b_sw_coexist_all_off = false;
+	} else if(BT_TX_RATE_ADAPTIVE_NORMAL == ra_type) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("Tx rate adaptive, set normal!!\n"));
+		tmp_u1 |= BIT(2);
+	}
+
+	rtl_write_byte(rtlpriv, 0x4fd, tmp_u1);
+}
+
+void rtl8821ae_dm_bt_btdm_structure_reload(struct ieee80211_hw *hw,
+													struct btdm_8821ae	*p_btdm)
+{
+	p_btdm->b_all_off = false;
+	p_btdm->b_agc_table_en = false;
+	p_btdm->b_adc_back_off_on = false;
+	p_btdm->b2_ant_hid_en = false;
+	p_btdm->b_low_penalty_rate_adaptive = false;
+	p_btdm->b_rf_rx_lpf_shrink = false;
+	p_btdm->b_reject_aggre_pkt= false;
+
+	p_btdm->b_tdma_on = false;
+	p_btdm->tdma_ant = TDMA_2ANT;
+	p_btdm->tdma_nav = TDMA_NAV_OFF;
+	p_btdm->tdma_dac_swing = TDMA_DAC_SWING_OFF;
+	p_btdm->fw_dac_swing_lvl = 0x20;
+
+	p_btdm->b_tra_tdma_on = false;
+	p_btdm->tra_tdma_ant = TDMA_2ANT;
+	p_btdm->tra_tdma_nav = TDMA_NAV_OFF;
+	p_btdm->b_ignore_wlan_act = false;
+
+	p_btdm->b_ps_tdma_on = false;
+	p_btdm->ps_tdma_byte[0] = 0x0;
+	p_btdm->ps_tdma_byte[1] = 0x0;
+	p_btdm->ps_tdma_byte[2] = 0x0;
+	p_btdm->ps_tdma_byte[3] = 0x8;
+	p_btdm->ps_tdma_byte[4] = 0x0;
+
+	p_btdm->b_pta_on = true;
+	p_btdm->val_0x6c0 = 0x5a5aaaaa;
+	p_btdm->val_0x6c8 = 0xcc;
+	p_btdm->val_0x6cc = 0x3;
+
+	p_btdm->b_sw_dac_swing_on = false;
+	p_btdm->sw_dac_swing_lvl = 0xc0;
+	p_btdm->wlan_act_hi = 0x20;
+	p_btdm->wlan_act_lo = 0x10;
+	p_btdm->bt_retry_index = 2;
+
+	p_btdm->b_dec_bt_pwr = false;
+}
+
+void rtl8821ae_dm_bt_btdm_structure_reload_all_off(struct ieee80211_hw *hw,
+													struct btdm_8821ae	*p_btdm)
+{
+	rtl8821ae_dm_bt_btdm_structure_reload(hw, p_btdm);
+	p_btdm->b_all_off = true;
+	p_btdm->b_pta_on = false;
+	p_btdm->wlan_act_hi = 0x10;
+}
+
+bool rtl8821ae_dm_bt_is_2_ant_common_action(struct ieee80211_hw *hw)
+{
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct btdm_8821ae btdm8821ae;
+	bool b_common = false;
+
+	rtl8821ae_dm_bt_btdm_structure_reload(hw, &btdm8821ae);
+
+	if(!rtl8821ae_dm_bt_is_wifi_busy(hw)
+		&& !rtlpcipriv->btcoexist.b_bt_busy) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("Wifi idle + Bt idle, bt coex mechanism always off!!\n"));
+		rtl8821ae_dm_bt_btdm_structure_reload_all_off(hw, &btdm8821ae);
+		b_common = true;
+	} else if (rtl8821ae_dm_bt_is_wifi_busy(hw)
+		&& !rtlpcipriv->btcoexist.b_bt_busy) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("Wifi non-idle + Bt disabled/idle!!\n"));
+		btdm8821ae.b_low_penalty_rate_adaptive = true;
+		btdm8821ae.b_rf_rx_lpf_shrink = false;
+		btdm8821ae.b_reject_aggre_pkt = false;
+
+		/* sw mechanism */
+		btdm8821ae.b_agc_table_en = false;
+		btdm8821ae.b_adc_back_off_on = false;
+		btdm8821ae.b_sw_dac_swing_on = false;
+
+		btdm8821ae.b_pta_on = true;
+		btdm8821ae.val_0x6c0 = 0x5a5aaaaa;
+		btdm8821ae.val_0x6c8 = 0xcccc;
+		btdm8821ae.val_0x6cc = 0x3;
+
+		btdm8821ae.b_tdma_on = false;
+		btdm8821ae.tdma_dac_swing = TDMA_DAC_SWING_OFF;
+		btdm8821ae.b2_ant_hid_en = false;
+
+		b_common = true;
+	}else if (rtlpcipriv->btcoexist.b_bt_busy) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("Bt non-idle!\n"));
+		if(mgnt_link_status_query(hw) == RT_MEDIA_CONNECT){
+			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("Wifi connection exist\n"))
+			b_common = false;
+		} else {
+			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+				("No Wifi connection!\n"));
+			btdm8821ae.b_rf_rx_lpf_shrink = true;
+			btdm8821ae.b_low_penalty_rate_adaptive = false;
+			btdm8821ae.b_reject_aggre_pkt = false;
+
+			/* sw mechanism */
+			btdm8821ae.b_agc_table_en = false;
+			btdm8821ae.b_adc_back_off_on = false;
+			btdm8821ae.b_sw_dac_swing_on = false;
+
+			btdm8821ae.b_pta_on = true;
+			btdm8821ae.val_0x6c0 = 0x55555555;
+			btdm8821ae.val_0x6c8 = 0x0000ffff;
+			btdm8821ae.val_0x6cc = 0x3;
+
+			btdm8821ae.b_tdma_on = false;
+			btdm8821ae.tdma_dac_swing = TDMA_DAC_SWING_OFF;
+			btdm8821ae.b2_ant_hid_en = false;
+
+			b_common = true;
+		}
+	}
+
+	if (rtl8821ae_dm_bt_need_to_dec_bt_pwr(hw)) {
+		btdm8821ae.b_dec_bt_pwr = true;
+	}
+
+	if(b_common)
+		 rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_BTINFO_COMMON;
+
+	if (b_common && rtl8821ae_dm_bt_is_coexist_state_changed(hw))
+		rtl8821ae_dm_bt_set_bt_dm(hw, &btdm8821ae);
+
+	return b_common;
+}
+
+void rtl8821ae_dm_bt_set_sw_full_time_dac_swing(
+		struct ieee80211_hw * hw, bool b_sw_dac_swing_on, u32 sw_dac_swing_lvl)
+{
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (b_sw_dac_swing_on) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+			("[BTCoex], SwDacSwing = 0x%x\n", sw_dac_swing_lvl));
+		rtl8821ae_phy_set_bb_reg(hw, 0x880, 0xff000000, sw_dac_swing_lvl);
+		rtlpcipriv->btcoexist.b_sw_coexist_all_off = false;
+	} else {
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], SwDacSwing Off!\n"));
+		rtl8821ae_phy_set_bb_reg(hw, 0x880, 0xff000000, 0xc0);
+	}
+}
+
+void rtl8821ae_dm_bt_set_fw_dec_bt_pwr(
+		struct ieee80211_hw *hw, bool b_dec_bt_pwr)
+{
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 h2c_parameter[1] ={0};
+
+	h2c_parameter[0] = 0;
+
+	if (b_dec_bt_pwr) {
+		h2c_parameter[0] |= BIT(1);
+		rtlpcipriv->btcoexist.b_fw_coexist_all_off = false;
+	}
+
+	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+		("[BTCoex], decrease Bt Power : %s, write 0x21=0x%x\n",
+		(b_dec_bt_pwr? "Yes!!":"No!!"), h2c_parameter[0]));
+
+	rtl8821ae_fill_h2c_cmd(hw, 0x21, 1, h2c_parameter);
+}
+
+
+void rtl8821ae_dm_bt_set_fw_2_ant_hid(struct ieee80211_hw *hw,
+									bool b_enable, bool b_dac_swing_on)
+{
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 h2c_parameter[1] ={0};
+
+	if (b_enable) {
+		h2c_parameter[0] |= BIT(0);
+		rtlpcipriv->btcoexist.b_fw_coexist_all_off = false;
+	}
+	if (b_dac_swing_on) {
+		h2c_parameter[0] |= BIT(1); /* Dac Swing default enable */
+	}
+	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+		("[BTCoex], turn 2-Ant+HID mode %s, DACSwing:%s, write 0x15=0x%x\n",
+		(b_enable ? "ON!!":"OFF!!"), (b_dac_swing_on ? "ON":"OFF"),
+		h2c_parameter[0]));
+
+	rtl8821ae_fill_h2c_cmd(hw, 0x15, 1, h2c_parameter);
+}
+
+void rtl8821ae_dm_bt_set_fw_tdma_ctrl(struct ieee80211_hw *hw,
+				bool b_enable, u8 ant_num, u8 nav_en, u8 dac_swing_en)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	u8 h2c_parameter[1] ={0};
+	u8 h2c_parameter1[1] = {0};
+
+	h2c_parameter[0] = 0;
+	h2c_parameter1[0] = 0;
+
+	if(b_enable) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+			("[BTCoex], set BT PTA update manager to trigger update!!\n"));
+		h2c_parameter1[0] |= BIT(0);
+
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+			("[BTCoex], turn TDMA mode ON!!\n"));
+		h2c_parameter[0] |= BIT(0);		/* function enable */
+		if (TDMA_1ANT == ant_num) {
+			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], TDMA_1ANT\n"));
+			h2c_parameter[0] |= BIT(1);
+		} else if(TDMA_2ANT == ant_num) {
+			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], TDMA_2ANT\n"));
+		} else {
+			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], Unknown Ant\n"));
+		}
+
+		if (TDMA_NAV_OFF == nav_en) {
+			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], TDMA_NAV_OFF\n"));
+		} else if (TDMA_NAV_ON == nav_en) {
+			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], TDMA_NAV_ON\n"));
+			h2c_parameter[0] |= BIT(2);
+		}
+
+		if (TDMA_DAC_SWING_OFF == dac_swing_en) {
+			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+				("[BTCoex], TDMA_DAC_SWING_OFF\n"));
+		} else if(TDMA_DAC_SWING_ON == dac_swing_en) {
+			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+				("[BTCoex], TDMA_DAC_SWING_ON\n"));
+			h2c_parameter[0] |= BIT(4);
+		}
+		rtlpcipriv->btcoexist.b_fw_coexist_all_off = false;
+	} else {
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+			("[BTCoex], set BT PTA update manager to no update!!\n"));
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+			("[BTCoex], turn TDMA mode OFF!!\n"));
+	}
+
+	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+		("[BTCoex], FW2AntTDMA, write 0x26=0x%x\n", h2c_parameter1[0]));
+	rtl8821ae_fill_h2c_cmd(hw, 0x26, 1, h2c_parameter1);
+
+	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+		("[BTCoex], FW2AntTDMA, write 0x14=0x%x\n", h2c_parameter[0]));
+	rtl8821ae_fill_h2c_cmd(hw, 0x14, 1, h2c_parameter);
+
+	if (!b_enable) {
+		/* delay_ms(2);
+		 * PlatformEFIOWrite1Byte(Adapter, 0x778, 0x1); */
+	}
+}
+
+
+void rtl8821ae_dm_bt_set_fw_ignore_wlan_act( struct ieee80211_hw *hw, bool b_enable)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	u8 h2c_parameter[1] ={0};
+
+	if (b_enable) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], BT Ignore Wlan_Act !!\n"));
+		h2c_parameter[0] |= BIT(0);		// function enable
+		rtlpcipriv->btcoexist.b_fw_coexist_all_off = false;
+	} else {
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], BT don't ignore Wlan_Act !!\n"));
+	}
+
+    RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], set FW for BT Ignore Wlan_Act, write 0x25=0x%x\n",
+		h2c_parameter[0]));
+
+	rtl8821ae_fill_h2c_cmd(hw, 0x25, 1, h2c_parameter);
+}
+
+
+void rtl8821ae_dm_bt_set_fw_tra_tdma_ctrl(struct ieee80211_hw *hw,
+		bool b_enable, u8 ant_num, u8 nav_en
+	)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	//struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	u8 h2c_parameter[2] ={0};
+
+
+	if (b_enable) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+			("[BTCoex], turn TTDMA mode ON!!\n"));
+		h2c_parameter[0] |= BIT(0);		// function enable
+		if (TDMA_1ANT == ant_num) {
+			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], TTDMA_1ANT\n"));
+			h2c_parameter[0] |= BIT(1);
+		} else if (TDMA_2ANT == ant_num) {
+			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], TTDMA_2ANT\n"));
+		} else {
+			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], Unknown Ant\n"));
+		}
+
+		if (TDMA_NAV_OFF == nav_en) {
+			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], TTDMA_NAV_OFF\n"));
+		} else if (TDMA_NAV_ON == nav_en) {
+			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], TTDMA_NAV_ON\n"));
+			h2c_parameter[1] |= BIT(0);
+		}
+
+		rtlpcipriv->btcoexist.b_fw_coexist_all_off = false;
+	} else {
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+			("[BTCoex], turn TTDMA mode OFF!!\n"));
+	}
+
+	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+		("[BTCoex], FW Traditional TDMA, write 0x33=0x%x\n",
+		h2c_parameter[0] << 8| h2c_parameter[1]));
+
+	rtl8821ae_fill_h2c_cmd(hw, 0x33, 2, h2c_parameter);
+}
+
+
+void rtl8821ae_dm_bt_set_fw_dac_swing_level(struct ieee80211_hw *hw,
+													u8 dac_swing_lvl)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 h2c_parameter[1] ={0};
+	h2c_parameter[0] = dac_swing_lvl;
+
+	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+		("[BTCoex], Set Dac Swing Level=0x%x\n", dac_swing_lvl));
+	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+		("[BTCoex], write 0x29=0x%x\n", h2c_parameter[0]));
+
+	rtl8821ae_fill_h2c_cmd(hw, 0x29, 1, h2c_parameter);
+}
+
+void rtl8821ae_dm_bt_set_fw_bt_hid_info(struct ieee80211_hw *hw, bool b_enable)
+{
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 h2c_parameter[1] ={0};
+	h2c_parameter[0] = 0;
+
+	if(b_enable){
+		h2c_parameter[0] |= BIT(0);
+		rtlpcipriv->btcoexist.b_fw_coexist_all_off = false;
+	}
+	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+		("[BTCoex], Set BT HID information=0x%x\n", b_enable));
+	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+		("[BTCoex], write 0x24=0x%x\n", h2c_parameter[0]));
+
+	rtl8821ae_fill_h2c_cmd(hw, 0x24, 1, h2c_parameter);
+}
+
+void rtl8821ae_dm_bt_set_fw_bt_retry_index(struct ieee80211_hw *hw,
+													u8 retry_index)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 h2c_parameter[1] ={0};
+	h2c_parameter[0] = retry_index;
+
+	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+		("[BTCoex], Set BT Retry Index=%d\n", retry_index));
+	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+		("[BTCoex], write 0x23=0x%x\n", h2c_parameter[0]));
+
+	rtl8821ae_fill_h2c_cmd(hw, 0x23, 1, h2c_parameter);
+}
+
+void rtl8821ae_dm_bt_set_fw_wlan_act(struct ieee80211_hw *hw,
+											u8 wlan_act_hi, u8 wlan_act_lo)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 h2c_parameter_hi[1] ={0};
+	u8 h2c_parameter_lo[1] ={0};
+	h2c_parameter_hi[0] = wlan_act_hi;
+	h2c_parameter_lo[0] = wlan_act_lo;
+
+	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+		("[BTCoex], Set WLAN_ACT Hi:Lo=0x%x/0x%x\n", wlan_act_hi, wlan_act_lo));
+	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+		("[BTCoex], write 0x22=0x%x\n", h2c_parameter_hi[0]));
+	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+		("[BTCoex], write 0x11=0x%x\n", h2c_parameter_lo[0]));
+
+	/* WLAN_ACT = High duration, unit:ms */
+	rtl8821ae_fill_h2c_cmd(hw, 0x22, 1, h2c_parameter_hi);
+	/*  WLAN_ACT = Low duration, unit:3*625us */
+	rtl8821ae_fill_h2c_cmd(hw, 0x11, 1, h2c_parameter_lo);
+}
+
+void rtl8821ae_dm_bt_set_bt_dm(struct ieee80211_hw *hw, struct btdm_8821ae *p_btdm)
+{
+	struct rtl_pci_priv	*rtlpcipriv = rtl_pcipriv(hw);
+	struct rtl_priv	*rtlpriv = rtl_priv(hw);
+	struct btdm_8821ae *p_btdm_8821ae = &hal_coex_8821ae.btdm;
+	u8 i;
+
+	bool b_fw_current_inpsmode = false;
+    bool b_fw_ps_awake = true;
+
+    rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
+			                      (u8 *) (&b_fw_current_inpsmode));
+    rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FWLPS_RF_ON,
+			                      (u8 *) (&b_fw_ps_awake));
+
+	// check new setting is different with the old one,
+	// if all the same, don't do the setting again.
+	if (memcmp(p_btdm_8821ae, p_btdm, sizeof(struct btdm_8821ae)) == 0) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], the same coexist setting, return!!\n"));
+		return;
+	} else {	//save the new coexist setting
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], UPDATE TO NEW COEX SETTING!!\n"));
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], original/new bAllOff=0x%x/ 0x%x \n",
+			p_btdm_8821ae->b_all_off, p_btdm->b_all_off));
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], original/new b_agc_table_en=0x%x/ 0x%x \n",
+			p_btdm_8821ae->b_agc_table_en, p_btdm->b_agc_table_en));
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], original/new b_adc_back_off_on=0x%x/ 0x%x \n",
+			p_btdm_8821ae->b_adc_back_off_on, p_btdm->b_adc_back_off_on));
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], original/new b2_ant_hid_en=0x%x/ 0x%x \n",
+			p_btdm_8821ae->b2_ant_hid_en, p_btdm->b2_ant_hid_en));
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], original/new bLowPenaltyRateAdaptive=0x%x/ 0x%x \n",
+			p_btdm_8821ae->b_low_penalty_rate_adaptive,
+			p_btdm->b_low_penalty_rate_adaptive));
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], original/new bRfRxLpfShrink=0x%x/ 0x%x \n",
+			p_btdm_8821ae->b_rf_rx_lpf_shrink, p_btdm->b_rf_rx_lpf_shrink));
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], original/new bRejectAggrePkt=0x%x/ 0x%x \n",
+			p_btdm_8821ae->b_reject_aggre_pkt, p_btdm->b_reject_aggre_pkt));
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], original/new b_tdma_on=0x%x/ 0x%x \n",
+			p_btdm_8821ae->b_tdma_on, p_btdm->b_tdma_on));
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], original/new tdmaAnt=0x%x/ 0x%x \n",
+			p_btdm_8821ae->tdma_ant, p_btdm->tdma_ant));
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], original/new tdmaNav=0x%x/ 0x%x \n",
+			p_btdm_8821ae->tdma_nav, p_btdm->tdma_nav));
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], original/new tdma_dac_swing=0x%x/ 0x%x \n",
+			p_btdm_8821ae->tdma_dac_swing, p_btdm->tdma_dac_swing));
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], original/new fw_dac_swing_lvl=0x%x/ 0x%x \n",
+			p_btdm_8821ae->fw_dac_swing_lvl, p_btdm->fw_dac_swing_lvl));
+
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], original/new bTraTdmaOn=0x%x/ 0x%x \n",
+			p_btdm_8821ae->b_tra_tdma_on, p_btdm->b_tra_tdma_on));
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], original/new traTdmaAnt=0x%x/ 0x%x \n",
+			p_btdm_8821ae->tra_tdma_ant, p_btdm->tra_tdma_ant));
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], original/new traTdmaNav=0x%x/ 0x%x \n",
+			p_btdm_8821ae->tra_tdma_nav, p_btdm->tra_tdma_nav));
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], original/new bPsTdmaOn=0x%x/ 0x%x \n",
+			p_btdm_8821ae->b_ps_tdma_on, p_btdm->b_ps_tdma_on));
+		for(i=0; i<5; i++)
+		{
+			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+				("[BTCoex], original/new psTdmaByte[i]=0x%x/ 0x%x \n",
+				p_btdm_8821ae->ps_tdma_byte[i], p_btdm->ps_tdma_byte[i]));
+		}
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], original/new bIgnoreWlanAct=0x%x/ 0x%x \n",
+			p_btdm_8821ae->b_ignore_wlan_act, p_btdm->b_ignore_wlan_act));
+
+
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], original/new bPtaOn=0x%x/ 0x%x \n",
+			p_btdm_8821ae->b_pta_on, p_btdm->b_pta_on));
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], original/new val_0x6c0=0x%x/ 0x%x \n",
+			p_btdm_8821ae->val_0x6c0, p_btdm->val_0x6c0));
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], original/new val_0x6c8=0x%x/ 0x%x \n",
+			p_btdm_8821ae->val_0x6c8, p_btdm->val_0x6c8));
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], original/new val_0x6cc=0x%x/ 0x%x \n",
+			p_btdm_8821ae->val_0x6cc, p_btdm->val_0x6cc));
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], original/new b_sw_dac_swing_on=0x%x/ 0x%x \n",
+			p_btdm_8821ae->b_sw_dac_swing_on, p_btdm->b_sw_dac_swing_on));
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], original/new sw_dac_swing_lvl=0x%x/ 0x%x \n",
+			p_btdm_8821ae->sw_dac_swing_lvl, p_btdm->sw_dac_swing_lvl));
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], original/new wlanActHi=0x%x/ 0x%x \n",
+			p_btdm_8821ae->wlan_act_hi, p_btdm->wlan_act_hi));
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], original/new wlanActLo=0x%x/ 0x%x \n",
+			p_btdm_8821ae->wlan_act_lo, p_btdm->wlan_act_lo));
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], original/new btRetryIndex=0x%x/ 0x%x \n",
+			p_btdm_8821ae->bt_retry_index, p_btdm->bt_retry_index));
+
+		memcpy(p_btdm_8821ae, p_btdm, sizeof(struct btdm_8821ae));
+	}
+	/*
+	 * Here we only consider when Bt Operation
+	 * inquiry/paging/pairing is ON
+	 * we only need to turn off TDMA */
+
+	if (rtlpcipriv->btcoexist.b_hold_for_bt_operation) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+			("[BTCoex], set to ignore wlanAct for BT OP!!\n"));
+		rtl8821ae_dm_bt_set_fw_ignore_wlan_act(hw, true);
+		return;
+	}
+
+	if (p_btdm->b_all_off) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+			("[BTCoex], disable all coexist mechanism !!\n"));
+		rtl8821ae_btdm_coex_all_off(hw);
+		return;
+	}
+
+	rtl8821ae_dm_bt_reject_ap_aggregated_packet(hw, p_btdm->b_reject_aggre_pkt);
+
+	if(p_btdm->b_low_penalty_rate_adaptive)
+		rtl8821ae_dm_bt_set_sw_penalty_tx_rate_adaptive(hw,
+			BT_TX_RATE_ADAPTIVE_LOW_PENALTY);
+	else
+		rtl8821ae_dm_bt_set_sw_penalty_tx_rate_adaptive(hw,
+			BT_TX_RATE_ADAPTIVE_NORMAL);
+
+	if(p_btdm->b_rf_rx_lpf_shrink)
+		rtl8821ae_dm_bt_set_sw_rf_rx_lpf_corner(hw, BT_RF_RX_LPF_CORNER_SHRINK);
+	else
+		rtl8821ae_dm_bt_set_sw_rf_rx_lpf_corner(hw, BT_RF_RX_LPF_CORNER_RESUME);
+
+	if(p_btdm->b_agc_table_en)
+		rtl8821ae_dm_bt_agc_table(hw, BT_AGCTABLE_ON);
+	else
+		rtl8821ae_dm_bt_agc_table(hw, BT_AGCTABLE_OFF);
+
+	if(p_btdm->b_adc_back_off_on)
+		rtl8821ae_dm_bt_bb_back_off_level(hw, BT_BB_BACKOFF_ON);
+	else
+		rtl8821ae_dm_bt_bb_back_off_level(hw, BT_BB_BACKOFF_OFF);
+
+	rtl8821ae_dm_bt_set_fw_bt_retry_index(hw, p_btdm->bt_retry_index);
+
+	rtl8821ae_dm_bt_set_fw_dac_swing_level(hw, p_btdm->fw_dac_swing_lvl);
+	rtl8821ae_dm_bt_set_fw_wlan_act(hw, p_btdm->wlan_act_hi, p_btdm->wlan_act_lo);
+
+	rtl8821ae_dm_bt_set_coex_table(hw, p_btdm->val_0x6c0,
+		p_btdm->val_0x6c8, p_btdm->val_0x6cc);
+	rtl8821ae_dm_bt_set_hw_pta_mode(hw, p_btdm->b_pta_on);
+
+	/*
+	 * Note: There is a constraint between TDMA and 2AntHID
+	 * Only one of 2AntHid and tdma can be turn on
+	 * We should turn off those mechanisms should be turned off first
+	 * and then turn on those mechanisms should be turned on.
+	*/
+#if 1
+	if(p_btdm->b2_ant_hid_en) {
+		// turn off tdma
+		rtl8821ae_dm_bt_set_fw_tra_tdma_ctrl(hw, p_btdm->b_tra_tdma_on,
+							p_btdm->tra_tdma_ant, p_btdm->tra_tdma_nav);
+		rtl8821ae_dm_bt_set_fw_tdma_ctrl(hw, false, p_btdm->tdma_ant,
+							p_btdm->tdma_nav, p_btdm->tdma_dac_swing);
+
+		// turn off Pstdma
+		rtl8821ae_dm_bt_set_fw_ignore_wlan_act(hw, p_btdm->b_ignore_wlan_act);
+		rtl8821ae_dm_bt_set_fw_3a(hw, 0x0, 0x0, 0x0, 0x8, 0x0); 	// Antenna control by PTA, 0x870 = 0x300.
+
+		// turn on 2AntHid
+		rtl8821ae_dm_bt_set_fw_bt_hid_info(hw, true);
+		rtl8821ae_dm_bt_set_fw_2_ant_hid(hw, true, true);
+	} else if(p_btdm->b_tdma_on) {
+		// turn off 2AntHid
+		rtl8821ae_dm_bt_set_fw_bt_hid_info(hw, false);
+		rtl8821ae_dm_bt_set_fw_2_ant_hid(hw, false, false);
+
+		// turn off pstdma
+		rtl8821ae_dm_bt_set_fw_ignore_wlan_act(hw, p_btdm->b_ignore_wlan_act);
+		rtl8821ae_dm_bt_set_fw_3a(hw, 0x0, 0x0, 0x0, 0x8, 0x0); 	// Antenna control by PTA, 0x870 = 0x300.
+
+		// turn on tdma
+		rtl8821ae_dm_bt_set_fw_tra_tdma_ctrl(hw, p_btdm->b_tra_tdma_on, p_btdm->tra_tdma_ant, p_btdm->tra_tdma_nav);
+		rtl8821ae_dm_bt_set_fw_tdma_ctrl(hw, true, p_btdm->tdma_ant, p_btdm->tdma_nav, p_btdm->tdma_dac_swing);
+	} else if(p_btdm->b_ps_tdma_on) {
+		// turn off 2AntHid
+		rtl8821ae_dm_bt_set_fw_bt_hid_info(hw, false);
+		rtl8821ae_dm_bt_set_fw_2_ant_hid(hw, false, false);
+
+		// turn off tdma
+		rtl8821ae_dm_bt_set_fw_tra_tdma_ctrl(hw, p_btdm->b_tra_tdma_on, p_btdm->tra_tdma_ant, p_btdm->tra_tdma_nav);
+		rtl8821ae_dm_bt_set_fw_tdma_ctrl(hw, false, p_btdm->tdma_ant, p_btdm->tdma_nav, p_btdm->tdma_dac_swing);
+
+		// turn on pstdma
+		rtl8821ae_dm_bt_set_fw_ignore_wlan_act(hw, p_btdm->b_ignore_wlan_act);
+		rtl8821ae_dm_bt_set_fw_3a(hw,
+			p_btdm->ps_tdma_byte[0],
+			p_btdm->ps_tdma_byte[1],
+			p_btdm->ps_tdma_byte[2],
+			p_btdm->ps_tdma_byte[3],
+			p_btdm->ps_tdma_byte[4]);
+	} else {
+		// turn off 2AntHid
+		rtl8821ae_dm_bt_set_fw_bt_hid_info(hw, false);
+		rtl8821ae_dm_bt_set_fw_2_ant_hid(hw, false, false);
+
+		// turn off tdma
+		rtl8821ae_dm_bt_set_fw_tra_tdma_ctrl(hw, p_btdm->b_tra_tdma_on, p_btdm->tra_tdma_ant, p_btdm->tra_tdma_nav);
+		rtl8821ae_dm_bt_set_fw_tdma_ctrl(hw, false, p_btdm->tdma_ant, p_btdm->tdma_nav, p_btdm->tdma_dac_swing);
+
+		// turn off pstdma
+		rtl8821ae_dm_bt_set_fw_ignore_wlan_act(hw, p_btdm->b_ignore_wlan_act);
+		rtl8821ae_dm_bt_set_fw_3a(hw, 0x0, 0x0, 0x0, 0x8, 0x0); 	// Antenna control by PTA, 0x870 = 0x300.
+	}
+#else
+	if (p_btdm->b_tdma_on) {
+		if(p_btdm->b_ps_tdma_on) {
+		} else {
+			rtl8821ae_dm_bt_set_fw_3a(hw, 0x0, 0x0, 0x0, 0x8, 0x0);
+		}
+		/* Turn off 2AntHID first then turn tdma ON */
+		rtl8821ae_dm_bt_set_fw_bt_hid_info(hw, false);
+		rtl8821ae_dm_bt_set_fw_2_ant_hid(hw, false, false);
+		rtl8821ae_dm_bt_set_fw_tra_tdma_ctrl(hw, p_btdm->b_tra_tdma_on, p_btdm->tra_tdma_ant, p_btdm->tra_tdma_nav);
+		rtl8821ae_dm_bt_set_fw_tdma_ctrl(hw, true,
+			p_btdm->tdma_ant, p_btdm->tdma_nav, p_btdm->tdma_dac_swing);
+	} else {
+		/* Turn off tdma first then turn 2AntHID ON if need */
+		rtl8821ae_dm_bt_set_fw_tra_tdma_ctrl(hw, p_btdm->b_tra_tdma_on, p_btdm->tra_tdma_ant, p_btdm->tra_tdma_nav);
+		rtl8821ae_dm_bt_set_fw_tdma_ctrl(hw, false, p_btdm->tdma_ant,
+			p_btdm->tdma_nav, p_btdm->tdma_dac_swing);
+		if (p_btdm->b2_ant_hid_en) {
+			rtl8821ae_dm_bt_set_fw_bt_hid_info(hw, true);
+			rtl8821ae_dm_bt_set_fw_2_ant_hid(hw, true, true);
+		} else {
+			rtl8821ae_dm_bt_set_fw_bt_hid_info(hw, false);
+			rtl8821ae_dm_bt_set_fw_2_ant_hid(hw, false, false);
+		}
+		if(p_btdm->b_ps_tdma_on) {
+			rtl8821ae_dm_bt_set_fw_3a(hw, p_btdm->ps_tdma_byte[0], p_btdm->ps_tdma_byte[1],
+				p_btdm->ps_tdma_byte[2], p_btdm->ps_tdma_byte[3], p_btdm->ps_tdma_byte[4]);
+		} else {
+			rtl8821ae_dm_bt_set_fw_3a(hw, 0x0, 0x0, 0x0, 0x8, 0x0);
+		}
+	}
+#endif
+
+	/*
+	 * Note:
+	 * We should add delay for making sure sw DacSwing can be set sucessfully.
+	 * because of that rtl8821ae_dm_bt_set_fw_2_ant_hid() and rtl8821ae_dm_bt_set_fw_tdma_ctrl()
+	 * will overwrite the reg 0x880.
+	*/
+	mdelay(30);
+	rtl8821ae_dm_bt_set_sw_full_time_dac_swing(hw,
+		p_btdm->b_sw_dac_swing_on, p_btdm->sw_dac_swing_lvl);
+	rtl8821ae_dm_bt_set_fw_dec_bt_pwr(hw, p_btdm->b_dec_bt_pwr);
+}
+
+void rtl8821ae_dm_bt_bt_state_update_2_ant_hid(struct ieee80211_hw *hw)
+{
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], HID busy!!\n"));
+	rtlpcipriv->btcoexist.b_bt_busy = true;
+	rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_BT_IDLE;
+}
+
+void rtl8821ae_dm_bt_bt_state_update_2_ant_pan(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	bool b_idle = false;
+
+	if (hal_coex_8821ae.low_priority_tx >=
+		hal_coex_8821ae.low_priority_rx) {
+		if((hal_coex_8821ae.low_priority_tx/
+			hal_coex_8821ae.low_priority_rx) > 10) {
+			b_idle = true;
+		}
+	} else {
+		if((hal_coex_8821ae.low_priority_rx/
+			hal_coex_8821ae.low_priority_tx) > 10) {
+			b_idle = true;
+		}
+	}
+
+	if(!b_idle) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], PAN busy!!\n"));
+		rtlpcipriv->btcoexist.b_bt_busy = true;
+		rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_BT_IDLE;
+	} else {
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], PAN idle!!\n"));
+	}
+}
+
+void rtl8821ae_dm_bt_2_ant_sco_action(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct btdm_8821ae btdm8821ae;
+	u8 bt_rssi_state;
+
+	rtl8821ae_dm_bt_btdm_structure_reload(hw, &btdm8821ae);
+	btdm8821ae.b_rf_rx_lpf_shrink = true;
+	btdm8821ae.b_low_penalty_rate_adaptive = true;
+	btdm8821ae.b_reject_aggre_pkt = false;
+
+	if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("HT40\n"));
+		/*  coex table */
+		btdm8821ae.val_0x6c0 = 0x5a5aaaaa;
+		btdm8821ae.val_0x6c8 = 0xcc;
+		btdm8821ae.val_0x6cc = 0x3;
+		/* sw mechanism */
+		btdm8821ae.b_agc_table_en = false;
+		btdm8821ae.b_adc_back_off_on = true;
+		btdm8821ae.b_sw_dac_swing_on = false;
+		/* fw mechanism */
+		btdm8821ae.b_tdma_on = false;
+		btdm8821ae.tdma_dac_swing = TDMA_DAC_SWING_OFF;
+	} else {
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("HT20 or Legacy\n"));
+		bt_rssi_state
+			= rtl8821ae_dm_bt_check_coex_rssi_state(hw, 2, BT_FW_COEX_THRESH_47, 0);
+
+		/* coex table */
+		btdm8821ae.val_0x6c0 = 0x5a5aaaaa;
+		btdm8821ae.val_0x6c8 = 0xcc;
+		btdm8821ae.val_0x6cc = 0x3;
+		/* sw mechanism */
+		if ((bt_rssi_state == BT_RSSI_STATE_HIGH) ||
+			(bt_rssi_state == BT_RSSI_STATE_STAY_HIGH) ) {
+			btdm8821ae.b_agc_table_en = true;
+			btdm8821ae.b_adc_back_off_on = true;
+			btdm8821ae.b_sw_dac_swing_on = false;
+		} else {
+			btdm8821ae.b_agc_table_en = false;
+			btdm8821ae.b_adc_back_off_on = false;
+			btdm8821ae.b_sw_dac_swing_on = false;
+		}
+		/* fw mechanism */
+		btdm8821ae.b_tdma_on = false;
+		btdm8821ae.tdma_dac_swing = TDMA_DAC_SWING_OFF;
+	}
+
+	if (rtl8821ae_dm_bt_need_to_dec_bt_pwr(hw)) {
+		btdm8821ae.b_dec_bt_pwr = true;
+	}
+
+	if(rtl8821ae_dm_bt_is_coexist_state_changed(hw))
+		rtl8821ae_dm_bt_set_bt_dm(hw, &btdm8821ae);
+}
+
+void rtl8821ae_dm_bt_2_ant_hid_action(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct btdm_8821ae btdm8821ae;
+	u8 bt_rssi_state;
+
+	rtl8821ae_dm_bt_btdm_structure_reload(hw, &btdm8821ae);
+
+	btdm8821ae.b_rf_rx_lpf_shrink = true;
+	btdm8821ae.b_low_penalty_rate_adaptive = true;
+	btdm8821ae.b_reject_aggre_pkt = false;
+
+	// coex table
+	btdm8821ae.val_0x6c0 = 0x55555555;
+	btdm8821ae.val_0x6c8 = 0xffff;
+	btdm8821ae.val_0x6cc = 0x3;
+	btdm8821ae.b_ignore_wlan_act = true;
+
+	if(rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("HT40\n"));
+		// sw mechanism
+		btdm8821ae.b_agc_table_en = false;
+		btdm8821ae.b_adc_back_off_on = false;
+		btdm8821ae.b_sw_dac_swing_on = false;
+
+		// fw mechanism
+		btdm8821ae.b_ps_tdma_on = true;
+		btdm8821ae.ps_tdma_byte[0] = 0xa3;
+		btdm8821ae.ps_tdma_byte[1] = 0xf;
+		btdm8821ae.ps_tdma_byte[2] = 0xf;
+		btdm8821ae.ps_tdma_byte[3] = 0x0;
+		btdm8821ae.ps_tdma_byte[4] = 0x80;
+
+		btdm8821ae.b_tra_tdma_on = false;
+		btdm8821ae.b_tdma_on = false;
+		btdm8821ae.tdma_dac_swing = TDMA_DAC_SWING_OFF;
+		btdm8821ae.b2_ant_hid_en = false;
+	} else {
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("HT20 or Legacy\n"));
+		bt_rssi_state =
+			rtl8821ae_dm_bt_check_coex_rssi_state(hw, 2, 47, 0);
+
+		if( (bt_rssi_state == BT_RSSI_STATE_HIGH) ||
+			(bt_rssi_state == BT_RSSI_STATE_STAY_HIGH) ) {
+			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("Wifi rssi high \n"));
+			// sw mechanism
+			btdm8821ae.b_agc_table_en = false;
+			btdm8821ae.b_adc_back_off_on = false;
+			btdm8821ae.b_sw_dac_swing_on = true;
+			btdm8821ae.sw_dac_swing_lvl = 0x20;
+
+			// fw mechanism
+			btdm8821ae.b_ps_tdma_on = false;
+			btdm8821ae.b_tdma_on = false;
+			btdm8821ae.tdma_dac_swing = TDMA_DAC_SWING_OFF;
+			btdm8821ae.b2_ant_hid_en = false;
+		} else {
+			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("Wifi rssi low \n"));
+			// sw mechanism
+			btdm8821ae.b_agc_table_en = false;
+			btdm8821ae.b_adc_back_off_on = false;
+			btdm8821ae.b_sw_dac_swing_on = false;
+
+			// fw mechanism
+			btdm8821ae.b_ps_tdma_on = false;
+			btdm8821ae.b_tdma_on = false;
+			btdm8821ae.tdma_dac_swing = TDMA_DAC_SWING_OFF;
+			btdm8821ae.b2_ant_hid_en = true;
+			btdm8821ae.fw_dac_swing_lvl = 0x20;
+		}
+	}
+
+	if (rtl8821ae_dm_bt_need_to_dec_bt_pwr(hw)) {
+		btdm8821ae.b_dec_bt_pwr = true;
+	}
+
+	if (rtl8821ae_dm_bt_is_coexist_state_changed(hw)) {
+		rtl8821ae_dm_bt_set_bt_dm(hw, &btdm8821ae);
+	}
+}
+
+
+void rtl8821ae_dm_bt_2_ant_2_dp_action_no_profile(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct btdm_8821ae btdm8821ae;
+	u8 bt_rssi_state;
+
+	rtl8821ae_dm_bt_btdm_structure_reload(hw, &btdm8821ae);
+
+	btdm8821ae.b_rf_rx_lpf_shrink = true;
+	btdm8821ae.b_low_penalty_rate_adaptive = true;
+	btdm8821ae.b_reject_aggre_pkt = false;
+
+	if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("HT40\n"));
+		if (rtl8821ae_dm_bt_is_wifi_up_link(hw)) {
+			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("Wifi Uplink\n"));
+			/* coex table */
+			btdm8821ae.val_0x6c0 = 0x5a5a5a5a;
+			btdm8821ae.val_0x6c8 = 0xcccc;
+			btdm8821ae.val_0x6cc = 0x3;
+			// sw mechanism
+			btdm8821ae.b_agc_table_en = false;
+			btdm8821ae.b_adc_back_off_on = true;
+			btdm8821ae.b_sw_dac_swing_on = false;
+			// fw mechanism
+			btdm8821ae.b_tra_tdma_on = true;
+			btdm8821ae.b_tdma_on = true;
+			btdm8821ae.tdma_dac_swing = TDMA_DAC_SWING_ON;
+			btdm8821ae.b2_ant_hid_en = false;
+			//btSpec = BTHCI_GetBTCoreSpecByProf(Adapter, BT_PROFILE_A2DP);
+			//if(btSpec >= BT_SPEC_2_1_EDR)
+			{
+				btdm8821ae.wlan_act_hi = 0x10;
+				btdm8821ae.wlan_act_lo = 0x10;
+			}
+			//else
+			//{
+				//btdm8821ae.wlanActHi = 0x20;
+				//btdm8821ae.wlanActLo = 0x20;
+			//}
+			btdm8821ae.bt_retry_index = 2;
+			btdm8821ae.fw_dac_swing_lvl = 0x18;
+		} else {
+			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("Wifi Downlink\n"));
+			// coex table
+			btdm8821ae.val_0x6c0 = 0x5a5a5a5a;
+			btdm8821ae.val_0x6c8 = 0xcc;
+			btdm8821ae.val_0x6cc = 0x3;
+			// sw mechanism
+			btdm8821ae.b_agc_table_en = false;
+			btdm8821ae.b_adc_back_off_on = true;
+			btdm8821ae.b_sw_dac_swing_on = false;
+			// fw mechanism
+			btdm8821ae.b_tra_tdma_on = true;
+			btdm8821ae.b_tdma_on = true;
+			btdm8821ae.tdma_dac_swing = TDMA_DAC_SWING_ON;
+			btdm8821ae.b2_ant_hid_en = false;
+			//btSpec = BTHCI_GetBTCoreSpecByProf(Adapter, BT_PROFILE_A2DP);
+			//if(btSpec >= BT_SPEC_2_1_EDR)
+			{
+				btdm8821ae.wlan_act_hi = 0x10;
+				btdm8821ae.wlan_act_lo = 0x10;
+			}
+			//else
+			//{
+			//	btdm8821ae.wlanActHi = 0x20;
+			//	btdm8821ae.wlanActLo = 0x20;
+			//}
+			btdm8821ae.bt_retry_index = 2;
+			btdm8821ae.fw_dac_swing_lvl = 0x40;
+		}
+	} else {
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("HT20 or Legacy\n"));
+		bt_rssi_state = rtl8821ae_dm_bt_check_coex_rssi_state(hw, 2, BT_FW_COEX_THRESH_47, 0);
+
+		if(rtl8821ae_dm_bt_is_wifi_up_link(hw))
+		{
+			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("Wifi Uplink\n"));
+			// coex table
+			btdm8821ae.val_0x6c0 = 0x5a5a5a5a;
+			btdm8821ae.val_0x6c8 = 0xcccc;
+			btdm8821ae.val_0x6cc = 0x3;
+			// sw mechanism
+			if( (bt_rssi_state == BT_RSSI_STATE_HIGH) ||
+				(bt_rssi_state == BT_RSSI_STATE_STAY_HIGH) )
+			{
+				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("Wifi rssi high \n"));
+				btdm8821ae.b_agc_table_en = true;
+				btdm8821ae.b_adc_back_off_on = true;
+				btdm8821ae.b_sw_dac_swing_on = false;
+			} else {
+				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("Wifi rssi low \n"));
+				btdm8821ae.b_agc_table_en = false;
+				btdm8821ae.b_adc_back_off_on = false;
+				btdm8821ae.b_sw_dac_swing_on = false;
+			}
+			// fw mechanism
+			btdm8821ae.b_tra_tdma_on = true;
+			btdm8821ae.b_tdma_on = true;
+			btdm8821ae.tdma_dac_swing = TDMA_DAC_SWING_ON;
+			btdm8821ae.b2_ant_hid_en = false;
+			//btSpec = BTHCI_GetBTCoreSpecByProf(Adapter, BT_PROFILE_A2DP);
+			//if(btSpec >= BT_SPEC_2_1_EDR)
+			{
+				btdm8821ae.wlan_act_hi = 0x10;
+				btdm8821ae.wlan_act_lo = 0x10;
+			}
+			//else
+			//{
+				//btdm8821ae.wlanActHi = 0x20;
+				//btdm8821ae.wlanActLo = 0x20;
+			//}
+			btdm8821ae.bt_retry_index = 2;
+			btdm8821ae.fw_dac_swing_lvl = 0x18;
+		}
+		else
+		{
+			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("Wifi Downlink\n"));
+			// coex table
+			btdm8821ae.val_0x6c0 = 0x5a5a5a5a;
+			btdm8821ae.val_0x6c8 = 0xcc;
+			btdm8821ae.val_0x6cc = 0x3;
+			// sw mechanism
+			if( (bt_rssi_state == BT_RSSI_STATE_HIGH) ||
+				(bt_rssi_state == BT_RSSI_STATE_STAY_HIGH) )
+			{
+				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("Wifi rssi high \n"));
+				btdm8821ae.b_agc_table_en = true;
+				btdm8821ae.b_adc_back_off_on = true;
+				btdm8821ae.b_sw_dac_swing_on = false;
+			}
+			else
+			{
+				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("Wifi rssi low \n"));
+				btdm8821ae.b_agc_table_en = false;
+				btdm8821ae.b_adc_back_off_on = false;
+				btdm8821ae.b_sw_dac_swing_on = false;
+			}
+			// fw mechanism
+			btdm8821ae.b_tra_tdma_on = true;
+			btdm8821ae.b_tdma_on = true;
+			btdm8821ae.tdma_dac_swing = TDMA_DAC_SWING_ON;
+			btdm8821ae.b2_ant_hid_en = false;
+			//btSpec = BTHCI_GetBTCoreSpecByProf(Adapter, BT_PROFILE_A2DP);
+			//if(btSpec >= BT_SPEC_2_1_EDR)
+			{
+				btdm8821ae.wlan_act_hi = 0x10;
+				btdm8821ae.wlan_act_lo = 0x10;
+			}
+			//else
+			//{
+				//btdm8821ae.wlanActHi = 0x20;
+				//btdm8821ae.wlanActLo = 0x20;
+			//}
+			btdm8821ae.bt_retry_index = 2;
+			btdm8821ae.fw_dac_swing_lvl = 0x40;
+		}
+	}
+
+	if (rtl8821ae_dm_bt_need_to_dec_bt_pwr(hw)) {
+		btdm8821ae.b_dec_bt_pwr = true;
+	}
+
+	if (rtl8821ae_dm_bt_is_coexist_state_changed(hw)) {
+		rtl8821ae_dm_bt_set_bt_dm(hw, &btdm8821ae);
+	}
+}
+
+
+//============================================================
+// extern function start with BTDM_
+//============================================================
+u32 rtl8821ae_dm_bt_tx_rx_couter_h(struct ieee80211_hw *hw)
+{
+	u32	counters=0;
+
+	counters = hal_coex_8821ae.high_priority_tx + hal_coex_8821ae.high_priority_rx ;
+	return counters;
+}
+
+u32 rtl8821ae_dm_bt_tx_rx_couter_l(struct ieee80211_hw *hw)
+{
+	u32 counters=0;
+
+	counters = hal_coex_8821ae.low_priority_tx + hal_coex_8821ae.low_priority_rx ;
+	return counters;
+}
+
+u8 rtl8821ae_dm_bt_bt_tx_rx_counter_level(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	u32	bt_tx_rx_cnt = 0;
+	u8	bt_tx_rx_cnt_lvl = 0;
+
+	bt_tx_rx_cnt = rtl8821ae_dm_bt_tx_rx_couter_h(hw)
+				+ rtl8821ae_dm_bt_tx_rx_couter_l(hw);
+	RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+		("[BTCoex], BT TxRx Counters = %d\n", bt_tx_rx_cnt));
+
+	rtlpcipriv->btcoexist.current_state_h &= ~\
+		 (BT_COEX_STATE_BT_CNT_LEVEL_0 | BT_COEX_STATE_BT_CNT_LEVEL_1|
+		  BT_COEX_STATE_BT_CNT_LEVEL_2);
+
+	if (bt_tx_rx_cnt >= BT_TXRX_CNT_THRES_3) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], BT TxRx Counters at level 3\n"));
+		bt_tx_rx_cnt_lvl = BT_TXRX_CNT_LEVEL_3;
+		rtlpcipriv->btcoexist.current_state_h |= BT_COEX_STATE_BT_CNT_LEVEL_3;
+	} else if(bt_tx_rx_cnt >= BT_TXRX_CNT_THRES_2) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], BT TxRx Counters at level 2\n"));
+		bt_tx_rx_cnt_lvl = BT_TXRX_CNT_LEVEL_2;
+		rtlpcipriv->btcoexist.current_state_h |= BT_COEX_STATE_BT_CNT_LEVEL_2;
+	} else if(bt_tx_rx_cnt >= BT_TXRX_CNT_THRES_1) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], BT TxRx Counters at level 1\n"));
+		bt_tx_rx_cnt_lvl = BT_TXRX_CNT_LEVEL_1;
+		rtlpcipriv->btcoexist.current_state_h  |= BT_COEX_STATE_BT_CNT_LEVEL_1;
+	} else {
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], BT TxRx Counters at level 0\n"));
+		bt_tx_rx_cnt_lvl = BT_TXRX_CNT_LEVEL_0;
+		rtlpcipriv->btcoexist.current_state_h |= BT_COEX_STATE_BT_CNT_LEVEL_0;
+	}
+	return bt_tx_rx_cnt_lvl;
+}
+
+
+void rtl8821ae_dm_bt_2_ant_hid_sco_esco(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct btdm_8821ae btdm8821ae;
+
+	u8 bt_rssi_state, bt_rssi_state1;
+	u8	bt_tx_rx_cnt_lvl = 0;
+
+	rtl8821ae_dm_bt_btdm_structure_reload(hw, &btdm8821ae);
+
+
+	btdm8821ae.b_rf_rx_lpf_shrink = true;
+	btdm8821ae.b_low_penalty_rate_adaptive = true;
+	btdm8821ae.b_reject_aggre_pkt = false;
+
+	bt_tx_rx_cnt_lvl = rtl8821ae_dm_bt_bt_tx_rx_counter_level(hw);
+	RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters = %d\n", bt_tx_rx_cnt_lvl));
+
+	if(rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40)
+	{
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("HT40\n"));
+		// coex table
+		btdm8821ae.val_0x6c0 = 0x55555555;
+		btdm8821ae.val_0x6c8 = 0xffff;
+		btdm8821ae.val_0x6cc = 0x3;
+
+		// sw mechanism
+		btdm8821ae.b_agc_table_en = false;
+		btdm8821ae.b_adc_back_off_on = false;
+		btdm8821ae.b_sw_dac_swing_on = false;
+
+		// fw mechanism
+		btdm8821ae.b_ps_tdma_on = true;
+		if (bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_2) {
+			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters >= 1400\n"));
+			btdm8821ae.ps_tdma_byte[0] = 0xa3;
+			btdm8821ae.ps_tdma_byte[1] = 0x5;
+			btdm8821ae.ps_tdma_byte[2] = 0x5;
+			btdm8821ae.ps_tdma_byte[3] = 0x2;
+			btdm8821ae.ps_tdma_byte[4] = 0x80;
+		} else if(bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_1) {
+			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters >= 1200 && < 1400\n"));
+			btdm8821ae.ps_tdma_byte[0] = 0xa3;
+			btdm8821ae.ps_tdma_byte[1] = 0xa;
+			btdm8821ae.ps_tdma_byte[2] = 0xa;
+			btdm8821ae.ps_tdma_byte[3] = 0x2;
+			btdm8821ae.ps_tdma_byte[4] = 0x80;
+		} else {
+			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters < 1200\n"));
+			btdm8821ae.ps_tdma_byte[0] = 0xa3;
+			btdm8821ae.ps_tdma_byte[1] = 0xf;
+			btdm8821ae.ps_tdma_byte[2] = 0xf;
+			btdm8821ae.ps_tdma_byte[3] = 0x2;
+			btdm8821ae.ps_tdma_byte[4] = 0x80;
+		}
+	} else {
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("HT20 or Legacy\n"));
+		bt_rssi_state = rtl8821ae_dm_bt_check_coex_rssi_state(hw, 2, 47, 0);
+		bt_rssi_state1 = rtl8821ae_dm_bt_check_coex_rssi_state1(hw, 2, 27, 0);
+
+		// coex table
+		btdm8821ae.val_0x6c0 = 0x55555555;
+		btdm8821ae.val_0x6c8 = 0xffff;
+		btdm8821ae.val_0x6cc = 0x3;
+
+		// sw mechanism
+		if( (bt_rssi_state == BT_RSSI_STATE_HIGH) ||
+			(bt_rssi_state == BT_RSSI_STATE_STAY_HIGH) ) {
+			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("Wifi rssi high \n"));
+			btdm8821ae.b_agc_table_en = true;
+			btdm8821ae.b_adc_back_off_on = true;
+			btdm8821ae.b_sw_dac_swing_on = false;
+		} else {
+			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("Wifi rssi low \n"));
+			btdm8821ae.b_agc_table_en = false;
+			btdm8821ae.b_adc_back_off_on = false;
+			btdm8821ae.b_sw_dac_swing_on = false;
+		}
+
+		// fw mechanism
+		btdm8821ae.b_ps_tdma_on = true;
+		if( (bt_rssi_state1 == BT_RSSI_STATE_HIGH) ||
+			(bt_rssi_state1 == BT_RSSI_STATE_STAY_HIGH) ) {
+			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,("Wifi rssi-1 high \n"));
+			// only rssi high we need to do this,
+			// when rssi low, the value will modified by fw
+			rtl_write_byte(rtlpriv, 0x883, 0x40);
+			if(bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_2) {
+				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters >= 1400\n"));
+				btdm8821ae.ps_tdma_byte[0] = 0xa3;
+				btdm8821ae.ps_tdma_byte[1] = 0x5;
+				btdm8821ae.ps_tdma_byte[2] = 0x5;
+				btdm8821ae.ps_tdma_byte[3] = 0x83;
+				btdm8821ae.ps_tdma_byte[4] = 0x80;
+			} else if(bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_1) {
+				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters >= 1200 && < 1400\n"));
+				btdm8821ae.ps_tdma_byte[0] = 0xa3;
+				btdm8821ae.ps_tdma_byte[1] = 0xa;
+				btdm8821ae.ps_tdma_byte[2] = 0xa;
+				btdm8821ae.ps_tdma_byte[3] = 0x83;
+				btdm8821ae.ps_tdma_byte[4] = 0x80;
+			} else {
+				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters < 1200\n"));
+				btdm8821ae.ps_tdma_byte[0] = 0xa3;
+				btdm8821ae.ps_tdma_byte[1] = 0xf;
+				btdm8821ae.ps_tdma_byte[2] = 0xf;
+				btdm8821ae.ps_tdma_byte[3] = 0x83;
+				btdm8821ae.ps_tdma_byte[4] = 0x80;
+			}
+		} else {
+			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("Wifi rssi-1 low \n"));
+			if(bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_2)
+			{
+				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters >= 1400\n"));
+				btdm8821ae.ps_tdma_byte[0] = 0xa3;
+				btdm8821ae.ps_tdma_byte[1] = 0x5;
+				btdm8821ae.ps_tdma_byte[2] = 0x5;
+				btdm8821ae.ps_tdma_byte[3] = 0x2;
+				btdm8821ae.ps_tdma_byte[4] = 0x80;
+			} else if(bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_1) {
+				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters >= 1200 && < 1400\n"));
+				btdm8821ae.ps_tdma_byte[0] = 0xa3;
+				btdm8821ae.ps_tdma_byte[1] = 0xa;
+				btdm8821ae.ps_tdma_byte[2] = 0xa;
+				btdm8821ae.ps_tdma_byte[3] = 0x2;
+				btdm8821ae.ps_tdma_byte[4] = 0x80;
+			} else {
+				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters < 1200\n"));
+				btdm8821ae.ps_tdma_byte[0] = 0xa3;
+				btdm8821ae.ps_tdma_byte[1] = 0xf;
+				btdm8821ae.ps_tdma_byte[2] = 0xf;
+				btdm8821ae.ps_tdma_byte[3] = 0x2;
+				btdm8821ae.ps_tdma_byte[4] = 0x80;
+			}
+		}
+	}
+
+	if (rtl8821ae_dm_bt_need_to_dec_bt_pwr(hw)) {
+		btdm8821ae.b_dec_bt_pwr = true;
+	}
+
+	// Always ignore WlanAct if bHid|bSCOBusy|bSCOeSCO
+
+	RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+		("[BTCoex], BT btInqPageStartTime = 0x%x, btTxRxCntLvl = %d\n",
+		hal_coex_8821ae.bt_inq_page_start_time, bt_tx_rx_cnt_lvl));
+	if( (hal_coex_8821ae.bt_inq_page_start_time) ||
+		(BT_TXRX_CNT_LEVEL_3 == bt_tx_rx_cnt_lvl) ) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], Set BT inquiry / page scan 0x3a setting\n"));
+		btdm8821ae.b_ps_tdma_on = true;
+		btdm8821ae.ps_tdma_byte[0] = 0xa3;
+		btdm8821ae.ps_tdma_byte[1] = 0x5;
+		btdm8821ae.ps_tdma_byte[2] = 0x5;
+		btdm8821ae.ps_tdma_byte[3] = 0x2;
+		btdm8821ae.ps_tdma_byte[4] = 0x80;
+	}
+
+	if(rtl8821ae_dm_bt_is_coexist_state_changed(hw)) {
+		rtl8821ae_dm_bt_set_bt_dm(hw, &btdm8821ae);
+	}
+}
+
+void rtl8821ae_dm_bt_2_ant_ftp_a2dp(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct btdm_8821ae btdm8821ae;
+
+	u8 bt_rssi_state, bt_rssi_state1;
+	u32 bt_tx_rx_cnt_lvl = 0;
+
+	rtl8821ae_dm_bt_btdm_structure_reload(hw, &btdm8821ae);
+
+	btdm8821ae.b_rf_rx_lpf_shrink = true;
+	btdm8821ae.b_low_penalty_rate_adaptive = true;
+	btdm8821ae.b_reject_aggre_pkt = false;
+
+	bt_tx_rx_cnt_lvl = rtl8821ae_dm_bt_bt_tx_rx_counter_level(hw);
+
+	RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters = %d\n", bt_tx_rx_cnt_lvl));
+
+	if(rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40)
+	{
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("HT40\n"));
+		bt_rssi_state = rtl8821ae_dm_bt_check_coex_rssi_state(hw, 2, 37, 0);
+
+		// coex table
+		btdm8821ae.val_0x6c0 = 0x55555555;
+		btdm8821ae.val_0x6c8 = 0xffff;
+		btdm8821ae.val_0x6cc = 0x3;
+
+		// sw mechanism
+		btdm8821ae.b_agc_table_en = false;
+		btdm8821ae.b_adc_back_off_on = true;
+		btdm8821ae.b_sw_dac_swing_on = false;
+
+		// fw mechanism
+		btdm8821ae.b_ps_tdma_on = true;
+		if ((bt_rssi_state == BT_RSSI_STATE_HIGH) ||
+			(bt_rssi_state == BT_RSSI_STATE_STAY_HIGH) ) {
+			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("Wifi rssi high \n"));
+			if (bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_2) {
+				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters >= 1400\n"));
+				btdm8821ae.ps_tdma_byte[0] = 0xa3;
+				btdm8821ae.ps_tdma_byte[1] = 0x5;
+				btdm8821ae.ps_tdma_byte[2] = 0x5;
+				btdm8821ae.ps_tdma_byte[3] = 0x81;
+				btdm8821ae.ps_tdma_byte[4] = 0x80;
+			} else if(bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_1) {
+				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters >= 1200 && < 1400\n"));
+				btdm8821ae.ps_tdma_byte[0] = 0xa3;
+				btdm8821ae.ps_tdma_byte[1] = 0xa;
+				btdm8821ae.ps_tdma_byte[2] = 0xa;
+				btdm8821ae.ps_tdma_byte[3] = 0x81;
+				btdm8821ae.ps_tdma_byte[4] = 0x80;
+			} else {
+				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters < 1200\n"));
+				btdm8821ae.ps_tdma_byte[0] = 0xa3;
+				btdm8821ae.ps_tdma_byte[1] = 0xf;
+				btdm8821ae.ps_tdma_byte[2] = 0xf;
+				btdm8821ae.ps_tdma_byte[3] = 0x81;
+				btdm8821ae.ps_tdma_byte[4] = 0x80;
+			}
+		} else {
+			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("Wifi rssi low \n"));
+			if(bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_2) {
+				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters >= 1400\n"));
+				btdm8821ae.ps_tdma_byte[0] = 0xa3;
+				btdm8821ae.ps_tdma_byte[1] = 0x5;
+				btdm8821ae.ps_tdma_byte[2] = 0x5;
+				btdm8821ae.ps_tdma_byte[3] = 0x0;
+				btdm8821ae.ps_tdma_byte[4] = 0x80;
+			} else if(bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_1) {
+				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters >= 1200 && < 1400\n"));
+				btdm8821ae.ps_tdma_byte[0] = 0xa3;
+				btdm8821ae.ps_tdma_byte[1] = 0xa;
+				btdm8821ae.ps_tdma_byte[2] = 0xa;
+				btdm8821ae.ps_tdma_byte[3] = 0x0;
+				btdm8821ae.ps_tdma_byte[4] = 0x80;
+			} else {
+				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters < 1200\n"));
+				btdm8821ae.ps_tdma_byte[0] = 0xa3;
+				btdm8821ae.ps_tdma_byte[1] = 0xf;
+				btdm8821ae.ps_tdma_byte[2] = 0xf;
+				btdm8821ae.ps_tdma_byte[3] = 0x0;
+				btdm8821ae.ps_tdma_byte[4] = 0x80;
+			}
+		}
+	} else {
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("HT20 or Legacy\n"));
+		bt_rssi_state = rtl8821ae_dm_bt_check_coex_rssi_state(hw, 2, 47, 0);
+		bt_rssi_state1 = rtl8821ae_dm_bt_check_coex_rssi_state1(hw, 2, 27, 0);
+
+		// coex table
+		btdm8821ae.val_0x6c0 = 0x55555555;
+		btdm8821ae.val_0x6c8 = 0xffff;
+		btdm8821ae.val_0x6cc = 0x3;
+
+		// sw mechanism
+		if( (bt_rssi_state == BT_RSSI_STATE_HIGH) ||
+			(bt_rssi_state == BT_RSSI_STATE_STAY_HIGH) ) {
+			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("Wifi rssi high \n"));
+			btdm8821ae.b_agc_table_en = true;
+			btdm8821ae.b_adc_back_off_on = true;
+			btdm8821ae.b_sw_dac_swing_on = false;
+		} else {
+			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("Wifi rssi low \n"));
+			btdm8821ae.b_agc_table_en = false;
+			btdm8821ae.b_adc_back_off_on = false;
+			btdm8821ae.b_sw_dac_swing_on = false;
+		}
+
+		// fw mechanism
+		btdm8821ae.b_ps_tdma_on = true;
+		if( (bt_rssi_state1 == BT_RSSI_STATE_HIGH) ||
+			(bt_rssi_state1 == BT_RSSI_STATE_STAY_HIGH) ) {
+			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("Wifi rssi-1 high \n"));
+			// only rssi high we need to do this,
+			// when rssi low, the value will modified by fw
+			rtl_write_byte(rtlpriv, 0x883, 0x40);
+			if (bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_2) {
+				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters >= 1400\n"));
+				btdm8821ae.ps_tdma_byte[0] = 0xa3;
+				btdm8821ae.ps_tdma_byte[1] = 0x5;
+				btdm8821ae.ps_tdma_byte[2] = 0x5;
+				btdm8821ae.ps_tdma_byte[3] = 0x81;
+				btdm8821ae.ps_tdma_byte[4] = 0x80;
+			} else if(bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_1) {
+				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters >= 1200 && < 1400\n"));
+				btdm8821ae.ps_tdma_byte[0] = 0xa3;
+				btdm8821ae.ps_tdma_byte[1] = 0xa;
+				btdm8821ae.ps_tdma_byte[2] = 0xa;
+				btdm8821ae.ps_tdma_byte[3] = 0x81;
+				btdm8821ae.ps_tdma_byte[4] = 0x80;
+			} else {
+				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters < 1200\n"));
+				btdm8821ae.ps_tdma_byte[0] = 0xa3;
+				btdm8821ae.ps_tdma_byte[1] = 0xf;
+				btdm8821ae.ps_tdma_byte[2] = 0xf;
+				btdm8821ae.ps_tdma_byte[3] = 0x81;
+				btdm8821ae.ps_tdma_byte[4] = 0x80;
+			}
+		} else {
+			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("Wifi rssi-1 low \n"));
+			if(bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_2) {
+				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters >= 1400\n"));
+				btdm8821ae.ps_tdma_byte[0] = 0xa3;
+				btdm8821ae.ps_tdma_byte[1] = 0x5;
+				btdm8821ae.ps_tdma_byte[2] = 0x5;
+				btdm8821ae.ps_tdma_byte[3] = 0x0;
+				btdm8821ae.ps_tdma_byte[4] = 0x80;
+			} else if(bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_1) {
+				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters >= 1200 && < 1400\n"));
+				btdm8821ae.ps_tdma_byte[0] = 0xa3;
+				btdm8821ae.ps_tdma_byte[1] = 0xa;
+				btdm8821ae.ps_tdma_byte[2] = 0xa;
+				btdm8821ae.ps_tdma_byte[3] = 0x0;
+				btdm8821ae.ps_tdma_byte[4] = 0x80;
+			} else {
+				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters < 1200\n"));
+				btdm8821ae.ps_tdma_byte[0] = 0xa3;
+				btdm8821ae.ps_tdma_byte[1] = 0xf;
+				btdm8821ae.ps_tdma_byte[2] = 0xf;
+				btdm8821ae.ps_tdma_byte[3] = 0x0;
+				btdm8821ae.ps_tdma_byte[4] = 0x80;
+			}
+		}
+	}
+
+	if(rtl8821ae_dm_bt_need_to_dec_bt_pwr(hw)) {
+		btdm8821ae.b_dec_bt_pwr = true;
+	}
+
+	RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+	       ("[BTCoex], BT btInqPageStartTime = 0x%x, btTxRxCntLvl = %d\n",
+	        hal_coex_8821ae.bt_inq_page_start_time, bt_tx_rx_cnt_lvl));
+
+	if( (hal_coex_8821ae.bt_inq_page_start_time) ||
+		(BT_TXRX_CNT_LEVEL_3 == bt_tx_rx_cnt_lvl) )
+	{
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+				("[BTCoex], Set BT inquiry / page scan 0x3a setting\n"));
+		btdm8821ae.b_ps_tdma_on = true;
+		btdm8821ae.ps_tdma_byte[0] = 0xa3;
+		btdm8821ae.ps_tdma_byte[1] = 0x5;
+		btdm8821ae.ps_tdma_byte[2] = 0x5;
+		btdm8821ae.ps_tdma_byte[3] = 0x83;
+		btdm8821ae.ps_tdma_byte[4] = 0x80;
+	}
+
+	if(rtl8821ae_dm_bt_is_coexist_state_changed(hw)){
+		rtl8821ae_dm_bt_set_bt_dm(hw, &btdm8821ae);
+	}
+}
+
+void rtl8821ae_dm_bt_inq_page_monitor(struct ieee80211_hw *hw)
+{
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 cur_time;
+	cur_time = jiffies;
+	if (hal_coex_8821ae.b_c2h_bt_inquiry_page) {
+		//pHalData->btcoexist.halCoex8821ae.btInquiryPageCnt++;
+		// bt inquiry or page is started.
+		if(hal_coex_8821ae.bt_inq_page_start_time == 0){
+			rtlpcipriv->btcoexist.current_state  |= BT_COEX_STATE_BT_INQ_PAGE;
+			hal_coex_8821ae.bt_inq_page_start_time = cur_time;
+			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+				("[BTCoex], BT Inquiry/page is started at time : 0x%x \n",
+				hal_coex_8821ae.bt_inq_page_start_time));
+		}
+	}
+	RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+		("[BTCoex], BT Inquiry/page started time : 0x%x, cur_time : 0x%x \n",
+		hal_coex_8821ae.bt_inq_page_start_time, cur_time));
+
+	if (hal_coex_8821ae.bt_inq_page_start_time) {
+		if ((((long)cur_time - (long)hal_coex_8821ae.bt_inq_page_start_time) / HZ) >= 10) {
+			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,  ("[BTCoex], BT Inquiry/page >= 10sec!!!"));
+			hal_coex_8821ae.bt_inq_page_start_time = 0;
+			rtlpcipriv->btcoexist.current_state &=~ BT_COEX_STATE_BT_INQ_PAGE;
+		}
+	}
+
+#if 0
+	if (hal_coex_8821ae.b_c2h_bt_inquiry_page) {
+		hal_coex_8821ae.b_c2h_bt_inquiry_page++;
+		// bt inquiry or page is started.
+	} if(hal_coex_8821ae.b_c2h_bt_inquiry_page) {
+		rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_BT_INQ_PAGE;
+		if(hal_coex_8821ae.bt_inquiry_page_cnt >= 4)
+			hal_coex_8821ae.bt_inquiry_page_cnt = 0;
+		hal_coex_8821ae.bt_inquiry_page_cnt++;
+	} else {
+		rtlpcipriv->btcoexist.current_state &=~ BT_COEX_STATE_BT_INQ_PAGE;
+	}
+#endif
+}
+
+void rtl8821ae_dm_bt_reset_action_profile_state(struct ieee80211_hw *hw)
+{
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+
+	rtlpcipriv->btcoexist.current_state &= ~\
+		(BT_COEX_STATE_PROFILE_HID | BT_COEX_STATE_PROFILE_A2DP|
+		BT_COEX_STATE_PROFILE_PAN | BT_COEX_STATE_PROFILE_SCO);
+
+	rtlpcipriv->btcoexist.current_state &= ~\
+		(BT_COEX_STATE_BTINFO_COMMON | BT_COEX_STATE_BTINFO_B_HID_SCOESCO|
+		BT_COEX_STATE_BTINFO_B_FTP_A2DP);
+}
+
+void _rtl8821ae_dm_bt_coexist_2_ant(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	u8 bt_retry_cnt;
+	u8 bt_info_original;
+	RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex] Get bt info by fw!!\n"));
+
+	_rtl8821ae_dm_bt_check_wifi_state(hw);
+
+	if (hal_coex_8821ae.b_c2h_bt_info_req_sent) {
+			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+				("[BTCoex] c2h for bt_info not rcvd yet!!\n"));
+	}
+
+	bt_retry_cnt = hal_coex_8821ae.bt_retry_cnt;
+	bt_info_original = hal_coex_8821ae.c2h_bt_info_original;
+
+	// when bt inquiry or page scan, we have to set h2c 0x25
+	// ignore wlanact for continuous 4x2secs
+	rtl8821ae_dm_bt_inq_page_monitor(hw);
+	rtl8821ae_dm_bt_reset_action_profile_state(hw);
+
+	if(rtl8821ae_dm_bt_is_2_ant_common_action(hw)) {
+		rtlpcipriv->btcoexist.bt_profile_case = BT_COEX_MECH_COMMON;
+		rtlpcipriv->btcoexist.bt_profile_action= BT_COEX_MECH_COMMON;
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("Action 2-Ant common.\n"));
+	} else {
+		if( (bt_info_original & BTINFO_B_HID) ||
+			(bt_info_original & BTINFO_B_SCO_BUSY) ||
+			(bt_info_original & BTINFO_B_SCO_ESCO) ) {
+				rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_BTINFO_B_HID_SCOESCO;
+				rtlpcipriv->btcoexist.bt_profile_case = BT_COEX_MECH_HID_SCO_ESCO;
+				rtlpcipriv->btcoexist.bt_profile_action = BT_COEX_MECH_HID_SCO_ESCO;
+				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BTInfo: bHid|bSCOBusy|bSCOeSCO\n"));
+				rtl8821ae_dm_bt_2_ant_hid_sco_esco(hw);
+		} else if( (bt_info_original & BTINFO_B_FTP) ||
+				(bt_info_original & BTINFO_B_A2DP) ) {
+				rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_BTINFO_B_FTP_A2DP;
+				rtlpcipriv->btcoexist.bt_profile_case = BT_COEX_MECH_FTP_A2DP;
+				rtlpcipriv->btcoexist.bt_profile_action = BT_COEX_MECH_FTP_A2DP;
+				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("BTInfo: bFTP|bA2DP\n"));
+				rtl8821ae_dm_bt_2_ant_ftp_a2dp(hw);
+		} else {
+				rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_BTINFO_B_HID_SCOESCO;
+				rtlpcipriv->btcoexist.bt_profile_case = BT_COEX_MECH_NONE;
+				rtlpcipriv->btcoexist.bt_profile_action= BT_COEX_MECH_NONE;
+				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BTInfo: undefined case!!!!\n"));
+				rtl8821ae_dm_bt_2_ant_hid_sco_esco(hw);
+		}
+	}
+}
+
+void _rtl8821ae_dm_bt_coexist_1_ant(struct ieee80211_hw *hw)
+{
+	return;
+}
+
+void rtl8821ae_dm_bt_hw_coex_all_off_8723a(struct ieee80211_hw *hw)
+{
+	rtl8821ae_dm_bt_set_coex_table(hw, 0x5a5aaaaa, 0xcc, 0x3);
+	rtl8821ae_dm_bt_set_hw_pta_mode(hw, true);
+}
+
+void rtl8821ae_dm_bt_fw_coex_all_off_8723a(struct ieee80211_hw *hw)
+{
+	rtl8821ae_dm_bt_set_fw_ignore_wlan_act(hw, false);
+	rtl8821ae_dm_bt_set_fw_3a(hw, 0x0, 0x0, 0x0, 0x8, 0x0);
+	rtl8821ae_dm_bt_set_fw_2_ant_hid(hw, false, false);
+	rtl8821ae_dm_bt_set_fw_tra_tdma_ctrl(hw, false, TDMA_2ANT, TDMA_NAV_OFF);
+	rtl8821ae_dm_bt_set_fw_tdma_ctrl(hw, false, TDMA_2ANT,
+				TDMA_NAV_OFF, TDMA_DAC_SWING_OFF);
+	rtl8821ae_dm_bt_set_fw_dac_swing_level(hw, 0);
+	rtl8821ae_dm_bt_set_fw_bt_hid_info(hw, false);
+	rtl8821ae_dm_bt_set_fw_bt_retry_index(hw, 2);
+	rtl8821ae_dm_bt_set_fw_wlan_act(hw, 0x10, 0x10);
+	rtl8821ae_dm_bt_set_fw_dec_bt_pwr(hw, false);
+}
+
+void rtl8821ae_dm_bt_sw_coex_all_off_8723a(struct ieee80211_hw *hw)
+{
+	rtl8821ae_dm_bt_agc_table(hw, BT_AGCTABLE_OFF);
+	rtl8821ae_dm_bt_bb_back_off_level(hw, BT_BB_BACKOFF_OFF);
+	rtl8821ae_dm_bt_reject_ap_aggregated_packet(hw, false);
+
+	rtl8821ae_dm_bt_set_sw_penalty_tx_rate_adaptive(hw,
+							BT_TX_RATE_ADAPTIVE_NORMAL);
+	rtl8821ae_dm_bt_set_sw_rf_rx_lpf_corner(hw, BT_RF_RX_LPF_CORNER_RESUME);
+	rtl8821ae_dm_bt_set_sw_full_time_dac_swing(hw, false, 0xc0);
+}
+
+void rtl8821ae_dm_bt_query_bt_information(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 h2c_parameter[1] = {0};
+
+	hal_coex_8821ae.b_c2h_bt_info_req_sent = true;
+
+	h2c_parameter[0] |=  BIT(0);
+
+	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+		("Query Bt information, write 0x38=0x%x\n", h2c_parameter[0]));
+
+	rtl8821ae_fill_h2c_cmd(hw, 0x38, 1, h2c_parameter);
+}
+
+void rtl8821ae_dm_bt_bt_hw_counters_monitor(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	u32 reg_hp_tx_rx, reg_lp_tx_rx, u32_tmp;
+	u32 reg_hp_tx=0, reg_hp_rx=0, reg_lp_tx=0, reg_lp_rx=0;
+
+	reg_hp_tx_rx = REG_HIGH_PRIORITY_TXRX;
+	reg_lp_tx_rx = REG_LOW_PRIORITY_TXRX;
+
+	u32_tmp = rtl_read_dword(rtlpriv, reg_hp_tx_rx);
+	reg_hp_tx = u32_tmp & MASKLWORD;
+	reg_hp_rx = (u32_tmp & MASKHWORD)>>16;
+
+	u32_tmp = rtl_read_dword(rtlpriv, reg_lp_tx_rx);
+	reg_lp_tx = u32_tmp & MASKLWORD;
+	reg_lp_rx = (u32_tmp & MASKHWORD)>>16;
+
+	if(rtlpcipriv->btcoexist.lps_counter > 1) {
+		reg_hp_tx %= rtlpcipriv->btcoexist.lps_counter;
+		reg_hp_rx %= rtlpcipriv->btcoexist.lps_counter;
+		reg_lp_tx %= rtlpcipriv->btcoexist.lps_counter;
+		reg_lp_rx %= rtlpcipriv->btcoexist.lps_counter;
+	}
+
+	hal_coex_8821ae.high_priority_tx = reg_hp_tx;
+	hal_coex_8821ae.high_priority_rx = reg_hp_rx;
+	hal_coex_8821ae.low_priority_tx = reg_lp_tx;
+	hal_coex_8821ae.low_priority_rx = reg_lp_rx;
+
+	RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+		("High Priority Tx/Rx (reg 0x%x)=%x(%d)/%x(%d)\n",
+		reg_hp_tx_rx, reg_hp_tx, reg_hp_tx, reg_hp_rx, reg_hp_rx));
+	RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+		("Low Priority Tx/Rx (reg 0x%x)=%x(%d)/%x(%d)\n",
+		reg_lp_tx_rx, reg_lp_tx, reg_lp_tx, reg_lp_rx, reg_lp_rx));
+	rtlpcipriv->btcoexist.lps_counter = 0;
+	//rtl_write_byte(rtlpriv, 0x76e, 0xc);
+}
+
+void rtl8821ae_dm_bt_bt_enable_disable_check(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	bool bt_alife = true;
+
+	if (hal_coex_8821ae.high_priority_tx == 0 &&
+		hal_coex_8821ae.high_priority_rx == 0 &&
+		hal_coex_8821ae.low_priority_tx == 0 &&
+		hal_coex_8821ae.low_priority_rx == 0) {
+		bt_alife = false;
+	}
+	if (hal_coex_8821ae.high_priority_tx == 0xeaea &&
+		hal_coex_8821ae.high_priority_rx == 0xeaea &&
+		hal_coex_8821ae.low_priority_tx == 0xeaea &&
+		hal_coex_8821ae.low_priority_rx == 0xeaea) {
+		bt_alife = false;
+	}
+	if (hal_coex_8821ae.high_priority_tx == 0xffff &&
+		hal_coex_8821ae.high_priority_rx == 0xffff &&
+		hal_coex_8821ae.low_priority_tx == 0xffff &&
+		hal_coex_8821ae.low_priority_rx == 0xffff) {
+		bt_alife = false;
+	}
+	if (bt_alife) {
+		rtlpcipriv->btcoexist.bt_active_zero_cnt = 0;
+		rtlpcipriv->btcoexist.b_cur_bt_disabled = false;
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("8821AE BT is enabled !!\n"));
+	} else {
+		rtlpcipriv->btcoexist.bt_active_zero_cnt++;
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
+			("8821AE bt all counters=0, %d times!!\n",
+			rtlpcipriv->btcoexist.bt_active_zero_cnt));
+		if (rtlpcipriv->btcoexist.bt_active_zero_cnt >= 2) {
+			rtlpcipriv->btcoexist.b_cur_bt_disabled = true;
+			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("8821AE BT is disabled !!\n"));
+		}
+	}
+	if (rtlpcipriv->btcoexist.b_pre_bt_disabled !=
+		rtlpcipriv->btcoexist.b_cur_bt_disabled) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("8821AE BT is from %s to %s!!\n",
+			(rtlpcipriv->btcoexist.b_pre_bt_disabled ? "disabled":"enabled"),
+			(rtlpcipriv->btcoexist.b_cur_bt_disabled ? "disabled":"enabled")));
+		rtlpcipriv->btcoexist.b_pre_bt_disabled
+			= rtlpcipriv->btcoexist.b_cur_bt_disabled;
+	}
+}
+
+
+void rtl8821ae_dm_bt_coexist(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+
+	rtl8821ae_dm_bt_query_bt_information(hw);
+	rtl8821ae_dm_bt_bt_hw_counters_monitor(hw);
+	rtl8821ae_dm_bt_bt_enable_disable_check(hw);
+
+	if (rtlpcipriv->btcoexist.bt_ant_num == ANT_X2) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], 2 Ant mechanism\n"));
+		_rtl8821ae_dm_bt_coexist_2_ant(hw);
+	} else {
+		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], 1 Ant mechanism\n"));
+		_rtl8821ae_dm_bt_coexist_1_ant(hw);
+	}
+
+	if (!rtl8821ae_dm_bt_is_same_coexist_state(hw)) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+			("[BTCoex], Coexist State[bitMap] change from 0x%x%8x to 0x%x%8x\n",
+			rtlpcipriv->btcoexist.previous_state_h,
+			rtlpcipriv->btcoexist.previous_state,
+			rtlpcipriv->btcoexist.current_state_h,
+			rtlpcipriv->btcoexist.current_state));
+		rtlpcipriv->btcoexist.previous_state
+			= rtlpcipriv->btcoexist.current_state;
+		rtlpcipriv->btcoexist.previous_state_h
+			= rtlpcipriv->btcoexist.current_state_h;
+	}
+}
+
+void rtl8821ae_dm_bt_parse_bt_info(struct ieee80211_hw *hw, u8 * tmp_buf, u8 len)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	u8 bt_info;
+	u8 i;
+
+	hal_coex_8821ae.b_c2h_bt_info_req_sent = false;
+	hal_coex_8821ae.bt_retry_cnt = 0;
+	for (i = 0; i < len; i++) {
+		if (i == 0) {
+			hal_coex_8821ae.c2h_bt_info_original = tmp_buf[i];
+		} else if (i == 1) {
+			hal_coex_8821ae.bt_retry_cnt = tmp_buf[i];
+		}
+		if(i == len-1) {
+			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("0x%2x]", tmp_buf[i]));
+		} else {
+			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("0x%2x, ", tmp_buf[i]));
+		}
+	}
+	RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
+		("BT info bt_info (Data)= 0x%x\n",hal_coex_8821ae.c2h_bt_info_original));
+	bt_info = hal_coex_8821ae.c2h_bt_info_original;
+
+	if(bt_info & BIT(2)){
+		hal_coex_8821ae.b_c2h_bt_inquiry_page = true;
+	} else {
+		hal_coex_8821ae.b_c2h_bt_inquiry_page = false;
+	}
+
+	if (bt_info & BTINFO_B_CONNECTION) {
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTC2H], BTInfo: bConnect=true\n"));
+		rtlpcipriv->btcoexist.b_bt_busy = true;
+		rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_BT_IDLE;
+	} else {
+		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTC2H], BTInfo: bConnect=false\n"));
+		rtlpcipriv->btcoexist.b_bt_busy = false;
+		rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_BT_IDLE;
+	}
+}
+void rtl_8821ae_c2h_command_handle(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct c2h_evt_hdr c2h_event;
+	u8 * ptmp_buf = NULL;
+	u8 index = 0;
+	u8 u1b_tmp = 0;
+	memset(&c2h_event, 0, sizeof(c2h_event));
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_C2HEVT_MSG_NORMAL);
+	RT_TRACE(COMP_FW, DBG_DMESG,
+		("&&&&&&: REG_C2HEVT_MSG_NORMAL is 0x%x\n", u1b_tmp));
+	c2h_event.cmd_id = u1b_tmp & 0xF;
+	c2h_event.cmd_len = (u1b_tmp & 0xF0) >> 4;
+	c2h_event.cmd_seq = rtl_read_byte(rtlpriv, REG_C2HEVT_MSG_NORMAL + 1);
+	RT_TRACE(COMP_FW, DBG_DMESG, ("cmd_id: %d, cmd_len: %d, cmd_seq: %d\n",
+		c2h_event.cmd_id , c2h_event.cmd_len, c2h_event.cmd_seq));
+	u1b_tmp = rtl_read_byte(rtlpriv, 0x01AF);
+	if (u1b_tmp == C2H_EVT_HOST_CLOSE) {
+		return;
+	} else if (u1b_tmp != C2H_EVT_FW_CLOSE) {
+		rtl_write_byte(rtlpriv, 0x1AF, 0x00);
+		return;
+	}
+	ptmp_buf = (u8 *) kmalloc(c2h_event.cmd_len, GFP_KERNEL);
+	if(ptmp_buf == NULL) {
+		RT_TRACE(COMP_FW, DBG_TRACE, ("malloc cmd buf failed\n"));
+		return;
+	}
+
+	/* Read the content */
+	for (index = 0; index < c2h_event.cmd_len; index ++) {
+		ptmp_buf[index] = rtl_read_byte(rtlpriv, REG_C2HEVT_MSG_NORMAL + 2+ index);
+	}
+
+	switch(c2h_event.cmd_id) {
+		case C2H_BT_RSSI:
+			break;
+
+	case C2H_BT_OP_MODE:
+			break;
+
+	case BT_INFO:
+		RT_TRACE(COMP_FW, DBG_TRACE,
+			("BT info Byte[0] (ID) is 0x%x\n", c2h_event.cmd_id));
+		RT_TRACE(COMP_FW, DBG_TRACE,
+			("BT info Byte[1] (Seq) is 0x%x\n", c2h_event.cmd_seq));
+		RT_TRACE(COMP_FW, DBG_TRACE,
+			("BT info Byte[2] (Data)= 0x%x\n", ptmp_buf[0]));
+
+		if (rtlpriv->cfg->ops->get_btc_status()){
+			rtlpriv->btcoexist.btc_ops->btc_btinfo_notify(rtlpriv, ptmp_buf, c2h_event.cmd_len);
+		}
+		break;
+	default:
+		break;
+	}
+
+	if(ptmp_buf)
+		kfree(ptmp_buf);
+
+	rtl_write_byte(rtlpriv, 0x01AF, C2H_EVT_HOST_CLOSE);
+}
+
+
+
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/hal_btc.h b/drivers/staging/rtl8821ae/rtl8821ae/hal_btc.h
new file mode 100644
index 000000000000..a94474faca49
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/hal_btc.h
@@ -0,0 +1,160 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL8821AE_HAL_BTC_H__
+#define __RTL8821AE_HAL_BTC_H__
+
+#include "../wifi.h"
+#include "btc.h"
+#include "hal_bt_coexist.h"
+
+#define	BT_TXRX_CNT_THRES_1				1200
+#define	BT_TXRX_CNT_THRES_2				1400
+#define	BT_TXRX_CNT_THRES_3				3000
+#define	BT_TXRX_CNT_LEVEL_0				0	// < 1200
+#define	BT_TXRX_CNT_LEVEL_1				1	// >= 1200 && < 1400
+#define	BT_TXRX_CNT_LEVEL_2				2	// >= 1400
+#define	BT_TXRX_CNT_LEVEL_3				3
+
+
+
+#define	BT_COEX_DISABLE		0
+#define	BT_Q_PKT_OFF		0
+#define	BT_Q_PKT_ON			1
+
+#define	BT_TX_PWR_OFF		0
+#define	BT_TX_PWR_ON		1
+
+/* TDMA mode definition */
+#define	TDMA_2ANT			0
+#define	TDMA_1ANT			1
+#define	TDMA_NAV_OFF		0
+#define	TDMA_NAV_ON			1
+#define	TDMA_DAC_SWING_OFF	0
+#define	TDMA_DAC_SWING_ON	1
+
+/* PTA mode related definition */
+#define	BT_PTA_MODE_OFF		0
+#define	BT_PTA_MODE_ON		1
+
+/* Penalty Tx Rate Adaptive */
+#define	BT_TX_RATE_ADAPTIVE_NORMAL	0
+#define	BT_TX_RATE_ADAPTIVE_LOW_PENALTY	1
+
+/* RF Corner */
+#define	BT_RF_RX_LPF_CORNER_RESUME	0
+#define	BT_RF_RX_LPF_CORNER_SHRINK	1
+
+#define C2H_EVT_HOST_CLOSE		0x00
+#define C2H_EVT_FW_CLOSE		0xFF
+
+enum bt_traffic_mode {
+	BT_MOTOR_EXT_BE = 0x00,
+	BT_MOTOR_EXT_GUL = 0x01,
+	BT_MOTOR_EXT_GUB = 0x02,
+	BT_MOTOR_EXT_GULB = 0x03
+};
+
+enum bt_traffic_mode_profile {
+	BT_PROFILE_NONE,
+	BT_PROFILE_A2DP,
+	BT_PROFILE_PAN,
+	BT_PROFILE_HID,
+	BT_PROFILE_SCO
+};
+
+enum hci_ext_bt_operation {
+	HCI_BT_OP_NONE = 0x0,
+	HCI_BT_OP_INQUIRE_START	= 0x1,
+	HCI_BT_OP_INQUIRE_FINISH = 0x2,
+	HCI_BT_OP_PAGING_START = 0x3,
+	HCI_BT_OP_PAGING_SUCCESS = 0x4,
+	HCI_BT_OP_PAGING_UNSUCCESS = 0x5,
+	HCI_BT_OP_PAIRING_START = 0x6,
+	HCI_BT_OP_PAIRING_FINISH = 0x7,
+	HCI_BT_OP_BT_DEV_ENABLE = 0x8,
+	HCI_BT_OP_BT_DEV_DISABLE = 0x9,
+	HCI_BT_OP_MAX,
+};
+
+enum bt_spec {
+	BT_SPEC_1_0_b = 0x00,
+	BT_SPEC_1_1 = 0x01,
+	BT_SPEC_1_2 = 0x02,
+	BT_SPEC_2_0_EDR = 0x03,
+	BT_SPEC_2_1_EDR = 0x04,
+	BT_SPEC_3_0_HS = 0x05,
+	BT_SPEC_4_0 = 0x06
+};
+
+struct c2h_evt_hdr {
+        u8 cmd_id;
+        u8 cmd_len;
+        u8 cmd_seq;
+};
+
+enum bt_state{
+	BT_INFO_STATE_DISABLED = 0,
+	BT_INFO_STATE_NO_CONNECTION = 1,
+	BT_INFO_STATE_CONNECT_IDLE = 2,
+	BT_INFO_STATE_INQ_OR_PAG = 3,
+	BT_INFO_STATE_ACL_ONLY_BUSY = 4,
+	BT_INFO_STATE_SCO_ONLY_BUSY = 5,
+	BT_INFO_STATE_ACL_SCO_BUSY = 6,
+	BT_INFO_STATE_HID_BUSY = 7,
+	BT_INFO_STATE_HID_SCO_BUSY = 8,
+	BT_INFO_STATE_MAX = 7
+};
+
+enum rtl8723be_c2h_evt {
+	C2H_DBG = 0,
+	C2H_TSF = 1,
+	C2H_AP_RPT_RSP = 2,
+	C2H_CCX_TX_RPT = 3,	// The FW notify the report of the specific tx packet.
+	C2H_BT_RSSI = 4,
+	C2H_BT_OP_MODE = 5,
+	C2H_HW_INFO_EXCH = 10,
+	C2H_C2H_H2C_TEST = 11,
+	BT_INFO = 9,
+	MAX_C2HEVENT
+};
+
+
+
+void rtl8821ae_dm_bt_fw_coex_all_off_8723a(struct ieee80211_hw *hw);
+void rtl8821ae_dm_bt_sw_coex_all_off_8723a(struct ieee80211_hw *hw);
+void rtl8821ae_dm_bt_hw_coex_all_off_8723a(struct ieee80211_hw *hw);
+void rtl8821ae_dm_bt_coexist(struct ieee80211_hw *hw);
+void rtl8821ae_dm_bt_set_bt_dm(struct ieee80211_hw *hw, struct btdm_8821ae *p_btdm);
+void rtl_8821ae_c2h_command_handle(struct ieee80211_hw * hw);
+void rtl_8821ae_bt_wifi_media_status_notify(struct ieee80211_hw * hw, bool mstatus);
+void rtl8821ae_dm_bt_turn_off_bt_coexist_before_enter_lps(struct ieee80211_hw *hw);
+
+
+
+#endif
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/hw.c b/drivers/staging/rtl8821ae/rtl8821ae/hw.c
new file mode 100644
index 000000000000..5ed7a114c56b
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/hw.c
@@ -0,0 +1,3346 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../efuse.h"
+#include "../base.h"
+#include "../regd.h"
+#include "../cam.h"
+#include "../ps.h"
+#include "../pci.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "dm.h"
+#include "fw.h"
+#include "led.h"
+#include "hw.h"
+#include "pwrseqcmd.h"
+#include "pwrseq.h"
+#include "btc.h"
+#include "../btcoexist/rtl_btc.h"
+
+#define LLT_CONFIG	5
+
+static void _rtl8821ae_return_beacon_queue_skb(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[BEACON_QUEUE];
+
+	while (skb_queue_len(&ring->queue)) {
+		struct rtl_tx_desc *entry = &ring->desc[ring->idx];
+		struct sk_buff *skb = __skb_dequeue(&ring->queue);
+
+		pci_unmap_single(rtlpci->pdev,
+				 le32_to_cpu(rtlpriv->cfg->ops->get_desc(
+				 (u8 *) entry, true, HW_DESC_TXBUFF_ADDR)),
+				 skb->len, PCI_DMA_TODEVICE);
+		kfree_skb(skb);
+		ring->idx = (ring->idx + 1) % ring->entries;
+	}
+
+}
+
+static void _rtl8821ae_set_bcn_ctrl_reg(struct ieee80211_hw *hw,
+				      u8 set_bits, u8 clear_bits)
+{
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpci->reg_bcn_ctrl_val |= set_bits;
+	rtlpci->reg_bcn_ctrl_val &= ~clear_bits;
+
+	rtl_write_byte(rtlpriv, REG_BCN_CTRL, (u8) rtlpci->reg_bcn_ctrl_val);
+}
+
+void _rtl8821ae_stop_tx_beacon(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmp1byte;
+
+	tmp1byte = rtl_read_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2);
+	rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2, tmp1byte & (~BIT(6)));
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 1, 0x64);
+	tmp1byte = rtl_read_byte(rtlpriv, REG_TBTT_PROHIBIT + 2);
+	tmp1byte &= ~(BIT(0));
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 2, tmp1byte);
+}
+
+void _rtl8821ae_resume_tx_beacon(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmp1byte;
+
+	tmp1byte = rtl_read_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2);
+	rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2, tmp1byte | BIT(6));
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 1, 0xff);
+	tmp1byte = rtl_read_byte(rtlpriv, REG_TBTT_PROHIBIT + 2);
+	tmp1byte |= BIT(0);
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 2, tmp1byte);
+}
+
+static void _rtl8821ae_enable_bcn_sub_func(struct ieee80211_hw *hw)
+{
+	_rtl8821ae_set_bcn_ctrl_reg(hw, 0, BIT(1));
+}
+
+static void _rtl8821ae_disable_bcn_sub_func(struct ieee80211_hw *hw)
+{
+	_rtl8821ae_set_bcn_ctrl_reg(hw, BIT(1), 0);
+}
+
+static void _rtl8821ae_set_fw_clock_on(struct ieee80211_hw *hw,
+	u8 rpwm_val, bool b_need_turn_off_ckk)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool b_support_remote_wake_up;
+	u32 count = 0,isr_regaddr,content;
+	bool b_schedule_timer = b_need_turn_off_ckk;
+	rtlpriv->cfg->ops->get_hw_reg(hw, HAL_DEF_WOWLAN,
+					(u8 *) (&b_support_remote_wake_up));
+
+	if (!rtlhal->bfw_ready)
+		return;
+	if (!rtlpriv->psc.b_fw_current_inpsmode)
+		return;
+
+	while (1) {
+		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+		if (rtlhal->bfw_clk_change_in_progress) {
+			while (rtlhal->bfw_clk_change_in_progress) {
+				spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+				count++;
+				udelay(100);
+				if (count > 1000)
+					return;
+				spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+			}
+			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+		} else {
+			rtlhal->bfw_clk_change_in_progress = false;
+			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+		}
+	}
+
+	if (IS_IN_LOW_POWER_STATE_8821AE(rtlhal->fw_ps_state)) {
+		rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_SET_RPWM,
+					(u8 *) (&rpwm_val));
+		if (FW_PS_IS_ACK(rpwm_val)) {
+			isr_regaddr = REG_HISR;
+			content = rtl_read_dword(rtlpriv, isr_regaddr);
+			while (!(content & IMR_CPWM) && (count < 500)) {
+				udelay(50);
+				count++;
+				content = rtl_read_dword(rtlpriv, isr_regaddr);
+			}
+
+			if (content & IMR_CPWM) {
+			rtl_write_word(rtlpriv,isr_regaddr, 0x0100);
+			rtlhal->fw_ps_state = FW_PS_STATE_RF_ON_8821AE;
+			RT_TRACE(COMP_POWER, DBG_LOUD, ("Receive CPWM INT!!! Set pHalData->FwPSState = %X\n", rtlhal->fw_ps_state));
+			}
+		}
+
+		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+		rtlhal->bfw_clk_change_in_progress = false;
+		spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+		if (b_schedule_timer) {
+			mod_timer(&rtlpriv->works.fw_clockoff_timer,
+				  jiffies + MSECS(10));
+		}
+
+	} else  {
+		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+		rtlhal->bfw_clk_change_in_progress = false;
+		spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+	}
+
+
+}
+
+static void _rtl8821ae_set_fw_clock_off(struct ieee80211_hw *hw,
+	u8 rpwm_val)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl8192_tx_ring *ring;
+	enum rf_pwrstate rtstate;
+	bool b_schedule_timer = false;
+	u8 queue;
+
+	if (!rtlhal->bfw_ready)
+		return;
+	if (!rtlpriv->psc.b_fw_current_inpsmode)
+		return;
+	if (!rtlhal->ballow_sw_to_change_hwclc)
+		return;
+	rtlpriv->cfg->ops->get_hw_reg(hw,HW_VAR_RF_STATE,(u8 *)(&rtstate));
+	if (rtstate == ERFOFF ||rtlpriv->psc.inactive_pwrstate ==ERFOFF)
+		return;
+
+	for (queue = 0; queue < RTL_PCI_MAX_TX_QUEUE_COUNT; queue++) {
+		ring = &rtlpci->tx_ring[queue];
+		if (skb_queue_len(&ring->queue)) {
+			b_schedule_timer = true;
+			break;
+		}
+	}
+
+	if (b_schedule_timer) {
+		mod_timer(&rtlpriv->works.fw_clockoff_timer,
+					  jiffies + MSECS(10));
+		return;
+	}
+
+	if (FW_PS_STATE(rtlhal->fw_ps_state) != FW_PS_STATE_RF_OFF_LOW_PWR_8821AE) {
+		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+		if (!rtlhal->bfw_clk_change_in_progress) {
+			rtlhal->bfw_clk_change_in_progress = true;
+			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+			rtlhal->fw_ps_state = FW_PS_STATE(rpwm_val);
+			rtl_write_word(rtlpriv, REG_HISR, 0x0100);
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
+				(u8 *) (&rpwm_val));
+			spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+			rtlhal->bfw_clk_change_in_progress = false;
+			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+		} else {
+			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+			mod_timer(&rtlpriv->works.fw_clockoff_timer,
+					  jiffies + MSECS(10));
+		}
+	}
+
+}
+
+static void _rtl8821ae_set_fw_ps_rf_on(struct ieee80211_hw *hw)
+{
+	u8 rpwm_val = 0;
+	rpwm_val |= (FW_PS_STATE_RF_OFF_8821AE | FW_PS_ACK);
+	_rtl8821ae_set_fw_clock_on(hw, rpwm_val, true);
+}
+
+static void _rtl8821ae_fwlps_leave(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool b_fw_current_inps = false;
+	u8 rpwm_val = 0,fw_pwrmode = FW_PS_ACTIVE_MODE;
+
+	if (ppsc->b_low_power_enable){
+		rpwm_val = (FW_PS_STATE_ALL_ON_8821AE|FW_PS_ACK);/* RF on */
+		_rtl8821ae_set_fw_clock_on(hw, rpwm_val, false);
+		rtlhal->ballow_sw_to_change_hwclc = false;
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_PWRMODE,
+				(u8 *) (&fw_pwrmode));
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
+				(u8 *) (&b_fw_current_inps));
+	} else {
+		rpwm_val = FW_PS_STATE_ALL_ON_8821AE;	/* RF on */
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
+				(u8 *) (&rpwm_val));
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_PWRMODE,
+				(u8 *) (&fw_pwrmode));
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
+				(u8 *) (&b_fw_current_inps));
+	}
+
+}
+
+static void _rtl8821ae_fwlps_enter(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool b_fw_current_inps = true;
+	u8 rpwm_val;
+
+	if (ppsc->b_low_power_enable){
+		rpwm_val = FW_PS_STATE_RF_OFF_LOW_PWR_8821AE;	/* RF off */
+		rtlpriv->cfg->ops->set_hw_reg(hw,
+				HW_VAR_FW_PSMODE_STATUS,
+				(u8 *) (&b_fw_current_inps));
+		rtlpriv->cfg->ops->set_hw_reg(hw,
+				HW_VAR_H2C_FW_PWRMODE,
+				(u8 *) (&ppsc->fwctrl_psmode));
+		rtlhal->ballow_sw_to_change_hwclc = true;
+		_rtl8821ae_set_fw_clock_off(hw, rpwm_val);
+
+
+	} else {
+		rpwm_val = FW_PS_STATE_RF_OFF_8821AE;	/* RF off */
+		rtlpriv->cfg->ops->set_hw_reg(hw,
+				HW_VAR_FW_PSMODE_STATUS,
+				(u8 *) (&b_fw_current_inps));
+		rtlpriv->cfg->ops->set_hw_reg(hw,
+				HW_VAR_H2C_FW_PWRMODE,
+				(u8 *) (&ppsc->fwctrl_psmode));
+		rtlpriv->cfg->ops->set_hw_reg(hw,
+				HW_VAR_SET_RPWM,
+				(u8 *) (&rpwm_val));
+	}
+
+}
+
+void rtl8821ae_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+
+	switch (variable) {
+	case HW_VAR_ETHER_ADDR:
+		*((u32 *)(val)) = rtl_read_dword(rtlpriv, REG_MACID);
+		*((u16 *)(val+4)) = rtl_read_word(rtlpriv, REG_MACID + 4);
+		break;
+	case HW_VAR_BSSID:
+		*((u32 *)(val)) = rtl_read_dword(rtlpriv, REG_BSSID);
+		*((u16 *)(val+4)) = rtl_read_word(rtlpriv, REG_BSSID+4);
+		break;
+	case HW_VAR_MEDIA_STATUS:
+		val[0] = rtl_read_byte(rtlpriv, REG_CR+2) & 0x3;
+		break;
+	case HW_VAR_SLOT_TIME:
+		*((u8 *)(val)) = mac->slot_time;
+		break;
+	case HW_VAR_BEACON_INTERVAL:
+		*((u16 *)(val)) = rtl_read_word(rtlpriv, REG_BCN_INTERVAL);
+		break;
+	case HW_VAR_ATIM_WINDOW:
+		*((u16 *)(val)) =  rtl_read_word(rtlpriv, REG_ATIMWND);
+		break;
+	case HW_VAR_RCR:
+		*((u32 *) (val)) = rtlpci->receive_config;
+		break;
+	case HW_VAR_RF_STATE:
+		*((enum rf_pwrstate *)(val)) = ppsc->rfpwr_state;
+		break;
+	case HW_VAR_FWLPS_RF_ON:{
+			enum rf_pwrstate rfState;
+			u32 val_rcr;
+
+			rtlpriv->cfg->ops->get_hw_reg(hw,
+						      HW_VAR_RF_STATE,
+						      (u8 *) (&rfState));
+			if (rfState == ERFOFF) {
+				*((bool *) (val)) = true;
+			} else {
+				val_rcr = rtl_read_dword(rtlpriv, REG_RCR);
+				val_rcr &= 0x00070000;
+				if (val_rcr)
+					*((bool *) (val)) = false;
+				else
+					*((bool *) (val)) = true;
+			}
+			break;
+		}
+	case HW_VAR_FW_PSMODE_STATUS:
+		*((bool *) (val)) = ppsc->b_fw_current_inpsmode;
+		break;
+	case HW_VAR_CORRECT_TSF:{
+			u64 tsf;
+			u32 *ptsf_low = (u32 *) & tsf;
+			u32 *ptsf_high = ((u32 *) & tsf) + 1;
+
+			*ptsf_high = rtl_read_dword(rtlpriv, (REG_TSFTR + 4));
+			*ptsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
+
+			*((u64 *) (val)) = tsf;
+
+			break;
+		}
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("switch case not process %x\n",variable));
+		break;
+	}
+}
+
+
+void rtl8821ae_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 idx;
+
+	switch (variable) {
+	case HW_VAR_ETHER_ADDR:{
+			for (idx = 0; idx < ETH_ALEN; idx++) {
+				rtl_write_byte(rtlpriv, (REG_MACID + idx),
+					       val[idx]);
+			}
+			break;
+		}
+	case HW_VAR_BASIC_RATE:{
+			u16 b_rate_cfg = ((u16 *) val)[0];
+			u8 rate_index = 0;
+			b_rate_cfg = b_rate_cfg & 0x15f;
+			b_rate_cfg |= 0x01;
+			rtl_write_byte(rtlpriv, REG_RRSR, b_rate_cfg & 0xff);
+			rtl_write_byte(rtlpriv, REG_RRSR + 1,
+				       (b_rate_cfg >> 8) & 0xff);
+			while (b_rate_cfg > 0x1) {
+				b_rate_cfg = (b_rate_cfg >> 1);
+				rate_index++;
+			}
+			rtl_write_byte(rtlpriv, REG_INIRTS_RATE_SEL,
+				       rate_index);
+			break;
+		}
+	case HW_VAR_BSSID:{
+			for (idx = 0; idx < ETH_ALEN; idx++) {
+				rtl_write_byte(rtlpriv, (REG_BSSID + idx),
+					       val[idx]);
+			}
+			break;
+		}
+	case HW_VAR_SIFS:{
+			rtl_write_byte(rtlpriv, REG_SIFS_CTX + 1, val[0]);
+			rtl_write_byte(rtlpriv, REG_SIFS_TRX + 1, val[1]);
+
+			rtl_write_byte(rtlpriv, REG_SPEC_SIFS + 1, val[0]);
+			rtl_write_byte(rtlpriv, REG_MAC_SPEC_SIFS + 1, val[0]);
+
+			if (!mac->ht_enable)
+				rtl_write_word(rtlpriv, REG_RESP_SIFS_OFDM,
+					       0x0e0e);
+			else
+				rtl_write_word(rtlpriv, REG_RESP_SIFS_OFDM,
+					       *((u16 *) val));
+			break;
+		}
+	case HW_VAR_SLOT_TIME:{
+			u8 e_aci;
+
+			RT_TRACE(COMP_MLME, DBG_LOUD,
+				 ("HW_VAR_SLOT_TIME %x\n", val[0]));
+
+			rtl_write_byte(rtlpriv, REG_SLOT, val[0]);
+
+			for (e_aci = 0; e_aci < AC_MAX; e_aci++) {
+				rtlpriv->cfg->ops->set_hw_reg(hw,
+							      HW_VAR_AC_PARAM,
+							      (u8 *) (&e_aci));
+			}
+			break;
+		}
+	case HW_VAR_ACK_PREAMBLE:{
+			u8 reg_tmp;
+			u8 short_preamble = (bool) (*(u8 *) val);
+			reg_tmp = rtl_read_byte(rtlpriv, REG_TRXPTCL_CTL+2);
+			if (short_preamble){
+				reg_tmp |= BIT(1);
+				rtl_write_byte(rtlpriv, REG_TRXPTCL_CTL + 2, reg_tmp);
+			} else {
+				reg_tmp &= (~BIT(1));
+				rtl_write_byte(rtlpriv, REG_TRXPTCL_CTL + 2, reg_tmp);
+			}
+			break;
+		}
+	case HW_VAR_WPA_CONFIG:
+		rtl_write_byte(rtlpriv, REG_SECCFG, *((u8 *) val));
+		break;
+	case HW_VAR_AMPDU_MIN_SPACE:{
+			u8 min_spacing_to_set;
+			u8 sec_min_space;
+
+			min_spacing_to_set = *((u8 *) val);
+			if (min_spacing_to_set <= 7) {
+				sec_min_space = 0;
+
+				if (min_spacing_to_set < sec_min_space)
+					min_spacing_to_set = sec_min_space;
+
+				mac->min_space_cfg = ((mac->min_space_cfg &
+						       0xf8) |
+						      min_spacing_to_set);
+
+				*val = min_spacing_to_set;
+
+				RT_TRACE(COMP_MLME, DBG_LOUD,
+					 ("Set HW_VAR_AMPDU_MIN_SPACE: %#x\n",
+					  mac->min_space_cfg));
+
+				rtl_write_byte(rtlpriv, REG_AMPDU_MIN_SPACE,
+					       mac->min_space_cfg);
+			}
+			break;
+		}
+	case HW_VAR_SHORTGI_DENSITY:{
+			u8 density_to_set;
+
+			density_to_set = *((u8 *) val);
+			mac->min_space_cfg |= (density_to_set << 3);
+
+			RT_TRACE(COMP_MLME, DBG_LOUD,
+				 ("Set HW_VAR_SHORTGI_DENSITY: %#x\n",
+				  mac->min_space_cfg));
+
+			rtl_write_byte(rtlpriv, REG_AMPDU_MIN_SPACE,
+				       mac->min_space_cfg);
+
+			break;
+		}
+	case HW_VAR_AMPDU_FACTOR:{
+			u32	ampdu_len =  (*((u8 *)val));
+			if(rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+				if(ampdu_len < VHT_AGG_SIZE_128K)
+					ampdu_len = (0x2000 << (*((u8 *)val))) -1;
+				else
+					ampdu_len = 0x1ffff;
+			} else if(rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+				if(ampdu_len < HT_AGG_SIZE_64K)
+					ampdu_len = (0x2000 << (*((u8 *)val))) -1;
+				else
+					ampdu_len = 0xffff;
+			}
+			ampdu_len |= BIT(31);
+
+			rtl_write_dword(rtlpriv,
+				REG_AMPDU_MAX_LENGTH_8812, ampdu_len);
+			break;
+		}
+	case HW_VAR_AC_PARAM:{
+			u8 e_aci = *((u8 *) val);
+			rtl8821ae_dm_init_edca_turbo(hw);
+
+			if (rtlpci->acm_method != eAcmWay2_SW)
+				rtlpriv->cfg->ops->set_hw_reg(hw,
+							      HW_VAR_ACM_CTRL,
+							      (u8 *) (&e_aci));
+			break;
+		}
+	case HW_VAR_ACM_CTRL:{
+			u8 e_aci = *((u8 *) val);
+			union aci_aifsn *p_aci_aifsn =
+			    (union aci_aifsn *)(&(mac->ac[0].aifs));
+			u8 acm = p_aci_aifsn->f.acm;
+			u8 acm_ctrl = rtl_read_byte(rtlpriv, REG_ACMHWCTRL);
+
+			acm_ctrl =
+			    acm_ctrl | ((rtlpci->acm_method == 2) ? 0x0 : 0x1);
+
+			if (acm) {
+				switch (e_aci) {
+				case AC0_BE:
+					acm_ctrl |= AcmHw_BeqEn;
+					break;
+				case AC2_VI:
+					acm_ctrl |= AcmHw_ViqEn;
+					break;
+				case AC3_VO:
+					acm_ctrl |= AcmHw_VoqEn;
+					break;
+				default:
+					RT_TRACE(COMP_ERR, DBG_WARNING,
+						 ("HW_VAR_ACM_CTRL acm set "
+						  "failed: eACI is %d\n", acm));
+					break;
+				}
+			} else {
+				switch (e_aci) {
+				case AC0_BE:
+					acm_ctrl &= (~AcmHw_BeqEn);
+					break;
+				case AC2_VI:
+					acm_ctrl &= (~AcmHw_ViqEn);
+					break;
+				case AC3_VO:
+					acm_ctrl &= (~AcmHw_BeqEn);
+					break;
+				default:
+					RT_TRACE(COMP_ERR, DBG_EMERG,
+						 ("switch case not process \n"));
+					break;
+				}
+			}
+
+			RT_TRACE(COMP_QOS, DBG_TRACE,
+				 ("SetHwReg8190pci(): [HW_VAR_ACM_CTRL] "
+				  "Write 0x%X\n", acm_ctrl));
+			rtl_write_byte(rtlpriv, REG_ACMHWCTRL, acm_ctrl);
+			break;
+		}
+	case HW_VAR_RCR:{
+			rtl_write_dword(rtlpriv, REG_RCR, ((u32 *) (val))[0]);
+			rtlpci->receive_config = ((u32 *) (val))[0];
+			break;
+		}
+	case HW_VAR_RETRY_LIMIT:{
+			u8 retry_limit = ((u8 *) (val))[0];
+
+			rtl_write_word(rtlpriv, REG_RL,
+				       retry_limit << RETRY_LIMIT_SHORT_SHIFT |
+				       retry_limit << RETRY_LIMIT_LONG_SHIFT);
+			break;
+		}
+	case HW_VAR_DUAL_TSF_RST:
+		rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, (BIT(0) | BIT(1)));
+		break;
+	case HW_VAR_EFUSE_BYTES:
+		rtlefuse->efuse_usedbytes = *((u16 *) val);
+		break;
+	case HW_VAR_EFUSE_USAGE:
+		rtlefuse->efuse_usedpercentage = *((u8 *) val);
+		break;
+	case HW_VAR_IO_CMD:
+		rtl8821ae_phy_set_io_cmd(hw, (*(enum io_type *)val));
+		break;
+	case HW_VAR_SET_RPWM:{
+			u8 rpwm_val;
+
+			rpwm_val = rtl_read_byte(rtlpriv, REG_PCIE_HRPWM);
+			udelay(1);
+
+			if (rpwm_val & BIT(7)) {
+				rtl_write_byte(rtlpriv, REG_PCIE_HRPWM,
+					       (*(u8 *) val));
+			} else {
+				rtl_write_byte(rtlpriv, REG_PCIE_HRPWM,
+					       ((*(u8 *) val) | BIT(7)));
+			}
+
+			break;
+		}
+	case HW_VAR_H2C_FW_PWRMODE:{
+			rtl8821ae_set_fw_pwrmode_cmd(hw, (*(u8 *) val));
+			break;
+		}
+	case HW_VAR_FW_PSMODE_STATUS:
+		ppsc->b_fw_current_inpsmode = *((bool *) val);
+		break;
+
+	case HW_VAR_RESUME_CLK_ON:
+		_rtl8821ae_set_fw_ps_rf_on(hw);
+		break;
+
+	case HW_VAR_FW_LPS_ACTION:{
+		bool b_enter_fwlps = *((bool *) val);
+
+		if (b_enter_fwlps)
+			_rtl8821ae_fwlps_enter(hw);
+		 else
+			_rtl8821ae_fwlps_leave(hw);
+
+		 break;
+		}
+
+	case HW_VAR_H2C_FW_JOINBSSRPT:{
+			u8 mstatus = (*(u8 *) val);
+			u8 tmp_regcr, tmp_reg422,bcnvalid_reg;
+			u8 count = 0, dlbcn_count = 0;
+			bool b_recover = false;
+
+			if (mstatus == RT_MEDIA_CONNECT) {
+				rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AID,
+							      NULL);
+
+				tmp_regcr = rtl_read_byte(rtlpriv, REG_CR + 1);
+				rtl_write_byte(rtlpriv, REG_CR + 1,
+					       (tmp_regcr | BIT(0)));
+
+				_rtl8821ae_set_bcn_ctrl_reg(hw, 0, BIT(3));
+				_rtl8821ae_set_bcn_ctrl_reg(hw, BIT(4), 0);
+
+				tmp_reg422 =
+				    rtl_read_byte(rtlpriv,
+						  REG_FWHW_TXQ_CTRL + 2);
+				rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2,
+					       tmp_reg422 & (~BIT(6)));
+				if (tmp_reg422 & BIT(6))
+					b_recover = true;
+
+				do {
+					bcnvalid_reg = rtl_read_byte(rtlpriv, REG_TDECTRL+2);
+					rtl_write_byte(rtlpriv, REG_TDECTRL+2,(bcnvalid_reg | BIT(0)));
+					_rtl8821ae_return_beacon_queue_skb(hw);
+
+					if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+						rtl8812ae_set_fw_rsvdpagepkt(hw, 0);
+					else
+						rtl8821ae_set_fw_rsvdpagepkt(hw, 0);
+					bcnvalid_reg = rtl_read_byte(rtlpriv, REG_TDECTRL+2);
+					count = 0;
+					while (!(bcnvalid_reg & BIT(0)) && count <20){
+						count++;
+						udelay(10);
+						bcnvalid_reg = rtl_read_byte(rtlpriv, REG_TDECTRL+2);
+					}
+					dlbcn_count++;
+				} while (!(bcnvalid_reg & BIT(0)) && dlbcn_count <5);
+
+				if (bcnvalid_reg & BIT(0))
+					rtl_write_byte(rtlpriv, REG_TDECTRL+2, BIT(0));
+
+				_rtl8821ae_set_bcn_ctrl_reg(hw, BIT(3), 0);
+				_rtl8821ae_set_bcn_ctrl_reg(hw, 0, BIT(4));
+
+				if (b_recover) {
+					rtl_write_byte(rtlpriv,
+						       REG_FWHW_TXQ_CTRL + 2,
+						       tmp_reg422);
+				}
+
+				rtl_write_byte(rtlpriv, REG_CR + 1,
+					       (tmp_regcr & ~(BIT(0))));
+			}
+			rtl8821ae_set_fw_joinbss_report_cmd(hw, (*(u8 *) val));
+
+			break;
+		}
+	case HW_VAR_H2C_FW_P2P_PS_OFFLOAD:{
+		rtl8821ae_set_p2p_ps_offload_cmd(hw, (*(u8 *) val));
+		break;
+	}
+
+	case HW_VAR_AID:{
+			u16 u2btmp;
+			u2btmp = rtl_read_word(rtlpriv, REG_BCN_PSR_RPT);
+			u2btmp &= 0xC000;
+			rtl_write_word(rtlpriv, REG_BCN_PSR_RPT, (u2btmp |
+						mac->assoc_id));
+
+			break;
+		}
+	case HW_VAR_CORRECT_TSF:{
+			u8 btype_ibss = ((u8 *) (val))[0];
+
+			if (btype_ibss == true)
+				_rtl8821ae_stop_tx_beacon(hw);
+
+			_rtl8821ae_set_bcn_ctrl_reg(hw, 0, BIT(3));
+
+			rtl_write_dword(rtlpriv, REG_TSFTR,
+					(u32) (mac->tsf & 0xffffffff));
+			rtl_write_dword(rtlpriv, REG_TSFTR + 4,
+					(u32) ((mac->tsf >> 32) & 0xffffffff));
+
+			_rtl8821ae_set_bcn_ctrl_reg(hw, BIT(3), 0);
+
+			if (btype_ibss == true)
+				_rtl8821ae_resume_tx_beacon(hw);
+
+			break;
+
+		}
+	case HW_VAR_NAV_UPPER: {
+			u32	us_nav_upper = ((u32)*val);
+
+			if(us_nav_upper > HAL_92C_NAV_UPPER_UNIT * 0xFF)
+			{
+				RT_TRACE(COMP_INIT , DBG_WARNING,
+					("The setting value (0x%08X us) of NAV_UPPER"
+					 " is larger than (%d * 0xFF)!!!\n",
+					 us_nav_upper, HAL_92C_NAV_UPPER_UNIT));
+				break;
+			}
+			rtl_write_byte(rtlpriv, REG_NAV_UPPER,
+				((u8)((us_nav_upper + HAL_92C_NAV_UPPER_UNIT - 1) / HAL_92C_NAV_UPPER_UNIT)));
+			break;
+		}
+	case HW_VAR_KEEP_ALIVE: {
+			u8 array[2];
+			array[0] = 0xff;
+			array[1] = *((u8 *)val);
+			rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_KEEP_ALIVE_CTRL, 2, array);
+		}
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("switch case "
+							"not process %x\n",variable));
+		break;
+	}
+}
+
+static bool _rtl8821ae_llt_write(struct ieee80211_hw *hw, u32 address, u32 data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	bool status = true;
+	long count = 0;
+	u32 value = _LLT_INIT_ADDR(address) |
+	    _LLT_INIT_DATA(data) | _LLT_OP(_LLT_WRITE_ACCESS);
+
+	rtl_write_dword(rtlpriv, REG_LLT_INIT, value);
+
+	do {
+		value = rtl_read_dword(rtlpriv, REG_LLT_INIT);
+		if (_LLT_NO_ACTIVE == _LLT_OP_VALUE(value))
+			break;
+
+		if (count > POLLING_LLT_THRESHOLD) {
+			RT_TRACE(COMP_ERR, DBG_EMERG,
+				 ("Failed to polling write LLT done at "
+				  "address %d!\n", address));
+			status = false;
+			break;
+		}
+	} while (++count);
+
+	return status;
+}
+
+static bool _rtl8821ae_llt_table_init(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	unsigned short i;
+	u8 txpktbuf_bndy;
+	u8 maxPage;
+	bool status;
+
+	maxPage = 255;
+	txpktbuf_bndy = 0xF8;
+
+
+	rtl_write_byte(rtlpriv, REG_TRXFF_BNDY, txpktbuf_bndy);
+	rtl_write_word(rtlpriv, REG_TRXFF_BNDY + 2, MAX_RX_DMA_BUFFER_SIZE - 1);
+
+	rtl_write_byte(rtlpriv, REG_TDECTRL + 1, txpktbuf_bndy);
+
+	rtl_write_byte(rtlpriv, REG_TXPKTBUF_BCNQ_BDNY, txpktbuf_bndy);
+	rtl_write_byte(rtlpriv, REG_TXPKTBUF_MGQ_BDNY, txpktbuf_bndy);
+
+	rtl_write_byte(rtlpriv, REG_PBP, 0x31);
+	rtl_write_byte(rtlpriv, REG_RX_DRVINFO_SZ, 0x4);
+
+	for (i = 0; i < (txpktbuf_bndy - 1); i++) {
+		status = _rtl8821ae_llt_write(hw, i, i + 1);
+		if (true != status)
+			return status;
+	}
+
+	status = _rtl8821ae_llt_write(hw, (txpktbuf_bndy - 1), 0xFF);
+	if (true != status)
+		return status;
+
+	for (i = txpktbuf_bndy; i < maxPage; i++) {
+		status = _rtl8821ae_llt_write(hw, i, (i + 1));
+		if (true != status)
+			return status;
+	}
+
+	status = _rtl8821ae_llt_write(hw, maxPage, txpktbuf_bndy);
+	if (true != status)
+		return status;
+
+	rtl_write_dword(rtlpriv, REG_RQPN, 0x80e70808);
+	rtl_write_byte(rtlpriv, REG_RQPN_NPQ, 0x00);
+
+	return true;
+}
+
+static void _rtl8821ae_gen_refresh_led_state(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_led *pLed0 = &(pcipriv->ledctl.sw_led0);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	if (rtlpriv->rtlhal.up_first_time)
+		return;
+
+	if (ppsc->rfoff_reason == RF_CHANGE_BY_IPS)
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+			rtl8812ae_sw_led_on(hw, pLed0);
+		else
+			rtl8821ae_sw_led_on(hw, pLed0);
+	else if (ppsc->rfoff_reason == RF_CHANGE_BY_INIT)
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+			rtl8812ae_sw_led_on(hw, pLed0);
+		else
+			rtl8821ae_sw_led_on(hw, pLed0);
+	else
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+			rtl8812ae_sw_led_off(hw, pLed0);
+		else
+			rtl8821ae_sw_led_off(hw, pLed0);
+}
+
+static bool _rtl8821ae_init_mac(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	u8 bytetmp = 0;
+	u16 wordtmp = 0;
+	bool b_mac_func_enable = rtlhal->b_mac_func_enable;
+
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL, 0x00);
+
+	/*Auto Power Down to CHIP-off State*/
+	bytetmp = rtl_read_byte(rtlpriv, REG_APS_FSMCO + 1) & (~BIT(7));
+	rtl_write_byte(rtlpriv, REG_APS_FSMCO + 1, bytetmp);
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+		/* HW Power on sequence*/
+		if(!rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
+			PWR_INTF_PCI_MSK, RTL8812_NIC_ENABLE_FLOW)) {
+				RT_TRACE(COMP_INIT,DBG_LOUD,("init 8812 MAC Fail as power on failure\n"));
+				return false;
+		}
+	} else {
+		/* HW Power on sequence */
+		if (!rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_A_MSK, PWR_FAB_ALL_MSK,
+			PWR_INTF_PCI_MSK, RTL8821A_NIC_ENABLE_FLOW)){
+			RT_TRACE(COMP_INIT,DBG_LOUD,("init 8821 MAC Fail as power on failure\n"));
+			return false;
+		}
+	}
+
+	bytetmp = rtl_read_byte(rtlpriv, REG_APS_FSMCO) | BIT(4);
+	rtl_write_byte(rtlpriv, REG_APS_FSMCO, bytetmp);
+
+	bytetmp = rtl_read_byte(rtlpriv, REG_CR);
+	bytetmp = 0xff;
+	rtl_write_byte(rtlpriv, REG_CR, bytetmp);
+	mdelay(2);
+
+	bytetmp |= 0x7f;
+	rtl_write_byte(rtlpriv, REG_HWSEQ_CTRL, bytetmp);
+	mdelay(2);
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+		bytetmp = rtl_read_byte(rtlpriv, REG_SYS_CFG + 3);
+		if (bytetmp & BIT(0)) {
+			bytetmp = rtl_read_byte(rtlpriv, 0x7c);
+			bytetmp |= BIT(6);
+			rtl_write_byte(rtlpriv, 0x7c, bytetmp);
+		}
+	}
+
+	bytetmp = rtl_read_byte(rtlpriv, REG_GPIO_MUXCFG + 1);
+	bytetmp &= ~BIT(4);
+	rtl_write_byte(rtlpriv, REG_GPIO_MUXCFG + 1, bytetmp);
+
+	rtl_write_word(rtlpriv, REG_CR, 0x2ff);
+
+	if (!b_mac_func_enable) {
+		if (!_rtl8821ae_llt_table_init(hw))
+			return false;
+	}
+
+	rtl_write_dword(rtlpriv, REG_HISR, 0xffffffff);
+	rtl_write_dword(rtlpriv, REG_HISRE, 0xffffffff);
+
+	/* Enable FW Beamformer Interrupt */
+	bytetmp = rtl_read_byte(rtlpriv, REG_FWIMR + 3);
+	rtl_write_byte(rtlpriv, REG_FWIMR + 3, bytetmp | BIT(6));
+
+	wordtmp = rtl_read_word(rtlpriv, REG_TRXDMA_CTRL);
+	wordtmp &= 0xf;
+	wordtmp |= 0xF5B1;
+	rtl_write_word(rtlpriv, REG_TRXDMA_CTRL, wordtmp);
+
+	rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 1, 0x1F);
+	rtl_write_dword(rtlpriv, REG_RCR, rtlpci->receive_config);
+	rtl_write_word(rtlpriv, REG_RXFLTMAP2, 0xFFFF);
+	/*low address*/
+	rtl_write_dword(rtlpriv, REG_BCNQ_DESA,
+			rtlpci->tx_ring[BEACON_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_MGQ_DESA,
+			rtlpci->tx_ring[MGNT_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_VOQ_DESA,
+			rtlpci->tx_ring[VO_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_VIQ_DESA,
+			rtlpci->tx_ring[VI_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_BEQ_DESA,
+			rtlpci->tx_ring[BE_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_BKQ_DESA,
+			rtlpci->tx_ring[BK_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_HQ_DESA,
+			rtlpci->tx_ring[HIGH_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_RX_DESA,
+			rtlpci->rx_ring[RX_MPDU_QUEUE].dma & DMA_BIT_MASK(32));
+
+	rtl_write_byte(rtlpriv, REG_PCIE_CTRL_REG + 3, 0x77);
+
+	rtl_write_dword(rtlpriv, REG_INT_MIG, 0);
+
+	rtl_write_byte(rtlpriv, REG_SECONDARY_CCA_CTRL, 0x3);
+	_rtl8821ae_gen_refresh_led_state(hw);
+
+	return true;
+}
+
+static void _rtl8821ae_hw_configure(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u32 reg_rrsr;
+
+	reg_rrsr = RATE_ALL_CCK | RATE_ALL_OFDM_AG;
+
+	rtl_write_dword(rtlpriv, REG_RRSR, reg_rrsr);
+	/* ARFB table 9 for 11ac 5G 2SS */
+	rtl_write_dword(rtlpriv, REG_ARFR0 + 4, 0xfffff000);
+	/* ARFB table 10 for 11ac 5G 1SS */
+	rtl_write_dword(rtlpriv, REG_ARFR1 + 4, 0x003ff000);
+	/* ARFB table 11 for 11ac 24G 1SS */
+	rtl_write_dword(rtlpriv, REG_ARFR2, 0x00000015);
+	rtl_write_dword(rtlpriv, REG_ARFR2 + 4, 0x003ff000);
+	/* ARFB table 12 for 11ac 24G 1SS */
+	rtl_write_dword(rtlpriv, REG_ARFR3, 0x00000015);
+	rtl_write_dword(rtlpriv, REG_ARFR3 + 4, 0xffcff000);
+	/* 0x420[7] = 0 , enable retry AMPDU in new AMPD not singal MPDU. */
+	rtl_write_word(rtlpriv, REG_FWHW_TXQ_CTRL, 0x1F00);
+	rtl_write_byte(rtlpriv, REG_AMPDU_MAX_TIME, 0x70);
+
+	/*Set retry limit*/
+	rtl_write_word(rtlpriv, REG_RL, 0x0707);
+
+
+	/* Set Data / Response auto rate fallack retry count*/
+	rtl_write_dword(rtlpriv, REG_DARFRC, 0x01000000);
+	rtl_write_dword(rtlpriv, REG_DARFRC + 4, 0x07060504);
+	rtl_write_dword(rtlpriv, REG_RARFRC, 0x01000000);
+	rtl_write_dword(rtlpriv, REG_RARFRC + 4, 0x07060504);
+
+	rtlpci->reg_bcn_ctrl_val = 0x1d;
+	rtl_write_byte(rtlpriv, REG_BCN_CTRL, rtlpci->reg_bcn_ctrl_val);
+
+	/* TBTT prohibit hold time. Suggested by designer TimChen. */
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 1,0xff); // 8 ms
+
+	/* AGGR_BK_TIME Reg51A 0x16 */
+	rtl_write_word(rtlpriv, REG_NAV_PROT_LEN, 0x0040);
+
+	/*For Rx TP. Suggested by SD1 Richard. Added by tynli. 2010.04.12.*/
+	rtl_write_dword(rtlpriv, REG_FAST_EDCA_CTRL, 0x03086666);
+
+	rtl_write_byte(rtlpriv, REG_HT_SINGLE_AMPDU, 0x80);
+	rtl_write_byte(rtlpriv, REG_RX_PKT_LIMIT, 0x20);
+	rtl_write_word(rtlpriv, REG_MAX_AGGR_NUM, 0x1F1F);
+}
+
+static u16 _rtl8821ae_mdio_read(struct rtl_priv *rtlpriv, u8 addr)
+{
+	u16 ret = 0;
+	u8 tmp = 0, count = 0;
+
+	rtl_write_byte(rtlpriv, REG_MDIO_CTL, addr | BIT(6));
+	tmp = rtl_read_byte(rtlpriv, REG_MDIO_CTL) & BIT(6) ;
+	count = 0;
+	while (tmp && count < 20) {
+		udelay(10);
+		tmp = rtl_read_byte(rtlpriv, REG_MDIO_CTL) & BIT(6);
+		count++;
+	}
+	if (0 == tmp)
+		ret = rtl_read_word(rtlpriv, REG_MDIO_RDATA);
+
+	return ret;
+}
+
+void _rtl8821ae_mdio_write(struct rtl_priv *rtlpriv, u8 addr, u16 data)
+{
+	u8 tmp = 0, count = 0;
+
+	rtl_write_word(rtlpriv, REG_MDIO_WDATA, data);
+	rtl_write_byte(rtlpriv, REG_MDIO_CTL, addr | BIT(5));
+	tmp = rtl_read_byte(rtlpriv, REG_MDIO_CTL) & BIT(5) ;
+	count = 0;
+	while (tmp && count < 20) {
+		udelay(10);
+		tmp = rtl_read_byte(rtlpriv, REG_MDIO_CTL) & BIT(5);
+		count++;
+	}
+}
+
+static u8 _rtl8821ae_dbi_read(struct rtl_priv *rtlpriv, u16 addr)
+{
+	u16 read_addr = addr & 0xfffc;
+	u8 tmp = 0, count = 0, ret = 0;
+
+	rtl_write_word(rtlpriv, REG_DBI_ADDR, read_addr);
+	rtl_write_byte(rtlpriv, REG_DBI_FLAG, 0x2);
+	tmp = rtl_read_byte(rtlpriv, REG_DBI_FLAG);
+	count = 0;
+	while (tmp && count < 20) {
+		udelay(10);
+		tmp = rtl_read_byte(rtlpriv, REG_DBI_FLAG);
+		count++;
+	}
+	if (0 == tmp) {
+		read_addr = REG_DBI_RDATA + addr % 4;
+		ret = rtl_read_word(rtlpriv, read_addr);
+	}
+	return ret;
+}
+
+void _rtl8821ae_dbi_write(struct rtl_priv *rtlpriv, u16 addr, u8 data)
+{
+	u8 tmp = 0, count = 0;
+	u16 wrtie_addr, remainder = addr % 4;
+
+	wrtie_addr = REG_DBI_WDATA + remainder;
+	rtl_write_byte(rtlpriv, wrtie_addr, data);
+
+	wrtie_addr = (addr & 0xfffc) | (BIT(0) << (remainder + 12));
+	rtl_write_word(rtlpriv, REG_DBI_ADDR, wrtie_addr);
+
+	rtl_write_byte(rtlpriv, REG_DBI_FLAG, 0x1);
+
+	tmp = rtl_read_byte(rtlpriv, REG_DBI_FLAG);
+	count = 0;
+	while (tmp && count < 20) {
+		udelay(10);
+		tmp = rtl_read_byte(rtlpriv, REG_DBI_FLAG);
+		count++;
+	}
+
+}
+
+static void _rtl8821ae_enable_aspm_back_door(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 tmp;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+		if (_rtl8821ae_mdio_read(rtlpriv, 0x04) != 0x8544)
+			_rtl8821ae_mdio_write(rtlpriv, 0x04, 0x8544);
+
+		if (_rtl8821ae_mdio_read(rtlpriv, 0x0b) != 0x0070)
+			_rtl8821ae_mdio_write(rtlpriv, 0x0b, 0x0070);
+	}
+
+	tmp = _rtl8821ae_dbi_read(rtlpriv, 0x70f);
+	_rtl8821ae_dbi_write(rtlpriv, 0x70f, tmp | BIT(7));
+
+	tmp = _rtl8821ae_dbi_read(rtlpriv, 0x719);
+	_rtl8821ae_dbi_write(rtlpriv, 0x719, tmp | BIT(3) | BIT(4));
+
+	if(rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+	{
+		tmp  = _rtl8821ae_dbi_read(rtlpriv, 0x718);
+		_rtl8821ae_dbi_write(rtlpriv, 0x718, tmp|BIT(4));
+	}
+}
+
+void rtl8821ae_enable_hw_security_config(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 sec_reg_value;
+	u8 tmp;
+
+	RT_TRACE(COMP_INIT, DBG_DMESG,
+		 ("PairwiseEncAlgorithm = %d GroupEncAlgorithm = %d\n",
+		  rtlpriv->sec.pairwise_enc_algorithm,
+		  rtlpriv->sec.group_enc_algorithm));
+
+	if (rtlpriv->cfg->mod_params->sw_crypto || rtlpriv->sec.use_sw_sec) {
+		RT_TRACE(COMP_SEC, DBG_DMESG, ("not open hw encryption\n"));
+		return;
+	}
+
+	sec_reg_value = SCR_TxEncEnable | SCR_RxDecEnable;
+
+	if (rtlpriv->sec.use_defaultkey) {
+		sec_reg_value |= SCR_TxUseDK;
+		sec_reg_value |= SCR_RxUseDK;
+	}
+
+	sec_reg_value |= (SCR_RXBCUSEDK | SCR_TXBCUSEDK);
+
+	tmp = rtl_read_byte(rtlpriv, REG_CR + 1);
+	rtl_write_byte(rtlpriv, REG_CR + 1, tmp | BIT(1));
+
+	RT_TRACE(COMP_SEC, DBG_DMESG,
+		 ("The SECR-value %x \n", sec_reg_value));
+
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_WPA_CONFIG, &sec_reg_value);
+
+}
+
+#if 0
+bool _rtl8821ae_check_pcie_dma_hang(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmp;
+	tmp = rtl_read_byte(rtlpriv, REG_DBI_CTRL+3);
+	if (!(tmp&BIT(2))) {
+		rtl_write_byte(rtlpriv, REG_DBI_CTRL+3, tmp|BIT(2));
+		mdelay(100);
+	}
+
+	tmp = rtl_read_byte(rtlpriv, REG_DBI_CTRL+3);
+	if (tmp&BIT(0) || tmp&BIT(1)) {
+		RT_TRACE(COMP_INIT, DBG_LOUD,
+			("rtl8821ae_check_pcie_dma_hang(): TRUE! Reset PCIE DMA!\n"));
+		return true;
+	} else {
+		return false;
+	}
+}
+
+void _rtl8821ae_reset_pcie_interface_dma(struct ieee80211_hw *hw,
+													bool mac_power_on, bool watch_dog)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmp;
+	bool release_mac_rx_pause;
+	u8 backup_pcie_dma_pause;
+
+	RT_TRACE(COMP_INIT, DBG_LOUD, ("_rtl8821ae_reset_pcie_interface_dma()\n"));
+
+	tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL);
+	tmp &= ~BIT(1);
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL, tmp);
+	tmp = rtl_read_byte(rtlpriv, REG_PMC_DBG_CTRL2);
+	tmp |= BIT2;
+	rtl_write_byte(rtlpriv, REG_PMC_DBG_CTRL2, tmp);
+
+	tmp = rtl_read_byte(rtlpriv, REG_RXDMA_CONTROL);
+	if (tmp & BIT(2)) {
+		release_mac_rx_pause = false;
+	} else {
+		rtl_write_byte(rtlpriv, REG_RXDMA_CONTROL, tmp | BIT(2));
+		release_mac_rx_pause = true;
+	}
+	backup_pcie_dma_pause = rtl_read_byte(rtlpriv, REG_PCIE_CTRL_REG+1);
+	if (backup_pcie_dma_pause != 0xFF)
+		rtl_write_byte(rtlpriv, REG_PCIE_CTRL_REG+1, 0xFF);
+
+	if (mac_power_on)
+		rtl_write_byte(rtlpriv, REG_CR, 0);
+
+	tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN+1);
+	tmp &= ~BIT(0);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN+1, tmp);
+
+	tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN+1);
+	tmp |= ~BIT(0);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN+1, tmp);
+
+	if (mac_power_on)
+		rtl_write_byte(rtlpriv, REG_CR, 0xFF);
+
+	tmp = rtl_read_byte(rtlpriv, REG_MAC_PHY_CTRL_NORMAL+2);
+	tmp |= BIT1;
+	rtl_write_byte(rtlpriv, REG_MAC_PHY_CTRL_NORMAL+2, tmp);
+
+	if (watch_dog) {
+		u32 rqpn = 0;
+		u32 rqpn_npq = 0;
+		u8 tx_page_boundary = _RQPN_Init_8812E(Adapter, &rqpn_npq, &rqpn);
+
+		if(LLT_table_init_8812(Adapter, TX_PAGE_BOUNDARY, RQPN, RQPN_NPQ) == RT_STATUS_FAILURE)
+			return false;
+
+			PlatformAcquireSpinLock(Adapter, RT_RX_SPINLOCK);
+			PlatformAcquireSpinLock(Adapter, RT_TX_SPINLOCK);
+
+			// <1> Reset Tx descriptor
+			Adapter->HalFunc.ResetTxDescHandler(Adapter,Adapter->NumTxDesc);
+
+			// <2> Reset Rx descriptor
+			Adapter->HalFunc.ResetRxDescHandler(Adapter,Adapter->NumRxDesc);
+
+			// <3> Reset RFDs
+			FreeRFDs( Adapter, TRUE);
+
+			// <4> Reset TCBs
+			FreeTCBs( Adapter, TRUE);
+
+			// We should set all Rx desc own bit to 1 to prevent from RDU after enable Rx DMA. 2013.02.18, by tynli.
+			PrepareAllRxDescBuffer(Adapter);
+
+			PlatformReleaseSpinLock(Adapter, RT_TX_SPINLOCK);
+			PlatformReleaseSpinLock(Adapter, RT_RX_SPINLOCK);
+
+			//
+			// Initialize TRx DMA address.
+			//
+			// Because set 0x100 to 0x0 will cause the Rx descriptor address 0x340 be cleared to zero on 88EE,
+			// we should re-initialize Rx desc. address before enable DMA. 2012.11.07. by tynli.
+			InitTRxDescHwAddress8812AE(Adapter);
+		}
+
+		// In MAC power on state, BB and RF maybe in ON state, if we release TRx DMA here
+		// it will cause packets to be started to Tx/Rx, so we release Tx/Rx DMA later.
+		if(!bInMACPowerOn || bInWatchDog)
+		{
+			// 8. release TRX DMA
+			//write 0x284 bit[18] = 1'b0
+			//write 0x301 = 0x00
+			if(bReleaseMACRxPause)
+			{
+				u1Tmp = PlatformEFIORead1Byte(Adapter, REG_RXDMA_CONTROL);
+				PlatformEFIOWrite1Byte(Adapter, REG_RXDMA_CONTROL, (u1Tmp&~BIT2));
+			}
+			PlatformEFIOWrite1Byte(Adapter, 	REG_PCIE_CTRL_REG+1, BackUpPcieDMAPause);
+		}
+
+		if(IS_HARDWARE_TYPE_8821E(Adapter))
+		{
+			//9. lock system register
+			//	 write 0xCC bit[2] = 1'b0
+			u1Tmp = PlatformEFIORead1Byte(Adapter, REG_PMC_DBG_CTRL2_8723B);
+			u1Tmp &= ~(BIT2);
+			PlatformEFIOWrite1Byte(Adapter, REG_PMC_DBG_CTRL2_8723B, u1Tmp);
+		}
+
+		return RT_STATUS_SUCCESS;
+}
+#endif
+
+// Static MacID Mapping (cf. Used in MacIdDoStaticMapping) ----------
+#define MAC_ID_STATIC_FOR_DEFAULT_PORT				0
+#define MAC_ID_STATIC_FOR_BROADCAST_MULTICAST		1
+#define MAC_ID_STATIC_FOR_BT_CLIENT_START				2
+#define MAC_ID_STATIC_FOR_BT_CLIENT_END				3
+// -----------------------------------------------------------
+
+void rtl8821ae_macid_initialize_mediastatus(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8	media_rpt[4] = {RT_MEDIA_CONNECT, 1, \
+		MAC_ID_STATIC_FOR_BROADCAST_MULTICAST, \
+		MAC_ID_STATIC_FOR_BT_CLIENT_END};
+
+	rtlpriv->cfg->ops->set_hw_reg(hw, \
+		HW_VAR_H2C_FW_MEDIASTATUSRPT, media_rpt);
+
+	RT_TRACE(COMP_INIT,DBG_LOUD, \
+		("Initialize MacId media status: from %d to %d\n", \
+		MAC_ID_STATIC_FOR_BROADCAST_MULTICAST, \
+		MAC_ID_STATIC_FOR_BT_CLIENT_END));
+}
+
+int rtl8821ae_hw_init(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	bool rtstatus = true;
+	int err;
+	u8 tmp_u1b;
+	u32 nav_upper = WIFI_NAV_UPPER_US;
+
+	rtlpriv->rtlhal.being_init_adapter = true;
+	rtlpriv->intf_ops->disable_aspm(hw);
+
+	/*YP wowlan not considered*/
+
+	tmp_u1b = rtl_read_byte(rtlpriv, REG_CR);
+	if (tmp_u1b!=0 && tmp_u1b != 0xEA) {
+		rtlhal->b_mac_func_enable = true;
+		RT_TRACE(COMP_INIT,DBG_LOUD,(" MAC has already power on.\n"));
+	} else {
+		rtlhal->b_mac_func_enable = false;
+		rtlhal->fw_ps_state = FW_PS_STATE_ALL_ON_8821AE;
+	}
+
+/*	if (_rtl8821ae_check_pcie_dma_hang(hw)) {
+		_rtl8821ae_reset_pcie_interface_dma(hw,rtlhal->b_mac_func_enable,false);
+		rtlhal->b_mac_func_enable = false;
+	} */
+
+	rtstatus = _rtl8821ae_init_mac(hw);
+	if (rtstatus != true) {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("Init MAC failed\n"));
+		err = 1;
+		return err;
+	}
+
+	tmp_u1b = rtl_read_byte(rtlpriv, REG_SYS_CFG);
+	tmp_u1b &= 0x7F;
+	rtl_write_byte(rtlpriv, REG_SYS_CFG, tmp_u1b);
+
+	err = rtl8821ae_download_fw(hw, false);
+	if (err) {
+		RT_TRACE(COMP_ERR, DBG_WARNING,
+			 ("Failed to download FW. Init HW "
+			  "without FW now..\n"));
+		err = 1;
+		rtlhal->bfw_ready = false;
+		return err;
+	} else {
+		rtlhal->bfw_ready = true;
+	}
+	rtlhal->fw_ps_state = FW_PS_STATE_ALL_ON_8821AE;
+	rtlhal->bfw_clk_change_in_progress = false;
+	rtlhal->ballow_sw_to_change_hwclc = false;
+	rtlhal->last_hmeboxnum = 0;
+
+	/*SIC_Init(Adapter);
+	if(pHalData->AMPDUBurstMode)
+		PlatformEFIOWrite1Byte(Adapter,REG_AMPDU_BURST_MODE_8812,  0x7F);*/
+
+	rtl8821ae_phy_mac_config(hw);
+	/* because last function modify RCR, so we update
+	 * rcr var here, or TP will unstable for receive_config
+	 * is wrong, RX RCR_ACRC32 will cause TP unstabel & Rx
+	 * RCR_APP_ICV will cause mac80211 unassoc for cisco 1252
+	rtlpci->receive_config = rtl_read_dword(rtlpriv, REG_RCR);
+	rtlpci->receive_config &= ~(RCR_ACRC32 | RCR_AICV);
+	rtl_write_dword(rtlpriv, REG_RCR, rtlpci->receive_config);*/
+	rtl8821ae_phy_bb_config(hw);
+
+	rtl8821ae_phy_rf_config(hw);
+
+	_rtl8821ae_hw_configure(hw);
+
+	rtl8821ae_phy_switch_wirelessband(hw, BAND_ON_2_4G);
+
+	/*set wireless mode*/
+
+	rtlhal->b_mac_func_enable = true;
+
+	rtl_cam_reset_all_entry(hw);
+
+	rtl8821ae_enable_hw_security_config(hw);
+
+	ppsc->rfpwr_state = ERFON;
+
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ETHER_ADDR, mac->mac_addr);
+	_rtl8821ae_enable_aspm_back_door(hw);
+	rtlpriv->intf_ops->enable_aspm(hw);
+
+	//rtl8821ae_bt_hw_init(hw);
+	rtlpriv->rtlhal.being_init_adapter = false;
+
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_NAV_UPPER, (u8 *)&nav_upper);
+
+	//rtl8821ae_dm_check_txpower_tracking(hw);
+	//rtl8821ae_phy_lc_calibrate(hw);
+
+	/* Release Rx DMA*/
+	tmp_u1b = rtl_read_byte(rtlpriv, REG_RXDMA_CONTROL);
+	if (tmp_u1b & BIT(2)) {
+		/* Release Rx DMA if needed*/
+		tmp_u1b &= ~BIT(2);
+		rtl_write_byte(rtlpriv, REG_RXDMA_CONTROL, tmp_u1b);
+	}
+
+	/* Release Tx/Rx PCIE DMA if*/
+	rtl_write_byte(rtlpriv, REG_PCIE_CTRL_REG + 1, 0);
+
+	rtl8821ae_dm_init(hw);
+	rtl8821ae_macid_initialize_mediastatus(hw);
+
+	RT_TRACE(COMP_INIT, DBG_LOUD, ("rtl8821ae_hw_init() <====\n"));
+	return err;
+}
+
+static enum version_8821ae _rtl8821ae_read_chip_version(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	enum version_8821ae version = VERSION_UNKNOWN;
+	u32 value32;
+
+	value32 = rtl_read_dword(rtlpriv, REG_SYS_CFG1);
+	RT_TRACE(COMP_INIT, DBG_LOUD, ("ReadChipVersion8812A 0xF0 = 0x%x \n", value32));
+
+
+
+	if(rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+		rtlphy->rf_type = RF_2T2R;
+	else if(rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE)
+		rtlphy->rf_type = RF_1T1R;
+
+	RT_TRACE(COMP_INIT, DBG_LOUD, ("RF_Type is %x!!\n", rtlphy->rf_type));
+
+
+	if (value32 & TRP_VAUX_EN)
+	{
+		if(rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+		{
+			if(rtlphy->rf_type == RF_2T2R)
+				version = VERSION_TEST_CHIP_2T2R_8812;
+			else
+				version = VERSION_TEST_CHIP_1T1R_8812;
+		}
+		else
+			version = VERSION_TEST_CHIP_8821;
+	} else {
+		if(rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+		{
+			u32	rtl_id = ((value32 & CHIP_VER_RTL_MASK) >> 12) +1 ;
+
+			if(rtlphy->rf_type == RF_2T2R)
+				version = (enum version_8821ae)(CHIP_8812 | NORMAL_CHIP | RF_TYPE_2T2R);
+			else
+				version = (enum version_8821ae)(CHIP_8812 | NORMAL_CHIP);
+
+			version = (enum version_8821ae)(version| (rtl_id << 12));
+		}
+		else if(rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE)
+		{
+			u32	rtl_id = value32 & CHIP_VER_RTL_MASK;
+
+			version = (enum version_8821ae)(CHIP_8821 | NORMAL_CHIP | rtl_id);
+		}
+	}
+
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("Chip RF Type: %s\n", (rtlphy->rf_type == RF_2T2R) ?
+		  "RF_2T2R" : "RF_1T1R"));
+
+	if(rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE)
+	{
+		/*WL_HWROF_EN.*/
+		value32 = rtl_read_dword(rtlpriv, REG_MULTI_FUNC_CTRL);
+		rtlphy->hw_rof_enable= ((value32 & WL_HWROF_EN) ? 1 : 0);
+	}
+
+	switch(version)
+	{
+		case VERSION_TEST_CHIP_1T1R_8812:
+			RT_TRACE(COMP_INIT, DBG_LOUD,
+				("Chip Version ID: VERSION_TEST_CHIP_1T1R_8812.\n"));
+			break;
+		case VERSION_TEST_CHIP_2T2R_8812:
+			RT_TRACE(COMP_INIT, DBG_LOUD,
+				("Chip Version ID: VERSION_TEST_CHIP_2T2R_8812.\n"));
+			break;
+		case VERSION_NORMAL_TSMC_CHIP_1T1R_8812:
+			RT_TRACE(COMP_INIT, DBG_LOUD,
+				("Chip Version ID: VERSION_NORMAL_TSMC_CHIP_1T1R_8812.\n"));
+			break;
+		case VERSION_NORMAL_TSMC_CHIP_2T2R_8812:
+			RT_TRACE(COMP_INIT, DBG_LOUD,
+				("Chip Version ID: VERSION_NORMAL_TSMC_CHIP_2T2R_8812.\n"));
+			break;
+		case VERSION_NORMAL_TSMC_CHIP_1T1R_8812_C_CUT:
+			RT_TRACE(COMP_INIT, DBG_LOUD,
+				("Chip Version ID: VERSION_NORMAL_TSMC_CHIP_1T1R_8812 C CUT.\n"));
+			break;
+		case VERSION_NORMAL_TSMC_CHIP_2T2R_8812_C_CUT:
+			RT_TRACE(COMP_INIT, DBG_LOUD,
+				("Chip Version ID: VERSION_NORMAL_TSMC_CHIP_2T2R_8812 C CUT.\n"));
+			break;
+		case VERSION_TEST_CHIP_8821:
+			RT_TRACE(COMP_INIT, DBG_LOUD,
+				("Chip Version ID: VERSION_TEST_CHIP_8821.\n"));
+			break;
+		case VERSION_NORMAL_TSMC_CHIP_8821:
+			RT_TRACE(COMP_INIT, DBG_LOUD,
+				("Chip Version ID: VERSION_NORMAL_TSMC_CHIP_8821 A CUT.\n"));
+			break;
+		case VERSION_NORMAL_TSMC_CHIP_8821_B_CUT:
+			RT_TRACE(COMP_INIT, DBG_LOUD,
+				("Chip Version ID: VERSION_NORMAL_TSMC_CHIP_8821 B CUT.\n"));
+			break;
+		default:
+			RT_TRACE(COMP_INIT, DBG_LOUD,
+				("Chip Version ID: Unknow (0x%X).\n", version));
+			break;
+	}
+
+	return version;
+}
+
+static int _rtl8821ae_set_media_status(struct ieee80211_hw *hw,
+				     enum nl80211_iftype type)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 bt_msr = rtl_read_byte(rtlpriv, MSR);
+	enum led_ctl_mode ledaction = LED_CTL_NO_LINK;
+	bt_msr &= 0xfc;
+
+	rtl_write_dword(rtlpriv, REG_BCN_CTRL, 0);
+	RT_TRACE(COMP_BEACON, DBG_LOUD,
+		("clear 0x550 when set HW_VAR_MEDIA_STATUS\n"));
+
+	if (type == NL80211_IFTYPE_UNSPECIFIED ||
+	    type == NL80211_IFTYPE_STATION) {
+		_rtl8821ae_stop_tx_beacon(hw);
+		_rtl8821ae_enable_bcn_sub_func(hw);
+	} else if (type == NL80211_IFTYPE_ADHOC ||
+		type == NL80211_IFTYPE_AP) {
+		_rtl8821ae_resume_tx_beacon(hw);
+		_rtl8821ae_disable_bcn_sub_func(hw);
+	} else {
+		RT_TRACE(COMP_ERR, DBG_WARNING,("Set HW_VAR_MEDIA_STATUS: "
+			  "No such media status(%x).\n", type));
+	}
+
+	switch (type) {
+	case NL80211_IFTYPE_UNSPECIFIED:
+		bt_msr |= MSR_NOLINK;
+		ledaction = LED_CTL_LINK;
+		RT_TRACE(COMP_INIT, DBG_TRACE, ("Set Network type to NO LINK!\n"));
+		break;
+	case NL80211_IFTYPE_ADHOC:
+		bt_msr |= MSR_ADHOC;
+		RT_TRACE(COMP_INIT, DBG_TRACE, ("Set Network type to Ad Hoc!\n"));
+		break;
+	case NL80211_IFTYPE_STATION:
+		bt_msr |= MSR_INFRA;
+		ledaction = LED_CTL_LINK;
+		RT_TRACE(COMP_INIT, DBG_TRACE, ("Set Network type to STA!\n"));
+		break;
+	case NL80211_IFTYPE_AP:
+		bt_msr |= MSR_AP;
+		RT_TRACE(COMP_INIT, DBG_TRACE, ("Set Network type to AP!\n"));
+		break;
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("Network type %d not support!\n", type));
+		return 1;
+		break;
+
+	}
+
+	rtl_write_byte(rtlpriv, (MSR), bt_msr);
+	rtlpriv->cfg->ops->led_control(hw, ledaction);
+	if ((bt_msr & 0xfc) == MSR_AP)
+		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x00);
+	else
+		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x66);
+
+	return 0;
+}
+
+void rtl8821ae_set_check_bssid(struct ieee80211_hw *hw, bool check_bssid)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u32 reg_rcr = rtlpci->receive_config;
+
+	if (rtlpriv->psc.rfpwr_state != ERFON)
+		return;
+
+	if (check_bssid == true) {
+		reg_rcr |= (RCR_CBSSID_DATA | RCR_CBSSID_BCN);
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_RCR,
+					      (u8 *) (&reg_rcr));
+		_rtl8821ae_set_bcn_ctrl_reg(hw, 0, BIT(4));
+	} else if (check_bssid == false) {
+		reg_rcr &= (~(RCR_CBSSID_DATA | RCR_CBSSID_BCN));
+		_rtl8821ae_set_bcn_ctrl_reg(hw, BIT(4), 0);
+		rtlpriv->cfg->ops->set_hw_reg(hw,
+			HW_VAR_RCR, (u8 *) (&reg_rcr));
+	}
+
+}
+
+int rtl8821ae_set_network_type(struct ieee80211_hw *hw, enum nl80211_iftype type)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	RT_TRACE(COMP_INIT, DBG_LOUD, ("rtl8821ae_set_network_type!\n"));
+
+	if (_rtl8821ae_set_media_status(hw, type))
+		return -EOPNOTSUPP;
+
+	if (rtlpriv->mac80211.link_state == MAC80211_LINKED) {
+		if (type != NL80211_IFTYPE_AP)
+			rtl8821ae_set_check_bssid(hw, true);
+	} else {
+		rtl8821ae_set_check_bssid(hw, false);
+	}
+
+	return 0;
+}
+
+/* don't set REG_EDCA_BE_PARAM here because mac80211 will send pkt when scan */
+void rtl8821ae_set_qos(struct ieee80211_hw *hw, int aci)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	rtl8821ae_dm_init_edca_turbo(hw);
+	switch (aci) {
+	case AC1_BK:
+		rtl_write_dword(rtlpriv, REG_EDCA_BK_PARAM, 0xa44f);
+		break;
+	case AC0_BE:
+		/* rtl_write_dword(rtlpriv, REG_EDCA_BE_PARAM, u4b_ac_param); */
+		break;
+	case AC2_VI:
+		rtl_write_dword(rtlpriv, REG_EDCA_VI_PARAM, 0x5e4322);
+		break;
+	case AC3_VO:
+		rtl_write_dword(rtlpriv, REG_EDCA_VO_PARAM, 0x2f3222);
+		break;
+	default:
+		RT_ASSERT(false, ("invalid aci: %d !\n", aci));
+		break;
+	}
+}
+
+void rtl8821ae_enable_interrupt(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	rtl_write_dword(rtlpriv, REG_HIMR, rtlpci->irq_mask[0] & 0xFFFFFFFF);
+	rtl_write_dword(rtlpriv, REG_HIMRE, rtlpci->irq_mask[1] & 0xFFFFFFFF);
+	rtlpci->irq_enabled = true;
+	/* there are some C2H CMDs have been sent before system interrupt is enabled, e.g., C2H, CPWM.
+	*So we need to clear all C2H events that FW has notified, otherwise FW won't schedule any commands anymore.
+	*/
+	//rtl_write_byte(rtlpriv, REG_C2HEVT_CLEAR, 0);
+	/*enable system interrupt*/
+	rtl_write_dword(rtlpriv, REG_HSIMR, rtlpci->sys_irq_mask & 0xFFFFFFFF);
+}
+
+void rtl8821ae_disable_interrupt(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	rtl_write_dword(rtlpriv, REG_HIMR, IMR_DISABLED);
+	rtl_write_dword(rtlpriv, REG_HIMRE, IMR_DISABLED);
+	rtlpci->irq_enabled = false;
+	synchronize_irq(rtlpci->pdev->irq);
+}
+
+static void _rtl8821ae_poweroff_adapter(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 u1b_tmp;
+
+	rtlhal->b_mac_func_enable = false;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+	/* Combo (PCIe + USB) Card and PCIe-MF Card */
+	/* 1. Run LPS WL RFOFF flow */
+		//RT_TRACE(COMP_INIT, DBG_LOUD, ("=====>CardDisableRTL8812E,RTL8821A_NIC_LPS_ENTER_FLOW\n"));
+		rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
+			PWR_INTF_PCI_MSK, RTL8821A_NIC_LPS_ENTER_FLOW);
+	}
+	/* 2. 0x1F[7:0] = 0 */
+	/* turn off RF */
+	//rtl_write_byte(rtlpriv, REG_RF_CTRL, 0x00);
+	if ((rtl_read_byte(rtlpriv, REG_MCUFWDL) & BIT(7)) &&
+		rtlhal->bfw_ready ) {
+		rtl8821ae_firmware_selfreset(hw);
+	}
+
+	/* Reset MCU. Suggested by Filen. */
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN+1);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN+1, (u1b_tmp & (~BIT(2))));
+
+	/* g.	MCUFWDL 0x80[1:0]=0	 */
+	/* reset MCU ready status */
+	rtl_write_byte(rtlpriv, REG_MCUFWDL, 0x00);
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+		/* HW card disable configuration. */
+		rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
+			PWR_INTF_PCI_MSK, RTL8821A_NIC_DISABLE_FLOW);
+	} else {
+		/* HW card disable configuration. */
+		rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
+			PWR_INTF_PCI_MSK, RTL8812_NIC_DISABLE_FLOW);
+	}
+
+	/* Reset MCU IO Wrapper */
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL + 1);
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL + 1, (u1b_tmp & (~BIT(0))));
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL + 1);
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL + 1, u1b_tmp | BIT(0));
+
+	/* 7. RSV_CTRL 0x1C[7:0] = 0x0E */
+	/* lock ISO/CLK/Power control register */
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL, 0x0e);
+}
+
+void rtl8821ae_card_disable(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	enum nl80211_iftype opmode;
+
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("rtl8821ae_card_disable.\n"));
+
+	mac->link_state = MAC80211_NOLINK;
+	opmode = NL80211_IFTYPE_UNSPECIFIED;
+	_rtl8821ae_set_media_status(hw, opmode);
+	if (rtlpriv->rtlhal.driver_is_goingto_unload ||
+	    ppsc->rfoff_reason > RF_CHANGE_BY_PS)
+		rtlpriv->cfg->ops->led_control(hw, LED_CTL_POWER_OFF);
+	RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
+	_rtl8821ae_poweroff_adapter(hw);
+
+	/* after power off we should do iqk again */
+	rtlpriv->phy.iqk_initialized = false;
+}
+
+void rtl8821ae_interrupt_recognized(struct ieee80211_hw *hw,
+				  u32 *p_inta, u32 *p_intb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	*p_inta = rtl_read_dword(rtlpriv, ISR) & rtlpci->irq_mask[0];
+	rtl_write_dword(rtlpriv, ISR, *p_inta);
+
+
+	*p_intb = rtl_read_dword(rtlpriv, REG_HISRE) & rtlpci->irq_mask[1];
+	rtl_write_dword(rtlpriv, REG_HISRE, *p_intb);
+
+}
+
+
+void rtl8821ae_set_beacon_related_registers(struct ieee80211_hw *hw)
+{
+
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u16 bcn_interval, atim_window;
+
+	bcn_interval = mac->beacon_interval;
+	atim_window = 2;	/*FIX MERGE */
+	rtl8821ae_disable_interrupt(hw);
+	rtl_write_word(rtlpriv, REG_ATIMWND, atim_window);
+	rtl_write_word(rtlpriv, REG_BCN_INTERVAL, bcn_interval);
+	rtl_write_word(rtlpriv, REG_BCNTCFG, 0x660f);
+	rtl_write_byte(rtlpriv, REG_RXTSF_OFFSET_CCK, 0x18);
+	rtl_write_byte(rtlpriv, REG_RXTSF_OFFSET_OFDM, 0x18);
+	rtl_write_byte(rtlpriv, 0x606, 0x30);
+	rtlpci->reg_bcn_ctrl_val |= BIT(3);
+	rtl_write_byte(rtlpriv, REG_BCN_CTRL, (u8) rtlpci->reg_bcn_ctrl_val);
+	rtl8821ae_enable_interrupt(hw);
+}
+
+void rtl8821ae_set_beacon_interval(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u16 bcn_interval = mac->beacon_interval;
+
+	RT_TRACE(COMP_BEACON, DBG_DMESG,
+		 ("beacon_interval:%d\n", bcn_interval));
+	rtl8821ae_disable_interrupt(hw);
+	rtl_write_word(rtlpriv, REG_BCN_INTERVAL, bcn_interval);
+	rtl8821ae_enable_interrupt(hw);
+}
+
+void rtl8821ae_update_interrupt_mask(struct ieee80211_hw *hw,
+				   u32 add_msr, u32 rm_msr)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	RT_TRACE(COMP_INTR, DBG_LOUD,
+		 ("add_msr:%x, rm_msr:%x\n", add_msr, rm_msr));
+
+	if (add_msr)
+		rtlpci->irq_mask[0] |= add_msr;
+	if (rm_msr)
+		rtlpci->irq_mask[0] &= (~rm_msr);
+	rtl8821ae_disable_interrupt(hw);
+	rtl8821ae_enable_interrupt(hw);
+}
+
+static u8 _rtl8821ae_get_chnl_group(u8 chnl)
+{
+	u8 group = 0;
+
+	if (chnl <= 14) {
+		if (1 <= chnl && chnl <= 2 )
+			group = 0;
+        else if (3 <= chnl && chnl <= 5 )
+			group = 1;
+        else if (6 <= chnl && chnl <= 8 )
+			group = 2;
+        else if (9 <= chnl && chnl <= 11)
+			group = 3;
+        else /*if (12 <= chnl && chnl <= 14)*/
+			group = 4;
+	} else {
+		if (36 <= chnl && chnl <= 42)
+			group = 0;
+        else if (44 <= chnl && chnl <= 48)
+			group = 1;
+        else if (50 <= chnl && chnl <= 58)
+			group = 2;
+        else if (60 <= chnl && chnl <= 64)
+			group = 3;
+        else if (100 <= chnl && chnl <= 106)
+			group = 4;
+        else if (108 <= chnl && chnl <= 114)
+			group = 5;
+        else if (116 <= chnl && chnl <= 122)
+			group = 6;
+        else if (124 <= chnl && chnl <= 130)
+			group = 7;
+        else if (132 <= chnl && chnl <= 138)
+			group = 8;
+        else if (140 <= chnl && chnl <= 144)
+			group = 9;
+        else if (149 <= chnl && chnl <= 155)
+			group = 10;
+        else if (157 <= chnl && chnl <= 161)
+			group = 11;
+        else if (165 <= chnl && chnl <= 171)
+			group = 12;
+        else if (173 <= chnl && chnl <= 177)
+			group = 13;
+		else
+			/*RT_TRACE(COMP_EFUSE,DBG_LOUD,
+				("5G, Channel %d in Group not found \n",chnl));*/
+			RT_ASSERT(!COMP_EFUSE,
+				("5G, Channel %d in Group not found \n",chnl));
+	}
+	return group;
+}
+
+static void _rtl8821ae_read_power_value_fromprom(struct ieee80211_hw *hw,
+	struct txpower_info_2g *pwrinfo24g,
+	struct txpower_info_5g *pwrinfo5g,
+	bool autoload_fail,
+	u8 *hwinfo)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 rfPath, eeAddr=EEPROM_TX_PWR_INX, group,TxCount=0;
+
+	RT_TRACE(COMP_INIT, DBG_LOUD, ("hal_ReadPowerValueFromPROM8821ae(): PROMContent[0x%x]=0x%x\n", (eeAddr+1), hwinfo[eeAddr+1]));
+	if (0xFF == hwinfo[eeAddr+1])  /*YJ,add,120316*/
+		autoload_fail = true;
+
+	if (autoload_fail)
+	{
+		RT_TRACE(COMP_INIT, DBG_LOUD, ("auto load fail : Use Default value!\n"));
+		for (rfPath = 0 ; rfPath < MAX_RF_PATH ; rfPath++) {
+			/*2.4G default value*/
+			for (group = 0 ; group < MAX_CHNL_GROUP_24G; group++) {
+				pwrinfo24g->index_cck_base[rfPath][group] =	0x2D;
+				pwrinfo24g->index_bw40_base[rfPath][group] = 0x2D;
+			}
+			for (TxCount = 0;TxCount < MAX_TX_COUNT;TxCount++) {
+				if (TxCount == 0) {
+					pwrinfo24g->bw20_diff[rfPath][0] = 0x02;
+					pwrinfo24g->ofdm_diff[rfPath][0] = 0x04;
+				} else {
+					pwrinfo24g->bw20_diff[rfPath][TxCount] = 0xFE;
+					pwrinfo24g->bw40_diff[rfPath][TxCount] = 0xFE;
+					pwrinfo24g->cck_diff[rfPath][TxCount] =	0xFE;
+					pwrinfo24g->ofdm_diff[rfPath][TxCount] = 0xFE;
+				}
+			}
+			/*5G default value*/
+			for (group = 0 ; group < MAX_CHNL_GROUP_5G; group++)
+				pwrinfo5g->index_bw40_base[rfPath][group] = 0x2A;
+
+			for (TxCount = 0; TxCount < MAX_TX_COUNT; TxCount++) {
+				if (TxCount == 0) {
+					pwrinfo5g->ofdm_diff[rfPath][0] = 0x04;
+					pwrinfo5g->bw20_diff[rfPath][0] = 0x00;
+					pwrinfo5g->bw80_diff[rfPath][0] = 0xFE;
+					pwrinfo5g->bw160_diff[rfPath][0] = 0xFE;
+				} else {
+					pwrinfo5g->ofdm_diff[rfPath][0] = 0xFE;
+					pwrinfo5g->bw20_diff[rfPath][0] = 0xFE;
+					pwrinfo5g->bw40_diff[rfPath][0] = 0xFE;
+					pwrinfo5g->bw80_diff[rfPath][0] = 0xFE;
+					pwrinfo5g->bw160_diff[rfPath][0] = 0xFE;
+				}
+			}
+		}
+		return;
+	}
+
+	rtl_priv(hw)->efuse.b_txpwr_fromeprom = true;
+
+	for (rfPath = 0 ; rfPath < MAX_RF_PATH ; rfPath++) {
+		/*2.4G default value*/
+		for (group = 0 ; group < MAX_CHNL_GROUP_24G; group++) {
+			pwrinfo24g->index_cck_base[rfPath][group] = hwinfo[eeAddr++];
+			if (pwrinfo24g->index_cck_base[rfPath][group] == 0xFF)
+				pwrinfo24g->index_cck_base[rfPath][group] = 0x2D;
+		}
+		for (group = 0 ; group < MAX_CHNL_GROUP_24G - 1; group++) {
+			pwrinfo24g->index_bw40_base[rfPath][group] = hwinfo[eeAddr++];
+			if (pwrinfo24g->index_bw40_base[rfPath][group] == 0xFF)
+				pwrinfo24g->index_bw40_base[rfPath][group] = 0x2D;
+		}
+		for (TxCount = 0; TxCount < MAX_TX_COUNT; TxCount ++) {
+			if (TxCount == 0) {
+				pwrinfo24g->bw40_diff[rfPath][TxCount] = 0;
+				if (hwinfo[eeAddr] == 0xFF) {
+					pwrinfo24g->bw20_diff[rfPath][TxCount] = 0x02;
+				} else {
+					pwrinfo24g->bw20_diff[rfPath][TxCount] = (hwinfo[eeAddr] & 0xf0) >> 4;
+					if (pwrinfo24g->bw20_diff[rfPath][TxCount] & BIT(3))	/*bit sign number to 8 bit sign number*/
+						pwrinfo24g->bw20_diff[rfPath][TxCount] |= 0xF0;
+				}
+
+				if (hwinfo[eeAddr] == 0xFF) {
+					pwrinfo24g->ofdm_diff[rfPath][TxCount] = 0x04;
+				} else {
+					pwrinfo24g->ofdm_diff[rfPath][TxCount] = (hwinfo[eeAddr] & 0x0f);
+					if(pwrinfo24g->ofdm_diff[rfPath][TxCount] & BIT(3))		/*bit sign number to 8 bit sign number*/
+						pwrinfo24g->ofdm_diff[rfPath][TxCount] |= 0xF0;
+				}
+				pwrinfo24g->cck_diff[rfPath][TxCount] = 0;
+				eeAddr++;
+			} else {
+				if (hwinfo[eeAddr] == 0xFF) {
+					pwrinfo24g->bw40_diff[rfPath][TxCount] = 0xFE;
+				} else {
+					pwrinfo24g->bw40_diff[rfPath][TxCount] = (hwinfo[eeAddr]&0xf0) >> 4;
+					if (pwrinfo24g->bw40_diff[rfPath][TxCount] & BIT(3))
+						pwrinfo24g->bw40_diff[rfPath][TxCount] |= 0xF0;
+				}
+
+				if (hwinfo[eeAddr] == 0xFF) {
+					pwrinfo24g->bw20_diff[rfPath][TxCount] = 0xFE;
+				} else {
+					pwrinfo24g->bw20_diff[rfPath][TxCount] = (hwinfo[eeAddr] & 0x0f);
+					if (pwrinfo24g->bw20_diff[rfPath][TxCount] & BIT(3))
+						pwrinfo24g->bw20_diff[rfPath][TxCount] |= 0xF0;
+				}
+
+				eeAddr++;
+
+				if (hwinfo[eeAddr] == 0xFF) {
+					pwrinfo24g->ofdm_diff[rfPath][TxCount] = 0xFE;
+				} else {
+					pwrinfo24g->ofdm_diff[rfPath][TxCount] = (hwinfo[eeAddr] & 0xf0) >> 4;
+					if(pwrinfo24g->ofdm_diff[rfPath][TxCount] & BIT(3))
+						pwrinfo24g->ofdm_diff[rfPath][TxCount] |= 0xF0;
+				}
+
+				if (hwinfo[eeAddr] == 0xFF) {
+					pwrinfo24g->cck_diff[rfPath][TxCount] = 0xFE;
+				} else {
+					pwrinfo24g->cck_diff[rfPath][TxCount] =	(hwinfo[eeAddr] & 0x0f);
+					if(pwrinfo24g->cck_diff[rfPath][TxCount] & BIT(3))
+						pwrinfo24g->cck_diff[rfPath][TxCount] |= 0xF0;
+				}
+				eeAddr++;
+			}
+		}
+
+		/*5G default value*/
+		for (group = 0 ; group < MAX_CHNL_GROUP_5G; group ++) {
+			pwrinfo5g->index_bw40_base[rfPath][group] = hwinfo[eeAddr++];
+			if (pwrinfo5g->index_bw40_base[rfPath][group] == 0xFF)
+				pwrinfo5g->index_bw40_base[rfPath][group] = 0xFE;
+		}
+
+		for (TxCount = 0; TxCount < MAX_TX_COUNT; TxCount++) {
+			if (TxCount == 0) {
+				pwrinfo5g->bw40_diff[rfPath][TxCount] = 0;
+				if (hwinfo[eeAddr] == 0xFF) {
+					pwrinfo5g->bw20_diff[rfPath][TxCount] = 0x0;
+				} else {
+					pwrinfo5g->bw20_diff[rfPath][0] = (hwinfo[eeAddr] & 0xf0) >> 4;
+					if(pwrinfo5g->bw20_diff[rfPath][TxCount] & BIT(3))
+						pwrinfo5g->bw20_diff[rfPath][TxCount] |= 0xF0;
+				}
+
+				if (hwinfo[eeAddr] == 0xFF) {
+					pwrinfo5g->ofdm_diff[rfPath][TxCount] = 0x4;
+				} else {
+					pwrinfo5g->ofdm_diff[rfPath][0] = (hwinfo[eeAddr] & 0x0f);
+					if(pwrinfo5g->ofdm_diff[rfPath][TxCount] & BIT(3))
+						pwrinfo5g->ofdm_diff[rfPath][TxCount] |= 0xF0;
+				}
+				eeAddr++;
+			} else {
+				if (hwinfo[eeAddr] == 0xFF) {
+					pwrinfo5g->bw40_diff[rfPath][TxCount] = 0xFE;
+				} else {
+					pwrinfo5g->bw40_diff[rfPath][TxCount]= (hwinfo[eeAddr] & 0xf0) >> 4;
+					if(pwrinfo5g->bw40_diff[rfPath][TxCount] & BIT(3))
+						pwrinfo5g->bw40_diff[rfPath][TxCount] |= 0xF0;
+				}
+
+				if (hwinfo[eeAddr] == 0xFF) {
+					pwrinfo5g->bw20_diff[rfPath][TxCount] = 0xFE;
+				} else {
+					pwrinfo5g->bw20_diff[rfPath][TxCount] = (hwinfo[eeAddr] & 0x0f);
+					if(pwrinfo5g->bw20_diff[rfPath][TxCount] & BIT(3))
+						pwrinfo5g->bw20_diff[rfPath][TxCount] |= 0xF0;
+				}
+				eeAddr++;
+			}
+		}
+
+		if (hwinfo[eeAddr] == 0xFF) {
+			pwrinfo5g->ofdm_diff[rfPath][1] = 0xFE;
+			pwrinfo5g->ofdm_diff[rfPath][2] = 0xFE;
+		} else {
+			pwrinfo5g->ofdm_diff[rfPath][1] =	(hwinfo[eeAddr] & 0xf0) >> 4;
+			pwrinfo5g->ofdm_diff[rfPath][2] =	(hwinfo[eeAddr] & 0x0f);
+		}
+		eeAddr++;
+		if (hwinfo[eeAddr] == 0xFF)
+			pwrinfo5g->ofdm_diff[rfPath][3] = 0xFE;
+		else
+			pwrinfo5g->ofdm_diff[rfPath][3] = (hwinfo[eeAddr] & 0x0f);
+
+		eeAddr++;
+
+		for (TxCount = 1; TxCount < MAX_TX_COUNT; TxCount++) {
+			if (pwrinfo5g->ofdm_diff[rfPath][TxCount] == 0xFF)
+				pwrinfo5g->ofdm_diff[rfPath][TxCount] = 0xFE;
+			else if(pwrinfo5g->ofdm_diff[rfPath][TxCount] & BIT(3))
+				pwrinfo5g->ofdm_diff[rfPath][TxCount] |= 0xF0;
+		}
+		for (TxCount = 0; TxCount < MAX_TX_COUNT; TxCount++) {
+			if (hwinfo[eeAddr] == 0xFF) {
+				pwrinfo5g->bw80_diff[rfPath][TxCount] = 0xFE;
+			} else {
+				pwrinfo5g->bw80_diff[rfPath][TxCount] =	(hwinfo[eeAddr] & 0xf0) >> 4;
+				if(pwrinfo5g->bw80_diff[rfPath][TxCount] & BIT(3))		//4bit sign number to 8 bit sign number
+					pwrinfo5g->bw80_diff[rfPath][TxCount] |= 0xF0;
+			}
+
+			if (hwinfo[eeAddr] == 0xFF) {
+				pwrinfo5g->bw160_diff[rfPath][TxCount] = 0xFE;
+			} else {
+				pwrinfo5g->bw160_diff[rfPath][TxCount]=	(hwinfo[eeAddr] & 0x0f);
+				if(pwrinfo5g->bw160_diff[rfPath][TxCount] & BIT(3))		//4bit sign number to 8 bit sign number
+					pwrinfo5g->bw160_diff[rfPath][TxCount] |= 0xF0;
+			}
+			eeAddr++;
+		}
+	}
+}
+
+static void _rtl8812ae_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
+						 bool autoload_fail,
+						 u8 *hwinfo)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct txpower_info_2g pwrinfo24g;
+	struct txpower_info_5g pwrinfo5g;
+	u8 channel5g[CHANNEL_MAX_NUMBER_5G] =
+				 {36,38,40,42,44,46,48,50,52,54,56,58,60,62,64,100,102,104,106,108,110,112,
+                          114,116,118,120,122,124,126,128,130,132,134,136,138,140,142,144,149,151,
+                          153,155,157,159,161,163,165,167,168,169,171,173,175,177};
+	u8 channel5g_80m[CHANNEL_MAX_NUMBER_5G_80M] = {42, 58, 106, 122, 138, 155, 171};
+	u8 rf_path, index;
+	u8 i;
+
+	_rtl8821ae_read_power_value_fromprom(hw, &pwrinfo24g, &pwrinfo5g, autoload_fail, hwinfo);
+
+	for (rf_path = 0; rf_path < 2; rf_path++) {
+		for (i = 0; i < CHANNEL_MAX_NUMBER_2G; i++) {
+			index = _rtl8821ae_get_chnl_group(i + 1);
+
+			if (i == CHANNEL_MAX_NUMBER_2G - 1) {
+				rtlefuse->txpwrlevel_cck[rf_path][i] =
+					pwrinfo24g.index_cck_base[rf_path][5];
+				rtlefuse->txpwrlevel_ht40_1s[rf_path][i] =
+					pwrinfo24g.index_bw40_base[rf_path][index];
+			} else {
+				rtlefuse->txpwrlevel_cck[rf_path][i] =
+					pwrinfo24g.index_cck_base[rf_path][index];
+				rtlefuse->txpwrlevel_ht40_1s[rf_path][i] =
+					pwrinfo24g.index_bw40_base[rf_path][index];
+			}
+		}
+
+		for (i = 0; i < CHANNEL_MAX_NUMBER_5G; i++) {
+			index = _rtl8821ae_get_chnl_group(channel5g[i]);
+			rtlefuse->txpwr_5g_bw40base[rf_path][i] = pwrinfo5g.index_bw40_base[rf_path][index];
+		}
+		for (i = 0; i < CHANNEL_MAX_NUMBER_5G_80M; i++) {
+			u8 upper, lower;
+			index = _rtl8821ae_get_chnl_group(channel5g_80m[i]);
+			upper = pwrinfo5g.index_bw40_base[rf_path][index];
+			lower = pwrinfo5g.index_bw40_base[rf_path][index + 1];
+
+			rtlefuse->txpwr_5g_bw80base[rf_path][i] = (upper + lower) / 2;
+		}
+		for (i = 0; i < MAX_TX_COUNT; i++) {
+			rtlefuse->txpwr_cckdiff[rf_path][i] = pwrinfo24g.cck_diff[rf_path][i];
+			rtlefuse->txpwr_legacyhtdiff[rf_path][i] = pwrinfo24g.ofdm_diff[rf_path][i];
+			rtlefuse->txpwr_ht20diff[rf_path][i] = pwrinfo24g.bw20_diff[rf_path][i];
+			rtlefuse->txpwr_ht40diff[rf_path][i] = pwrinfo24g.bw40_diff[rf_path][i];
+
+			rtlefuse->txpwr_5g_ofdmdiff[rf_path][i] = pwrinfo5g.ofdm_diff[rf_path][i];
+			rtlefuse->txpwr_5g_bw20diff[rf_path][i] = pwrinfo5g.bw20_diff[rf_path][i];
+			rtlefuse->txpwr_5g_bw40diff[rf_path][i] = pwrinfo5g.bw40_diff[rf_path][i];
+			rtlefuse->txpwr_5g_bw80diff[rf_path][i] = pwrinfo5g.bw80_diff[rf_path][i];
+		}
+	}
+
+	if (!autoload_fail){
+		rtlefuse->eeprom_regulatory =
+			hwinfo[EEPROM_RF_BOARD_OPTION] & 0x07;/*bit0~2*/
+		if (hwinfo[EEPROM_RF_BOARD_OPTION] == 0xFF)
+			rtlefuse->eeprom_regulatory = 0;
+	} else {
+		rtlefuse->eeprom_regulatory = 0;
+	}
+
+	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+	("eeprom_regulatory = 0x%x\n", rtlefuse->eeprom_regulatory ));
+}
+
+static void _rtl8821ae_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
+						 bool autoload_fail,
+						 u8 *hwinfo)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct txpower_info_2g pwrinfo24g;
+	struct txpower_info_5g pwrinfo5g;
+	u8 channel5g[CHANNEL_MAX_NUMBER_5G] =
+				{36,38,40,42,44,46,48,50,52,54,56,58,60,62,64,100,102,104,106,108,110,112,
+                 114,116,118,120,122,124,126,128,130,132,134,136,138,140,142,144,149,151,
+                 153,155,157,159,161,163,165,167,168,169,171,173,175,177};
+	u8 channel5g_80m[CHANNEL_MAX_NUMBER_5G_80M] = {42, 58, 106, 122, 138, 155, 171};
+	u8 rf_path, index;
+	u8 i;
+
+	_rtl8821ae_read_power_value_fromprom(hw, &pwrinfo24g, &pwrinfo5g, autoload_fail, hwinfo);
+
+	for (rf_path = 0; rf_path < 2; rf_path++) {
+		for (i = 0; i < CHANNEL_MAX_NUMBER_2G; i++) {
+			index = _rtl8821ae_get_chnl_group(i + 1);
+
+			if (i == CHANNEL_MAX_NUMBER_2G - 1) {
+				rtlefuse->txpwrlevel_cck[rf_path][i] = pwrinfo24g.index_cck_base[rf_path][5];
+				rtlefuse->txpwrlevel_ht40_1s[rf_path][i] = pwrinfo24g.index_bw40_base[rf_path][index];
+			} else {
+				rtlefuse->txpwrlevel_cck[rf_path][i] = pwrinfo24g.index_cck_base[rf_path][index];
+				rtlefuse->txpwrlevel_ht40_1s[rf_path][i] = pwrinfo24g.index_bw40_base[rf_path][index];
+			}
+		}
+
+		for (i = 0; i < CHANNEL_MAX_NUMBER_5G; i++) {
+			index = _rtl8821ae_get_chnl_group(channel5g[i]);
+			rtlefuse->txpwr_5g_bw40base[rf_path][i] = pwrinfo5g.index_bw40_base[rf_path][index];
+		}
+		for (i = 0; i < CHANNEL_MAX_NUMBER_5G_80M; i++) {
+			u8 upper, lower;
+			index = _rtl8821ae_get_chnl_group(channel5g_80m[i]);
+			upper = pwrinfo5g.index_bw40_base[rf_path][index];
+			lower = pwrinfo5g.index_bw40_base[rf_path][index + 1];
+
+			rtlefuse->txpwr_5g_bw80base[rf_path][i] = (upper + lower) / 2;
+		}
+		for (i = 0; i < MAX_TX_COUNT; i++) {
+			rtlefuse->txpwr_cckdiff[rf_path][i] = pwrinfo24g.cck_diff[rf_path][i];
+			rtlefuse->txpwr_legacyhtdiff[rf_path][i] = pwrinfo24g.ofdm_diff[rf_path][i];
+			rtlefuse->txpwr_ht20diff[rf_path][i] = pwrinfo24g.bw20_diff[rf_path][i];
+			rtlefuse->txpwr_ht40diff[rf_path][i] = pwrinfo24g.bw40_diff[rf_path][i];
+
+			rtlefuse->txpwr_5g_ofdmdiff[rf_path][i] = pwrinfo5g.ofdm_diff[rf_path][i];
+			rtlefuse->txpwr_5g_bw20diff[rf_path][i] = pwrinfo5g.bw20_diff[rf_path][i];
+			rtlefuse->txpwr_5g_bw40diff[rf_path][i] = pwrinfo5g.bw40_diff[rf_path][i];
+			rtlefuse->txpwr_5g_bw80diff[rf_path][i] = pwrinfo5g.bw80_diff[rf_path][i];
+		}
+	}
+
+	if (!autoload_fail){
+		rtlefuse->eeprom_regulatory = hwinfo[EEPROM_RF_BOARD_OPTION] & 0x07;/*bit0~2*/
+		if (hwinfo[EEPROM_RF_BOARD_OPTION] == 0xFF)
+			rtlefuse->eeprom_regulatory = 0;
+	} else {
+		rtlefuse->eeprom_regulatory = 0;
+	}
+
+	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+	("eeprom_regulatory = 0x%x\n", rtlefuse->eeprom_regulatory ));
+}
+
+static void _rtl8812ae_read_adapter_info(struct ieee80211_hw *hw, bool b_pseudo_test )
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	u16 i, usvalue;
+	u8 hwinfo[HWSET_MAX_SIZE];
+	u16 eeprom_id;
+
+	if (b_pseudo_test) {
+		/* need add */
+	}
+
+	if (rtlefuse->epromtype == EEPROM_BOOT_EFUSE) {
+		rtl_efuse_shadow_map_update(hw);
+		memcpy(hwinfo, &rtlefuse->efuse_map[EFUSE_INIT_MAP][0],
+		       HWSET_MAX_SIZE);
+	} else if (rtlefuse->epromtype == EEPROM_93C46) {
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("RTL819X Not boot from eeprom, check it !!"));
+	}
+
+	RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_DMESG, ("MAP \n"),
+		      hwinfo, HWSET_MAX_SIZE);
+
+	eeprom_id = *((u16 *) & hwinfo[0]);
+	if (eeprom_id != RTL_EEPROM_ID) {
+		RT_TRACE(COMP_ERR, DBG_WARNING,
+			 ("EEPROM ID(%#x) is invalid!!\n", eeprom_id));
+		rtlefuse->autoload_failflag = true;
+	} else {
+		RT_TRACE(COMP_INIT, DBG_LOUD, ("Autoload OK\n"));
+		rtlefuse->autoload_failflag = false;
+	}
+
+	if (rtlefuse->autoload_failflag == true) {
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("RTL8812AE autoload_failflag, check it !!"));
+		return;
+	}
+
+	rtlefuse->eeprom_version = *(u8 *) & hwinfo[EEPROM_VERSION];
+	if (rtlefuse->eeprom_version == 0xff)
+			rtlefuse->eeprom_version = 0;
+
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("EEPROM version: 0x%2x\n", rtlefuse->eeprom_version));
+
+	rtlefuse->eeprom_vid = *(u16 *) &hwinfo[EEPROM_VID];
+	rtlefuse->eeprom_did = *(u16 *) &hwinfo[EEPROM_DID];
+	rtlefuse->eeprom_svid = *(u16 *) &hwinfo[EEPROM_SVID];
+	rtlefuse->eeprom_smid = *(u16 *) &hwinfo[EEPROM_SMID];
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("EEPROMId = 0x%4x\n", eeprom_id));
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("EEPROM VID = 0x%4x\n", rtlefuse->eeprom_vid));
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("EEPROM DID = 0x%4x\n", rtlefuse->eeprom_did));
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("EEPROM SVID = 0x%4x\n", rtlefuse->eeprom_svid));
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("EEPROM SMID = 0x%4x\n", rtlefuse->eeprom_smid));
+
+	/*customer ID*/
+	rtlefuse->eeprom_oemid = *(u8 *) & hwinfo[EEPROM_CUSTOMER_ID];
+	if (rtlefuse->eeprom_oemid == 0xFF)
+		rtlefuse->eeprom_oemid = 0;
+
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("EEPROM Customer ID: 0x%2x\n", rtlefuse->eeprom_oemid));
+
+	for (i = 0; i < 6; i += 2) {
+		usvalue = *(u16 *) & hwinfo[EEPROM_MAC_ADDR + i];
+		*((u16 *) (&rtlefuse->dev_addr[i])) = usvalue;
+	}
+
+	RT_TRACE(COMP_INIT, DBG_DMESG,
+		 ("dev_addr: %pM\n", rtlefuse->dev_addr));
+
+	_rtl8812ae_read_txpower_info_from_hwpg(hw,
+		rtlefuse->autoload_failflag, hwinfo);
+
+	/*board type*/
+	rtlefuse->board_type = (((*(u8 *) & hwinfo[EEPROM_RF_BOARD_OPTION]) & 0xE0 ) >> 5);
+	if ((*(u8 *) & hwinfo[EEPROM_RF_BOARD_OPTION]) == 0xff )
+		rtlefuse->board_type = 0;
+	rtlhal->boad_type = rtlefuse->board_type;
+
+	rtl8812ae_read_bt_coexist_info_from_hwpg(hw,
+			rtlefuse->autoload_failflag, hwinfo);
+
+	rtlefuse->eeprom_channelplan = *(u8 *) & hwinfo[EEPROM_CHANNELPLAN];
+	if (rtlefuse->eeprom_channelplan == 0xff)
+		rtlefuse->eeprom_channelplan = 0x7F;
+
+	/* set channel paln to world wide 13 */
+	//rtlefuse->channel_plan = (u8) rtlefuse->eeprom_channelplan;
+
+	/*parse xtal*/
+	rtlefuse->crystalcap = hwinfo[EEPROM_XTAL_8821AE];
+	if ( rtlefuse->crystalcap == 0xFF )
+		rtlefuse->crystalcap = 0x20;
+
+	rtlefuse->eeprom_thermalmeter = *(u8 *) & hwinfo[EEPROM_THERMAL_METER];
+	if ((rtlefuse->eeprom_thermalmeter == 0xff) ||rtlefuse->autoload_failflag )
+	{
+		rtlefuse->b_apk_thermalmeterignore = true;
+		rtlefuse->eeprom_thermalmeter = 0xff;
+	}
+
+	rtlefuse->thermalmeter[0] = rtlefuse->eeprom_thermalmeter;
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("thermalmeter = 0x%x\n", rtlefuse->eeprom_thermalmeter));
+
+	if (rtlefuse->autoload_failflag == false) {
+		rtlefuse->antenna_div_cfg = *(u8 *) & hwinfo[EEPROM_RF_BOARD_OPTION] & 0x18 >> 3;
+		if (*(u8 *) & hwinfo[EEPROM_RF_BOARD_OPTION] == 0xff)
+			rtlefuse->antenna_div_cfg = 0x00;
+		/*if (BT_1ant())
+			rtlefuse->antenna_div_cfg = 0;*/
+		rtlefuse->antenna_div_type = *(u8 *) & hwinfo[EEPROM_RF_ANTENNA_OPT_88E];
+		if (rtlefuse->antenna_div_type == 0xFF)
+		{
+			rtlefuse->antenna_div_type = FIXED_HW_ANTDIV;
+		}
+	} else {
+		rtlefuse->antenna_div_cfg = 0;
+		rtlefuse->antenna_div_type = 0;
+	}
+
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		("SWAS: bHwAntDiv = %x, TRxAntDivType = %x\n",
+                rtlefuse->antenna_div_cfg, rtlefuse->antenna_div_type));
+
+	/*Hal_ReadPAType_8821A()*/
+	/*Hal_EfuseParseRateIndicationOption8821A()*/
+	/*Hal_ReadEfusePCIeCap8821AE()*/
+
+	pcipriv->ledctl.bled_opendrain = true;
+
+	if (rtlhal->oem_id == RT_CID_DEFAULT) {
+		switch (rtlefuse->eeprom_oemid) {
+		case RT_CID_DEFAULT:
+			break;
+		case EEPROM_CID_TOSHIBA:
+			rtlhal->oem_id = RT_CID_TOSHIBA;
+			break;
+		case EEPROM_CID_CCX:
+			rtlhal->oem_id = RT_CID_CCX;
+			break;
+		case EEPROM_CID_QMI:
+			rtlhal->oem_id = RT_CID_819x_QMI;
+			break;
+		case EEPROM_CID_WHQL:
+			break;
+		default:
+			break;
+
+		}
+	}
+}
+
+static void _rtl8821ae_read_adapter_info(struct ieee80211_hw *hw, bool b_pseudo_test )
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	u16 i, usvalue;
+	u8 hwinfo[HWSET_MAX_SIZE];
+	u16 eeprom_id;
+
+	if (b_pseudo_test) {
+		/* need add */
+	}
+
+	if (rtlefuse->epromtype == EEPROM_BOOT_EFUSE) {
+		rtl_efuse_shadow_map_update(hw);
+		memcpy(hwinfo, &rtlefuse->efuse_map[EFUSE_INIT_MAP][0],
+		       HWSET_MAX_SIZE);
+	} else if (rtlefuse->epromtype == EEPROM_93C46) {
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("RTL819X Not boot from eeprom, check it !!"));
+	}
+
+	RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_DMESG, ("MAP \n"),
+		      hwinfo, HWSET_MAX_SIZE);
+
+	eeprom_id = *((u16 *) & hwinfo[0]);
+	if (eeprom_id != RTL_EEPROM_ID) {
+		RT_TRACE(COMP_ERR, DBG_WARNING,
+			 ("EEPROM ID(%#x) is invalid!!\n", eeprom_id));
+		rtlefuse->autoload_failflag = true;
+	} else {
+		RT_TRACE(COMP_INIT, DBG_LOUD, ("Autoload OK\n"));
+		rtlefuse->autoload_failflag = false;
+	}
+
+	if (rtlefuse->autoload_failflag == true) {
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("RTL8812AE autoload_failflag, check it !!"));
+		return;
+	}
+
+	rtlefuse->eeprom_version = *(u8 *) & hwinfo[EEPROM_VERSION];
+	if (rtlefuse->eeprom_version == 0xff)
+			rtlefuse->eeprom_version = 0;
+
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("EEPROM version: 0x%2x\n", rtlefuse->eeprom_version));
+
+	rtlefuse->eeprom_vid = *(u16 *) &hwinfo[EEPROM_VID];
+	rtlefuse->eeprom_did = *(u16 *) &hwinfo[EEPROM_DID];
+	rtlefuse->eeprom_svid = *(u16 *) &hwinfo[EEPROM_SVID];
+	rtlefuse->eeprom_smid = *(u16 *) &hwinfo[EEPROM_SMID];
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("EEPROMId = 0x%4x\n", eeprom_id));
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("EEPROM VID = 0x%4x\n", rtlefuse->eeprom_vid));
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("EEPROM DID = 0x%4x\n", rtlefuse->eeprom_did));
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("EEPROM SVID = 0x%4x\n", rtlefuse->eeprom_svid));
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("EEPROM SMID = 0x%4x\n", rtlefuse->eeprom_smid));
+
+	/*customer ID*/
+	rtlefuse->eeprom_oemid = *(u8 *) & hwinfo[EEPROM_CUSTOMER_ID];
+	if (rtlefuse->eeprom_oemid == 0xFF)
+		rtlefuse->eeprom_oemid = 0;
+
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("EEPROM Customer ID: 0x%2x\n", rtlefuse->eeprom_oemid));
+
+	for (i = 0; i < 6; i += 2) {
+		usvalue = *(u16 *) & hwinfo[EEPROM_MAC_ADDR + i];
+		*((u16 *) (&rtlefuse->dev_addr[i])) = usvalue;
+	}
+
+	RT_TRACE(COMP_INIT, DBG_DMESG,
+		 ("dev_addr: %pM\n", rtlefuse->dev_addr));
+
+	_rtl8821ae_read_txpower_info_from_hwpg(hw,
+		rtlefuse->autoload_failflag, hwinfo);
+
+	/*board type*/
+	rtlefuse->board_type = (((*(u8 *) & hwinfo[EEPROM_RF_BOARD_OPTION]) & 0xE0 ) >> 5);
+	if ((*(u8 *) & hwinfo[EEPROM_RF_BOARD_OPTION]) == 0xff )
+		rtlefuse->board_type = 0;
+	rtlhal->boad_type = rtlefuse->board_type;
+
+	rtl8821ae_read_bt_coexist_info_from_hwpg(hw,
+			rtlefuse->autoload_failflag, hwinfo);
+
+	rtlefuse->eeprom_channelplan = *(u8 *) & hwinfo[EEPROM_CHANNELPLAN];
+	if (rtlefuse->eeprom_channelplan == 0xff)
+		rtlefuse->eeprom_channelplan = 0x7F;
+
+	/* set channel paln to world wide 13 */
+	//rtlefuse->channel_plan = (u8) rtlefuse->eeprom_channelplan;
+
+	/*parse xtal*/
+	rtlefuse->crystalcap = hwinfo[EEPROM_XTAL_8821AE];
+	if ( rtlefuse->crystalcap == 0xFF )
+		rtlefuse->crystalcap = 0x20;
+
+	rtlefuse->eeprom_thermalmeter = *(u8 *) & hwinfo[EEPROM_THERMAL_METER];
+	if ((rtlefuse->eeprom_thermalmeter == 0xff) ||rtlefuse->autoload_failflag )
+	{
+		rtlefuse->b_apk_thermalmeterignore = true;
+		rtlefuse->eeprom_thermalmeter = 0x18;
+	}
+
+	rtlefuse->thermalmeter[0] = rtlefuse->eeprom_thermalmeter;
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("thermalmeter = 0x%x\n", rtlefuse->eeprom_thermalmeter));
+
+	if (rtlefuse->autoload_failflag == false) {
+		rtlefuse->antenna_div_cfg = (*(u8 *) & hwinfo[EEPROM_RF_BOARD_OPTION] & BIT(3))?true:false;
+		/*if (BT_1ant())
+			rtlefuse->antenna_div_cfg = 0;*/
+
+		rtlefuse->antenna_div_type = CG_TRX_HW_ANTDIV;
+	} else {
+		rtlefuse->antenna_div_cfg = 0;
+		rtlefuse->antenna_div_type = 0;
+	}
+
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		("SWAS: bHwAntDiv = %x, TRxAntDivType = %x\n",
+                rtlefuse->antenna_div_cfg, rtlefuse->antenna_div_type));
+
+	pcipriv->ledctl.bled_opendrain = true;
+
+	if (rtlhal->oem_id == RT_CID_DEFAULT) {
+		switch (rtlefuse->eeprom_oemid) {
+		case RT_CID_DEFAULT:
+			break;
+		case EEPROM_CID_TOSHIBA:
+			rtlhal->oem_id = RT_CID_TOSHIBA;
+			break;
+		case EEPROM_CID_CCX:
+			rtlhal->oem_id = RT_CID_CCX;
+			break;
+		case EEPROM_CID_QMI:
+			rtlhal->oem_id = RT_CID_819x_QMI;
+			break;
+		case EEPROM_CID_WHQL:
+			break;
+		default:
+			break;
+		}
+	}
+}
+
+
+/*static void _rtl8821ae_hal_customized_behavior(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	pcipriv->ledctl.bled_opendrain = true;
+	switch (rtlhal->oem_id) {
+	case RT_CID_819x_HP:
+		pcipriv->ledctl.bled_opendrain = true;
+		break;
+	case RT_CID_819x_Lenovo:
+	case RT_CID_DEFAULT:
+	case RT_CID_TOSHIBA:
+	case RT_CID_CCX:
+	case RT_CID_819x_Acer:
+	case RT_CID_WHQL:
+	default:
+		break;
+	}
+	RT_TRACE(COMP_INIT, DBG_DMESG,
+		 ("RT Customized ID: 0x%02X\n", rtlhal->oem_id));
+}*/
+
+void rtl8821ae_read_eeprom_info(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 tmp_u1b;
+
+	rtlhal->version = _rtl8821ae_read_chip_version(hw);
+
+	if (get_rf_type(rtlphy) == RF_1T1R)
+		rtlpriv->dm.brfpath_rxenable[0] = true;
+	else
+		rtlpriv->dm.brfpath_rxenable[0] =
+		    rtlpriv->dm.brfpath_rxenable[1] = true;
+	RT_TRACE(COMP_INIT, DBG_LOUD, ("VersionID = 0x%4x\n",
+						rtlhal->version));
+
+	tmp_u1b = rtl_read_byte(rtlpriv, REG_9346CR);
+	if (tmp_u1b & BIT(4)) {
+		RT_TRACE(COMP_INIT, DBG_DMESG, ("Boot from EEPROM\n"));
+		rtlefuse->epromtype = EEPROM_93C46;
+	} else {
+		RT_TRACE(COMP_INIT, DBG_DMESG, ("Boot from EFUSE\n"));
+		rtlefuse->epromtype = EEPROM_BOOT_EFUSE;
+	}
+
+	if (tmp_u1b & BIT(5)) {
+		RT_TRACE(COMP_INIT, DBG_LOUD, ("Autoload OK\n"));
+		rtlefuse->autoload_failflag = false;
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+			_rtl8812ae_read_adapter_info(hw, false);
+		else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE)
+			_rtl8821ae_read_adapter_info(hw, false);
+	} else {
+			RT_TRACE(COMP_ERR, DBG_EMERG, ("Autoload ERR!!\n"));
+	}
+	/*hal_ReadRFType_8812A()*/
+	//_rtl8821ae_hal_customized_behavior(hw);
+}
+
+static void rtl8821ae_update_hal_rate_table(struct ieee80211_hw *hw,
+		struct ieee80211_sta *sta)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u32 ratr_value;
+	u8 ratr_index = 0;
+	u8 b_nmode = mac->ht_enable;
+	u8 mimo_ps = IEEE80211_SMPS_OFF;
+	u16 shortgi_rate;
+	u32 tmp_ratr_value;
+	u8 b_curtxbw_40mhz = mac->bw_40;
+	u8 b_curshortgi_40mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40) ?
+				1 : 0;
+	u8 b_curshortgi_20mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20) ?
+				1 : 0;
+	enum wireless_mode wirelessmode = mac->mode;
+
+	if (rtlhal->current_bandtype == BAND_ON_5G)
+		ratr_value = sta->supp_rates[1] << 4;
+	else
+		ratr_value = sta->supp_rates[0];
+	if (mac->opmode == NL80211_IFTYPE_ADHOC)
+		ratr_value = 0xfff;
+	ratr_value |= (sta->ht_cap.mcs.rx_mask[1] << 20 |
+			sta->ht_cap.mcs.rx_mask[0] << 12);
+	switch (wirelessmode) {
+	case WIRELESS_MODE_B:
+		if (ratr_value & 0x0000000c)
+			ratr_value &= 0x0000000d;
+		else
+			ratr_value &= 0x0000000f;
+		break;
+	case WIRELESS_MODE_G:
+		ratr_value &= 0x00000FF5;
+		break;
+	case WIRELESS_MODE_N_24G:
+	case WIRELESS_MODE_N_5G:
+		b_nmode = 1;
+		if (mimo_ps == IEEE80211_SMPS_STATIC) {
+			ratr_value &= 0x0007F005;
+		} else {
+			u32 ratr_mask;
+
+			if (get_rf_type(rtlphy) == RF_1T2R ||
+			    get_rf_type(rtlphy) == RF_1T1R)
+				ratr_mask = 0x000ff005;
+			else
+				ratr_mask = 0x0f0ff005;
+
+			ratr_value &= ratr_mask;
+		}
+		break;
+	default:
+		if (rtlphy->rf_type == RF_1T2R)
+			ratr_value &= 0x000ff0ff;
+		else
+			ratr_value &= 0x0f0ff0ff;
+
+		break;
+	}
+
+	if ( (rtlpcipriv->btcoexist.bt_coexistence) &&
+	     (rtlpcipriv->btcoexist.bt_coexist_type == BT_CSR_BC4) &&
+	     (rtlpcipriv->btcoexist.bt_cur_state) &&
+	     (rtlpcipriv->btcoexist.bt_ant_isolation) &&
+	     ((rtlpcipriv->btcoexist.bt_service == BT_SCO)||
+	     (rtlpcipriv->btcoexist.bt_service == BT_BUSY)) )
+		ratr_value &= 0x0fffcfc0;
+	else
+		ratr_value &= 0x0FFFFFFF;
+
+	if (b_nmode && ((b_curtxbw_40mhz &&
+			 b_curshortgi_40mhz) || (!b_curtxbw_40mhz &&
+						 b_curshortgi_20mhz))) {
+
+		ratr_value |= 0x10000000;
+		tmp_ratr_value = (ratr_value >> 12);
+
+		for (shortgi_rate = 15; shortgi_rate > 0; shortgi_rate--) {
+			if ((1 << shortgi_rate) & tmp_ratr_value)
+				break;
+		}
+
+		shortgi_rate = (shortgi_rate << 12) | (shortgi_rate << 8) |
+		    (shortgi_rate << 4) | (shortgi_rate);
+	}
+
+	rtl_write_dword(rtlpriv, REG_ARFR0 + ratr_index * 4, ratr_value);
+
+	RT_TRACE(COMP_RATR, DBG_DMESG,
+		 ("%x\n", rtl_read_dword(rtlpriv, REG_ARFR0)));
+}
+
+
+static u8 _rtl8821ae_mrate_idx_to_arfr_id(
+	struct ieee80211_hw *hw, u8 rate_index,
+	enum wireless_mode wirelessmode)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 ret = 0;
+	switch(rate_index){
+		case RATR_INX_WIRELESS_NGB:
+			if(rtlphy->rf_type == RF_1T1R)
+				ret = 1;
+			else
+				ret = 0;
+			;break;
+		case RATR_INX_WIRELESS_N:
+		case RATR_INX_WIRELESS_NG:
+			if(rtlphy->rf_type == RF_1T1R)
+				ret = 5;
+			else
+				ret = 4;
+			;break;
+		case RATR_INX_WIRELESS_NB:
+			if(rtlphy->rf_type == RF_1T1R)
+				ret = 3;
+			else
+				ret = 2;
+			;break;
+		case RATR_INX_WIRELESS_GB:
+			ret = 6;
+			break;
+		case RATR_INX_WIRELESS_G:
+			ret = 7;
+			break;
+		case RATR_INX_WIRELESS_B:
+			ret = 8;
+			break;
+		case RATR_INX_WIRELESS_MC:
+			if ((wirelessmode == WIRELESS_MODE_B)
+				|| (wirelessmode == WIRELESS_MODE_G)
+				|| (wirelessmode == WIRELESS_MODE_N_24G)
+				|| (wirelessmode == WIRELESS_MODE_AC_24G))
+				ret = 6;
+			else
+				ret = 7;
+		case RATR_INX_WIRELESS_AC_5N:
+			if(rtlphy->rf_type == RF_1T1R)
+				ret = 10;
+			else
+				ret = 9;
+			break;
+		case RATR_INX_WIRELESS_AC_24N:
+			if(rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80)
+			{
+				if(rtlphy->rf_type == RF_1T1R)
+					ret = 10;
+				else
+					ret = 9;
+			} else {
+				if(rtlphy->rf_type == RF_1T1R)
+					ret = 11;
+				else
+					ret = 12;
+			}
+			break;
+		default:
+			ret = 0;break;
+	}
+	return ret;
+}
+
+static void rtl8821ae_update_hal_rate_mask(struct ieee80211_hw *hw,
+		struct ieee80211_sta *sta, u8 rssi_level)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_sta_info * sta_entry = NULL;
+	u32 ratr_bitmap;
+	u8 ratr_index;
+	u8 b_curtxbw_40mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40)
+				? 1 : 0;
+	u8 b_curshortgi_40mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40) ?
+				1 : 0;
+	u8 b_curshortgi_20mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20) ?
+				1 : 0;
+	enum wireless_mode wirelessmode = 0;
+	bool b_shortgi = false;
+	u8 rate_mask[7];
+	u8 macid = 0;
+	u8 mimo_ps = IEEE80211_SMPS_OFF;
+
+	sta_entry = (struct rtl_sta_info *) sta->drv_priv;
+	wirelessmode = sta_entry->wireless_mode;
+	if (mac->opmode == NL80211_IFTYPE_STATION ||
+		mac->opmode == NL80211_IFTYPE_MESH_POINT)
+		b_curtxbw_40mhz = mac->bw_40;
+	else if (mac->opmode == NL80211_IFTYPE_AP ||
+		mac->opmode == NL80211_IFTYPE_ADHOC)
+		macid = sta->aid + 1;
+
+	ratr_bitmap = sta->supp_rates[0];
+
+	if (mac->opmode == NL80211_IFTYPE_ADHOC)
+		ratr_bitmap = 0xfff;
+
+	ratr_bitmap |= (sta->ht_cap.mcs.rx_mask[1] << 20 |
+			sta->ht_cap.mcs.rx_mask[0] << 12);
+/*mac id owner*/
+	switch (wirelessmode) {
+	case WIRELESS_MODE_B:
+		ratr_index = RATR_INX_WIRELESS_B;
+		if (ratr_bitmap & 0x0000000c)
+			ratr_bitmap &= 0x0000000d;
+		else
+			ratr_bitmap &= 0x0000000f;
+		break;
+	case WIRELESS_MODE_G:
+		ratr_index = RATR_INX_WIRELESS_GB;
+
+		if (rssi_level == 1)
+			ratr_bitmap &= 0x00000f00;
+		else if (rssi_level == 2)
+			ratr_bitmap &= 0x00000ff0;
+		else
+			ratr_bitmap &= 0x00000ff5;
+		break;
+	case WIRELESS_MODE_A:
+		ratr_index = RATR_INX_WIRELESS_G;
+		ratr_bitmap &= 0x00000ff0;
+		break;
+	case WIRELESS_MODE_N_24G:
+	case WIRELESS_MODE_N_5G:
+		if (wirelessmode == WIRELESS_MODE_N_24G)
+			ratr_index = RATR_INX_WIRELESS_NGB;
+		else
+			ratr_index = RATR_INX_WIRELESS_NG;
+
+		if (mimo_ps == IEEE80211_SMPS_STATIC  || mimo_ps == IEEE80211_SMPS_DYNAMIC) {
+			if (rssi_level == 1)
+				ratr_bitmap &= 0x00070000;
+			else if (rssi_level == 2)
+				ratr_bitmap &= 0x0007f000;
+			else
+				ratr_bitmap &= 0x0007f005;
+		} else {
+			if ( rtlphy->rf_type == RF_1T1R) {
+				if (b_curtxbw_40mhz) {
+					if (rssi_level == 1)
+						ratr_bitmap &= 0x000f0000;
+					else if (rssi_level == 2)
+						ratr_bitmap &= 0x000ff000;
+					else
+						ratr_bitmap &= 0x000ff015;
+				} else {
+					if (rssi_level == 1)
+						ratr_bitmap &= 0x000f0000;
+					else if (rssi_level == 2)
+						ratr_bitmap &= 0x000ff000;
+					else
+						ratr_bitmap &= 0x000ff005;
+				}
+			} else {
+				if (b_curtxbw_40mhz) {
+					if (rssi_level == 1)
+						ratr_bitmap &= 0x0fff0000;
+					else if (rssi_level == 2)
+						ratr_bitmap &= 0x0ffff000;
+					else
+						ratr_bitmap &= 0x0ffff015;
+				} else {
+					if (rssi_level == 1)
+						ratr_bitmap &= 0x0fff0000;
+					else if (rssi_level == 2)
+						ratr_bitmap &= 0x0ffff000;
+					else
+						ratr_bitmap &= 0x0ffff005;
+				}
+			}
+		}
+		if ((b_curtxbw_40mhz && b_curshortgi_40mhz) ||
+		    (!b_curtxbw_40mhz && b_curshortgi_20mhz)) {
+
+			if (macid == 0)
+				b_shortgi = true;
+			else if (macid == 1)
+				b_shortgi = false;
+		}
+		break;
+
+	case WIRELESS_MODE_AC_24G:
+		ratr_index = RATR_INX_WIRELESS_AC_24N;
+		if(rssi_level == 1)
+			ratr_bitmap &= 0xfc3f0000;
+		else if(rssi_level == 2)
+			ratr_bitmap &= 0xfffff000;
+		else
+			ratr_bitmap &= 0xffffffff;
+		break;
+
+	case WIRELESS_MODE_AC_5G:
+		ratr_index = RATR_INX_WIRELESS_AC_5N;
+
+		if (rtlphy->rf_type == RF_1T1R)
+		{
+			if(rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+			{
+				if(rssi_level == 1)				/*add by Gary for ac-series*/
+					ratr_bitmap &= 0x003f8000;
+				else if (rssi_level == 2)
+					ratr_bitmap &= 0x003ff000;
+				else
+					ratr_bitmap &= 0x003ff010;
+			}
+			else
+				ratr_bitmap &= 0x000ff010;
+		}
+		else
+		{
+			if(rssi_level == 1)				/* add by Gary for ac-series*/
+				ratr_bitmap &= 0xfe3f8000;       /*VHT 2SS MCS3~9*/
+			else if (rssi_level == 2)
+				ratr_bitmap &= 0xfffff000;       /*VHT 2SS MCS0~9*/
+			else
+				ratr_bitmap &= 0xfffff010;       /*All*/
+		}
+		break;
+
+	default:
+		ratr_index = RATR_INX_WIRELESS_NGB;
+
+		if (rtlphy->rf_type == RF_1T2R)
+			ratr_bitmap &= 0x000ff0ff;
+		else
+			ratr_bitmap &= 0x0f0ff0ff;
+		break;
+
+	}
+
+	sta_entry->ratr_index = ratr_index;
+
+	RT_TRACE(COMP_RATR, DBG_DMESG,
+		 ("ratr_bitmap :%x\n", ratr_bitmap));
+	*(u32 *) & rate_mask = EF4BYTE((ratr_bitmap & 0x0fffffff) |
+				       (ratr_index << 28));
+	rate_mask[0] = macid;
+	rate_mask[1] = _rtl8821ae_mrate_idx_to_arfr_id(hw, ratr_index, wirelessmode) | (b_shortgi ? 0x80 : 0x00);
+	rate_mask[2] = b_curtxbw_40mhz;
+	/* if (prox_priv->proxim_modeinfo->power_output > 0)
+		rate_mask[2] |= BIT(6); */
+
+	rate_mask[3] = (u8)(ratr_bitmap & 0x000000ff);
+	rate_mask[4] = (u8)((ratr_bitmap & 0x0000ff00) >>8);
+	rate_mask[5] = (u8)((ratr_bitmap & 0x00ff0000) >> 16);
+	rate_mask[6] = (u8)((ratr_bitmap & 0xff000000) >> 24);
+
+	RT_TRACE(COMP_RATR, DBG_DMESG, ("Rate_index:%x, "
+						 "ratr_val:%x, %x:%x:%x:%x:%x:%x:%x\n",
+						 ratr_index, ratr_bitmap,
+						 rate_mask[0], rate_mask[1],
+						 rate_mask[2], rate_mask[3],
+						 rate_mask[4], rate_mask[5],
+						 rate_mask[6]));
+	rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_RA_MASK, 7, rate_mask);
+	_rtl8821ae_set_bcn_ctrl_reg(hw, BIT(3), 0);
+}
+
+void rtl8821ae_update_hal_rate_tbl(struct ieee80211_hw *hw,
+		struct ieee80211_sta *sta, u8 rssi_level)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	if (rtlpriv->dm.b_useramask)
+		rtl8821ae_update_hal_rate_mask(hw, sta, rssi_level);
+	else
+		/*RT_TRACE(COMP_RATR,DBG_LOUD,("rtl8821ae_update_hal_rate_tbl(): Error! 8821ae FW RA Only"));*/
+		rtl8821ae_update_hal_rate_table(hw, sta);
+}
+
+void rtl8821ae_update_channel_access_setting(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u16 sifs_timer;
+
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SLOT_TIME,
+				      (u8 *) & mac->slot_time);
+	if (!mac->ht_enable)
+		sifs_timer = 0x0a0a;
+	else
+		sifs_timer = 0x0e0e;
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SIFS, (u8 *) & sifs_timer);
+}
+
+bool rtl8821ae_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 * valid)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	enum rf_pwrstate e_rfpowerstate_toset, cur_rfstate;
+	u8 u1tmp = 0;
+	bool b_actuallyset = false;
+
+	if (rtlpriv->rtlhal.being_init_adapter)
+		return false;
+
+	if (ppsc->b_swrf_processing)
+		return false;
+
+	spin_lock(&rtlpriv->locks.rf_ps_lock);
+	if (ppsc->rfchange_inprogress) {
+		spin_unlock(&rtlpriv->locks.rf_ps_lock);
+		return false;
+	} else {
+		ppsc->rfchange_inprogress = true;
+		spin_unlock(&rtlpriv->locks.rf_ps_lock);
+	}
+
+	cur_rfstate = ppsc->rfpwr_state;
+
+	rtl_write_byte(rtlpriv, REG_GPIO_IO_SEL_2,
+					rtl_read_byte(rtlpriv, REG_GPIO_IO_SEL_2) & ~(BIT(1)));
+
+	u1tmp = rtl_read_byte(rtlpriv, REG_GPIO_PIN_CTRL_2);
+
+	if (rtlphy->polarity_ctl) {
+		e_rfpowerstate_toset = (u1tmp & BIT(1)) ? ERFOFF : ERFON;
+	} else {
+		e_rfpowerstate_toset = (u1tmp & BIT(1)) ? ERFON : ERFOFF;
+	}
+
+	if ((ppsc->b_hwradiooff == true) && (e_rfpowerstate_toset == ERFON)) {
+		RT_TRACE(COMP_RF, DBG_DMESG,
+			 ("GPIOChangeRF  - HW Radio ON, RF ON\n"));
+
+		e_rfpowerstate_toset = ERFON;
+		ppsc->b_hwradiooff = false;
+		b_actuallyset = true;
+	} else if ((ppsc->b_hwradiooff == false)
+		   && (e_rfpowerstate_toset == ERFOFF)) {
+		RT_TRACE(COMP_RF, DBG_DMESG,
+			 ("GPIOChangeRF  - HW Radio OFF, RF OFF\n"));
+
+		e_rfpowerstate_toset = ERFOFF;
+		ppsc->b_hwradiooff = true;
+		b_actuallyset = true;
+	}
+
+	if (b_actuallyset) {
+		spin_lock(&rtlpriv->locks.rf_ps_lock);
+		ppsc->rfchange_inprogress = false;
+		spin_unlock(&rtlpriv->locks.rf_ps_lock);
+	} else {
+		if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_HALT_NIC)
+			RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
+
+		spin_lock(&rtlpriv->locks.rf_ps_lock);
+		ppsc->rfchange_inprogress = false;
+		spin_unlock(&rtlpriv->locks.rf_ps_lock);
+	}
+
+	*valid = 1;
+	return !ppsc->b_hwradiooff;
+
+}
+
+void rtl8821ae_set_key(struct ieee80211_hw *hw, u32 key_index,
+		     u8 *p_macaddr, bool is_group, u8 enc_algo,
+		     bool is_wepkey, bool clear_all)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u8 *macaddr = p_macaddr;
+	u32 entry_id = 0;
+	bool is_pairwise = false;
+
+	static u8 cam_const_addr[4][6] = {
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x01},
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x02},
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x03}
+	};
+	static u8 cam_const_broad[] = {
+		0xff, 0xff, 0xff, 0xff, 0xff, 0xff
+	};
+
+	if (clear_all) {
+		u8 idx = 0;
+		u8 cam_offset = 0;
+		u8 clear_number = 5;
+
+		RT_TRACE(COMP_SEC, DBG_DMESG, ("clear_all\n"));
+
+		for (idx = 0; idx < clear_number; idx++) {
+			rtl_cam_mark_invalid(hw, cam_offset + idx);
+			rtl_cam_empty_entry(hw, cam_offset + idx);
+
+			if (idx < 5) {
+				memset(rtlpriv->sec.key_buf[idx], 0,
+				       MAX_KEY_LEN);
+				rtlpriv->sec.key_len[idx] = 0;
+			}
+		}
+
+	} else {
+		switch (enc_algo) {
+		case WEP40_ENCRYPTION:
+			enc_algo = CAM_WEP40;
+			break;
+		case WEP104_ENCRYPTION:
+			enc_algo = CAM_WEP104;
+			break;
+		case TKIP_ENCRYPTION:
+			enc_algo = CAM_TKIP;
+			break;
+		case AESCCMP_ENCRYPTION:
+			enc_algo = CAM_AES;
+			break;
+		default:
+			RT_TRACE(COMP_ERR, DBG_EMERG, ("switch case "
+								"not process \n"));
+			enc_algo = CAM_TKIP;
+			break;
+		}
+
+		if (is_wepkey || rtlpriv->sec.use_defaultkey) {
+			macaddr = cam_const_addr[key_index];
+			entry_id = key_index;
+		} else {
+			if (is_group) {
+				macaddr = cam_const_broad;
+				entry_id = key_index;
+			} else {
+				if (mac->opmode == NL80211_IFTYPE_AP) {
+					entry_id = rtl_cam_get_free_entry(hw, p_macaddr);
+					if (entry_id >=  TOTAL_CAM_ENTRY) {
+						RT_TRACE(COMP_SEC, DBG_EMERG,
+								("Can not find free hw security cam entry\n"));
+						return;
+					}
+				} else {
+					entry_id = CAM_PAIRWISE_KEY_POSITION;
+				}
+
+				key_index = PAIRWISE_KEYIDX;
+				is_pairwise = true;
+			}
+		}
+
+		if (rtlpriv->sec.key_len[key_index] == 0) {
+			RT_TRACE(COMP_SEC, DBG_DMESG,
+				 ("delete one entry, entry_id is %d\n",entry_id));
+			if (mac->opmode == NL80211_IFTYPE_AP)
+				rtl_cam_del_entry(hw, p_macaddr);
+			rtl_cam_delete_one_entry(hw, p_macaddr, entry_id);
+		} else {
+			RT_TRACE(COMP_SEC, DBG_DMESG, ("add one entry\n"));
+			if (is_pairwise) {
+				RT_TRACE(COMP_SEC, DBG_DMESG, ("set Pairwiase key\n"));
+
+				rtl_cam_add_one_entry(hw, macaddr, key_index,
+						      entry_id, enc_algo,
+						      CAM_CONFIG_NO_USEDK,
+						      rtlpriv->sec.key_buf[key_index]);
+			} else {
+				RT_TRACE(COMP_SEC, DBG_DMESG, ("set group key\n"));
+
+				if (mac->opmode == NL80211_IFTYPE_ADHOC) {
+					rtl_cam_add_one_entry(hw,
+							rtlefuse->dev_addr,
+							PAIRWISE_KEYIDX,
+							CAM_PAIRWISE_KEY_POSITION,
+							enc_algo,
+							CAM_CONFIG_NO_USEDK,
+							rtlpriv->sec.key_buf
+							[entry_id]);
+				}
+
+				rtl_cam_add_one_entry(hw, macaddr, key_index,
+						entry_id, enc_algo,
+						CAM_CONFIG_NO_USEDK,
+						rtlpriv->sec.key_buf[entry_id]);
+			}
+
+		}
+	}
+}
+
+
+void rtl8812ae_read_bt_coexist_info_from_hwpg(struct ieee80211_hw *hw,
+					      bool auto_load_fail, u8 *hwinfo)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 value;
+
+	if (!auto_load_fail) {
+		value = *(u8 *) & hwinfo[EEPROM_RF_BOARD_OPTION];
+		if (((value & 0xe0) >> 5) == 0x1)
+			rtlpriv->btcoexist.btc_info.btcoexist = 1;
+		else
+			rtlpriv->btcoexist.btc_info.btcoexist = 0;
+		rtlpriv->btcoexist.btc_info.bt_type = BT_RTL8812A;
+
+		value = hwinfo[EEPROM_RF_BT_SETTING];
+		rtlpriv->btcoexist.btc_info.ant_num = (value & 0x1);
+	} else {
+		rtlpriv->btcoexist.btc_info.btcoexist = 0;
+		rtlpriv->btcoexist.btc_info.bt_type = BT_RTL8812A;
+		rtlpriv->btcoexist.btc_info.ant_num = ANT_X2;
+	}
+	/*move BT_InitHalVars() to init_sw_vars*/
+}
+
+void rtl8821ae_read_bt_coexist_info_from_hwpg(struct ieee80211_hw *hw,
+					      bool auto_load_fail, u8 *hwinfo)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 value;
+	u32 tmpu_32;
+
+	if (!auto_load_fail) {
+		tmpu_32 = rtl_read_dword(rtlpriv, REG_MULTI_FUNC_CTRL);
+		if(tmpu_32 & BIT(18))
+			rtlpriv->btcoexist.btc_info.btcoexist = 1;
+		else
+			rtlpriv->btcoexist.btc_info.btcoexist = 0;
+		rtlpriv->btcoexist.btc_info.bt_type = BT_RTL8821A;
+
+		value = hwinfo[EEPROM_RF_BT_SETTING];
+		rtlpriv->btcoexist.btc_info.ant_num = (value & 0x1);
+	} else {
+		rtlpriv->btcoexist.btc_info.btcoexist = 0;
+		rtlpriv->btcoexist.btc_info.bt_type = BT_RTL8821A;
+		rtlpriv->btcoexist.btc_info.ant_num = ANT_X2;
+	}
+	/*move BT_InitHalVars() to init_sw_vars*/
+}
+
+void rtl8821ae_bt_reg_init(struct ieee80211_hw* hw)
+{
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+
+	/* 0:Low, 1:High, 2:From Efuse. */
+	rtlpcipriv->btcoexist.b_reg_bt_iso = 2;
+	/* 0:Idle, 1:None-SCO, 2:SCO, 3:From Counter. */
+	rtlpcipriv->btcoexist.b_reg_bt_sco= 3;
+	/* 0:Disable BT control A-MPDU, 1:Enable BT control A-MPDU. */
+	rtlpcipriv->btcoexist.b_reg_bt_sco= 0;
+}
+
+
+void rtl8821ae_bt_hw_init(struct ieee80211_hw* hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (rtlpriv->cfg->ops->get_btc_status()){
+		rtlpriv->btcoexist.btc_ops->btc_init_hw_config(rtlpriv);
+	}
+}
+
+void rtl8821ae_suspend(struct ieee80211_hw *hw)
+{
+}
+
+void rtl8821ae_resume(struct ieee80211_hw *hw)
+{
+}
+
+/* Turn on AAP (RCR:bit 0) for promicuous mode. */
+void rtl8821ae_allow_all_destaddr(struct ieee80211_hw *hw,
+	bool allow_all_da, bool write_into_reg)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	if (allow_all_da) /* Set BIT0 */
+		rtlpci->receive_config |= RCR_AAP;
+	else /* Clear BIT0 */
+		rtlpci->receive_config &= ~RCR_AAP;
+
+	if(write_into_reg)
+		rtl_write_dword(rtlpriv, REG_RCR, rtlpci->receive_config);
+
+
+	RT_TRACE(COMP_TURBO | COMP_INIT, DBG_LOUD,
+		("receive_config=0x%08X, write_into_reg=%d\n",
+		rtlpci->receive_config, write_into_reg ));
+}
+
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/hw.h b/drivers/staging/rtl8821ae/rtl8821ae/hw.h
new file mode 100644
index 000000000000..4fb6bf0d1da2
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/hw.h
@@ -0,0 +1,75 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL8821AE_HW_H__
+#define __RTL8821AE_HW_H__
+
+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);
+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);
+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);
+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);
+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);
+
+void rtl8821ae_read_bt_coexist_info_from_hwpg(struct ieee80211_hw *hw,
+															bool autoload_fail,
+															u8* hwinfo);
+void rtl8812ae_read_bt_coexist_info_from_hwpg(struct ieee80211_hw *hw,
+															bool autoload_fail,
+															u8* hwinfo);
+void rtl8821ae_bt_reg_init(struct ieee80211_hw* hw);
+void rtl8821ae_bt_hw_init(struct ieee80211_hw* hw);
+void rtl8821ae_suspend(struct ieee80211_hw *hw);
+void rtl8821ae_resume(struct ieee80211_hw *hw);
+void rtl8821ae_allow_all_destaddr(struct ieee80211_hw *hw,
+										  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/led.c b/drivers/staging/rtl8821ae/rtl8821ae/led.c
new file mode 100644
index 000000000000..130a4f4b24a2
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/led.c
@@ -0,0 +1,239 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../pci.h"
+#include "reg.h"
+
+static void _rtl8821ae_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 rtl8821ae_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled)
+{
+	u8 ledcfg;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	RT_TRACE(COMP_LED, DBG_LOUD,
+		 ("LedAddr:%X ledpin=%d\n", REG_LEDCFG2, pled->ledpin));
+
+	switch (pled->ledpin) {
+	case LED_PIN_GPIO0:
+		break;
+	case LED_PIN_LED0:
+		ledcfg = rtl_read_byte(rtlpriv, REG_LEDCFG2);
+		ledcfg &= ~BIT(6);
+		rtl_write_byte(rtlpriv,
+			       REG_LEDCFG2, (ledcfg & 0xf0) | BIT(5));
+		break;
+	case LED_PIN_LED1:
+		ledcfg = rtl_read_byte(rtlpriv, REG_LEDCFG1);
+		rtl_write_byte(rtlpriv, REG_LEDCFG1, ledcfg & 0x10);
+		break;
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("switch case not process \n"));
+		break;
+	}
+	pled->b_ledon = true;
+}
+
+void rtl8812ae_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled)
+{
+	u16 	ledreg = REG_LEDCFG1;
+	u8 	ledcfg = 0;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	switch (pled->ledpin) {
+		case LED_PIN_LED0:
+			ledreg = REG_LEDCFG1;
+			break;
+
+		case LED_PIN_LED1:
+			ledreg = REG_LEDCFG2;
+			break;
+
+		case LED_PIN_GPIO0:
+		default:
+			break;
+	}
+
+	RT_TRACE(COMP_LED, DBG_LOUD, ("In SwLedOn, LedAddr:%X LEDPIN=%d \n", ledreg, pled->ledpin));
+
+	ledcfg =  rtl_read_byte(rtlpriv, ledreg);
+	ledcfg |= BIT(5); /*Set 0x4c[21]*/
+	ledcfg &= ~(BIT(7) | BIT(6) | BIT(3) |BIT(2) | BIT(1) |BIT(0));
+		/*Clear 0x4c[23:22] and 0x4c[19:16]*/
+	rtl_write_byte(rtlpriv, ledreg, ledcfg); /*SW control led0 on.*/
+	pled->b_ledon = true;
+}
+
+void rtl8821ae_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	u8 ledcfg;
+
+	RT_TRACE(COMP_LED, DBG_LOUD,
+		 ("LedAddr:%X ledpin=%d\n", REG_LEDCFG2, pled->ledpin));
+
+	ledcfg = rtl_read_byte(rtlpriv, REG_LEDCFG2);
+
+	switch (pled->ledpin) {
+	case LED_PIN_GPIO0:
+		break;
+	case LED_PIN_LED0:
+		ledcfg &= 0xf0;
+		if (pcipriv->ledctl.bled_opendrain == true) {
+			ledcfg &= 0x90; /* Set to software control. */
+			rtl_write_byte(rtlpriv, REG_LEDCFG2, (ledcfg|BIT(3)));
+			ledcfg = rtl_read_byte(rtlpriv, REG_MAC_PINMUX_CFG);
+			ledcfg &= 0xFE;
+			rtl_write_byte(rtlpriv, REG_MAC_PINMUX_CFG, ledcfg);
+		}
+		else {
+			ledcfg &= ~BIT(6);
+			rtl_write_byte(rtlpriv, REG_LEDCFG2,
+					(ledcfg | BIT(3) | BIT(5)));
+		}
+		break;
+	case LED_PIN_LED1:
+		ledcfg = rtl_read_byte(rtlpriv, REG_LEDCFG1);
+		ledcfg &= 0x10; /* Set to software control. */
+		rtl_write_byte(rtlpriv, REG_LEDCFG1, ledcfg|BIT(3));
+
+		break;
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("switch case not process \n"));
+		break;
+	}
+	pled->b_ledon = false;
+}
+
+void rtl8812ae_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled){
+	u16 ledreg = REG_LEDCFG1;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+
+	switch(pled->ledpin)
+	{
+		case LED_PIN_LED0:
+			ledreg = REG_LEDCFG1;
+			break;
+
+		case LED_PIN_LED1:
+			ledreg = REG_LEDCFG2;
+			break;
+
+		case LED_PIN_GPIO0:
+		default:
+			break;
+	}
+
+	RT_TRACE(COMP_LED,DBG_LOUD,("In SwLedOff,LedAddr:%X LEDPIN=%d\n", ledreg, pled->ledpin));
+
+	if(pcipriv->ledctl.bled_opendrain == true) /*Open-drain arrangement for controlling the LED*/
+	{
+		u8 ledcfg = rtl_read_byte(rtlpriv,  ledreg);
+
+		ledreg &= 0xd0; /* Set to software control.*/
+		rtl_write_byte(rtlpriv, ledreg, (ledcfg | BIT(3)));
+
+		/*Open-drain arrangement*/
+		ledcfg = rtl_read_byte(rtlpriv, REG_MAC_PINMUX_CFG);
+		ledcfg &= 0xFE;/*Set GPIO[8] to input mode*/
+		rtl_write_byte(rtlpriv, REG_MAC_PINMUX_CFG, ledcfg);
+	}
+	else
+	{
+		rtl_write_byte(rtlpriv, ledreg, 0x28);
+	}
+
+	pled->b_ledon = false;
+}
+
+void rtl8821ae_init_sw_leds(struct ieee80211_hw *hw)
+{
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	_rtl8821ae_init_led(hw, &(pcipriv->ledctl.sw_led0), LED_PIN_LED0);
+	_rtl8821ae_init_led(hw, &(pcipriv->ledctl.sw_led1), LED_PIN_LED1);
+}
+
+static void _rtl8821ae_sw_led_control(struct ieee80211_hw *hw,
+				    enum led_ctl_mode ledaction)
+{
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_led *pLed0 = &(pcipriv->ledctl.sw_led0);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	switch (ledaction) {
+	case LED_CTL_POWER_ON:
+	case LED_CTL_LINK:
+	case LED_CTL_NO_LINK:
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+			rtl8812ae_sw_led_on(hw, pLed0);
+		else
+			rtl8821ae_sw_led_on(hw, pLed0);
+		break;
+	case LED_CTL_POWER_OFF:
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)\
+			rtl8812ae_sw_led_off(hw, pLed0);
+		else
+			rtl8821ae_sw_led_off(hw, pLed0);
+		break;
+	default:
+		break;
+	}
+}
+
+void rtl8821ae_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_LOUD, ("ledaction %d, \n",
+				ledaction));
+	_rtl8821ae_sw_led_control(hw, ledaction);
+}
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/led.h b/drivers/staging/rtl8821ae/rtl8821ae/led.h
new file mode 100644
index 000000000000..44be401ba21f
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/led.h
@@ -0,0 +1,40 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL8821AE_LED_H__
+#define __RTL8821AE_LED_H__
+
+void rtl8821ae_init_sw_leds(struct ieee80211_hw *hw);
+void rtl8821ae_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled);
+void rtl8812ae_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled);
+void rtl8821ae_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled);
+void rtl8812ae_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled);
+void rtl8821ae_led_control(struct ieee80211_hw *hw, enum led_ctl_mode ledaction);
+
+#endif
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/phy.c b/drivers/staging/rtl8821ae/rtl8821ae/phy.c
new file mode 100644
index 000000000000..d02fca38a2b2
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/phy.c
@@ -0,0 +1,5525 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../pci.h"
+#include "../ps.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "rf.h"
+#include "dm.h"
+#include "table.h"
+#include "trx.h"
+#include "../btcoexist/halbt_precomp.h"
+#include "hw.h"
+
+#define READ_NEXT_PAIR(array_table,v1, v2, i) do { i += 2; v1 = array_table[i]; v2 = array_table[i+1]; } while(0)
+
+static u32 _rtl8821ae_phy_rf_serial_read(struct ieee80211_hw *hw,
+				      enum radio_path rfpath, u32 offset);
+static void _rtl8821ae_phy_rf_serial_write(struct ieee80211_hw *hw,
+					enum radio_path rfpath, u32 offset,
+					u32 data);
+static u32 _rtl8821ae_phy_calculate_bit_shift(u32 bitmask);
+static bool _rtl8821ae_phy_bb8821a_config_parafile(struct ieee80211_hw *hw);
+static bool _rtl8812ae_phy_config_mac_with_headerfile(struct ieee80211_hw *hw);
+static bool _rtl8821ae_phy_config_mac_with_headerfile(struct ieee80211_hw *hw);
+static bool _rtl8821ae_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
+						u8 configtype);
+static bool _rtl8812ae_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
+						u8 configtype);
+static bool _rtl8821ae_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
+						  u8 configtype);
+static bool _rtl8812ae_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
+						u8 configtype);
+static void _rtl8821ae_phy_init_bb_rf_register_definition(struct ieee80211_hw *hw);
+
+static long _rtl8821ae_phy_txpwr_idx_to_dbm(struct ieee80211_hw *hw,
+					 enum wireless_mode wirelessmode,
+					 u8 txpwridx);
+static void rtl8821ae_phy_set_rf_on(struct ieee80211_hw *hw);
+static void rtl8821ae_phy_set_io(struct ieee80211_hw *hw);
+
+void rtl8812ae_fixspur(
+	struct ieee80211_hw *hw,
+	enum ht_channel_width band_width,
+	u8 channel
+)
+{
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	/*C cut Item12 ADC FIFO CLOCK*/
+	if(IS_VENDOR_8812A_C_CUT(rtlhal->version))
+	{
+		if(band_width == HT_CHANNEL_WIDTH_20_40 && channel == 11)
+			rtl_set_bbreg(hw, RRFMOD, 0xC00, 0x3)	;
+			/* 0x8AC[11:10] = 2'b11*/
+		else
+			rtl_set_bbreg(hw, RRFMOD, 0xC00, 0x2);
+			/* 0x8AC[11:10] = 2'b10*/
+
+
+		/* <20120914, Kordan> A workarould to resolve
+		2480Mhz spur by setting ADC clock as 160M. (Asked by Binson)*/
+		if (band_width == HT_CHANNEL_WIDTH_20 &&
+			(channel == 13 || channel == 14)) {
+			rtl_set_bbreg(hw, RRFMOD, 0x300, 0x3);
+			/*0x8AC[9:8] = 2'b11*/
+			rtl_set_bbreg(hw, RADC_BUF_CLK, BIT(30), 1);
+			/* 0x8C4[30] = 1*/
+		} else if (band_width == HT_CHANNEL_WIDTH_20_40 &&
+			channel == 11) {
+			rtl_set_bbreg(hw, RADC_BUF_CLK, BIT(30), 1);
+			/*0x8C4[30] = 1*/
+		} else if (band_width != HT_CHANNEL_WIDTH_80) {
+			rtl_set_bbreg(hw, RRFMOD, 0x300, 0x2);
+			/*0x8AC[9:8] = 2'b10*/
+			rtl_set_bbreg(hw, RADC_BUF_CLK, BIT(30), 0);
+			/*0x8C4[30] = 0*/
+		}
+	}
+	else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+	{
+		/* <20120914, Kordan> A workarould to resolve
+		2480Mhz spur by setting ADC clock as 160M. (Asked by Binson)*/
+		if (band_width == HT_CHANNEL_WIDTH_20 &&
+			(channel == 13 || channel == 14))
+			rtl_set_bbreg(hw, RRFMOD, 0x300, 0x3);
+			/*0x8AC[9:8] = 11*/
+		else if (channel  <= 14) /*2.4G only*/
+			rtl_set_bbreg(hw, RRFMOD, 0x300, 0x2);
+			/*0x8AC[9:8] = 10*/
+	}
+
+}
+
+u32 rtl8821ae_phy_query_bb_reg(struct ieee80211_hw *hw, u32 regaddr, u32 bitmask)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 returnvalue, originalvalue, bitshift;
+
+	RT_TRACE(COMP_RF, DBG_TRACE, ("regaddr(%#x), "
+					       "bitmask(%#x)\n", regaddr,
+					       bitmask));
+	originalvalue = rtl_read_dword(rtlpriv, regaddr);
+	bitshift = _rtl8821ae_phy_calculate_bit_shift(bitmask);
+	returnvalue = (originalvalue & bitmask) >> bitshift;
+
+	RT_TRACE(COMP_RF, DBG_TRACE, ("BBR MASK=0x%x "
+					       "Addr[0x%x]=0x%x\n", bitmask,
+					       regaddr, originalvalue));
+
+	return returnvalue;
+
+}
+
+void rtl8821ae_phy_set_bb_reg(struct ieee80211_hw *hw,
+			   u32 regaddr, u32 bitmask, u32 data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 originalvalue, bitshift;
+
+	RT_TRACE(COMP_RF, DBG_TRACE, ("regaddr(%#x), bitmask(%#x),"
+					       " data(%#x)\n", regaddr, bitmask,
+					       data));
+
+	if (bitmask != MASKDWORD) {
+		originalvalue = rtl_read_dword(rtlpriv, regaddr);
+		bitshift = _rtl8821ae_phy_calculate_bit_shift(bitmask);
+		data = ((originalvalue & (~bitmask)) | ((data << bitshift) & bitmask));
+	}
+
+	rtl_write_dword(rtlpriv, regaddr, data);
+
+	RT_TRACE(COMP_RF, DBG_TRACE, ("regaddr(%#x), bitmask(%#x),"
+					       " data(%#x)\n", regaddr, bitmask,
+					       data));
+
+}
+
+u32 rtl8821ae_phy_query_rf_reg(struct ieee80211_hw *hw,
+			    enum radio_path rfpath, u32 regaddr, u32 bitmask)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 original_value, readback_value, bitshift;
+	unsigned long flags;
+
+	RT_TRACE(COMP_RF, DBG_TRACE, ("regaddr(%#x), "
+					       "rfpath(%#x), bitmask(%#x)\n",
+					       regaddr, rfpath, bitmask));
+
+	spin_lock_irqsave(&rtlpriv->locks.rf_lock, flags);
+
+
+	original_value = _rtl8821ae_phy_rf_serial_read(hw,rfpath, regaddr);
+	bitshift = _rtl8821ae_phy_calculate_bit_shift(bitmask);
+	readback_value = (original_value & bitmask) >> bitshift;
+
+	spin_unlock_irqrestore(&rtlpriv->locks.rf_lock, flags);
+
+	RT_TRACE(COMP_RF, DBG_TRACE,
+		 ("regaddr(%#x), rfpath(%#x), "
+		  "bitmask(%#x), original_value(%#x)\n",
+		  regaddr, rfpath, bitmask, original_value));
+
+	return readback_value;
+}
+
+void rtl8821ae_phy_set_rf_reg(struct ieee80211_hw *hw,
+			   enum radio_path rfpath,
+			   u32 regaddr, u32 bitmask, u32 data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 original_value, bitshift;
+	unsigned long flags;
+
+	RT_TRACE(COMP_RF, DBG_TRACE,
+		 ("regaddr(%#x), bitmask(%#x), data(%#x), rfpath(%#x)\n",
+		  regaddr, bitmask, data, rfpath));
+
+	spin_lock_irqsave(&rtlpriv->locks.rf_lock, flags);
+
+	if (bitmask != RFREG_OFFSET_MASK) {
+			original_value = _rtl8821ae_phy_rf_serial_read(hw,
+								    rfpath,
+								    regaddr);
+			bitshift = _rtl8821ae_phy_calculate_bit_shift(bitmask);
+			data =
+			    ((original_value & (~bitmask)) |
+			     (data << bitshift));
+		}
+
+	_rtl8821ae_phy_rf_serial_write(hw, rfpath, regaddr, data);
+
+
+	spin_unlock_irqrestore(&rtlpriv->locks.rf_lock, flags);
+
+	RT_TRACE(COMP_RF, DBG_TRACE, ("regaddr(%#x), "
+					       "bitmask(%#x), data(%#x), rfpath(%#x)\n",
+					       regaddr, bitmask, data, rfpath));
+
+}
+
+static u32 _rtl8821ae_phy_rf_serial_read(struct ieee80211_hw *hw,
+				      enum radio_path rfpath, u32 offset)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool b_is_pi_mode =false;
+	u32 retvalue = 0;
+
+	/* 2009/06/17 MH We can not execute IO for power save or other accident mode.*/
+	if (RT_CANNOT_IO(hw)) {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("return all one\n"));
+		return 0xFFFFFFFF;
+	}
+
+	/* <20120809, Kordan> CCA OFF(when entering), asked by James to avoid reading the wrong value.
+	    <20120828, Kordan> Toggling CCA would affect RF 0x0, skip it!*/
+	if (offset != 0x0 &&
+		!((rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE)
+		|| (IS_VENDOR_8812A_C_CUT(rtlhal->version))))
+		rtl_set_bbreg(hw, RCCAONSEC, 0x8, 1);
+
+	offset &= 0xff;
+
+	if (rfpath == RF90_PATH_A)
+		b_is_pi_mode = (bool) rtl_get_bbreg(hw, 0xC00, 0x4);
+	else if (rfpath == RF90_PATH_B)
+		b_is_pi_mode = (bool) rtl_get_bbreg(hw, 0xE00, 0x4);
+
+	rtl_set_bbreg(hw, RHSSIREAD_8821AE, 0xff, offset);
+
+	if ((rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE)
+		|| (IS_VENDOR_8812A_C_CUT(rtlhal->version)))
+		udelay(20);
+
+	if (b_is_pi_mode)
+	{
+		if (rfpath == RF90_PATH_A) {
+			retvalue = rtl_get_bbreg(hw, RA_PIREAD_8821A, BLSSIREADBACKDATA);
+		}
+		else if (rfpath == RF90_PATH_B){
+			retvalue = rtl_get_bbreg(hw, RB_PIREAD_8821A, BLSSIREADBACKDATA);
+		}
+	}
+	else
+	{
+		if (rfpath == RF90_PATH_A) {
+			retvalue = rtl_get_bbreg(hw, RA_SIREAD_8821A, BLSSIREADBACKDATA);
+		}
+		else if (rfpath == RF90_PATH_B){
+			retvalue = rtl_get_bbreg(hw, RB_SIREAD_8821A, BLSSIREADBACKDATA);
+		}
+	}
+
+	/*<20120809, Kordan> CCA ON(when exiting), asked by James to avoid reading the wrong value.
+	    <20120828, Kordan> Toggling CCA would affect RF 0x0, skip it!*/
+	if (offset != 0x0 &&  ! ((rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE)
+		|| (IS_VENDOR_8812A_C_CUT(rtlhal->version))))
+		rtl_set_bbreg(hw, RCCAONSEC, 0x8, 0);
+	return retvalue;
+}
+
+#if 0
+static u32 _rtl8821ae_phy_rf_serial_read(struct ieee80211_hw *hw,
+				      enum radio_path rfpath, u32 offset)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct bb_reg_def *pphyreg = &rtlphy->phyreg_def[rfpath];
+	u32 newoffset;
+	u32 tmplong, tmplong2;
+	u8 rfpi_enable = 0;
+	u32 retvalue;
+
+	offset &= 0xff;
+	newoffset = offset;
+	if (RT_CANNOT_IO(hw)) {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("return all one\n"));
+		return 0xFFFFFFFF;
+	}
+	tmplong = rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD);
+	if (rfpath == RF90_PATH_A)
+		tmplong2 = tmplong;
+	else
+		tmplong2 = rtl_get_bbreg(hw, pphyreg->rfhssi_para2, MASKDWORD);
+	tmplong2 = (tmplong2 & (~BLSSIREADADDRESS)) |
+	    (newoffset << 23) | BLSSIREADEDGE;
+	rtl_set_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD,
+		      tmplong & (~BLSSIREADEDGE));
+	mdelay(1);
+	rtl_set_bbreg(hw, pphyreg->rfhssi_para2, MASKDWORD, tmplong2);
+	mdelay(1);
+	/*rtl_set_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD,
+		      tmplong | BLSSIREADEDGE);*/
+	mdelay(1);
+	if (rfpath == RF90_PATH_A)
+		rfpi_enable = (u8) rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER1,
+						 BIT(8));
+	else if (rfpath == RF90_PATH_B)
+		rfpi_enable = (u8) rtl_get_bbreg(hw, RFPGA0_XB_HSSIPARAMETER1,
+						 BIT(8));
+	if (rfpi_enable)
+		retvalue = rtl_get_bbreg(hw, pphyreg->rflssi_readbackpi,
+					 BLSSIREADBACKDATA);
+	else
+		retvalue = rtl_get_bbreg(hw, pphyreg->rflssi_readback,
+					 BLSSIREADBACKDATA);
+	RT_TRACE(COMP_RF, DBG_TRACE, ("RFR-%d Addr[0x%x]=0x%x\n",
+					       rfpath, pphyreg->rflssi_readback,
+					       retvalue));
+	return retvalue;
+}
+#endif
+
+static void _rtl8821ae_phy_rf_serial_write(struct ieee80211_hw *hw,
+					enum radio_path rfpath, u32 offset,
+					u32 data)
+{
+	u32 data_and_addr;
+	u32 newoffset;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct bb_reg_def *pphyreg = &rtlphy->phyreg_def[rfpath];
+
+	if (RT_CANNOT_IO(hw)) {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("stop\n"));
+		return;
+	}
+	offset &= 0xff;
+	newoffset = offset;
+	data_and_addr = ((newoffset << 20) | (data & 0x000fffff)) & 0x0fffffff;
+	rtl_set_bbreg(hw, pphyreg->rf3wire_offset, MASKDWORD, data_and_addr);
+	RT_TRACE(COMP_RF, DBG_TRACE, ("RFW-%d Addr[0x%x]=0x%x\n",
+					       rfpath, pphyreg->rf3wire_offset,
+					       data_and_addr));
+}
+
+static u32 _rtl8821ae_phy_calculate_bit_shift(u32 bitmask)
+{
+	u32 i;
+
+	for (i = 0; i <= 31; i++) {
+		if (((bitmask >> i) & 0x1) == 1)
+			break;
+	}
+	return i;
+}
+
+bool rtl8821ae_phy_mac_config(struct ieee80211_hw *hw)
+{
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool rtstatus = 0;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+		rtstatus = _rtl8812ae_phy_config_mac_with_headerfile(hw);
+	else
+		rtstatus = _rtl8821ae_phy_config_mac_with_headerfile(hw);
+
+	return rtstatus;
+}
+
+bool rtl8821ae_phy_bb_config(struct ieee80211_hw *hw)
+{
+	bool rtstatus = true;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 regval;
+	u8 crystal_cap;
+	//u32 tmp;
+
+	_rtl8821ae_phy_init_bb_rf_register_definition(hw);
+
+	regval = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN);
+	regval |= regval | FEN_PCIEA;
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, regval);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN,
+				regval | FEN_BB_GLB_RSTN | FEN_BBRSTB);
+
+	rtl_write_byte(rtlpriv, REG_RF_CTRL, 0x7);/*RF_EN | RF_RSTB | RF_SDMRSTB*/
+	rtl_write_byte(rtlpriv, REG_OPT_CTRL + 2, 0x7);/*RF_EN | RF_RSTB | RF_SDMRSTB*/
+
+	rtstatus = _rtl8821ae_phy_bb8821a_config_parafile(hw);
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+	{
+		crystal_cap = rtlefuse->crystalcap & 0x3F;
+		rtl_set_bbreg(hw, REG_MAC_PHY_CTRL, 0x7FF80000, (crystal_cap | (crystal_cap << 6)));
+	}else{
+		crystal_cap = rtlefuse->crystalcap & 0x3F;
+		rtl_set_bbreg(hw, REG_MAC_PHY_CTRL, 0xFFF000, (crystal_cap | (crystal_cap << 6)));
+	}
+	rtlphy->reg_837 = rtl_read_byte(rtlpriv, 0x837);
+
+	return rtstatus;
+}
+
+bool rtl8821ae_phy_rf_config(struct ieee80211_hw *hw)
+{
+	return rtl8821ae_phy_rf6052_config(hw);
+}
+
+
+u32 phy_get_tx_bb_swing_8812A(
+	struct ieee80211_hw *hw,
+	u8 	band,
+	u8 	rf_path
+	)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_dm *rtldm = rtl_dm(rtlpriv);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+
+	char bb_swing_2g = (char) (-1 * 0xFF);
+	char bb_swing_5g = (char) (-1 * 0xFF);
+	u32  out = 0x200;
+	const char auto_temp = -1;
+
+	RT_TRACE(COMP_SCAN, DBG_LOUD,
+		("===> PHY_GetTxBBSwing_8812A, bbSwing_2G: %d, bbSwing_5G: %d\n",
+		(int)bb_swing_2g, (int)bb_swing_5g));
+
+    if ( rtlefuse->autoload_failflag) {
+        	if ( band == BAND_ON_2_4G ) {
+			rtldm->bb_swing_diff_2g = bb_swing_2g;
+			if      (bb_swing_2g == 0)  out = 0x200; //  0 dB
+			else if (bb_swing_2g == -3) out = 0x16A; // -3 dB
+			else if (bb_swing_2g == -6) out = 0x101; // -6 dB
+			else if (bb_swing_2g == -9) out = 0x0B6; // -9 dB
+			else {
+			        rtldm->bb_swing_diff_2g = 0;
+			        out = 0x200;
+			}
+
+		} else if ( band == BAND_ON_5G ) {
+			rtldm->bb_swing_diff_5g = bb_swing_5g;
+			if      (bb_swing_5g == 0)  out = 0x200; //  0 dB
+			else if (bb_swing_5g == -3) out = 0x16A; // -3 dB
+			else if (bb_swing_5g == -6) out = 0x101; // -6 dB
+			else if (bb_swing_5g == -9) out = 0x0B6; // -9 dB
+			else {
+				if ( rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+					rtldm->bb_swing_diff_5g = -3;
+					out = 0x16A;
+				} else  {
+					rtldm->bb_swing_diff_5g = 0;
+					out = 0x200;
+				}
+			}
+		} else  {
+				rtldm->bb_swing_diff_2g = -3;
+				rtldm->bb_swing_diff_5g = -3;
+				out = 0x16A; // -3 dB
+		}
+	}
+	else
+	{
+	    u32 swing = 0, swing_a = 0, swing_b = 0;
+
+	    if (band == BAND_ON_2_4G)
+	    {
+			if (0xFF == auto_temp)
+			{
+				efuse_shadow_read(hw, 1, 0xC6, (u32 *)&swing);
+				swing = (swing == 0xFF) ? 0x00 : swing;
+			}
+			else if (bb_swing_2g ==  0) swing = 0x00; //  0 dB
+			else if (bb_swing_2g == -3) swing = 0x05; // -3 dB
+			else if (bb_swing_2g == -6) swing = 0x0A; // -6 dB
+			else if (bb_swing_2g == -9) swing = 0xFF; // -9 dB
+			else swing = 0x00;
+		}
+		else
+		{
+			if (0xFF == auto_temp)
+			{
+				efuse_shadow_read(hw, 1, 0xC7, (u32 *)&swing);
+				swing = (swing == 0xFF) ? 0x00 : swing;
+			}
+			else if (bb_swing_5g ==  0) swing = 0x00; //  0 dB
+			else if (bb_swing_5g == -3) swing = 0x05; // -3 dB
+			else if (bb_swing_5g == -6) swing = 0x0A; // -6 dB
+			else if (bb_swing_5g == -9) swing = 0xFF; // -9 dB
+			else swing = 0x00;
+		}
+
+		swing_a = (swing & 0x3) >> 0; // 0xC6/C7[1:0]
+		swing_b = (swing & 0xC) >> 2; // 0xC6/C7[3:2]
+		RT_TRACE(COMP_SCAN, DBG_LOUD,
+		("===> PHY_GetTxBBSwing_8812A, swingA: 0x%X, swingB: 0x%X\n",
+		swing_a, swing_b));
+
+		//3 Path-A
+		if (swing_a == 0x0) {
+			if (band == BAND_ON_2_4G)
+				rtldm->bb_swing_diff_2g = 0;
+			else
+				rtldm->bb_swing_diff_5g = 0;
+			out = 0x200; // 0 dB
+		} else if (swing_a == 0x1) {
+			if (band == BAND_ON_2_4G)
+				rtldm->bb_swing_diff_2g = -3;
+			else
+				rtldm->bb_swing_diff_5g = -3;
+			out = 0x16A; // -3 dB
+		} else if (swing_a == 0x2) {
+			if (band == BAND_ON_2_4G)
+				rtldm->bb_swing_diff_2g = -6;
+			else
+				rtldm->bb_swing_diff_5g = -6;
+			out = 0x101; // -6 dB
+		} else if (swing_a == 0x3) {
+			if (band == BAND_ON_2_4G)
+				rtldm->bb_swing_diff_2g = -9;
+			else
+				rtldm->bb_swing_diff_5g = -9;
+			out = 0x0B6; // -9 dB
+		}
+
+			//3 Path-B
+		if (swing_b == 0x0) {
+			if (band == BAND_ON_2_4G)
+				rtldm->bb_swing_diff_2g = 0;
+			else
+				rtldm->bb_swing_diff_5g = 0;
+			out = 0x200; // 0 dB
+		} else if (swing_b == 0x1) {
+			if (band == BAND_ON_2_4G)
+				rtldm->bb_swing_diff_2g = -3;
+			else
+				rtldm->bb_swing_diff_5g = -3;
+			out = 0x16A; // -3 dB
+		} else if (swing_b == 0x2) {
+			if (band == BAND_ON_2_4G)
+				rtldm->bb_swing_diff_2g = -6;
+			else
+				rtldm->bb_swing_diff_5g = -6;
+			out = 0x101; // -6 dB
+		} else if (swing_b == 0x3) {
+			if (band == BAND_ON_2_4G)
+				rtldm->bb_swing_diff_2g = -9;
+			else
+				rtldm->bb_swing_diff_5g = -9;
+			out = 0x0B6; // -9 dB
+		}
+	}
+
+	RT_TRACE(COMP_SCAN, DBG_LOUD,
+		("<=== PHY_GetTxBBSwing_8812A, out = 0x%X\n", out));
+	 return out;
+}
+void rtl8821ae_phy_switch_wirelessband(struct ieee80211_hw *hw, u8 band)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_dm *rtldm = rtl_dm(rtlpriv);
+	u8 current_band = rtlhal->current_bandtype;
+	u32 txpath, rxpath;
+	//u8 i, value8;
+	char bb_diff_between_band;
+
+	RT_TRACE(COMP_INIT, DBG_LOUD, ("\n"));
+	txpath = rtl8821ae_phy_query_bb_reg(hw, RTXPATH, 0xf0);
+	rxpath = rtl8821ae_phy_query_bb_reg(hw, RCCK_RX, 0x0f000000);
+	rtlhal->current_bandtype = (enum band_type) band;
+	/* reconfig BB/RF according to wireless mode */
+	if (rtlhal->current_bandtype == BAND_ON_2_4G) {
+		/* BB & RF Config */
+		RT_TRACE(COMP_CMD, DBG_DMESG, ("2.4G\n"));
+		rtl_set_bbreg(hw, ROFDMCCKEN, BOFDMEN|BCCKEN, 0x03);
+
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+			/* 0xCB0[15:12] = 0x7 (LNA_On)*/
+			rtl_set_bbreg(hw, RA_RFE_PINMUX, 0xF000, 0x7);
+			/* 0xCB0[7:4] = 0x7 (PAPE_A)*/
+			rtl_set_bbreg(hw, RA_RFE_PINMUX, 0xF0, 0x7);
+		}
+
+		if(rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+			rtl_set_bbreg(hw, 0x830, 0xE, 0x4);	/*0x830[3:1] = 0x4*/
+			rtl_set_bbreg(hw, 0x834, 0x3, 0x1);	/*0x834[1:0] = 0x1*/
+		}
+
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE)
+			rtl_set_bbreg(hw, RA_TXSCALE, 0xF00, 0); // 0xC1C[11:8] = 0
+		else
+			rtl_set_bbreg(hw, 0x82c, 0x3, 0);	// 0x82C[1:0] = 2b'00
+
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+			rtl_set_bbreg(hw, RA_RFE_PINMUX, BMASKDWORD, 0x77777777);
+			rtl_set_bbreg(hw, RB_RFE_PINMUX, BMASKDWORD, 0x77777777);
+			rtl_set_bbreg(hw, RA_RFE_INV, 0x3ff00000, 0x000);
+			rtl_set_bbreg(hw, RB_RFE_INV, 0x3ff00000, 0x000);
+		}
+
+		rtl_set_bbreg(hw, RTXPATH, 0xf0, txpath);
+		rtl_set_bbreg(hw, RCCK_RX, 0x0f000000, rxpath);
+
+		rtl_write_byte(rtlpriv, REG_CCK_CHECK, 0x0);
+	} else {/* 5G band */
+		u16 count, reg_41a;
+		RT_TRACE(COMP_CMD, DBG_DMESG, ("5G\n"));
+
+		if(rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+			/*0xCB0[15:12] = 0x5 (LNA_On)*/
+			rtl_set_bbreg(hw, RA_RFE_PINMUX, 0xF000, 0x5);
+			/*0xCB0[7:4] = 0x4 (PAPE_A)*/
+			rtl_set_bbreg(hw, RA_RFE_PINMUX, 0xF0, 0x4);
+		}
+		/*CCK_CHECK_en*/
+		rtl_write_byte(rtlpriv, REG_CCK_CHECK, 0x80);
+
+		count = 0;
+		reg_41a = rtl_read_word(rtlpriv, REG_TXPKT_EMPTY);
+		RT_TRACE(COMP_SCAN, DBG_LOUD, ("Reg41A value %d", reg_41a));
+		reg_41a &= 0x30;
+		while ((reg_41a!= 0x30) && (count < 50)) {
+			udelay(50);
+			RT_TRACE(COMP_SCAN, DBG_LOUD, ("Delay 50us \n"));
+
+			reg_41a = rtl_read_word(rtlpriv, REG_TXPKT_EMPTY);
+			reg_41a &= 0x30;
+			count++;
+			RT_TRACE(COMP_SCAN, DBG_LOUD, ("Reg41A value %d", reg_41a));
+		}
+		if (count != 0)
+			RT_TRACE(COMP_MLME, DBG_LOUD,
+			("PHY_SwitchWirelessBand8812(): Switch to 5G Band. "
+			"Count = %d reg41A=0x%x\n", count, reg_41a));
+
+		// 2012/02/01, Sinda add registry to switch workaround without long-run verification for scan issue.
+		rtl_set_bbreg(hw, ROFDMCCKEN, BOFDMEN|BCCKEN, 0x03);
+
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+			rtl_set_bbreg(hw, 0x830, 0xE, 0x3);	/*0x830[3:1] = 0x3*/
+			rtl_set_bbreg(hw, 0x834, 0x3, 0x2);	/*0x834[1:0] = 0x2*/
+		}
+
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+			/* AGC table select */
+			rtl_set_bbreg(hw, RA_TXSCALE, 0xF00, 1); /* 0xC1C[11:8] = 1*/
+		} else
+			rtl_set_bbreg(hw, 0x82c, 0x3, 1);	// 0x82C[1:0] = 2'b00
+
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+			rtl_set_bbreg(hw, RA_RFE_PINMUX, BMASKDWORD, 0x77337777);
+			rtl_set_bbreg(hw, RB_RFE_PINMUX, BMASKDWORD, 0x77337777);
+			rtl_set_bbreg(hw, RA_RFE_INV, 0x3ff00000, 0x010);
+			rtl_set_bbreg(hw, RB_RFE_INV, 0x3ff00000, 0x010);
+		}
+
+		rtl_set_bbreg(hw, RTXPATH, 0xf0, txpath);
+		rtl_set_bbreg(hw, RCCK_RX, 0x0f000000, rxpath);
+
+		RT_TRACE(COMP_SCAN, DBG_LOUD,
+			("==>PHY_SwitchWirelessBand8812() BAND_ON_5G settings OFDM index 0x%x\n",
+			rtlpriv->dm.ofdm_index[RF90_PATH_A]));
+	}
+
+	if ((rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) ||
+			(rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)) {
+		rtl_set_bbreg(hw, RA_TXSCALE, 0xFFE00000,
+						 phy_get_tx_bb_swing_8812A(hw, band, RF90_PATH_A)); // 0xC1C[31:21]
+		rtl_set_bbreg(hw, RB_TXSCALE, 0xFFE00000,
+						 phy_get_tx_bb_swing_8812A(hw, band, RF90_PATH_B)); // 0xE1C[31:21]
+
+		/* <20121005, Kordan> When TxPowerTrack is ON, we should take care of the change of BB swing.
+		    That is, reset all info to trigger Tx power tracking.*/
+		if (band != current_band) {
+			bb_diff_between_band = (rtldm->bb_swing_diff_2g - rtldm->bb_swing_diff_5g);
+			bb_diff_between_band = (band == BAND_ON_2_4G) ? bb_diff_between_band : (-1 * bb_diff_between_band);
+			rtldm->default_ofdm_index += bb_diff_between_band * 2;
+		}
+		rtl8821ae_dm_clear_txpower_tracking_state(hw);
+	}
+
+	RT_TRACE(COMP_SCAN, DBG_TRACE,
+		("<==rtl8821ae_phy_switch_wirelessband():Switch Band OK.\n"));
+	return;
+}
+
+static bool _rtl8821ae_check_condition(struct ieee80211_hw *hw,
+	const u32  Condition
+	)
+{
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u32 _board = rtlefuse->board_type; /*need efuse define*/
+	u32 _interface = rtlhal->interface;
+	u32 _platform = 0x08;/*SupportPlatform */
+	u32 cond = Condition;
+
+	if ( Condition == 0xCDCDCDCD )
+		return true;
+
+	cond = Condition & 0xFF;
+	if ( (_board != cond) == 0 && cond != 0xFF)
+		return false;
+
+	cond = Condition & 0xFF00;
+	cond = cond >> 8;
+	if ( (_interface & cond) == 0 && cond != 0x07)
+		return false;
+
+	cond = Condition & 0xFF0000;
+	cond = cond >> 16;
+	if ( (_platform & cond) == 0 && cond != 0x0F)
+		return false;
+	return true;
+}
+
+static void _rtl8821ae_config_rf_reg(struct ieee80211_hw *hw,
+	u32 addr,
+	u32 data,
+	enum radio_path rfpath,
+	u32 regaddr
+	)
+{
+	if ( addr == 0xfe || addr == 0xffe) {
+		mdelay(50);
+	} else {
+		rtl_set_rfreg(hw, rfpath, regaddr, RFREG_OFFSET_MASK, data);
+		udelay(1);
+	}
+}
+
+static void _rtl8821ae_config_rf_radio_a(struct ieee80211_hw *hw,
+	u32 addr, u32 data)
+{
+	u32 content = 0x1000; /*RF Content: radio_a_txt*/
+	u32 maskforphyset = (u32)(content & 0xE000);
+
+	_rtl8821ae_config_rf_reg(hw, addr, data, RF90_PATH_A, addr | maskforphyset);
+
+}
+
+static void _rtl8821ae_config_rf_radio_b(struct ieee80211_hw *hw,
+	u32 addr, u32 data)
+{
+	u32 content = 0x1001; /*RF Content: radio_b_txt*/
+	u32 maskforphyset = (u32)(content & 0xE000);
+
+	_rtl8821ae_config_rf_reg(hw, addr, data, RF90_PATH_B, addr | maskforphyset);
+
+}
+
+static void _rtl8812ae_config_bb_reg(struct ieee80211_hw *hw,
+	u32 addr, u32 data)
+{
+	if ( addr == 0xfe) {
+		mdelay(50);
+	} else if ( addr == 0xfd)
+		mdelay(5);
+	else if ( addr == 0xfc)
+		mdelay(1);
+	else if ( addr == 0xfb)
+		udelay(50);
+	else if ( addr == 0xfa)
+		udelay(5);
+	else if ( addr == 0xf9)
+		udelay(1);
+	else {
+		rtl_set_bbreg(hw, addr, MASKDWORD,data);
+	}
+	udelay(1);
+}
+
+static void _rtl8821ae_config_bb_reg(struct ieee80211_hw *hw,
+	u32 addr, u32 data)
+{
+	if ( addr == 0xfe) {
+		mdelay(50);
+	} else if ( addr == 0xfd)
+		mdelay(5);
+	else if ( addr == 0xfc)
+		mdelay(1);
+	else if ( addr == 0xfb)
+		udelay(50);
+	else if ( addr == 0xfa)
+		udelay(5);
+	else if ( addr == 0xf9)
+		udelay(1);
+
+	rtl_set_bbreg(hw, addr, MASKDWORD,data);
+	udelay(1);
+}
+
+static void _rtl8821ae_phy_init_tx_power_by_rate(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	u8 band, rfpath, txnum, rate_section;
+
+	for ( band = BAND_ON_2_4G; band <= BAND_ON_5G; ++band )
+		for ( rfpath = 0; rfpath < TX_PWR_BY_RATE_NUM_RF; ++rfpath )
+			for ( txnum = 0; txnum < TX_PWR_BY_RATE_NUM_RF; ++txnum )
+				for ( rate_section = 0; rate_section < TX_PWR_BY_RATE_NUM_SECTION; ++rate_section )
+					rtlphy->tx_power_by_rate_offset[band][rfpath][txnum][rate_section] = 0;
+}
+
+void _rtl8821ae_phy_set_txpower_by_rate_base(struct ieee80211_hw *hw,
+														   u8 band, u8 path,
+														   u8 rate_section,
+														   u8 txnum, u8 value)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	if (path > RF90_PATH_D) {
+		RT_TRACE(COMP_INIT, DBG_LOUD,
+			("Invalid Rf Path %d in phy_SetTxPowerByRatBase()\n", path));
+		return;
+	}
+
+	if (band == BAND_ON_2_4G) {
+		switch (rate_section) {
+		case CCK:
+			rtlphy->txpwr_by_rate_base_24g[path][txnum][0] = value;
+			break;
+		case OFDM:
+			rtlphy->txpwr_by_rate_base_24g[path][txnum][1] = value;
+			break;
+		case HT_MCS0_MCS7:
+			rtlphy->txpwr_by_rate_base_24g[path][txnum][2] = value;
+			break;
+		case HT_MCS8_MCS15:
+			rtlphy->txpwr_by_rate_base_24g[path][txnum][3] = value;
+			break;
+		case VHT_1SSMCS0_1SSMCS9:
+			rtlphy->txpwr_by_rate_base_24g[path][txnum][4] = value;
+			break;
+		case VHT_2SSMCS0_2SSMCS9:
+			rtlphy->txpwr_by_rate_base_24g[path][txnum][5] = value;
+			break;
+		default:
+			RT_TRACE(COMP_INIT, DBG_LOUD, ( "Invalid RateSection %d in Band 2.4G, Rf Path %d, %dTx in PHY_SetTxPowerByRateBase()\n",
+					 rate_section, path, txnum ) );
+			break;
+		};
+	} else if (band == BAND_ON_5G) {
+		switch (rate_section) {
+		case OFDM:
+			rtlphy->txpwr_by_rate_base_5g[path][txnum][0] = value;
+			break;
+		case HT_MCS0_MCS7:
+			rtlphy->txpwr_by_rate_base_5g[path][txnum][1] = value;
+			break;
+		case HT_MCS8_MCS15:
+			rtlphy->txpwr_by_rate_base_5g[path][txnum][2] = value;
+			break;
+		case VHT_1SSMCS0_1SSMCS9:
+			rtlphy->txpwr_by_rate_base_5g[path][txnum][3] = value;
+			break;
+		case VHT_2SSMCS0_2SSMCS9:
+			rtlphy->txpwr_by_rate_base_5g[path][txnum][4] = value;
+			break;
+		default:
+			RT_TRACE(COMP_INIT, DBG_LOUD,
+				("Invalid RateSection %d in Band 5G, Rf Path %d, "
+				"%dTx in PHY_SetTxPowerByRateBase()\n",
+				rate_section, path, txnum));
+			break;
+		};
+	} else {
+		RT_TRACE(COMP_INIT, DBG_LOUD,
+			("Invalid Band %d in PHY_SetTxPowerByRateBase()\n", band));
+	}
+
+}
+
+u8 _rtl8821ae_phy_get_txpower_by_rate_base(struct ieee80211_hw *hw,
+														 u8 band, u8 path,
+														 u8 txnum, u8 rate_section)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 value = 0;
+
+	if (path > RF90_PATH_D) {
+		RT_TRACE(COMP_INIT, DBG_LOUD,
+			("Invalid Rf Path %d in PHY_GetTxPowerByRateBase()\n", path));
+		return 0;
+	}
+
+	if (band == BAND_ON_2_4G) {
+		switch (rate_section) {
+		case CCK:
+			value = rtlphy->txpwr_by_rate_base_24g[path][txnum][0];
+			break;
+		case OFDM:
+			value = rtlphy->txpwr_by_rate_base_24g[path][txnum][1];
+			break;
+		case HT_MCS0_MCS7:
+			value = rtlphy->txpwr_by_rate_base_24g[path][txnum][2];
+			break;
+		case HT_MCS8_MCS15:
+			value = rtlphy->txpwr_by_rate_base_24g[path][txnum][3];
+			break;
+		case VHT_1SSMCS0_1SSMCS9:
+			value = rtlphy->txpwr_by_rate_base_24g[path][txnum][4];
+			break;
+		case VHT_2SSMCS0_2SSMCS9:
+			value = rtlphy->txpwr_by_rate_base_24g[path][txnum][5];
+			break;
+		default:
+			RT_TRACE(COMP_INIT, DBG_LOUD,
+				("Invalid RateSection %d in Band 2.4G, Rf Path %d,"
+				" %dTx in PHY_GetTxPowerByRateBase()\n",
+				rate_section, path, txnum));
+			break;
+		};
+	} else if (band == BAND_ON_5G) {
+		switch (rate_section) {
+		case OFDM:
+			value = rtlphy->txpwr_by_rate_base_5g[path][txnum][0];
+			break;
+		case HT_MCS0_MCS7:
+			value = rtlphy->txpwr_by_rate_base_5g[path][txnum][1];
+			break;
+		case HT_MCS8_MCS15:
+			value = rtlphy->txpwr_by_rate_base_5g[path][txnum][2];
+			break;
+		case VHT_1SSMCS0_1SSMCS9:
+			value = rtlphy->txpwr_by_rate_base_5g[path][txnum][3];
+			break;
+		case VHT_2SSMCS0_2SSMCS9:
+			value = rtlphy->txpwr_by_rate_base_5g[path][txnum][4];
+			break;
+		default:
+			RT_TRACE(COMP_INIT, DBG_LOUD,
+				("Invalid RateSection %d in Band 5G, Rf Path %d,"
+				" %dTx in PHY_GetTxPowerByRateBase()\n",
+				rate_section, path, txnum));
+			break;
+		};
+	} else {
+		RT_TRACE(COMP_INIT, DBG_LOUD,
+			("Invalid Band %d in PHY_GetTxPowerByRateBase()\n", band));
+	}
+
+	return value;
+
+}
+void _rtl8821ae_phy_store_txpower_by_rate_base(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u16 rawValue = 0;
+	u8 base = 0, path = 0;
+
+	for (path = RF90_PATH_A; path <= RF90_PATH_B; ++path) {
+		rawValue = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][path][RF_1TX][0] >> 24) & 0xFF;
+		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
+		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G, path, CCK, RF_1TX, base);
+
+		rawValue = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][path][RF_1TX][2] >> 24) & 0xFF;
+		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
+		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G, path, OFDM, RF_1TX, base );
+
+		rawValue = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][path][RF_1TX][4] >> 24) & 0xFF;
+		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
+		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G, path, HT_MCS0_MCS7, RF_1TX, base );
+
+		rawValue = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][path][RF_2TX][6] >> 24) & 0xFF;
+		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
+		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G, path, HT_MCS8_MCS15, RF_2TX, base );
+
+		rawValue = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][path][RF_1TX][8] >> 24) & 0xFF;
+		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
+		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G, path, VHT_1SSMCS0_1SSMCS9, RF_1TX, base );
+
+		rawValue = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][path][RF_2TX][11] >> 8) & 0xFF;
+		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
+		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G, path, VHT_2SSMCS0_2SSMCS9, RF_2TX, base );
+
+		rawValue = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_5G][path][RF_1TX][2] >> 24) & 0xFF;
+		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
+		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_5G, path, OFDM, RF_1TX, base );
+
+		rawValue = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_5G][path][RF_1TX][4] >> 24) & 0xFF;
+		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
+		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_5G, path, HT_MCS0_MCS7, RF_1TX, base );
+
+		rawValue = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_5G][path][RF_2TX][6] >> 24) & 0xFF;
+		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
+		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_5G, path, HT_MCS8_MCS15, RF_2TX, base );
+
+		rawValue = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_5G][path][RF_1TX][8] >> 24) & 0xFF;
+		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
+		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_5G, path, VHT_1SSMCS0_1SSMCS9, RF_1TX, base );
+
+		rawValue = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_5G][path][RF_2TX][11] >> 8) & 0xFF;
+		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
+		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_5G, path, VHT_2SSMCS0_2SSMCS9, RF_2TX, base );
+	}
+}
+
+void _phy_convert_txpower_dbm_to_relative_value(u32 *data, u8 start,
+																u8 end, u8 base_val)
+{
+	char i = 0;
+	u8 temp_value = 0;
+	u32 temp_data = 0;
+
+	for (i = 3; i >= 0; --i)
+	{
+		if (i >= start && i <= end) {
+			// Get the exact value
+			temp_value = (u8) (*data >> (i * 8)) & 0xF;
+			temp_value += ((u8) ((*data >> (i * 8 + 4)) & 0xF)) * 10;
+
+			// Change the value to a relative value
+			temp_value = (temp_value > base_val) ? temp_value - base_val : base_val - temp_value;
+		} else {
+			temp_value = (u8) (*data >> (i * 8)) & 0xFF;
+		}
+		temp_data <<= 8;
+		temp_data |= temp_value;
+	}
+	*data = temp_data;
+}
+
+void _rtl8821ae_phy_convert_txpower_dbm_to_relative_value(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 base = 0, rfPath = 0;
+
+	for (rfPath = RF90_PATH_A; rfPath <= RF90_PATH_B; ++rfPath) {
+		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_2_4G, rfPath, RF_1TX, CCK);
+		RT_DISP( FPHY, PHY_TXPWR, ( "base of 2.4G CCK 1TX: %d\n", base ) );
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_1TX][0] ),
+			0, 3, base );
+
+		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_2_4G, rfPath, RF_1TX, OFDM );
+		RT_DISP( FPHY, PHY_TXPWR, ( "base of 2.4G OFDM 1TX: %d\n", base ) );
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_1TX][1] ),
+			0, 3, base );
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_1TX][2] ),
+			0, 3, base );
+
+		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_2_4G, rfPath, RF_1TX, HT_MCS0_MCS7 );
+		RT_DISP( FPHY, PHY_TXPWR, ( "base of 2.4G HTMCS0-7 1TX: %d\n", base ) );
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_1TX][3] ),
+			0, 3, base );
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_1TX][4] ),
+			0, 3, base );
+
+		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_2_4G, rfPath, RF_2TX, HT_MCS8_MCS15 );
+		RT_DISP( FPHY, PHY_TXPWR, ( "base of 2.4G HTMCS8-15 2TX: %d\n", base ) );
+
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_2TX][5] ),
+			0, 3, base );
+
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_2TX][6] ),
+			0, 3, base );
+
+		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_2_4G, rfPath, RF_1TX, VHT_1SSMCS0_1SSMCS9 );
+		RT_DISP( FPHY, PHY_TXPWR, ( "base of 2.4G VHT1SSMCS0-9 1TX: %d\n", base ) );
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_1TX][7] ),
+			0, 3, base );
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_1TX][8] ),
+			0, 3, base );
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_1TX][9] ),
+			0, 1, base );
+
+		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_2_4G, rfPath, RF_2TX, VHT_2SSMCS0_2SSMCS9 );
+		RT_DISP( FPHY, PHY_TXPWR, ( "base of 2.4G VHT2SSMCS0-9 2TX: %d\n", base ) );
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_1TX][9] ),
+			2, 3, base );
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_2TX][10] ),
+			0, 3, base );
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_2TX][11] ),
+			0, 3, base );
+
+		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_5G, rfPath, RF_1TX, OFDM );
+		RT_DISP( FPHY, PHY_TXPWR, ( "base of 5G OFDM 1TX: %d\n", base ) );
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_1TX][1] ),
+			0, 3, base );
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_1TX][2] ),
+			0, 3, base );
+
+		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_5G, rfPath, RF_1TX, HT_MCS0_MCS7 );
+		RT_DISP( FPHY, PHY_TXPWR, ( "base of 5G HTMCS0-7 1TX: %d\n", base ) );
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_1TX][3] ),
+			0, 3, base );
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_1TX][4] ),
+			0, 3, base );
+
+		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_5G, rfPath, RF_2TX, HT_MCS8_MCS15 );
+		RT_DISP( FPHY, PHY_TXPWR, ( "base of 5G HTMCS8-15 2TX: %d\n", base ) );
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_2TX][5] ),
+			0, 3, base );
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_2TX][6] ),
+			0, 3, base );
+
+		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_5G, rfPath, RF_1TX, VHT_1SSMCS0_1SSMCS9 );
+		RT_DISP( FPHY, PHY_TXPWR, ( "base of 5G VHT1SSMCS0-9 1TX: %d\n", base ) );
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_1TX][7] ),
+			0, 3, base );
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_1TX][8] ),
+			0, 3, base );
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_1TX][9] ),
+			0, 1, base );
+
+		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_5G, rfPath, RF_2TX, VHT_2SSMCS0_2SSMCS9 );
+		RT_DISP( FPHY, PHY_TXPWR, ( "base of 5G VHT2SSMCS0-9 2TX: %d\n", base ) );
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_1TX][9] ),
+			2, 3, base );
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_2TX][10] ),
+			0, 3, base );
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_2TX][11] ),
+			0, 3, base );
+	}
+
+	RT_TRACE(COMP_POWER, DBG_TRACE,
+		("<===_rtl8821ae_phy_convert_txpower_dbm_to_relative_value()\n"));
+
+}
+
+void _rtl8821ae_phy_txpower_by_rate_configuration(struct ieee80211_hw *hw)
+{
+	_rtl8821ae_phy_store_txpower_by_rate_base(hw);
+	_rtl8821ae_phy_convert_txpower_dbm_to_relative_value(hw);
+}
+
+static bool _rtl8821ae_phy_bb8821a_config_parafile(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool rtstatus;
+
+	/*TX POWER LIMIT
+	    PHY_InitTxPowerLimit
+	    PHY_ConfigRFWithCustomPowerLimitTableParaFile*/
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+		rtstatus = _rtl8812ae_phy_config_bb_with_headerfile(hw,
+								BASEBAND_CONFIG_PHY_REG);
+	else{
+		rtstatus = _rtl8821ae_phy_config_bb_with_headerfile(hw,
+								BASEBAND_CONFIG_PHY_REG);
+	}
+	if (rtstatus != true) {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("Write BB Reg Fail!!"));
+		return false;
+	}
+	_rtl8821ae_phy_init_tx_power_by_rate(hw);
+	if (rtlefuse->autoload_failflag == false) {
+		//rtlphy->pwrgroup_cnt = 0;
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+			rtstatus = _rtl8812ae_phy_config_bb_with_pgheaderfile(hw,
+								   BASEBAND_CONFIG_PHY_REG);
+		else{
+			rtstatus = _rtl8821ae_phy_config_bb_with_pgheaderfile(hw,
+								   BASEBAND_CONFIG_PHY_REG);
+		}
+	}
+	if (rtstatus != true) {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("BB_PG Reg Fail!!"));
+		return false;
+	}
+
+	_rtl8821ae_phy_txpower_by_rate_configuration(hw);
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+		rtstatus = _rtl8812ae_phy_config_bb_with_headerfile(hw,
+							 BASEBAND_CONFIG_AGC_TAB);
+	else
+		rtstatus = _rtl8821ae_phy_config_bb_with_headerfile(hw,
+							 BASEBAND_CONFIG_AGC_TAB);
+
+	if (rtstatus != true) {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("AGC Table Fail\n"));
+		return false;
+	}
+	rtlphy->bcck_high_power = (bool) (rtl_get_bbreg(hw,
+							RFPGA0_XA_HSSIPARAMETER2,
+							0x200));
+	return true;
+}
+
+static bool _rtl8812ae_phy_config_mac_with_headerfile(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i, v1, v2;
+	u32 arraylength;
+	u32 *ptrarray;
+
+	RT_TRACE(COMP_INIT, DBG_TRACE, ("Read rtl8812AE_MAC_REG_Array\n"));
+	arraylength = RTL8812AEMAC_1T_ARRAYLEN;
+	ptrarray = RTL8812AE_MAC_REG_ARRAY;
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("Img:RTL8812AE_MAC_REG_ARRAY LEN %d\n",arraylength));
+	for (i = 0; i < arraylength; i += 2) {
+		v1 = ptrarray[i];
+		v2 = (u8) ptrarray[i + 1];
+		if (v1<0xCDCDCDCD) {
+			rtl_write_byte(rtlpriv, v1, (u8) v2);
+		} else {
+			if (!_rtl8821ae_check_condition(hw,v1)) {
+				/*Discard the following (offset, data) pairs*/
+				READ_NEXT_PAIR(ptrarray, v1, v2, i);
+				while (v2 != 0xDEAD &&
+					   v2 != 0xCDEF &&
+					   v2 != 0xCDCD && i < arraylength -2)
+					READ_NEXT_PAIR(ptrarray, v1, v2, i);
+
+				i -= 2; /* prevent from for-loop += 2*/
+			} else {/*Configure matched pairs and skip to end of if-else.*/
+				READ_NEXT_PAIR(ptrarray, v1, v2, i);
+				while (v2 != 0xDEAD &&
+					   v2 != 0xCDEF &&
+					   v2 != 0xCDCD && i < arraylength -2) {
+					rtl_write_byte(rtlpriv,v1,v2);
+					READ_NEXT_PAIR(ptrarray, v1, v2, i);
+				}
+
+				while (v2 != 0xDEAD && i < arraylength -2)
+					READ_NEXT_PAIR(ptrarray, v1, v2, i);
+			}
+		}
+	}
+	return true;
+}
+
+static bool _rtl8821ae_phy_config_mac_with_headerfile(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i, v1, v2;
+	u32 arraylength;
+	u32 *ptrarray;
+
+	RT_TRACE(COMP_INIT, DBG_TRACE, ("Read rtl8821AE_MAC_REG_Array\n"));
+	arraylength = RTL8821AEMAC_1T_ARRAYLEN;
+	ptrarray = RTL8821AE_MAC_REG_ARRAY;
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("Img:RTL8821AE_MAC_REG_ARRAY LEN %d\n",arraylength));
+	for (i = 0; i < arraylength; i += 2) {
+		v1 = ptrarray[i];
+		v2 = (u8) ptrarray[i + 1];
+		if (v1<0xCDCDCDCD) {
+			rtl_write_byte(rtlpriv, v1, (u8) v2);
+			continue;
+		} else {
+			if (!_rtl8821ae_check_condition(hw,v1)) {
+				/*Discard the following (offset, data) pairs*/
+				READ_NEXT_PAIR(ptrarray, v1, v2, i);
+				while (v2 != 0xDEAD &&
+					   v2 != 0xCDEF &&
+					   v2 != 0xCDCD && i < arraylength -2)
+					READ_NEXT_PAIR(ptrarray, v1, v2, i);
+
+				i -= 2; /* prevent from for-loop += 2*/
+			} else {/*Configure matched pairs and skip to end of if-else.*/
+				READ_NEXT_PAIR(ptrarray, v1, v2, i);
+				while (v2 != 0xDEAD &&
+					   v2 != 0xCDEF &&
+					   v2 != 0xCDCD && i < arraylength -2) {
+					rtl_write_byte(rtlpriv,v1,v2);
+					READ_NEXT_PAIR(ptrarray, v1, v2, i);
+				}
+
+				while (v2 != 0xDEAD && i < arraylength -2)
+					READ_NEXT_PAIR(ptrarray, v1, v2, i);
+			}
+		}
+	}
+	return true;
+}
+
+static bool _rtl8812ae_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
+						  u8 configtype)
+{
+	int i;
+	u32 *array_table;
+	u16 arraylen;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 v1 = 0, v2 = 0;
+
+	if (configtype == BASEBAND_CONFIG_PHY_REG) {
+		arraylen = RTL8812AEPHY_REG_1TARRAYLEN;
+		array_table = RTL8812AE_PHY_REG_ARRAY;
+
+		for (i = 0; i < arraylen; i += 2) {
+			v1 = array_table[i];
+			v2 = array_table[i+1];
+			if (v1<0xCDCDCDCD) {
+				_rtl8812ae_config_bb_reg(hw, v1, v2);
+				continue;
+			} else {/*This line is the start line of branch.*/
+				if (!_rtl8821ae_check_condition(hw,v1)) {
+					/*Discard the following (offset, data) pairs*/
+					READ_NEXT_PAIR(array_table,v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD && i < arraylen -2)
+					    READ_NEXT_PAIR(array_table,v1, v2, i);
+
+					i -= 2; /* prevent from for-loop += 2*/
+				} else {/*Configure matched pairs and skip to end of if-else.*/
+					READ_NEXT_PAIR(array_table,v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD && i < arraylen -2) {
+						_rtl8812ae_config_bb_reg(hw,v1,v2);
+						READ_NEXT_PAIR(array_table,v1, v2, i);
+					}
+
+					while (v2 != 0xDEAD && i < arraylen -2)
+						READ_NEXT_PAIR(array_table,v1, v2, i);
+				}
+			}
+		}
+	} else if (configtype == BASEBAND_CONFIG_AGC_TAB) {
+			arraylen = RTL8812AEAGCTAB_1TARRAYLEN;
+			array_table = RTL8812AE_AGC_TAB_ARRAY;
+
+			for (i = 0; i < arraylen; i = i + 2) {
+				v1 = array_table[i];
+				v2 = array_table[i+1];
+				if (v1 < 0xCDCDCDCD) {
+					rtl_set_bbreg(hw, v1, MASKDWORD, v2);
+					udelay(1);
+					continue;
+			    } else {/*This line is the start line of branch.*/
+					if (!_rtl8821ae_check_condition(hw,v1)) {
+						/*Discard the following (offset, data) pairs*/
+						READ_NEXT_PAIR(array_table,v1, v2, i);
+						while (v2 != 0xDEAD &&
+						       v2 != 0xCDEF &&
+						       v2 != 0xCDCD && i < arraylen -2)
+						    READ_NEXT_PAIR(array_table,v1, v2, i);
+
+						i -= 2; /* prevent from for-loop += 2*/
+					}else{/*Configure matched pairs and skip to end of if-else.*/
+						READ_NEXT_PAIR(array_table,v1, v2, i);
+						while (v2 != 0xDEAD &&
+						       v2 != 0xCDEF &&
+						       v2 != 0xCDCD && i < arraylen -2)
+						{
+							rtl_set_bbreg(hw, v1, MASKDWORD, v2);
+							udelay(1);
+							READ_NEXT_PAIR(array_table,v1, v2, i);
+						}
+
+						while (v2 != 0xDEAD && i < arraylen -2)
+							READ_NEXT_PAIR(array_table,v1, v2, i);
+					}
+				}
+				RT_TRACE(COMP_INIT, DBG_TRACE,
+				 ("The agctab_array_table[0] is "
+				  "%x Rtl818EEPHY_REGArray[1] is %x \n",
+				  array_table[i],
+				  array_table[i + 1]));
+		}
+	}
+	return true;
+}
+
+static bool _rtl8821ae_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
+						  u8 configtype)
+{
+	int i;
+	u32 *array_table;
+	u16 arraylen;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 v1 = 0, v2 = 0;
+
+	if (configtype == BASEBAND_CONFIG_PHY_REG) {
+		arraylen = RTL8821AEPHY_REG_1TARRAYLEN;
+		array_table = RTL8821AE_PHY_REG_ARRAY;
+
+		for (i = 0; i < arraylen; i += 2) {
+			v1 = array_table[i];
+			v2 = array_table[i+1];
+			if (v1<0xCDCDCDCD) {
+				_rtl8821ae_config_bb_reg(hw, v1, v2);
+				continue;
+			} else {/*This line is the start line of branch.*/
+				if (!_rtl8821ae_check_condition(hw,v1)) {
+					/*Discard the following (offset, data) pairs*/
+					READ_NEXT_PAIR(array_table, v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD && i < arraylen -2)
+					    READ_NEXT_PAIR(array_table, v1, v2, i);
+
+					i -= 2; /* prevent from for-loop += 2*/
+				} else {/*Configure matched pairs and skip to end of if-else.*/
+					READ_NEXT_PAIR(array_table, v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD && i < arraylen -2) {
+						_rtl8821ae_config_bb_reg(hw,v1,v2);
+						READ_NEXT_PAIR(array_table, v1, v2, i);
+					}
+
+					while (v2 != 0xDEAD && i < arraylen -2)
+						READ_NEXT_PAIR(array_table, v1, v2, i);
+				}
+			}
+		}
+	} else if (configtype == BASEBAND_CONFIG_AGC_TAB) {
+			arraylen = RTL8821AEAGCTAB_1TARRAYLEN;
+			array_table = RTL8821AE_AGC_TAB_ARRAY;
+
+			for (i = 0; i < arraylen; i = i + 2) {
+				v1 = array_table[i];
+				v2 = array_table[i+1];
+				if (v1 < 0xCDCDCDCD) {
+					rtl_set_bbreg(hw, v1, MASKDWORD, v2);
+					udelay(1);
+					continue;
+			    } else {/*This line is the start line of branch.*/
+					if (!_rtl8821ae_check_condition(hw,v1)) {
+						/*Discard the following (offset, data) pairs*/
+						READ_NEXT_PAIR(array_table, v1, v2, i);
+						while (v2 != 0xDEAD &&
+						       v2 != 0xCDEF &&
+						       v2 != 0xCDCD && i < arraylen -2)
+						    READ_NEXT_PAIR(array_table, v1, v2, i);
+
+						i -= 2; /* prevent from for-loop += 2*/
+					}else{/*Configure matched pairs and skip to end of if-else.*/
+						READ_NEXT_PAIR(array_table, v1, v2, i);
+						while (v2 != 0xDEAD &&
+						       v2 != 0xCDEF &&
+						       v2 != 0xCDCD && i < arraylen -2)
+						{
+							rtl_set_bbreg(hw, v1, MASKDWORD, v2);
+							udelay(1);
+							READ_NEXT_PAIR(array_table, v1, v2, i);
+						}
+
+						while (v2 != 0xDEAD && i < arraylen -2)
+							READ_NEXT_PAIR(array_table, v1, v2, i);
+					}
+				}
+				RT_TRACE(COMP_INIT, DBG_TRACE,
+				 ("The agctab_array_table[0] is "
+				  "%x Rtl818EEPHY_REGArray[1] is %x \n",
+				  array_table[i],
+				  array_table[i + 1]));
+		}
+	}
+	return true;
+}
+
+static u8 _rtl8821ae_get_rate_selection_index(u32 regaddr)
+{
+	u8 index = 0;
+
+	regaddr &= 0xFFF;
+	if (regaddr >= 0xC20 && regaddr <= 0xC4C)
+		index = (u8) ((regaddr - 0xC20) / 4);
+	else if (regaddr >= 0xE20 && regaddr <= 0xE4C)
+		index = (u8) ((regaddr - 0xE20) / 4);
+	else
+		RT_ASSERT(!COMP_INIT,
+			("Invalid RegAddr 0x%x in"
+			 "PHY_GetRateSectionIndexOfTxPowerByRate()\n",regaddr));
+
+	return index;
+}
+
+static void _rtl8821ae_store_tx_power_by_rate(struct ieee80211_hw *hw,
+						   								 u32 band, u32 rfpath,
+						   								 u32 txnum, u32 regaddr,
+						   								 u32 bitmask, u32 data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 rate_section = _rtl8821ae_get_rate_selection_index(regaddr);
+
+	if (band != BAND_ON_2_4G && band != BAND_ON_5G)
+		RT_TRACE(COMP_INIT, DBG_WARNING, ("Invalid Band %d\n", band));
+
+	if (rfpath > MAX_RF_PATH)
+		RT_TRACE(COMP_INIT, DBG_WARNING, ("Invalid RfPath %d\n", rfpath));
+
+	if (txnum > MAX_RF_PATH)
+		RT_TRACE(COMP_INIT, DBG_WARNING, ("Invalid TxNum %d\n", txnum ) );
+
+	rtlphy->tx_power_by_rate_offset[band][rfpath][txnum][rate_section] = data;
+	RT_TRACE(COMP_INIT, DBG_WARNING,( "pHalData->TxPwrByRateOffset[Band %d][RfPath %d][TxNum %d][RateSection %d] = 0x%x\n",
+			band, rfpath, txnum, rate_section, rtlphy->tx_power_by_rate_offset[band][rfpath][txnum][rate_section]));
+
+}
+
+static bool _rtl8812ae_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
+						   											 u8 configtype)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int i;
+	u32 *phy_regarray_table_pg;
+	u16 phy_regarray_pg_len;
+	u32 v1, v2, v3, v4, v5, v6;
+
+	phy_regarray_pg_len = RTL8812AEPHY_REG_ARRAY_PGLEN;
+	phy_regarray_table_pg = RTL8812AE_PHY_REG_ARRAY_PG;
+
+	if (configtype == BASEBAND_CONFIG_PHY_REG) {
+		for (i = 0; i < phy_regarray_pg_len; i += 6) {
+			v1 = phy_regarray_table_pg[i];
+			v2 = phy_regarray_table_pg[i+1];
+			v3 = phy_regarray_table_pg[i+2];
+			v4 = phy_regarray_table_pg[i+3];
+			v5 = phy_regarray_table_pg[i+4];
+			v6 = phy_regarray_table_pg[i+5];
+
+			if (v1<0xCDCDCDCD) {
+				if ( (v4 == 0xfe) || (v4 == 0xffe))
+					mdelay(50);
+				else
+					/*_rtl8821ae_store_pwrIndex_diffrate_offset*/
+					_rtl8821ae_store_tx_power_by_rate(hw, v1, v2, v3, v4, v5, v6);
+				continue;
+			} else {
+				if (!_rtl8821ae_check_condition(hw,v1)) { /*don't need the hw_body*/
+			    	i += 2; /* skip the pair of expression*/
+			        v1 = phy_regarray_table_pg[i];
+			        v2 = phy_regarray_table_pg[i+1];
+			        v3 = phy_regarray_table_pg[i+2];
+			        while (v2 != 0xDEAD) {
+			            i += 3;
+			            v1 = phy_regarray_table_pg[i];
+			            v2 = phy_regarray_table_pg[i+1];
+			            v3 = phy_regarray_table_pg[i+2];
+			        }
+			    }
+			}
+		}
+	} else {
+
+		RT_TRACE(COMP_SEND, DBG_TRACE,
+			 ("configtype != BaseBand_Config_PHY_REG\n"));
+	}
+	return true;
+}
+
+static bool _rtl8821ae_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
+						   											 u8 configtype)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int i;
+	u32 *phy_regarray_table_pg;
+	u16 phy_regarray_pg_len;
+	u32 v1, v2, v3, v4, v5, v6;
+
+	phy_regarray_pg_len = RTL8821AEPHY_REG_ARRAY_PGLEN;
+	phy_regarray_table_pg = RTL8821AE_PHY_REG_ARRAY_PG;
+
+	if (configtype == BASEBAND_CONFIG_PHY_REG) {
+		for (i = 0; i < phy_regarray_pg_len; i += 6) {
+			v1 = phy_regarray_table_pg[i];
+			v2 = phy_regarray_table_pg[i+1];
+			v3 = phy_regarray_table_pg[i+2];
+			v4 = phy_regarray_table_pg[i+3];
+			v5 = phy_regarray_table_pg[i+4];
+			v6 = phy_regarray_table_pg[i+5];
+
+			if (v1<0xCDCDCDCD) {
+				if (v4 == 0xfe)
+					mdelay(50);
+				else if (v4 == 0xfd)
+					mdelay(5);
+				else if (v4 == 0xfc)
+					mdelay(1);
+				else if (v4 == 0xfb)
+					udelay(50);
+				else if (v4 == 0xfa)
+					udelay(5);
+				else if (v4 == 0xf9)
+					udelay(1);
+
+				/*_rtl8821ae_store_pwrIndex_diffrate_offset*/
+				_rtl8821ae_store_tx_power_by_rate(hw, v1, v2, v3, v4, v5, v6);
+				continue;
+			} else {
+				if (!_rtl8821ae_check_condition(hw,v1)) { /*don't need the hw_body*/
+			    	i += 2; /* skip the pair of expression*/
+			        v1 = phy_regarray_table_pg[i];
+			        v2 = phy_regarray_table_pg[i+1];
+			        v3 = phy_regarray_table_pg[i+2];
+			        while (v2 != 0xDEAD) {
+			            i += 3;
+			            v1 = phy_regarray_table_pg[i];
+			            v2 = phy_regarray_table_pg[i+1];
+			            v3 = phy_regarray_table_pg[i+2];
+			        }
+			    }
+			}
+		}
+	} else {
+
+		RT_TRACE(COMP_SEND, DBG_TRACE,
+			 ("configtype != BaseBand_Config_PHY_REG\n"));
+	}
+	return true;
+}
+
+bool rtl8812ae_phy_config_rf_with_headerfile(struct ieee80211_hw * hw,
+					  									 enum radio_path rfpath)
+{
+	#define READ_NEXT_RF_PAIR_8812(radioa_array_table,v1, v2, i) do { i += 2; v1 = radioa_array_table[i]; v2 = radioa_array_table[i+1]; } while(0)
+
+	int i;
+	bool rtstatus = true;
+	u32 *radioa_array_table_a, *radioa_array_table_b;
+	u16 radioa_arraylen_a, radioa_arraylen_b;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 v1 = 0, v2 = 0;
+
+	radioa_arraylen_a = RTL8812AE_RADIOA_1TARRAYLEN;
+	radioa_array_table_a= RTL8812AE_RADIOA_ARRAY;
+	radioa_arraylen_b= RTL8812AE_RADIOB_1TARRAYLEN;
+	radioa_array_table_b = RTL8812AE_RADIOB_ARRAY;
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("Radio_A:RTL8821AE_RADIOA_ARRAY %d\n",radioa_arraylen_a));
+	RT_TRACE(COMP_INIT, DBG_LOUD, ("Radio No %x\n", rfpath));
+	rtstatus = true;
+	switch (rfpath) {
+	case RF90_PATH_A:
+		for (i = 0; i < radioa_arraylen_a; i = i + 2) {
+			v1 = radioa_array_table_a[i];
+			v2 = radioa_array_table_a[i+1];
+			if (v1<0xcdcdcdcd) {
+				_rtl8821ae_config_rf_radio_a(hw,v1,v2);
+				continue;
+			}else{/*This line is the start line of branch.*/
+				if(!_rtl8821ae_check_condition(hw,v1)){
+					/*Discard the following (offset, data) pairs*/
+					READ_NEXT_RF_PAIR_8812(radioa_array_table_a,v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD && i < radioa_arraylen_a-2)
+					    READ_NEXT_RF_PAIR_8812(radioa_array_table_a,v1, v2, i);
+
+					i -= 2; /* prevent from for-loop += 2*/
+				} else {/*Configure matched pairs and skip to end of if-else.*/
+					READ_NEXT_RF_PAIR_8812(radioa_array_table_a,v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD && i < radioa_arraylen_a -2) {
+						_rtl8821ae_config_rf_radio_a(hw,v1,v2);
+						READ_NEXT_RF_PAIR_8812(radioa_array_table_a,v1, v2, i);
+					}
+
+					while (v2 != 0xDEAD && i < radioa_arraylen_a-2)
+						READ_NEXT_RF_PAIR_8812(radioa_array_table_a,v1, v2, i);
+				}
+			}
+		}
+		break;
+	case RF90_PATH_B:
+		for (i = 0; i < radioa_arraylen_b; i = i + 2) {
+			v1 = radioa_array_table_b[i];
+			v2 = radioa_array_table_b[i+1];
+			if (v1<0xcdcdcdcd) {
+				_rtl8821ae_config_rf_radio_b(hw,v1,v2);
+				continue;
+			}else{/*This line is the start line of branch.*/
+				if(!_rtl8821ae_check_condition(hw,v1)){
+					/*Discard the following (offset, data) pairs*/
+					READ_NEXT_RF_PAIR_8812(radioa_array_table_b,v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD && i < radioa_arraylen_b-2)
+					    READ_NEXT_RF_PAIR_8812(radioa_array_table_b,v1, v2, i);
+
+					i -= 2; /* prevent from for-loop += 2*/
+				} else {/*Configure matched pairs and skip to end of if-else.*/
+					READ_NEXT_RF_PAIR_8812(radioa_array_table_b,v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD && i < radioa_arraylen_b-2) {
+						_rtl8821ae_config_rf_radio_b(hw,v1,v2);
+						READ_NEXT_RF_PAIR_8812(radioa_array_table_b,v1, v2, i);
+					}
+
+					while (v2 != 0xDEAD && i < radioa_arraylen_b-2)
+						READ_NEXT_RF_PAIR_8812(radioa_array_table_b,v1, v2, i);
+				}
+			}
+		}
+		break;
+	case RF90_PATH_C:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("switch case not process \n"));
+		break;
+	case RF90_PATH_D:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("switch case not process \n"));
+		break;
+	}
+	return true;
+}
+
+
+bool rtl8821ae_phy_config_rf_with_headerfile(struct ieee80211_hw * hw,
+					  									 enum radio_path rfpath)
+{
+	#define READ_NEXT_RF_PAIR(v1, v2, i) do { i += 2; v1 = radioa_array_table[i]; v2 = radioa_array_table[i+1]; } while(0)
+
+	int i;
+	bool rtstatus = true;
+	u32 *radioa_array_table;
+	u16 radioa_arraylen;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	//struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u32 v1 = 0, v2 = 0;
+
+	radioa_arraylen = RTL8821AE_RADIOA_1TARRAYLEN;
+	radioa_array_table = RTL8821AE_RADIOA_ARRAY;
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("Radio_A:RTL8821AE_RADIOA_ARRAY %d\n",radioa_arraylen));
+	RT_TRACE(COMP_INIT, DBG_LOUD, ("Radio No %x\n", rfpath));
+	rtstatus = true;
+	switch (rfpath) {
+	case RF90_PATH_A:
+		for (i = 0; i < radioa_arraylen; i = i + 2) {
+			v1 = radioa_array_table[i];
+			v2 = radioa_array_table[i+1];
+			if (v1<0xcdcdcdcd) {
+				_rtl8821ae_config_rf_radio_a(hw,v1,v2);
+			}else{/*This line is the start line of branch.*/
+				if(!_rtl8821ae_check_condition(hw,v1)){
+					/*Discard the following (offset, data) pairs*/
+					READ_NEXT_RF_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD && i < radioa_arraylen -2)
+					    READ_NEXT_RF_PAIR(v1, v2, i);
+
+					i -= 2; /* prevent from for-loop += 2*/
+				} else {/*Configure matched pairs and skip to end of if-else.*/
+					READ_NEXT_RF_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD && i < radioa_arraylen -2) {
+						_rtl8821ae_config_rf_radio_a(hw,v1,v2);
+						READ_NEXT_RF_PAIR(v1, v2, i);
+					}
+
+					while (v2 != 0xDEAD && i < radioa_arraylen -2)
+						READ_NEXT_RF_PAIR(v1, v2, i);
+				}
+			}
+		}
+		break;
+
+	case RF90_PATH_B:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("switch case not process \n"));
+		break;
+	case RF90_PATH_C:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("switch case not process \n"));
+		break;
+	case RF90_PATH_D:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("switch case not process \n"));
+		break;
+	}
+	return true;
+}
+
+void rtl8821ae_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	rtlphy->default_initialgain[0] =
+	    (u8) rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, MASKBYTE0);
+	rtlphy->default_initialgain[1] =
+	    (u8) rtl_get_bbreg(hw, ROFDM0_XBAGCCORE1, MASKBYTE0);
+	rtlphy->default_initialgain[2] =
+	    (u8) rtl_get_bbreg(hw, ROFDM0_XCAGCCORE1, MASKBYTE0);
+	rtlphy->default_initialgain[3] =
+	    (u8) rtl_get_bbreg(hw, ROFDM0_XDAGCCORE1, MASKBYTE0);
+
+	RT_TRACE(COMP_INIT, DBG_TRACE,
+		 ("Default initial gain (c50=0x%x, "
+		  "c58=0x%x, c60=0x%x, c68=0x%x \n",
+		  rtlphy->default_initialgain[0],
+		  rtlphy->default_initialgain[1],
+		  rtlphy->default_initialgain[2],
+		  rtlphy->default_initialgain[3]));
+
+	rtlphy->framesync = (u8) rtl_get_bbreg(hw,
+					       ROFDM0_RXDETECTOR3, MASKBYTE0);
+	rtlphy->framesync_c34 = rtl_get_bbreg(hw,
+					      ROFDM0_RXDETECTOR2, MASKDWORD);
+
+	RT_TRACE(COMP_INIT, DBG_TRACE,
+		 ("Default framesync (0x%x) = 0x%x \n",
+		  ROFDM0_RXDETECTOR3, rtlphy->framesync));
+}
+
+static void _rtl8821ae_phy_init_bb_rf_register_definition(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfintfs = RFPGA0_XAB_RFINTERFACESW;
+	rtlphy->phyreg_def[RF90_PATH_B].rfintfs = RFPGA0_XAB_RFINTERFACESW;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfintfo = RFPGA0_XA_RFINTERFACEOE;
+	rtlphy->phyreg_def[RF90_PATH_B].rfintfo = RFPGA0_XB_RFINTERFACEOE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfintfe = RFPGA0_XA_RFINTERFACEOE;
+	rtlphy->phyreg_def[RF90_PATH_B].rfintfe = RFPGA0_XB_RFINTERFACEOE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rf3wire_offset = RA_LSSIWRITE_8821A;
+	rtlphy->phyreg_def[RF90_PATH_B].rf3wire_offset = RB_LSSIWRITE_8821A;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfhssi_para2 = RHSSIREAD_8821AE;
+	rtlphy->phyreg_def[RF90_PATH_B].rfhssi_para2 = RHSSIREAD_8821AE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rflssi_readback = RA_SIREAD_8821A;
+	rtlphy->phyreg_def[RF90_PATH_B].rflssi_readback = RB_SIREAD_8821A;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rflssi_readbackpi = RA_PIREAD_8821A;
+	rtlphy->phyreg_def[RF90_PATH_B].rflssi_readbackpi = RB_PIREAD_8821A;
+}
+
+void rtl8821ae_phy_get_txpower_level(struct ieee80211_hw *hw, long *powerlevel)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 txpwr_level;
+	long txpwr_dbm;
+
+	txpwr_level = rtlphy->cur_cck_txpwridx;
+	txpwr_dbm = _rtl8821ae_phy_txpwr_idx_to_dbm(hw,
+						 WIRELESS_MODE_B, txpwr_level);
+	txpwr_level = rtlphy->cur_ofdm24g_txpwridx;
+	if (_rtl8821ae_phy_txpwr_idx_to_dbm(hw,
+					 WIRELESS_MODE_G,
+					 txpwr_level) > txpwr_dbm)
+		txpwr_dbm =
+		    _rtl8821ae_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_G,
+						 txpwr_level);
+	txpwr_level = rtlphy->cur_ofdm24g_txpwridx;
+	if (_rtl8821ae_phy_txpwr_idx_to_dbm(hw,
+					 WIRELESS_MODE_N_24G,
+					 txpwr_level) > txpwr_dbm)
+		txpwr_dbm =
+		    _rtl8821ae_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_N_24G,
+						 txpwr_level);
+	*powerlevel = txpwr_dbm;
+}
+
+static bool _rtl8821ae_phy_get_chnl_index(u8 channel, u8 *chnl_index)
+{
+	u8 channel_5g[CHANNEL_MAX_NUMBER_5G] =
+				 {36,38,40,42,44,46,48,50,52,54,56,58,60,62,64,100,102,104,106,108,110,112,
+				114,116,118,120,122,124,126,128,130,132,134,136,138,140,142,144,149,151,
+				153,155,157,159,161,163,165,167,168,169,171,173,175,177};
+	u8 i = 0;
+	bool in_24g = true;
+
+	if (channel <= 14) {
+		in_24g = true;
+		*chnl_index = channel - 1;
+	} else {
+		in_24g = false;
+
+		for (i = 0; i < sizeof(channel_5g) / sizeof(u8); ++i) {
+			if (channel_5g[i] == channel) {
+				*chnl_index = i;
+				return in_24g;
+			}
+		}
+	}
+	return in_24g;
+}
+
+static char _rtl8821ae_phy_get_ratesection_intxpower_byrate(u8 path, u8 rate)
+{
+	char rate_section = 0;
+	switch (rate) {
+	case DESC_RATE1M:
+	case DESC_RATE2M:
+	case DESC_RATE5_5M:
+	case DESC_RATE11M:
+		rate_section = 0;
+		break;
+
+	case DESC_RATE6M:
+	case DESC_RATE9M:
+	case DESC_RATE12M:
+	case DESC_RATE18M:
+		rate_section = 1;
+		break;
+
+	case DESC_RATE24M:
+	case DESC_RATE36M:
+	case DESC_RATE48M:
+	case DESC_RATE54M:
+		rate_section = 2;
+		break;
+
+	case DESC_RATEMCS0:
+	case DESC_RATEMCS1:
+	case DESC_RATEMCS2:
+	case DESC_RATEMCS3:
+		rate_section = 3;
+		break;
+
+	case DESC_RATEMCS4:
+	case DESC_RATEMCS5:
+	case DESC_RATEMCS6:
+	case DESC_RATEMCS7:
+		rate_section = 4;
+		break;
+
+	case DESC_RATEMCS8:
+	case DESC_RATEMCS9:
+	case DESC_RATEMCS10:
+	case DESC_RATEMCS11:
+		rate_section = 5;
+		break;
+
+	case DESC_RATEMCS12:
+	case DESC_RATEMCS13:
+	case DESC_RATEMCS14:
+	case DESC_RATEMCS15:
+		rate_section = 6;
+		break;
+
+	case DESC_RATEVHT1SS_MCS0:
+	case DESC_RATEVHT1SS_MCS1:
+	case DESC_RATEVHT1SS_MCS2:
+	case DESC_RATEVHT1SS_MCS3:
+		rate_section = 7;
+		break;
+
+	case DESC_RATEVHT1SS_MCS4:
+	case DESC_RATEVHT1SS_MCS5:
+	case DESC_RATEVHT1SS_MCS6:
+	case DESC_RATEVHT1SS_MCS7:
+		rate_section = 8;
+		break;
+
+	case DESC_RATEVHT1SS_MCS8:
+	case DESC_RATEVHT1SS_MCS9:
+	case DESC_RATEVHT2SS_MCS0:
+	case DESC_RATEVHT2SS_MCS1:
+		rate_section = 9;
+		break;
+
+	case DESC_RATEVHT2SS_MCS2:
+	case DESC_RATEVHT2SS_MCS3:
+	case DESC_RATEVHT2SS_MCS4:
+	case DESC_RATEVHT2SS_MCS5:
+		rate_section = 10;
+		break;
+
+	case DESC_RATEVHT2SS_MCS6:
+	case DESC_RATEVHT2SS_MCS7:
+	case DESC_RATEVHT2SS_MCS8:
+	case DESC_RATEVHT2SS_MCS9:
+		rate_section = 11;
+		break;
+
+	default:
+		RT_ASSERT(true, ("Rate_Section is Illegal\n"));
+		break;
+	}
+
+	return rate_section;
+}
+
+static char _rtl8821ae_phy_get_txpower_by_rate(struct ieee80211_hw *hw,
+														   u8 band, u8 path, u8 rate)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 shift = 0, rate_section, tx_num;
+	char tx_pwr_diff = 0;
+
+	rate_section = _rtl8821ae_phy_get_ratesection_intxpower_byrate(path, rate);
+	tx_num = RF_TX_NUM_NONIMPLEMENT;
+
+	if (tx_num == RF_TX_NUM_NONIMPLEMENT) {
+		if ((rate >= DESC_RATEMCS8 && rate <= DESC_RATEMCS15 ) ||
+			(rate >= DESC_RATEVHT2SS_MCS2 && rate <= DESC_RATEVHT2SS_MCS9))
+			 tx_num = RF_2TX;
+		else
+			tx_num = RF_1TX;
+	}
+
+	switch (rate) {
+	case DESC_RATE1M:	shift = 0;		break;
+	case DESC_RATE2M:	shift = 8;		break;
+	case DESC_RATE5_5M:	shift = 16;		break;
+	case DESC_RATE11M:	shift = 24;		break;
+
+	case DESC_RATE6M:	shift = 0;		break;
+	case DESC_RATE9M:	shift = 8;      break;
+	case DESC_RATE12M:	shift = 16;     break;
+	case DESC_RATE18M:	shift = 24;     break;
+
+	case DESC_RATE24M:	shift = 0;    	break;
+	case DESC_RATE36M:	shift = 8;      break;
+	case DESC_RATE48M:	shift = 16;     break;
+	case DESC_RATE54M:	shift = 24;     break;
+
+	case DESC_RATEMCS0:	shift = 0; 		break;
+	case DESC_RATEMCS1:	shift = 8;      break;
+	case DESC_RATEMCS2:	shift = 16;     break;
+	case DESC_RATEMCS3:	shift = 24;     break;
+
+	case DESC_RATEMCS4:	shift = 0; 		break;
+	case DESC_RATEMCS5:	shift = 8;      break;
+	case DESC_RATEMCS6:	shift = 16;     break;
+	case DESC_RATEMCS7:	shift = 24;     break;
+
+	case DESC_RATEMCS8:	shift = 0; 		break;
+	case DESC_RATEMCS9:	shift = 8;      break;
+	case DESC_RATEMCS10:	shift = 16;     break;
+	case DESC_RATEMCS11:	shift = 24;     break;
+
+	case DESC_RATEMCS12:	shift = 0; 		break;
+	case DESC_RATEMCS13:	shift = 8;      break;
+	case DESC_RATEMCS14:	shift = 16;     break;
+	case DESC_RATEMCS15:	shift = 24;     break;
+
+	case DESC_RATEVHT1SS_MCS0:	shift = 0; 		break;
+	case DESC_RATEVHT1SS_MCS1:	shift = 8;      break;
+	case DESC_RATEVHT1SS_MCS2:	shift = 16;     break;
+	case DESC_RATEVHT1SS_MCS3:	shift = 24;     break;
+
+	case DESC_RATEVHT1SS_MCS4:	shift = 0; 		break;
+	case DESC_RATEVHT1SS_MCS5:	shift = 8;      break;
+	case DESC_RATEVHT1SS_MCS6:	shift = 16;     break;
+	case DESC_RATEVHT1SS_MCS7:	shift = 24;     break;
+
+	case DESC_RATEVHT1SS_MCS8:	shift = 0; 		break;
+	case DESC_RATEVHT1SS_MCS9:	shift = 8;      break;
+	case DESC_RATEVHT2SS_MCS0:	shift = 16;     break;
+	case DESC_RATEVHT2SS_MCS1:	shift = 24;     break;
+
+	case DESC_RATEVHT2SS_MCS2:	shift = 0; 		break;
+	case DESC_RATEVHT2SS_MCS3:	shift = 8;      break;
+	case DESC_RATEVHT2SS_MCS4:	shift = 16;     break;
+	case DESC_RATEVHT2SS_MCS5:	shift = 24;     break;
+
+	case DESC_RATEVHT2SS_MCS6:	shift = 0; 		break;
+	case DESC_RATEVHT2SS_MCS7:	shift = 8;      break;
+	case DESC_RATEVHT2SS_MCS8:	shift = 16;     break;
+	case DESC_RATEVHT2SS_MCS9:	shift = 24;     break;
+
+	default:
+		RT_ASSERT(true, ("Rate_Section is Illegal\n"));
+		break;
+	}
+
+	tx_pwr_diff = (u8) (rtlphy->tx_power_by_rate_offset[band][path][tx_num][rate_section] >> shift) & 0xff;
+
+	return	tx_pwr_diff;
+}
+
+static u8 _rtl8821ae_get_txpower_index(struct ieee80211_hw *hw, u8 path,
+				      							 u8 rate, u8 bandwidth, u8 channel)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u8 index = (channel - 1);
+	u8 txpower = 0;
+	bool in_24g = false;
+	char powerdiff_byrate = 0;
+
+	if (((rtlhal->current_bandtype == BAND_ON_2_4G) && (channel > 14 || channel < 1)) ||
+		((rtlhal->current_bandtype == BAND_ON_5G) && (channel <= 14))) {
+		index = 0;
+		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD, ("Illegal channel!!\n"));
+	}
+
+	in_24g = _rtl8821ae_phy_get_chnl_index(channel, &index);
+	if (in_24g) {
+		if (RX_HAL_IS_CCK_RATE(rate))
+			txpower = rtlefuse->txpwrlevel_cck[path][index];
+		else if ( DESC_RATE6M <= rate )
+			txpower = rtlefuse->txpwrlevel_ht40_1s[path][index];
+		else
+			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD, ("invalid rate\n"));
+
+		if (DESC_RATE6M <= rate && rate <= DESC_RATE54M && !RX_HAL_IS_CCK_RATE(rate))
+			txpower += rtlefuse->txpwr_legacyhtdiff[path][TX_1S];
+
+		if (bandwidth == HT_CHANNEL_WIDTH_20) {
+			if ((DESC_RATEMCS0 <= rate && rate <= DESC_RATEMCS15) ||
+				(DESC_RATEVHT1SS_MCS0 <= rate && rate <= DESC_RATEVHT2SS_MCS9))
+				txpower += rtlefuse->txpwr_ht20diff[path][TX_1S];
+			if ((DESC_RATEMCS8 <= rate && rate <= DESC_RATEMCS15) ||
+				(DESC_RATEVHT2SS_MCS0 <= rate && rate <= DESC_RATEVHT2SS_MCS9))
+				txpower += rtlefuse->txpwr_ht20diff[path][TX_2S];
+		} else if (bandwidth == HT_CHANNEL_WIDTH_20_40) {
+			if ((DESC_RATEMCS0 <= rate && rate <= DESC_RATEMCS15) ||
+				(DESC_RATEVHT1SS_MCS0 <= rate && rate <= DESC_RATEVHT2SS_MCS9))
+				txpower += rtlefuse->txpwr_ht40diff[path][TX_1S];
+			if ((DESC_RATEMCS8 <= rate && rate <= DESC_RATEMCS15) ||
+				(DESC_RATEVHT2SS_MCS0 <= rate && rate <= DESC_RATEVHT2SS_MCS9))
+				txpower += rtlefuse->txpwr_ht40diff[path][TX_2S];
+		} else if (bandwidth == HT_CHANNEL_WIDTH_80) {
+			if ((DESC_RATEMCS0 <= rate && rate <= DESC_RATEMCS15) ||
+				(DESC_RATEVHT1SS_MCS0 <= rate && rate <= DESC_RATEVHT2SS_MCS9))
+				txpower += rtlefuse->txpwr_ht40diff[path][TX_1S];
+			if ((DESC_RATEMCS8 <= rate && rate <= DESC_RATEMCS15) ||
+				(DESC_RATEVHT2SS_MCS0 <= rate && rate <= DESC_RATEVHT2SS_MCS9))
+				txpower += rtlefuse->txpwr_ht40diff[path][TX_2S];
+		}
+
+	} else {
+		if (DESC_RATE6M <= rate)
+			txpower = rtlefuse->txpwr_5g_bw40base[path][index];
+	    else
+	        RT_TRACE(COMP_POWER_TRACKING, DBG_WARNING,("INVALID Rate.\n"));
+
+        if (DESC_RATE6M <= rate && rate <= DESC_RATE54M && !RX_HAL_IS_CCK_RATE(rate))
+	    	txpower += rtlefuse->txpwr_5g_ofdmdiff[path][TX_1S];
+
+	    if (bandwidth == HT_CHANNEL_WIDTH_20) {
+	        if ((DESC_RATEMCS0 <= rate && rate <= DESC_RATEMCS15) ||
+				(DESC_RATEVHT1SS_MCS0 <= rate && rate <= DESC_RATEVHT2SS_MCS9))
+	    	    txpower += rtlefuse->txpwr_5g_bw20diff[path][TX_1S];
+	        if ((DESC_RATEMCS8 <= rate && rate <= DESC_RATEMCS15) ||
+				(DESC_RATEVHT2SS_MCS0 <= rate && rate <= DESC_RATEVHT2SS_MCS9))
+	    	    txpower += rtlefuse->txpwr_5g_bw20diff[path][TX_2S];
+	    } else if (bandwidth == HT_CHANNEL_WIDTH_20_40) {
+	        if ((DESC_RATEMCS0 <= rate && rate <= DESC_RATEMCS15) ||
+				(DESC_RATEVHT1SS_MCS0 <= rate && rate <= DESC_RATEVHT2SS_MCS9))
+	    	    txpower += rtlefuse->txpwr_5g_bw40diff[path][TX_1S];
+	        if ((DESC_RATEMCS8 <= rate && rate <= DESC_RATEMCS15) ||
+				(DESC_RATEVHT2SS_MCS0 <= rate && rate <= DESC_RATEVHT2SS_MCS9))
+	    	    txpower += rtlefuse->txpwr_5g_bw40diff[path][TX_2S];
+	    } else if (bandwidth == HT_CHANNEL_WIDTH_80) {
+			u8 channel_5g_80m[CHANNEL_MAX_NUMBER_5G_80M] = {42, 58, 106, 122, 138, 155, 171};
+			u8 i = 0;
+			for (i = 0; i < sizeof(channel_5g_80m) / sizeof(u8); ++i)
+				if (channel_5g_80m[i] == channel)
+					index = i;
+
+	        if ((DESC_RATEMCS0 <= rate && rate <= DESC_RATEMCS15) ||
+				(DESC_RATEVHT1SS_MCS0 <= rate && rate <= DESC_RATEVHT2SS_MCS9))
+	    	    txpower = rtlefuse->txpwr_5g_bw80base[path][index]
+	    	    		+ rtlefuse->txpwr_5g_bw80diff[path][TX_1S];
+	        if ((DESC_RATEMCS8 <= rate && rate <= DESC_RATEMCS15) ||
+				(DESC_RATEVHT2SS_MCS0 <= rate && rate <= DESC_RATEVHT2SS_MCS9))
+	    	    txpower = rtlefuse->txpwr_5g_bw80base[path][index]
+	    	    		+ rtlefuse->txpwr_5g_bw80diff[path][TX_1S]
+	    	    		+ rtlefuse->txpwr_5g_bw80diff[path][TX_2S];
+	    }
+	}
+	if (rtlefuse->eeprom_regulatory != 2)
+		powerdiff_byrate = _rtl8821ae_phy_get_txpower_by_rate(hw,
+									(u8)(!in_24g), path, rate);
+
+	if (rate == DESC_RATEVHT1SS_MCS8 || rate == DESC_RATEVHT1SS_MCS9 ||
+		rate == DESC_RATEVHT2SS_MCS8 || rate == DESC_RATEVHT2SS_MCS9)
+		txpower -= powerdiff_byrate;
+	else
+		txpower += powerdiff_byrate;
+
+	if (rate > DESC_RATE11M)
+		txpower += rtlpriv->dm.remnant_ofdm_swing_idx[path];
+	else
+		txpower += rtlpriv->dm.remnant_cck_idx;
+
+	if (txpower > MAX_POWER_INDEX)
+		txpower = MAX_POWER_INDEX;
+
+	return txpower;
+}
+
+static void _rtl8821ae_phy_set_txpower_index(struct ieee80211_hw *hw,
+												u8 power_index, u8 path, u8 rate)
+{
+	struct rtl_priv* rtlpriv = rtl_priv(hw);
+
+	if (path == RF90_PATH_A) {
+        switch (rate) {
+        case DESC_RATE1M:
+		rtl_set_bbreg(hw, RTXAGC_A_CCK11_CCK1, MASKBYTE0, power_index);
+		break;
+        case DESC_RATE2M:
+		rtl_set_bbreg(hw, RTXAGC_A_CCK11_CCK1, MASKBYTE1, power_index);
+		break;
+        case DESC_RATE5_5M:
+		rtl_set_bbreg(hw, RTXAGC_A_CCK11_CCK1, MASKBYTE2, power_index);
+		break;
+        case DESC_RATE11M:
+		rtl_set_bbreg(hw, RTXAGC_A_CCK11_CCK1, MASKBYTE3, power_index);
+		break;
+
+        case DESC_RATE6M:
+		rtl_set_bbreg(hw, RTXAGC_A_OFDM18_OFDM6, MASKBYTE0, power_index);
+		break;
+        case DESC_RATE9M:
+		rtl_set_bbreg(hw, RTXAGC_A_OFDM18_OFDM6, MASKBYTE1, power_index);
+		break;
+        case DESC_RATE12M:
+		rtl_set_bbreg(hw, RTXAGC_A_OFDM18_OFDM6, MASKBYTE2, power_index);
+		break;
+        case DESC_RATE18M:
+		rtl_set_bbreg(hw, RTXAGC_A_OFDM18_OFDM6, MASKBYTE3, power_index);
+		break;
+
+        case DESC_RATE24M:
+		rtl_set_bbreg(hw, RTXAGC_A_OFDM54_OFDM24, MASKBYTE0, power_index);
+		break;
+        case DESC_RATE36M:
+		rtl_set_bbreg(hw, RTXAGC_A_OFDM54_OFDM24, MASKBYTE1, power_index);
+		break;
+        case DESC_RATE48M:
+		rtl_set_bbreg(hw, RTXAGC_A_OFDM54_OFDM24, MASKBYTE2, power_index);
+		break;
+        case DESC_RATE54M:
+		rtl_set_bbreg(hw, RTXAGC_A_OFDM54_OFDM24, MASKBYTE3, power_index);
+		break;
+
+        case DESC_RATEMCS0:
+		rtl_set_bbreg(hw, RTXAGC_A_MCS03_MCS00, MASKBYTE0, power_index);
+		break;
+        case DESC_RATEMCS1:
+		rtl_set_bbreg(hw, RTXAGC_A_MCS03_MCS00, MASKBYTE1, power_index);
+		break;
+        case DESC_RATEMCS2:
+		rtl_set_bbreg(hw, RTXAGC_A_MCS03_MCS00, MASKBYTE2, power_index);
+		break;
+        case DESC_RATEMCS3:
+		rtl_set_bbreg(hw, RTXAGC_A_MCS03_MCS00, MASKBYTE3, power_index);
+		break;
+
+        case DESC_RATEMCS4:
+		rtl_set_bbreg(hw, RTXAGC_A_MCS07_MCS04, MASKBYTE0, power_index);
+		break;
+        case DESC_RATEMCS5:
+		rtl_set_bbreg(hw, RTXAGC_A_MCS07_MCS04, MASKBYTE1, power_index);
+		break;
+        case DESC_RATEMCS6:
+		rtl_set_bbreg(hw, RTXAGC_A_MCS07_MCS04, MASKBYTE2, power_index);
+		break;
+        case DESC_RATEMCS7:
+		rtl_set_bbreg(hw, RTXAGC_A_MCS07_MCS04, MASKBYTE3, power_index);
+		break;
+
+        case DESC_RATEMCS8:
+		rtl_set_bbreg(hw, RTXAGC_A_MCS11_MCS08, MASKBYTE0, power_index);
+		break;
+        case DESC_RATEMCS9:
+		rtl_set_bbreg(hw, RTXAGC_A_MCS11_MCS08, MASKBYTE1, power_index);
+		break;
+        case DESC_RATEMCS10:
+		rtl_set_bbreg(hw, RTXAGC_A_MCS11_MCS08, MASKBYTE2, power_index);
+		break;
+        case DESC_RATEMCS11:
+		rtl_set_bbreg(hw, RTXAGC_A_MCS11_MCS08, MASKBYTE3, power_index);
+		break;
+
+        case DESC_RATEMCS12:
+		rtl_set_bbreg(hw, RTXAGC_A_MCS15_MCS12, MASKBYTE0, power_index);
+		break;
+        case DESC_RATEMCS13:
+		rtl_set_bbreg(hw, RTXAGC_A_MCS15_MCS12, MASKBYTE1, power_index);
+		break;
+        case DESC_RATEMCS14:
+		rtl_set_bbreg(hw, RTXAGC_A_MCS15_MCS12, MASKBYTE2, power_index);
+		break;
+        case DESC_RATEMCS15:
+		rtl_set_bbreg(hw, RTXAGC_A_MCS15_MCS12, MASKBYTE3, power_index);
+		break;
+
+	case DESC_RATEVHT1SS_MCS0:
+		rtl_set_bbreg(hw, RTXAGC_A_NSS1INDEX3_NSS1INDEX0, MASKBYTE0, power_index);
+		break;
+	case DESC_RATEVHT1SS_MCS1:
+		rtl_set_bbreg(hw, RTXAGC_A_NSS1INDEX3_NSS1INDEX0, MASKBYTE1, power_index);
+		break;
+	case DESC_RATEVHT1SS_MCS2:
+		rtl_set_bbreg(hw, RTXAGC_A_NSS1INDEX3_NSS1INDEX0, MASKBYTE2, power_index);
+		break;
+	case DESC_RATEVHT1SS_MCS3:
+		rtl_set_bbreg(hw, RTXAGC_A_NSS1INDEX3_NSS1INDEX0, MASKBYTE3, power_index);
+		break;
+
+	case DESC_RATEVHT1SS_MCS4:
+		rtl_set_bbreg(hw, RTXAGC_A_NSS1INDEX7_NSS1INDEX4, MASKBYTE0, power_index);
+		break;
+	case DESC_RATEVHT1SS_MCS5:
+		rtl_set_bbreg(hw, RTXAGC_A_NSS1INDEX7_NSS1INDEX4, MASKBYTE1, power_index);
+		break;
+	case DESC_RATEVHT1SS_MCS6:
+		rtl_set_bbreg(hw, RTXAGC_A_NSS1INDEX7_NSS1INDEX4, MASKBYTE2, power_index);
+		break;
+	case DESC_RATEVHT1SS_MCS7:
+		rtl_set_bbreg(hw, RTXAGC_A_NSS1INDEX7_NSS1INDEX4, MASKBYTE3, power_index);
+		break;
+
+	case DESC_RATEVHT1SS_MCS8:
+		rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX1_NSS1INDEX8, MASKBYTE0, power_index);
+		break;
+	case DESC_RATEVHT1SS_MCS9:
+		rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX1_NSS1INDEX8, MASKBYTE1, power_index);
+		break;
+	case DESC_RATEVHT2SS_MCS0:
+		rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX1_NSS1INDEX8, MASKBYTE2, power_index);
+		break;
+	case DESC_RATEVHT2SS_MCS1:
+		rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX1_NSS1INDEX8, MASKBYTE3, power_index);
+		break;
+
+	case DESC_RATEVHT2SS_MCS2:
+		rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX5_NSS2INDEX2, MASKBYTE0, power_index);
+		break;
+	case DESC_RATEVHT2SS_MCS3:
+		rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX5_NSS2INDEX2, MASKBYTE1, power_index);
+		break;
+	case DESC_RATEVHT2SS_MCS4:
+		rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX5_NSS2INDEX2, MASKBYTE2, power_index);
+		break;
+	case DESC_RATEVHT2SS_MCS5:
+		rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX5_NSS2INDEX2, MASKBYTE3, power_index);
+		break;
+
+	case DESC_RATEVHT2SS_MCS6:
+		rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX9_NSS2INDEX6, MASKBYTE0, power_index);
+		break;
+	case DESC_RATEVHT2SS_MCS7:
+		rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX9_NSS2INDEX6, MASKBYTE1, power_index);
+		break;
+	case DESC_RATEVHT2SS_MCS8:
+		rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX9_NSS2INDEX6, MASKBYTE2, power_index);
+		break;
+	case DESC_RATEVHT2SS_MCS9:
+		rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX9_NSS2INDEX6, MASKBYTE3, power_index);
+		break;
+
+        default:
+			RT_TRACE(COMP_POWER, DBG_LOUD, ("Invalid Rate!!\n"));
+            break;
+        }
+    } else if (path == RF90_PATH_B) {
+	switch (rate) {
+	case DESC_RATE1M:
+		rtl_set_bbreg(hw, RTXAGC_B_CCK11_CCK1, MASKBYTE0, power_index);
+		break;
+	case DESC_RATE2M:
+		rtl_set_bbreg(hw, RTXAGC_B_CCK11_CCK1, MASKBYTE1, power_index);
+		break;
+	case DESC_RATE5_5M:
+		rtl_set_bbreg(hw, RTXAGC_B_CCK11_CCK1, MASKBYTE2, power_index);
+		break;
+	case DESC_RATE11M:
+		rtl_set_bbreg(hw, RTXAGC_B_CCK11_CCK1, MASKBYTE3, power_index);
+		break;
+
+	case DESC_RATE6M:
+		rtl_set_bbreg(hw, RTXAGC_B_OFDM18_OFDM6, MASKBYTE0, power_index);
+		break;
+	case DESC_RATE9M:
+		rtl_set_bbreg(hw, RTXAGC_B_OFDM18_OFDM6, MASKBYTE1, power_index);
+		break;
+	case DESC_RATE12M:
+		rtl_set_bbreg(hw, RTXAGC_B_OFDM18_OFDM6, MASKBYTE2, power_index);
+		break;
+	case DESC_RATE18M:
+		rtl_set_bbreg(hw, RTXAGC_B_OFDM18_OFDM6, MASKBYTE3, power_index);
+		break;
+
+	case DESC_RATE24M:
+		rtl_set_bbreg(hw, RTXAGC_B_OFDM54_OFDM24, MASKBYTE0, power_index);
+		break;
+	case DESC_RATE36M:
+		rtl_set_bbreg(hw, RTXAGC_B_OFDM54_OFDM24, MASKBYTE1, power_index);
+		break;
+	case DESC_RATE48M:
+		rtl_set_bbreg(hw, RTXAGC_B_OFDM54_OFDM24, MASKBYTE2, power_index);
+		break;
+	case DESC_RATE54M:
+		rtl_set_bbreg(hw, RTXAGC_B_OFDM54_OFDM24, MASKBYTE3, power_index);
+		break;
+
+	case DESC_RATEMCS0:
+		rtl_set_bbreg(hw, RTXAGC_B_MCS03_MCS00, MASKBYTE0, power_index);
+		break;
+	case DESC_RATEMCS1:
+		rtl_set_bbreg(hw, RTXAGC_B_MCS03_MCS00, MASKBYTE1, power_index);
+		break;
+	case DESC_RATEMCS2:
+		rtl_set_bbreg(hw, RTXAGC_B_MCS03_MCS00, MASKBYTE2, power_index);
+		break;
+	case DESC_RATEMCS3:
+		rtl_set_bbreg(hw, RTXAGC_B_MCS03_MCS00, MASKBYTE3, power_index);
+		break;
+
+	case DESC_RATEMCS4:
+		rtl_set_bbreg(hw, RTXAGC_B_MCS07_MCS04, MASKBYTE0, power_index);
+		break;
+	case DESC_RATEMCS5:
+		rtl_set_bbreg(hw, RTXAGC_B_MCS07_MCS04, MASKBYTE1, power_index);
+		break;
+	case DESC_RATEMCS6:
+		rtl_set_bbreg(hw, RTXAGC_B_MCS07_MCS04, MASKBYTE2, power_index);
+		break;
+	case DESC_RATEMCS7:
+		rtl_set_bbreg(hw, RTXAGC_B_MCS07_MCS04, MASKBYTE3, power_index);
+		break;
+
+	case DESC_RATEMCS8:
+		rtl_set_bbreg(hw, RTXAGC_B_MCS11_MCS08, MASKBYTE0, power_index);
+		break;
+	case DESC_RATEMCS9:
+		rtl_set_bbreg(hw, RTXAGC_B_MCS11_MCS08, MASKBYTE1, power_index);
+		break;
+	case DESC_RATEMCS10:
+		rtl_set_bbreg(hw, RTXAGC_B_MCS11_MCS08, MASKBYTE2, power_index);
+		break;
+	case DESC_RATEMCS11:
+		rtl_set_bbreg(hw, RTXAGC_B_MCS11_MCS08, MASKBYTE3, power_index);
+		break;
+
+	case DESC_RATEMCS12:
+		rtl_set_bbreg(hw, RTXAGC_B_MCS15_MCS12, MASKBYTE0, power_index);
+		break;
+	case DESC_RATEMCS13:
+		rtl_set_bbreg(hw, RTXAGC_B_MCS15_MCS12, MASKBYTE1, power_index);
+		break;
+	case DESC_RATEMCS14:
+		rtl_set_bbreg(hw, RTXAGC_B_MCS15_MCS12, MASKBYTE2, power_index);
+		break;
+	case DESC_RATEMCS15:
+		rtl_set_bbreg(hw, RTXAGC_B_MCS15_MCS12, MASKBYTE3, power_index);
+		break;
+
+	case DESC_RATEVHT1SS_MCS0:
+		rtl_set_bbreg(hw, RTXAGC_B_NSS1INDEX3_NSS1INDEX0, MASKBYTE0, power_index);
+		break;
+	case DESC_RATEVHT1SS_MCS1:
+		rtl_set_bbreg(hw, RTXAGC_B_NSS1INDEX3_NSS1INDEX0, MASKBYTE1, power_index);
+		break;
+	case DESC_RATEVHT1SS_MCS2:
+		rtl_set_bbreg(hw, RTXAGC_B_NSS1INDEX3_NSS1INDEX0, MASKBYTE2, power_index);
+		break;
+	case DESC_RATEVHT1SS_MCS3:
+		rtl_set_bbreg(hw, RTXAGC_B_NSS1INDEX3_NSS1INDEX0, MASKBYTE3, power_index);
+		break;
+
+	case DESC_RATEVHT1SS_MCS4:
+		rtl_set_bbreg(hw, RTXAGC_B_NSS1INDEX7_NSS1INDEX4, MASKBYTE0, power_index);
+		break;
+	case DESC_RATEVHT1SS_MCS5:
+		rtl_set_bbreg(hw, RTXAGC_B_NSS1INDEX7_NSS1INDEX4, MASKBYTE1, power_index);
+		break;
+	case DESC_RATEVHT1SS_MCS6:
+		rtl_set_bbreg(hw, RTXAGC_B_NSS1INDEX7_NSS1INDEX4, MASKBYTE2, power_index);
+		break;
+	case DESC_RATEVHT1SS_MCS7:
+		rtl_set_bbreg(hw, RTXAGC_B_NSS1INDEX7_NSS1INDEX4, MASKBYTE3, power_index);
+		break;
+
+	case DESC_RATEVHT1SS_MCS8:
+		rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX1_NSS1INDEX8, MASKBYTE0, power_index);
+		break;
+	case DESC_RATEVHT1SS_MCS9:
+		rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX1_NSS1INDEX8, MASKBYTE1, power_index);
+		break;
+	case DESC_RATEVHT2SS_MCS0:
+		rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX1_NSS1INDEX8, MASKBYTE2, power_index);
+		break;
+	case DESC_RATEVHT2SS_MCS1:
+		rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX1_NSS1INDEX8, MASKBYTE3, power_index);
+		break;
+
+	case DESC_RATEVHT2SS_MCS2:
+		rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX5_NSS2INDEX2, MASKBYTE0, power_index);
+		break;
+	case DESC_RATEVHT2SS_MCS3:
+		rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX5_NSS2INDEX2, MASKBYTE1, power_index);
+		break;
+	case DESC_RATEVHT2SS_MCS4:
+		rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX5_NSS2INDEX2, MASKBYTE2, power_index);
+		break;
+	case DESC_RATEVHT2SS_MCS5:
+		rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX5_NSS2INDEX2, MASKBYTE3, power_index);
+		break;
+
+	case DESC_RATEVHT2SS_MCS6:
+		rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX9_NSS2INDEX6, MASKBYTE0, power_index);
+		break;
+	case DESC_RATEVHT2SS_MCS7:
+		rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX9_NSS2INDEX6, MASKBYTE1, power_index);
+		break;
+	case DESC_RATEVHT2SS_MCS8:
+		rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX9_NSS2INDEX6, MASKBYTE2, power_index);
+		break;
+	case DESC_RATEVHT2SS_MCS9:
+		rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX9_NSS2INDEX6, MASKBYTE3, power_index);
+		break;
+
+       default:
+		RT_TRACE(COMP_POWER, DBG_LOUD, ("Invalid Rate!!\n"));
+            	break;
+        }
+    } else {
+		RT_TRACE(COMP_POWER, DBG_LOUD, ("Invalid RFPath!!\n"));
+    }
+}
+
+void _rtl8821ae_phy_set_txpower_level_by_path(struct ieee80211_hw *hw,
+													u8 *array, u8 path, u8 channel,
+													u8 size)
+{
+	struct rtl_phy *rtlphy = &(rtl_priv(hw)->phy);
+	u8 i;
+	u8 power_index;
+	for (i = 0; i < size; i ++) {
+		power_index = _rtl8821ae_get_txpower_index(hw, path, array[i],
+									rtlphy->current_chan_bw, channel);
+		_rtl8821ae_phy_set_txpower_index(hw, power_index, path, array[i]);
+	}
+}
+
+static void _rtl8821ae_phy_txpower_training_by_path(struct ieee80211_hw *hw,
+														u8 bw, u8 channel, u8 path)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	u8 i;
+	u32 power_level, data, offset;
+
+	if(path >= rtlphy->num_total_rfpath)
+		return;
+
+	data = 0;
+	if (path == RF90_PATH_A) {
+		power_level =
+			_rtl8821ae_get_txpower_index(hw, RF90_PATH_A,
+			DESC_RATEMCS7, bw, channel);
+		offset =  RA_TXPWRTRAING;
+	} else {
+		power_level =
+			_rtl8821ae_get_txpower_index(hw, RF90_PATH_A,
+			DESC_RATEMCS7, bw, channel);
+		offset =  RB_TXPWRTRAING;
+	}
+
+	for (i = 0; i < 3; i++) {
+		if (i == 0)
+			power_level = power_level - 10;
+		else if (i == 1)
+			power_level = power_level - 8;
+		else
+			power_level = power_level - 6;
+
+		data |= (((power_level > 2) ? (power_level) : 2) << (i * 8));
+	}
+	rtl_set_bbreg(hw, offset, 0xffffff, data);
+}
+
+void rtl8821ae_phy_set_txpower_level_by_path(struct ieee80211_hw *hw, u8 channel, u8 path)
+{
+	//struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtl_priv(hw)->phy);
+	u8 cck_rates[]  = {DESC_RATE1M, DESC_RATE2M, DESC_RATE5_5M, DESC_RATE11M};
+	u8 ofdm_rates[]  = {DESC_RATE6M, DESC_RATE9M, DESC_RATE12M, DESC_RATE18M,
+						DESC_RATE24M, DESC_RATE36M, DESC_RATE48M, DESC_RATE54M};
+	u8 ht_rates_1t[]  = {DESC_RATEMCS0, DESC_RATEMCS1, DESC_RATEMCS2, DESC_RATEMCS3,
+						 DESC_RATEMCS4, DESC_RATEMCS5, DESC_RATEMCS6, DESC_RATEMCS7};
+	u8 ht_rates_2t[]  = {DESC_RATEMCS8, DESC_RATEMCS9, DESC_RATEMCS10, DESC_RATEMCS11,
+						 DESC_RATEMCS12, DESC_RATEMCS13, DESC_RATEMCS14, DESC_RATEMCS15};
+	u8 vht_rates_1t[]  = {DESC_RATEVHT1SS_MCS0, DESC_RATEVHT1SS_MCS1, DESC_RATEVHT1SS_MCS2,
+						DESC_RATEVHT1SS_MCS3, DESC_RATEVHT1SS_MCS4,
+						  DESC_RATEVHT1SS_MCS5, DESC_RATEVHT1SS_MCS6, DESC_RATEVHT1SS_MCS7,
+						  DESC_RATEVHT1SS_MCS8, DESC_RATEVHT1SS_MCS9};
+	u8 vht_rates_2t[]  = {DESC_RATEVHT2SS_MCS0, DESC_RATEVHT2SS_MCS1, DESC_RATEVHT2SS_MCS2,
+						DESC_RATEVHT2SS_MCS3, DESC_RATEVHT2SS_MCS4,
+						  DESC_RATEVHT2SS_MCS5, DESC_RATEVHT2SS_MCS6, DESC_RATEVHT2SS_MCS7,
+						  DESC_RATEVHT2SS_MCS8, DESC_RATEVHT2SS_MCS9};
+	//u8 i,size;
+	//u8 power_index;
+
+	if (rtlhal->current_bandtype == BAND_ON_2_4G)
+		_rtl8821ae_phy_set_txpower_level_by_path(hw,cck_rates,path,channel,
+		                                         sizeof(cck_rates) / sizeof(u8));
+
+	_rtl8821ae_phy_set_txpower_level_by_path(hw,ofdm_rates,path,channel,
+		                                     sizeof(ofdm_rates) / sizeof(u8));
+	_rtl8821ae_phy_set_txpower_level_by_path(hw,ht_rates_1t,path,channel,
+		                                     sizeof(ht_rates_1t) / sizeof(u8));
+	_rtl8821ae_phy_set_txpower_level_by_path(hw,vht_rates_1t,path,channel,
+		                                     sizeof(vht_rates_1t) / sizeof(u8));
+
+	if (rtlphy->num_total_rfpath >= 2) {
+		_rtl8821ae_phy_set_txpower_level_by_path(hw,ht_rates_2t,path,channel,
+			                                     sizeof(ht_rates_2t) / sizeof(u8));
+		_rtl8821ae_phy_set_txpower_level_by_path(hw,vht_rates_2t,path,channel,
+			                                     sizeof(vht_rates_2t) / sizeof(u8));
+	}
+
+	_rtl8821ae_phy_txpower_training_by_path(hw, rtlphy->current_chan_bw, channel, path);
+}
+/*just in case, write txpower in DW, to reduce time*/
+#if 0
+void _rtl8821ae_phy_get_txpower_index_by_rate_array(struct ieee80211_hw *hw, u8 channel,
+											u8 *rate, u8 path, u8 bw, u8 *power_index, u8 size)
+{
+	u8 i;
+	for (i = 0; i < size; i++)
+		power_index[i] = _rtl8821ae_get_txpower_index(hw, path, rate[i], bw, channel);
+}
+
+void rtl8821ae_phy_set_txpower_level_by_path2(struct ieee80211_hw *hw, u8 channel, u8 path)
+{
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtl_priv(hw)->phy);
+	u8 cck_rates[]  = {DESC_RATE1M, DESC_RATE2M, DESC_RATE5_5M, DESC_RATE11M};
+	u8 ofdm_rates[]  = {DESC_RATE6M, DESC_RATE9M, DESC_RATE12M, DESC_RATE18M,
+						DESC_RATE24M, DESC_RATE36M, DESC_RATE48M, DESC_RATE54M};
+	u8 ht_rates_1t[]  = {DESC_RATEMCS0, DESC_RATEMCS1, DESC_RATEMCS2, DESC_RATEMCS3,
+						 DESC_RATEMCS4, DESC_RATEMCS5, DESC_RATEMCS6, DESC_RATEMCS7};
+	u8 ht_rates_2t[]  = {DESC_RATEMCS8, DESC_RATEMCS9, DESC_RATEMCS10, DESC_RATEMCS11,
+						 DESC_RATEMCS12, DESC_RATEMCS13, DESC_RATEMCS14, DESC_RATEMCS15};
+	u8 vht_rates_1t[]  = {DESC_RATEVHT1SS_MCS0, DESC_RATEVHT1SS_MCS1, DESC_RATEVHT1SS_MCS2, DESC_RATEVHT1SS_MCS3, DESC_RATEVHT1SS_MCS4,
+						  DESC_RATEVHT1SS_MCS5, DESC_RATEVHT1SS_MCS6, DESC_RATEVHT1SS_MCS7, DESC_RATEVHT1SS_MCS8, DESC_RATEVHT1SS_MCS9};
+	u8 vht_rates_2t[]  = {DESC_RATEVHT2SS_MCS0, DESC_RATEVHT2SS_MCS1, DESC_RATEVHT2SS_MCS2, DESC_RATEVHT2SS_MCS3, DESC_RATEVHT2SS_MCS4,
+						  DESC_RATEVHT2SS_MCS5, DESC_RATEVHT2SS_MCS6, DESC_RATEVHT2SS_MCS7, DESC_RATEVHT2SS_MCS8, DESC_RATEVHT2SS_MCS9};
+	u8 i, j;
+	u8 pwridx[48] = {0};
+	u8 cs = sizeof(cck_rates) / sizeof(u8);
+	u8 os = sizeof(ofdm_rates) / sizeof(u8);
+	u8 h1s = sizeof(ht_rates_1t) / sizeof(u8);
+	u8 h2s = sizeof(ht_rates_2t) / sizeof(u8);
+	u8 v1s = sizeof(vht_rates_1t) / sizeof(u8);
+	u8 v2s = sizeof(vht_rates_2t) / sizeof(u8);
+
+	u8 len, start;
+	u32 reg_addr, power_index;
+	u8 bw = rtlphy->current_chan_bw;
+
+	_rtl8821ae_phy_get_txpower_index_by_rate_array(hw, channel,
+		ofdm_rates, path, bw, &pwridx[cs], os);
+
+	_rtl8821ae_phy_get_txpower_index_by_rate_array(hw, channel,
+		ht_rates_1t, path, bw, &pwridx[cs+os], h1s);
+
+	_rtl8821ae_phy_get_txpower_index_by_rate_array(hw, channel,
+		vht_rates_1t, path, bw, &pwridx[cs+os+h1s+h2s], v1s);
+
+
+	if (rtlhal->current_bandtype == BAND_ON_2_4G) {
+		_rtl8821ae_phy_get_txpower_index_by_rate_array(hw, channel,
+			cck_rates, path, bw, pwridx, cs);
+
+		start = 0;
+	} else {
+		start = cs;
+	}
+
+	reg_addr = (path == 0) ? RTXAGC_A_CCK11_CCK1 : RTXAGC_B_CCK11_CCK1;
+	reg_addr += start;
+
+	len = cs + os + h1s + h2s + v1s;
+	if (rtlphy->num_total_rfpath >= 2) {
+		_rtl8821ae_phy_get_txpower_index_by_rate_array(hw, channel,
+			ht_rates_2t, path, bw, &pwridx[cs+os+h1s], h2s);
+
+		_rtl8821ae_phy_get_txpower_index_by_rate_array(hw, channel,
+			vht_rates_2t, path, bw, &pwridx[cs+os+h1s+h2s+v1s], v2s);
+
+		len += v2s;
+	}
+	for (i = start; i < len; i += 4) {
+		power_index = 0;
+		for (j = 0; j < 4; j++)
+			power_index |= (pwridx[i+j] << (j*8));
+		rtl_set_bbreg(hw, reg_addr + i, MASKDWORD, power_index);
+	}
+
+	_rtl8821ae_phy_txpower_training_by_path(hw, rtlphy->current_chan_bw, channel, path);
+}
+#endif
+
+void rtl8821ae_phy_set_txpower_level(struct ieee80211_hw *hw, u8 channel)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 path = 0;
+
+	for (path = RF90_PATH_A; path < rtlphy->num_total_rfpath; ++path )
+		rtl8821ae_phy_set_txpower_level_by_path(hw, channel, path);
+}
+
+static long _rtl8821ae_phy_txpwr_idx_to_dbm(struct ieee80211_hw *hw,
+					 enum wireless_mode wirelessmode,
+					 u8 txpwridx)
+{
+	long offset;
+	long pwrout_dbm;
+
+	switch (wirelessmode) {
+	case WIRELESS_MODE_B:
+		offset = -7;
+		break;
+	case WIRELESS_MODE_G:
+	case WIRELESS_MODE_N_24G:
+		offset = -8;
+		break;
+	default:
+		offset = -8;
+		break;
+	}
+	pwrout_dbm = txpwridx / 2 + offset;
+	return pwrout_dbm;
+}
+
+void rtl8821ae_phy_scan_operation_backup(struct ieee80211_hw *hw, u8 operation)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	enum io_type iotype = IO_CMD_PAUSE_BAND0_DM_BY_SCAN;
+
+	if (!is_hal_stop(rtlhal)) {
+		switch (operation) {
+		case SCAN_OPT_BACKUP_BAND0:
+			iotype = IO_CMD_PAUSE_BAND0_DM_BY_SCAN;
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						      HW_VAR_IO_CMD,
+						      (u8 *) & iotype);
+
+			break;
+		case SCAN_OPT_BACKUP_BAND1:
+			iotype = IO_CMD_PAUSE_BAND1_DM_BY_SCAN;
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						      HW_VAR_IO_CMD,
+						      (u8 *) & iotype);
+
+			break;
+		case SCAN_OPT_RESTORE:
+			iotype = IO_CMD_RESUME_DM_BY_SCAN;
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						      HW_VAR_IO_CMD,
+						      (u8 *) & iotype);
+			break;
+		default:
+			RT_TRACE(COMP_ERR, DBG_EMERG,
+				 ("Unknown Scan Backup operation.\n"));
+			break;
+		}
+	}
+}
+
+static void _rtl8821ae_phy_set_reg_bw(struct rtl_priv * rtlpriv, u8 bw)
+{
+	u16 reg_rf_mode_bw, tmp = 0;
+	reg_rf_mode_bw = rtl_read_word(rtlpriv, REG_TRXPTCL_CTL);
+	switch (bw) {
+	case HT_CHANNEL_WIDTH_20:
+		rtl_write_word(rtlpriv, REG_TRXPTCL_CTL, reg_rf_mode_bw & 0xFE7F);
+		break;
+	case HT_CHANNEL_WIDTH_20_40:
+		tmp = reg_rf_mode_bw | BIT(7);
+		rtl_write_word(rtlpriv, REG_TRXPTCL_CTL, tmp & 0xFEFF);
+		break;
+	case HT_CHANNEL_WIDTH_80:
+		tmp = reg_rf_mode_bw | BIT(8);
+		rtl_write_word(rtlpriv, REG_TRXPTCL_CTL, tmp & 0xFF7F);
+		break;
+	default:
+		RT_TRACE(COMP_ERR, DBG_WARNING,("unknown Bandwidth: 0x%x\n",bw));
+		break;
+	}
+}
+
+static u8 _rtl8821ae_phy_get_secondary_chnl(struct rtl_priv * rtlpriv)
+{
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_mac *mac = rtl_mac(rtlpriv);
+	u8 sc_set_40 = 0, sc_set_20 =0;
+
+	if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80) {
+		if(mac->cur_80_prime_sc == PRIME_CHNL_OFFSET_LOWER)
+			sc_set_40 = VHT_DATA_SC_40_LOWER_OF_80MHZ;
+		else if(mac->cur_80_prime_sc == PRIME_CHNL_OFFSET_UPPER)
+			sc_set_40 = VHT_DATA_SC_40_UPPER_OF_80MHZ;
+		else
+			RT_TRACE(COMP_ERR, DBG_EMERG,
+				("SCMapping: Not Correct Primary40MHz Setting \n"));
+
+		if((mac->cur_40_prime_sc == PRIME_CHNL_OFFSET_LOWER) &&
+			(mac->cur_80_prime_sc == HAL_PRIME_CHNL_OFFSET_LOWER))
+			sc_set_20 = VHT_DATA_SC_20_LOWEST_OF_80MHZ;
+		else if((mac->cur_40_prime_sc == PRIME_CHNL_OFFSET_UPPER) &&
+			(mac->cur_80_prime_sc == HAL_PRIME_CHNL_OFFSET_LOWER))
+			sc_set_20 = VHT_DATA_SC_20_LOWER_OF_80MHZ;
+		else if((mac->cur_40_prime_sc == PRIME_CHNL_OFFSET_LOWER) &&
+			(mac->cur_80_prime_sc == HAL_PRIME_CHNL_OFFSET_UPPER))
+			sc_set_20 = VHT_DATA_SC_20_UPPER_OF_80MHZ;
+		else if((mac->cur_40_prime_sc == PRIME_CHNL_OFFSET_UPPER) &&
+			(mac->cur_80_prime_sc == HAL_PRIME_CHNL_OFFSET_UPPER))
+			sc_set_20 = VHT_DATA_SC_20_UPPERST_OF_80MHZ;
+		else
+			RT_TRACE(COMP_ERR, DBG_EMERG,
+				("SCMapping: Not Correct Primary40MHz Setting \n"));
+	} else if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40) {
+		if (mac->cur_40_prime_sc == PRIME_CHNL_OFFSET_UPPER)
+			sc_set_20 = VHT_DATA_SC_20_UPPER_OF_80MHZ;
+		else if (mac->cur_40_prime_sc == PRIME_CHNL_OFFSET_LOWER)
+			sc_set_20 = VHT_DATA_SC_20_LOWER_OF_80MHZ;
+		else
+			RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("SCMapping: Not Correct Primary40MHz Setting \n"));
+	}
+	return ((sc_set_40 << 4) | sc_set_20);
+}
+
+void rtl8821ae_phy_set_bw_mode_callback(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 sub_chnl = 0;
+	u8 l1pk_val = 0;
+
+	RT_TRACE(COMP_SCAN, DBG_TRACE,
+		 ("Switch to %s bandwidth\n",
+		  (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20 ?
+		  "20MHz" :
+		  (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40 ?
+		  "40MHz" : "80MHz"))))
+
+
+
+	_rtl8821ae_phy_set_reg_bw(rtlpriv, rtlphy->current_chan_bw);
+	sub_chnl = _rtl8821ae_phy_get_secondary_chnl(rtlpriv);
+	rtl_write_byte(rtlpriv, 0x0483, sub_chnl);
+
+	switch (rtlphy->current_chan_bw) {
+	case HT_CHANNEL_WIDTH_20:
+		rtl_set_bbreg(hw, RRFMOD, 0x003003C3, 0x00300200);
+		rtl_set_bbreg(hw, RADC_BUF_CLK, BIT(30), 0);
+
+		if(rtlphy->rf_type == RF_2T2R)
+			rtl_set_bbreg(hw, RL1PEAKTH, 0x03C00000, 7);
+		else
+			rtl_set_bbreg(hw, RL1PEAKTH, 0x03C00000, 8);
+		break;
+	case HT_CHANNEL_WIDTH_20_40:
+		rtl_set_bbreg(hw, RRFMOD, 0x003003C3, 0x00300201);
+		rtl_set_bbreg(hw, RADC_BUF_CLK, BIT(30), 0);
+		rtl_set_bbreg(hw, RRFMOD, 0x3C, sub_chnl);
+		rtl_set_bbreg(hw, RCCAONSEC, 0xf0000000, sub_chnl);
+
+		if(rtlphy->reg_837 & BIT(2))
+			l1pk_val = 6;
+		else
+		{
+			if(rtlphy->rf_type == RF_2T2R)
+				l1pk_val = 7;
+			else
+				l1pk_val = 8;
+		}
+		rtl_set_bbreg(hw, RL1PEAKTH, 0x03C00000, l1pk_val);	// 0x848[25:22] = 0x6
+
+		if(sub_chnl == VHT_DATA_SC_20_UPPER_OF_80MHZ)
+			rtl_set_bbreg(hw, RCCK_SYSTEM, BCCK_SYSTEM, 1);
+		else
+			rtl_set_bbreg(hw, RCCK_SYSTEM, BCCK_SYSTEM, 0);
+		break;
+
+	case HT_CHANNEL_WIDTH_80:
+		rtl_set_bbreg(hw, RRFMOD, 0x003003C3, 0x00300202); // 0x8ac[21,20,9:6,1,0]=8'b11100010
+		rtl_set_bbreg(hw, RADC_BUF_CLK, BIT(30), 1);			// 0x8c4[30] = 1
+		rtl_set_bbreg(hw, RRFMOD, 0x3C, sub_chnl);
+		rtl_set_bbreg(hw, RCCAONSEC, 0xf0000000, sub_chnl);
+
+		if(rtlphy->reg_837 & BIT(2))
+			l1pk_val = 5;
+		else
+		{
+			if(rtlphy->rf_type == RF_2T2R)
+				l1pk_val = 6;
+			else
+				l1pk_val = 7;
+		}
+		rtl_set_bbreg(hw, RL1PEAKTH, 0x03C00000, l1pk_val);
+
+		break;
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("unknown bandwidth: %#X\n", rtlphy->current_chan_bw));
+		break;
+	}
+
+	rtl8812ae_fixspur(hw, rtlphy->current_chan_bw, rtlphy->current_channel);
+
+	rtl8821ae_phy_rf6052_set_bandwidth(hw, rtlphy->current_chan_bw);
+	rtlphy->set_bwmode_inprogress = false;
+
+	RT_TRACE(COMP_SCAN, DBG_LOUD, (" \n"));
+}
+
+void rtl8821ae_phy_set_bw_mode(struct ieee80211_hw *hw,
+			    enum nl80211_channel_type ch_type)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 tmp_bw = rtlphy->current_chan_bw;
+
+	if (rtlphy->set_bwmode_inprogress)
+		return;
+	rtlphy->set_bwmode_inprogress = true;
+	if ((!is_hal_stop(rtlhal)) && !(RT_CANNOT_IO(hw))) {
+		rtl8821ae_phy_set_bw_mode_callback(hw);
+	} else {
+		RT_TRACE(COMP_ERR, DBG_WARNING,
+			 ("FALSE driver sleep or unload\n"));
+		rtlphy->set_bwmode_inprogress = false;
+		rtlphy->current_chan_bw = tmp_bw;
+	}
+}
+
+void rtl8821ae_phy_sw_chnl_callback(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 channel = rtlphy->current_channel;
+	u8 path;
+	u32 data;
+
+	RT_TRACE(COMP_SCAN, DBG_TRACE,
+		 ("switch to channel%d\n", rtlphy->current_channel));
+	if (is_hal_stop(rtlhal))
+		return;
+
+	if (36 <= channel && channel <= 48)
+		data = 0x494;
+	else if (50 <= channel && channel <= 64)
+		data = 0x453;
+	else if (100 <= channel && channel <= 116)
+		data = 0x452;
+	else if (118 <= channel)
+		data = 0x412;
+	else
+		data = 0x96a;
+	rtl_set_bbreg(hw, RFC_AREA, 0x1ffe0000, data);
+
+
+	for(path = RF90_PATH_A; path < rtlphy->num_total_rfpath; path++)
+	{
+		if (36 <= channel && channel <= 64)
+			data = 0x101;
+		else if (100 <= channel && channel <= 140)
+			data = 0x301;
+		else if (140 < channel)
+			data = 0x501;
+		else
+			data = 0x000;
+		rtl8821ae_phy_set_rf_reg(hw, path, RF_CHNLBW,
+			BIT(18)|BIT(17)|BIT(16)|BIT(9)|BIT(8), data);
+
+		rtl8821ae_phy_set_rf_reg(hw, path, RF_CHNLBW,
+			BMASKBYTE0, channel);
+
+		if (channel > 14) {
+			if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+				if (36 <= channel && channel <= 64)
+					data = 0x114E9;
+				else if (100 <= channel && channel <= 140)
+					data = 0x110E9;
+				else
+					data = 0x110E9;
+				rtl8821ae_phy_set_rf_reg(hw, path, RF_APK,
+					BRFREGOFFSETMASK, data);
+			}
+		}
+	}
+	RT_TRACE(COMP_SCAN, DBG_TRACE, ("\n"));
+}
+
+u8 rtl8821ae_phy_sw_chnl(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u32 timeout = 1000, timecount = 0;
+	u8 channel = rtlphy->current_channel;
+
+	if (rtlphy->sw_chnl_inprogress)
+		return 0;
+	if (rtlphy->set_bwmode_inprogress)
+		return 0;
+
+	if ((is_hal_stop(rtlhal)) || (RT_CANNOT_IO(hw))) {
+		RT_TRACE(COMP_CHAN, DBG_LOUD,
+			 ("sw_chnl_inprogress false driver sleep or unload\n"));
+		return 0;
+	}
+	while (rtlphy->lck_inprogress && timecount < timeout) {
+		mdelay(50);
+		timecount += 50;
+	}
+
+	if (rtlphy->current_channel > 14 && rtlhal->current_bandtype != BAND_ON_5G)
+		rtl8821ae_phy_switch_wirelessband(hw, BAND_ON_5G);
+	else if (rtlphy->current_channel <= 14 && rtlhal->current_bandtype != BAND_ON_2_4G)
+		rtl8821ae_phy_switch_wirelessband(hw, BAND_ON_2_4G);
+
+	rtlphy->sw_chnl_inprogress = true;
+	if (channel == 0)
+		channel = 1;
+
+	RT_TRACE(COMP_SCAN, DBG_TRACE,
+		 ("switch to channel%d, band type is %d\n", rtlphy->current_channel, rtlhal->current_bandtype));
+
+	rtl8821ae_phy_sw_chnl_callback(hw);
+
+	rtl8821ae_dm_clear_txpower_tracking_state(hw);
+	rtl8821ae_phy_set_txpower_level(hw, rtlphy->current_channel);
+
+	RT_TRACE(COMP_SCAN, DBG_TRACE, ("\n"));
+	rtlphy->sw_chnl_inprogress = false;
+	return 1;
+}
+
+#if 0
+static u8 _rtl8821ae_phy_path_b_iqk(struct ieee80211_hw *hw)
+{
+	u32 reg_eac, reg_eb4, reg_ebc, reg_ec4, reg_ecc;
+	u8 result = 0x00;
+
+	rtl_set_bbreg(hw, 0xe60, MASKDWORD, 0x00000002);
+	rtl_set_bbreg(hw, 0xe60, MASKDWORD, 0x00000000);
+	mdelay(IQK_DELAY_TIME);
+	reg_eac = rtl_get_bbreg(hw, 0xeac, MASKDWORD);
+	reg_eb4 = rtl_get_bbreg(hw, 0xeb4, MASKDWORD);
+	reg_ebc = rtl_get_bbreg(hw, 0xebc, MASKDWORD);
+	reg_ec4 = rtl_get_bbreg(hw, 0xec4, MASKDWORD);
+	reg_ecc = rtl_get_bbreg(hw, 0xecc, MASKDWORD);
+
+	if (!(reg_eac & BIT(31)) &&
+	    (((reg_eb4 & 0x03FF0000) >> 16) != 0x142) &&
+	    (((reg_ebc & 0x03FF0000) >> 16) != 0x42))
+		result |= 0x01;
+	else
+		return result;
+	if (!(reg_eac & BIT(30)) &&
+	    (((reg_ec4 & 0x03FF0000) >> 16) != 0x132) &&
+	    (((reg_ecc & 0x03FF0000) >> 16) != 0x36))
+		result |= 0x02;
+	return result;
+}
+
+static u8 _rtl8821ae_phy_path_a_rx_iqk(struct ieee80211_hw *hw, bool config_pathb)
+{
+	u32 reg_eac, reg_e94, reg_e9c, reg_ea4,u32temp;
+	u8 result = 0x00;
+
+	/*Get TXIMR Setting*/
+	/*Modify RX IQK mode table*/
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_WE_LUT, RFREG_OFFSET_MASK, 0x800a0);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_RCK_OS, RFREG_OFFSET_MASK, 0x30000);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0000f);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xf117b);
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
+
+	/*IQK Setting*/
+	rtl_set_bbreg(hw, RTx_IQK, MASKDWORD, 0x01007c00);
+	rtl_set_bbreg(hw, RRx_IQK, MASKDWORD, 0x81004800);
+
+	/*path a IQK setting*/
+	rtl_set_bbreg(hw, RTx_IQK_Tone_A, MASKDWORD, 0x10008c1c);
+	rtl_set_bbreg(hw, RRx_IQK_Tone_A, MASKDWORD, 0x30008c1c);
+	rtl_set_bbreg(hw, RTx_IQK_PI_A, MASKDWORD, 0x82160804);
+	rtl_set_bbreg(hw, RRx_IQK_PI_A, MASKDWORD, 0x28160000);
+
+	/*LO calibration Setting*/
+	rtl_set_bbreg(hw, RIQK_AGC_Rsp, MASKDWORD, 0x0046a911);
+	/*one shot,path A LOK & iqk*/
+	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xf9000000);
+	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xf8000000);
+
+	mdelay(IQK_DELAY_TIME);
+
+	reg_eac = rtl_get_bbreg(hw, RRx_Power_After_IQK_A_2, MASKDWORD);
+	reg_e94 = rtl_get_bbreg(hw, RTx_Power_Before_IQK_A, MASKDWORD);
+	reg_e9c = rtl_get_bbreg(hw, RTx_Power_After_IQK_A, MASKDWORD);
+
+
+	if (!(reg_eac & BIT(28)) &&
+	    (((reg_e94 & 0x03FF0000) >> 16) != 0x142) &&
+	    (((reg_e9c & 0x03FF0000) >> 16) != 0x42))
+		result |= 0x01;
+	else
+		return result;
+
+	u32temp = 0x80007C00 | (reg_e94&0x3FF0000)  | ((reg_e9c&0x3FF0000) >> 16);
+	rtl_set_bbreg(hw, RTx_IQK, MASKDWORD, u32temp);
+	/*RX IQK*/
+	/*Modify RX IQK mode table*/
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_WE_LUT, RFREG_OFFSET_MASK, 0x800a0);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_RCK_OS, RFREG_OFFSET_MASK, 0x30000);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0000f);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xf7ffa);
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
+
+	/*IQK Setting*/
+	rtl_set_bbreg(hw, RRx_IQK, MASKDWORD, 0x01004800);
+
+	/*path a IQK setting*/
+	rtl_set_bbreg(hw, RTx_IQK_Tone_A, MASKDWORD, 0x30008c1c);
+	rtl_set_bbreg(hw, RRx_IQK_Tone_A, MASKDWORD, 0x10008c1c);
+	rtl_set_bbreg(hw, RTx_IQK_PI_A, MASKDWORD, 0x82160c05);
+	rtl_set_bbreg(hw, RRx_IQK_PI_A, MASKDWORD, 0x28160c05);
+
+	/*LO calibration Setting*/
+	rtl_set_bbreg(hw, RIQK_AGC_Rsp, MASKDWORD, 0x0046a911);
+	/*one shot,path A LOK & iqk*/
+	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xf9000000);
+	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xf8000000);
+
+	mdelay(IQK_DELAY_TIME);
+
+	reg_eac = rtl_get_bbreg(hw, RRx_Power_After_IQK_A_2, MASKDWORD);
+	reg_e94 = rtl_get_bbreg(hw, RTx_Power_Before_IQK_A, MASKDWORD);
+	reg_e9c = rtl_get_bbreg(hw, RTx_Power_After_IQK_A, MASKDWORD);
+	reg_ea4 = rtl_get_bbreg(hw, RRx_Power_Before_IQK_A_2, MASKDWORD);
+
+	if (!(reg_eac & BIT(27)) &&
+	    (((reg_ea4 & 0x03FF0000) >> 16) != 0x132) &&
+	    (((reg_eac & 0x03FF0000) >> 16) != 0x36))
+		result |= 0x02;
+	return result;
+}
+#endif
+
+u8 _rtl8812ae_get_right_chnl_place_for_iqk(u8 chnl)
+{
+	u8 channel_all[TARGET_CHNL_NUM_2G_5G_8812] =
+		{1,2,3,4,5,6,7,8,9,10,11,12,13,14,36,38,40,42,\
+		44,46,48,50,52,54,56,58,60,62,64,100,\
+		102,104,106,108,110,112,114,116,118,\
+		120,122,124,126,128,130,132,134,136,\
+		138,140,149,151,153,155,157,159,161,\
+		163,165};
+	u8 place = chnl;
+
+	if(chnl > 14)
+	{
+		for(place = 14; place<sizeof(channel_all); place++)
+		{
+			if(channel_all[place] == chnl)
+			{
+				return place-13;
+			}
+		}
+	}
+
+	return 0;
+}
+
+void _rtl8812ae_iqk_rx_fill_iqc(
+	struct ieee80211_hw *hw,
+	enum radio_path path,
+	u32 rx_x,
+	u32 rx_y
+	)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	switch (path) {
+	case RF90_PATH_A:
+		{
+			rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
+			if (rx_x >> 1 ==0x112 || rx_y >> 1 == 0x3ee){
+				rtl_set_bbreg(hw, 0xc10, 0x000003ff, 0x100);
+				rtl_set_bbreg(hw, 0xc10, 0x03ff0000, 0);
+				RT_TRACE(COMP_IQK, DBG_LOUD,
+					("RX_X = %x;;RX_Y = %x ====>fill to IQC\n",
+					rx_x >> 1 & 0x000003ff, rx_y >> 1 & 0x000003ff));
+			}
+			else{
+				rtl_set_bbreg(hw, 0xc10, 0x000003ff, rx_x >> 1);
+				rtl_set_bbreg(hw, 0xc10, 0x03ff0000, rx_y >> 1);
+				RT_TRACE(COMP_IQK, DBG_LOUD,
+					("RX_X = %x;;RX_Y = %x ====>fill to IQC\n",
+					rx_x >> 1 & 0x000003ff, rx_y >> 1 & 0x000003ff));
+				RT_TRACE(COMP_IQK, DBG_LOUD,
+					("0xc10 = %x ====>fill to IQC\n",
+					rtl_read_dword(rtlpriv, 0xc10)));
+			}
+		}
+		break;
+	case RF90_PATH_B:
+		{
+			rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
+			if (rx_x >> 1 ==0x112 || rx_y >> 1 == 0x3ee){
+				rtl_set_bbreg(hw, 0xe10, 0x000003ff, 0x100);
+				rtl_set_bbreg(hw, 0xe10, 0x03ff0000, 0);
+				RT_TRACE(COMP_IQK, DBG_LOUD,
+					("RX_X = %x;;RX_Y = %x ====>fill to IQC\n",
+					rx_x >> 1 & 0x000003ff, rx_y >> 1 & 0x000003ff));
+			}
+			else{
+				rtl_set_bbreg(hw, 0xe10, 0x000003ff, rx_x >> 1);
+				rtl_set_bbreg(hw, 0xe10, 0x03ff0000, rx_y >> 1);
+				RT_TRACE(COMP_IQK, DBG_LOUD,
+					("RX_X = %x;;RX_Y = %x====>fill to IQC\n ",
+					rx_x >> 1 & 0x000003ff, rx_y >> 1 & 0x000003ff));
+				RT_TRACE(COMP_IQK, DBG_LOUD,
+					("0xe10 = %x====>fill to IQC\n",
+					rtl_read_dword(rtlpriv, 0xe10)));
+			}
+		}
+		break;
+	default:
+		break;
+	};
+}
+
+void _rtl8812ae_iqk_tx_fill_iqc(
+	struct ieee80211_hw *hw,
+	enum radio_path path,
+	u32 tx_x,
+	u32 tx_y
+	)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	switch (path) {
+	case RF90_PATH_A:
+		{
+			rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /*[31] = 1 --> Page C1*/
+			rtl_write_dword(rtlpriv, 0xc90, 0x00000080);
+			rtl_write_dword(rtlpriv, 0xcc4, 0x20040000);
+			rtl_write_dword(rtlpriv, 0xcc8, 0x20000000);
+			rtl_set_bbreg(hw, 0xccc, 0x000007ff, tx_y);
+			rtl_set_bbreg(hw, 0xcd4, 0x000007ff, tx_x);
+			RT_TRACE(COMP_IQK, DBG_LOUD,
+				("TX_X = %x;;TX_Y = %x =====> fill to IQC\n",
+				tx_x & 0x000007ff, tx_y & 0x000007ff));
+			RT_TRACE(COMP_IQK, DBG_LOUD,
+				("0xcd4 = %x;;0xccc = %x ====>fill to IQC\n",
+				rtl_get_bbreg(hw, 0xcd4, 0x000007ff),
+				rtl_get_bbreg(hw, 0xccc, 0x000007ff)));
+		}
+		break;
+	case RF90_PATH_B:
+		{
+			rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /*[31] = 1 --> Page C1*/
+			rtl_write_dword(rtlpriv, 0xe90, 0x00000080);
+			rtl_write_dword(rtlpriv, 0xec4, 0x20040000);
+			rtl_write_dword(rtlpriv, 0xec8, 0x20000000);
+			rtl_set_bbreg(hw, 0xecc, 0x000007ff, tx_y);
+			rtl_set_bbreg(hw, 0xed4, 0x000007ff, tx_x);
+			RT_TRACE(COMP_IQK, DBG_LOUD,
+				("TX_X = %x;;TX_Y = %x =====> fill to IQC\n",
+				tx_x&0x000007ff, tx_y&0x000007ff));
+			RT_TRACE(COMP_IQK, DBG_LOUD,
+				("0xed4 = %x;;0xecc = %x ====>fill to IQC\n",
+				rtl_get_bbreg(hw, 0xed4, 0x000007ff),
+				rtl_get_bbreg(hw, 0xecc, 0x000007ff)));
+		}
+		break;
+	default:
+		break;
+	};
+}
+
+void _rtl8812ae_iqk_backup_macbb(
+	struct ieee80211_hw *hw,
+	u32 *macbb_backup,
+	u32 *backup_macbb_reg,
+	u32 mac_bb_num
+	)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i;
+
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
+	 /*save MACBB default value*/
+	for (i = 0; i < mac_bb_num; i++) {
+		macbb_backup[i] =rtl_read_dword(rtlpriv,backup_macbb_reg[i]);
+	}
+
+	RT_TRACE(COMP_IQK, DBG_LOUD, ("BackupMacBB Success!!!!\n"));
+}
+
+void _rtl8812ae_iqk_backup_afe(
+	struct ieee80211_hw *hw,
+	u32 *afe_backup,
+	u32 *backup_afe_REG,
+	u32 afe_num
+	)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i;
+
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
+	/*Save AFE Parameters */
+    	for (i = 0; i < afe_num; i++){
+        	afe_backup[i] = rtl_read_dword(rtlpriv, backup_afe_REG[i]);
+    	}
+    	RT_TRACE(COMP_IQK, DBG_LOUD, ("BackupAFE Success!!!!\n"));
+}
+
+void _rtl8812ae_iqk_backup_rf(
+	struct ieee80211_hw *hw,
+	u32 *rfa_backup,
+	u32 *rfb_backup,
+	u32 *backup_rf_reg,
+	u32 rf_num
+	)
+{
+
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i;
+
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
+	/*Save RF Parameters*/
+    	for (i = 0; i < rf_num; i++){
+        	rfa_backup[i] = rtl_get_rfreg(hw, RF90_PATH_A, backup_rf_reg[i], BMASKDWORD);
+        	rfb_backup[i] = rtl_get_rfreg(hw, RF90_PATH_B, backup_rf_reg[i], BMASKDWORD);
+    	}
+	RT_TRACE(COMP_IQK, DBG_LOUD, ("BackupRF Success!!!!\n"));
+}
+
+void _rtl8812ae_iqk_configure_mac(
+	struct ieee80211_hw *hw
+	)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	/* ========MAC register setting========*/
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
+	rtl_write_byte(rtlpriv, 0x522, 0x3f);
+	rtl_set_bbreg(hw, 0x550, BIT(11) | BIT(3), 0x0);
+	rtl_write_byte(rtlpriv, 0x808, 0x00);		/*RX ante off*/
+	rtl_set_bbreg(hw, 0x838, 0xf, 0xc);		/*CCA off*/
+}
+
+#define cal_num 10
+
+void _rtl8812ae_iqk_tx(
+	struct ieee80211_hw *hw,
+	u8 chnl_idx
+	)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	u8 delay_count;
+	u8 cal0_retry, cal1_retry;
+	u8 tx0_average = 0, tx1_average = 0, rx0_average = 0, rx1_average = 0;
+    	int tx0_x = 0, tx0_y = 0, rx0_x = 0, rx0_y = 0;
+	int tx_x0[cal_num], tx_y0[cal_num], rx_x0[cal_num], rx_y0[cal_num];
+	int tx1_x = 0, tx1_y = 0, rx1_x = 0, rx1_y = 0;
+	int tx_x1[cal_num], tx_y1[cal_num], rx_x1[cal_num], rx_y1[cal_num];
+	bool tx0iqkok= false, rx0iqkok = false, tx0_fail = true, rx0_fail;
+	bool iqk0_ready = false, tx0_finish = false, rx0_finish = false;
+	bool tx1iqkok = false, rx1iqkok = false, tx1_fail = true, rx1_fail;
+	bool iqk1_ready = false, tx1_finish = false, rx1_finish = false, vdf_enable = false;
+	int i, tx_dt[3] = {0}, rx_dt[3] = {0}, ii, dx = 0, dy = 0;
+
+	RT_TRACE(COMP_IQK, DBG_LOUD,
+			("BandWidth = %d.\n",
+			rtlphy->current_chan_bw));
+	if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80){
+		vdf_enable = true;
+	}
+	vdf_enable = false;
+
+
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
+	/*========Path-A AFE all on========*/
+	/*Port 0 DAC/ADC on*/
+	rtl_write_dword(rtlpriv, 0xc60, 0x77777777);
+	rtl_write_dword(rtlpriv, 0xc64, 0x77777777);
+
+	/* Port 1 DAC/ADC off*/
+	rtl_write_dword(rtlpriv, 0xe60, 0x77777777);
+	rtl_write_dword(rtlpriv, 0xe64, 0x77777777);
+
+	rtl_write_dword(rtlpriv, 0xc68, 0x19791979);
+	rtl_write_dword(rtlpriv, 0xe68, 0x19791979);
+	rtl_set_bbreg(hw,0xc00, 0xf, 0x4);/*hardware 3-wire off*/
+	rtl_set_bbreg(hw,0xe00, 0xf, 0x4);/*hardware 3-wire off*/
+
+	/*DAC/ADC sampling rate (160 MHz)*/
+	rtl_set_bbreg(hw, 0xc5c, BIT(26) | BIT(25) | BIT(24), 0x7);
+	rtl_set_bbreg(hw, 0xe5c, BIT(26) | BIT(25) | BIT(24), 0x7);
+	rtl_set_bbreg(hw, 0x8c4, BIT(30), 0x1);
+
+        /*====== Path A TX IQK RF Setting ======*/
+	rtl_set_bbreg(hw,0x82c, BIT(31), 0x0); // [31] = 0 --> Page C
+	rtl_set_rfreg(hw,RF90_PATH_A, 0xef, BRFREGOFFSETMASK, 0x80002);
+	rtl_set_rfreg(hw,RF90_PATH_A, 0x30, BRFREGOFFSETMASK, 0x20000);
+	rtl_set_rfreg(hw,RF90_PATH_A, 0x31, BRFREGOFFSETMASK, 0x3fffd);
+	rtl_set_rfreg(hw,RF90_PATH_A, 0x32, BRFREGOFFSETMASK, 0xfe83f);
+	rtl_set_rfreg(hw,RF90_PATH_A, 0x65, BRFREGOFFSETMASK, 0x931d5);
+	rtl_set_rfreg(hw,RF90_PATH_A, 0x8f, BRFREGOFFSETMASK, 0x8a001);
+	/*====== Path A TX IQK RF Setting ======*/
+	rtl_set_rfreg(hw,RF90_PATH_B, 0xef, BRFREGOFFSETMASK, 0x80002);
+	rtl_set_rfreg(hw,RF90_PATH_B, 0x30, BRFREGOFFSETMASK, 0x20000);
+	rtl_set_rfreg(hw,RF90_PATH_B, 0x31, BRFREGOFFSETMASK, 0x3fffd);
+	rtl_set_rfreg(hw,RF90_PATH_B, 0x32, BRFREGOFFSETMASK, 0xfe83f);
+	rtl_set_rfreg(hw,RF90_PATH_B, 0x65, BRFREGOFFSETMASK, 0x931d5);
+	rtl_set_rfreg(hw,RF90_PATH_B, 0x8f, BRFREGOFFSETMASK, 0x8a001);
+	rtl_write_dword(rtlpriv, 0x90c, 0x00008000);
+	rtl_write_dword(rtlpriv, 0xb00, 0x03000100);
+	rtl_set_bbreg(hw, 0xc94, BIT(0), 0x1);
+	rtl_set_bbreg(hw, 0xe94, BIT(0), 0x1);
+	rtl_write_dword(rtlpriv, 0x978, 0x29002000);/* TX (X,Y)*/
+	rtl_write_dword(rtlpriv, 0x97c, 0xa9002000);/* RX (X,Y)*/
+	rtl_write_dword(rtlpriv, 0x984, 0x00462910);/*[0]:AGC_en, [15]:idac_K_Mask*/
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /* [31] = 1 --> Page C1*/
+
+	/*ExternalPA_5G == 0*/
+	rtl_write_dword(rtlpriv, 0xc88, 0x821403f1);
+	rtl_write_dword(rtlpriv, 0xe88, 0x821403f1);
+
+	if (rtlhal->current_bandtype){
+		rtl_write_dword(rtlpriv, 0xc8c, 0x68163e96);
+		rtl_write_dword(rtlpriv, 0xe8c, 0x68163e96);
+	}
+	else{
+		rtl_write_dword(rtlpriv, 0xc8c, 0x28163e96);
+		rtl_write_dword(rtlpriv, 0xe8c, 0x28163e96);
+	}
+
+	if (vdf_enable){}
+	else{
+		rtl_write_dword(rtlpriv, 0xc80, 0x18008c10);/*TX_Tone_idx[9:0], TxK_Mask[29] TX_Tone = 16*/
+		rtl_write_dword(rtlpriv, 0xc84, 0x38008c10);/*RX_Tone_idx[9:0], RxK_Mask[29]*/
+		rtl_write_dword(rtlpriv, 0xce8, 0x00000000);
+		rtl_write_dword(rtlpriv, 0xe80, 0x18008c10);/*TX_Tone_idx[9:0], TxK_Mask[29] TX_Tone = 16*/
+		rtl_write_dword(rtlpriv, 0xe84, 0x38008c10);/*RX_Tone_idx[9:0], RxK_Mask[29]*/
+		rtl_write_dword(rtlpriv, 0xee8, 0x00000000);
+
+		cal0_retry = 0;
+		cal1_retry = 0;
+		while(1){
+			/*one shot*/
+			rtl_write_dword(rtlpriv, 0xcb8, 0x00100000);/* cb8[20] ±N SI/PI ¨Ï¥ÎÅv¤Áµ¹ iqk_dpk module*/
+			rtl_write_dword(rtlpriv, 0xeb8, 0x00100000);/* cb8[20] ±N SI/PI ¨Ï¥ÎÅv¤Áµ¹ iqk_dpk module*/
+			rtl_write_dword(rtlpriv, 0x980, 0xfa000000);
+			rtl_write_dword(rtlpriv, 0x980, 0xf8000000);
+
+			mdelay(10); /*Delay 25ms*/
+			rtl_write_dword(rtlpriv, 0xcb8, 0x00000000);
+			rtl_write_dword(rtlpriv, 0xeb8, 0x00000000);
+			delay_count = 0;
+			while (1){
+				if (!tx0_finish)
+					iqk0_ready = (bool) rtl_get_bbreg(hw, 0xd00, BIT(10));
+				if (!tx1_finish)
+					iqk1_ready = (bool) rtl_get_bbreg(hw, 0xd40, BIT(10));
+				if ((iqk0_ready && iqk1_ready) || (delay_count>20))
+					break;
+				else{
+				mdelay(1);
+				delay_count++;
+				}
+			}
+			RT_TRACE(COMP_IQK, DBG_LOUD, ("TX delay_count = %d\n", delay_count));
+			if (delay_count < 20){							// If 20ms No Result, then cal_retry++
+				/* ============TXIQK Check==============*/
+				tx0_fail = (bool) rtl_get_bbreg(hw, 0xd00, BIT(12));
+				tx1_fail = (bool) rtl_get_bbreg(hw, 0xd40, BIT(12));
+				if (!(tx0_fail || tx0_finish)){
+					rtl_write_dword(rtlpriv, 0xcb8, 0x02000000);
+					tx_x0[tx0_average] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000) << 21;
+					rtl_write_dword(rtlpriv, 0xcb8, 0x04000000);
+					tx_y0[tx0_average] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000) << 21;
+					tx0iqkok = true;
+					RT_TRACE(COMP_IQK, DBG_LOUD,
+						("TX_X0[%d] = %x ;; TX_Y0[%d] = %x\n",
+						tx0_average, (tx_x0[tx0_average]) >> 21 & 0x000007ff,
+						tx0_average, (tx_y0[tx0_average]) >> 21 & 0x000007ff));
+
+					tx0_average++;
+			}
+			else{
+				tx0iqkok = false;
+				cal0_retry++;
+				if (cal0_retry == 10)
+					break;
+				}
+			if (!(tx1_fail || tx1_finish)){
+				rtl_write_dword(rtlpriv, 0xeb8, 0x02000000);
+				tx_x1[tx1_average] = rtl_get_bbreg(hw, 0xd40, 0x07ff0000) << 21;
+				rtl_write_dword(rtlpriv, 0xeb8, 0x04000000);
+				tx_y1[tx1_average] = rtl_get_bbreg(hw, 0xd40, 0x07ff0000) << 21;
+				tx1iqkok= true;
+				RT_TRACE(COMP_IQK, DBG_LOUD,
+					("TX_X1[%d] = %x ;; TX_Y1[%d] = %x\n",
+					tx1_average, (tx_x1[tx1_average]) >> 21 & 0x000007ff,
+					tx1_average, (tx_y1[tx1_average]) >> 21 & 0x000007ff));
+
+				tx1_average++;
+				}
+			else{
+				tx1iqkok = false;
+				cal1_retry++;
+				if (cal1_retry == 10)
+					break;
+				}
+			}
+			else{
+				tx0iqkok = false;
+				tx1iqkok = false;
+				cal0_retry++;
+				cal1_retry++;
+				RT_TRACE(COMP_IQK, DBG_LOUD,
+					("Delay 20ms TX IQK Not Ready!!!!!\n"));
+				if (cal0_retry == 10)
+					break;
+			}
+			if (tx0_average >= 2){
+				for (i = 0; i < tx0_average; i++){
+					for (ii = i+1; ii <tx0_average; ii++){
+						dx = (tx_x0[i] >> 21) - (tx_x0[ii] >> 21);
+						if (dx < 4 && dx > -4){
+							dy = (tx_y0[i]>>21) - (tx_y0[ii]>>21);
+							if (dy < 4 && dy > -4){
+								tx0_x = ((tx_x0[i] >> 21) + (tx_x0[ii] >> 21)) / 2;
+								tx0_y = ((tx_y0[i] >> 21) + (tx_y0[ii] >> 21)) / 2;
+								tx_x0[0] = tx_x0[i];
+								tx_y0[1] = tx_y0[ii];
+								RT_TRACE(COMP_IQK, DBG_LOUD,
+									("TX0_X = %x;;TX0_Y = %x\n",
+									tx0_x & 0x000007ff, tx0_y & 0x000007ff));
+								if ((rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80)
+									&& vdf_enable) {
+									tx_dt[0] = (tx_dt[i] + tx_dt[ii]) / 2;
+								}
+								tx0_finish = true;
+							}
+						}
+					}
+				}
+			}
+			if (tx1_average >= 2){
+				for (i = 0; i < tx1_average; i++){
+					for (ii = i+1; ii < tx1_average; ii++){
+						dx = (tx_x1[i] >> 21) - (tx_x1[ii] >> 21);
+						if (dx < 4 && dx > -4){
+							dy = (tx_y1[i] >> 21) - (tx_y1[ii] >> 21);
+							if (dy < 4 && dy > -4){
+								tx1_x = ((tx_x1[i] >> 21) + (tx_x1[ii] >> 21)) / 2;
+								tx1_y = ((tx_y1[i] >> 21) + (tx_y1[ii] >> 21)) / 2;
+								tx_x1[0] = tx_x1[i];
+								tx_y1[1] = tx_y1[ii];
+								RT_TRACE(COMP_IQK, DBG_LOUD,
+									("TX1_X = %x;;TX1_Y = %x\n",
+									tx1_x & 0x000007ff, tx1_y & 0x000007ff));
+								if ((rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80)
+									&& vdf_enable) {
+									tx_dt[0] = (tx_dt[i] +  tx_dt[ii]) / 2;
+								}
+								tx1_finish = true;
+							}
+						}
+					}
+				}
+			}
+			RT_TRACE(COMP_IQK, DBG_LOUD,
+				("TX0_Average = %d, TX1_Average = %d\n",
+				tx0_average, tx1_average));
+			RT_TRACE(COMP_IQK, DBG_LOUD,
+				("TX0_finish = %d, TX1_finish = %d\n",
+				tx0_finish, tx1_finish));
+			if (tx0_finish && tx1_finish)
+				break;
+			if ((cal0_retry + tx0_average) >= 10
+				|| (cal1_retry + tx1_average) >= 10 )
+				break;
+		}
+		RT_TRACE(COMP_IQK, DBG_LOUD,
+			("TXA_cal_retry = %d\n", cal0_retry));
+		RT_TRACE(COMP_IQK, DBG_LOUD,
+			("TXB_cal_retry = %d\n", cal1_retry));
+
+	}
+
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /* [31] = 0 --> Page C*/
+	rtl_set_rfreg(hw, RF90_PATH_A, 0x58, 0x7fe00,
+		rtl_get_rfreg(hw, RF90_PATH_A, 0x8, 0xffc00)); /*Load LOK*/
+	rtl_set_rfreg(hw, RF90_PATH_B, 0x58, 0x7fe00,
+		rtl_get_rfreg(hw, RF90_PATH_B, 0x8, 0xffc00)); /* Load LOK*/
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /*[31] = 1 --> Page C1*/
+
+
+	if (vdf_enable) {}
+	else{
+		rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
+		if (tx0_finish) {
+			/*====== Path A RX IQK RF Setting======*/
+			rtl_set_rfreg(hw, RF90_PATH_A, 0xef, BRFREGOFFSETMASK, 0x80000);
+			rtl_set_rfreg(hw, RF90_PATH_A, 0x18, 0x00c00, 0x3);     /* BW 20M*/
+			rtl_set_rfreg(hw, RF90_PATH_A, 0x30, BRFREGOFFSETMASK, 0x30000);
+			rtl_set_rfreg(hw, RF90_PATH_A, 0x31, BRFREGOFFSETMASK, 0x3f7ff);
+			rtl_set_rfreg(hw, RF90_PATH_A, 0x32, BRFREGOFFSETMASK, 0xfe7bf);
+			rtl_set_rfreg(hw, RF90_PATH_A, 0x8f, BRFREGOFFSETMASK, 0x88001);
+			rtl_set_rfreg(hw, RF90_PATH_A, 0x65, BRFREGOFFSETMASK, 0x931d6);
+			rtl_set_rfreg(hw, RF90_PATH_A, 0xef, BRFREGOFFSETMASK, 0x00000);
+		}
+		if (tx1_finish){
+			/*====== Path B RX IQK RF Setting======*/
+			rtl_set_rfreg(hw, RF90_PATH_B, 0xef, BRFREGOFFSETMASK, 0x80000);
+			rtl_set_rfreg(hw, RF90_PATH_B, 0x30, BRFREGOFFSETMASK, 0x30000);
+			rtl_set_rfreg(hw, RF90_PATH_B, 0x31, BRFREGOFFSETMASK, 0x3f7ff);
+			rtl_set_rfreg(hw, RF90_PATH_B, 0x32, BRFREGOFFSETMASK, 0xfe7bf);
+			rtl_set_rfreg(hw, RF90_PATH_B, 0x8f, BRFREGOFFSETMASK, 0x88001);
+			rtl_set_rfreg(hw, RF90_PATH_B, 0x65, BRFREGOFFSETMASK, 0x931d1);
+			rtl_set_rfreg(hw, RF90_PATH_B, 0xef, BRFREGOFFSETMASK, 0x00000);
+		}
+		rtl_set_bbreg(hw, 0x978, BIT(31), 0x1);
+		rtl_set_bbreg(hw, 0x97c, BIT(31), 0x0);
+		rtl_write_dword(rtlpriv, 0x90c, 0x00008000);
+		rtl_write_dword(rtlpriv, 0x984, 0x0046a890);
+
+		rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /*[31] = 1 --> Page C1*/
+		if (tx0_finish) {
+			rtl_write_dword(rtlpriv, 0xc80, 0x38008c10);/*TX_Tone_idx[9:0], TxK_Mask[29] TX_Tone = 16*/
+			rtl_write_dword(rtlpriv, 0xc84, 0x18008c10);/*RX_Tone_idx[9:0], RxK_Mask[29]*/
+			rtl_write_dword(rtlpriv, 0xc88, 0x02140119);
+			rtl_write_dword(rtlpriv, 0xc8c, 0x28160cc0);
+		}
+		if (tx1_finish){
+			rtl_write_dword(rtlpriv, 0xe80, 0x38008c10);/*TX_Tone_idx[9:0], TxK_Mask[29] TX_Tone = 16*/
+			rtl_write_dword(rtlpriv, 0xe84, 0x18008c10);/*RX_Tone_idx[9:0], RxK_Mask[29]*/
+			rtl_write_dword(rtlpriv, 0xe88, 0x02140119);
+			rtl_write_dword(rtlpriv, 0xe8c, 0x28160ca0);
+		}
+              cal0_retry = 0;
+		cal1_retry = 0;
+		while(1){
+		    /* one shot*/
+		    	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
+		    	if (tx0_finish){
+				rtl_set_bbreg(hw, 0x978, 0x03FF8000, (tx_x0[rx0_average % 2]) >> 21 & 0x000007ff);
+				rtl_set_bbreg(hw, 0x978, 0x000007FF, (tx_y0[rx0_average % 2]) >> 21 & 0x000007ff);
+				rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /* [31] = 1 --> Page C1*/
+				rtl_write_dword(rtlpriv, 0xcb8, 0x00300000);/*cb8[20] ±N SI/PI ¨Ï¥ÎÅv¤Áµ¹ iqk_dpk module*/
+				rtl_write_dword(rtlpriv, 0xcb8, 0x00100000);
+				mdelay(5); /*Delay 10ms*/
+		    	}
+			if (tx1_finish){
+				rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
+				rtl_set_bbreg(hw, 0x978, 0x03FF8000, (tx_x1[rx1_average % 2]) >> 21 & 0x000007ff);
+				rtl_set_bbreg(hw, 0x978, 0x000007FF, (tx_y1[rx1_average % 2]) >> 21 & 0x000007ff);
+				rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /*[31] = 1 --> Page C1*/
+				rtl_write_dword(rtlpriv, 0xeb8, 0x00300000);/*cb8[20] ±N SI/PI ¨Ï¥ÎÅv¤Áµ¹ iqk_dpk module*/
+				rtl_write_dword(rtlpriv, 0xeb8, 0x00100000);/* cb8[20] ±N SI/PI ¨Ï¥ÎÅv¤Áµ¹ iqk_dpk module*/
+			}
+			mdelay(10); /*Delay 10ms*/
+			rtl_write_dword(rtlpriv, 0xcb8, 0x00000000);
+			rtl_write_dword(rtlpriv, 0xeb8, 0x00000000);
+			delay_count = 0;
+			while (1){
+				if (!rx0_finish && tx0_finish)
+					iqk0_ready = (bool) rtl_get_bbreg(hw, 0xd00, BIT(10));
+				if (!rx1_finish && tx1_finish)
+					iqk1_ready = (bool) rtl_get_bbreg(hw, 0xd40, BIT(10));
+				if ((iqk0_ready && iqk1_ready)||(delay_count>20))
+					break;
+				else{
+					mdelay(1);
+					delay_count++;
+				}
+			}
+			RT_TRACE(COMP_IQK, DBG_LOUD,
+				("RX delay_count = %d\n", delay_count));
+			if (delay_count < 20){	// If 20ms No Result, then cal_retry++
+				// ============RXIQK Check==============
+				rx0_fail = (bool) rtl_get_bbreg(hw, 0xd00, BIT(11));
+				rx1_fail = (bool) rtl_get_bbreg(hw, 0xd40, BIT(11));
+				if (!(rx0_fail || rx0_finish) && tx0_finish){
+					rtl_write_dword(rtlpriv, 0xcb8, 0x06000000);
+					rx_x0[rx0_average] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000) << 21;
+					rtl_write_dword(rtlpriv, 0xcb8, 0x08000000);
+					rx_y0[rx0_average] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000) << 21;
+					rx0iqkok= true;
+					RT_TRACE(COMP_IQK, DBG_LOUD,
+						("RX_X0[%d] = %x ;; RX_Y0[%d] = %x\n",
+						rx0_average, (rx_x0[rx0_average]) >> 21 & 0x000007ff,
+						rx0_average, (rx_y0[rx0_average]) >> 21 & 0x000007ff));
+
+					rx0_average++;
+				}
+				else{
+					RT_TRACE(COMP_IQK, DBG_LOUD,
+						("1. RXA_cal_retry = %d\n", cal0_retry));
+					rx0iqkok = false;
+					cal0_retry++;
+					if (cal0_retry == 10)
+					break;
+				}
+				if (!(rx1_fail || rx1_finish) && tx1_finish){
+                            		rtl_write_dword(rtlpriv, 0xeb8, 0x06000000);
+                            		rx_x1[rx1_average] = rtl_get_bbreg(hw, 0xd40, 0x07ff0000) << 21;
+                            		rtl_write_dword(rtlpriv, 0xeb8, 0x08000000);
+                            		rx_y1[rx1_average] = rtl_get_bbreg(hw, 0xd40, 0x07ff0000) << 21;
+                            		rx1iqkok = true;
+					RT_TRACE(COMP_IQK, DBG_LOUD,
+						("RX_X1[%d] = %x ;; RX_Y1[%d] = %x\n",
+						rx1_average, (rx_x1[rx1_average]) >> 21 & 0x000007ff,
+						rx1_average, (rx_y1[rx1_average]) >> 21 & 0x000007ff));
+
+					rx1_average++;
+                        	}
+                        	else{
+                            		rx1iqkok= false;
+                            		cal1_retry++;
+                            		if (cal1_retry == 10)
+                                		break;
+                    		}
+
+			}
+			else{
+				RT_TRACE(COMP_IQK, DBG_LOUD,
+					("2. RXA_cal_retry = %d\n", cal0_retry));
+				rx0iqkok = false;
+				rx1iqkok = false;
+				cal0_retry++;
+				cal1_retry++;
+				RT_TRACE(COMP_IQK, DBG_LOUD,
+					("Delay 20ms RX IQK Not Ready!!!!!\n"));
+			    if (cal0_retry == 10)
+			        break;
+			}
+			RT_TRACE(COMP_IQK, DBG_LOUD,
+				("3. RXA_cal_retry = %d\n", cal0_retry));
+			if (rx0_average >= 2){
+				for (i = 0; i < rx0_average; i++){
+					for (ii = i+1; ii < rx0_average; ii++){
+					dx = (rx_x0[i] >> 21) - (rx_x0[ii] >> 21);
+						if (dx < 4 && dx > -4){
+						dy = (rx_y0[i] >> 21) - (rx_y0[ii] >> 21);
+							if (dy < 4 && dy > -4){
+								rx0_x = ((rx_x0[i]>>21) + (rx_x0[ii] >> 21)) / 2;
+								rx0_y = ((rx_y0[i]>>21) + (rx_y0[ii] >> 21)) / 2;
+								if ((rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80)
+									&& vdf_enable) {
+									rx_dt[0] = (rx_dt[i] + rx_dt[ii]) / 2;
+								}
+								rx0_finish = true;
+								break;
+							}
+						}
+					}
+				}
+			}
+			if (rx1_average >= 2){
+				for (i = 0; i < rx1_average; i++){
+					for (ii = i+1; ii < rx1_average; ii++){
+					dx = (rx_x1[i] >> 21) - (rx_x1[ii] >> 21);
+						if (dx < 4 && dx > -4){
+						dy = (rx_y1[i] >> 21) - (rx_y1[ii] >> 21);
+							if (dy < 4 && dy > -4){
+								rx1_x = ((rx_x1[i] >> 21) + (rx_x1[ii] >> 21)) / 2;
+								rx1_y = ((rx_y1[i] >> 21) + (rx_y1[ii] >> 21)) / 2;
+								if ((rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80)
+									&& vdf_enable) {
+									rx_dt[0] = (rx_dt[i] + rx_dt[ii]) / 2;
+								}
+								rx1_finish = true;
+								break;
+							}
+						}
+					}
+				}
+			}
+			RT_TRACE(COMP_IQK, DBG_LOUD,
+				("RX0_Average = %d, RX1_Average = %d\n",
+				rx0_average, rx1_average));
+			RT_TRACE(COMP_IQK, DBG_LOUD,
+				("RX0_finish = %d, RX1_finish = %d\n",
+				rx0_finish, rx1_finish));
+			if ((rx0_finish|| !tx0_finish) && (rx1_finish || !tx1_finish) )
+				break;
+			if ((cal0_retry + rx0_average) >= 10
+				|| (cal1_retry + rx1_average) >= 10
+				|| rx0_average == 3
+				|| rx1_average == 3)
+				break;
+		}
+		RT_TRACE(COMP_IQK, DBG_LOUD,
+			("RXA_cal_retry = %d\n", cal0_retry));
+		RT_TRACE(COMP_IQK, DBG_LOUD,
+			("RXB_cal_retry = %d\n", cal1_retry));
+	}
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /* [31] = 0 --> Page C*/
+	switch (rtlphy->current_chan_bw)
+	{
+	case HT_CHANNEL_WIDTH_20_40:
+		{
+		rtl_set_rfreg(hw, RF90_PATH_A, 0x18, 0x00c00, 0x1);
+		}
+		break;
+	case HT_CHANNEL_WIDTH_80:
+		{
+		rtl_set_rfreg(hw, RF90_PATH_A, 0x18, 0x00c00, 0x0);
+		}
+		break;
+	default:
+		break;
+
+	}
+
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /* [31] = 0 --> Page C*/
+	/*FillIQK Result*/
+	RT_TRACE(COMP_IQK, DBG_LOUD,
+		("========Path_A =======\n"));
+
+	if (tx0_finish){
+		_rtl8812ae_iqk_tx_fill_iqc(hw, RF90_PATH_A, tx0_x, tx0_y);
+	}
+	else{
+		_rtl8812ae_iqk_tx_fill_iqc(hw, RF90_PATH_A, 0x200, 0x0);
+	}
+
+	if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80
+		|| vdf_enable){
+		rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /*[31] = 0 --> Page C*/
+		rtl_set_bbreg(hw, 0xce8, 0x3fff0000, tx_dt[0] & 0x00003fff);
+		rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
+	}
+
+	if (rx0_finish == 1){
+		_rtl8812ae_iqk_rx_fill_iqc(hw, RF90_PATH_A, rx0_x, rx0_y);
+	}
+	else{
+		_rtl8812ae_iqk_rx_fill_iqc(hw, RF90_PATH_A, 0x200, 0x0);
+	}
+
+	if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80
+		|| vdf_enable){
+		rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /*[31] = 0 --> Page C*/
+		rtl_set_bbreg(hw, 0xce8, 0x00003fff, rx_dt[0] & 0x00003fff);
+		rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /* [31] = 0 --> Page C*/
+	}
+
+	RT_TRACE(COMP_IQK, DBG_LOUD,
+		("========Path_B =======\n"));
+
+	if (tx1_finish){
+		_rtl8812ae_iqk_tx_fill_iqc(hw, RF90_PATH_B, tx1_x, tx1_y);
+	}
+	else{
+		_rtl8812ae_iqk_tx_fill_iqc(hw, RF90_PATH_B, 0x200, 0x0);
+	}
+
+	if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80
+		|| vdf_enable){
+		rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /* [31] = 0 --> Page C*/
+		rtl_set_bbreg(hw, 0xee8, 0x3fff0000, tx_dt[0] & 0x00003fff);
+		rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /* [31] = 0 --> Page C*/
+	}
+
+	if (rx1_finish == 1){
+		_rtl8812ae_iqk_rx_fill_iqc(hw, RF90_PATH_B, rx1_x, rx1_y);
+	}
+	else{
+		_rtl8812ae_iqk_rx_fill_iqc(hw, RF90_PATH_B, 0x200, 0x0);
+	}
+
+	if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80
+		|| vdf_enable){
+		rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /* [31] = 0 --> Page C*/
+		rtl_set_bbreg(hw, 0xee8, 0x00003fff, rx_dt[0] & 0x00003fff);
+		rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /* [31] = 0 --> Page C*/
+	}
+}
+
+void _rtl8812ae_iqk_restore_rf(
+	struct ieee80211_hw *hw,
+	enum radio_path path,
+	u32 *backup_rf_reg,
+	u32 *rf_backup,
+	u32 rf_reg_num
+	)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i;
+
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
+    	for (i = 0; i < rf_reg_num; i++)
+        	rtl_set_rfreg(hw, path, backup_rf_reg[i], BRFREGOFFSETMASK, rf_backup[i]);
+
+	rtl_set_rfreg(hw, path, 0xef, BRFREGOFFSETMASK, 0x0);
+
+	switch(path){
+	case RF90_PATH_A:
+       {
+       	RT_TRACE(COMP_IQK, DBG_LOUD,
+			("RestoreRF Path A Success!!!!\n"));
+       }
+		break;
+	case RF90_PATH_B:
+       {
+       	RT_TRACE(COMP_IQK, DBG_LOUD,
+			("RestoreRF Path B Success!!!!\n"));
+       }
+		break;
+	default:
+		break;
+	}
+}
+
+void _rtl8812ae_iqk_restore_afe(
+	struct ieee80211_hw *hw,
+	u32 *afe_backup,
+	u32 *backup_afe_reg,
+	u32 afe_num
+	)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i;
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
+	/*Reload AFE Parameters */
+    	for (i = 0; i < afe_num; i++){
+        	rtl_write_dword(rtlpriv, backup_afe_reg[i], afe_backup[i]);
+    	}
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /* [31] = 1 --> Page C1*/
+	rtl_write_dword(rtlpriv, 0xc80, 0x0);
+	rtl_write_dword(rtlpriv, 0xc84, 0x0);
+	rtl_write_dword(rtlpriv, 0xc88, 0x0);
+	rtl_write_dword(rtlpriv, 0xc8c, 0x3c000000);
+	rtl_write_dword(rtlpriv, 0xc90, 0x00000080);
+	rtl_write_dword(rtlpriv, 0xc94, 0x00000000);
+	rtl_write_dword(rtlpriv, 0xcc4, 0x20040000);
+	rtl_write_dword(rtlpriv, 0xcc8, 0x20000000);
+	rtl_write_dword(rtlpriv, 0xcb8, 0x0);
+	rtl_write_dword(rtlpriv, 0xe80, 0x0);
+	rtl_write_dword(rtlpriv, 0xe84, 0x0);
+	rtl_write_dword(rtlpriv, 0xe88, 0x0);
+	rtl_write_dword(rtlpriv, 0xe8c, 0x3c000000);
+	rtl_write_dword(rtlpriv, 0xe90, 0x00000080);
+	rtl_write_dword(rtlpriv, 0xe94, 0x00000000);
+	rtl_write_dword(rtlpriv, 0xec4, 0x20040000);
+	rtl_write_dword(rtlpriv, 0xec8, 0x20000000);
+	rtl_write_dword(rtlpriv, 0xeb8, 0x0);
+	RT_TRACE(COMP_IQK, DBG_LOUD,
+		("RestoreAFE Success!!!!\n"));
+}
+
+void _rtl8812ae_iqk_restore_macbb(
+	struct ieee80211_hw *hw,
+	u32 *macbb_backup,
+	u32 *backup_macbb_reg,
+	u32 macbb_num
+	)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i;
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /* [31] = 0 --> Page C*/
+	//Reload MacBB Parameters
+    	for (i = 0; i < macbb_num; i++){
+        	rtl_write_dword(rtlpriv, backup_macbb_reg[i], macbb_backup[i]);
+    	}
+    	RT_TRACE(COMP_IQK, DBG_LOUD,
+			("RestoreMacBB Success!!!!\n"));
+}
+
+#define MACBB_REG_NUM 10
+#define AFE_REG_NUM 14
+#define RF_REG_NUM 3
+
+static void _rtl8812ae_phy_iq_calibrate(
+		struct ieee80211_hw *hw,
+		u8 channel)
+{
+	u32	macbb_backup[MACBB_REG_NUM];
+	u32 afe_backup[AFE_REG_NUM];
+	u32 rfa_backup[RF_REG_NUM];
+	u32 rfb_backup[RF_REG_NUM];
+	u32 	backup_macbb_reg[MACBB_REG_NUM] = {0xb00, 0x520, 0x550,
+											0x808, 0x90c, 0xc00, 0xe00,
+											0x8c4,0x838,  0x82c};
+	u32	backup_afe_reg[AFE_REG_NUM] = {0xc5c, 0xc60, 0xc64, 0xc68,
+										0xcb8, 0xcb0, 0xcb4,0xe5c,
+										0xe60, 0xe64, 0xe68, 0xeb8,
+										0xeb0, 0xeb4};
+	u32 	backup_rf_reg[RF_REG_NUM] = {0x65, 0x8f, 0x0};
+	u8 	chnl_idx = _rtl8812ae_get_right_chnl_place_for_iqk(channel);
+
+	_rtl8812ae_iqk_backup_macbb(hw, macbb_backup, backup_macbb_reg, MACBB_REG_NUM);
+	_rtl8812ae_iqk_backup_afe(hw, afe_backup, backup_afe_reg, AFE_REG_NUM);
+	_rtl8812ae_iqk_backup_rf(hw, rfa_backup, rfb_backup, backup_rf_reg, RF_REG_NUM);
+
+	_rtl8812ae_iqk_configure_mac(hw);
+	_rtl8812ae_iqk_tx(hw, chnl_idx);
+	_rtl8812ae_iqk_restore_rf(hw, RF90_PATH_A, backup_rf_reg, rfa_backup, RF_REG_NUM);
+	_rtl8812ae_iqk_restore_rf(hw, RF90_PATH_A, backup_rf_reg, rfb_backup, RF_REG_NUM); // PATH_A ?
+
+	_rtl8812ae_iqk_restore_afe(hw, afe_backup, backup_afe_reg, AFE_REG_NUM);
+	_rtl8812ae_iqk_restore_macbb(hw, macbb_backup, backup_macbb_reg, MACBB_REG_NUM);
+}
+
+
+void _rtl8821ae_iqk_backup_macbb(
+		struct ieee80211_hw *hw,
+		u32 *macbb_backup,
+		u32 *backup_macbb_reg,
+		u32 mac_bb_num
+		)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i;
+
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
+	/*save MACBB default value*/
+	for (i = 0; i < mac_bb_num; i++) {
+		macbb_backup[i] =rtl_read_dword(rtlpriv,backup_macbb_reg[i]);
+	}
+
+	RT_TRACE(COMP_IQK, DBG_LOUD, ("BackupMacBB Success!!!!\n"));
+}
+
+void _rtl8821ae_iqk_backup_afe(
+		struct ieee80211_hw *hw,
+		u32 *afe_backup,
+		u32 *backup_afe_REG,
+		u32 afe_num
+		)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i;
+
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
+	/*Save AFE Parameters */
+	for (i = 0; i < afe_num; i++){
+		afe_backup[i] = rtl_read_dword(rtlpriv, backup_afe_REG[i]);
+	}
+	RT_TRACE(COMP_IQK, DBG_LOUD, ("BackupAFE Success!!!!\n"));
+}
+
+void _rtl8821ae_iqk_backup_rf(
+		struct ieee80211_hw *hw,
+		u32 *rfa_backup,
+		u32 *rfb_backup,
+		u32 *backup_rf_reg,
+		u32 rf_num
+		)
+{
+
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i;
+
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
+	/*Save RF Parameters*/
+	for (i = 0; i < rf_num; i++){
+		rfa_backup[i] = rtl_get_rfreg(hw, RF90_PATH_A, backup_rf_reg[i], BMASKDWORD);
+		rfb_backup[i] = rtl_get_rfreg(hw, RF90_PATH_B, backup_rf_reg[i], BMASKDWORD);
+	}
+	RT_TRACE(COMP_IQK, DBG_LOUD, ("BackupRF Success!!!!\n"));
+}
+
+void _rtl8821ae_iqk_configure_mac(
+		struct ieee80211_hw *hw
+		)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	/* ========MAC register setting========*/
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
+	rtl_write_byte(rtlpriv, 0x522, 0x3f);
+	rtl_set_bbreg(hw, 0x550, BIT(11) | BIT(3), 0x0);
+	rtl_write_byte(rtlpriv, 0x808, 0x00);		/*RX ante off*/
+	rtl_set_bbreg(hw, 0x838, 0xf, 0xc);		/*CCA off*/
+}
+
+
+void _rtl8821ae_iqk_tx_fill_iqc(
+		struct ieee80211_hw *hw,
+		enum radio_path path,
+		u32 tx_x,
+		u32 tx_y
+		)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	switch (path) {
+		case RF90_PATH_A:
+			{
+				rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); // [31] = 1 --> Page C1
+				rtl_write_dword(rtlpriv, 0xc90, 0x00000080);
+				rtl_write_dword(rtlpriv, 0xcc4, 0x20040000);
+				rtl_write_dword(rtlpriv, 0xcc8, 0x20000000);
+				rtl_set_bbreg(hw, 0xccc, 0x000007ff, tx_y);
+				rtl_set_bbreg(hw, 0xcd4, 0x000007ff, tx_x);
+				RT_TRACE(COMP_IQK, DBG_LOUD, ("TX_X = %x;;TX_Y = %x =====> fill to IQC\n", tx_x, tx_y));
+				RT_TRACE(COMP_IQK, DBG_LOUD, ("0xcd4 = %x;;0xccc = %x ====>fill to IQC\n", rtl_get_bbreg(hw, 0xcd4, 0x000007ff), rtl_get_bbreg(hw, 0xccc, 0x000007ff)));
+			}
+			break;
+		default:
+			break;
+	};
+}
+
+
+void _rtl8821ae_iqk_rx_fill_iqc(
+		struct ieee80211_hw *hw,
+		enum radio_path path,
+		u32 rx_x,
+		u32 rx_y
+		)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	switch (path) {
+		case RF90_PATH_A:
+			{
+				rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); // [31] = 0 --> Page C
+				rtl_set_bbreg(hw, 0xc10, 0x000003ff, rx_x>>1);
+				rtl_set_bbreg(hw, 0xc10, 0x03ff0000, rx_y>>1);
+				RT_TRACE(COMP_IQK, DBG_LOUD, ("rx_x = %x;;rx_y = %x ====>fill to IQC\n", rx_x>>1, rx_y>>1));
+				RT_TRACE(COMP_IQK, DBG_LOUD, ("0xc10 = %x ====>fill to IQC\n", rtl_read_dword(rtlpriv, 0xc10)));
+			}
+			break;
+		default:
+			break;
+	};
+}
+
+
+
+#define cal_num 10
+
+void _rtl8821ae_iqk_tx(
+		struct ieee80211_hw *hw,
+		enum radio_path path
+		)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	u32 	tx_fail, rx_fail, delay_count, iqk_ready, cal_retry, cal = 0, temp_reg65;
+	int	tx_x = 0, tx_y = 0, rx_x = 0, rx_y = 0, tx_average = 0, rx_average = 0;
+	int	tx_x0[cal_num], tx_y0[cal_num], tx_x0_rxk[cal_num], tx_y0_rxk[cal_num], rx_x0[cal_num], rx_y0[cal_num];
+	bool 	tx0iqkok = false, rx0iqkok = false;
+	bool  	vdf_enable = false;
+	int	i, k, vdf_y[3], vdf_x[3], tx_dt[3], rx_dt[3], ii, dx = 0, dy = 0, tx_finish = 0, rx_finish = 0;
+
+
+	RT_TRACE(COMP_IQK, DBG_LOUD,
+			("BandWidth = %d.\n",
+			 rtlphy->current_chan_bw));
+	if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80){
+		vdf_enable = true;
+	}
+
+	while (cal < cal_num) {
+		switch (path) {
+			case RF90_PATH_A:
+				{
+					temp_reg65 = rtl_get_rfreg(hw, path, 0x65, 0xffffffff);
+					//Path-A LOK
+					rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
+					/*========Path-A AFE all on========*/
+					/*Port 0 DAC/ADC on*/
+					rtl_write_dword(rtlpriv, 0xc60, 0x77777777);
+					rtl_write_dword(rtlpriv, 0xc64, 0x77777777);
+					rtl_write_dword(rtlpriv, 0xc68, 0x19791979);
+					rtl_write_dword(rtlpriv, 0xc6c, 0x19791979);
+					rtl_write_dword(rtlpriv, 0xc70, 0x19791979);
+					rtl_write_dword(rtlpriv, 0xc74, 0x19791979);
+					rtl_write_dword(rtlpriv, 0xc78, 0x19791979);
+					rtl_write_dword(rtlpriv, 0xc7c, 0x19791979);
+					rtl_write_dword(rtlpriv, 0xc80, 0x19791979);
+					rtl_write_dword(rtlpriv, 0xc84, 0x19791979);
+
+					rtl_set_bbreg(hw, 0xc00, 0xf, 0x4); /*hardware 3-wire off*/
+
+					// LOK Setting
+					//====== LOK ======
+					/*DAC/ADC sampling rate (160 MHz)*/
+					rtl_set_bbreg(hw, 0xc5c, BIT(26) | BIT(25) | BIT(24), 0x7);
+
+					// 2. LoK RF Setting (at BW = 20M)
+					rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x80002);
+					rtl_set_rfreg(hw, path, 0x18, 0x00c00, 0x3);     // BW 20M
+					rtl_set_rfreg(hw, path, 0x30, RFREG_OFFSET_MASK, 0x20000);
+					rtl_set_rfreg(hw, path, 0x31, RFREG_OFFSET_MASK, 0x0003f);
+					rtl_set_rfreg(hw, path, 0x32, RFREG_OFFSET_MASK, 0xf3fc3);
+					rtl_set_rfreg(hw, path, 0x65, RFREG_OFFSET_MASK, 0x931d5);
+					rtl_set_rfreg(hw, path, 0x8f, RFREG_OFFSET_MASK, 0x8a001);
+					rtl_set_bbreg(hw, 0xcb8, 0xf, 0xd);
+					rtl_write_dword(rtlpriv, 0x90c, 0x00008000);
+					rtl_write_dword(rtlpriv, 0xb00, 0x03000100);
+					rtl_set_bbreg(hw, 0xc94, BIT(0), 0x1);
+					rtl_write_dword(rtlpriv, 0x978, 0x29002000);// TX (X,Y)
+					rtl_write_dword(rtlpriv, 0x97c, 0xa9002000);// RX (X,Y)
+					rtl_write_dword(rtlpriv, 0x984, 0x00462910);// [0]:AGC_en, [15]:idac_K_Mask
+
+					rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); // [31] = 1 --> Page C1
+					rtl_write_dword(rtlpriv, 0xc88, 0x821403f4);
+
+					if (rtlhal->current_bandtype)
+						rtl_write_dword(rtlpriv, 0xc8c, 0x68163e96);
+					else
+						rtl_write_dword(rtlpriv, 0xc8c, 0x28163e96);
+
+					rtl_write_dword(rtlpriv, 0xc80, 0x18008c10);// TX_Tone_idx[9:0], TxK_Mask[29] TX_Tone = 16
+					rtl_write_dword(rtlpriv, 0xc84, 0x38008c10);// RX_Tone_idx[9:0], RxK_Mask[29]
+					rtl_write_dword(rtlpriv, 0xcb8, 0x00100000);// cb8[20] ±N SI/PI ¨Ï¥ÎÅv¤Áµ¹ iqk_dpk module
+					rtl_write_dword(rtlpriv, 0x980, 0xfa000000);
+					rtl_write_dword(rtlpriv, 0x980, 0xf8000000);
+
+					mdelay(10); //Delay 10ms
+					rtl_write_dword(rtlpriv, 0xcb8, 0x00000000);
+
+					rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); // [31] = 0 --> Page C
+					rtl_set_rfreg(hw, path, 0x58, 0x7fe00, rtl_get_rfreg(hw, path, 0x8, 0xffc00)); // Load LOK
+
+					switch (rtlphy->current_chan_bw)
+					{
+						case 1:
+							{
+								rtl_set_rfreg(hw, path, 0x18, 0x00c00, 0x1);
+							}
+							break;
+						case 2:
+							{
+								rtl_set_rfreg(hw, path, 0x18, 0x00c00, 0x0);
+							}
+							break;
+						default:
+							break;
+
+					}
+
+					rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); // [31] = 1 --> Page C1
+
+					// 3. TX RF Setting
+					rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); // [31] = 0 --> Page C
+					rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x80000);
+					rtl_set_rfreg(hw, path, 0x30, RFREG_OFFSET_MASK, 0x20000);
+					rtl_set_rfreg(hw, path, 0x31, RFREG_OFFSET_MASK, 0x0003f);
+					rtl_set_rfreg(hw, path, 0x32, RFREG_OFFSET_MASK, 0xf3fc3);
+					rtl_set_rfreg(hw, path, 0x65, RFREG_OFFSET_MASK, 0x931d5);
+					rtl_set_rfreg(hw, path, 0x8f, RFREG_OFFSET_MASK, 0x8a001);
+					rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x00000);
+					//ODM_SetBBReg(pDM_Odm, 0xcb8, 0xf, 0xd);
+					rtl_write_dword(rtlpriv, 0x90c, 0x00008000);
+					rtl_write_dword(rtlpriv, 0xb00, 0x03000100);
+					rtl_set_bbreg(hw, 0xc94, BIT(0), 0x1);
+					rtl_write_dword(rtlpriv, 0x978, 0x29002000);// TX (X,Y)
+					rtl_write_dword(rtlpriv, 0x97c, 0xa9002000);// RX (X,Y)
+					rtl_write_dword(rtlpriv, 0x984, 0x0046a910);// [0]:AGC_en, [15]:idac_K_Mask
+
+					rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); // [31] = 1 --> Page C1
+					rtl_write_dword(rtlpriv, 0xc88, 0x821403f1);
+					if (rtlhal->current_bandtype)
+						rtl_write_dword(rtlpriv, 0xc8c, 0x40163e96);
+					else
+						rtl_write_dword(rtlpriv, 0xc8c, 0x00163e96);
+
+					if (vdf_enable == 1){
+						RT_TRACE(COMP_IQK, DBG_LOUD, ("VDF_enable\n"));
+						for (k = 0;k <= 2; k++){
+							switch (k){
+								case 0:
+									{
+										rtl_write_dword(rtlpriv, 0xc80, 0x18008c38);// TX_Tone_idx[9:0], TxK_Mask[29] TX_Tone = 16
+										rtl_write_dword(rtlpriv, 0xc84, 0x38008c38);// RX_Tone_idx[9:0], RxK_Mask[29]
+										rtl_set_bbreg(hw, 0xce8, BIT(31), 0x0);
+									}
+									break;
+								case 1:
+									{
+										rtl_set_bbreg(hw, 0xc80, BIT(28), 0x0);
+										rtl_set_bbreg(hw, 0xc84, BIT(28), 0x0);
+										rtl_set_bbreg(hw, 0xce8, BIT(31), 0x0);
+									}
+									break;
+								case 2:
+									{
+										RT_TRACE(COMP_IQK, DBG_LOUD, ("vdf_y[1] = %x;;;vdf_y[0] = %x\n", vdf_y[1]>>21 & 0x00007ff, vdf_y[0]>>21 & 0x00007ff));
+										RT_TRACE(COMP_IQK, DBG_LOUD, ("vdf_x[1] = %x;;;vdf_x[0] = %x\n", vdf_x[1]>>21 & 0x00007ff, vdf_x[0]>>21 & 0x00007ff));
+										tx_dt[cal] = (vdf_y[1]>>20)-(vdf_y[0]>>20);
+										tx_dt[cal] = ((16*tx_dt[cal])*10000/15708);
+										tx_dt[cal] = (tx_dt[cal] >> 1 )+(tx_dt[cal] & BIT(0));
+										rtl_write_dword(rtlpriv, 0xc80, 0x18008c20);// TX_Tone_idx[9:0], TxK_Mask[29] TX_Tone = 16
+										rtl_write_dword(rtlpriv, 0xc84, 0x38008c20);// RX_Tone_idx[9:0], RxK_Mask[29]
+										rtl_set_bbreg(hw, 0xce8, BIT(31), 0x1);
+										rtl_set_bbreg(hw, 0xce8, 0x3fff0000, tx_dt[cal] & 0x00003fff);
+									}
+									break;
+								default:
+									break;
+							}
+							rtl_write_dword(rtlpriv, 0xcb8, 0x00100000);// cb8[20] ±N SI/PI ¨Ï¥ÎÅv¤Áµ¹ iqk_dpk module
+							cal_retry = 0;
+							while(1){
+								// one shot
+								rtl_write_dword(rtlpriv, 0x980, 0xfa000000);
+								rtl_write_dword(rtlpriv, 0x980, 0xf8000000);
+
+								mdelay(10); //Delay 10ms
+								rtl_write_dword(rtlpriv, 0xcb8, 0x00000000);
+								delay_count = 0;
+								while (1){
+									iqk_ready = rtl_get_bbreg(hw, 0xd00, BIT(10));
+									if ((~iqk_ready) || (delay_count>20)){
+										break;
+									}
+									else{
+										mdelay(1);
+										delay_count++;
+									}
+								}
+
+								if (delay_count < 20){							// If 20ms No Result, then cal_retry++
+									// ============TXIQK Check==============
+									tx_fail = rtl_get_bbreg(hw, 0xd00, BIT(12));
+
+									if (~tx_fail){
+										rtl_write_dword(rtlpriv, 0xcb8, 0x02000000);
+										vdf_x[k] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
+										rtl_write_dword(rtlpriv, 0xcb8, 0x04000000);
+										vdf_y[k] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
+										tx0iqkok = true;
+										break;
+									}
+									else{
+										rtl_set_bbreg(hw, 0xccc, 0x000007ff, 0x0);
+										rtl_set_bbreg(hw, 0xcd4, 0x000007ff, 0x200);
+										tx0iqkok = false;
+										cal_retry++;
+										if (cal_retry == 10) {
+											break;
+										}
+									}
+								}
+								else{
+									tx0iqkok = false;
+									cal_retry++;
+									if (cal_retry == 10){
+										break;
+									}
+								}
+							}
+						}
+						if (k == 3){
+							tx_x0[cal] = vdf_x[k-1] ;
+							tx_y0[cal] = vdf_y[k-1];
+						}
+					}
+
+					else {
+						rtl_write_dword(rtlpriv, 0xc80, 0x18008c10);// TX_Tone_idx[9:0], TxK_Mask[29] TX_Tone = 16
+						rtl_write_dword(rtlpriv, 0xc84, 0x38008c10);// RX_Tone_idx[9:0], RxK_Mask[29]
+						rtl_write_dword(rtlpriv, 0xcb8, 0x00100000);// cb8[20] ±N SI/PI ¨Ï¥ÎÅv¤Áµ¹ iqk_dpk module
+						cal_retry = 0;
+						while(1){
+							// one shot
+							rtl_write_dword(rtlpriv, 0x980, 0xfa000000);
+							rtl_write_dword(rtlpriv, 0x980, 0xf8000000);
+
+							mdelay(10); //Delay 10ms
+							rtl_write_dword(rtlpriv, 0xcb8, 0x00000000);
+							delay_count = 0;
+							while (1){
+								iqk_ready = rtl_get_bbreg(hw, 0xd00, BIT(10));
+								if ((~iqk_ready) || (delay_count>20)) {
+									break;
+								}
+								else{
+									mdelay(1);
+									delay_count++;
+								}
+							}
+
+							if (delay_count < 20){							// If 20ms No Result, then cal_retry++
+								// ============TXIQK Check==============
+								tx_fail = rtl_get_bbreg(hw, 0xd00, BIT(12));
+
+								if (~tx_fail){
+									rtl_write_dword(rtlpriv, 0xcb8, 0x02000000);
+									tx_x0[cal] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
+									rtl_write_dword(rtlpriv, 0xcb8, 0x04000000);
+									tx_y0[cal] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
+									tx0iqkok = true;
+									break;
+								}
+								else{
+									rtl_set_bbreg(hw, 0xccc, 0x000007ff, 0x0);
+									rtl_set_bbreg(hw, 0xcd4, 0x000007ff, 0x200);
+									tx0iqkok = false;
+									cal_retry++;
+									if (cal_retry == 10) {
+										break;
+									}
+								}
+							}
+							else{
+								tx0iqkok = false;
+								cal_retry++;
+								if (cal_retry == 10)
+									break;
+							}
+						}
+					}
+
+
+					if (tx0iqkok == false)
+						break;				// TXK fail, Don't do RXK
+
+					if (vdf_enable == 1){
+						rtl_set_bbreg(hw, 0xce8, BIT(31), 0x0);    // TX VDF Disable
+						RT_TRACE(COMP_IQK, DBG_LOUD, ("RXVDF Start\n"));
+						for (k = 0;k <= 2; k++){
+							//====== RX mode TXK (RXK Step 1) ======
+							rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); // [31] = 0 --> Page C
+							// 1. TX RF Setting
+							rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x80000);
+							rtl_set_rfreg(hw, path, 0x30, RFREG_OFFSET_MASK, 0x30000);
+							rtl_set_rfreg(hw, path, 0x31, RFREG_OFFSET_MASK, 0x00029);
+							rtl_set_rfreg(hw, path, 0x32, RFREG_OFFSET_MASK, 0xd7ffb);
+							rtl_set_rfreg(hw, path, 0x65, RFREG_OFFSET_MASK, temp_reg65);
+							rtl_set_rfreg(hw, path, 0x8f, RFREG_OFFSET_MASK, 0x8a001);
+							rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x00000);
+
+							rtl_set_bbreg(hw, 0xcb8, 0xf, 0xd);
+							rtl_write_dword(rtlpriv, 0x978, 0x29002000);// TX (X,Y)
+							rtl_write_dword(rtlpriv, 0x97c, 0xa9002000);// RX (X,Y)
+							rtl_write_dword(rtlpriv, 0x984, 0x0046a910);// [0]:AGC_en, [15]:idac_K_Mask
+							rtl_write_dword(rtlpriv, 0x90c, 0x00008000);
+							rtl_write_dword(rtlpriv, 0xb00, 0x03000100);
+							rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); // [31] = 1 --> Page C1
+							switch (k){
+								case 0:
+									{
+										rtl_write_dword(rtlpriv, 0xc80, 0x18008c38);// TX_Tone_idx[9:0], TxK_Mask[29] TX_Tone = 16
+										rtl_write_dword(rtlpriv, 0xc84, 0x38008c38);// RX_Tone_idx[9:0], RxK_Mask[29]
+										rtl_set_bbreg(hw, 0xce8, BIT(30), 0x0);
+									}
+									break;
+								case 1:
+									{
+										rtl_write_dword(rtlpriv, 0xc80, 0x08008c38);// TX_Tone_idx[9:0], TxK_Mask[29] TX_Tone = 16
+										rtl_write_dword(rtlpriv, 0xc84, 0x28008c38);// RX_Tone_idx[9:0], RxK_Mask[29]
+										rtl_set_bbreg(hw, 0xce8, BIT(30), 0x0);
+									}
+									break;
+								case 2:
+									{
+										RT_TRACE(COMP_IQK, DBG_LOUD, ("VDF_Y[1] = %x;;;VDF_Y[0] = %x\n", vdf_y[1]>>21 & 0x00007ff, vdf_y[0]>>21 & 0x00007ff));
+										RT_TRACE(COMP_IQK, DBG_LOUD, ("VDF_X[1] = %x;;;VDF_X[0] = %x\n", vdf_x[1]>>21 & 0x00007ff, vdf_x[0]>>21 & 0x00007ff));
+										rx_dt[cal] = (vdf_y[1]>>20)-(vdf_y[0]>>20);
+										RT_TRACE(COMP_IQK, DBG_LOUD, ("Rx_dt = %d\n", rx_dt[cal]));
+										rx_dt[cal] = ((16*rx_dt[cal])*10000/13823);
+										rx_dt[cal] = (rx_dt[cal] >> 1 )+(rx_dt[cal] & BIT(0));
+										rtl_write_dword(rtlpriv, 0xc80, 0x18008c20);// TX_Tone_idx[9:0], TxK_Mask[29] TX_Tone = 16
+										rtl_write_dword(rtlpriv, 0xc84, 0x38008c20);// RX_Tone_idx[9:0], RxK_Mask[29]
+										rtl_set_bbreg(hw, 0xce8, 0x00003fff, rx_dt[cal] & 0x00003fff);
+									}
+									break;
+								default:
+									break;
+							}
+							rtl_write_dword(rtlpriv, 0xc88, 0x821603e0);
+							rtl_write_dword(rtlpriv, 0xc8c, 0x68163e96);
+							rtl_write_dword(rtlpriv, 0xcb8, 0x00100000);// cb8[20] ±N SI/PI ¨Ï¥ÎÅv¤Áµ¹ iqk_dpk module
+							cal_retry = 0;
+							while(1){
+								// one shot
+								rtl_write_dword(rtlpriv, 0x980, 0xfa000000);
+								rtl_write_dword(rtlpriv, 0x980, 0xf8000000);
+
+								mdelay(10); //Delay 10ms
+								rtl_write_dword(rtlpriv, 0xcb8, 0x00000000);
+								delay_count = 0;
+								while (1){
+									iqk_ready = rtl_get_bbreg(hw, 0xd00, BIT(10));
+									if ((~iqk_ready)||(delay_count>20)){
+										break;
+									}
+									else{
+										mdelay(1);
+										delay_count++;
+									}
+								}
+
+								if (delay_count < 20){							// If 20ms No Result, then cal_retry++
+									// ============TXIQK Check==============
+									tx_fail = rtl_get_bbreg(hw, 0xd00, BIT(12));
+
+									if (~tx_fail){
+										rtl_write_dword(rtlpriv, 0xcb8, 0x02000000);
+										tx_x0_rxk[cal] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
+										rtl_write_dword(rtlpriv, 0xcb8, 0x04000000);
+										tx_y0_rxk[cal] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
+										tx0iqkok = true;
+										break;
+									}
+									else{
+										tx0iqkok = false;
+										cal_retry++;
+										if (cal_retry == 10)
+											break;
+									}
+								}
+								else{
+									tx0iqkok = false;
+									cal_retry++;
+									if (cal_retry == 10)
+										break;
+								}
+							}
+
+							if (tx0iqkok == false){   //If RX mode TXK fail, then take TXK Result
+								tx_x0_rxk[cal] = tx_x0[cal];
+								tx_y0_rxk[cal] = tx_y0[cal];
+								tx0iqkok = true;
+								RT_TRACE(COMP_IQK, DBG_LOUD, ("RXK Step 1 fail\n"));
+							}
+
+
+							//====== RX IQK ======
+							rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); // [31] = 0 --> Page C
+							// 1. RX RF Setting
+							rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x80000);
+							rtl_set_rfreg(hw, path, 0x30, RFREG_OFFSET_MASK, 0x30000);
+							rtl_set_rfreg(hw, path, 0x31, RFREG_OFFSET_MASK, 0x0002f);
+							rtl_set_rfreg(hw, path, 0x32, RFREG_OFFSET_MASK, 0xfffbb);
+							rtl_set_rfreg(hw, path, 0x8f, RFREG_OFFSET_MASK, 0x88001);
+							rtl_set_rfreg(hw, path, 0x65, RFREG_OFFSET_MASK, 0x931d8);
+							rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x00000);
+
+							rtl_set_bbreg(hw, 0x978, 0x03FF8000, (tx_x0_rxk[cal])>>21&0x000007ff);
+							rtl_set_bbreg(hw, 0x978, 0x000007FF, (tx_y0_rxk[cal])>>21&0x000007ff);
+							rtl_set_bbreg(hw, 0x978, BIT(31), 0x1);
+							rtl_set_bbreg(hw, 0x97c, BIT(31), 0x0);
+							rtl_set_bbreg(hw, 0xcb8, 0xF, 0xe);
+							rtl_write_dword(rtlpriv, 0x90c, 0x00008000);
+							rtl_write_dword(rtlpriv, 0x984, 0x0046a911);
+
+							rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); // [31] = 1 --> Page C1
+							rtl_set_bbreg(hw, 0xc80, BIT(29), 0x1);
+							rtl_set_bbreg(hw, 0xc84, BIT(29), 0x0);
+							rtl_write_dword(rtlpriv, 0xc88, 0x02140119);
+
+							rtl_write_dword(rtlpriv, 0xc8c, 0x28160d00); /* pDM_Odm->SupportInterface == 1 */
+
+							if (k==2){
+								rtl_set_bbreg(hw, 0xce8, BIT(30), 0x1);  //RX VDF Enable
+							}
+							rtl_write_dword(rtlpriv, 0xcb8, 0x00100000);// cb8[20] ±N SI/PI ¨Ï¥ÎÅv¤Áµ¹ iqk_dpk module
+
+							cal_retry = 0;
+							while(1){
+								// one shot
+								rtl_write_dword(rtlpriv, 0x980, 0xfa000000);
+								rtl_write_dword(rtlpriv, 0x980, 0xf8000000);
+
+								mdelay(10); //Delay 10ms
+								rtl_write_dword(rtlpriv, 0xcb8, 0x00000000);
+								delay_count = 0;
+								while (1){
+									iqk_ready = rtl_get_bbreg(hw, 0xd00, BIT(10));
+									if ((~iqk_ready)||(delay_count>20)){
+										break;
+									}
+									else{
+										mdelay(1);
+										delay_count++;
+									}
+								}
+
+								if (delay_count < 20){	// If 20ms No Result, then cal_retry++
+									// ============RXIQK Check==============
+									rx_fail = rtl_get_bbreg(hw, 0xd00, BIT(11));
+									if (rx_fail == 0){
+										rtl_write_dword(rtlpriv, 0xcb8, 0x06000000);
+										vdf_x[k] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
+										rtl_write_dword(rtlpriv, 0xcb8, 0x08000000);
+										vdf_y[k] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
+										rx0iqkok = true;
+										break;
+									}
+									else{
+										rtl_set_bbreg(hw, 0xc10, 0x000003ff, 0x200>>1);
+										rtl_set_bbreg(hw, 0xc10, 0x03ff0000, 0x0>>1);
+										rx0iqkok = false;
+										cal_retry++;
+										if (cal_retry == 10)
+											break;
+
+									}
+								}
+								else{
+									rx0iqkok = false;
+									cal_retry++;
+									if (cal_retry == 10)
+										break;
+								}
+							}
+
+						}
+						if (k == 3){
+							rx_x0[cal] = vdf_x[k-1] ;
+							rx_y0[cal] = vdf_y[k-1];
+						}
+						rtl_set_bbreg(hw, 0xce8, BIT(31), 0x1);    // TX VDF Enable
+					}
+
+					else{
+						//====== RX mode TXK (RXK Step 1) ======
+						rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); // [31] = 0 --> Page C
+						// 1. TX RF Setting
+						rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x80000);
+						rtl_set_rfreg(hw, path, 0x30, RFREG_OFFSET_MASK, 0x30000);
+						rtl_set_rfreg(hw, path, 0x31, RFREG_OFFSET_MASK, 0x00029);
+						rtl_set_rfreg(hw, path, 0x32, RFREG_OFFSET_MASK, 0xd7ffb);
+						rtl_set_rfreg(hw, path, 0x65, RFREG_OFFSET_MASK, temp_reg65);
+						rtl_set_rfreg(hw, path, 0x8f, RFREG_OFFSET_MASK, 0x8a001);
+						rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x00000);
+						rtl_write_dword(rtlpriv, 0x90c, 0x00008000);
+						rtl_write_dword(rtlpriv, 0xb00, 0x03000100);
+						rtl_write_dword(rtlpriv, 0x984, 0x0046a910);// [0]:AGC_en, [15]:idac_K_Mask
+
+						rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); // [31] = 1 --> Page C1
+						rtl_write_dword(rtlpriv, 0xc80, 0x18008c10);// TX_Tone_idx[9:0], TxK_Mask[29] TX_Tone = 16
+						rtl_write_dword(rtlpriv, 0xc84, 0x38008c10);// RX_Tone_idx[9:0], RxK_Mask[29]
+						rtl_write_dword(rtlpriv, 0xc88, 0x821603e0);
+						//ODM_Write4Byte(pDM_Odm, 0xc8c, 0x68163e96);
+						rtl_write_dword(rtlpriv, 0xcb8, 0x00100000);// cb8[20] ±N SI/PI ¨Ï¥ÎÅv¤Áµ¹ iqk_dpk module
+						cal_retry = 0;
+						while(1){
+							// one shot
+							rtl_write_dword(rtlpriv, 0x980, 0xfa000000);
+							rtl_write_dword(rtlpriv, 0x980, 0xf8000000);
+
+							mdelay(10); //Delay 10ms
+							rtl_write_dword(rtlpriv, 0xcb8, 0x00000000);
+							delay_count = 0;
+							while (1){
+								iqk_ready = rtl_get_bbreg(hw, 0xd00, BIT(10));
+								if ((~iqk_ready)||(delay_count>20)){
+									break;
+								}
+								else{
+									mdelay(1);
+									delay_count++;
+								}
+							}
+
+							if (delay_count < 20){							// If 20ms No Result, then cal_retry++
+								// ============TXIQK Check==============
+								tx_fail = rtl_get_bbreg(hw, 0xd00, BIT(12));
+
+								if (~tx_fail){
+									rtl_write_dword(rtlpriv, 0xcb8, 0x02000000);
+									tx_x0_rxk[cal] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
+									rtl_write_dword(rtlpriv, 0xcb8, 0x04000000);
+									tx_y0_rxk[cal] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
+									tx0iqkok = true;
+									break;
+								}
+								else{
+									tx0iqkok = false;
+									cal_retry++;
+									if (cal_retry == 10)
+										break;
+								}
+							}
+							else{
+								tx0iqkok = false;
+								cal_retry++;
+								if (cal_retry == 10)
+									break;
+							}
+						}
+
+
+						if (tx0iqkok == false){   //If RX mode TXK fail, then take TXK Result
+							tx_x0_rxk[cal] = tx_x0[cal];
+							tx_y0_rxk[cal] = tx_y0[cal];
+							tx0iqkok = true;
+							RT_TRACE(COMP_IQK, DBG_LOUD, ("1"));
+						}
+
+
+						//====== RX IQK ======
+						rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); // [31] = 0 --> Page C
+						// 1. RX RF Setting
+						rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x80000);
+						rtl_set_rfreg(hw, path, 0x30, RFREG_OFFSET_MASK, 0x30000);
+						rtl_set_rfreg(hw, path, 0x31, RFREG_OFFSET_MASK, 0x0002f);
+						rtl_set_rfreg(hw, path, 0x32, RFREG_OFFSET_MASK, 0xfffbb);
+						rtl_set_rfreg(hw, path, 0x8f, RFREG_OFFSET_MASK, 0x88001);
+						rtl_set_rfreg(hw, path, 0x65, RFREG_OFFSET_MASK, 0x931d8);
+						rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x00000);
+
+						rtl_set_bbreg(hw, 0x978, 0x03FF8000, (tx_x0_rxk[cal])>>21&0x000007ff);
+						rtl_set_bbreg(hw, 0x978, 0x000007FF, (tx_y0_rxk[cal])>>21&0x000007ff);
+						rtl_set_bbreg(hw, 0x978, BIT(31), 0x1);
+						rtl_set_bbreg(hw, 0x97c, BIT(31), 0x0);
+						//ODM_SetBBReg(pDM_Odm, 0xcb8, 0xF, 0xe);
+						rtl_write_dword(rtlpriv, 0x90c, 0x00008000);
+						rtl_write_dword(rtlpriv, 0x984, 0x0046a911);
+
+						rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); // [31] = 1 --> Page C1
+						rtl_write_dword(rtlpriv, 0xc80, 0x38008c10);// TX_Tone_idx[9:0], TxK_Mask[29] TX_Tone = 16
+						rtl_write_dword(rtlpriv, 0xc84, 0x18008c10);// RX_Tone_idx[9:0], RxK_Mask[29]
+						rtl_write_dword(rtlpriv, 0xc88, 0x02140119);
+
+						rtl_write_dword(rtlpriv, 0xc8c, 0x28160d00); /*pDM_Odm->SupportInterface == 1*/
+
+						rtl_write_dword(rtlpriv, 0xcb8, 0x00100000);// cb8[20] ±N SI/PI ¨Ï¥ÎÅv¤Áµ¹ iqk_dpk module
+
+						cal_retry = 0;
+						while(1){
+							// one shot
+							rtl_write_dword(rtlpriv, 0x980, 0xfa000000);
+							rtl_write_dword(rtlpriv, 0x980, 0xf8000000);
+
+							mdelay(10); //Delay 10ms
+							rtl_write_dword(rtlpriv, 0xcb8, 0x00000000);
+							delay_count = 0;
+							while (1){
+								iqk_ready = rtl_get_bbreg(hw, 0xd00, BIT(10));
+								if ((~iqk_ready)||(delay_count>20)){
+									break;
+								}
+								else{
+									mdelay(1);
+									delay_count++;
+								}
+							}
+
+							if (delay_count < 20){	// If 20ms No Result, then cal_retry++
+								// ============RXIQK Check==============
+								rx_fail = rtl_get_bbreg(hw, 0xd00, BIT(11));
+								if (rx_fail == 0){
+									/*
+									   ODM_Write4Byte(pDM_Odm, 0xcb8, 0x05000000);
+									   reg1 = ODM_GetBBReg(pDM_Odm, 0xd00, 0xffffffff);
+									   ODM_Write4Byte(pDM_Odm, 0xcb8, 0x06000000);
+									   reg2 = ODM_GetBBReg(pDM_Odm, 0xd00, 0x0000001f);
+									   DbgPrint("reg1 = %d, reg2 = %d", reg1, reg2);
+									   Image_Power = (reg2<<32)+reg1;
+									   DbgPrint("Before PW = %d\n", Image_Power);
+									   ODM_Write4Byte(pDM_Odm, 0xcb8, 0x07000000);
+									   reg1 = ODM_GetBBReg(pDM_Odm, 0xd00, 0xffffffff);
+									   ODM_Write4Byte(pDM_Odm, 0xcb8, 0x08000000);
+									   reg2 = ODM_GetBBReg(pDM_Odm, 0xd00, 0x0000001f);
+									   Image_Power = (reg2<<32)+reg1;
+									   DbgPrint("After PW = %d\n", Image_Power);
+									   */
+
+									rtl_write_dword(rtlpriv, 0xcb8, 0x06000000);
+									rx_x0[cal] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
+									rtl_write_dword(rtlpriv, 0xcb8, 0x08000000);
+									rx_y0[cal] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
+									rx0iqkok = true;
+									break;
+								}
+								else{
+									rtl_set_bbreg(hw, 0xc10, 0x000003ff, 0x200>>1);
+									rtl_set_bbreg(hw, 0xc10, 0x03ff0000, 0x0>>1);
+									rx0iqkok = false;
+									cal_retry++;
+									if (cal_retry == 10)
+										break;
+
+								}
+							}
+							else{
+								rx0iqkok = false;
+								cal_retry++;
+								if (cal_retry == 10)
+									break;
+							}
+						}
+					}
+
+					if (tx0iqkok)
+						tx_average++;
+					if (rx0iqkok)
+						rx_average++;
+					rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); // [31] = 0 --> Page C
+					rtl_set_rfreg(hw, path, 0x65, RFREG_OFFSET_MASK, temp_reg65);
+				}
+				break;
+			default:
+				break;
+		}
+		cal++;
+	}
+
+	// FillIQK Result
+	switch (path){
+		case RF90_PATH_A:
+			{
+				RT_TRACE(COMP_IQK, DBG_LOUD, ("========Path_A =======\n"));
+				if (tx_average == 0)
+					break;
+
+				for (i = 0; i < tx_average; i++){
+					RT_TRACE(COMP_IQK, DBG_LOUD, (" TX_X0_RXK[%d] = %x ;; TX_Y0_RXK[%d] = %x\n", i, (tx_x0_rxk[i])>>21&0x000007ff, i, (tx_y0_rxk[i])>>21&0x000007ff));
+					RT_TRACE(COMP_IQK, DBG_LOUD, ("TX_X0[%d] = %x ;; TX_Y0[%d] = %x\n", i, (tx_x0[i])>>21&0x000007ff, i, (tx_y0[i])>>21&0x000007ff));
+				}
+				for (i = 0; i < tx_average; i++){
+					for (ii = i+1; ii <tx_average; ii++){
+						dx = (tx_x0[i]>>21) - (tx_x0[ii]>>21);
+						if (dx < 3 && dx > -3){
+							dy = (tx_y0[i]>>21) - (tx_y0[ii]>>21);
+							if (dy < 3 && dy > -3){
+								tx_x = ((tx_x0[i]>>21) + (tx_x0[ii]>>21))/2;
+								tx_y = ((tx_y0[i]>>21) + (tx_y0[ii]>>21))/2;
+								tx_finish = 1;
+								break;
+							}
+						}
+					}
+					if (tx_finish == 1)
+						break;
+				}
+
+				if (tx_finish == 1){
+					_rtl8821ae_iqk_tx_fill_iqc(hw, path, tx_x, tx_y); // ?
+				}
+				else{
+					_rtl8821ae_iqk_tx_fill_iqc(hw, path, 0x200, 0x0);
+				}
+
+				if (rx_average == 0)
+					break;
+
+				for (i = 0; i < rx_average; i++){
+					RT_TRACE(COMP_IQK, DBG_LOUD, ("RX_X0[%d] = %x ;; RX_Y0[%d] = %x\n", i, (rx_x0[i])>>21&0x000007ff, i, (rx_y0[i])>>21&0x000007ff));
+				}
+				for (i = 0; i < rx_average; i++){
+					for (ii = i+1; ii <rx_average; ii++){
+						dx = (rx_x0[i]>>21) - (rx_x0[ii]>>21);
+						if (dx < 4 && dx > -4){
+							dy = (rx_y0[i]>>21) - (rx_y0[ii]>>21);
+							if (dy < 4 && dy > -4){
+								rx_x = ((rx_x0[i]>>21) + (rx_x0[ii]>>21))/2;
+								rx_y = ((rx_y0[i]>>21) + (rx_y0[ii]>>21))/2;
+								rx_finish = 1;
+								break;
+							}
+						}
+					}
+					if (rx_finish == 1)
+						break;
+				}
+
+				if (rx_finish == 1){
+					_rtl8821ae_iqk_rx_fill_iqc(hw, path, rx_x, rx_y);
+				}
+				else{
+					_rtl8821ae_iqk_rx_fill_iqc(hw, path, 0x200, 0x0);
+				}
+			}
+			break;
+		default:
+			break;
+	}
+}
+
+void _rtl8821ae_iqk_restore_rf(
+		struct ieee80211_hw *hw,
+		enum radio_path path,
+		u32*			backup_rf_reg,
+		u32* 			rf_backup,
+		u32			rf_reg_num
+		)
+{
+	u32 i;
+	struct rtl_priv* rtlpriv = rtl_priv(hw);
+
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); // [31] = 0 --> Page C
+	for (i = 0; i < RF_REG_NUM; i++)
+		rtl_set_rfreg(hw, path, backup_rf_reg[i], RFREG_OFFSET_MASK, rf_backup[i]);
+
+	switch(path){
+		case RF90_PATH_A:
+			{
+				RT_TRACE(COMP_IQK, DBG_LOUD, ("RestoreRF Path A Success!!!!\n"));
+			}
+			break;
+		default:
+			break;
+	}
+}
+
+void _rtl8821ae_iqk_restore_afe(
+		struct ieee80211_hw *hw,
+		u32*		afe_backup,
+		u32*		backup_afe_reg,
+		u32		afe_num
+		)
+{
+	u32 i;
+	struct rtl_priv* rtlpriv = rtl_priv(hw);
+
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); // [31] = 0 --> Page C
+	//Reload AFE Parameters
+	for (i = 0; i < afe_num; i++){
+		rtl_write_dword(rtlpriv, backup_afe_reg[i], afe_backup[i]);
+	}
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); // [31] = 1 --> Page C1
+	rtl_write_dword(rtlpriv, 0xc80, 0x0);
+	rtl_write_dword(rtlpriv, 0xc84, 0x0);
+	rtl_write_dword(rtlpriv, 0xc88, 0x0);
+	rtl_write_dword(rtlpriv, 0xc8c, 0x3c000000);
+	rtl_write_dword(rtlpriv, 0xc90, 0x00000080);
+	rtl_write_dword(rtlpriv, 0xc94, 0x00000000);
+	rtl_write_dword(rtlpriv, 0xcc4, 0x20040000);
+	rtl_write_dword(rtlpriv, 0xcc8, 0x20000000);
+	rtl_write_dword(rtlpriv, 0xcb8, 0x0);
+	RT_TRACE(COMP_IQK, DBG_LOUD, ("RestoreAFE Success!!!!\n"));
+}
+
+void _rtl8821ae_iqk_restore_macbb(
+		struct ieee80211_hw *hw,
+		u32*		macbb_backup,
+		u32*		backup_macbb_reg,
+		u32		macbb_num
+		)
+{
+	u32 i;
+	struct rtl_priv* rtlpriv = rtl_priv(hw);
+
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); // [31] = 0 --> Page C
+	//Reload MacBB Parameters
+	for (i = 0; i < macbb_num; i++){
+		rtl_write_dword(rtlpriv, backup_macbb_reg[i], macbb_backup[i]);
+	}
+	RT_TRACE(COMP_IQK, DBG_LOUD, ("RestoreMacBB Success!!!!\n"));
+}
+
+
+#undef MACBB_REG_NUM
+#undef AFE_REG_NUM
+#undef RF_REG_NUM
+
+#define MACBB_REG_NUM 11
+#define AFE_REG_NUM 12
+#define RF_REG_NUM 3
+
+static void _rtl8821ae_phy_iq_calibrate(struct ieee80211_hw *hw)
+{
+	u32	macbb_backup[MACBB_REG_NUM];
+	u32 afe_backup[AFE_REG_NUM];
+	u32 rfa_backup[RF_REG_NUM];
+	u32 rfb_backup[RF_REG_NUM];
+	u32 	backup_macbb_reg[MACBB_REG_NUM] = {0xb00, 0x520, 0x550, 0x808, 0x90c, 0xc00, 0xc50,
+							0xe00, 0xe50, 0x838, 0x82c};
+	u32	backup_afe_reg[AFE_REG_NUM] = {0xc5c, 0xc60, 0xc64, 0xc68, 0xc6c, 0xc70, 0xc74,
+							0xc78, 0xc7c, 0xc80, 0xc84, 0xcb8};
+	u32 	backup_rf_reg[RF_REG_NUM] = {0x65, 0x8f, 0x0};
+
+	_rtl8821ae_iqk_backup_macbb(hw, macbb_backup, backup_macbb_reg, MACBB_REG_NUM);
+	_rtl8821ae_iqk_backup_afe(hw, afe_backup, backup_afe_reg, AFE_REG_NUM);
+	_rtl8821ae_iqk_backup_rf(hw, rfa_backup, rfb_backup, backup_rf_reg, RF_REG_NUM);
+
+	_rtl8821ae_iqk_configure_mac(hw);
+	_rtl8821ae_iqk_tx(hw, RF90_PATH_A);
+	_rtl8821ae_iqk_restore_rf(hw, RF90_PATH_A, backup_rf_reg, rfa_backup, RF_REG_NUM);
+
+	_rtl8821ae_iqk_restore_afe(hw, afe_backup, backup_afe_reg, AFE_REG_NUM);
+	_rtl8821ae_iqk_restore_macbb(hw, macbb_backup, backup_macbb_reg, MACBB_REG_NUM);
+}
+
+static void _rtl8821ae_phy_lc_calibrate(struct ieee80211_hw *hw, bool is2t)
+{
+	u8 tmpreg;
+	u32 rf_a_mode = 0, rf_b_mode = 0, lc_cal;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	tmpreg = rtl_read_byte(rtlpriv, 0xd03);
+
+	if ((tmpreg & 0x70) != 0)
+		rtl_write_byte(rtlpriv, 0xd03, tmpreg & 0x8F);
+	else
+		rtl_write_byte(rtlpriv, REG_TXPAUSE, 0xFF);
+
+	if ((tmpreg & 0x70) != 0) {
+		rf_a_mode = rtl_get_rfreg(hw, RF90_PATH_A, 0x00, MASK12BITS);
+
+		if (is2t)
+			rf_b_mode = rtl_get_rfreg(hw, RF90_PATH_B, 0x00,
+						  MASK12BITS);
+
+		rtl_set_rfreg(hw, RF90_PATH_A, 0x00, MASK12BITS,
+			      (rf_a_mode & 0x8FFFF) | 0x10000);
+
+		if (is2t)
+			rtl_set_rfreg(hw, RF90_PATH_B, 0x00, MASK12BITS,
+				      (rf_b_mode & 0x8FFFF) | 0x10000);
+	}
+	lc_cal = rtl_get_rfreg(hw, RF90_PATH_A, 0x18, MASK12BITS);
+
+	rtl_set_rfreg(hw, RF90_PATH_A, 0xb0, RFREG_OFFSET_MASK, 0xdfbe0);
+	/* rtl_set_rfreg(hw, RF90_PATH_A, 0x18, MASK12BITS, lc_cal | 0x08000); */
+	rtl_set_rfreg(hw, RF90_PATH_A, 0x18, MASK12BITS, 0x8c0a);
+
+	mdelay(100);
+
+	rtl_set_rfreg(hw, RF90_PATH_A, 0xb0, RFREG_OFFSET_MASK, 0xdffe0);
+
+	if ((tmpreg & 0x70) != 0) {
+		rtl_write_byte(rtlpriv, 0xd03, tmpreg);
+		rtl_set_rfreg(hw, RF90_PATH_A, 0x00, MASK12BITS, rf_a_mode);
+
+		if (is2t)
+			rtl_set_rfreg(hw, RF90_PATH_B, 0x00, MASK12BITS, rf_b_mode);
+	} else {
+		rtl_write_byte(rtlpriv, REG_TXPAUSE, 0x00);
+	}
+RT_TRACE(COMP_INIT,DBG_LOUD,("\n"));
+
+}
+
+static void _rtl8821ae_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool main)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	//struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	//struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	RT_TRACE(COMP_INIT,DBG_LOUD,("\n"));
+
+	if (main)
+		rtl_set_bbreg(hw, RA_RFE_PINMUX + 4, BIT(29) | BIT(28), 0x1);
+	else
+		rtl_set_bbreg(hw, RA_RFE_PINMUX + 4, BIT(29) | BIT(28), 0x2);
+}
+
+#undef IQK_ADDA_REG_NUM
+#undef IQK_DELAY_TIME
+
+void rtl8812ae_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	if (!rtlphy->b_iqk_in_progress)
+	{
+		spin_lock(&rtlpriv->locks.iqk_lock);
+		rtlphy->b_iqk_in_progress = true;
+		spin_unlock(&rtlpriv->locks.iqk_lock);
+
+		_rtl8812ae_phy_iq_calibrate(hw, rtlphy->current_channel);
+
+		spin_lock(&rtlpriv->locks.iqk_lock);
+		rtlphy->b_iqk_in_progress = false;
+		spin_unlock(&rtlpriv->locks.iqk_lock);
+	}
+}
+
+void rtl8812ae_reset_iqk_result(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 i;
+
+	RT_TRACE(COMP_IQK, DBG_LOUD,
+		("rtl8812ae_dm_reset_iqk_result:: settings regs %d default regs %d\n",
+		(int)(sizeof(rtlphy->iqk_matrix_regsetting) /
+		sizeof(struct iqk_matrix_regs)),
+		IQK_MATRIX_SETTINGS_NUM));
+
+	for(i = 0; i < IQK_MATRIX_SETTINGS_NUM; i++) {
+		{
+			rtlphy->iqk_matrix_regsetting[i].value[0][0] =
+				rtlphy->iqk_matrix_regsetting[i].value[0][2] =
+				rtlphy->iqk_matrix_regsetting[i].value[0][4] =
+				rtlphy->iqk_matrix_regsetting[i].value[0][6] = 0x100;
+
+			rtlphy->iqk_matrix_regsetting[i].value[0][1] =
+				rtlphy->iqk_matrix_regsetting[i].value[0][3] =
+				rtlphy->iqk_matrix_regsetting[i].value[0][5] =
+				rtlphy->iqk_matrix_regsetting[i].value[0][7] = 0x0;
+
+			rtlphy->iqk_matrix_regsetting[i].b_iqk_done = false;
+
+		}
+	}
+}
+
+void rtl8812ae_do_iqk(struct ieee80211_hw *hw,u8 delta_thermal_index,
+	u8 thermal_value, u8 threshold)
+{
+	struct rtl_dm	*rtldm = rtl_dm(rtl_priv(hw));
+
+	rtl8812ae_reset_iqk_result(hw);
+
+	rtldm->thermalvalue_iqk= thermal_value;
+	rtl8812ae_phy_iq_calibrate(hw, false);
+}
+
+void rtl8821ae_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	if (!rtlphy->b_iqk_in_progress)
+	{
+		spin_lock(&rtlpriv->locks.iqk_lock);
+		rtlphy->b_iqk_in_progress = true;
+		spin_unlock(&rtlpriv->locks.iqk_lock);
+
+		_rtl8821ae_phy_iq_calibrate(hw);
+
+		spin_lock(&rtlpriv->locks.iqk_lock);
+		rtlphy->b_iqk_in_progress = false;
+		spin_unlock(&rtlpriv->locks.iqk_lock);
+	}
+}
+
+void rtl8821ae_reset_iqk_result(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 i;
+
+	RT_TRACE(COMP_IQK, DBG_LOUD,
+		("rtl8812ae_dm_reset_iqk_result:: settings regs %d default regs %d\n",
+		(int)(sizeof(rtlphy->iqk_matrix_regsetting) /
+		sizeof(struct iqk_matrix_regs)),
+		IQK_MATRIX_SETTINGS_NUM));
+
+	for(i = 0; i < IQK_MATRIX_SETTINGS_NUM; i++) {
+		{
+			rtlphy->iqk_matrix_regsetting[i].value[0][0] =
+				rtlphy->iqk_matrix_regsetting[i].value[0][2] =
+				rtlphy->iqk_matrix_regsetting[i].value[0][4] =
+				rtlphy->iqk_matrix_regsetting[i].value[0][6] = 0x100;
+
+			rtlphy->iqk_matrix_regsetting[i].value[0][1] =
+				rtlphy->iqk_matrix_regsetting[i].value[0][3] =
+				rtlphy->iqk_matrix_regsetting[i].value[0][5] =
+				rtlphy->iqk_matrix_regsetting[i].value[0][7] = 0x0;
+
+			rtlphy->iqk_matrix_regsetting[i].b_iqk_done = false;
+
+		}
+	}
+}
+
+void rtl8821ae_do_iqk(struct ieee80211_hw *hw,u8 delta_thermal_index,
+	u8 thermal_value, u8 threshold)
+{
+	struct rtl_dm	*rtldm = rtl_dm(rtl_priv(hw));
+
+	rtl8821ae_reset_iqk_result(hw);
+
+	rtldm->thermalvalue_iqk= thermal_value;
+	rtl8821ae_phy_iq_calibrate(hw, false);
+}
+
+void rtl8821ae_phy_lc_calibrate(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_hal *rtlhal = &(rtlpriv->rtlhal);
+	u32 timeout = 2000, timecount = 0;
+
+
+	while (rtlpriv->mac80211.act_scanning && timecount < timeout) {
+		udelay(50);
+		timecount += 50;
+	}
+
+	rtlphy->lck_inprogress = true;
+	RTPRINT(rtlpriv, FINIT, INIT_IQK,
+		("LCK:Start!!! currentband %x delay %d ms\n",
+		 rtlhal->current_bandtype, timecount));
+
+	_rtl8821ae_phy_lc_calibrate(hw, false);
+
+	rtlphy->lck_inprogress = false;
+}
+
+void rtl8821ae_phy_ap_calibrate(struct ieee80211_hw *hw, char delta)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	if (rtlphy->b_apk_done)
+		return;
+
+	return;
+}
+
+void rtl8821ae_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool bmain)
+{
+	_rtl8821ae_phy_set_rfpath_switch(hw, bmain);
+}
+
+bool rtl8821ae_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	bool b_postprocessing = false;
+
+	RT_TRACE(COMP_CMD, DBG_TRACE,
+		 ("-->IO Cmd(%#x), set_io_inprogress(%d)\n",
+		  iotype, rtlphy->set_io_inprogress));
+	do {
+		switch (iotype) {
+		case IO_CMD_RESUME_DM_BY_SCAN:
+			RT_TRACE(COMP_CMD, DBG_TRACE,
+				 ("[IO CMD] Resume DM after scan.\n"));
+			b_postprocessing = true;
+			break;
+		case IO_CMD_PAUSE_BAND0_DM_BY_SCAN:
+		case IO_CMD_PAUSE_BAND1_DM_BY_SCAN:
+			RT_TRACE(COMP_CMD, DBG_TRACE,
+				 ("[IO CMD] Pause DM before scan.\n"));
+			b_postprocessing = true;
+			break;
+		default:
+			RT_TRACE(COMP_ERR, DBG_EMERG,
+				 ("switch case not process \n"));
+			break;
+		}
+	} while (false);
+	if (b_postprocessing && !rtlphy->set_io_inprogress) {
+		rtlphy->set_io_inprogress = true;
+		rtlphy->current_io_type = iotype;
+	} else {
+		return false;
+	}
+	rtl8821ae_phy_set_io(hw);
+	RT_TRACE(COMP_CMD, DBG_TRACE, ("IO Type(%#x)\n", iotype));
+	return true;
+}
+
+static void rtl8821ae_phy_set_io(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	RT_TRACE(COMP_CMD, DBG_TRACE,
+		 ("--->Cmd(%#x), set_io_inprogress(%d)\n",
+		  rtlphy->current_io_type, rtlphy->set_io_inprogress));
+	switch (rtlphy->current_io_type) {
+	case IO_CMD_RESUME_DM_BY_SCAN:
+		if (rtlpriv->mac80211.opmode== NL80211_IFTYPE_ADHOC)
+			_rtl8821ae_resume_tx_beacon(hw);
+		rtl8821ae_dm_write_dig(hw, rtlphy->initgain_backup.xaagccore1);
+		rtl8821ae_dm_write_cck_cca_thres(hw, rtlphy->initgain_backup.cca);
+		break;
+	case IO_CMD_PAUSE_BAND0_DM_BY_SCAN:
+		if (rtlpriv->mac80211.opmode== NL80211_IFTYPE_ADHOC)
+			_rtl8821ae_stop_tx_beacon(hw);
+		rtlphy->initgain_backup.xaagccore1 = dm_digtable.cur_igvalue;
+		rtl8821ae_dm_write_dig(hw, 0x17);
+		rtlphy->initgain_backup.cca = dm_digtable.cur_cck_cca_thres;
+		rtl8821ae_dm_write_cck_cca_thres(hw, 0x40);
+		break;
+	case IO_CMD_PAUSE_BAND1_DM_BY_SCAN:
+		break;
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("switch case not process \n"));
+		break;
+	}
+	rtlphy->set_io_inprogress = false;
+	RT_TRACE(COMP_CMD, DBG_TRACE,
+		 ("(%#x)\n", rtlphy->current_io_type));
+}
+
+static void rtl8821ae_phy_set_rf_on(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtl_write_byte(rtlpriv, REG_SPS0_CTRL, 0x2b);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE3);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE2);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE3);
+	rtl_write_byte(rtlpriv, REG_TXPAUSE, 0x00);
+}
+
+#if 0
+static void _rtl8821ae_phy_set_rf_sleep(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtl_write_byte(rtlpriv, REG_TXPAUSE, 0xFF);
+	rtl_set_rfreg(hw, RF90_PATH_A, 0x00, RFREG_OFFSET_MASK, 0x00);
+	/*rtl_write_byte(rtlpriv, REG_APSD_CTRL, 0x40);
+	u4b_tmp = rtl_get_rfreg(hw, RF90_PATH_A, 0, RFREG_OFFSET_MASK);
+	while (u4b_tmp != 0 && delay > 0) {
+		rtl_write_byte(rtlpriv, REG_APSD_CTRL, 0x0);
+		rtl_set_rfreg(hw, RF90_PATH_A, 0x00, RFREG_OFFSET_MASK, 0x00);
+		rtl_write_byte(rtlpriv, REG_APSD_CTRL, 0x40);
+		u4b_tmp = rtl_get_rfreg(hw, RF90_PATH_A, 0, RFREG_OFFSET_MASK);
+		delay--;
+	}
+	if (delay == 0) {
+		rtl_write_byte(rtlpriv, REG_APSD_CTRL, 0x00);
+		rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE2);
+		rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE3);
+		rtl_write_byte(rtlpriv, REG_TXPAUSE, 0x00);
+		RT_TRACE(COMP_POWER, DBG_TRACE,
+			 ("Switch RF timeout !!!.\n"));
+		return;
+	}*/
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE2);
+	rtl_write_byte(rtlpriv, REG_SPS0_CTRL, 0x22);
+}
+#endif
+
+static bool _rtl8821ae_phy_set_rf_power_state(struct ieee80211_hw *hw,
+					    enum rf_pwrstate rfpwr_state)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	bool bresult = true;
+	u8 i, queue_id;
+	struct rtl8192_tx_ring *ring = NULL;
+
+	switch (rfpwr_state) {
+	case ERFON:{
+			if ((ppsc->rfpwr_state == ERFOFF) &&
+			    RT_IN_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC)) {
+				bool rtstatus = false;
+				u32 InitializeCount = 0;
+				do {
+					InitializeCount++;
+					RT_TRACE(COMP_RF, DBG_DMESG,
+						 ("IPS Set eRf nic enable\n"));
+					rtstatus = rtl_ps_enable_nic(hw);
+				} while ((rtstatus != true)
+					 && (InitializeCount < 10));
+				RT_CLEAR_PS_LEVEL(ppsc,
+						  RT_RF_OFF_LEVL_HALT_NIC);
+			} else {
+				RT_TRACE(COMP_RF, DBG_DMESG,
+					 ("Set ERFON sleeped:%d ms\n",
+					  jiffies_to_msecs(jiffies -
+							   ppsc->
+							   last_sleep_jiffies)));
+				ppsc->last_awake_jiffies = jiffies;
+				rtl8821ae_phy_set_rf_on(hw);
+			}
+			if (mac->link_state == MAC80211_LINKED) {
+				rtlpriv->cfg->ops->led_control(hw,
+							       LED_CTL_LINK);
+			} else {
+				rtlpriv->cfg->ops->led_control(hw,
+							       LED_CTL_NO_LINK);
+			}
+			break;
+		}
+	case ERFOFF:{
+			for (queue_id = 0, i = 0;
+			     queue_id < RTL_PCI_MAX_TX_QUEUE_COUNT;) {
+				ring = &pcipriv->dev.tx_ring[queue_id];
+				if (skb_queue_len(&ring->queue) == 0) {
+					queue_id++;
+					continue;
+				} else {
+					RT_TRACE(COMP_ERR, DBG_WARNING,
+						 ("eRf Off/Sleep: %d times "
+						  "TcbBusyQueue[%d] =%d before "
+						  "doze!\n", (i + 1), queue_id,
+						  skb_queue_len(&ring->queue)));
+
+					udelay(10);
+					i++;
+				}
+				if (i >= MAX_DOZE_WAITING_TIMES_9x) {
+					RT_TRACE(COMP_ERR, DBG_WARNING,
+						 ("\n ERFSLEEP: %d times "
+						  "TcbBusyQueue[%d] = %d !\n",
+						  MAX_DOZE_WAITING_TIMES_9x,
+						  queue_id,
+						  skb_queue_len(&ring->queue)));
+					break;
+				}
+			}
+
+			if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_HALT_NIC) {
+				RT_TRACE(COMP_RF, DBG_DMESG,
+					 ("IPS Set eRf nic disable\n"));
+				rtl_ps_disable_nic(hw);
+				RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
+			} else {
+				if (ppsc->rfoff_reason == RF_CHANGE_BY_IPS) {
+					rtlpriv->cfg->ops->led_control(hw,
+								       LED_CTL_NO_LINK);
+				} else {
+					rtlpriv->cfg->ops->led_control(hw,
+								       LED_CTL_POWER_OFF);
+				}
+			}
+			break;
+		}
+	/*case ERFSLEEP:{
+			if (ppsc->rfpwr_state == ERFOFF)
+				break;
+			for (queue_id = 0, i = 0;
+			     queue_id < RTL_PCI_MAX_TX_QUEUE_COUNT;) {
+				ring = &pcipriv->dev.tx_ring[queue_id];
+				if (skb_queue_len(&ring->queue) == 0) {
+					queue_id++;
+					continue;
+				} else {
+					RT_TRACE(COMP_ERR, DBG_WARNING,
+						 ("eRf Off/Sleep: %d times "
+						  "TcbBusyQueue[%d] =%d before "
+						  "doze!\n", (i + 1), queue_id,
+						  skb_queue_len(&ring->queue)));
+
+					udelay(10);
+					i++;
+				}
+				if (i >= MAX_DOZE_WAITING_TIMES_9x) {
+					RT_TRACE(COMP_ERR, DBG_WARNING,
+						 ("\n ERFSLEEP: %d times "
+						  "TcbBusyQueue[%d] = %d !\n",
+						  MAX_DOZE_WAITING_TIMES_9x,
+						  queue_id,
+						  skb_queue_len(&ring->queue)));
+					break;
+				}
+			}
+			RT_TRACE(COMP_RF, DBG_DMESG,
+				 ("Set ERFSLEEP awaked:%d ms\n",
+				  jiffies_to_msecs(jiffies -
+						   ppsc->last_awake_jiffies)));
+			ppsc->last_sleep_jiffies = jiffies;
+			_rtl8821ae_phy_set_rf_sleep(hw);
+			break;
+		}*/
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("switch case not process \n"));
+		bresult = false;
+		break;
+	}
+	if (bresult)
+		ppsc->rfpwr_state = rfpwr_state;
+	return bresult;
+}
+
+bool rtl8821ae_phy_set_rf_power_state(struct ieee80211_hw *hw,
+				   enum rf_pwrstate rfpwr_state)
+{
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+
+	bool bresult = false;
+
+	if (rfpwr_state == ppsc->rfpwr_state)
+		return bresult;
+	bresult = _rtl8821ae_phy_set_rf_power_state(hw, rfpwr_state);
+	return bresult;
+}
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/phy.h b/drivers/staging/rtl8821ae/rtl8821ae/phy.h
new file mode 100644
index 000000000000..a932d8c9d45d
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/phy.h
@@ -0,0 +1,258 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL8821AE_PHY_H__
+#define __RTL8821AE_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_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 IQK_ADDA_REG_NUM			16
+#define IQK_MAC_REG_NUM				4
+
+#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 RTL8821AE_MAX_PATH_NUM					2
+
+#define TARGET_CHNL_NUM_2G_5G_8812	59
+
+enum swchnlcmd_id {
+	CMDID_END,
+	CMDID_SET_TXPOWEROWER_LEVEL,
+	CMDID_BBREGWRITE10,
+	CMDID_WRITEPORT_ULONG,
+	CMDID_WRITEPORT_USHORT,
+	CMDID_WRITEPORT_UCHAR,
+	CMDID_RF_WRITEREG,
+};
+
+struct swchnlcmd {
+	enum swchnlcmd_id cmdid;
+	u32 para1;
+	u32 para2;
+	u32 msdelay;
+};
+
+enum hw90_block_e {
+	HW90_BLOCK_MAC = 0,
+	HW90_BLOCK_PHY0 = 1,
+	HW90_BLOCK_PHY1 = 2,
+	HW90_BLOCK_RF = 3,
+	HW90_BLOCK_MAXIMUM = 4,
+};
+
+enum baseband_config_type {
+	BASEBAND_CONFIG_PHY_REG = 0,
+	BASEBAND_CONFIG_AGC_TAB = 1,
+};
+
+enum ra_offset_area {
+	RA_OFFSET_LEGACY_OFDM1,
+	RA_OFFSET_LEGACY_OFDM2,
+	RA_OFFSET_HT_OFDM1,
+	RA_OFFSET_HT_OFDM2,
+	RA_OFFSET_HT_OFDM3,
+	RA_OFFSET_HT_OFDM4,
+	RA_OFFSET_HT_CCK,
+};
+
+enum antenna_path {
+	ANTENNA_NONE,
+	ANTENNA_D,
+	ANTENNA_C,
+	ANTENNA_CD,
+	ANTENNA_B,
+	ANTENNA_BD,
+	ANTENNA_BC,
+	ANTENNA_BCD,
+	ANTENNA_A,
+	ANTENNA_AD,
+	ANTENNA_AC,
+	ANTENNA_ACD,
+	ANTENNA_AB,
+	ANTENNA_ABD,
+	ANTENNA_ABC,
+	ANTENNA_ABCD
+};
+
+struct r_antenna_select_ofdm {
+	u32 r_tx_antenna:4;
+	u32 r_ant_l:4;
+	u32 r_ant_non_ht:4;
+	u32 r_ant_ht1:4;
+	u32 r_ant_ht2:4;
+	u32 r_ant_ht_s1:4;
+	u32 r_ant_non_ht_s1:4;
+	u32 ofdm_txsc:2;
+	u32 reserved:2;
+};
+
+struct r_antenna_select_cck {
+	u8 r_cckrx_enable_2:2;
+	u8 r_cckrx_enable:2;
+	u8 r_ccktx_enable:4;
+};
+
+
+struct efuse_contents {
+	u8 mac_addr[ETH_ALEN];
+	u8 cck_tx_power_idx[6];
+	u8 ht40_1s_tx_power_idx[6];
+	u8 ht40_2s_tx_power_idx_diff[3];
+	u8 ht20_tx_power_idx_diff[3];
+	u8 ofdm_tx_power_idx_diff[3];
+	u8 ht40_max_power_offset[3];
+	u8 ht20_max_power_offset[3];
+	u8 channel_plan;
+	u8 thermal_meter;
+	u8 rf_option[5];
+	u8 version;
+	u8 oem_id;
+	u8 regulatory;
+};
+
+struct tx_power_struct {
+	u8 cck[RTL8821AE_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
+	u8 ht40_1s[RTL8821AE_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
+	u8 ht40_2s[RTL8821AE_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
+	u8 ht20_diff[RTL8821AE_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
+	u8 legacy_ht_diff[RTL8821AE_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
+	u8 legacy_ht_txpowerdiff;
+	u8 groupht20[RTL8821AE_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
+	u8 groupht40[RTL8821AE_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
+	u8 pwrgroup_cnt;
+	u32 mcs_original_offset[4][16];
+};
+enum _ANT_DIV_TYPE
+{
+	NO_ANTDIV				= 0xFF,
+	CG_TRX_HW_ANTDIV		= 0x01,
+	CGCS_RX_HW_ANTDIV 		= 0x02,
+	FIXED_HW_ANTDIV         = 0x03,
+	CG_TRX_SMART_ANTDIV		= 0x04,
+	CGCS_RX_SW_ANTDIV		= 0x05,
+
+};
+
+extern u32 rtl8821ae_phy_query_bb_reg(struct ieee80211_hw *hw,
+				   u32 regaddr, u32 bitmask);
+extern void rtl8821ae_phy_set_bb_reg(struct ieee80211_hw *hw,
+				  u32 regaddr, u32 bitmask, u32 data);
+extern u32 rtl8821ae_phy_query_rf_reg(struct ieee80211_hw *hw,
+				   enum radio_path rfpath, u32 regaddr,
+				   u32 bitmask);
+extern void rtl8821ae_phy_set_rf_reg(struct ieee80211_hw *hw,
+				  enum radio_path rfpath, u32 regaddr,
+				  u32 bitmask, u32 data);
+extern bool rtl8821ae_phy_mac_config(struct ieee80211_hw *hw);
+extern bool rtl8821ae_phy_bb_config(struct ieee80211_hw *hw);
+extern bool rtl8821ae_phy_rf_config(struct ieee80211_hw *hw);
+extern void rtl8821ae_phy_switch_wirelessband(struct ieee80211_hw *hw, u8 band);
+extern void rtl8821ae_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw);
+extern void rtl8821ae_phy_get_txpower_level(struct ieee80211_hw *hw,
+					 long *powerlevel);
+extern void rtl8821ae_phy_set_txpower_level(struct ieee80211_hw *hw, u8 channel);
+extern void rtl8821ae_phy_scan_operation_backup(struct ieee80211_hw *hw,
+					     u8 operation);
+extern void rtl8821ae_phy_set_bw_mode_callback(struct ieee80211_hw *hw);
+extern void rtl8821ae_phy_set_bw_mode(struct ieee80211_hw *hw,
+				   enum nl80211_channel_type ch_type);
+extern void rtl8821ae_phy_sw_chnl_callback(struct ieee80211_hw *hw);
+extern u8 rtl8821ae_phy_sw_chnl(struct ieee80211_hw *hw);
+extern void rtl8821ae_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery);
+extern void rtl8812ae_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery);
+void rtl8821ae_phy_ap_calibrate(struct ieee80211_hw *hw, char delta);
+void rtl8821ae_phy_lc_calibrate(struct ieee80211_hw *hw);
+void rtl8821ae_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool bmain);
+bool rtl8812ae_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
+					  enum radio_path rfpath);
+bool rtl8821ae_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
+					  enum radio_path rfpath);
+bool rtl8821ae_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype);
+extern bool rtl8821ae_phy_set_rf_power_state(struct ieee80211_hw *hw,
+					  enum rf_pwrstate rfpwr_state);
+u8 _rtl8812ae_get_right_chnl_place_for_iqk(u8 chnl);
+void rtl8821ae_phy_set_txpower_level_by_path(struct ieee80211_hw *hw, u8 channel, u8 path);
+void rtl8812ae_do_iqk(struct ieee80211_hw *hw,u8 delta_thermal_index,
+	u8 thermal_value, u8 threshold);
+void rtl8821ae_do_iqk(struct ieee80211_hw *hw,u8 delta_thermal_index,
+	u8 thermal_value, u8 threshold);
+void rtl8821ae_reset_iqk_result(struct ieee80211_hw *hw);
+
+
+#endif
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/pwrseq.c b/drivers/staging/rtl8821ae/rtl8821ae/pwrseq.c
new file mode 100644
index 000000000000..a2e4a01b712b
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/pwrseq.c
@@ -0,0 +1,199 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "pwrseqcmd.h"
+#include "pwrseq.h"
+
+/*
+    drivers should parse below arrays and do the corresponding actions
+*/
+//3 Power on  Array
+struct wlan_pwr_cfg rtl8812_power_on_flow[RTL8812_TRANS_CARDEMU_TO_ACT_STEPS+RTL8812_TRANS_END_STEPS]=
+{
+	RTL8812_TRANS_CARDEMU_TO_ACT
+	RTL8812_TRANS_END
+};
+
+//3Radio off GPIO Array
+struct wlan_pwr_cfg rtl8812_radio_off_flow[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS+RTL8812_TRANS_END_STEPS]=
+{
+	RTL8812_TRANS_ACT_TO_CARDEMU
+	RTL8812_TRANS_END
+};
+
+//3Card Disable Array
+struct wlan_pwr_cfg rtl8812_card_disable_flow[ RTL8812_TRANS_ACT_TO_CARDEMU_STEPS
+	+ RTL8812_TRANS_CARDEMU_TO_PDN_STEPS
+	+ RTL8812_TRANS_END_STEPS ] =
+{
+	RTL8812_TRANS_ACT_TO_CARDEMU
+	RTL8812_TRANS_CARDEMU_TO_CARDDIS
+	RTL8812_TRANS_END
+};
+
+//3 Card Enable Array
+struct wlan_pwr_cfg rtl8812_card_enable_flow[ RTL8812_TRANS_ACT_TO_CARDEMU_STEPS
+	+ RTL8812_TRANS_CARDEMU_TO_PDN_STEPS
+	+ RTL8812_TRANS_END_STEPS ] =
+{
+	RTL8812_TRANS_CARDDIS_TO_CARDEMU
+	RTL8812_TRANS_CARDEMU_TO_ACT
+	RTL8812_TRANS_END
+};
+
+//3Suspend Array
+struct wlan_pwr_cfg rtl8812_suspend_flow[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS+RTL8812_TRANS_CARDEMU_TO_SUS_STEPS+RTL8812_TRANS_END_STEPS]=
+{
+	RTL8812_TRANS_ACT_TO_CARDEMU
+	RTL8812_TRANS_CARDEMU_TO_SUS
+	RTL8812_TRANS_END
+};
+
+//3 Resume Array
+struct wlan_pwr_cfg rtl8812_resume_flow[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS+RTL8812_TRANS_CARDEMU_TO_SUS_STEPS+RTL8812_TRANS_END_STEPS]=
+{
+	RTL8812_TRANS_SUS_TO_CARDEMU
+	RTL8812_TRANS_CARDEMU_TO_ACT
+	RTL8812_TRANS_END
+};
+
+
+
+//3HWPDN Array
+struct wlan_pwr_cfg rtl8812_hwpdn_flow[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS+RTL8812_TRANS_CARDEMU_TO_PDN_STEPS+RTL8812_TRANS_END_STEPS]=
+{
+	RTL8812_TRANS_ACT_TO_CARDEMU
+	RTL8812_TRANS_CARDEMU_TO_PDN
+	RTL8812_TRANS_END
+};
+
+//3 Enter LPS
+struct wlan_pwr_cfg rtl8812_enter_lps_flow[RTL8812_TRANS_ACT_TO_LPS_STEPS+RTL8812_TRANS_END_STEPS]=
+{
+	//FW behavior
+	RTL8812_TRANS_ACT_TO_LPS
+	RTL8812_TRANS_END
+};
+
+//3 Leave LPS
+struct wlan_pwr_cfg rtl8812_leave_lps_flow[RTL8812_TRANS_LPS_TO_ACT_STEPS+RTL8812_TRANS_END_STEPS]=
+{
+	//FW behavior
+	RTL8812_TRANS_LPS_TO_ACT
+	RTL8812_TRANS_END
+};
+
+
+/*
+    drivers should parse below arrays and do the corresponding actions
+*/
+/*3 Power on  Array*/
+struct wlan_pwr_cfg rtl8821A_power_on_flow[RTL8821A_TRANS_CARDEMU_TO_ACT_STEPS
+					+ RTL8821A_TRANS_END_STEPS] =
+{
+	RTL8821A_TRANS_CARDEMU_TO_ACT
+	RTL8821A_TRANS_END
+};
+
+/*3Radio off GPIO Array */
+struct wlan_pwr_cfg rtl8821A_radio_off_flow[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
+					+ RTL8821A_TRANS_END_STEPS] =
+{
+	RTL8821A_TRANS_ACT_TO_CARDEMU
+	RTL8821A_TRANS_END
+};
+
+/*3Card Disable Array*/
+struct wlan_pwr_cfg rtl8821A_card_disable_flow[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
+					+ RTL8821A_TRANS_CARDEMU_TO_PDN_STEPS
+					+ RTL8821A_TRANS_END_STEPS] =
+{
+	RTL8821A_TRANS_ACT_TO_CARDEMU
+	RTL8821A_TRANS_CARDEMU_TO_CARDDIS
+	RTL8821A_TRANS_END
+};
+
+/*3 Card Enable Array*/
+struct wlan_pwr_cfg rtl8821A_card_enable_flow[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
+					+ RTL8821A_TRANS_CARDEMU_TO_ACT_STEPS /*RTL8821A_TRANS_CARDEMU_TO_PDN_STEPS*/
+					+ RTL8821A_TRANS_END_STEPS] =
+{
+	RTL8821A_TRANS_CARDDIS_TO_CARDEMU
+	RTL8821A_TRANS_CARDEMU_TO_ACT
+	RTL8821A_TRANS_END
+};
+
+/*3Suspend Array*/
+struct wlan_pwr_cfg rtl8821A_suspend_flow[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
+					+ RTL8821A_TRANS_CARDEMU_TO_SUS_STEPS
+					+ RTL8821A_TRANS_END_STEPS] =
+{
+	RTL8821A_TRANS_ACT_TO_CARDEMU
+	RTL8821A_TRANS_CARDEMU_TO_SUS
+	RTL8821A_TRANS_END
+};
+
+/*3 Resume Array*/
+struct wlan_pwr_cfg rtl8821A_resume_flow[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
+					+ RTL8821A_TRANS_CARDEMU_TO_SUS_STEPS
+					+ RTL8821A_TRANS_END_STEPS] =
+{
+	RTL8821A_TRANS_SUS_TO_CARDEMU
+	RTL8821A_TRANS_CARDEMU_TO_ACT
+	RTL8821A_TRANS_END
+};
+
+/*3HWPDN Array*/
+struct wlan_pwr_cfg rtl8821A_hwpdn_flow[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
+				+ RTL8821A_TRANS_CARDEMU_TO_PDN_STEPS
+				+ RTL8821A_TRANS_END_STEPS] =
+{
+	RTL8821A_TRANS_ACT_TO_CARDEMU
+	RTL8821A_TRANS_CARDEMU_TO_PDN
+	RTL8821A_TRANS_END
+};
+
+/*3 Enter LPS */
+struct wlan_pwr_cfg rtl8821A_enter_lps_flow[RTL8821A_TRANS_ACT_TO_LPS_STEPS
+					+ RTL8821A_TRANS_END_STEPS] =
+{
+	/*FW behavior*/
+	RTL8821A_TRANS_ACT_TO_LPS
+	RTL8821A_TRANS_END
+};
+
+/*3 Leave LPS */
+struct wlan_pwr_cfg rtl8821A_leave_lps_flow[RTL8821A_TRANS_LPS_TO_ACT_STEPS
+					+ RTL8821A_TRANS_END_STEPS] =
+{
+	/*FW behavior*/
+	RTL8821A_TRANS_LPS_TO_ACT
+	RTL8821A_TRANS_END
+};
+
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/pwrseq.h b/drivers/staging/rtl8821ae/rtl8821ae/pwrseq.h
new file mode 100644
index 000000000000..8b39c042fa93
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/pwrseq.h
@@ -0,0 +1,413 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL8821AE_PWRSEQ_H__
+#define __RTL8821AE_PWRSEQ_H__
+
+#include "pwrseqcmd.h"
+#include "../btcoexist/halbt_precomp.h"
+
+#define	RTL8812_TRANS_CARDEMU_TO_ACT_STEPS	15
+#define	RTL8812_TRANS_ACT_TO_CARDEMU_STEPS	15
+#define	RTL8812_TRANS_CARDEMU_TO_SUS_STEPS	15
+#define	RTL8812_TRANS_SUS_TO_CARDEMU_STEPS	15
+#define	RTL8812_TRANS_CARDEMU_TO_PDN_STEPS	25
+#define	RTL8812_TRANS_PDN_TO_CARDEMU_STEPS	15
+#define	RTL8812_TRANS_ACT_TO_LPS_STEPS	15
+#define	RTL8812_TRANS_LPS_TO_ACT_STEPS	15
+#define	RTL8812_TRANS_END_STEPS	1
+
+
+#define RTL8812_TRANS_CARDEMU_TO_ACT 														\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT2, 0},/* disable SW LPS 0x04[10]=0*/	\
+	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, BIT1, BIT1},/* wait till 0x04[17] = 1    power ready*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT7, 0},/* disable HWPDN 0x04[15]=0*/ \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT3, 0},/* disable WL suspend*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, BIT0},/* polling until return 0*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, BIT0, 0},/**/
+
+#define RTL8812_TRANS_ACT_TO_CARDEMU													\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
+	{0x0c00, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x04}, /* 0xc00[7:0] = 4	turn off 3-wire */	\
+	{0x0e00, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x04}, /* 0xe00[7:0] = 4	turn off 3-wire */	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, 0}, /* 0x2[0] = 0	 RESET BB, CLOSE RF */	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_DELAY, 0, PWRSEQ_DELAY_US},/*Delay 1us*/	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, 0},  /* Whole BB is reset*/			\
+	/*{0x001F, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0},//0x1F[7:0] = 0 turn off RF*/	\
+	/*{0x004E, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT7, 0},//0x4C[23] = 0x4E[7] = 0, switch DPDT_SEL_P output from register 0x65[2] */	\
+	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x2A}, /* 0x07[7:0] = 0x28 sps pwm mode 0x2a for BT coex*/	\
+	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x02, 0},/*0x8[1] = 0 ANA clk =500k */	\
+	/*{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0|BIT1, 0}, //  0x02[1:0] = 0	reset BB */	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, BIT1}, /*0x04[9] = 1 turn off MAC by HW state machine*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, BIT1, 0}, /*wait till 0x04[9] = 0 polling until return 0 to disable*/
+
+#define RTL8812_TRANS_CARDEMU_TO_SUS													\
+	/* format */								\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
+	{0x0042, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xF0, 0xcc},\
+	{0x0042, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xF0, 0xEC},\
+	{0x0043, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x07},/* gpio11 input mode, gpio10~8 output mode */	\
+	{0x0045, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x00},/* gpio 0~7 output same value as input ?? */	\
+	{0x0046, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0xff},/* gpio0~7 output mode */	\
+	{0x0047, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0},/* 0x47[7:0] = 00 gpio mode */	\
+	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0},/* suspend option all off */	\
+	{0x0014, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x80, BIT7},/*0x14[7] = 1 turn on ZCD */	\
+	{0x0015, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x01, BIT0},/* 0x15[0] =1 trun on ZCD */	\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x10, BIT4},/*0x23[4] = 1 hpon LDO sleep mode */	\
+	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x02, 0},/*0x8[1] = 0 ANA clk =500k */	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT3, BIT3}, /*0x04[11] = 2b'11 enable WL suspend for PCIe*/
+
+#define RTL8812_TRANS_SUS_TO_CARDEMU													\
+	/* format */								\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT3, 0}, /*0x04[11] = 2b'01enable WL suspend*/   \
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x10, 0},/*0x23[4] = 0 hpon LDO sleep mode leave */	\
+	{0x0015, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x01, 0},/* 0x15[0] =0 trun off ZCD */	\
+	{0x0014, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x80, 0},/*0x14[7] = 0 turn off ZCD */	\
+	{0x0046, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x00},/* gpio0~7 input mode */	\
+	{0x0043, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x00},/* gpio11 input mode, gpio10~8 input mode */
+
+#define RTL8812_TRANS_CARDEMU_TO_CARDDIS													\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
+	/**{0x0194, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, 0}, //0x194[0]=0 , disable 32K clock*/	\
+	/**{0x0093, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x94}, //0x93=0x94 , 90[30] =0 enable 500k ANA clock .switch clock from 12M to 500K , 90 [26] =0 disable EEprom loader clock*/	\
+	{0x0003, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT2, 0}, /*0x03[2] = 0, reset 8051*/	\
+	{0x0080, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x05}, /*0x80=05h if reload fw, fill the default value of host_CPU handshake field*/	\
+	{0x0042, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xF0, 0xcc},\
+	{0x0042, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xF0, 0xEC},\
+	{0x0043, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x07},/* gpio11 input mode, gpio10~8 output mode */	\
+	{0x0045, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x00},/* gpio 0~7 output same value as input ?? */	\
+	{0x0046, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0xff},/* gpio0~7 output mode */	\
+	{0x0047, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0},/* 0x47[7:0] = 00 gpio mode */	\
+	{0x0014, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x80, BIT7},/*0x14[7] = 1 turn on ZCD */	\
+	{0x0015, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x01, BIT0},/* 0x15[0] =1 trun on ZCD */	\
+	{0x0012, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x01, 0},/*0x12[0] = 0 force PFM mode */	\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x10, BIT4},/*0x23[4] = 1 hpon LDO sleep mode */	\
+	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x02, 0},/*0x8[1] = 0 ANA clk =500k */	\
+	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x20}, /*0x07=0x20 , SOP option to disable BG/MB*/	\
+	{0x001f, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, 0}, /*0x01f[1]=0 , disable RFC_0  control  REG_RF_CTRL_8812 */	\
+	{0x0076, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, 0}, /*0x076[1]=0 , disable RFC_1  control REG_OPT_CTRL_8812 +2 */	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT3, BIT3}, /*0x04[11] = 2b'01 enable WL suspend*/
+
+#define RTL8812_TRANS_CARDDIS_TO_CARDEMU													\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/                       \
+	{0x0012, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, BIT0},/*0x12[0] = 1 force PWM mode */	\
+	{0x0014, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x80, 0},/*0x14[7] = 0 turn off ZCD */	\
+	{0x0015, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x01, 0},/* 0x15[0] =0 trun off ZCD */	\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x10, 0},/*0x23[4] = 0 hpon LDO leave sleep mode */	\
+	{0x0046, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x00},/* gpio0~7 input mode */	\
+	{0x0043, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x00},/* gpio11 input mode, gpio10~8 input mode */ \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT2, 0}, /*0x04[10] = 0, enable SW LPS PCIE only*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT3, 0}, /*0x04[11] = 2b'01enable WL suspend*/	\
+	{0x0003, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT2, BIT2}, /*0x03[2] = 1, enable 8051*/	\
+	{0x0301, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0},/*PCIe DMA start*/
+
+
+#define RTL8812_TRANS_CARDEMU_TO_PDN												\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT7, BIT7},/* 0x04[15] = 1*/
+
+#define RTL8812_TRANS_PDN_TO_CARDEMU												\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT7, 0},/* 0x04[15] = 0*/
+
+#define RTL8812_TRANS_ACT_TO_LPS														\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
+	{0x0301, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0xFF},/*PCIe DMA stop*/	\
+	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x7F},/*Tx Pause*/		\
+	{0x05F8, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, 0xFF, 0},/*Should be zero if no packet is transmitting*/	\
+	{0x05F9, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, 0xFF, 0},/*Should be zero if no packet is transmitting*/	\
+	{0x05FA, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, 0xFF, 0},/*Should be zero if no packet is transmitting*/	\
+	{0x05FB, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, 0xFF, 0},/*Should be zero if no packet is transmitting*/	\
+	{0x0c00, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x04}, /* 0xc00[7:0] = 4	turn off 3-wire */	\
+	{0x0e00, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x04}, /* 0xe00[7:0] = 4	turn off 3-wire */	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, 0},/*CCK and OFDM are disabled,and clock are gated,and RF closed*/	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_DELAY, 0, PWRSEQ_DELAY_US},/*Delay 1us*/	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, 0},  /* Whole BB is reset*/			\
+	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x03},/*Reset MAC TRX*/			\
+	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, 0},/*check if removed later*/		\
+	{0x0553, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT5, BIT5},/*Respond TxOK to scheduler*/
+
+
+#define RTL8812_TRANS_LPS_TO_ACT															\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/	\
+	{0x0080, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_SDIO,PWR_CMD_WRITE, 0xFF, 0x84}, /*SDIO RPWM*/	\
+	{0xFE58, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x84}, /*USB RPWM*/	\
+	{0x0361, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x84}, /*PCIe RPWM*/	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_DELAY, 0, PWRSEQ_DELAY_MS}, /*Delay*/	\
+	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT4, 0}, /*.	0x08[4] = 0		 switch TSF to 40M*/	\
+	{0x0109, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, BIT7, 0}, /*Polling 0x109[7]=0  TSF in 40M*/			\
+	{0x0029, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT6|BIT7, 0}, /*.	0x29[7:6] = 2b'00	 enable BB clock*/	\
+	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, BIT1}, /*.	0x101[1] = 1*/					\
+	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0xFF}, /*.	0x100[7:0] = 0xFF	 enable WMAC TRX*/	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1|BIT0, BIT1|BIT0}, /*.	0x02[1:0] = 2b'11	 enable BB macro*/	\
+	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0}, /*.	0x522 = 0*/
+
+#define RTL8812_TRANS_END																\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/		\
+	{0xFFFF, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,0,PWR_CMD_END, 0, 0}, //
+
+
+extern struct wlan_pwr_cfg  rtl8812_power_on_flow[RTL8812_TRANS_CARDEMU_TO_ACT_STEPS+RTL8812_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg  rtl8812_radio_off_flow[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS+RTL8812_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg  rtl8812_card_disable_flow[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS+RTL8812_TRANS_CARDEMU_TO_PDN_STEPS+RTL8812_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg  rtl8812_card_enable_flow[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS+RTL8812_TRANS_CARDEMU_TO_PDN_STEPS+RTL8812_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg  rtl8812_suspend_flow[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS+RTL8812_TRANS_CARDEMU_TO_SUS_STEPS+RTL8812_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg  rtl8812_resume_flow[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS+RTL8812_TRANS_CARDEMU_TO_SUS_STEPS+RTL8812_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg  rtl8812_hwpdn_flow[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS+RTL8812_TRANS_CARDEMU_TO_PDN_STEPS+RTL8812_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg  rtl8812_enter_lps_flow[RTL8812_TRANS_ACT_TO_LPS_STEPS+RTL8812_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg  rtl8812_leave_lps_flow[RTL8812_TRANS_LPS_TO_ACT_STEPS+RTL8812_TRANS_END_STEPS];
+
+/*
+	Check document WM-20130516-JackieLau-RTL8821A_Power_Architecture-R10.vsd
+	There are 6 HW Power States:
+	0: POFF--Power Off
+	1: PDN--Power Down
+	2: CARDEMU--Card Emulation
+	3: ACT--Active Mode
+	4: LPS--Low Power State
+	5: SUS--Suspend
+
+	The transision from different states are defined below
+	TRANS_CARDEMU_TO_ACT
+	TRANS_ACT_TO_CARDEMU
+	TRANS_CARDEMU_TO_SUS
+	TRANS_SUS_TO_CARDEMU
+	TRANS_CARDEMU_TO_PDN
+	TRANS_ACT_TO_LPS
+	TRANS_LPS_TO_ACT
+
+	TRANS_END
+*/
+#define	RTL8821A_TRANS_CARDEMU_TO_ACT_STEPS	25
+#define	RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS	15
+#define	RTL8821A_TRANS_CARDEMU_TO_SUS_STEPS	15
+#define	RTL8821A_TRANS_SUS_TO_CARDEMU_STEPS	15
+#define RTL8821A_TRANS_CARDDIS_TO_CARDEMU_STEPS	15
+#define	RTL8821A_TRANS_CARDEMU_TO_PDN_STEPS	15
+#define	RTL8821A_TRANS_PDN_TO_CARDEMU_STEPS	15
+#define	RTL8821A_TRANS_ACT_TO_LPS_STEPS	15
+#define	RTL8821A_TRANS_LPS_TO_ACT_STEPS	15
+#define	RTL8821A_TRANS_END_STEPS	1
+
+
+#define RTL8821A_TRANS_CARDEMU_TO_ACT 														\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
+	{0x0020, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, BIT0}, /*0x20[0] = 1b'1 enable LDOA12 MACRO block for all interface*/   \
+	{0x0067, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT4, 0}, /*0x67[0] = 0 to disable BT_GPS_SEL pins*/	\
+	{0x0001, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_DELAY, 1, PWRSEQ_DELAY_MS},/*Delay 1ms*/   \
+	{0x0000, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT5, 0}, /*0x00[5] = 1b'0 release analog Ips to digital ,1:isolation*/   \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, (BIT4|BIT3|BIT2), 0},/* disable SW LPS 0x04[10]=0 and WLSUS_EN 0x04[12:11]=0*/	\
+	{0x0075, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0 , BIT0},/* Disable USB suspend */	\
+	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, BIT1, BIT1},/* wait till 0x04[17] = 1    power ready*/	\
+	{0x0075, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0 , 0},/* Enable USB suspend */	\
+	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, BIT0},/* release WLON reset  0x04[16]=1*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT7, 0},/* disable HWPDN 0x04[15]=0*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, (BIT4|BIT3), 0},/* disable WL suspend*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, BIT0},/* polling until return 0*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, BIT0, 0},/**/	\
+	{0x004F, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, BIT0},/*0x4C[24] = 0x4F[0] = 1, switch DPDT_SEL_P output from WL BB */\
+	{0x0067, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, (BIT5|BIT4), (BIT5|BIT4)},/*0x66[13] = 0x67[5] = 1, switch for PAPE_G/PAPE_A from WL BB ; 0x66[12] = 0x67[4] = 1, switch LNAON from WL BB */\
+	{0x0025, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT6, 0},/*anapar_mac<118> , 0x25[6]=0 by wlan single function*/\
+	{0x0049, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, BIT1},/*Enable falling edge triggering interrupt*/\
+	{0x0063, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, BIT1},/*Enable GPIO9 interrupt mode*/\
+	{0x0062, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, 0},/*Enable GPIO9 input mode*/\
+	{0x0058, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, BIT0},/*Enable HSISR GPIO[C:0] interrupt*/\
+	{0x005A, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, BIT1},/*Enable HSISR GPIO9 interrupt*/\
+	{0x007A, PWR_CUT_TESTCHIP_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x3A},/*0x7A = 0x3A start BT*/\
+	{0x002E, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF , 0x82 },/* 0x2C[23:12]=0x820 ; XTAL trim */ \
+	{0x0010, PWR_CUT_A_MSK , PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT6 , BIT6 },/* 0x10[6]=1 ; MP·s¼W¹ï©ó0x2Cªº±±¨îÅv¡A¶·§â0x10[6]³]¬°1¤~¯àÅýWLAN±±¨î */ \
+
+
+#define RTL8821A_TRANS_ACT_TO_CARDEMU													\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
+	{0x001F, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0},/*0x1F[7:0] = 0 turn off RF*/	\
+	{0x004F, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, 0},/*0x4C[24] = 0x4F[0] = 0, switch DPDT_SEL_P output from register 0x65[2] */\
+	{0x0049, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, 0},/*Enable rising edge triggering interrupt*/ \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, BIT1}, /*0x04[9] = 1 turn off MAC by HW state machine*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, BIT1, 0}, /*wait till 0x04[9] = 0 polling until return 0 to disable*/	\
+	{0x0000, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT5, BIT5}, /*0x00[5] = 1b'1 analog Ips to digital ,1:isolation*/   \
+	{0x0020, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, 0}, /*0x20[0] = 1b'0 disable LDOA12 MACRO block*/   \
+
+
+#define RTL8821A_TRANS_CARDEMU_TO_SUS													\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT4|BIT3, (BIT4|BIT3)}, /*0x04[12:11] = 2b'11 enable WL suspend for PCIe*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT3|BIT4, BIT3}, /*0x04[12:11] = 2b'01 enable WL suspend*/	\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT4, BIT4}, /*0x23[4] = 1b'1 12H LDO enter sleep mode*/   \
+	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x20}, /*0x07[7:0] = 0x20 SDIO SOP option to disable BG/MB/ACK/SWR*/   \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT3|BIT4, BIT3|BIT4}, /*0x04[12:11] = 2b'11 enable WL suspend for PCIe*/	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_SDIO,PWR_CMD_WRITE, BIT0, BIT0}, /*Set SDIO suspend local register*/	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_SDIO,PWR_CMD_POLLING, BIT1, 0}, /*wait power state to suspend*/
+
+#define RTL8821A_TRANS_SUS_TO_CARDEMU													\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT3 | BIT7, 0}, /*clear suspend enable and power down enable*/	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_SDIO,PWR_CMD_WRITE, BIT0, 0}, /*Set SDIO suspend local register*/	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_SDIO,PWR_CMD_POLLING, BIT1, BIT1}, /*wait power state to suspend*/\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT4, 0}, /*0x23[4] = 1b'0 12H LDO enter normal mode*/   \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT3|BIT4, 0}, /*0x04[12:11] = 2b'01enable WL suspend*/
+
+#define RTL8821A_TRANS_CARDEMU_TO_CARDDIS													\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
+	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x20}, /*0x07=0x20 , SOP option to disable BG/MB*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT3|BIT4, BIT3}, /*0x04[12:11] = 2b'01 enable WL suspend*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT2, BIT2}, /*0x04[10] = 1, enable SW LPS*/	\
+        {0x004A, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, 1}, /*0x48[16] = 1 to enable GPIO9 as EXT WAKEUP*/   \
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT4, BIT4}, /*0x23[4] = 1b'1 12H LDO enter sleep mode*/   \
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_SDIO,PWR_CMD_WRITE, BIT0, BIT0}, /*Set SDIO suspend local register*/	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_SDIO,PWR_CMD_POLLING, BIT1, 0}, /*wait power state to suspend*/
+
+#define RTL8821A_TRANS_CARDDIS_TO_CARDEMU													\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT3 | BIT7, 0}, /*clear suspend enable and power down enable*/	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_SDIO,PWR_CMD_WRITE, BIT0, 0}, /*Set SDIO suspend local register*/	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_SDIO,PWR_CMD_POLLING, BIT1, BIT1}, /*wait power state to suspend*/\
+	{0x004A, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, 0}, /*0x48[16] = 0 to disable GPIO9 as EXT WAKEUP*/   \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT3|BIT4, 0}, /*0x04[12:11] = 2b'01enable WL suspend*/\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT4, 0}, /*0x23[4] = 1b'0 12H LDO enter normal mode*/   \
+	{0x0301, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0},/*PCIe DMA start*/
+
+
+#define RTL8821A_TRANS_CARDEMU_TO_PDN												\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT4, BIT4}, /*0x23[4] = 1b'1 12H LDO enter sleep mode*/   \
+	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK|PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x20}, /*0x07[7:0] = 0x20 SOP option to disable BG/MB/ACK/SWR*/   \
+	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, 0},/* 0x04[16] = 0*/\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT7, BIT7},/* 0x04[15] = 1*/
+
+#define RTL8821A_TRANS_PDN_TO_CARDEMU												\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT7, 0},/* 0x04[15] = 0*/
+
+#define RTL8821A_TRANS_ACT_TO_LPS														\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
+	{0x0301, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0xFF},/*PCIe DMA stop*/	\
+	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0xFF},/*Tx Pause*/	\
+	{0x05F8, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, 0xFF, 0},/*Should be zero if no packet is transmitting*/	\
+	{0x05F9, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, 0xFF, 0},/*Should be zero if no packet is transmitting*/	\
+	{0x05FA, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, 0xFF, 0},/*Should be zero if no packet is transmitting*/	\
+	{0x05FB, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, 0xFF, 0},/*Should be zero if no packet is transmitting*/	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, 0},/*CCK and OFDM are disabled,and clock are gated*/	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_DELAY, 0, PWRSEQ_DELAY_US},/*Delay 1us*/	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, 0},/*Whole BB is reset*/	\
+	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x03},/*Reset MAC TRX*/	\
+	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, 0},/*check if removed later*/	\
+	{0x0093, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x00},/*When driver enter Sus/ Disable, enable LOP for BT*/	\
+	{0x0553, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT5, BIT5},/*Respond TxOK to scheduler*/	\
+
+
+#define RTL8821A_TRANS_LPS_TO_ACT															\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
+	{0x0080, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_SDIO,PWR_CMD_WRITE, 0xFF, 0x84}, /*SDIO RPWM*/\
+	{0xFE58, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x84}, /*USB RPWM*/\
+	{0x0361, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x84}, /*PCIe RPWM*/\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_DELAY, 0, PWRSEQ_DELAY_MS}, /*Delay*/\
+	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT4, 0}, /*.	0x08[4] = 0		 switch TSF to 40M*/\
+	{0x0109, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, BIT7, 0}, /*Polling 0x109[7]=0  TSF in 40M*/\
+	{0x0029, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT6|BIT7, 0}, /*.	0x29[7:6] = 2b'00	 enable BB clock*/\
+	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, BIT1}, /*.	0x101[1] = 1*/\
+	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0xFF}, /*.	0x100[7:0] = 0xFF	 enable WMAC TRX*/\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1|BIT0, BIT1|BIT0}, /*.	0x02[1:0] = 2b'11	 enable BB macro*/\
+	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0}, /*.	0x522 = 0*/
+
+#define RTL8821A_TRANS_END															\
+	/* format */																\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
+	{0xFFFF, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,0,PWR_CMD_END, 0, 0}, //
+
+extern struct wlan_pwr_cfg rtl8821A_power_on_flow[RTL8821A_TRANS_CARDEMU_TO_ACT_STEPS
+						+ RTL8821A_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8821A_radio_off_flow[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
+						+ RTL8821A_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8821A_card_disable_flow[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
+						+ RTL8821A_TRANS_CARDEMU_TO_PDN_STEPS
+						+ RTL8821A_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8821A_card_enable_flow[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
+						+ RTL8821A_TRANS_CARDEMU_TO_ACT_STEPS/*RTL8821A_TRANS_CARDEMU_TO_PDN_STEPS*/
+						+ RTL8821A_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8821A_suspend_flow[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
+						+ RTL8821A_TRANS_CARDEMU_TO_SUS_STEPS
+						+ RTL8821A_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8821A_resume_flow[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
+						+ RTL8821A_TRANS_CARDEMU_TO_SUS_STEPS
+						+ RTL8821A_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8821A_hwpdn_flow[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
+						+ RTL8821A_TRANS_CARDEMU_TO_PDN_STEPS
+						+ RTL8821A_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8821A_enter_lps_flow[RTL8821A_TRANS_ACT_TO_LPS_STEPS
+						+ RTL8821A_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8821A_leave_lps_flow[RTL8821A_TRANS_LPS_TO_ACT_STEPS
+						+ RTL8821A_TRANS_END_STEPS];
+
+/*RTL8812 Power Configuration CMDs for PCIe interface*/
+#define RTL8812_NIC_PWR_ON_FLOW				rtl8812_power_on_flow
+#define RTL8812_NIC_RF_OFF_FLOW				rtl8812_radio_off_flow
+#define RTL8812_NIC_DISABLE_FLOW 			rtl8812_card_disable_flow
+#define RTL8812_NIC_ENABLE_FLOW				rtl8812_card_enable_flow
+#define RTL8812_NIC_SUSPEND_FLOW			rtl8812_suspend_flow
+#define RTL8812_NIC_RESUME_FLOW				rtl8812_resume_flow
+#define RTL8812_NIC_PDN_FLOW					rtl8812_hwpdn_flow
+#define RTL8812_NIC_LPS_ENTER_FLOW			rtl8812_enter_lps_flow
+#define RTL8812_NIC_LPS_LEAVE_FLOW			rtl8812_leave_lps_flow
+
+/* RTL8821 Power Configuration CMDs for PCIe interface */
+#define RTL8821A_NIC_PWR_ON_FLOW			rtl8821A_power_on_flow
+#define RTL8821A_NIC_RF_OFF_FLOW			rtl8821A_radio_off_flow
+#define RTL8821A_NIC_DISABLE_FLOW			rtl8821A_card_disable_flow
+#define RTL8821A_NIC_ENABLE_FLOW			rtl8821A_card_enable_flow
+#define RTL8821A_NIC_SUSPEND_FLOW			rtl8821A_suspend_flow
+#define RTL8821A_NIC_RESUME_FLOW			rtl8821A_resume_flow
+#define RTL8821A_NIC_PDN_FLOW				rtl8821A_hwpdn_flow
+#define RTL8821A_NIC_LPS_ENTER_FLOW			rtl8821A_enter_lps_flow
+#define RTL8821A_NIC_LPS_LEAVE_FLOW			rtl8821A_leave_lps_flow
+
+
+#endif
+
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/pwrseqcmd.c b/drivers/staging/rtl8821ae/rtl8821ae/pwrseqcmd.c
new file mode 100644
index 000000000000..710bc015251c
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/pwrseqcmd.c
@@ -0,0 +1,140 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "pwrseq.h"
+
+
+/*
+*	Description:
+*		This routine deal with the Power Configuration CMDs
+*		 parsing for RTL8723/RTL8188E Series IC.
+*	Assumption:
+*		We should follow specific format which was released from HW SD.
+*
+*	2011.07.07, added by Roger.
+*/
+bool rtl_hal_pwrseqcmdparsing (struct rtl_priv* rtlpriv, u8 cut_version,
+										u8 fab_version, u8 interface_type,
+										struct wlan_pwr_cfg	pwrcfgcmd[])
+
+{
+	struct wlan_pwr_cfg pwr_cfg_cmd = {0};
+	bool polling_bit = false;
+	u32 ary_idx=0;
+	u8 value = 0;
+	u32 offset = 0;
+	u32 polling_count = 0;
+	u32 max_polling_cnt = 5000;
+
+	do {
+		pwr_cfg_cmd = pwrcfgcmd[ary_idx];
+		RT_TRACE(COMP_INIT, DBG_TRACE,
+			("rtl_hal_pwrseqcmdparsing(): offset(%#x),cut_msk(%#x), fab_msk(%#x),"
+			"interface_msk(%#x), base(%#x), cmd(%#x), msk(%#x), value(%#x)\n",
+			GET_PWR_CFG_OFFSET(pwr_cfg_cmd), GET_PWR_CFG_CUT_MASK(pwr_cfg_cmd),
+			GET_PWR_CFG_FAB_MASK(pwr_cfg_cmd), GET_PWR_CFG_INTF_MASK(pwr_cfg_cmd),
+			GET_PWR_CFG_BASE(pwr_cfg_cmd), GET_PWR_CFG_CMD(pwr_cfg_cmd),
+			GET_PWR_CFG_MASK(pwr_cfg_cmd), GET_PWR_CFG_VALUE(pwr_cfg_cmd)));
+
+		if ((GET_PWR_CFG_FAB_MASK(pwr_cfg_cmd)&fab_version) &&
+			(GET_PWR_CFG_CUT_MASK(pwr_cfg_cmd)&cut_version) &&
+			(GET_PWR_CFG_INTF_MASK(pwr_cfg_cmd)&interface_type)) {
+			switch (GET_PWR_CFG_CMD(pwr_cfg_cmd)) {
+			case PWR_CMD_READ:
+				RT_TRACE(COMP_INIT, DBG_TRACE,
+					("rtl_hal_pwrseqcmdparsing(): PWR_CMD_READ\n"));
+				break;
+
+			case PWR_CMD_WRITE: {
+				RT_TRACE(COMP_INIT, DBG_TRACE,
+					("rtl_hal_pwrseqcmdparsing(): PWR_CMD_WRITE\n"));
+				offset = GET_PWR_CFG_OFFSET(pwr_cfg_cmd);
+
+					/*Read the value from system register*/
+					value = rtl_read_byte(rtlpriv, offset);
+					value = value & (~(GET_PWR_CFG_MASK(pwr_cfg_cmd)));
+					value = value | (GET_PWR_CFG_VALUE(pwr_cfg_cmd)
+							& GET_PWR_CFG_MASK(pwr_cfg_cmd));
+
+					/*Write the value back to sytem register*/
+					rtl_write_byte(rtlpriv, offset, value);
+				}
+				break;
+
+			case PWR_CMD_POLLING:
+				RT_TRACE(COMP_INIT, DBG_TRACE,
+					("rtl_hal_pwrseqcmdparsing(): PWR_CMD_POLLING\n"));
+				polling_bit = false;
+				offset = GET_PWR_CFG_OFFSET(pwr_cfg_cmd);
+
+				do {
+					value = rtl_read_byte(rtlpriv, offset);
+
+					value = value & GET_PWR_CFG_MASK(pwr_cfg_cmd);
+					if (value == (GET_PWR_CFG_VALUE(pwr_cfg_cmd)
+							& GET_PWR_CFG_MASK(pwr_cfg_cmd)))
+						polling_bit=true;
+					else
+						udelay(10);
+
+					if (polling_count++ > max_polling_cnt) {
+						return false;
+					}
+				} while (!polling_bit);
+
+				break;
+
+			case PWR_CMD_DELAY:
+				RT_TRACE(COMP_INIT, DBG_TRACE,
+					("rtl_hal_pwrseqcmdparsing(): PWR_CMD_DELAY\n"));
+				if (GET_PWR_CFG_VALUE(pwr_cfg_cmd) == PWRSEQ_DELAY_US)
+					udelay(GET_PWR_CFG_OFFSET(pwr_cfg_cmd));
+				else
+					mdelay(GET_PWR_CFG_OFFSET(pwr_cfg_cmd));
+				break;
+
+			case PWR_CMD_END:
+				RT_TRACE(COMP_INIT, DBG_TRACE,
+					("rtl_hal_pwrseqcmdparsing(): PWR_CMD_END\n"));
+				return true;
+				break;
+
+			default:
+				RT_ASSERT(false,
+					("rtl_hal_pwrseqcmdparsing(): Unknown CMD!!\n"));
+				break;
+			}
+
+		}
+
+		ary_idx++;
+	} while (1);
+
+	return true;
+}
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/pwrseqcmd.h b/drivers/staging/rtl8821ae/rtl8821ae/pwrseqcmd.h
new file mode 100644
index 000000000000..571e7e50d5b5
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/pwrseqcmd.h
@@ -0,0 +1,71 @@
+#ifndef __RTL8821AE_PWRSEQCMD_H__
+#define __RTL8821AE_PWRSEQCMD_H__
+
+#include "../wifi.h"
+/*---------------------------------------------*/
+/*The value of cmd: 4 bits */
+/*---------------------------------------------*/
+#define	PWR_CMD_READ 		0x00
+#define	PWR_CMD_WRITE	0x01
+#define	PWR_CMD_POLLING	0x02
+#define	PWR_CMD_DELAY	0x03
+#define	PWR_CMD_END		0x04
+
+/* define the base address of each block */
+#define	PWR_BASEADDR_MAC	0x00
+#define	PWR_BASEADDR_USB	0x01
+#define	PWR_BASEADDR_PCIE	0x02
+#define	PWR_BASEADDR_SDIO	0x03
+
+#define	PWR_INTF_SDIO_MSK	BIT(0)
+#define	PWR_INTF_USB_MSK	BIT(1)
+#define	PWR_INTF_PCI_MSK	BIT(2)
+#define	PWR_INTF_ALL_MSK	(BIT(0)|BIT(1)|BIT(2)|BIT(3))
+
+#define	PWR_FAB_TSMC_MSK	BIT(0)
+#define	PWR_FAB_UMC_MSK		BIT(1)
+#define	PWR_FAB_ALL_MSK		(BIT(0)|BIT(1)|BIT(2)|BIT(3))
+
+#define	PWR_CUT_TESTCHIP_MSK	BIT(0)
+#define	PWR_CUT_A_MSK		BIT(1)
+#define	PWR_CUT_B_MSK		BIT(2)
+#define	PWR_CUT_C_MSK		BIT(3)
+#define	PWR_CUT_D_MSK		BIT(4)
+#define	PWR_CUT_E_MSK		BIT(5)
+#define	PWR_CUT_F_MSK		BIT(6)
+#define	PWR_CUT_G_MSK		BIT(7)
+#define	PWR_CUT_ALL_MSK		0xFF
+
+
+enum pwrseq_delay_unit {
+   PWRSEQ_DELAY_US,
+   PWRSEQ_DELAY_MS,
+};
+
+struct wlan_pwr_cfg {
+	u16 offset;
+	u8 cut_msk;
+	u8 fab_msk:4;
+	u8 interface_msk:4;
+	u8 base:4;
+	u8 cmd:4;
+	u8 msk;
+	u8 value;
+
+};
+
+#define	GET_PWR_CFG_OFFSET(__PWR_CMD)	__PWR_CMD.offset
+#define	GET_PWR_CFG_CUT_MASK(__PWR_CMD)	__PWR_CMD.cut_msk
+#define	GET_PWR_CFG_FAB_MASK(__PWR_CMD)	__PWR_CMD.fab_msk
+#define	GET_PWR_CFG_INTF_MASK(__PWR_CMD)	__PWR_CMD.interface_msk
+#define	GET_PWR_CFG_BASE(__PWR_CMD)	__PWR_CMD.base
+#define	GET_PWR_CFG_CMD(__PWR_CMD)	__PWR_CMD.cmd
+#define	GET_PWR_CFG_MASK(__PWR_CMD)	__PWR_CMD.msk
+#define	GET_PWR_CFG_VALUE(__PWR_CMD)	__PWR_CMD.value
+
+bool rtl_hal_pwrseqcmdparsing(struct rtl_priv * rtlpriv, u8 cut_version,
+									   u8 fab_version, u8 interface_type,
+									   struct wlan_pwr_cfg pwrcfgcmd[]);
+
+#endif
+
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/reg.h b/drivers/staging/rtl8821ae/rtl8821ae/reg.h
new file mode 100644
index 000000000000..09c5f00d2603
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/reg.h
@@ -0,0 +1,2427 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL8821AE_REG_H__
+#define __RTL8821AE_REG_H__
+
+#define TXPKT_BUF_SELECT				0x69
+#define RXPKT_BUF_SELECT				0xA5
+#define DISABLE_TRXPKT_BUF_ACCESS			0x0
+
+#define REG_SYS_ISO_CTRL			0x0000
+#define REG_SYS_FUNC_EN				0x0002
+#define REG_APS_FSMCO				0x0004
+#define REG_SYS_CLKR				0x0008
+#define REG_9346CR					0x000A
+#define REG_EE_VPD					0x000C
+#define REG_AFE_MISC				0x0010
+#define REG_SPS0_CTRL				0x0011
+#define REG_SPS_OCP_CFG				0x0018
+#define REG_RSV_CTRL				0x001C
+#define REG_RF_CTRL					0x001F
+#define REG_LDOA15_CTRL				0x0020
+#define REG_LDOV12D_CTRL			0x0021
+#define REG_LDOHCI12_CTRL			0x0022
+#define REG_LPLDO_CTRL				0x0023
+#define REG_AFE_XTAL_CTRL			0x0024
+#define REG_AFE_LDO_CTRL                	0x0027 /* 1.5v for 8188EE test chip, 1.4v for MP chip */
+#define REG_AFE_PLL_CTRL			0x0028
+#define REG_MAC_PHY_CTRL			0x002c
+#define REG_EFUSE_CTRL				0x0030
+#define REG_EFUSE_TEST				0x0034
+#define REG_PWR_DATA				0x0038
+#define REG_CAL_TIMER				0x003C
+#define REG_ACLK_MON				0x003E
+#define REG_GPIO_MUXCFG			0x0040
+#define REG_GPIO_IO_SEL				0x0042
+#define REG_MAC_PINMUX_CFG		0x0043
+#define REG_GPIO_PIN_CTRL			0x0044
+#define REG_GPIO_INTM				0x0048
+#define REG_LEDCFG0					0x004C
+#define REG_LEDCFG1					0x004D
+#define REG_LEDCFG2					0x004E
+#define REG_LEDCFG3					0x004F
+#define REG_FSIMR					0x0050
+#define REG_FSISR					0x0054
+#define REG_HSIMR					0x0058
+#define REG_HSISR					0x005c
+#define REG_GPIO_PIN_CTRL_2		0x0060
+#define REG_GPIO_IO_SEL_2			0x0062
+#define REG_MULTI_FUNC_CTRL			0x0068
+#define REG_GPIO_OUTPUT			0x006c
+#define REG_OPT_CTRL			0x0074
+#define REG_AFE_XTAL_CTRL_EXT		0x0078
+#define REG_XCK_OUT_CTRL			0x007c
+#define REG_MCUFWDL				0x0080
+#define REG_WOL_EVENT				0x0081
+#define REG_MCUTSTCFG				0x0084
+
+
+#define REG_HIMR					0x00B0
+#define REG_HISR					0x00B4
+#define REG_HIMRE					0x00B8
+#define REG_HISRE					0x00BC
+
+#define REG_PMC_DBG_CTRL2			0x00CC
+
+#define REG_EFUSE_ACCESS			0x00CF
+
+#define REG_BIST_SCAN				0x00D0
+#define REG_BIST_RPT				0x00D4
+#define REG_BIST_ROM_RPT			0x00D8
+#define REG_USB_SIE_INTF			0x00E0
+#define REG_PCIE_MIO_INTF			0x00E4
+#define REG_PCIE_MIO_INTD			0x00E8
+#define REG_HPON_FSM				0x00EC
+#define REG_SYS_CFG					0x00F0
+#define REG_GPIO_OUTSTS				0x00F4
+#define REG_SYS_CFG1				0x00FC
+#define REG_ROM_VERSION				0x00FD
+
+#define REG_CR						0x0100
+#define REG_PBP						0x0104
+#define REG_PKT_BUFF_ACCESS_CTRL			0x0106
+#define REG_TRXDMA_CTRL				0x010C
+#define REG_TRXFF_BNDY				0x0114
+#define REG_TRXFF_STATUS			0x0118
+#define REG_RXFF_PTR				0x011C
+
+#define REG_CPWM					0x012F
+#define REG_FWIMR					0x0130
+#define REG_FWISR					0x0134
+#define REG_PKTBUF_DBG_CTRL			0x0140
+#define REG_PKTBUF_DBG_DATA_L		0x0144
+#define REG_PKTBUF_DBG_DATA_H		0x0148
+#define REG_RXPKTBUF_CTRL				(REG_PKTBUF_DBG_CTRL+2)
+
+#define REG_TC0_CTRL				0x0150
+#define REG_TC1_CTRL				0x0154
+#define REG_TC2_CTRL				0x0158
+#define REG_TC3_CTRL				0x015C
+#define REG_TC4_CTRL				0x0160
+#define REG_TCUNIT_BASE				0x0164
+#define REG_MBIST_START				0x0174
+#define REG_MBIST_DONE				0x0178
+#define REG_MBIST_FAIL				0x017C
+#define REG_32K_CTRL					0x0194
+#define REG_C2HEVT_MSG_NORMAL		0x01A0
+#define REG_C2HEVT_CLEAR			0x01AF
+#define REG_C2HEVT_MSG_TEST			0x01B8
+#define REG_MCUTST_1				0x01c0
+#define REG_FMETHR					0x01C8
+#define REG_HMETFR					0x01CC
+#define REG_HMEBOX_0				0x01D0
+#define REG_HMEBOX_1				0x01D4
+#define REG_HMEBOX_2				0x01D8
+#define REG_HMEBOX_3				0x01DC
+
+#define REG_LLT_INIT				0x01E0
+#define REG_BB_ACCEESS_CTRL			0x01E8
+#define REG_BB_ACCESS_DATA			0x01EC
+
+#define REG_HMEBOX_EXT_0			0x01F0
+#define REG_HMEBOX_EXT_1			0x01F4
+#define REG_HMEBOX_EXT_2			0x01F8
+#define REG_HMEBOX_EXT_3			0x01FC
+
+#define REG_RQPN					0x0200
+#define REG_FIFOPAGE				0x0204
+#define REG_TDECTRL					0x0208
+#define REG_TXDMA_OFFSET_CHK		0x020C
+#define REG_TXDMA_STATUS			0x0210
+#define REG_RQPN_NPQ				0x0214
+
+#define REG_RXDMA_AGG_PG_TH		0x0280
+#define REG_FW_UPD_RDPTR			0x0284 /* FW shall update this register before FW write RXPKT_RELEASE_POLL to 1 */
+#define REG_RXDMA_CONTROL			0x0286 /* Control the RX DMA.*/
+#define REG_RXPKT_NUM				0x0287 /* The number of packets in RXPKTBUF.	*/
+
+#define	REG_PCIE_CTRL_REG			0x0300
+#define	REG_INT_MIG					0x0304
+#define	REG_BCNQ_DESA				0x0308
+#define	REG_HQ_DESA					0x0310
+#define	REG_MGQ_DESA				0x0318
+#define	REG_VOQ_DESA				0x0320
+#define	REG_VIQ_DESA				0x0328
+#define	REG_BEQ_DESA				0x0330
+#define	REG_BKQ_DESA				0x0338
+#define	REG_RX_DESA					0x0340
+
+#define	REG_DBI_WDATA				0x0348
+#define	REG_DBI_RDATA				0x034C
+#define	REG_DBI_ADDR				0x0350
+#define	REG_DBI_FLAG				0x0352
+#define	REG_MDIO_WDATA				0x0354
+#define	REG_MDIO_RDATA				0x0356
+#define	REG_MDIO_CTL				0x0358
+#define	REG_DBG_SEL					0x0360
+#define	REG_PCIE_HRPWM				0x0361
+#define	REG_PCIE_HCPWM				0x0363
+#define	REG_UART_CTRL				0x0364
+#define	REG_WATCH_DOG				0x0368
+#define	REG_UART_TX_DESA			0x0370
+#define	REG_UART_RX_DESA			0x0378
+
+
+#define	REG_HDAQ_DESA_NODEF			0x0000
+#define	REG_CMDQ_DESA_NODEF			0x0000
+
+#define REG_VOQ_INFORMATION			0x0400
+#define REG_VIQ_INFORMATION			0x0404
+#define REG_BEQ_INFORMATION			0x0408
+#define REG_BKQ_INFORMATION			0x040C
+#define REG_MGQ_INFORMATION			0x0410
+#define REG_HGQ_INFORMATION			0x0414
+#define REG_BCNQ_INFORMATION			0x0418
+#define REG_TXPKT_EMPTY				0x041A
+
+
+#define REG_CPU_MGQ_INFORMATION		0x041C
+#define REG_FWHW_TXQ_CTRL			0x0420
+#define REG_HWSEQ_CTRL				0x0423
+#define REG_TXPKTBUF_BCNQ_BDNY		0x0424
+#define REG_TXPKTBUF_MGQ_BDNY		0x0425
+#define REG_MULTI_BCNQ_EN			0x0426
+#define REG_MULTI_BCNQ_OFFSET		0x0427
+#define REG_SPEC_SIFS				0x0428
+#define REG_RL						0x042A
+#define REG_DARFRC					0x0430
+#define REG_RARFRC					0x0438
+#define REG_RRSR					0x0440
+#define REG_ARFR0					0x0444
+#define REG_ARFR1					0x044C
+#define REG_CCK_CHECK				0x0454
+#define REG_AMPDU_MAX_TIME			0x0456
+#define REG_AGGLEN_LMT				0x0458
+#define REG_AMPDU_MIN_SPACE			0x045C
+#define REG_TXPKTBUF_WMAC_LBK_BF_HD	0x045D
+#define REG_FAST_EDCA_CTRL			0x0460
+#define REG_RD_RESP_PKT_TH			0x0463
+#define REG_INIRTS_RATE_SEL			0x0480
+#define REG_INIDATA_RATE_SEL		0x0484
+#define REG_ARFR2					0x048C
+#define REG_ARFR3					0x0494
+#define REG_POWER_STATUS			0x04A4
+#define REG_POWER_STAGE1			0x04B4
+#define REG_POWER_STAGE2			0x04B8
+#define REG_PKT_LIFE_TIME			0x04C0
+#define REG_STBC_SETTING			0x04C4
+#define REG_HT_SINGLE_AMPDU			0x04C7
+#define REG_PROT_MODE_CTRL			0x04C8
+#define REG_MAX_AGGR_NUM			0x04CA
+#define REG_BAR_MODE_CTRL			0x04CC
+#define REG_RA_TRY_RATE_AGG_LMT		0x04CF
+#define REG_EARLY_MODE_CONTROL			0x04D0
+#define REG_NQOS_SEQ				0x04DC
+#define REG_QOS_SEQ					0x04DE
+#define REG_NEED_CPU_HANDLE			0x04E0
+#define REG_PKT_LOSE_RPT			0x04E1
+#define REG_PTCL_ERR_STATUS			0x04E2
+#define REG_TX_RPT_CTRL				0x04EC
+#define REG_TX_RPT_TIME				0x04F0
+#define REG_DUMMY					0x04FC
+
+#define REG_EDCA_VO_PARAM			0x0500
+#define REG_EDCA_VI_PARAM			0x0504
+#define REG_EDCA_BE_PARAM			0x0508
+#define REG_EDCA_BK_PARAM			0x050C
+#define REG_BCNTCFG					0x0510
+#define REG_PIFS					0x0512
+#define REG_RDG_PIFS				0x0513
+#define REG_SIFS_CTX				0x0514
+#define REG_SIFS_TRX				0x0516
+#define REG_AGGR_BREAK_TIME			0x051A
+#define REG_SLOT					0x051B
+#define REG_TX_PTCL_CTRL			0x0520
+#define REG_TXPAUSE					0x0522
+#define REG_DIS_TXREQ_CLR			0x0523
+#define REG_RD_CTRL					0x0524
+#define REG_TBTT_PROHIBIT			0x0540
+#define REG_RD_NAV_NXT				0x0544
+#define REG_NAV_PROT_LEN			0x0546
+#define REG_BCN_CTRL				0x0550
+#define REG_USTIME_TSF				0x0551
+#define REG_MBID_NUM				0x0552
+#define REG_DUAL_TSF_RST			0x0553
+#define REG_BCN_INTERVAL			0x0554
+#define REG_MBSSID_BCN_SPACE		0x0554
+#define REG_DRVERLYINT				0x0558
+#define REG_BCNDMATIM				0x0559
+#define REG_ATIMWND					0x055A
+#define REG_BCN_MAX_ERR				0x055D
+#define REG_RXTSF_OFFSET_CCK		0x055E
+#define REG_RXTSF_OFFSET_OFDM		0x055F
+#define REG_TSFTR					0x0560
+#define REG_INIT_TSFTR				0x0564
+#define REG_SECONDARY_CCA_CTRL		0x0577
+#define REG_PSTIMER					0x0580
+#define REG_TIMER0					0x0584
+#define REG_TIMER1					0x0588
+#define REG_ACMHWCTRL				0x05C0
+#define REG_ACMRSTCTRL				0x05C1
+#define REG_ACMAVG					0x05C2
+#define REG_VO_ADMTIME				0x05C4
+#define REG_VI_ADMTIME				0x05C6
+#define REG_BE_ADMTIME				0x05C8
+#define REG_EDCA_RANDOM_GEN			0x05CC
+#define REG_NOA_DESC_SEL			0x05CF
+#define REG_NOA_DESC_DURATION		0x05E0
+#define REG_NOA_DESC_INTERVAL		0x05E4
+#define REG_NOA_DESC_START			0x05E8
+#define REG_NOA_DESC_COUNT			0x05EC
+#define REG_SCH_TX_CMD				0x05F8
+
+#define REG_APSD_CTRL				0x0600
+#define REG_BWOPMODE				0x0603
+#define REG_TCR						0x0604
+#define REG_RCR						0x0608
+#define REG_RX_PKT_LIMIT			0x060C
+#define REG_RX_DLK_TIME				0x060D
+#define REG_RX_DRVINFO_SZ			0x060F
+
+#define REG_MACID					0x0610
+#define REG_BSSID					0x0618
+#define REG_MAR						0x0620
+#define REG_MBIDCAMCFG				0x0628
+
+#define REG_USTIME_EDCA				0x0638
+#define REG_MAC_SPEC_SIFS			0x063A
+#define REG_RESP_SIFS_CCK			0x063C
+#define REG_RESP_SIFS_OFDM			0x063E
+#define REG_ACKTO					0x0640
+#define REG_CTS2TO					0x0641
+#define REG_EIFS					0x0642
+
+#define REG_NAV_CTRL				0x0650
+#define REG_NAV_UPPER				0x0652
+#define REG_BACAMCMD				0x0654
+#define REG_BACAMCONTENT			0x0658
+#define REG_LBDLY					0x0660
+#define REG_FWDLY					0x0661
+#define REG_RXERR_RPT				0x0664
+#define REG_TRXPTCL_CTL				0x0668
+
+#define REG_CAMCMD					0x0670
+#define REG_CAMWRITE				0x0674
+#define REG_CAMREAD					0x0678
+#define REG_CAMDBG					0x067C
+#define REG_SECCFG					0x0680
+
+#define REG_WOW_CTRL				0x0690
+#define REG_PSSTATUS				0x0691
+#define REG_PS_RX_INFO				0x0692
+#define REG_UAPSD_TID				0x0693
+#define REG_LPNAV_CTRL				0x0694
+#define REG_WKFMCAM_NUM				0x0698
+#define REG_WKFMCAM_RWD				0x069C
+#define REG_RXFLTMAP0				0x06A0
+#define REG_RXFLTMAP1				0x06A2
+#define REG_RXFLTMAP2				0x06A4
+#define REG_BCN_PSR_RPT				0x06A8
+#define REG_CALB32K_CTRL			0x06AC
+#define REG_PKT_MON_CTRL			0x06B4
+#define REG_BT_COEX_TABLE			0x06C0
+#define REG_WMAC_RESP_TXINFO		0x06D8
+
+#define REG_USB_INFO				0xFE17
+#define REG_USB_SPECIAL_OPTION		0xFE55
+#define REG_USB_DMA_AGG_TO			0xFE5B
+#define REG_USB_AGG_TO				0xFE5C
+#define REG_USB_AGG_TH				0xFE5D
+
+#define REG_TEST_USB_TXQS			0xFE48
+#define REG_TEST_SIE_VID			0xFE60
+#define REG_TEST_SIE_PID			0xFE62
+#define REG_TEST_SIE_OPTIONAL		0xFE64
+#define REG_TEST_SIE_CHIRP_K		0xFE65
+#define REG_TEST_SIE_PHY			0xFE66
+#define REG_TEST_SIE_MAC_ADDR		0xFE70
+#define REG_TEST_SIE_STRING			0xFE80
+
+#define REG_NORMAL_SIE_VID			0xFE60
+#define REG_NORMAL_SIE_PID			0xFE62
+#define REG_NORMAL_SIE_OPTIONAL		0xFE64
+#define REG_NORMAL_SIE_EP			0xFE65
+#define REG_NORMAL_SIE_PHY			0xFE68
+#define REG_NORMAL_SIE_MAC_ADDR		0xFE70
+#define REG_NORMAL_SIE_STRING		0xFE80
+
+#define	CR9346				REG_9346CR
+#define	MSR				(REG_CR + 2)
+#define	ISR				REG_HISR
+#define	TSFR				REG_TSFTR
+
+#define	MACIDR0				REG_MACID
+#define	MACIDR4				(REG_MACID + 4)
+
+#define PBP				REG_PBP
+
+#define	IDR0				MACIDR0
+#define	IDR4				MACIDR4
+
+#define	UNUSED_REGISTER			0x1BF
+#define	DCAM				UNUSED_REGISTER
+#define	PSR				UNUSED_REGISTER
+#define BBADDR				UNUSED_REGISTER
+#define	PHYDATAR			UNUSED_REGISTER
+
+#define	INVALID_BBRF_VALUE		0x12345678
+
+#define	MAX_MSS_DENSITY_2T 		0x13
+#define	MAX_MSS_DENSITY_1T 		0x0A
+
+#define	CMDEEPROM_EN			BIT(5)
+#define	CMDEEPROM_SEL			BIT(4)
+#define	CMD9346CR_9356SEL		BIT(4)
+#define	AUTOLOAD_EEPROM			(CMDEEPROM_EN|CMDEEPROM_SEL)
+#define	AUTOLOAD_EFUSE			CMDEEPROM_EN
+
+#define	GPIOSEL_GPIO			0
+#define	GPIOSEL_ENBT			BIT(5)
+
+#define	GPIO_IN				REG_GPIO_PIN_CTRL
+#define	GPIO_OUT			(REG_GPIO_PIN_CTRL+1)
+#define	GPIO_IO_SEL			(REG_GPIO_PIN_CTRL+2)
+#define	GPIO_MOD			(REG_GPIO_PIN_CTRL+3)
+
+/*      8723/8188E Host System Interrupt Mask Register (offset 0x58, 32 byte) */
+#define	HSIMR_GPIO12_0_INT_EN			BIT(0)
+#define	HSIMR_SPS_OCP_INT_EN			BIT(5)
+#define	HSIMR_RON_INT_EN			BIT(6)
+#define	HSIMR_PDN_INT_EN			BIT(7)
+#define	HSIMR_GPIO9_INT_EN			BIT(25)
+
+
+/*
+*       8723/8188E Host System Interrupt Status Register (offset 0x5C, 32 byte)
+*/
+#define	HSISR_GPIO12_0_INT			BIT(0)
+#define	HSISR_SPS_OCP_INT			BIT(5)
+#define	HSISR_RON_INT_EN			BIT(6)
+#define	HSISR_PDNINT				BIT(7)
+#define	HSISR_GPIO9_INT				BIT(25)
+
+#define	MSR_NOLINK					0x00
+#define	MSR_ADHOC					0x01
+#define	MSR_INFRA					0x02
+#define	MSR_AP						0x03
+
+#define	RRSR_RSC_OFFSET				21
+#define	RRSR_SHORT_OFFSET			23
+#define	RRSR_RSC_BW_40M				0x600000
+#define	RRSR_RSC_UPSUBCHNL			0x400000
+#define	RRSR_RSC_LOWSUBCHNL			0x200000
+#define	RRSR_SHORT					0x800000
+#define	RRSR_1M						BIT(0)
+#define	RRSR_2M						BIT(1)
+#define	RRSR_5_5M					BIT(2)
+#define	RRSR_11M					BIT(3)
+#define	RRSR_6M						BIT(4)
+#define	RRSR_9M						BIT(5)
+#define	RRSR_12M					BIT(6)
+#define	RRSR_18M					BIT(7)
+#define	RRSR_24M					BIT(8)
+#define	RRSR_36M					BIT(9)
+#define	RRSR_48M					BIT(10)
+#define	RRSR_54M					BIT(11)
+#define	RRSR_MCS0					BIT(12)
+#define	RRSR_MCS1					BIT(13)
+#define	RRSR_MCS2					BIT(14)
+#define	RRSR_MCS3					BIT(15)
+#define	RRSR_MCS4					BIT(16)
+#define	RRSR_MCS5					BIT(17)
+#define	RRSR_MCS6					BIT(18)
+#define	RRSR_MCS7					BIT(19)
+#define	BRSR_ACKSHORTPMB			BIT(23)
+
+#define	RATR_1M						0x00000001
+#define	RATR_2M						0x00000002
+#define	RATR_55M					0x00000004
+#define	RATR_11M					0x00000008
+#define	RATR_6M						0x00000010
+#define	RATR_9M						0x00000020
+#define	RATR_12M					0x00000040
+#define	RATR_18M					0x00000080
+#define	RATR_24M					0x00000100
+#define	RATR_36M					0x00000200
+#define	RATR_48M					0x00000400
+#define	RATR_54M					0x00000800
+#define	RATR_MCS0					0x00001000
+#define	RATR_MCS1					0x00002000
+#define	RATR_MCS2					0x00004000
+#define	RATR_MCS3					0x00008000
+#define	RATR_MCS4					0x00010000
+#define	RATR_MCS5					0x00020000
+#define	RATR_MCS6					0x00040000
+#define	RATR_MCS7					0x00080000
+#define	RATR_MCS8					0x00100000
+#define	RATR_MCS9					0x00200000
+#define	RATR_MCS10					0x00400000
+#define	RATR_MCS11					0x00800000
+#define	RATR_MCS12					0x01000000
+#define	RATR_MCS13					0x02000000
+#define	RATR_MCS14					0x04000000
+#define	RATR_MCS15					0x08000000
+
+#define RATE_1M						BIT(0)
+#define RATE_2M						BIT(1)
+#define RATE_5_5M					BIT(2)
+#define RATE_11M					BIT(3)
+#define RATE_6M						BIT(4)
+#define RATE_9M						BIT(5)
+#define RATE_12M					BIT(6)
+#define RATE_18M					BIT(7)
+#define RATE_24M					BIT(8)
+#define RATE_36M					BIT(9)
+#define RATE_48M					BIT(10)
+#define RATE_54M					BIT(11)
+#define RATE_MCS0					BIT(12)
+#define RATE_MCS1					BIT(13)
+#define RATE_MCS2					BIT(14)
+#define RATE_MCS3					BIT(15)
+#define RATE_MCS4					BIT(16)
+#define RATE_MCS5					BIT(17)
+#define RATE_MCS6					BIT(18)
+#define RATE_MCS7					BIT(19)
+#define RATE_MCS8					BIT(20)
+#define RATE_MCS9					BIT(21)
+#define RATE_MCS10					BIT(22)
+#define RATE_MCS11					BIT(23)
+#define RATE_MCS12					BIT(24)
+#define RATE_MCS13					BIT(25)
+#define RATE_MCS14					BIT(26)
+#define RATE_MCS15					BIT(27)
+
+#define	RATE_ALL_CCK		(RATR_1M | RATR_2M | RATR_55M | RATR_11M)
+#define	RATE_ALL_OFDM_AG	(RATR_6M | RATR_9M | RATR_12M | RATR_18M |\
+							RATR_24M| RATR_36M | RATR_48M | RATR_54M)
+#define	RATE_ALL_OFDM_1SS	(RATR_MCS0 | RATR_MCS1 | RATR_MCS2 |\
+							RATR_MCS3 | RATR_MCS4 | RATR_MCS5 |\
+							RATR_MCS6 | RATR_MCS7)
+#define	RATE_ALL_OFDM_2SS	(RATR_MCS8 | RATR_MCS9 | RATR_MCS10 |\
+							RATR_MCS11| RATR_MCS12 | RATR_MCS13 |\
+							RATR_MCS14 | RATR_MCS15)
+
+#define	BW_OPMODE_20MHZ				BIT(2)
+#define	BW_OPMODE_5G				BIT(1)
+#define	BW_OPMODE_11J				BIT(0)
+
+#define	CAM_VALID					BIT(15)
+#define	CAM_NOTVALID				0x0000
+#define	CAM_USEDK					BIT(5)
+
+#define	CAM_NONE					0x0
+#define	CAM_WEP40					0x01
+#define	CAM_TKIP					0x02
+#define	CAM_AES						0x04
+#define	CAM_WEP104					0x05
+
+#define	TOTAL_CAM_ENTRY				32
+#define	HALF_CAM_ENTRY				16
+
+#define	CAM_WRITE					BIT(16)
+#define	CAM_READ					0x00000000
+#define	CAM_POLLINIG				BIT(31)
+
+#define	SCR_USEDK					0x01
+#define	SCR_TXSEC_ENABLE			0x02
+#define	SCR_RXSEC_ENABLE			0x04
+
+#define	WOW_PMEN					BIT(0)
+#define	WOW_WOMEN					BIT(1)
+#define	WOW_MAGIC					BIT(2)
+#define	WOW_UWF						BIT(3)
+
+/*********************************************
+*       8188 IMR/ISR bits
+**********************************************/
+#define	IMR_DISABLED			0x0
+/* IMR DW0(0x0060-0063) Bit 0-31 */
+#define	IMR_TXCCK				BIT(30)		/* TXRPT interrupt when CCX bit of the packet is set	*/
+#define	IMR_PSTIMEOUT			BIT(29)		/* Power Save Time Out Interrupt */
+#define	IMR_GTINT4				BIT(28)		/* When GTIMER4 expires, this bit is set to 1	*/
+#define	IMR_GTINT3				BIT(27)		/* When GTIMER3 expires, this bit is set to 1	*/
+#define	IMR_TBDER				BIT(26)		/* Transmit Beacon0 Error			*/
+#define	IMR_TBDOK				BIT(25)		/* Transmit Beacon0 OK			*/
+#define	IMR_TSF_BIT32_TOGGLE		BIT(24)		/* TSF Timer BIT32 toggle indication interrupt		*/
+#define	IMR_BCNDMAINT0			BIT(20)		/* Beacon DMA Interrupt 0			*/
+#define	IMR_BCNDOK0				BIT(16)		/* Beacon Queue DMA OK0			*/
+#define	IMR_HSISR_IND_ON_INT		BIT(15)		/* HSISR Indicator (HSIMR & HSISR is true, this bit is set to 1)		*/
+#define	IMR_BCNDMAINT_E			BIT(14)		/* Beacon DMA Interrupt Extension for Win7			*/
+#define	IMR_ATIMEND				BIT(12)		/* CTWidnow End or ATIM Window End */
+#define	IMR_HISR1_IND_INT			BIT(11)		/* HISR1 Indicator (HISR1 & HIMR1 is true, this bit is set to 1)*/
+#define	IMR_C2HCMD				BIT(10)		/* CPU to Host Command INT Status, Write 1 clear	*/
+#define	IMR_CPWM2			BIT(9)			/* CPU power Mode exchange INT Status, Write 1 clear	*/
+#define	IMR_CPWM				BIT(8)			/* CPU power Mode exchange INT Status, Write 1 clear	*/
+#define	IMR_HIGHDOK				BIT(7)			/* High Queue DMA OK	*/
+#define	IMR_MGNTDOK				BIT(6)			/* Management Queue DMA OK	*/
+#define	IMR_BKDOK				BIT(5)			/* AC_BK DMA OK		*/
+#define	IMR_BEDOK				BIT(4)		/* AC_BE DMA OK	*/
+#define	IMR_VIDOK				BIT(3)			/* AC_VI DMA OK	*/
+#define	IMR_VODOK				BIT(2)			/* AC_VO DMA OK	*/
+#define	IMR_RDU				BIT(1)			/* Rx Descriptor Unavailable	*/
+#define	IMR_ROK				BIT(0)			/* Receive DMA OK */
+
+/* IMR DW1(0x00B4-00B7) Bit 0-31 */
+#define	IMR_BCNDMAINT7			BIT(27)		/* Beacon DMA Interrupt 7 	*/
+#define	IMR_BCNDMAINT6			BIT(26)		/* Beacon DMA Interrupt 6		*/
+#define	IMR_BCNDMAINT5			BIT(25)		/* Beacon DMA Interrupt 5		*/
+#define	IMR_BCNDMAINT4			BIT(24)		/* Beacon DMA Interrupt 4		*/
+#define	IMR_BCNDMAINT3			BIT(23)		/* Beacon DMA Interrupt 3		*/
+#define	IMR_BCNDMAINT2			BIT(22)		/* Beacon DMA Interrupt 2		*/
+#define	IMR_BCNDMAINT1			BIT(21)		/* Beacon DMA Interrupt 1		*/
+#define	IMR_BCNDOK7				BIT(20)		/* Beacon Queue DMA OK Interrup 7 */
+#define	IMR_BCNDOK6				BIT(19)		/* Beacon Queue DMA OK Interrup 6 */
+#define	IMR_BCNDOK5				BIT(18)		/* Beacon Queue DMA OK Interrup 5 */
+#define	IMR_BCNDOK4				BIT(17)		/* Beacon Queue DMA OK Interrup 4 */
+#define	IMR_BCNDOK3				BIT(16)		/* Beacon Queue DMA OK Interrup 3 */
+#define	IMR_BCNDOK2				BIT(15)		/* Beacon Queue DMA OK Interrup 2 */
+#define	IMR_BCNDOK1				BIT(14)		/* Beacon Queue DMA OK Interrup 1 */
+#define	IMR_ATIMEND_E		BIT(13)		/* ATIM Window End Extension for Win7 */
+#define	IMR_TXERR				BIT(11)		/* Tx Error Flag Interrupt Status, write 1 clear. */
+#define	IMR_RXERR				BIT(10)		/* Rx Error Flag INT Status, Write 1 clear */
+#define	IMR_TXFOVW				BIT(9)			/* Transmit FIFO Overflow */
+#define	IMR_RXFOVW				BIT(8)			/* Receive FIFO Overflow */
+
+
+#define	HWSET_MAX_SIZE				512
+#define   EFUSE_MAX_SECTION			64
+#define   EFUSE_REAL_CONTENT_LEN			256
+#define 	EFUSE_OOB_PROTECT_BYTES    	18 	/* PG data exclude header, dummy 7 bytes frome CP test and reserved 1byte.*/
+
+
+#define	EEPROM_DEFAULT_TSSI					0x0
+#define EEPROM_DEFAULT_TXPOWERDIFF			0x0
+#define EEPROM_DEFAULT_CRYSTALCAP			0x5
+#define EEPROM_DEFAULT_BOARDTYPE			0x02
+#define EEPROM_DEFAULT_TXPOWER				0x1010
+#define	EEPROM_DEFAULT_HT2T_TXPWR			0x10
+
+#define	EEPROM_DEFAULT_LEGACYHTTXPOWERDIFF	0x3
+#define	EEPROM_DEFAULT_THERMALMETER			0x18
+#define	EEPROM_DEFAULT_ANTTXPOWERDIFF		0x0
+#define	EEPROM_DEFAULT_TXPWDIFF_CRYSTALCAP	0x5
+#define	EEPROM_DEFAULT_TXPOWERLEVEL			0x22
+#define	EEPROM_DEFAULT_HT40_2SDIFF			0x0
+#define EEPROM_DEFAULT_HT20_DIFF			2
+#define	EEPROM_DEFAULT_LEGACYHTTXPOWERDIFF	0x3
+#define EEPROM_DEFAULT_HT40_PWRMAXOFFSET	0
+#define EEPROM_DEFAULT_HT20_PWRMAXOFFSET	0
+
+#define RF_OPTION1							0x79
+#define RF_OPTION2							0x7A
+#define RF_OPTION3							0x7B
+#define RF_OPTION4							0xC3
+
+#define EEPROM_DEFAULT_PID					0x1234
+#define EEPROM_DEFAULT_VID					0x5678
+#define EEPROM_DEFAULT_CUSTOMERID			0xAB
+#define EEPROM_DEFAULT_SUBCUSTOMERID		0xCD
+#define EEPROM_DEFAULT_VERSION				0
+
+#define	EEPROM_CHANNEL_PLAN_FCC				0x0
+#define	EEPROM_CHANNEL_PLAN_IC				0x1
+#define	EEPROM_CHANNEL_PLAN_ETSI			0x2
+#define	EEPROM_CHANNEL_PLAN_SPAIN			0x3
+#define	EEPROM_CHANNEL_PLAN_FRANCE			0x4
+#define	EEPROM_CHANNEL_PLAN_MKK				0x5
+#define	EEPROM_CHANNEL_PLAN_MKK1			0x6
+#define	EEPROM_CHANNEL_PLAN_ISRAEL			0x7
+#define	EEPROM_CHANNEL_PLAN_TELEC			0x8
+#define	EEPROM_CHANNEL_PLAN_GLOBAL_DOMAIN	0x9
+#define	EEPROM_CHANNEL_PLAN_WORLD_WIDE_13	0xA
+#define	EEPROM_CHANNEL_PLAN_NCC				0xB
+#define	EEPROM_CHANNEL_PLAN_BY_HW_MASK		0x80
+
+#define EEPROM_CID_DEFAULT					0x0
+#define EEPROM_CID_TOSHIBA					0x4
+#define	EEPROM_CID_CCX						0x10
+#define	EEPROM_CID_QMI						0x0D
+#define EEPROM_CID_WHQL 					0xFE
+
+#define	RTL_EEPROM_ID					0x8129
+
+#define EEPROM_HPON							0x02
+#define EEPROM_CLK							0x06
+#define EEPROM_TESTR						0x08
+
+
+#define EEPROM_TXPOWERCCK			0x10
+#define	EEPROM_TXPOWERHT40_1S		0x16
+#define EEPROM_TXPOWERHT20DIFF		0x1B
+#define EEPROM_TXPOWER_OFDMDIFF		0x1B
+
+
+
+#define	EEPROM_TX_PWR_INX				0x10
+
+#define	EEPROM_CHANNELPLAN					0xB8
+#define	EEPROM_XTAL_8821AE					0xB9
+#define	EEPROM_THERMAL_METER				0xBA
+#define	EEPROM_IQK_LCK_88E					0xBB
+
+#define	EEPROM_RF_BOARD_OPTION			0xC1
+#define	EEPROM_RF_FEATURE_OPTION_88E		0xC2
+#define	EEPROM_RF_BT_SETTING				0xC3
+#define	EEPROM_VERSION					0xC4
+#define	EEPROM_CUSTOMER_ID					0xC5
+#define	EEPROM_RF_ANTENNA_OPT_88E			0xC9
+
+#define	EEPROM_MAC_ADDR					0xD0
+#define EEPROM_VID							0xD6
+#define EEPROM_DID							0xD8
+#define EEPROM_SVID							0xDA
+#define EEPROM_SMID						0xDC
+
+#define	STOPBECON					BIT(6)
+#define	STOPHIGHT					BIT(5)
+#define	STOPMGT						BIT(4)
+#define	STOPVO						BIT(3)
+#define	STOPVI						BIT(2)
+#define	STOPBE						BIT(1)
+#define	STOPBK						BIT(0)
+
+#define	RCR_APPFCS					BIT(31)
+#define	RCR_APP_MIC					BIT(30)
+#define	RCR_APP_ICV					BIT(29)
+#define	RCR_APP_PHYST_RXFF			BIT(28)
+#define	RCR_APP_BA_SSN				BIT(27)
+#define	RCR_NONQOS_VHT				BIT(26)
+#define	RCR_ENMBID					BIT(24)
+#define	RCR_LSIGEN					BIT(23)
+#define	RCR_MFBEN					BIT(22)
+#define	RCR_HTC_LOC_CTRL			BIT(14)
+#define	RCR_AMF						BIT(13)
+#define	RCR_ACF						BIT(12)
+#define	RCR_ADF						BIT(11)
+#define	RCR_AICV					BIT(9)
+#define	RCR_ACRC32					BIT(8)
+#define	RCR_CBSSID_BCN				BIT(7)
+#define	RCR_CBSSID_DATA				BIT(6)
+#define	RCR_CBSSID					RCR_CBSSID_DATA
+#define	RCR_APWRMGT					BIT(5)
+#define	RCR_ADD3					BIT(4)
+#define	RCR_AB						BIT(3)
+#define	RCR_AM						BIT(2)
+#define	RCR_APM						BIT(1)
+#define	RCR_AAP						BIT(0)
+#define	RCR_MXDMA_OFFSET			8
+#define	RCR_FIFO_OFFSET				13
+
+#define RSV_CTRL					0x001C
+#define RD_CTRL						0x0524
+
+#define REG_USB_INFO				0xFE17
+#define REG_USB_SPECIAL_OPTION		0xFE55
+#define REG_USB_DMA_AGG_TO			0xFE5B
+#define REG_USB_AGG_TO				0xFE5C
+#define REG_USB_AGG_TH				0xFE5D
+
+#define REG_USB_VID					0xFE60
+#define REG_USB_PID					0xFE62
+#define REG_USB_OPTIONAL			0xFE64
+#define REG_USB_CHIRP_K				0xFE65
+#define REG_USB_PHY					0xFE66
+#define REG_USB_MAC_ADDR			0xFE70
+#define REG_USB_HRPWM				0xFE58
+#define REG_USB_HCPWM				0xFE57
+
+#define SW18_FPWM					BIT(3)
+
+#define ISO_MD2PP					BIT(0)
+#define ISO_UA2USB					BIT(1)
+#define ISO_UD2CORE					BIT(2)
+#define ISO_PA2PCIE					BIT(3)
+#define ISO_PD2CORE					BIT(4)
+#define ISO_IP2MAC					BIT(5)
+#define ISO_DIOP					BIT(6)
+#define ISO_DIOE					BIT(7)
+#define ISO_EB2CORE					BIT(8)
+#define ISO_DIOR					BIT(9)
+
+#define PWC_EV25V					BIT(14)
+#define PWC_EV12V					BIT(15)
+
+#define FEN_BBRSTB					BIT(0)
+#define FEN_BB_GLB_RSTN				BIT(1)
+#define FEN_USBA					BIT(2)
+#define FEN_UPLL					BIT(3)
+#define FEN_USBD					BIT(4)
+#define FEN_DIO_PCIE				BIT(5)
+#define FEN_PCIEA					BIT(6)
+#define FEN_PPLL					BIT(7)
+#define FEN_PCIED					BIT(8)
+#define FEN_DIOE					BIT(9)
+#define FEN_CPUEN					BIT(10)
+#define FEN_DCORE					BIT(11)
+#define FEN_ELDR					BIT(12)
+#define FEN_DIO_RF					BIT(13)
+#define FEN_HWPDN					BIT(14)
+#define FEN_MREGEN					BIT(15)
+
+#define PFM_LDALL					BIT(0)
+#define PFM_ALDN					BIT(1)
+#define PFM_LDKP					BIT(2)
+#define PFM_WOWL					BIT(3)
+#define EnPDN						BIT(4)
+#define PDN_PL						BIT(5)
+#define APFM_ONMAC					BIT(8)
+#define APFM_OFF					BIT(9)
+#define APFM_RSM					BIT(10)
+#define AFSM_HSUS					BIT(11)
+#define AFSM_PCIE					BIT(12)
+#define APDM_MAC					BIT(13)
+#define APDM_HOST					BIT(14)
+#define APDM_HPDN					BIT(15)
+#define RDY_MACON					BIT(16)
+#define SUS_HOST					BIT(17)
+#define ROP_ALD						BIT(20)
+#define ROP_PWR						BIT(21)
+#define ROP_SPS						BIT(22)
+#define SOP_MRST					BIT(25)
+#define SOP_FUSE					BIT(26)
+#define SOP_ABG						BIT(27)
+#define SOP_AMB						BIT(28)
+#define SOP_RCK						BIT(29)
+#define SOP_A8M						BIT(30)
+#define XOP_BTCK					BIT(31)
+
+#define ANAD16V_EN					BIT(0)
+#define ANA8M						BIT(1)
+#define MACSLP						BIT(4)
+#define LOADER_CLK_EN				BIT(5)
+#define _80M_SSC_DIS				BIT(7)
+#define _80M_SSC_EN_HO				BIT(8)
+#define PHY_SSC_RSTB				BIT(9)
+#define SEC_CLK_EN					BIT(10)
+#define MAC_CLK_EN					BIT(11)
+#define SYS_CLK_EN					BIT(12)
+#define RING_CLK_EN					BIT(13)
+
+#define	BOOT_FROM_EEPROM			BIT(4)
+#define	EEPROM_EN					BIT(5)
+
+#define AFE_BGEN					BIT(0)
+#define AFE_MBEN					BIT(1)
+#define MAC_ID_EN					BIT(7)
+
+#define WLOCK_ALL					BIT(0)
+#define WLOCK_00					BIT(1)
+#define WLOCK_04					BIT(2)
+#define WLOCK_08					BIT(3)
+#define WLOCK_40					BIT(4)
+#define R_DIS_PRST_0				BIT(5)
+#define R_DIS_PRST_1				BIT(6)
+#define LOCK_ALL_EN					BIT(7)
+
+#define RF_EN						BIT(0)
+#define RF_RSTB						BIT(1)
+#define RF_SDMRSTB					BIT(2)
+
+#define LDA15_EN					BIT(0)
+#define LDA15_STBY					BIT(1)
+#define LDA15_OBUF					BIT(2)
+#define LDA15_REG_VOS				BIT(3)
+#define _LDA15_VOADJ(x)				(((x) & 0x7) << 4)
+
+#define LDV12_EN					BIT(0)
+#define LDV12_SDBY					BIT(1)
+#define LPLDO_HSM					BIT(2)
+#define LPLDO_LSM_DIS				BIT(3)
+#define _LDV12_VADJ(x)				(((x) & 0xF) << 4)
+
+#define XTAL_EN						BIT(0)
+#define XTAL_BSEL					BIT(1)
+#define _XTAL_BOSC(x)				(((x) & 0x3) << 2)
+#define _XTAL_CADJ(x)				(((x) & 0xF) << 4)
+#define XTAL_GATE_USB				BIT(8)
+#define _XTAL_USB_DRV(x)			(((x) & 0x3) << 9)
+#define XTAL_GATE_AFE				BIT(11)
+#define _XTAL_AFE_DRV(x)			(((x) & 0x3) << 12)
+#define XTAL_RF_GATE				BIT(14)
+#define _XTAL_RF_DRV(x)				(((x) & 0x3) << 15)
+#define XTAL_GATE_DIG				BIT(17)
+#define _XTAL_DIG_DRV(x)			(((x) & 0x3) << 18)
+#define XTAL_BT_GATE				BIT(20)
+#define _XTAL_BT_DRV(x)				(((x) & 0x3) << 21)
+#define _XTAL_GPIO(x)				(((x) & 0x7) << 23)
+
+#define CKDLY_AFE					BIT(26)
+#define CKDLY_USB					BIT(27)
+#define CKDLY_DIG					BIT(28)
+#define CKDLY_BT					BIT(29)
+
+#define APLL_EN						BIT(0)
+#define APLL_320_EN					BIT(1)
+#define APLL_FREF_SEL				BIT(2)
+#define APLL_EDGE_SEL				BIT(3)
+#define APLL_WDOGB					BIT(4)
+#define APLL_LPFEN					BIT(5)
+
+#define APLL_REF_CLK_13MHZ			0x1
+#define APLL_REF_CLK_19_2MHZ		0x2
+#define APLL_REF_CLK_20MHZ			0x3
+#define APLL_REF_CLK_25MHZ			0x4
+#define APLL_REF_CLK_26MHZ			0x5
+#define APLL_REF_CLK_38_4MHZ		0x6
+#define APLL_REF_CLK_40MHZ			0x7
+
+#define APLL_320EN					BIT(14)
+#define APLL_80EN					BIT(15)
+#define APLL_1MEN					BIT(24)
+
+#define ALD_EN						BIT(18)
+#define EF_PD						BIT(19)
+#define EF_FLAG						BIT(31)
+
+#define EF_TRPT						BIT(7)
+#define LDOE25_EN					BIT(31)
+
+#define RSM_EN						BIT(0)
+#define Timer_EN					BIT(4)
+
+#define TRSW0EN						BIT(2)
+#define TRSW1EN						BIT(3)
+#define EROM_EN						BIT(4)
+#define EnBT						BIT(5)
+#define EnUart						BIT(8)
+#define Uart_910					BIT(9)
+#define EnPMAC						BIT(10)
+#define SIC_SWRST					BIT(11)
+#define EnSIC						BIT(12)
+#define SIC_23						BIT(13)
+#define EnHDP						BIT(14)
+#define SIC_LBK						BIT(15)
+
+#define LED0PL 						BIT(4)
+#define LED1PL 						BIT(12)
+#define LED0DIS						BIT(7)
+
+#define MCUFWDL_EN					BIT(0)
+#define MCUFWDL_RDY					BIT(1)
+#define FWDL_CHKSUM_RPT				BIT(2)
+#define MACINI_RDY					BIT(3)
+#define BBINI_RDY					BIT(4)
+#define RFINI_RDY					BIT(5)
+#define WINTINI_RDY					BIT(6)
+#define CPRST						BIT(23)
+
+#define XCLK_VLD					BIT(0)
+#define ACLK_VLD					BIT(1)
+#define UCLK_VLD					BIT(2)
+#define PCLK_VLD					BIT(3)
+#define PCIRSTB						BIT(4)
+#define V15_VLD						BIT(5)
+#define TRP_B15V_EN					BIT(7)
+#define SIC_IDLE					BIT(8)
+#define BD_MAC2						BIT(9)
+#define BD_MAC1						BIT(10)
+#define IC_MACPHY_MODE				BIT(11)
+#define VENDOR_ID					BIT(19)
+#define PAD_HWPD_IDN				BIT(22)
+#define TRP_VAUX_EN					BIT(23)
+#define TRP_BT_EN					BIT(24)
+#define BD_PKG_SEL					BIT(25)
+#define BD_HCI_SEL					BIT(26)
+#define TYPE_ID						BIT(27)
+
+#define CHIP_VER_RTL_MASK			0xF000
+#define CHIP_VER_RTL_SHIFT			12
+
+#define REG_LBMODE					(REG_CR + 3)
+
+#define HCI_TXDMA_EN				BIT(0)
+#define HCI_RXDMA_EN				BIT(1)
+#define TXDMA_EN					BIT(2)
+#define RXDMA_EN					BIT(3)
+#define PROTOCOL_EN					BIT(4)
+#define SCHEDULE_EN					BIT(5)
+#define MACTXEN						BIT(6)
+#define MACRXEN						BIT(7)
+#define ENSWBCN						BIT(8)
+#define ENSEC						BIT(9)
+
+#define _NETTYPE(x)					(((x) & 0x3) << 16)
+#define MASK_NETTYPE				0x30000
+#define NT_NO_LINK					0x0
+#define NT_LINK_AD_HOC				0x1
+#define NT_LINK_AP					0x2
+#define NT_AS_AP					0x3
+
+#define _LBMODE(x)					(((x) & 0xF) << 24)
+#define MASK_LBMODE					0xF000000
+#define LOOPBACK_NORMAL				0x0
+#define LOOPBACK_IMMEDIATELY		0xB
+#define LOOPBACK_MAC_DELAY			0x3
+#define LOOPBACK_PHY				0x1
+#define LOOPBACK_DMA				0x7
+
+#define GET_RX_PAGE_SIZE(value)		((value) & 0xF)
+#define GET_TX_PAGE_SIZE(value)		(((value) & 0xF0) >> 4)
+#define _PSRX_MASK					0xF
+#define _PSTX_MASK					0xF0
+#define _PSRX(x)					(x)
+#define _PSTX(x)					((x) << 4)
+
+#define PBP_64						0x0
+#define PBP_128						0x1
+#define PBP_256						0x2
+#define PBP_512						0x3
+#define PBP_1024					0x4
+
+#define RXDMA_ARBBW_EN				BIT(0)
+#define RXSHFT_EN					BIT(1)
+#define RXDMA_AGG_EN				BIT(2)
+#define QS_VO_QUEUE					BIT(8)
+#define QS_VI_QUEUE					BIT(9)
+#define QS_BE_QUEUE					BIT(10)
+#define QS_BK_QUEUE					BIT(11)
+#define QS_MANAGER_QUEUE			BIT(12)
+#define QS_HIGH_QUEUE				BIT(13)
+
+#define HQSEL_VOQ					BIT(0)
+#define HQSEL_VIQ					BIT(1)
+#define HQSEL_BEQ					BIT(2)
+#define HQSEL_BKQ					BIT(3)
+#define HQSEL_MGTQ					BIT(4)
+#define HQSEL_HIQ					BIT(5)
+
+#define _TXDMA_HIQ_MAP(x) 	 		(((x)&0x3) << 14)
+#define _TXDMA_MGQ_MAP(x) 	 		(((x)&0x3) << 12)
+#define _TXDMA_BKQ_MAP(x) 	 		(((x)&0x3) << 10)
+#define _TXDMA_BEQ_MAP(x) 	 		(((x)&0x3) << 8 )
+#define _TXDMA_VIQ_MAP(x) 	 		(((x)&0x3) << 6 )
+#define _TXDMA_VOQ_MAP(x) 	 		(((x)&0x3) << 4 )
+
+#define QUEUE_LOW					1
+#define QUEUE_NORMAL				2
+#define QUEUE_HIGH					3
+
+#define _LLT_NO_ACTIVE				0x0
+#define _LLT_WRITE_ACCESS			0x1
+#define _LLT_READ_ACCESS			0x2
+
+#define _LLT_INIT_DATA(x)			((x) & 0xFF)
+#define _LLT_INIT_ADDR(x)			(((x) & 0xFF) << 8)
+#define _LLT_OP(x)					(((x) & 0x3) << 30)
+#define _LLT_OP_VALUE(x)			(((x) >> 30) & 0x3)
+
+#define BB_WRITE_READ_MASK			(BIT(31) | BIT(30))
+#define BB_WRITE_EN					BIT(30)
+#define BB_READ_EN					BIT(31)
+
+#define _HPQ(x)			((x) & 0xFF)
+#define _LPQ(x)			(((x) & 0xFF) << 8)
+#define _PUBQ(x)		(((x) & 0xFF) << 16)
+#define _NPQ(x)			((x) & 0xFF)
+
+#define HPQ_PUBLIC_DIS		BIT(24)
+#define LPQ_PUBLIC_DIS		BIT(25)
+#define LD_RQPN			BIT(31)
+
+#define BCN_VALID		BIT(16)
+#define BCN_HEAD(x)		(((x) & 0xFF) << 8)
+#define	BCN_HEAD_MASK		0xFF00
+
+#define BLK_DESC_NUM_SHIFT			4
+#define BLK_DESC_NUM_MASK			0xF
+
+#define DROP_DATA_EN				BIT(9)
+
+#define EN_AMPDU_RTY_NEW			BIT(7)
+
+#define _INIRTSMCS_SEL(x)			((x) & 0x3F)
+
+#define _SPEC_SIFS_CCK(x)			((x) & 0xFF)
+#define _SPEC_SIFS_OFDM(x)			(((x) & 0xFF) << 8)
+
+#define RATE_REG_BITMAP_ALL			0xFFFFF
+
+#define _RRSC_BITMAP(x)				((x) & 0xFFFFF)
+
+#define _RRSR_RSC(x)				(((x) & 0x3) << 21)
+#define RRSR_RSC_RESERVED			0x0
+#define RRSR_RSC_UPPER_SUBCHANNEL	0x1
+#define RRSR_RSC_LOWER_SUBCHANNEL	0x2
+#define RRSR_RSC_DUPLICATE_MODE		0x3
+
+#define USE_SHORT_G1				BIT(20)
+
+#define _AGGLMT_MCS0(x)				((x) & 0xF)
+#define _AGGLMT_MCS1(x)				(((x) & 0xF) << 4)
+#define _AGGLMT_MCS2(x)				(((x) & 0xF) << 8)
+#define _AGGLMT_MCS3(x)				(((x) & 0xF) << 12)
+#define _AGGLMT_MCS4(x)				(((x) & 0xF) << 16)
+#define _AGGLMT_MCS5(x)				(((x) & 0xF) << 20)
+#define _AGGLMT_MCS6(x)				(((x) & 0xF) << 24)
+#define _AGGLMT_MCS7(x)				(((x) & 0xF) << 28)
+
+#define	RETRY_LIMIT_SHORT_SHIFT		8
+#define	RETRY_LIMIT_LONG_SHIFT		0
+
+#define _DARF_RC1(x)				((x) & 0x1F)
+#define _DARF_RC2(x)				(((x) & 0x1F) << 8)
+#define _DARF_RC3(x)				(((x) & 0x1F) << 16)
+#define _DARF_RC4(x)				(((x) & 0x1F) << 24)
+#define _DARF_RC5(x)				((x) & 0x1F)
+#define _DARF_RC6(x)				(((x) & 0x1F) << 8)
+#define _DARF_RC7(x)				(((x) & 0x1F) << 16)
+#define _DARF_RC8(x)				(((x) & 0x1F) << 24)
+
+#define _RARF_RC1(x)				((x) & 0x1F)
+#define _RARF_RC2(x)				(((x) & 0x1F) << 8)
+#define _RARF_RC3(x)				(((x) & 0x1F) << 16)
+#define _RARF_RC4(x)				(((x) & 0x1F) << 24)
+#define _RARF_RC5(x)				((x) & 0x1F)
+#define _RARF_RC6(x)				(((x) & 0x1F) << 8)
+#define _RARF_RC7(x)				(((x) & 0x1F) << 16)
+#define _RARF_RC8(x)				(((x) & 0x1F) << 24)
+
+#define AC_PARAM_TXOP_LIMIT_OFFSET	16
+#define AC_PARAM_ECW_MAX_OFFSET		12
+#define AC_PARAM_ECW_MIN_OFFSET		8
+#define AC_PARAM_AIFS_OFFSET		0
+
+#define _AIFS(x)					(x)
+#define _ECW_MAX_MIN(x)				((x) << 8)
+#define _TXOP_LIMIT(x)				((x) << 16)
+
+#define _BCNIFS(x)					((x) & 0xFF)
+#define _BCNECW(x)					((((x) & 0xF))<< 8)
+
+#define _LRL(x)						((x) & 0x3F)
+#define _SRL(x)						(((x) & 0x3F) << 8)
+
+#define _SIFS_CCK_CTX(x)			((x) & 0xFF)
+#define _SIFS_CCK_TRX(x)			(((x) & 0xFF) << 8);
+
+#define _SIFS_OFDM_CTX(x)			((x) & 0xFF)
+#define _SIFS_OFDM_TRX(x)			(((x) & 0xFF) << 8);
+
+#define _TBTT_PROHIBIT_HOLD(x)		(((x) & 0xFF) << 8)
+
+#define DIS_EDCA_CNT_DWN			BIT(11)
+
+#define EN_MBSSID					BIT(1)
+#define EN_TXBCN_RPT				BIT(2)
+#define	EN_BCN_FUNCTION				BIT(3)
+
+#define TSFTR_RST					BIT(0)
+#define TSFTR1_RST					BIT(1)
+
+#define STOP_BCNQ					BIT(6)
+
+#define	DIS_TSF_UDT0_NORMAL_CHIP	BIT(4)
+#define	DIS_TSF_UDT0_TEST_CHIP		BIT(5)
+
+#define	AcmHw_HwEn					BIT(0)
+#define	AcmHw_BeqEn					BIT(1)
+#define	AcmHw_ViqEn					BIT(2)
+#define	AcmHw_VoqEn					BIT(3)
+#define	AcmHw_BeqStatus				BIT(4)
+#define	AcmHw_ViqStatus				BIT(5)
+#define	AcmHw_VoqStatus				BIT(6)
+
+#define APSDOFF						BIT(6)
+#define APSDOFF_STATUS				BIT(7)
+
+#define BW_20MHZ					BIT(2)
+
+#define RATE_BITMAP_ALL				0xFFFFF
+
+#define RATE_RRSR_CCK_ONLY_1M		0xFFFF1
+
+#define TSFRST						BIT(0)
+#define DIS_GCLK					BIT(1)
+#define PAD_SEL						BIT(2)
+#define PWR_ST						BIT(6)
+#define PWRBIT_OW_EN				BIT(7)
+#define ACRC						BIT(8)
+#define CFENDFORM					BIT(9)
+#define ICV							BIT(10)
+
+#define AAP							BIT(0)
+#define APM							BIT(1)
+#define AM							BIT(2)
+#define AB							BIT(3)
+#define ADD3						BIT(4)
+#define APWRMGT						BIT(5)
+#define CBSSID						BIT(6)
+#define CBSSID_DATA					BIT(6)
+#define CBSSID_BCN					BIT(7)
+#define ACRC32						BIT(8)
+#define AICV						BIT(9)
+#define ADF							BIT(11)
+#define ACF							BIT(12)
+#define AMF							BIT(13)
+#define HTC_LOC_CTRL				BIT(14)
+#define UC_DATA_EN					BIT(16)
+#define BM_DATA_EN					BIT(17)
+#define MFBEN						BIT(22)
+#define LSIGEN						BIT(23)
+#define EnMBID						BIT(24)
+#define APP_BASSN					BIT(27)
+#define APP_PHYSTS					BIT(28)
+#define APP_ICV						BIT(29)
+#define APP_MIC						BIT(30)
+#define APP_FCS						BIT(31)
+
+#define _MIN_SPACE(x)				((x) & 0x7)
+#define _SHORT_GI_PADDING(x)		(((x) & 0x1F) << 3)
+
+#define RXERR_TYPE_OFDM_PPDU		0
+#define RXERR_TYPE_OFDM_FALSE_ALARM	1
+#define	RXERR_TYPE_OFDM_MPDU_OK		2
+#define RXERR_TYPE_OFDM_MPDU_FAIL	3
+#define RXERR_TYPE_CCK_PPDU			4
+#define RXERR_TYPE_CCK_FALSE_ALARM	5
+#define RXERR_TYPE_CCK_MPDU_OK		6
+#define RXERR_TYPE_CCK_MPDU_FAIL	7
+#define RXERR_TYPE_HT_PPDU			8
+#define RXERR_TYPE_HT_FALSE_ALARM	9
+#define RXERR_TYPE_HT_MPDU_TOTAL	10
+#define RXERR_TYPE_HT_MPDU_OK		11
+#define RXERR_TYPE_HT_MPDU_FAIL		12
+#define RXERR_TYPE_RX_FULL_DROP		15
+
+#define RXERR_COUNTER_MASK			0xFFFFF
+#define RXERR_RPT_RST				BIT(27)
+#define _RXERR_RPT_SEL(type)		((type) << 28)
+
+#define	SCR_TxUseDK					BIT(0)
+#define	SCR_RxUseDK					BIT(1)
+#define	SCR_TxEncEnable				BIT(2)
+#define	SCR_RxDecEnable				BIT(3)
+#define	SCR_SKByA2					BIT(4)
+#define	SCR_NoSKMC					BIT(5)
+#define SCR_TXBCUSEDK				BIT(6)
+#define SCR_RXBCUSEDK				BIT(7)
+
+#define XCLK_VLD					BIT(0)
+#define ACLK_VLD					BIT(1)
+#define UCLK_VLD					BIT(2)
+#define PCLK_VLD					BIT(3)
+#define PCIRSTB						BIT(4)
+#define V15_VLD						BIT(5)
+#define TRP_B15V_EN					BIT(7)
+#define SIC_IDLE					BIT(8)
+#define BD_MAC2						BIT(9)
+#define BD_MAC1						BIT(10)
+#define IC_MACPHY_MODE				BIT(11)
+#define BT_FUNC						BIT(16)
+#define VENDOR_ID					BIT(19)
+#define PAD_HWPD_IDN				BIT(22)
+#define TRP_VAUX_EN					BIT(23)
+#define TRP_BT_EN					BIT(24)
+#define BD_PKG_SEL					BIT(25)
+#define BD_HCI_SEL					BIT(26)
+#define TYPE_ID						BIT(27)
+
+#define USB_IS_HIGH_SPEED			0
+#define USB_IS_FULL_SPEED			1
+#define USB_SPEED_MASK				BIT(5)
+
+#define USB_NORMAL_SIE_EP_MASK		0xF
+#define USB_NORMAL_SIE_EP_SHIFT		4
+
+#define USB_TEST_EP_MASK			0x30
+#define USB_TEST_EP_SHIFT			4
+
+#define USB_AGG_EN					BIT(3)
+
+#define MAC_ADDR_LEN				6
+#define LAST_ENTRY_OF_TX_PKT_BUFFER	175/*255    88e*/
+
+#define POLLING_LLT_THRESHOLD		20
+#define POLLING_READY_TIMEOUT_COUNT		3000
+
+#define	MAX_MSS_DENSITY_2T			0x13
+#define	MAX_MSS_DENSITY_1T			0x0A
+
+#define EPROM_CMD_OPERATING_MODE_MASK 	((1<<7)|(1<<6))
+#define EPROM_CMD_CONFIG 			0x3
+#define EPROM_CMD_LOAD 				1
+
+#define	HWSET_MAX_SIZE_92S			HWSET_MAX_SIZE
+
+#define	HAL_8192C_HW_GPIO_WPS_BIT	BIT(2)
+
+#define RA_LSSIWRITE_8821A			0xc90
+#define RB_LSSIWRITE_8821A			0xe90
+
+#define	RA_PIREAD_8821A		0xd04
+#define	RB_PIREAD_8821A		0xd44
+#define	RA_SIREAD_8821A		0xd08
+#define	RB_SIREAD_8821A		0xd48
+
+#define	RPMAC_RESET					0x100
+#define	RPMAC_TXSTART				0x104
+#define	RPMAC_TXLEGACYSIG			0x108
+#define	RPMAC_TXHTSIG1				0x10c
+#define	RPMAC_TXHTSIG2				0x110
+#define	RPMAC_PHYDEBUG				0x114
+#define	RPMAC_TXPACKETNUM			0x118
+#define	RPMAC_TXIDLE				0x11c
+#define	RPMAC_TXMACHEADER0			0x120
+#define	RPMAC_TXMACHEADER1			0x124
+#define	RPMAC_TXMACHEADER2			0x128
+#define	RPMAC_TXMACHEADER3			0x12c
+#define	RPMAC_TXMACHEADER4			0x130
+#define	RPMAC_TXMACHEADER5			0x134
+#define	RPMAC_TXDADATYPE			0x138
+#define	RPMAC_TXRANDOMSEED			0x13c
+#define	RPMAC_CCKPLCPPREAMBLE		0x140
+#define	RPMAC_CCKPLCPHEADER			0x144
+#define	RPMAC_CCKCRC16				0x148
+#define	RPMAC_OFDMRXCRC32OK			0x170
+#define	RPMAC_OFDMRXCRC32Er			0x174
+#define	RPMAC_OFDMRXPARITYER		0x178
+#define	RPMAC_OFDMRXCRC8ER			0x17c
+#define	RPMAC_CCKCRXRC16ER			0x180
+#define	RPMAC_CCKCRXRC32ER			0x184
+#define	RPMAC_CCKCRXRC32OK			0x188
+#define	RPMAC_TXSTATUS				0x18c
+
+#define	RFPGA0_RFMOD				0x800
+
+#define	RFPGA0_TXINFO				0x804
+#define	RFPGA0_PSDFUNCTION			0x808
+
+#define	RFPGA0_TXGAINSTAGE			0x80c
+
+#define	RFPGA0_RFTIMING1			0x810
+#define	RFPGA0_RFTIMING2			0x814
+
+#define	RFPGA0_XA_HSSIPARAMETER1	0x820
+#define	RFPGA0_XA_HSSIPARAMETER2	0x824
+#define	RFPGA0_XB_HSSIPARAMETER1	0x828
+#define	RFPGA0_XB_HSSIPARAMETER2	0x82c
+#define 	RCCAONSEC					0x838
+
+#define	RFPGA0_XA_LSSIPARAMETER		0x840
+#define	RFPGA0_XB_LSSIPARAMETER		0x844
+#define	RL1PEAKTH					0x848
+
+#define	RFPGA0_RFWAKEUPPARAMETER	0x850
+#define	RFPGA0_RFSLEEPUPPARAMETER	0x854
+
+#define	RFPGA0_XAB_SWITCHCONTROL	0x858
+#define	RFPGA0_XCD_SWITCHCONTROL	0x85c
+
+#define	RFPGA0_XA_RFINTERFACEOE		0x860
+#define RFC_AREA					0x860
+#define	RFPGA0_XB_RFINTERFACEOE		0x864
+
+#define	RFPGA0_XAB_RFINTERFACESW	0x870
+#define	RFPGA0_XCD_RFINTERFACESW	0x874
+
+#define	rFPGA0_XAB_RFPARAMETER		0x878
+#define	rFPGA0_XCD_RFPARAMETER		0x87c
+
+#define	RFPGA0_ANALOGPARAMETER1		0x880
+#define	RFPGA0_ANALOGPARAMETER2		0x884
+#define	RFPGA0_ANALOGPARAMETER3		0x888
+#define	RFPGA0_ANALOGPARAMETER4		0x88c
+
+#define	RFPGA0_XA_LSSIREADBACK		0x8a0
+#define	RFPGA0_XB_LSSIREADBACK		0x8a4
+#define	RFPGA0_XC_LSSIREADBACK		0x8a8
+//#define	RFPGA0_XD_LSSIREADBACK		0x8ac
+#define RRFMOD						0x8ac
+#define	RHSSIREAD_8821AE			0x8b0
+
+#define	RFPGA0_PSDREPORT			0x8b4
+#define	TRANSCEIVEA_HSPI_READBACK	0x8b8
+#define	TRANSCEIVEB_HSPI_READBACK	0x8bc
+//#define	REG_SC_CNT					0x8c4
+#define RADC_BUF_CLK				0x8c4
+#define	RFPGA0_XAB_RFINTERFACERB	0x8e0
+#define	RFPGA0_XCD_RFINTERFACERB	0x8e4
+
+#define	RFPGA1_RFMOD				0x900
+
+#define	RFPGA1_TXBLOCK				0x904
+#define	RFPGA1_DEBUGSELECT			0x908
+#define	RFPGA1_TXINFO				0x90c
+
+#define	RCCK_SYSTEM					0xa00
+#define	BCCK_SYSTEM					0x10
+
+
+#define	RCCK0_AFESETTING			0xa04
+#define	RCCK0_CCA					0xa08
+
+#define	RCCK0_RXAGC1				0xa0c
+#define	RCCK0_RXAGC2				0xa10
+
+#define	RCCK0_RXHP					0xa14
+
+#define	RCCK0_DSPPARAMETER1			0xa18
+#define	RCCK0_DSPPARAMETER2			0xa1c
+
+#define	RCCK0_TXFILTER1				0xa20
+#define	RCCK0_TXFILTER2				0xa24
+#define	RCCK0_DEBUGPORT				0xa28
+#define	RCCK0_FALSEALARMREPORT		0xa2c
+#define	RCCK0_TRSSIREPORT         	0xa50
+#define	RCCK0_RXREPORT            	0xa54
+#define	RCCK0_FACOUNTERLOWER      	0xa5c
+#define	RCCK0_FACOUNTERUPPER      	0xa58
+#define	RCCK0_CCA_CNT			0xa60
+
+
+/* PageB(0xB00) */
+#define	rPdp_AntA      					0xb00
+#define	rPdp_AntA_4    				0xb04
+#define	rPdp_AntA_8    				0xb08
+#define	rPdp_AntA_C    				0xb0c
+#define	rPdp_AntA_10    				0xb10
+#define	rPdp_AntA_14    				0xb14
+#define	rPdp_AntA_18    				0xb18
+#define	rPdp_AntA_1C    				0xb1c
+#define	rPdp_AntA_20    				0xb20
+#define	rPdp_AntA_24    				0xb24
+
+#define	rConfig_Pmpd_AntA 			0xb28
+#define	rConfig_ram64x16				0xb2c
+
+#define	rBndA						0xb30
+#define	rHssiPar						0xb34
+
+#define	rConfig_AntA 					0xb68
+#define	rConfig_AntB 					0xb6c
+
+#define	rPdp_AntB 					0xb70
+#define	rPdp_AntB_4 					0xb74
+#define	rPdp_AntB_8 					0xb78
+#define	rPdp_AntB_C 					0xb7c
+#define	rPdp_AntB_10 					0xb80
+#define	rPdp_AntB_14 					0xb84
+#define	rPdp_AntB_18 					0xb88
+#define	rPdp_AntB_1C 					0xb8c
+#define	rPdp_AntB_20 					0xb90
+#define	rPdp_AntB_24 					0xb94
+
+#define	rConfig_Pmpd_AntB			0xb98
+
+#define	rBndB						0xba0
+
+#define	rAPK							0xbd8
+#define	rPm_Rx0_AntA				0xbdc
+#define	rPm_Rx1_AntA				0xbe0
+#define	rPm_Rx2_AntA				0xbe4
+#define	rPm_Rx3_AntA				0xbe8
+#define	rPm_Rx0_AntB				0xbec
+#define	rPm_Rx1_AntB				0xbf0
+#define	rPm_Rx2_AntB				0xbf4
+#define	rPm_Rx3_AntB				0xbf8
+
+/*RSSI Dump*/
+#define		RA_RSSI_DUMP		0xBF0
+#define		RB_RSSI_DUMP		0xBF1
+#define		RS1_RX_EVM_DUMP	0xBF4
+#define		RS2_RX_EVM_DUMP	0xBF5
+#define		RA_RX_SNR_DUMP		0xBF6
+#define		RB_RX_SNR_DUMP		0xBF7
+#define		RA_CFO_SHORT_DUMP	0xBF8
+#define		RB_CFO_SHORT_DUMP	0xBFA
+#define		RA_CFO_LONG_DUMP	0xBEC
+#define		RB_CFO_LONG_DUMP	0xBEE
+
+/*Page C*/
+#define	ROFDM0_LSTF					0xc00
+
+#define	ROFDM0_TRXPATHENABLE		0xc04
+#define	ROFDM0_TRMUXPAR				0xc08
+#define	ROFDM0_TRSWISOLATION		0xc0c
+
+#define	ROFDM0_XARXAFE				0xc10
+#define	ROFDM0_XARXIQIMBALANCE    	0xc14
+#define	ROFDM0_XBRXAFE            	0xc18
+#define	ROFDM0_XBRXIQIMBALANCE    	0xc1c
+#define	ROFDM0_XCRXAFE            	0xc20
+#define	ROFDM0_XCRXIQIMBANLANCE    	0xc24
+#define	ROFDM0_XDRXAFE            	0xc28
+#define	ROFDM0_XDRXIQIMBALANCE    	0xc2c
+
+#define	ROFDM0_RXDETECTOR1			0xc30
+#define	ROFDM0_RXDETECTOR2			0xc34
+#define	ROFDM0_RXDETECTOR3			0xc38
+#define	ROFDM0_RXDETECTOR4			0xc3c
+
+#define	ROFDM0_RXDSP				0xc40
+#define	ROFDM0_CFOANDDAGC			0xc44
+#define	ROFDM0_CCADROPTHRESHOLD		0xc48
+#define	ROFDM0_ECCATHRESHOLD		0xc4c
+
+#define	ROFDM0_XAAGCCORE1			0xc50
+#define	ROFDM0_XAAGCCORE2			0xc54
+#define	ROFDM0_XBAGCCORE1			0xc58
+#define	ROFDM0_XBAGCCORE2			0xc5c
+#define	ROFDM0_XCAGCCORE1			0xc60
+#define	ROFDM0_XCAGCCORE2			0xc64
+#define	ROFDM0_XDAGCCORE1			0xc68
+#define	ROFDM0_XDAGCCORE2			0xc6c
+
+#define	ROFDM0_AGCPARAMETER1		0xc70
+#define	ROFDM0_AGCPARAMETER2		0xc74
+#define	ROFDM0_AGCRSSITABLE			0xc78
+#define	ROFDM0_HTSTFAGC				0xc7c
+
+#define	ROFDM0_XATXIQIMBALANCE		0xc80
+#define	ROFDM0_XATXAFE				0xc84
+#define	ROFDM0_XBTXIQIMBALANCE		0xc88
+#define	ROFDM0_XBTXAFE				0xc8c
+#define	ROFDM0_XCTXIQIMBALANCE		0xc90
+#define	ROFDM0_XCTXAFE            	0xc94
+#define	ROFDM0_XDTXIQIMBALANCE		0xc98
+#define	ROFDM0_XDTXAFE				0xc9c
+
+#define ROFDM0_RXIQEXTANTA			0xca0
+#define	ROFDM0_TXCOEFF1				0xca4
+#define	ROFDM0_TXCOEFF2				0xca8
+#define	ROFDM0_TXCOEFF3				0xcac
+#define	ROFDM0_TXCOEFF4				0xcb0
+#define	ROFDM0_TXCOEFF5				0xcb4
+#define	ROFDM0_TXCOEFF6				0xcb8
+
+/*Path_A RFE cotrol */
+#define	RA_RFE_CTRL_8812				0xcb8
+/*Path_B RFE control*/
+#define	RB_RFE_CTRL_8812				0xeb8
+
+#define	ROFDM0_RXHPPARAMETER		0xce0
+#define	ROFDM0_TXPSEUDONOISEWGT		0xce4
+#define	ROFDM0_FRAMESYNC			0xcf0
+#define	ROFDM0_DFSREPORT			0xcf4
+
+
+#define	ROFDM1_LSTF					0xd00
+#define	ROFDM1_TRXPATHENABLE		0xd04
+
+#define	ROFDM1_CF0					0xd08
+#define	ROFDM1_CSI1					0xd10
+#define	ROFDM1_SBD					0xd14
+#define	ROFDM1_CSI2					0xd18
+#define	ROFDM1_CFOTRACKING			0xd2c
+#define	ROFDM1_TRXMESAURE1			0xd34
+#define	ROFDM1_INTFDET				0xd3c
+#define	ROFDM1_PSEUDONOISESTATEAB	0xd50
+#define	ROFDM1_PSEUDONOISESTATECD	0xd54
+#define	ROFDM1_RXPSEUDONOISEWGT		0xd58
+
+#define	ROFDM_PHYCOUNTER1			0xda0
+#define	ROFDM_PHYCOUNTER2			0xda4
+#define	ROFDM_PHYCOUNTER3			0xda8
+
+#define	ROFDM_SHORTCFOAB			0xdac
+#define	ROFDM_SHORTCFOCD			0xdb0
+#define	ROFDM_LONGCFOAB				0xdb4
+#define	ROFDM_LONGCFOCD				0xdb8
+#define	ROFDM_TAILCF0AB				0xdbc
+#define	ROFDM_TAILCF0CD				0xdc0
+#define	ROFDM_PWMEASURE1          	0xdc4
+#define	ROFDM_PWMEASURE2          	0xdc8
+#define	ROFDM_BWREPORT				0xdcc
+#define	ROFDM_AGCREPORT				0xdd0
+#define	ROFDM_RXSNR					0xdd4
+#define	ROFDM_RXEVMCSI				0xdd8
+#define	ROFDM_SIGREPORT				0xddc
+
+#define RTXAGC_A_CCK11_CCK1			0xc20
+#define RTXAGC_A_OFDM18_OFDM6		0xc24
+#define RTXAGC_A_OFDM54_OFDM24		0xc28
+#define RTXAGC_A_MCS03_MCS00			0xc2c
+#define RTXAGC_A_MCS07_MCS04			0xc30
+#define RTXAGC_A_MCS11_MCS08			0xc34
+#define RTXAGC_A_MCS15_MCS12		0xc38
+#define RTXAGC_A_NSS1INDEX3_NSS1INDEX0	0xc3c
+#define	RTXAGC_A_NSS1INDEX7_NSS1INDEX4	0xc40
+#define	RTXAGC_A_NSS2INDEX1_NSS1INDEX8	0xc44
+#define	RTXAGC_A_NSS2INDEX5_NSS2INDEX2	0xc48
+#define	RTXAGC_A_NSS2INDEX9_NSS2INDEX6	0xc4c
+#define	RTXAGC_B_CCK11_CCK1			0xe20
+#define	RTXAGC_B_OFDM18_OFDM6		0xe24
+#define	RTXAGC_B_OFDM54_OFDM24		0xe28
+#define	RTXAGC_B_MCS03_MCS00			0xe2c
+#define	RTXAGC_B_MCS07_MCS04			0xe30
+#define	RTXAGC_B_MCS11_MCS08			0xe34
+#define	RTXAGC_B_MCS15_MCS12		0xe38
+#define	RTXAGC_B_NSS1INDEX3_NSS1INDEX0	0xe3c
+#define	RTXAGC_B_NSS1INDEX7_NSS1INDEX4	0xe40
+#define	RTXAGC_B_NSS2INDEX1_NSS1INDEX8	0xe44
+#define	RTXAGC_B_NSS2INDEX5_NSS2INDEX2	0xe48
+#define	RTXAGC_B_NSS2INDEX9_NSS2INDEX6	0xe4c
+
+#define	RA_TXPWRTRAING		0xc54
+#define	RB_TXPWRTRAING		0xe54
+
+
+#define	RFPGA0_IQK					0xe28
+#define	RTx_IQK_Tone_A				0xe30
+#define	RRx_IQK_Tone_A				0xe34
+#define	RTx_IQK_PI_A					0xe38
+#define	RRx_IQK_PI_A					0xe3c
+
+#define	RTx_IQK 						0xe40
+#define	RRx_IQK						0xe44
+#define	RIQK_AGC_Pts					0xe48
+#define	RIQK_AGC_Rsp					0xe4c
+#define	RTx_IQK_Tone_B				0xe50
+#define	RRx_IQK_Tone_B				0xe54
+#define	RTx_IQK_PI_B					0xe58
+#define	RRx_IQK_PI_B					0xe5c
+#define	RIQK_AGC_Cont				0xe60
+
+#define	RBlue_Tooth					0xe6c
+#define	RRx_Wait_CCA					0xe70
+#define	RTx_CCK_RFON					0xe74
+#define	RTx_CCK_BBON				0xe78
+#define	RTx_OFDM_RFON				0xe7c
+#define	RTx_OFDM_BBON				0xe80
+#define	RTx_To_Rx					0xe84
+#define	RTx_To_Tx					0xe88
+#define	RRx_CCK						0xe8c
+
+#define	RTx_Power_Before_IQK_A		0xe94
+#define	RTx_Power_After_IQK_A			0xe9c
+
+#define	RRx_Power_Before_IQK_A		0xea0
+#define	RRx_Power_Before_IQK_A_2		0xea4
+#define	RRx_Power_After_IQK_A			0xea8
+#define	RRx_Power_After_IQK_A_2		0xeac
+
+#define	RTx_Power_Before_IQK_B		0xeb4
+#define	RTx_Power_After_IQK_B			0xebc
+
+#define	RRx_Power_Before_IQK_B		0xec0
+#define	RRx_Power_Before_IQK_B_2		0xec4
+#define	RRx_Power_After_IQK_B			0xec8
+#define	RRx_Power_After_IQK_B_2		0xecc
+
+#define	RRx_OFDM					0xed0
+#define	RRx_Wait_RIFS 				0xed4
+#define	RRx_TO_Rx 					0xed8
+#define	RStandby 						0xedc
+#define	RSleep 						0xee0
+#define	RPMPD_ANAEN				0xeec
+
+#define	RZEBRA1_HSSIENABLE			0x0
+#define	RZEBRA1_TRXENABLE1			0x1
+#define	RZEBRA1_TRXENABLE2			0x2
+#define	RZEBRA1_AGC					0x4
+#define	RZEBRA1_CHARGEPUMP			0x5
+#define	RZEBRA1_CHANNEL				0x7
+
+#define	RZEBRA1_TXGAIN				0x8
+#define	RZEBRA1_TXLPF				0x9
+#define	RZEBRA1_RXLPF				0xb
+#define	RZEBRA1_RXHPFCORNER			0xc
+
+#define	RGLOBALCTRL					0
+#define	RRTL8256_TXLPF				19
+#define	RRTL8256_RXLPF				11
+#define	RRTL8258_TXLPF				0x11
+#define	RRTL8258_RXLPF				0x13
+#define	RRTL8258_RSSILPF			0xa
+
+#define	RF_AC						0x00
+
+#define	RF_IQADJ_G1					0x01
+#define	RF_IQADJ_G2					0x02
+#define	RF_POW_TRSW					0x05
+
+#define	RF_GAIN_RX					0x06
+#define	RF_GAIN_TX					0x07
+
+#define	RF_TXM_IDAC					0x08
+#define	RF_BS_IQGEN					0x0F
+
+#define	RF_MODE1					0x10
+#define	RF_MODE2					0x11
+
+#define	RF_RX_AGC_HP				0x12
+#define	RF_TX_AGC					0x13
+#define	RF_BIAS						0x14
+#define	RF_IPA						0x15
+#define	RF_POW_ABILITY				0x17
+#define	RF_MODE_AG					0x18
+#define	RRFCHANNEL					0x18
+#define	RF_CHNLBW					0x18
+#define	RF_TOP						0x19
+
+#define	RF_RX_G1					0x1A
+#define	RF_RX_G2					0x1B
+
+#define	RF_RX_BB2					0x1C
+#define	RF_RX_BB1					0x1D
+
+#define	RF_RCK1						0x1E
+#define	RF_RCK2						0x1F
+
+#define	RF_TX_G1					0x20
+#define	RF_TX_G2					0x21
+#define	RF_TX_G3					0x22
+
+#define	RF_TX_BB1					0x23
+#define	RF_T_METER					0x24
+#define	RF_T_METER_88E				0x42
+#define  RF_T_METER_8812A 		0x42
+
+#define	RF_SYN_G1					0x25
+#define	RF_SYN_G2					0x26
+#define	RF_SYN_G3					0x27
+#define	RF_SYN_G4					0x28
+#define	RF_SYN_G5					0x29
+#define	RF_SYN_G6					0x2A
+#define	RF_SYN_G7					0x2B
+#define	RF_SYN_G8					0x2C
+
+#define	RF_RCK_OS					0x30
+#define	RF_TXPA_G1					0x31
+#define	RF_TXPA_G2					0x32
+#define	RF_TXPA_G3					0x33
+
+#define	RF_TX_BIAS_A					0x35
+#define	RF_TX_BIAS_D					0x36
+#define	RF_LOBF_9					0x38
+#define	RF_RXRF_A3					0x3C
+#define	RF_TRSW						0x3F
+
+#define	RF_TXRF_A2					0x41
+#define	RF_TXPA_G4					0x46
+#define	RF_TXPA_A4					0x4B
+
+#define RF_APK						0x63
+
+#define	RF_WE_LUT					0xEF
+
+#define	BBBRESETB					0x100
+#define	BGLOBALRESETB				0x200
+#define	BOFDMTXSTART				0x4
+#define	BCCKTXSTART					0x8
+#define	BCRC32DEBUG					0x100
+#define	BPMACLOOPBACK				0x10
+#define	BTXLSIG						0xffffff
+#define	BOFDMTXRATE					0xf
+#define	BOFDMTXRESERVED				0x10
+#define	BOFDMTXLENGTH				0x1ffe0
+#define	BOFDMTXPARITY				0x20000
+#define	BTXHTSIG1					0xffffff
+#define	BTXHTMCSRATE				0x7f
+#define	BTXHTBW						0x80
+#define	BTXHTLENGTH					0xffff00
+#define	BTXHTSIG2					0xffffff
+#define	BTXHTSMOOTHING				0x1
+#define	BTXHTSOUNDING				0x2
+#define	BTXHTRESERVED				0x4
+#define	BTXHTAGGREATION				0x8
+#define	BTXHTSTBC					0x30
+#define	BTXHTADVANCECODING			0x40
+#define	BTXHTSHORTGI				0x80
+#define	BTXHTNUMBERHT_LTF			0x300
+#define	BTXHTCRC8					0x3fc00
+#define	BCOUNTERRESET				0x10000
+#define	BNUMOFOFDMTX				0xffff
+#define	BNUMOFCCKTX					0xffff0000
+#define	BTXIDLEINTERVAL				0xffff
+#define	BOFDMSERVICE				0xffff0000
+#define	BTXMACHEADER				0xffffffff
+#define	BTXDATAINIT					0xff
+#define	BTXHTMODE					0x100
+#define	BTXDATATYPE					0x30000
+#define	BTXRANDOMSEED				0xffffffff
+#define	BCCKTXPREAMBLE				0x1
+#define	BCCKTXSFD					0xffff0000
+#define	BCCKTXSIG					0xff
+#define	BCCKTXSERVICE				0xff00
+#define	BCCKLENGTHEXT				0x8000
+#define	BCCKTXLENGHT				0xffff0000
+#define	BCCKTXCRC16					0xffff
+#define	BCCKTXSTATUS				0x1
+#define	BOFDMTXSTATUS				0x2
+#define IS_BB_REG_OFFSET_92S(_Offset)	\
+	((_Offset >= 0x800) && (_Offset <= 0xfff))
+
+#define	BRFMOD						0x1
+#define	BJAPANMODE					0x2
+#define	BCCKTXSC					0x30
+/* Block & Path enable*/
+#define ROFDMCCKEN					0x808
+#define	BCCKEN						0x10000000
+#define	BOFDMEN						0x20000000
+#define	RRXPATH						0x808	/* Rx antenna*/
+#define	BRXPATH						0xff
+#define	RTXPATH						0x80c	/* Tx antenna*/
+#define	BTXPATH						0x0fffffff
+#define	RCCK_RX						0xa04	/* for cck rx path selection*/
+#define	BCCK_RX						0x0c000000
+#define	RVHTLEN_USE_LSIG			0x8c3	/* Use LSIG for VHT length*/
+
+
+#define	BOFDMRXADCPHASE           	0x10000
+#define	BOFDMTXDACPHASE           	0x40000
+#define	BXATXAGC                  	0x3f
+
+#define	BXBTXAGC                  	0xf00
+#define	BXCTXAGC                  	0xf000
+#define	BXDTXAGC                  	0xf0000
+
+#define	BPASTART                  	0xf0000000
+#define	BTRSTART                  	0x00f00000
+#define	BRFSTART                  	0x0000f000
+#define	BBBSTART                  	0x000000f0
+#define	BBBCCKSTART               	0x0000000f
+#define	BPAEND                    	0xf
+#define	BTREND                    	0x0f000000
+#define	BRFEND                    	0x000f0000
+#define	BCCAMASK                  	0x000000f0
+#define	BR2RCCAMASK               	0x00000f00
+#define	BHSSI_R2TDELAY            	0xf8000000
+#define	BHSSI_T2RDELAY            	0xf80000
+#define	BCONTXHSSI               	0x400
+#define	BIGFROMCCK                	0x200
+#define	BAGCADDRESS               	0x3f
+#define	BRXHPTX                   	0x7000
+#define	BRXHP2RX                  	0x38000
+#define	BRXHPCCKINI               	0xc0000
+#define	BAGCTXCODE                	0xc00000
+#define	BAGCRXCODE                	0x300000
+
+#define	B3WIREDATALENGTH          	0x800
+#define	B3WIREADDREAALENGTH       	0x400
+
+#define	B3WIRERFPOWERDOWN         	0x1
+#define	B5GPAPEPOLARITY           	0x40000000
+#define	B2GPAPEPOLARITY           	0x80000000
+#define	BRFSW_TXDEFAULTANT        	0x3
+#define	BRFSW_TXOPTIONANT         	0x30
+#define	BRFSW_RXDEFAULTANT        	0x300
+#define	BRFSW_RXOPTIONANT         	0x3000
+#define	BRFSI_3WIREDATA           	0x1
+#define	BRFSI_3WIRECLOCK          	0x2
+#define	BRFSI_3WIRELOAD           	0x4
+#define	BRFSI_3WIRERW             	0x8
+#define	BRFSI_3WIRE               	0xf
+
+#define	BRFSI_RFENV               	0x10
+
+#define	BRFSI_TRSW                	0x20
+#define	BRFSI_TRSWB               	0x40
+#define	BRFSI_ANTSW               	0x100
+#define	BRFSI_ANTSWB              	0x200
+#define	BRFSI_PAPE                	0x400
+#define	BRFSI_PAPE5G              	0x800
+#define	BBANDSELECT               	0x1
+#define	BHTSIG2_GI                	0x80
+#define	BHTSIG2_SMOOTHING         	0x01
+#define	BHTSIG2_SOUNDING          	0x02
+#define	BHTSIG2_AGGREATON         	0x08
+#define	BHTSIG2_STBC              	0x30
+#define	BHTSIG2_ADVCODING         	0x40
+#define	BHTSIG2_NUMOFHTLTF        	0x300
+#define	BHTSIG2_CRC8              	0x3fc
+#define	BHTSIG1_MCS               	0x7f
+#define	BHTSIG1_BANDWIDTH         	0x80
+#define	BHTSIG1_HTLENGTH          	0xffff
+#define	BLSIG_RATE                	0xf
+#define	BLSIG_RESERVED            	0x10
+#define	BLSIG_LENGTH              	0x1fffe
+#define	BLSIG_PARITY              	0x20
+#define	BCCKRXPHASE               	0x4
+
+#define	BLSSIREADADDRESS          	0x7f800000
+#define	BLSSIREADEDGE             	0x80000000
+
+#define	BLSSIREADBACKDATA         	0xfffff
+
+#define	BLSSIREADOKFLAG           	0x1000
+#define	BCCKSAMPLERATE            	0x8
+#define	BREGULATOR0STANDBY        	0x1
+#define	BREGULATORPLLSTANDBY      	0x2
+#define	BREGULATOR1STANDBY        	0x4
+#define	BPLLPOWERUP               	0x8
+#define	BDPLLPOWERUP              	0x10
+#define	BDA10POWERUP              	0x20
+#define	BAD7POWERUP               	0x200
+#define	BDA6POWERUP               	0x2000
+#define	BXTALPOWERUP              	0x4000
+#define	B40MDCLKPOWERUP           	0x8000
+#define	BDA6DEBUGMODE             	0x20000
+#define	BDA6SWING                 	0x380000
+
+#define	BADCLKPHASE               	0x4000000
+#define	B80MCLKDELAY              	0x18000000
+#define	BAFEWATCHDOGENABLE        	0x20000000
+
+#define	BXTALCAP01                	0xc0000000
+#define	BXTALCAP23                	0x3
+#define	BXTALCAP92X					0x0f000000
+#define BXTALCAP                	0x0f000000
+
+#define	BINTDIFCLKENABLE          	0x400
+#define	BEXTSIGCLKENABLE         	0x800
+#define	BBANDGAP_MBIAS_POWERUP      0x10000
+#define	BAD11SH_GAIN               	0xc0000
+#define	BAD11NPUT_RANGE           	0x700000
+#define	BAD110P_CURRENT            	0x3800000
+#define	BLPATH_LOOPBACK            	0x4000000
+#define	BQPATH_LOOPBACK            	0x8000000
+#define	BAFE_LOOPBACK              	0x10000000
+#define	BDA10_SWING                	0x7e0
+#define	BDA10_REVERSE              	0x800
+#define	BDA_CLK_SOURCE              0x1000
+#define	BDA7INPUT_RANGE            	0x6000
+#define	BDA7_GAIN                  	0x38000
+#define	BDA7OUTPUT_CM_MODE          0x40000
+#define	BDA7INPUT_CM_MODE           0x380000
+#define	BDA7CURRENT               	0xc00000
+#define	BREGULATOR_ADJUST          	0x7000000
+#define	BAD11POWERUP_ATTX          	0x1
+#define	BDA10PS_ATTX               	0x10
+#define	BAD11POWERUP_ATRX          	0x100
+#define	BDA10PS_ATRX               	0x1000
+#define	BCCKRX_AGC_FORMAT           0x200
+#define	BPSDFFT_SAMPLE_POINT       	0xc000
+#define	BPSD_AVERAGE_NUM            0x3000
+#define	BIQPATH_CONTROL            	0xc00
+#define	BPSD_FREQ                  	0x3ff
+#define	BPSD_ANTENNA_PATH           0x30
+#define	BPSD_IQ_SWITCH              0x40
+#define	BPSD_RX_TRIGGER             0x400000
+#define	BPSD_TX_TRIGGER             0x80000000
+#define	BPSD_SINE_TONE_SCALE        0x7f000000
+#define	BPSD_REPORT                	0xffff
+
+#define	BOFDM_TXSC                	0x30000000
+#define	BCCK_TXON                  	0x1
+#define	BOFDM_TXON                 	0x2
+#define	BDEBUG_PAGE                	0xfff
+#define	BDEBUG_ITEM                	0xff
+#define	BANTL              	       	0x10
+#define	BANT_NONHT           	    0x100
+#define	BANT_HT1               		0x1000
+#define	BANT_HT2                   	0x10000
+#define	BANT_HT1S1                 	0x100000
+#define	BANT_NONHTS1               	0x1000000
+
+#define	BCCK_BBMODE                	0x3
+#define	BCCK_TXPOWERSAVING         	0x80
+#define	BCCK_RXPOWERSAVING         	0x40
+
+#define	BCCK_SIDEBAND              	0x10
+
+#define	BCCK_SCRAMBLE              	0x8
+#define	BCCK_ANTDIVERSITY    		0x8000
+#define	BCCK_CARRIER_RECOVERY   	0x4000
+#define	BCCK_TXRATE           		0x3000
+#define	BCCK_DCCANCEL             	0x0800
+#define	BCCK_ISICANCEL             	0x0400
+#define	BCCK_MATCH_FILTER           0x0200
+#define	BCCK_EQUALIZER             	0x0100
+#define	BCCK_PREAMBLE_DETECT       	0x800000
+#define	BCCK_FAST_FALSECCA          0x400000
+#define	BCCK_CH_ESTSTART            0x300000
+#define	BCCK_CCA_COUNT              0x080000
+#define	BCCK_CS_LIM                	0x070000
+#define	BCCK_BIST_MODE              0x80000000
+#define	BCCK_CCAMASK             	0x40000000
+#define	BCCK_TX_DAC_PHASE         	0x4
+#define	BCCK_RX_ADC_PHASE         	0x20000000
+#define	BCCKR_CP_MODE         	   	0x0100
+#define	BCCK_TXDC_OFFSET           	0xf0
+#define	BCCK_RXDC_OFFSET           	0xf
+#define	BCCK_CCA_MODE              	0xc000
+#define	BCCK_FALSECS_LIM           	0x3f00
+#define	BCCK_CS_RATIO              	0xc00000
+#define	BCCK_CORGBIT_SEL           	0x300000
+#define	BCCK_PD_LIM                	0x0f0000
+#define	BCCK_NEWCCA                	0x80000000
+#define	BCCK_RXHP_OF_IG             0x8000
+#define	BCCK_RXIG                  	0x7f00
+#define	BCCK_LNA_POLARITY           0x800000
+#define	BCCK_RX1ST_BAIN             0x7f0000
+#define	BCCK_RF_EXTEND              0x20000000
+#define	BCCK_RXAGC_SATLEVEL        	0x1f000000
+#define	BCCK_RXAGC_SATCOUNT       	0xe0
+#define	bCCKRxRFSettle            	0x1f
+#define	BCCK_FIXED_RXAGC           	0x8000
+#define	BCCK_ANTENNA_POLARITY      	0x2000
+#define	BCCK_TXFILTER_TYPE          0x0c00
+#define	BCCK_RXAGC_REPORTTYPE   	0x0300
+#define	BCCK_RXDAGC_EN              0x80000000
+#define	BCCK_RXDAGC_PERIOD        	0x20000000
+#define	BCCK_RXDAGC_SATLEVEL     	0x1f000000
+#define	BCCK_TIMING_RECOVERY       	0x800000
+#define	BCCK_TXC0                  	0x3f0000
+#define	BCCK_TXC1                  	0x3f000000
+#define	BCCK_TXC2                  	0x3f
+#define	BCCK_TXC3                  	0x3f00
+#define	BCCK_TXC4                  	0x3f0000
+#define	BCCK_TXC5                  	0x3f000000
+#define	BCCK_TXC6                  	0x3f
+#define	BCCK_TXC7                  	0x3f00
+#define	BCCK_DEBUGPORT             	0xff0000
+#define	BCCK_DAC_DEBUG              0x0f000000
+#define	BCCK_FALSEALARM_ENABLE      0x8000
+#define	BCCK_FALSEALARM_READ        0x4000
+#define	BCCK_TRSSI                 	0x7f
+#define	BCCK_RXAGC_REPORT           0xfe
+#define	BCCK_RXREPORT_ANTSEL       	0x80000000
+#define	BCCK_RXREPORT_MFOFF        	0x40000000
+#define	BCCK_RXREPORT_SQLOSS     	0x20000000
+#define	BCCK_RXREPORT_PKTLOSS      	0x10000000
+#define	BCCK_RXREPORT_LOCKEDBIT    	0x08000000
+#define	BCCK_RXREPORT_RATEERROR    	0x04000000
+#define	BCCK_RXREPORT_RXRATE       	0x03000000
+#define	BCCK_RXFA_COUNTER_LOWER     0xff
+#define	BCCK_RXFA_COUNTER_UPPER     0xff000000
+#define	BCCK_RXHPAGC_START          0xe000
+#define	BCCK_RXHPAGC_FINAL          0x1c00
+#define	BCCK_RXFALSEALARM_ENABLE    0x8000
+#define	BCCK_FACOUNTER_FREEZE       0x4000
+#define	BCCK_TXPATH_SEL             0x10000000
+#define	BCCK_DEFAULT_RXPATH         0xc000000
+#define	BCCK_OPTION_RXPATH          0x3000000
+
+#define	BNUM_OFSTF                	0x3
+#define	BSHIFT_L                 	0xc0
+#define	BGI_TH                   	0xc
+#define	BRXPATH_A                 	0x1
+#define	BRXPATH_B                 	0x2
+#define	BRXPATH_C                 	0x4
+#define	BRXPATH_D                 	0x8
+#define	BTXPATH_A                 	0x1
+#define	BTXPATH_B                 	0x2
+#define	BTXPATH_C                 	0x4
+#define	BTXPATH_D                 	0x8
+#define	BTRSSI_FREQ               	0x200
+#define	BADC_BACKOFF              	0x3000
+#define	BDFIR_BACKOFF             	0xc000
+#define	BTRSSI_LATCH_PHASE         	0x10000
+#define	BRX_LDC_OFFSET             	0xff
+#define	BRX_QDC_OFFSET             	0xff00
+#define	BRX_DFIR_MODE              	0x1800000
+#define	BRX_DCNF_TYPE              	0xe000000
+#define	BRXIQIMB_A               	0x3ff
+#define	BRXIQIMB_B               	0xfc00
+#define	BRXIQIMB_C               	0x3f0000
+#define	BRXIQIMB_D               	0xffc00000
+#define	BDC_DC_NOTCH             	0x60000
+#define	BRXNB_NOTCH              	0x1f000000
+#define	BPD_TH                   	0xf
+#define	BPD_TH_OPT2              	0xc000
+#define	BPWED_TH                 	0x700
+#define	BIFMF_WIN_L              	0x800
+#define	BPD_OPTION               	0x1000
+#define	BMF_WIN_L                	0xe000
+#define	BBW_SEARCH_L             	0x30000
+#define	BWIN_ENH_L               	0xc0000
+#define	BBW_TH                   	0x700000
+#define	BED_TH2                  	0x3800000
+#define	BBW_OPTION               	0x4000000
+#define	BRADIO_TH                	0x18000000
+#define	BWINDOW_L                	0xe0000000
+#define	BSBD_OPTION              	0x1
+#define	BFRAME_TH                	0x1c
+#define	BFS_OPTION               	0x60
+#define	BDC_SLOPE_CHECK          	0x80
+#define	BFGUARD_COUNTER_DC_L     	0xe00
+#define	BFRAME_WEIGHT_SHORT      	0x7000
+#define	BSUB_TUNE                	0xe00000
+#define	BFRAME_DC_LENGTH         	0xe000000
+#define	BSBD_START_OFFSET        	0x30000000
+#define	BFRAME_TH_2              	0x7
+#define	BFRAME_GI2_TH            	0x38
+#define	BGI2_SYNC_EN             	0x40
+#define	BSARCH_SHORT_EARLY       	0x300
+#define	BSARCH_SHORT_LATE        	0xc00
+#define	BSARCH_GI2_LATE          	0x70000
+#define	BCFOANTSUM               	0x1
+#define	BCFOACC                  	0x2
+#define	BCFOSTARTOFFSET          	0xc
+#define	BCFOLOOPBACK             	0x70
+#define	BCFOSUMWEIGHT            	0x80
+#define	BDAGCENABLE              	0x10000
+#define	BTXIQIMB_A               	0x3ff
+#define	BTXIQIMB_b               	0xfc00
+#define	BTXIQIMB_C               	0x3f0000
+#define	BTXIQIMB_D               	0xffc00000
+#define	BTXIDCOFFSET             	0xff
+#define	BTXIQDCOFFSET             	0xff00
+#define	BTXDFIRMODE              	0x10000
+#define	BTXPESUDO_NOISEON         	0x4000000
+#define	BTXPESUDO_NOISE_A         	0xff
+#define	BTXPESUDO_NOISE_B         	0xff00
+#define	BTXPESUDO_NOISE_C         	0xff0000
+#define	BTXPESUDO_NOISE_D         	0xff000000
+#define	BCCA_DROPOPTION           	0x20000
+#define	BCCA_DROPTHRES            	0xfff00000
+#define	BEDCCA_H                 	0xf
+#define	BEDCCA_L                 	0xf0
+#define	BLAMBDA_ED               	0x300
+#define	BRX_INITIALGAIN           	0x7f
+#define	BRX_ANTDIV_EN              	0x80
+#define	BRX_AGC_ADDRESS_FOR_LNA     0x7f00
+#define	BRX_HIGHPOWER_FLOW         	0x8000
+#define	BRX_AGC_FREEZE_THRES        0xc0000
+#define	BRX_FREEZESTEP_AGC1       	0x300000
+#define	BRX_FREEZESTEP_AGC2       	0xc00000
+#define	BRX_FREEZESTEP_AGC3       	0x3000000
+#define	BRX_FREEZESTEP_AGC0       	0xc000000
+#define	BRXRSSI_CMP_EN           	0x10000000
+#define	BRXQUICK_AGCEN            	0x20000000
+#define	BRXAGC_FREEZE_THRES_MODE    0x40000000
+#define	BRX_OVERFLOW_CHECKTYPE     	0x80000000
+#define	BRX_AGCSHIFT              	0x7f
+#define	BTRSW_TRI_ONLY           	0x80
+#define	BPOWER_THRES              	0x300
+#define	BRXAGC_EN                 	0x1
+#define	BRXAGC_TOGETHER_EN         	0x2
+#define	BRXAGC_MIN                	0x4
+#define	BRXHP_INI                	0x7
+#define	BRXHP_TRLNA              	0x70
+#define	BRXHP_RSSI               	0x700
+#define	BRXHP_BBP1               	0x7000
+#define	BRXHP_BBP2               	0x70000
+#define	BRXHP_BBP3               	0x700000
+#define	BRSSI_H                  	0x7f0000
+#define	BRSSI_GEN                	0x7f000000
+#define	BRXSETTLE_TRSW           	0x7
+#define	BRXSETTLE_LNA            	0x38
+#define	BRXSETTLE_RSSI           	0x1c0
+#define	BRXSETTLE_BBP            	0xe00
+#define	BRXSETTLE_RXHP           	0x7000
+#define	BRXSETTLE_ANTSW_RSSI     	0x38000
+#define	BRXSETTLE_ANTSW          	0xc0000
+#define	BRXPROCESS_TIME_DAGC      	0x300000
+#define	BRXSETTLE_HSSI           	0x400000
+#define	BRXPROCESS_TIME_BBPPW     	0x800000
+#define	BRXANTENNA_POWER_SHIFT     	0x3000000
+#define	BRSSI_TABLE_SELECT         	0xc000000
+#define	BRXHP_FINAL              	0x7000000
+#define	BRXHPSETTLE_BBP          	0x7
+#define	BRXHTSETTLE_HSSI         	0x8
+#define	BRXHTSETTLE_RXHP         	0x70
+#define	BRXHTSETTLE_BBPPW        	0x80
+#define	BRXHTSETTLE_IDLE         	0x300
+#define	BRXHTSETTLE_RESERVED     	0x1c00
+#define	BRXHT_RXHP_EN              	0x8000
+#define	BRXAGC_FREEZE_THRES      	0x30000
+#define	BRXAGC_TOGETHEREN       	0x40000
+#define	BRXHTAGC_MIN              	0x80000
+#define	BRXHTAGC_EN               	0x100000
+#define	BRXHTDAGC_EN              	0x200000
+#define	BRXHT_RXHP_BBP            	0x1c00000
+#define	BRXHT_RXHP_FINAL          	0xe0000000
+#define	BRXPW_RADIO_TH             	0x3
+#define	BRXPW_RADIO_EN             	0x4
+#define	BRXMF_HOLD                	0x3800
+#define	BRXPD_DELAY_TH1          	0x38
+#define	BRXPD_DELAY_TH2          	0x1c0
+#define	BRXPD_DC_COUNT_MAX       	0x600
+#define	BRXPD_DELAY_TH           	0x8000
+#define	BRXPROCESS_DELAY         	0xf0000
+#define	BRXSEARCHRANGE_GI2_EARLY 	0x700000
+#define	BRXFRAME_FUARD_COUNTER_L 	0x3800000
+#define	BRXSGI_GUARD_L           	0xc000000
+#define	BRXSGI_SEARCH_L          	0x30000000
+#define	BRXSGI_TH                	0xc0000000
+#define	BDFSCNT0                 	0xff
+#define	BDFSCNT1                 	0xff00
+#define	BDFSFLAG                 	0xf0000
+#define	BMF_WEIGHT_SUM             	0x300000
+#define	BMINIDX_TH                	0x7f000000
+#define	BDAFORMAT                	0x40000
+#define	BTXCH_EMU_ENABLE           	0x01000000
+#define	BTRSW_ISOLATION_A         	0x7f
+#define	BTRSW_ISOLATION_B         	0x7f00
+#define	BTRSW_ISOLATION_C         	0x7f0000
+#define	BTRSW_ISOLATION_D         	0x7f000000
+#define	BEXT_LNA_GAIN              	0x7c00
+
+#define	BSTBC_EN                  	0x4
+#define	BANTENNA_MAPPING          	0x10
+#define	BNSS                     	0x20
+#define	BCFO_ANTSUM_ID              0x200
+#define	BPHY_COUNTER_RESET         	0x8000000
+#define	BCFO_REPORT_GET            	0x4000000
+#define	BOFDM_CONTINUE_TX          	0x10000000
+#define	BOFDM_SINGLE_CARRIER       	0x20000000
+#define	BOFDM_SINGLE_TONE          	0x40000000
+#define	BHT_DETECT                	0x100
+#define	BCFOEN                   	0x10000
+#define	BCFOVALUE                	0xfff00000
+#define	BSIGTONE_RE              	0x3f
+#define	BSIGTONE_IM              	0x7f00
+#define	BCOUNTER_CCA             	0xffff
+#define	BCOUNTER_PARITYFAIL      	0xffff0000
+#define	BCOUNTER_RATEILLEGAL     	0xffff
+#define	BCOUNTER_CRC8FAIL        	0xffff0000
+#define	BCOUNTER_MCSNOSUPPORT    	0xffff
+#define	BCOUNTER_FASTSYNC        	0xffff
+#define	BSHORTCFO                	0xfff
+#define	BSHORTCFOT_LENGTH         	12
+#define	BSHORTCFOF_LENGTH         	11
+#define	BLONGCFO                 	0x7ff
+#define	BLONGCFOT_LENGTH          	11
+#define	BLONGCFOF_LENGTH          	11
+#define	BTAILCFO                 	0x1fff
+#define	BTAILCFOT_LENGTH          	13
+#define	BTAILCFOF_LENGTH          	12
+#define	BNOISE_EN_PWDB             	0xffff
+#define	BCC_POWER_DB             	0xffff0000
+#define	BMOISE_PWDB              	0xffff
+#define	BPOWERMEAST_LENGTH        	10
+#define	BPOWERMEASF_LENGTH        	3
+#define	BRX_HT_BW                	0x1
+#define	BRXSC                    	0x6
+#define	BRX_HT                   	0x8
+#define	BNB_INTF_DET_ON          	0x1
+#define	BINTF_WIN_LEN_CFG        	0x30
+#define	BNB_INTF_TH_CFG          	0x1c0
+#define	BRFGAIN                  	0x3f
+#define	BTABLESEL                	0x40
+#define	BTRSW                    	0x80
+#define	BRXSNR_A                 	0xff
+#define	BRXSNR_B                 	0xff00
+#define	BRXSNR_C                 	0xff0000
+#define	BRXSNR_D                 	0xff000000
+#define	BSNR_EVMT_LENGTH           	8
+#define	BSNR_EVMF_LENGTH           	1
+#define	BCSI1ST                  	0xff
+#define	BCSI2ND                  	0xff00
+#define	BRXEVM1ST                	0xff0000
+#define	BRXEVM2ND                	0xff000000
+#define	BSIGEVM                  	0xff
+#define	BPWDB                    	0xff00
+#define	BSGIEN                   	0x10000
+
+#define	BSFACTOR_QMA1             	0xf
+#define	BSFACTOR_QMA2             	0xf0
+#define	BSFACTOR_QMA3             	0xf00
+#define	BSFACTOR_QMA4             	0xf000
+#define	BSFACTOR_QMA5             	0xf0000
+#define	BSFACTOR_QMA6             	0xf0000
+#define	BSFACTOR_QMA7             	0xf00000
+#define	BSFACTOR_QMA8             	0xf000000
+#define	BSFACTOR_QMA9             	0xf0000000
+#define	BCSI_SCHEME               	0x100000
+
+#define	BNOISE_LVL_TOP_SET          0x3
+#define	BCHSMOOTH                	0x4
+#define	BCHSMOOTH_CFG1            	0x38
+#define	BCHSMOOTH_CFG2            	0x1c0
+#define	BCHSMOOTH_CFG3            	0xe00
+#define	BCHSMOOTH_CFG4            	0x7000
+#define	BMRCMODE                 	0x800000
+#define	BTHEVMCFG                	0x7000000
+
+#define	BLOOP_FIT_TYPE             	0x1
+#define	BUPD_CFO                  	0x40
+#define	BUPD_CFO_OFFDATA           	0x80
+#define	BADV_UPD_CFO               	0x100
+#define	BADV_TIME_CTRL             	0x800
+#define	BUPD_CLKO                 	0x1000
+#define	BFC                      	0x6000
+#define	BTRACKING_MODE            	0x8000
+#define	BPHCMP_ENABLE             	0x10000
+#define	BUPD_CLKO_LTF              	0x20000
+#define	BCOM_CH_CFO                	0x40000
+#define	BCSI_ESTI_MODE             	0x80000
+#define	BADV_UPD_EQZ               	0x100000
+#define	BUCHCFG                  	0x7000000
+#define	BUPDEQZ                  	0x8000000
+
+#define	BRX_PESUDO_NOISE_ON         0x20000000
+#define	BRX_PESUDO_NOISE_A         	0xff
+#define	BRX_PESUDO_NOISE_B         	0xff00
+#define	BRX_PESUDO_NOISE_C         	0xff0000
+#define	BRX_PESUDO_NOISE_D         	0xff000000
+#define	BRX_PESUDO_NOISESTATE_A     0xffff
+#define	BRX_PESUDO_NOISESTATE_B     0xffff0000
+#define	BRX_PESUDO_NOISESTATE_C     0xffff
+#define	BRX_PESUDO_NOISESTATE_D     0xffff0000
+
+#define	BZEBRA1_HSSIENABLE        	0x8
+#define	BZEBRA1_TRXCONTROL        	0xc00
+#define	BZEBRA1_TRXGAINSETTING    	0x07f
+#define	BZEBRA1_RXCOUNTER          	0xc00
+#define	BZEBRA1_TXCHANGEPUMP      	0x38
+#define	BZEBRA1_RXCHANGEPUMP      	0x7
+#define	BZEBRA1_CHANNEL_NUM        	0xf80
+#define	BZEBRA1_TXLPFBW           	0x400
+#define	BZEBRA1_RXLPFBW           	0x600
+
+#define	BRTL8256REG_MODE_CTRL1      0x100
+#define	BRTL8256REG_MODE_CTRL0      0x40
+#define	BRTL8256REG_TXLPFBW         0x18
+#define	BRTL8256REG_RXLPFBW         0x600
+
+#define	BRTL8258_TXLPFBW          	0xc
+#define	BRTL8258_RXLPFBW          	0xc00
+#define	BRTL8258_RSSILPFBW        	0xc0
+
+#define	BBYTE0                    	0x1
+#define	BBYTE1                    	0x2
+#define	BBYTE2                    	0x4
+#define	BBYTE3                    	0x8
+#define	BWORD0                    	0x3
+#define	BWORD1                    	0xc
+#define	BWORD                    	0xf
+
+#define	MASKBYTE0                	0xff
+#define	MASKBYTE1                	0xff00
+#define	MASKBYTE2                	0xff0000
+#define	MASKBYTE3                	0xff000000
+#define	MASKHWORD                	0xffff0000
+#define	MASKLWORD                	0x0000ffff
+#define	MASKDWORD					0xffffffff
+#define	MASK12BITS					0xfff
+#define	MASKH4BITS					0xf0000000
+#define MASKOFDM_D					0xffc00000
+#define	MASKCCK						0x3f3f3f3f
+
+#define	MASK4BITS              		0x0f
+#define	MASK20BITS             		0xfffff
+#define RFREG_OFFSET_MASK			0xfffff
+
+#define	BENABLE                   	0x1
+#define	BDISABLE                  	0x0
+
+#define	LEFT_ANTENNA               	0x0
+#define	RIGHT_ANTENNA              	0x1
+
+#define	TCHECK_TXSTATUS            	500
+#define	TUPDATE_RXCOUNTER          	100
+
+#define	REG_UN_used_register		0x01bf
+
+/* WOL bit information */
+#define	HAL92C_WOL_PTK_UPDATE_EVENT		BIT(0)
+#define	HAL92C_WOL_GTK_UPDATE_EVENT		BIT(1)
+#define	HAL92C_WOL_DISASSOC_EVENT		BIT(2)
+#define	HAL92C_WOL_DEAUTH_EVENT			BIT(3)
+#define	HAL92C_WOL_FW_DISCONNECT_EVENT	BIT(4)
+
+#define		WOL_REASON_PTK_UPDATE		BIT(0)
+#define		WOL_REASON_GTK_UPDATE		BIT(1)
+#define		WOL_REASON_DISASSOC			BIT(2)
+#define		WOL_REASON_DEAUTH			BIT(3)
+#define		WOL_REASON_FW_DISCONNECT	BIT(4)
+
+#define		RA_RFE_PINMUX	0xcb0  /* Path_A RFE cotrol pinmux*/
+#define		RB_RFE_PINMUX	0xeb0 /* Path_B RFE control pinmux*/
+
+#define		RA_RFE_INV 0xcb4
+#define		RB_RFE_INV 0xeb4
+
+/* RXIQC */
+#define		RA_RXIQC_AB    	0xc10  /*RxIQ imblance matrix coeff. A & B*/
+#define		RA_RXIQC_CD    	0xc14  /*RxIQ imblance matrix coeff. C & D*/
+#define	 	RA_TXSCALE 		0xc1c  /* Pah_A TX scaling factor*/
+#define		RB_TXSCALE 		0xe1c  /* Path_B TX scaling factor*/
+#define		RB_RXIQC_AB    	0xe10  /*RxIQ imblance matrix coeff. A & B*/
+#define		RB_RXIQC_CD    	0xe14  /*RxIQ imblance matrix coeff. C & D*/
+#define		RXIQC_AC		0x02ff  /*bit mask for IQC matrix element A & C*/
+#define		RXIQC_BD		0x02ff0000 /*bit mask for IQC matrix element A & C*/
+
+/* 2 EFUSE_TEST (For RTL8723 partially) */
+#define EFUSE_SEL(x)					(((x) & 0x3) << 8)
+#define EFUSE_SEL_MASK				0x300
+#define EFUSE_WIFI_SEL_0				0x0
+
+/*REG_MULTI_FUNC_CTRL(For RTL8723 Only)*/
+#define	WL_HWPDN_EN				BIT(0)	/* Enable GPIO[9] as WiFi HW PDn source*/
+#define	WL_HWPDN_SL				BIT(1)	/* WiFi HW PDn polarity control*/
+#define	WL_FUNC_EN				BIT(2)	// WiFi function enable
+#define	WL_HWROF_EN				BIT(3)	// Enable GPIO[9] as WiFi RF HW PDn source
+#define	BT_HWPDN_EN				BIT(16)	// Enable GPIO[11] as BT HW PDn source
+#define	BT_HWPDN_SL				BIT(17)	// BT HW PDn polarity control
+#define	BT_FUNC_EN				BIT(18)	// BT function enable
+#define	BT_HWROF_EN				BIT(19)	// Enable GPIO[11] as BT/GPS RF HW PDn source
+#define	GPS_HWPDN_EN				BIT(20)	// Enable GPIO[10] as GPS HW PDn source
+#define	GPS_HWPDN_SL				BIT(21)	// GPS HW PDn polarity control
+#define	GPS_FUNC_EN				BIT(22)	// GPS function enable
+
+
+#define	BMASKBYTE0                	0xff
+#define	BMASKBYTE1                	0xff00
+#define	BMASKBYTE2                	0xff0000
+#define	BMASKBYTE3                	0xff000000
+#define	BMASKHWORD                	0xffff0000
+#define	BMASKLWORD                	0x0000ffff
+#define	BMASKDWORD                	0xffffffff
+#define	BMASK12BITS				  	0xfff
+#define	BMASKH4BITS				  	0xf0000000
+#define BMASKOFDM_D					0xffc00000
+#define	BMASKCCK				  	0x3f3f3f3f
+
+#define BRFREGOFFSETMASK			0xfffff
+
+#define	ODM_REG_CCK_RPT_FORMAT_11AC	0x804
+#define	ODM_REG_BB_RX_PATH_11AC			0x808
+/*PAGE 9*/
+#define	ODM_REG_OFDM_FA_RST_11AC		0x9A4
+/*PAGE A*/
+#define	ODM_REG_CCK_CCA_11AC			0xA0A
+#define	ODM_REG_CCK_FA_RST_11AC			0xA2C
+#define	ODM_REG_CCK_FA_11AC				0xA5C
+/*PAGE C*/
+#define	ODM_REG_IGI_A_11AC				0xC50
+/*PAGE E*/
+#define	ODM_REG_IGI_B_11AC				0xE50
+/*PAGE F*/
+#define	ODM_REG_OFDM_FA_11AC			0xF48
+
+
+//2 MAC REG LIST
+
+
+
+
+//DIG Related
+#define	ODM_BIT_IGI_11AC					0xFFFFFFFF
+#define	ODM_BIT_CCK_RPT_FORMAT_11AC		BIT16
+#define	ODM_BIT_BB_RX_PATH_11AC			0xF
+
+typedef enum AGGRE_SIZE{
+	HT_AGG_SIZE_8K = 0,
+	HT_AGG_SIZE_16K = 1,
+	HT_AGG_SIZE_32K = 2,
+	HT_AGG_SIZE_64K = 3,
+	VHT_AGG_SIZE_128K = 4,
+	VHT_AGG_SIZE_256K = 5,
+	VHT_AGG_SIZE_512K = 6,
+	VHT_AGG_SIZE_1024K = 7,
+}AGGRE_SIZE_E, *PAGGRE_SIZE_E;
+
+#define REG_AMPDU_MAX_LENGTH_8812	0x0458
+
+#endif
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/rf.c b/drivers/staging/rtl8821ae/rtl8821ae/rf.c
new file mode 100644
index 000000000000..87c1c9746c43
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/rf.c
@@ -0,0 +1,464 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "rf.h"
+#include "dm.h"
+
+static bool _rtl8821ae_phy_rf6052_config_parafile(struct ieee80211_hw *hw);
+
+void rtl8821ae_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw, u8 bandwidth)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	switch (bandwidth) {
+	case HT_CHANNEL_WIDTH_20:
+		rtl_set_rfreg(hw, RF90_PATH_A, RF_CHNLBW, BIT(11)|BIT(10), 3);
+		rtl_set_rfreg(hw, RF90_PATH_B, RF_CHNLBW, BIT(11)|BIT(10), 3);
+		break;
+	case HT_CHANNEL_WIDTH_20_40:
+		rtl_set_rfreg(hw, RF90_PATH_A, RF_CHNLBW, BIT(11)|BIT(10), 1);
+		rtl_set_rfreg(hw, RF90_PATH_B, RF_CHNLBW, BIT(11)|BIT(10), 1);
+		break;
+	case HT_CHANNEL_WIDTH_80:
+		rtl_set_rfreg(hw, RF90_PATH_A, RF_CHNLBW, BIT(11)|BIT(10), 0);
+		rtl_set_rfreg(hw, RF90_PATH_B, RF_CHNLBW, BIT(11)|BIT(10), 0);
+		break;
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("unknown bandwidth: %#X\n", bandwidth));
+		break;
+	}
+}
+
+void rtl8821ae_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
+				       u8 *ppowerlevel)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u32 tx_agc[2] = {0, 0}, tmpval;
+	bool turbo_scanoff = false;
+	u8 idx1, idx2;
+	u8 *ptr;
+	u8 direction;
+	u32 pwrtrac_value;
+
+	if (rtlefuse->eeprom_regulatory != 0)
+		turbo_scanoff = true;
+
+	if (mac->act_scanning == true) {
+		tx_agc[RF90_PATH_A] = 0x3f3f3f3f;
+		tx_agc[RF90_PATH_B] = 0x3f3f3f3f;
+
+		if (turbo_scanoff) {
+			for (idx1 = RF90_PATH_A; idx1 <= RF90_PATH_B; idx1++) {
+				tx_agc[idx1] = ppowerlevel[idx1] |
+				    (ppowerlevel[idx1] << 8) |
+				    (ppowerlevel[idx1] << 16) |
+				    (ppowerlevel[idx1] << 24);
+			}
+		}
+	} else {
+		for (idx1 = RF90_PATH_A; idx1 <= RF90_PATH_B; idx1++) {
+			tx_agc[idx1] = ppowerlevel[idx1] |
+			    (ppowerlevel[idx1] << 8) |
+			    (ppowerlevel[idx1] << 16) |
+			    (ppowerlevel[idx1] << 24);
+		}
+
+		if (rtlefuse->eeprom_regulatory == 0) {
+			tmpval =
+			    (rtlphy->mcs_txpwrlevel_origoffset[0][6]) +
+			    (rtlphy->mcs_txpwrlevel_origoffset[0][7] <<
+			     8);
+			tx_agc[RF90_PATH_A] += tmpval;
+
+			tmpval = (rtlphy->mcs_txpwrlevel_origoffset[0][14]) +
+			    (rtlphy->mcs_txpwrlevel_origoffset[0][15] <<
+			     24);
+			tx_agc[RF90_PATH_B] += tmpval;
+		}
+	}
+
+	for (idx1 = RF90_PATH_A; idx1 <= RF90_PATH_B; idx1++) {
+		ptr = (u8 *) (&(tx_agc[idx1]));
+		for (idx2 = 0; idx2 < 4; idx2++) {
+			if (*ptr > RF6052_MAX_TX_PWR)
+				*ptr = RF6052_MAX_TX_PWR;
+			ptr++;
+		}
+	}
+	rtl8821ae_dm_txpower_track_adjust(hw,1,&direction,&pwrtrac_value);
+	if (direction ==1){
+		tx_agc[0] += pwrtrac_value;
+		tx_agc[1] += pwrtrac_value;
+	} else if (direction == 2){
+		tx_agc[0] -= pwrtrac_value;
+		tx_agc[1] -= pwrtrac_value;
+	}
+	tmpval = tx_agc[RF90_PATH_A] ;
+	rtl_set_bbreg(hw, RTXAGC_A_CCK11_CCK1, MASKDWORD, tmpval);
+
+	RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+		("CCK PWR 1~11M (rf-A) = 0x%x (reg 0x%x)\n", tmpval,
+		 RTXAGC_A_CCK11_CCK1));
+
+	tmpval = tx_agc[RF90_PATH_B] ;
+	rtl_set_bbreg(hw, RTXAGC_B_CCK11_CCK1, MASKDWORD, tmpval);
+
+	RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+		("CCK PWR 11M (rf-B) = 0x%x (reg 0x%x)\n", tmpval,
+		 RTXAGC_B_CCK11_CCK1));
+}
+
+static void rtl8821ae_phy_get_power_base(struct ieee80211_hw *hw,
+				      u8 *ppowerlevel_ofdm, u8 *ppowerlevel_bw20, u8 *ppowerlevel_bw40, u8 channel,
+				      u32 *ofdmbase, u32 *mcsbase)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u32 powerBase0, powerBase1;
+	u8 i, powerlevel[2];
+
+	for (i = 0; i < 2; i++) {
+		powerBase0 = ppowerlevel_ofdm[i];
+
+		powerBase0 = (powerBase0 << 24) | (powerBase0 << 16) |
+		    (powerBase0 << 8) | powerBase0;
+		*(ofdmbase + i) = powerBase0;
+		RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+			(" [OFDM power base index rf(%c) = 0x%x]\n",
+			 ((i == 0) ? 'A' : 'B'), *(ofdmbase + i)));
+	}
+
+	for (i = 0; i < 2; i++) {
+		if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20) {
+			powerlevel[i] = ppowerlevel_bw20[i];
+		}else{
+			powerlevel[i] = ppowerlevel_bw40[i];
+		}
+		powerBase1 = powerlevel[i];
+		powerBase1 = (powerBase1 << 24) |
+		    (powerBase1 << 16) | (powerBase1 << 8) | powerBase1;
+
+		*(mcsbase + i) = powerBase1;
+
+		RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+			(" [MCS power base index rf(%c) = 0x%x]\n",
+			 ((i == 0) ? 'A' : 'B'), *(mcsbase + i)));
+	}
+}
+
+static void _rtl8821ae_get_txpower_writeval_by_regulatory(struct ieee80211_hw *hw,
+						       u8 channel, u8 index,
+						       u32 *powerBase0,
+						       u32 *powerBase1,
+						       u32 *p_outwriteval)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u8 i, chnlgroup = 0, pwr_diff_limit[4],pwr_diff = 0,customer_pwr_diff;
+	u32 writeVal, customer_limit, rf;
+
+	for (rf = 0; rf < 2; rf++) {
+		switch (rtlefuse->eeprom_regulatory) {
+		case 0:
+			chnlgroup = 0;
+
+			writeVal =
+			    rtlphy->mcs_txpwrlevel_origoffset[chnlgroup][index +
+									 (rf ? 8 : 0)]
+			    + ((index < 2) ? powerBase0[rf] : powerBase1[rf]);
+
+			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+				("RTK better performance, "
+				 "writeVal(%c) = 0x%x\n",
+				 ((rf == 0) ? 'A' : 'B'), writeVal));
+			break;
+		case 1:
+			if (rtlphy->pwrgroup_cnt == 1)
+				chnlgroup = 0;
+			else {
+				if(channel<3)
+					chnlgroup = 0;
+				else if (channel <6)
+					chnlgroup = 1;
+				else if (channel <9)
+					chnlgroup = 2;
+				else if (channel <12)
+					chnlgroup = 3;
+				else if (channel < 14)
+					chnlgroup = 4;
+				else if (channel == 14)
+					chnlgroup = 5;
+			}
+
+			writeVal =
+			    rtlphy->mcs_txpwrlevel_origoffset[chnlgroup]
+			    [index + (rf ? 8 : 0)] + ((index < 2) ?
+						      powerBase0[rf] :
+						      powerBase1[rf]);
+
+			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+				("Realtek regulatory, 20MHz, "
+				 "writeVal(%c) = 0x%x\n",
+				 ((rf == 0) ? 'A' : 'B'), writeVal));
+
+			break;
+		case 2:
+			writeVal =
+			    ((index < 2) ? powerBase0[rf] : powerBase1[rf]);
+
+			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+				("Better regulatory, "
+				 "writeVal(%c) = 0x%x\n",
+				 ((rf == 0) ? 'A' : 'B'), writeVal));
+			break;
+		case 3:
+			chnlgroup = 0;
+
+			if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40) {
+				RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+					("customer's limit, 40MHz "
+					 "rf(%c) = 0x%x\n",
+					 ((rf == 0) ? 'A' : 'B'),
+					 rtlefuse->pwrgroup_ht40[rf][channel -
+								     1]));
+			} else {
+				RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+					("customer's limit, 20MHz "
+					 "rf(%c) = 0x%x\n",
+					 ((rf == 0) ? 'A' : 'B'),
+					 rtlefuse->pwrgroup_ht20[rf][channel -
+								     1]));
+			}
+
+			if (index < 2)
+				pwr_diff = rtlefuse->txpwr_legacyhtdiff[rf][channel-1];
+			else if (rtlphy->current_chan_bw ==  HT_CHANNEL_WIDTH_20)
+				pwr_diff = rtlefuse->txpwr_ht20diff[rf][channel-1];
+
+			if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40)
+				customer_pwr_diff = rtlefuse->pwrgroup_ht40[rf][channel-1];
+			else
+				customer_pwr_diff = rtlefuse->pwrgroup_ht20[rf][channel-1];
+
+			if (pwr_diff > customer_pwr_diff)
+				pwr_diff = 0;
+			else
+				pwr_diff = customer_pwr_diff - pwr_diff;
+
+			for (i = 0; i < 4; i++) {
+				pwr_diff_limit[i] =
+				    (u8) ((rtlphy->mcs_txpwrlevel_origoffset
+					   [chnlgroup][index + (rf ? 8 : 0)] & (0x7f <<
+									(i * 8))) >> (i * 8));
+
+					if(pwr_diff_limit[i] > pwr_diff)
+						pwr_diff_limit[i] = pwr_diff;
+			}
+
+			customer_limit = (pwr_diff_limit[3] << 24) |
+			    (pwr_diff_limit[2] << 16) |
+			    (pwr_diff_limit[1] << 8) | (pwr_diff_limit[0]);
+
+			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+				("Customer's limit rf(%c) = 0x%x\n",
+				 ((rf == 0) ? 'A' : 'B'), customer_limit));
+
+			writeVal = customer_limit +
+			    ((index < 2) ? powerBase0[rf] : powerBase1[rf]);
+
+			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+				("Customer, writeVal rf(%c)= 0x%x\n",
+				 ((rf == 0) ? 'A' : 'B'), writeVal));
+			break;
+		default:
+			chnlgroup = 0;
+			writeVal =
+			    rtlphy->mcs_txpwrlevel_origoffset[chnlgroup]
+			    [index + (rf ? 8 : 0)]
+			    + ((index < 2) ? powerBase0[rf] : powerBase1[rf]);
+
+			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+				("RTK better performance, writeVal "
+				 "rf(%c) = 0x%x\n",
+				 ((rf == 0) ? 'A' : 'B'), writeVal));
+			break;
+		}
+
+		if (rtlpriv->dm.dynamic_txhighpower_lvl == TXHIGHPWRLEVEL_BT1)
+			writeVal = writeVal - 0x06060606;
+		else if (rtlpriv->dm.dynamic_txhighpower_lvl ==
+			 TXHIGHPWRLEVEL_BT2)
+			writeVal = writeVal - 0x0c0c0c0c;
+		*(p_outwriteval + rf) = writeVal;
+	}
+}
+
+static void _rtl8821ae_write_ofdm_power_reg(struct ieee80211_hw *hw,
+					 u8 index, u32 *pValue)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u16 regoffset_a[6] = {
+		RTXAGC_A_OFDM18_OFDM6, RTXAGC_A_OFDM54_OFDM24,
+		RTXAGC_A_MCS03_MCS00, RTXAGC_A_MCS07_MCS04,
+		RTXAGC_A_MCS11_MCS08, RTXAGC_A_MCS15_MCS12
+	};
+	u16 regoffset_b[6] = {
+		RTXAGC_B_OFDM18_OFDM6, RTXAGC_B_OFDM54_OFDM24,
+		RTXAGC_B_MCS03_MCS00, RTXAGC_B_MCS07_MCS04,
+		RTXAGC_B_MCS11_MCS08, RTXAGC_B_MCS15_MCS12
+	};
+	u8 i, rf, pwr_val[4];
+	u32 writeVal;
+	u16 regoffset;
+
+	for (rf = 0; rf < 2; rf++) {
+		writeVal = pValue[rf];
+		for (i = 0; i < 4; i++) {
+			pwr_val[i] = (u8) ((writeVal & (0x7f <<
+							(i * 8))) >> (i * 8));
+
+			if (pwr_val[i] > RF6052_MAX_TX_PWR)
+				pwr_val[i] = RF6052_MAX_TX_PWR;
+		}
+		writeVal = (pwr_val[3] << 24) | (pwr_val[2] << 16) |
+		    (pwr_val[1] << 8) | pwr_val[0];
+
+		if (rf == 0)
+			regoffset = regoffset_a[index];
+		else
+			regoffset = regoffset_b[index];
+		rtl_set_bbreg(hw, regoffset, MASKDWORD, writeVal);
+
+		RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+			("Set 0x%x = %08x\n", regoffset, writeVal));
+	}
+}
+
+void rtl8821ae_phy_rf6052_set_ofdm_txpower(struct ieee80211_hw *hw,
+					      u8 *ppowerlevel_ofdm, u8 *ppowerlevel_bw20, u8 *ppowerlevel_bw40, u8 channel)
+{
+	u32 writeVal[2], powerBase0[2], powerBase1[2];
+	u8 index;
+	u8 direction;
+	u32 pwrtrac_value;
+
+	rtl8821ae_phy_get_power_base(hw, ppowerlevel_ofdm, ppowerlevel_bw20, ppowerlevel_bw40,
+				  channel, &powerBase0[0], &powerBase1[0]);
+
+	rtl8821ae_dm_txpower_track_adjust(hw,1,&direction,&pwrtrac_value);
+
+	for (index = 0; index < 6; index++) {
+		_rtl8821ae_get_txpower_writeval_by_regulatory(hw,
+							   channel, index,
+							   &powerBase0[0],
+							   &powerBase1[0],
+							   &writeVal[0]);
+		if (direction ==1){
+			writeVal[0] += pwrtrac_value;
+			writeVal[1] += pwrtrac_value;
+		} else if (direction == 2){
+			writeVal[0] -= pwrtrac_value;
+			writeVal[1] -= pwrtrac_value;
+		}
+		_rtl8821ae_write_ofdm_power_reg(hw, index, &writeVal[0]);
+	}
+}
+
+bool rtl8821ae_phy_rf6052_config(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	if (rtlphy->rf_type == RF_1T1R)
+		rtlphy->num_total_rfpath = 1;
+	else
+		rtlphy->num_total_rfpath = 2;
+
+	return _rtl8821ae_phy_rf6052_config_parafile(hw);
+
+}
+
+static bool _rtl8821ae_phy_rf6052_config_parafile(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	//u32 u4_regvalue = 0;
+	u8 rfpath;
+	bool rtstatus = true;
+	//struct bb_reg_def *pphyreg;
+
+	for (rfpath = 0; rfpath < rtlphy->num_total_rfpath; rfpath++) {
+		switch (rfpath) {
+		case RF90_PATH_A: {
+			if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+				rtstatus = rtl8812ae_phy_config_rf_with_headerfile(hw,
+									(enum radio_path)rfpath);
+			else
+				rtstatus = rtl8821ae_phy_config_rf_with_headerfile(hw,
+									(enum radio_path)rfpath);
+			break;
+			}
+		case RF90_PATH_B: {
+			if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+				rtstatus = rtl8812ae_phy_config_rf_with_headerfile(hw,
+									(enum radio_path)rfpath);
+			else
+				rtstatus = rtl8821ae_phy_config_rf_with_headerfile(hw,
+									(enum radio_path)rfpath);
+			break;
+			}
+		case RF90_PATH_C:
+			break;
+		case RF90_PATH_D:
+			break;
+		}
+
+		if (rtstatus != true) {
+			RT_TRACE(COMP_INIT, DBG_TRACE,
+				 ("Radio[%d] Fail!!", rfpath));
+			return false;
+		}
+
+	}
+
+	/*put arrays in dm.c*/
+	/*_rtl8821ae_config_rf_txpwr_track_headerfile(hw);*/
+	RT_TRACE(COMP_INIT, DBG_TRACE, ("\n"));
+	return rtstatus;
+}
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/rf.h b/drivers/staging/rtl8821ae/rtl8821ae/rf.h
new file mode 100644
index 000000000000..b665c0ff1b7d
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/rf.h
@@ -0,0 +1,46 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL8821AE_RF_H__
+#define __RTL8821AE_RF_H__
+
+#define RF6052_MAX_TX_PWR		0x3F
+#define RF6052_MAX_REG			0x3F
+
+extern void rtl8821ae_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw,
+					    									u8 bandwidth);
+extern void rtl8821ae_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
+					      									  u8 *ppowerlevel);
+extern void rtl8821ae_phy_rf6052_set_ofdm_txpower(struct ieee80211_hw *hw,
+					      									   u8 *ppowerlevel_ofdm,
+					      									   u8 *ppowerlevel_bw20,
+					      									   u8 *ppowerlevel_bw40,
+					      									   u8 channel);
+extern bool rtl8821ae_phy_rf6052_config(struct ieee80211_hw *hw);
+#endif
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/sw.c b/drivers/staging/rtl8821ae/rtl8821ae/sw.c
new file mode 100644
index 000000000000..85a3474fc099
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/sw.c
@@ -0,0 +1,499 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include <linux/vmalloc.h>
+#include <linux/module.h>
+
+#include "../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 "trx.h"
+#include "led.h"
+#include "table.h"
+#include "hal_btc.h"
+#include "../btcoexist/rtl_btc.h"
+
+void rtl8821ae_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;
+}
+
+/*InitializeVariables8812E*/
+int rtl8821ae_init_sw_vars(struct ieee80211_hw *hw)
+{
+	int err = 0;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	const struct firmware *firmware;
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	char *fw_name = NULL;
+
+	rtl8821ae_bt_reg_init(hw);
+	rtlpriv->btcoexist.btc_ops = 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) | BIT(24) | BIT(25);
+
+	mac->ht_enable = true;
+
+	rtlpriv->rtlhal.current_bandtype = BAND_ON_2_4G;
+	/*following 2 is for register 5G band, refer to _rtl_init_mac80211()*/
+	rtlpriv->rtlhal.bandset = BAND_ON_BOTH;
+	rtlpriv->rtlhal.macphymode = SINGLEMAC_SINGLEPHY;
+
+	rtlpci->receive_config = (RCR_APPFCS			|
+							  RCR_APP_MIC 			|
+							  RCR_APP_ICV 			|
+							  RCR_APP_PHYST_RXFF	|
+							  RCR_NONQOS_VHT		|
+							  RCR_HTC_LOC_CTRL		|
+							  RCR_AMF 				|
+							  RCR_ACF 				|
+							  RCR_ADF 				|	/*This bit controls the PS-Poll packet filter.*/
+							  RCR_AICV				|
+							  RCR_ACRC32			|
+							  RCR_AB				|
+							  RCR_AM				|
+							  RCR_APM 				|
+							  0);
+
+
+	rtlpci->irq_mask[0] =
+	     (u32) (IMR_PSTIMEOUT			|
+				IMR_GTINT3 				|
+				/*IMR_TBDER				|
+				IMR_TBDOK				|
+				IMR_BCNDMAINT0 			|*/
+				IMR_HSISR_IND_ON_INT	|
+				IMR_C2HCMD 				|
+				IMR_HIGHDOK				|
+				IMR_MGNTDOK				|
+				IMR_BKDOK				|
+				IMR_BEDOK				|
+				IMR_VIDOK				|
+				IMR_VODOK				|
+				IMR_RDU					|
+				IMR_ROK					|
+				0);
+
+	rtlpci->irq_mask[1]	=
+		 (u32)(	IMR_RXFOVW |
+		 		IMR_TXFOVW |
+				0);
+
+	/* 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 */
+	rtl8821ae_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 firmware buf */
+	rtlpriv->rtlhal.pfirmware = (u8 *) vmalloc(0x8000);
+	if (!rtlpriv->rtlhal.pfirmware) {
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("Can't alloc buffer for fw.\n"));
+		return 1;
+	}
+
+	fw_name = "rtlwifi/rtl8821aefw.bin";
+	err = request_firmware(&firmware, fw_name, rtlpriv->io.dev);
+	if (err) {
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("Failed to request firmware!\n"));
+		return 1;
+	}
+
+	if (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);
+
+	if (rtlpriv->cfg->ops->get_btc_status()){
+		rtlpriv->btcoexist.btc_ops->btc_init_variables(rtlpriv);
+		rtlpriv->btcoexist.btc_ops->btc_init_hal_vars(rtlpriv);
+	}
+
+	RT_TRACE(COMP_INIT, DBG_LOUD, (" FirmwareDownload OK\n"));
+	return err;
+}
+
+void rtl8821ae_deinit_sw_vars(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	//printk("=========>rtl8821ae_deinit_sw_vars().\n");
+	if (rtlpriv->cfg->ops->get_btc_status()){
+		//printk("=========>rtl8821ae_deinit_sw_vars().get_btc_status\n");
+		rtlpriv->btcoexist.btc_ops->btc_halt_notify();
+	}
+	if (rtlpriv->rtlhal.pfirmware) {
+		//printk("=========>rtl8821ae_deinit_sw_vars().rtlpriv->rtlhal.pfirmware\n");
+		vfree(rtlpriv->rtlhal.pfirmware);
+		rtlpriv->rtlhal.pfirmware = NULL;
+	}
+	//printk("<=========rtl8821ae_deinit_sw_vars().\n");
+}
+
+u32 rtl8812ae_rx_command_packet_handler(
+	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:
+			rtl8812ae_c2h_packet_handler(hw, skb->data, (u8) skb->len);
+			result = 1;
+			RT_TRACE(COMP_RECV, DBG_LOUD,
+				("===>rtl8821ae_rx_command_packet_handler(): (u8) skb->len=%d\n\n", skb->len));
+			break;
+		default:
+			RT_TRACE(COMP_RECV, DBG_LOUD,
+				("===>rtl8821ae_rx_command_packet_handler(): No this packet type!!\n"));
+			break;
+	}
+
+	return result;
+}
+
+
+/* get bt coexist status */
+bool rtl8821ae_get_btc_status(void)
+{
+	return true;
+}
+
+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,
+	.interrupt_recognized = rtl8821ae_interrupt_recognized,
+	.hw_init = rtl8821ae_hw_init,
+	.hw_disable = rtl8821ae_card_disable,
+	.hw_suspend = rtl8821ae_suspend,
+	.hw_resume = rtl8821ae_resume,
+	.enable_interrupt = rtl8821ae_enable_interrupt,
+	.disable_interrupt = rtl8821ae_disable_interrupt,
+	.set_network_type = rtl8821ae_set_network_type,
+	.set_chk_bssid = rtl8821ae_set_check_bssid,
+	.set_qos = rtl8821ae_set_qos,
+	.set_bcn_reg = rtl8821ae_set_beacon_related_registers,
+	.set_bcn_intv = rtl8821ae_set_beacon_interval,
+	.update_interrupt_mask = rtl8821ae_update_interrupt_mask,
+	.get_hw_reg = rtl8821ae_get_hw_reg,
+	.set_hw_reg = rtl8821ae_set_hw_reg,
+	.update_rate_tbl = rtl8821ae_update_hal_rate_tbl,
+	.fill_tx_desc = rtl8821ae_tx_fill_desc,
+	.fill_tx_cmddesc = rtl8821ae_tx_fill_cmddesc,
+	.query_rx_desc = rtl8821ae_rx_query_desc,
+	.set_channel_access = rtl8821ae_update_channel_access_setting,
+	.radio_onoff_checking = rtl8821ae_gpio_radio_on_off_checking,
+	.set_bw_mode = rtl8821ae_phy_set_bw_mode,
+	.switch_channel = rtl8821ae_phy_sw_chnl,
+	.dm_watchdog = rtl8821ae_dm_watchdog,
+	.scan_operation_backup = rtl8821ae_phy_scan_operation_backup,
+	.set_rf_power_state = rtl8821ae_phy_set_rf_power_state,
+	.led_control = rtl8821ae_led_control,
+	.set_desc = rtl8821ae_set_desc,
+	.get_desc = rtl8821ae_get_desc,
+	.is_tx_desc_closed = rtl8821ae_is_tx_desc_closed,
+	.tx_polling = rtl8821ae_tx_polling,
+	.enable_hw_sec = rtl8821ae_enable_hw_security_config,
+	.set_key = rtl8821ae_set_key,
+	.init_sw_leds = rtl8821ae_init_sw_leds,
+	.allow_all_destaddr = rtl8821ae_allow_all_destaddr,
+	.get_bbreg = rtl8821ae_phy_query_bb_reg,
+	.set_bbreg = rtl8821ae_phy_set_bb_reg,
+	.get_rfreg = rtl8821ae_phy_query_rf_reg,
+	.set_rfreg = rtl8821ae_phy_set_rf_reg,
+	.c2h_command_handle = rtl_8821ae_c2h_command_handle,
+	.bt_wifi_media_status_notify = rtl_8821ae_bt_wifi_media_status_notify,
+	.bt_turn_off_bt_coexist_before_enter_lps = rtl8821ae_dm_bt_turn_off_bt_coexist_before_enter_lps,
+	.fill_h2c_cmd = rtl8821ae_fill_h2c_cmd,
+	.get_btc_status = rtl8821ae_get_btc_status,
+	.rx_command_packet_handler = rtl8812ae_rx_command_packet_handler,
+};
+
+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 = {
+	.bar_id = 2,
+	.write_readback = true,
+	.name = "rtl8821ae_pci",
+	.fw_name = "rtlwifi/rtl8821aefw.bin",
+	.ops = &rtl8821ae_hal_ops,
+	.mod_params = &rtl8821ae_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] = DESC_RATE1M,
+	.maps[RTL_RC_CCK_RATE2M] =  DESC_RATE2M,
+	.maps[RTL_RC_CCK_RATE5_5M] =  DESC_RATE5_5M,
+	.maps[RTL_RC_CCK_RATE11M] =  DESC_RATE11M,
+	.maps[RTL_RC_OFDM_RATE6M] =  DESC_RATE6M,
+	.maps[RTL_RC_OFDM_RATE9M] =  DESC_RATE9M,
+	.maps[RTL_RC_OFDM_RATE12M] =  DESC_RATE12M,
+	.maps[RTL_RC_OFDM_RATE18M] =  DESC_RATE18M,
+	.maps[RTL_RC_OFDM_RATE24M] =  DESC_RATE24M,
+	.maps[RTL_RC_OFDM_RATE36M] =  DESC_RATE36M,
+	.maps[RTL_RC_OFDM_RATE48M] =  DESC_RATE48M,
+	.maps[RTL_RC_OFDM_RATE54M] =  DESC_RATE54M,
+
+	.maps[RTL_RC_HT_RATEMCS7] =  DESC_RATEMCS7,
+	.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);
+
+MODULE_AUTHOR("Ping Yan<ping_yan@realsil.com.cn>");
+MODULE_AUTHOR("Realtek WlanFAE	<wlanfae@realtek.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Realtek 8821ae 802.11ac PCI wireless");
+MODULE_FIRMWARE("rtlwifi/rtl8821aefw.bin");
+
+module_param_named(swenc, rtl8821ae_mod_params.sw_crypto, bool, 0444);
+module_param_named(ips, rtl8821ae_mod_params.b_inactiveps, bool, 0444);
+module_param_named(swlps, rtl8821ae_mod_params.b_swctrl_lps, bool, 0444);
+module_param_named(fwlps, rtl8821ae_mod_params.b_fwctrl_lps, bool, 0444);
+MODULE_PARM_DESC(swenc, "using hardware crypto (default 0 [hardware])\n");
+MODULE_PARM_DESC(ips, "using no link power save (default 1 is open)\n");
+MODULE_PARM_DESC(fwlps, "using linked fw control power save (default 1 is open)\n");
+
+#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 struct pci_driver rtl8821ae_driver = {
+	.name = KBUILD_MODNAME,
+	.id_table = rtl8821ae_pci_ids,
+	.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
+
+};
+
+
+extern int rtl_core_module_init(void);
+extern void rtl_core_module_exit(void);
+
+static int __init rtl8821ae_module_init(void)
+{
+	int ret;
+
+	ret = rtl_core_module_init();
+	if (ret)
+		return ret;
+
+	//printk("==========>rtl8821ae_module_init().\n");
+	ret = pci_register_driver(&rtl8821ae_driver);
+	if (ret)
+		RT_ASSERT(false, (": No device found\n"));
+
+	return ret;
+}
+
+static void __exit rtl8821ae_module_exit(void)
+{
+	pci_unregister_driver(&rtl8821ae_driver);
+	rtl_core_module_exit();
+}
+
+module_init(rtl8821ae_module_init);
+module_exit(rtl8821ae_module_exit);
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/sw.h b/drivers/staging/rtl8821ae/rtl8821ae/sw.h
new file mode 100644
index 000000000000..3d49b2f043da
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/sw.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 __RTL8821AE_SW_H__
+#define __RTL8821AE_SW_H__
+
+int rtl8821ae_init_sw_vars(struct ieee80211_hw *hw);
+void rtl8821ae_deinit_sw_vars(struct ieee80211_hw *hw);
+void rtl8821ae_init_var_map(struct ieee80211_hw *hw);
+bool rtl8821ae_get_btc_status(void);
+
+
+#endif
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/table.c b/drivers/staging/rtl8821ae/rtl8821ae/table.c
new file mode 100644
index 000000000000..a6c4ca4fd9b2
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/table.c
@@ -0,0 +1,4002 @@
+/******************************************************************************
+ *
+ * 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>
+ *
+ *****************************************************************************/
+
+#include "table.h"
+u32 RTL8812AE_PHY_REG_ARRAY[] = {
+		0x800, 0x8020D010,
+		0x804, 0x080112E0,
+		0x808, 0x0E028233,
+		0x80C, 0x12131113,
+		0x810, 0x20101263,
+		0x814, 0x020C3D10,
+		0x818, 0x03A00385,
+		0x820, 0x00000000,
+		0x824, 0x00030FE0,
+		0x828, 0x00000000,
+		0x82C, 0x002083DD,
+		0x830, 0x2AAA6C86,
+		0x834, 0x0037A706,
+		0x838, 0x06C89B44,
+		0x83C, 0x0000095B,
+		0x840, 0xC0000001,
+		0x844, 0x40003CDE,
+		0x848, 0x6210FF8B,
+		0x84C, 0x6CFDFFB8,
+		0x850, 0x28874706,
+		0x854, 0x0001520C,
+		0x858, 0x8060E000,
+		0x85C, 0x74210168,
+		0x860, 0x6929C321,
+		0x864, 0x79727432,
+		0x868, 0x8CA7A314,
+		0x86C, 0x338C2878,
+		0x870, 0x03333333,
+		0x874, 0x31602C2E,
+		0x878, 0x00003152,
+		0x87C, 0x000FC000,
+		0x8A0, 0x00000013,
+		0x8A4, 0x7F7F7F7F,
+		0x8A8, 0xA202033E,
+		0x8AC, 0x0FF0FA0A,
+		0x8B0, 0x00000600,
+		0x8B4, 0x000FC080,
+		0x8B8, 0x6C0057FF,
+		0x8BC, 0x4CA520A3,
+		0x8C0, 0x27F00020,
+		0x8C4, 0x00000000,
+		0x8C8, 0x00013169,
+		0x8CC, 0x08248492,
+		0x8D0, 0x0000B800,
+		0x8DC, 0x00000000,
+		0x8D4, 0x940008A0,
+		0x8D8, 0x290B5612,
+		0x8F8, 0x400002C0,
+		0x8FC, 0x00000000,
+	0xFF0F07D8, 0xABCD,
+		0x900, 0x00000700,
+	0xFF0F07D0, 0xCDEF,
+		0x900, 0x00000700,
+	0xCDCDCDCD, 0xCDCD,
+		0x900, 0x00000700,
+	0xFF0F07D8, 0xDEAD,
+		0x90C, 0x00000000,
+		0x910, 0x0000FC00,
+		0x914, 0x00000404,
+		0x918, 0x1C1028C0,
+		0x91C, 0x64B11A1C,
+		0x920, 0xE0767233,
+		0x924, 0x055AA500,
+		0x928, 0x00000004,
+		0x92C, 0xFFFE0000,
+		0x930, 0xFFFFFFFE,
+		0x934, 0x001FFFFF,
+		0x960, 0x00000000,
+		0x964, 0x00000000,
+		0x968, 0x00000000,
+		0x96C, 0x00000000,
+		0x970, 0x801FFFFF,
+		0x978, 0x00000000,
+		0x97C, 0x00000000,
+		0x980, 0x00000000,
+		0x984, 0x00000000,
+		0x988, 0x00000000,
+		0x990, 0x27100000,
+		0x994, 0xFFFF0100,
+		0x998, 0xFFFFFF5C,
+		0x99C, 0xFFFFFFFF,
+		0x9A0, 0x000000FF,
+		0x9A4, 0x00080080,
+		0x9A8, 0x00000000,
+		0x9AC, 0x00000000,
+		0x9B0, 0x81081008,
+		0x9B4, 0x00000000,
+		0x9B8, 0x01081008,
+		0x9BC, 0x01081008,
+		0x9D0, 0x00000000,
+		0x9D4, 0x00000000,
+		0x9D8, 0x00000000,
+		0x9DC, 0x00000000,
+		0x9E4, 0x00000002,
+		0x9E8, 0x000002D5,
+		0xA00, 0x00D047C8,
+		0xA04, 0x01FF000C,
+		0xA08, 0x8C838300,
+		0xA0C, 0x2E7F000F,
+		0xA10, 0x9500BB78,
+		0xA14, 0x11144028,
+		0xA18, 0x00881117,
+		0xA1C, 0x89140F00,
+		0xA20, 0x1A1B0000,
+		0xA24, 0x090E1317,
+		0xA28, 0x00000204,
+		0xA2C, 0x00900000,
+		0xA70, 0x101FFF00,
+		0xA74, 0x00000008,
+		0xA78, 0x00000900,
+		0xA7C, 0x225B0606,
+		0xA80, 0x218075B2,
+		0xA84, 0x001F8C80,
+		0xB00, 0x03100000,
+		0xB04, 0x0000B000,
+		0xB08, 0xAE0201EB,
+		0xB0C, 0x01003207,
+		0xB10, 0x00009807,
+		0xB14, 0x01000000,
+		0xB18, 0x00000002,
+		0xB1C, 0x00000002,
+		0xB20, 0x0000001F,
+		0xB24, 0x03020100,
+		0xB28, 0x07060504,
+		0xB2C, 0x0B0A0908,
+		0xB30, 0x0F0E0D0C,
+		0xB34, 0x13121110,
+		0xB38, 0x17161514,
+		0xB3C, 0x0000003A,
+		0xB40, 0x00000000,
+		0xB44, 0x00000000,
+		0xB48, 0x13000032,
+		0xB4C, 0x48080000,
+		0xB50, 0x00000000,
+		0xB54, 0x00000000,
+		0xB58, 0x00000000,
+		0xB5C, 0x00000000,
+		0xC00, 0x00000007,
+		0xC04, 0x00042020,
+		0xC08, 0x80410231,
+		0xC0C, 0x00000000,
+		0xC10, 0x00000100,
+		0xC14, 0x01000000,
+		0xC1C, 0x40000003,
+		0xC20, 0x12121212,
+		0xC24, 0x12121212,
+		0xC28, 0x12121212,
+		0xC2C, 0x12121212,
+		0xC30, 0x12121212,
+		0xC34, 0x12121212,
+		0xC38, 0x12121212,
+		0xC3C, 0x12121212,
+		0xC40, 0x12121212,
+		0xC44, 0x12121212,
+		0xC48, 0x12121212,
+		0xC4C, 0x12121212,
+		0xC50, 0x00000020,
+		0xC54, 0x0008121C,
+		0xC58, 0x30000C1C,
+		0xC5C, 0x00000058,
+		0xC60, 0x34344443,
+		0xC64, 0x07003333,
+		0xC68, 0x59791979,
+		0xC6C, 0x59795979,
+		0xC70, 0x19795979,
+		0xC74, 0x19795979,
+		0xC78, 0x19791979,
+		0xC7C, 0x19791979,
+		0xC80, 0x19791979,
+		0xC84, 0x19791979,
+		0xC94, 0x0100005C,
+		0xC98, 0x00000000,
+		0xC9C, 0x00000000,
+		0xCA0, 0x00000029,
+		0xCA4, 0x08040201,
+		0xCA8, 0x80402010,
+	0xFF0F0740, 0xABCD,
+		0xCB0, 0x77547717,
+	0xFF0F01C0, 0xCDEF,
+		0xCB0, 0x77547717,
+	0xFF0F02C0, 0xCDEF,
+		0xCB0, 0x77547717,
+	0xFF0F07D8, 0xCDEF,
+		0xCB0, 0x54547710,
+	0xFF0F07D0, 0xCDEF,
+		0xCB0, 0x54547710,
+	0xCDCDCDCD, 0xCDCD,
+		0xCB0, 0x77547777,
+	0xFF0F0740, 0xDEAD,
+		0xCB4, 0x00000077,
+		0xCB8, 0x00508242,
+		0xE00, 0x00000007,
+		0xE04, 0x00042020,
+		0xE08, 0x80410231,
+		0xE0C, 0x00000000,
+		0xE10, 0x00000100,
+		0xE14, 0x01000000,
+		0xE1C, 0x40000003,
+		0xE20, 0x12121212,
+		0xE24, 0x12121212,
+		0xE28, 0x12121212,
+		0xE2C, 0x12121212,
+		0xE30, 0x12121212,
+		0xE34, 0x12121212,
+		0xE38, 0x12121212,
+		0xE3C, 0x12121212,
+		0xE40, 0x12121212,
+		0xE44, 0x12121212,
+		0xE48, 0x12121212,
+		0xE4C, 0x12121212,
+		0xE50, 0x00000020,
+		0xE54, 0x0008121C,
+		0xE58, 0x30000C1C,
+		0xE5C, 0x00000058,
+		0xE60, 0x34344443,
+		0xE64, 0x07003333,
+		0xE68, 0x59791979,
+		0xE6C, 0x59795979,
+		0xE70, 0x19795979,
+		0xE74, 0x19795979,
+		0xE78, 0x19791979,
+		0xE7C, 0x19791979,
+		0xE80, 0x19791979,
+		0xE84, 0x19791979,
+		0xE94, 0x0100005C,
+		0xE98, 0x00000000,
+		0xE9C, 0x00000000,
+		0xEA0, 0x00000029,
+		0xEA4, 0x08040201,
+		0xEA8, 0x80402010,
+	0xFF0F0740, 0xABCD,
+		0xEB0, 0x77547717,
+	0xFF0F01C0, 0xCDEF,
+		0xEB0, 0x77547717,
+	0xFF0F02C0, 0xCDEF,
+		0xEB0, 0x77547717,
+	0xFF0F07D8, 0xCDEF,
+		0xEB0, 0x54547710,
+	0xFF0F07D0, 0xCDEF,
+		0xEB0, 0x54547710,
+	0xCDCDCDCD, 0xCDCD,
+		0xEB0, 0x77547777,
+	0xFF0F0740, 0xDEAD,
+		0xEB4, 0x00000077,
+		0xEB8, 0x00508242,
+};
+
+u32 RTL8821AE_PHY_REG_ARRAY[] = {
+	0x800, 0x0020D090,
+	0x804, 0x080112E0,
+	0x808, 0x0E028211,
+	0x80C, 0x92131111,
+	0x810, 0x20101261,
+	0x814, 0x020C3D10,
+	0x818, 0x03A00385,
+	0x820, 0x00000000,
+	0x824, 0x00030FE0,
+	0x828, 0x00000000,
+	0x82C, 0x002081DD,
+	0x830, 0x2AAA8E24,
+	0x834, 0x0037A706,
+	0x838, 0x06489B44,
+	0x83C, 0x0000095B,
+	0x840, 0xC0000001,
+	0x844, 0x40003CDE,
+	0x848, 0x62103F8B,
+	0x84C, 0x6CFDFFB8,
+	0x850, 0x28874706,
+	0x854, 0x0001520C,
+	0x858, 0x8060E000,
+	0x85C, 0x74210168,
+	0x860, 0x6929C321,
+	0x864, 0x79727432,
+	0x868, 0x8CA7A314,
+	0x86C, 0x888C2878,
+	0x870, 0x08888888,
+	0x874, 0x31612C2E,
+	0x878, 0x00000152,
+	0x87C, 0x000FD000,
+	0x8A0, 0x00000013,
+	0x8A4, 0x7F7F7F7F,
+	0x8A8, 0xA2000338,
+	0x8AC, 0x0FF0FA0A,
+	0x8B4, 0x000FC080,
+	0x8B8, 0x6C10D7FF,
+	0x8BC, 0x0CA52090,
+	0x8C0, 0x1BF00020,
+	0x8C4, 0x00000000,
+	0x8C8, 0x00013169,
+	0x8CC, 0x08248492,
+	0x8D4, 0x940008A0,
+	0x8D8, 0x290B5612,
+	0x8F8, 0x400002C0,
+	0x8FC, 0x00000000,
+	0x900, 0x00000700,
+	0x90C, 0x00000000,
+	0x910, 0x0000FC00,
+	0x914, 0x00000404,
+	0x918, 0x1C1028C0,
+	0x91C, 0x64B11A1C,
+	0x920, 0xE0767233,
+	0x924, 0x055AA500,
+	0x928, 0x00000004,
+	0x92C, 0xFFFE0000,
+	0x930, 0xFFFFFFFE,
+	0x934, 0x001FFFFF,
+	0x960, 0x00000000,
+	0x964, 0x00000000,
+	0x968, 0x00000000,
+	0x96C, 0x00000000,
+	0x970, 0x801FFFFF,
+	0x974, 0x000003FF,
+	0x978, 0x00000000,
+	0x97C, 0x00000000,
+	0x980, 0x00000000,
+	0x984, 0x00000000,
+	0x988, 0x00000000,
+	0x990, 0x27100000,
+	0x994, 0xFFFF0100,
+	0x998, 0xFFFFFF5C,
+	0x99C, 0xFFFFFFFF,
+	0x9A0, 0x000000FF,
+	0x9A4, 0x00480080,
+	0x9A8, 0x00000000,
+	0x9AC, 0x00000000,
+	0x9B0, 0x81081008,
+	0x9B4, 0x01081008,
+	0x9B8, 0x01081008,
+	0x9BC, 0x01081008,
+	0x9D0, 0x00000000,
+	0x9D4, 0x00000000,
+	0x9D8, 0x00000000,
+	0x9DC, 0x00000000,
+	0x9E0, 0x00005D00,
+	0x9E4, 0x00000002,
+	0x9E8, 0x00000001,
+	0xA00, 0x00D047C8,
+	0xA04, 0x01FF000C,
+	0xA08, 0x8C8A8300,
+	0xA0C, 0x2E68000F,
+	0xA10, 0x9500BB78,
+	0xA14, 0x11144028,
+	0xA18, 0x00881117,
+	0xA1C, 0x89140F00,
+	0xA20, 0x1A1B0000,
+	0xA24, 0x090E1317,
+	0xA28, 0x00000204,
+	0xA2C, 0x00900000,
+	0xA70, 0x101FFF00,
+	0xA74, 0x00000008,
+	0xA78, 0x00000900,
+	0xA7C, 0x225B0606,
+	0xA80, 0x21805490,
+	0xA84, 0x001F0000,
+	0xB00, 0x03100040,
+	0xB04, 0x0000B000,
+	0xB08, 0xAE0201EB,
+	0xB0C, 0x01003207,
+	0xB10, 0x00009807,
+	0xB14, 0x01000000,
+	0xB18, 0x00000002,
+	0xB1C, 0x00000002,
+	0xB20, 0x0000001F,
+	0xB24, 0x03020100,
+	0xB28, 0x07060504,
+	0xB2C, 0x0B0A0908,
+	0xB30, 0x0F0E0D0C,
+	0xB34, 0x13121110,
+	0xB38, 0x17161514,
+	0xB3C, 0x0000003A,
+	0xB40, 0x00000000,
+	0xB44, 0x00000000,
+	0xB48, 0x13000032,
+	0xB4C, 0x48080000,
+	0xB50, 0x00000000,
+	0xB54, 0x00000000,
+	0xB58, 0x00000000,
+	0xB5C, 0x00000000,
+	0xC00, 0x00000007,
+	0xC04, 0x00042020,
+	0xC08, 0x80410231,
+	0xC0C, 0x00000000,
+	0xC10, 0x00000100,
+	0xC14, 0x01000000,
+	0xC1C, 0x40000003,
+	0xC20, 0x2C2C2C2C,
+	0xC24, 0x30303030,
+	0xC28, 0x30303030,
+	0xC2C, 0x2C2C2C2C,
+	0xC30, 0x2C2C2C2C,
+	0xC34, 0x2C2C2C2C,
+	0xC38, 0x2C2C2C2C,
+	0xC3C, 0x2A2A2A2A,
+	0xC40, 0x2A2A2A2A,
+	0xC44, 0x2A2A2A2A,
+	0xC48, 0x2A2A2A2A,
+	0xC4C, 0x2A2A2A2A,
+	0xC50, 0x00000020,
+	0xC54, 0x001C1208,
+	0xC58, 0x30000C1C,
+	0xC5C, 0x00000058,
+	0xC60, 0x34344443,
+	0xC64, 0x07003333,
+	0xC68, 0x19791979,
+	0xC6C, 0x19791979,
+	0xC70, 0x19791979,
+	0xC74, 0x19791979,
+	0xC78, 0x19791979,
+	0xC7C, 0x19791979,
+	0xC80, 0x19791979,
+	0xC84, 0x19791979,
+	0xC94, 0x0100005C,
+	0xC98, 0x00000000,
+	0xC9C, 0x00000000,
+	0xCA0, 0x00000029,
+	0xCA4, 0x08040201,
+	0xCA8, 0x80402010,
+	0xCB0, 0x77775747,
+	0xCB4, 0x10000077,
+	0xCB8, 0x00508240,
+};
+
+u32 RTL8812AE_PHY_REG_ARRAY_PG[] = {
+	0, 0, 0, 0x00000c20, 0xffffffff, 0x34363840,
+	0, 0, 0, 0x00000c24, 0xffffffff, 0x42424444,
+	0, 0, 0, 0x00000c28, 0xffffffff, 0x30323638,
+	0, 0, 0, 0x00000c2c, 0xffffffff, 0x40424444,
+	0, 0, 0, 0x00000c30, 0xffffffff, 0x28303236,
+	0, 0, 1, 0x00000c34, 0xffffffff, 0x38404242,
+	0, 0, 1, 0x00000c38, 0xffffffff, 0x26283034,
+	0, 0, 0, 0x00000c3c, 0xffffffff, 0x40424444,
+	0, 0, 0, 0x00000c40, 0xffffffff, 0x28303236,
+	0, 0, 0, 0x00000c44, 0xffffffff, 0x42422426,
+	0, 0, 1, 0x00000c48, 0xffffffff, 0x30343840,
+	0, 0, 1, 0x00000c4c, 0xffffffff, 0x22242628,
+	0, 1, 0, 0x00000e20, 0xffffffff, 0x34363840,
+	0, 1, 0, 0x00000e24, 0xffffffff, 0x42424444,
+	0, 1, 0, 0x00000e28, 0xffffffff, 0x30323638,
+	0, 1, 0, 0x00000e2c, 0xffffffff, 0x40424444,
+	0, 1, 0, 0x00000e30, 0xffffffff, 0x28303236,
+	0, 1, 1, 0x00000e34, 0xffffffff, 0x38404242,
+	0, 1, 1, 0x00000e38, 0xffffffff, 0x26283034,
+	0, 1, 0, 0x00000e3c, 0xffffffff, 0x40424444,
+	0, 1, 0, 0x00000e40, 0xffffffff, 0x28303236,
+	0, 1, 0, 0x00000e44, 0xffffffff, 0x42422426,
+	0, 1, 1, 0x00000e48, 0xffffffff, 0x30343840,
+	0, 1, 1, 0x00000e4c, 0xffffffff, 0x22242628,
+	1, 0, 0, 0x00000c24, 0xffffffff, 0x42424444,
+	1, 0, 0, 0x00000c28, 0xffffffff, 0x30323640,
+	1, 0, 0, 0x00000c2c, 0xffffffff, 0x40424444,
+	1, 0, 0, 0x00000c30, 0xffffffff, 0x28303236,
+	1, 0, 1, 0x00000c34, 0xffffffff, 0x38404242,
+	1, 0, 1, 0x00000c38, 0xffffffff, 0x26283034,
+	1, 0, 0, 0x00000c3c, 0xffffffff, 0x40424444,
+	1, 0, 0, 0x00000c40, 0xffffffff, 0x28303236,
+	1, 0, 0, 0x00000c44, 0xffffffff, 0x42422426,
+	1, 0, 1, 0x00000c48, 0xffffffff, 0x30343840,
+	1, 0, 1, 0x00000c4c, 0xffffffff, 0x22242628,
+	1, 1, 0, 0x00000e24, 0xffffffff, 0x42424444,
+	1, 1, 0, 0x00000e28, 0xffffffff, 0x30323640,
+	1, 1, 0, 0x00000e2c, 0xffffffff, 0x40424444,
+	1, 1, 0, 0x00000e30, 0xffffffff, 0x28303236,
+	1, 1, 1, 0x00000e34, 0xffffffff, 0x38404242,
+	1, 1, 1, 0x00000e38, 0xffffffff, 0x26283034,
+	1, 1, 0, 0x00000e3c, 0xffffffff, 0x40424444,
+	1, 1, 0, 0x00000e40, 0xffffffff, 0x28303236,
+	1, 1, 0, 0x00000e44, 0xffffffff, 0x42422426,
+	1, 1, 1, 0x00000e48, 0xffffffff, 0x30343840,
+	1, 1, 1, 0x00000e4c, 0xffffffff, 0x22242628
+};
+
+u32 RTL8821AE_PHY_REG_ARRAY_PG[] = {
+	0, 0, 0, 0x00000c20, 0xffffffff, 0x32343638,
+	0, 0, 0, 0x00000c24, 0xffffffff, 0x36363838,
+	0, 0, 0, 0x00000c28, 0xffffffff, 0x28303234,
+	0, 0, 0, 0x00000c2c, 0xffffffff, 0x34363838,
+	0, 0, 0, 0x00000c30, 0xffffffff, 0x26283032,
+	0, 0, 0, 0x00000c3c, 0xffffffff, 0x32343636,
+	0, 0, 0, 0x00000c40, 0xffffffff, 0x24262830,
+	0, 0, 0, 0x00000c44, 0x0000ffff, 0x00002022,
+	1, 0, 0, 0x00000c24, 0xffffffff, 0x34343636,
+	1, 0, 0, 0x00000c28, 0xffffffff, 0x26283032,
+	1, 0, 0, 0x00000c2c, 0xffffffff, 0x32343636,
+	1, 0, 0, 0x00000c30, 0xffffffff, 0x24262830,
+	1, 0, 0, 0x00000c3c, 0xffffffff, 0x32343636,
+	1, 0, 0, 0x00000c40, 0xffffffff, 0x24262830,
+	1, 0, 0, 0x00000c44, 0x0000ffff, 0x00002022
+};
+
+/* it seems not used
+u8 *RTL8821AE_TXPWR_LMT_ARRAY[] = {
+	"FCC", "2.4G", "20M", "CCK", "1T", "01", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "01", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "01", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "02", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "02", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "02", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "03", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "03", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "03", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "04", "34",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "04", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "04", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "05", "34",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "05", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "05", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "06", "34",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "06", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "06", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "07", "34",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "07", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "07", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "08", "34",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "08", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "08", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "09", "34",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "09", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "09", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "10", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "10", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "10", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "11", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "11", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "11", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "12", "63",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "12", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "12", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "13", "63",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "13", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "13", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "14", "63",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "14", "63",
+	"MKK", "2.4G", "20M", "CCK", "1T", "14", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "01", "30",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "01", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "01", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "02", "30",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "02", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "02", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "03", "30",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "03", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "03", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "04", "32",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "04", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "04", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "05", "32",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "05", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "05", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "06", "32",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "06", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "06", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "07", "32",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "07", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "07", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "08", "32",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "08", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "08", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "09", "32",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "09", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "09", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "10", "30",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "10", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "10", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "11", "30",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "11", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "11", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "12", "63",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "12", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "12", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "13", "63",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "13", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "13", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "14", "63",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "14", "63",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "14", "63",
+	"FCC", "2.4G", "20M", "HT", "1T", "01", "26",
+	"ETSI", "2.4G", "20M", "HT", "1T", "01", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "01", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "02", "26",
+	"ETSI", "2.4G", "20M", "HT", "1T", "02", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "02", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "03", "26",
+	"ETSI", "2.4G", "20M", "HT", "1T", "03", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "03", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "04", "32",
+	"ETSI", "2.4G", "20M", "HT", "1T", "04", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "04", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "05", "32",
+	"ETSI", "2.4G", "20M", "HT", "1T", "05", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "05", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "06", "32",
+	"ETSI", "2.4G", "20M", "HT", "1T", "06", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "06", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "07", "32",
+	"ETSI", "2.4G", "20M", "HT", "1T", "07", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "07", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "08", "32",
+	"ETSI", "2.4G", "20M", "HT", "1T", "08", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "08", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "09", "32",
+	"ETSI", "2.4G", "20M", "HT", "1T", "09", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "09", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "10", "26",
+	"ETSI", "2.4G", "20M", "HT", "1T", "10", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "10", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "11", "26",
+	"ETSI", "2.4G", "20M", "HT", "1T", "11", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "11", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "12", "63",
+	"ETSI", "2.4G", "20M", "HT", "1T", "12", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "12", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "13", "63",
+	"ETSI", "2.4G", "20M", "HT", "1T", "13", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "13", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "14", "63",
+	"ETSI", "2.4G", "20M", "HT", "1T", "14", "63",
+	"MKK", "2.4G", "20M", "HT", "1T", "14", "63",
+	"FCC", "2.4G", "20M", "HT", "2T", "01", "30",
+	"ETSI", "2.4G", "20M", "HT", "2T", "01", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "01", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "02", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "02", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "02", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "03", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "03", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "03", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "04", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "04", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "04", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "05", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "05", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "05", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "06", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "06", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "06", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "07", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "07", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "07", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "08", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "08", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "08", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "09", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "09", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "09", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "10", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "10", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "10", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "11", "30",
+	"ETSI", "2.4G", "20M", "HT", "2T", "11", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "11", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "12", "63",
+	"ETSI", "2.4G", "20M", "HT", "2T", "12", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "12", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "13", "63",
+	"ETSI", "2.4G", "20M", "HT", "2T", "13", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "13", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "14", "63",
+	"ETSI", "2.4G", "20M", "HT", "2T", "14", "63",
+	"MKK", "2.4G", "20M", "HT", "2T", "14", "63",
+	"FCC", "2.4G", "40M", "HT", "1T", "01", "63",
+	"ETSI", "2.4G", "40M", "HT", "1T", "01", "63",
+	"MKK", "2.4G", "40M", "HT", "1T", "01", "63",
+	"FCC", "2.4G", "40M", "HT", "1T", "02", "63",
+	"ETSI", "2.4G", "40M", "HT", "1T", "02", "63",
+	"MKK", "2.4G", "40M", "HT", "1T", "02", "63",
+	"FCC", "2.4G", "40M", "HT", "1T", "03", "26",
+	"ETSI", "2.4G", "40M", "HT", "1T", "03", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "03", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "04", "26",
+	"ETSI", "2.4G", "40M", "HT", "1T", "04", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "04", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "05", "32",
+	"ETSI", "2.4G", "40M", "HT", "1T", "05", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "05", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "06", "32",
+	"ETSI", "2.4G", "40M", "HT", "1T", "06", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "06", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "07", "32",
+	"ETSI", "2.4G", "40M", "HT", "1T", "07", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "07", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "08", "26",
+	"ETSI", "2.4G", "40M", "HT", "1T", "08", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "08", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "09", "26",
+	"ETSI", "2.4G", "40M", "HT", "1T", "09", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "09", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "10", "26",
+	"ETSI", "2.4G", "40M", "HT", "1T", "10", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "10", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "11", "26",
+	"ETSI", "2.4G", "40M", "HT", "1T", "11", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "11", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "12", "63",
+	"ETSI", "2.4G", "40M", "HT", "1T", "12", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "12", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "13", "63",
+	"ETSI", "2.4G", "40M", "HT", "1T", "13", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "13", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "14", "63",
+	"ETSI", "2.4G", "40M", "HT", "1T", "14", "63",
+	"MKK", "2.4G", "40M", "HT", "1T", "14", "63",
+	"FCC", "2.4G", "40M", "HT", "2T", "01", "63",
+	"ETSI", "2.4G", "40M", "HT", "2T", "01", "63",
+	"MKK", "2.4G", "40M", "HT", "2T", "01", "63",
+	"FCC", "2.4G", "40M", "HT", "2T", "02", "63",
+	"ETSI", "2.4G", "40M", "HT", "2T", "02", "63",
+	"MKK", "2.4G", "40M", "HT", "2T", "02", "63",
+	"FCC", "2.4G", "40M", "HT", "2T", "03", "30",
+	"ETSI", "2.4G", "40M", "HT", "2T", "03", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "03", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "04", "32",
+	"ETSI", "2.4G", "40M", "HT", "2T", "04", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "04", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "05", "32",
+	"ETSI", "2.4G", "40M", "HT", "2T", "05", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "05", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "06", "32",
+	"ETSI", "2.4G", "40M", "HT", "2T", "06", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "06", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "07", "32",
+	"ETSI", "2.4G", "40M", "HT", "2T", "07", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "07", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "08", "32",
+	"ETSI", "2.4G", "40M", "HT", "2T", "08", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "08", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "09", "32",
+	"ETSI", "2.4G", "40M", "HT", "2T", "09", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "09", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "10", "32",
+	"ETSI", "2.4G", "40M", "HT", "2T", "10", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "10", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "11", "30",
+	"ETSI", "2.4G", "40M", "HT", "2T", "11", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "11", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "12", "63",
+	"ETSI", "2.4G", "40M", "HT", "2T", "12", "32",
+	"MKK", "2.4G", "40M", "HT", "2T", "12", "32",
+	"FCC", "2.4G", "40M", "HT", "2T", "13", "63",
+	"ETSI", "2.4G", "40M", "HT", "2T", "13", "32",
+	"MKK", "2.4G", "40M", "HT", "2T", "13", "32",
+	"FCC", "2.4G", "40M", "HT", "2T", "14", "63",
+	"ETSI", "2.4G", "40M", "HT", "2T", "14", "63",
+	"MKK", "2.4G", "40M", "HT", "2T", "14", "63",
+	"FCC", "5G", "20M", "OFDM", "1T", "36", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "36", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "36", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "40", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "40", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "40", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "44", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "44", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "44", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "48", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "48", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "48", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "52", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "52", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "52", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "56", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "56", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "56", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "60", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "60", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "60", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "64", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "64", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "64", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "100", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "100", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "100", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "114", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "114", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "114", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "108", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "108", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "108", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "112", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "112", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "112", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "116", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "116", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "116", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "120", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "120", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "120", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "124", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "124", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "124", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "128", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "128", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "128", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "132", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "132", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "132", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "136", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "136", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "136", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "140", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "140", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "140", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "149", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "149", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "149", "63",
+	"FCC", "5G", "20M", "OFDM", "1T", "153", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "153", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "153", "63",
+	"FCC", "5G", "20M", "OFDM", "1T", "157", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "157", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "157", "63",
+	"FCC", "5G", "20M", "OFDM", "1T", "161", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "161", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "161", "63",
+	"FCC", "5G", "20M", "OFDM", "1T", "165", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "165", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "165", "63",
+	"FCC", "5G", "20M", "HT", "1T", "36", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "36", "30",
+	"MKK", "5G", "20M", "HT", "1T", "36", "30",
+	"FCC", "5G", "20M", "HT", "1T", "40", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "40", "30",
+	"MKK", "5G", "20M", "HT", "1T", "40", "30",
+	"FCC", "5G", "20M", "HT", "1T", "44", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "44", "30",
+	"MKK", "5G", "20M", "HT", "1T", "44", "30",
+	"FCC", "5G", "20M", "HT", "1T", "48", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "48", "30",
+	"MKK", "5G", "20M", "HT", "1T", "48", "30",
+	"FCC", "5G", "20M", "HT", "1T", "52", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "52", "30",
+	"MKK", "5G", "20M", "HT", "1T", "52", "30",
+	"FCC", "5G", "20M", "HT", "1T", "56", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "56", "30",
+	"MKK", "5G", "20M", "HT", "1T", "56", "30",
+	"FCC", "5G", "20M", "HT", "1T", "60", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "60", "30",
+	"MKK", "5G", "20M", "HT", "1T", "60", "30",
+	"FCC", "5G", "20M", "HT", "1T", "64", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "64", "30",
+	"MKK", "5G", "20M", "HT", "1T", "64", "30",
+	"FCC", "5G", "20M", "HT", "1T", "100", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "100", "30",
+	"MKK", "5G", "20M", "HT", "1T", "100", "30",
+	"FCC", "5G", "20M", "HT", "1T", "114", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "114", "30",
+	"MKK", "5G", "20M", "HT", "1T", "114", "30",
+	"FCC", "5G", "20M", "HT", "1T", "108", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "108", "30",
+	"MKK", "5G", "20M", "HT", "1T", "108", "30",
+	"FCC", "5G", "20M", "HT", "1T", "112", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "112", "30",
+	"MKK", "5G", "20M", "HT", "1T", "112", "30",
+	"FCC", "5G", "20M", "HT", "1T", "116", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "116", "30",
+	"MKK", "5G", "20M", "HT", "1T", "116", "30",
+	"FCC", "5G", "20M", "HT", "1T", "120", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "120", "30",
+	"MKK", "5G", "20M", "HT", "1T", "120", "30",
+	"FCC", "5G", "20M", "HT", "1T", "124", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "124", "30",
+	"MKK", "5G", "20M", "HT", "1T", "124", "30",
+	"FCC", "5G", "20M", "HT", "1T", "128", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "128", "30",
+	"MKK", "5G", "20M", "HT", "1T", "128", "30",
+	"FCC", "5G", "20M", "HT", "1T", "132", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "132", "30",
+	"MKK", "5G", "20M", "HT", "1T", "132", "30",
+	"FCC", "5G", "20M", "HT", "1T", "136", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "136", "30",
+	"MKK", "5G", "20M", "HT", "1T", "136", "30",
+	"FCC", "5G", "20M", "HT", "1T", "140", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "140", "30",
+	"MKK", "5G", "20M", "HT", "1T", "140", "30",
+	"FCC", "5G", "20M", "HT", "1T", "149", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "149", "30",
+	"MKK", "5G", "20M", "HT", "1T", "149", "63",
+	"FCC", "5G", "20M", "HT", "1T", "153", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "153", "30",
+	"MKK", "5G", "20M", "HT", "1T", "153", "63",
+	"FCC", "5G", "20M", "HT", "1T", "157", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "157", "30",
+	"MKK", "5G", "20M", "HT", "1T", "157", "63",
+	"FCC", "5G", "20M", "HT", "1T", "161", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "161", "30",
+	"MKK", "5G", "20M", "HT", "1T", "161", "63",
+	"FCC", "5G", "20M", "HT", "1T", "165", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "165", "30",
+	"MKK", "5G", "20M", "HT", "1T", "165", "63",
+	"FCC", "5G", "20M", "HT", "2T", "36", "28",
+	"ETSI", "5G", "20M", "HT", "2T", "36", "30",
+	"MKK", "5G", "20M", "HT", "2T", "36", "30",
+	"FCC", "5G", "20M", "HT", "2T", "40", "28",
+	"ETSI", "5G", "20M", "HT", "2T", "40", "30",
+	"MKK", "5G", "20M", "HT", "2T", "40", "30",
+	"FCC", "5G", "20M", "HT", "2T", "44", "28",
+	"ETSI", "5G", "20M", "HT", "2T", "44", "30",
+	"MKK", "5G", "20M", "HT", "2T", "44", "30",
+	"FCC", "5G", "20M", "HT", "2T", "48", "28",
+	"ETSI", "5G", "20M", "HT", "2T", "48", "30",
+	"MKK", "5G", "20M", "HT", "2T", "48", "30",
+	"FCC", "5G", "20M", "HT", "2T", "52", "34",
+	"ETSI", "5G", "20M", "HT", "2T", "52", "30",
+	"MKK", "5G", "20M", "HT", "2T", "52", "30",
+	"FCC", "5G", "20M", "HT", "2T", "56", "32",
+	"ETSI", "5G", "20M", "HT", "2T", "56", "30",
+	"MKK", "5G", "20M", "HT", "2T", "56", "30",
+	"FCC", "5G", "20M", "HT", "2T", "60", "30",
+	"ETSI", "5G", "20M", "HT", "2T", "60", "30",
+	"MKK", "5G", "20M", "HT", "2T", "60", "30",
+	"FCC", "5G", "20M", "HT", "2T", "64", "26",
+	"ETSI", "5G", "20M", "HT", "2T", "64", "30",
+	"MKK", "5G", "20M", "HT", "2T", "64", "30",
+	"FCC", "5G", "20M", "HT", "2T", "100", "28",
+	"ETSI", "5G", "20M", "HT", "2T", "100", "30",
+	"MKK", "5G", "20M", "HT", "2T", "100", "30",
+	"FCC", "5G", "20M", "HT", "2T", "114", "28",
+	"ETSI", "5G", "20M", "HT", "2T", "114", "30",
+	"MKK", "5G", "20M", "HT", "2T", "114", "30",
+	"FCC", "5G", "20M", "HT", "2T", "108", "30",
+	"ETSI", "5G", "20M", "HT", "2T", "108", "30",
+	"MKK", "5G", "20M", "HT", "2T", "108", "30",
+	"FCC", "5G", "20M", "HT", "2T", "112", "32",
+	"ETSI", "5G", "20M", "HT", "2T", "112", "30",
+	"MKK", "5G", "20M", "HT", "2T", "112", "30",
+	"FCC", "5G", "20M", "HT", "2T", "116", "32",
+	"ETSI", "5G", "20M", "HT", "2T", "116", "30",
+	"MKK", "5G", "20M", "HT", "2T", "116", "30",
+	"FCC", "5G", "20M", "HT", "2T", "120", "34",
+	"ETSI", "5G", "20M", "HT", "2T", "120", "30",
+	"MKK", "5G", "20M", "HT", "2T", "120", "30",
+	"FCC", "5G", "20M", "HT", "2T", "124", "32",
+	"ETSI", "5G", "20M", "HT", "2T", "124", "30",
+	"MKK", "5G", "20M", "HT", "2T", "124", "30",
+	"FCC", "5G", "20M", "HT", "2T", "128", "30",
+	"ETSI", "5G", "20M", "HT", "2T", "128", "30",
+	"MKK", "5G", "20M", "HT", "2T", "128", "30",
+	"FCC", "5G", "20M", "HT", "2T", "132", "28",
+	"ETSI", "5G", "20M", "HT", "2T", "132", "30",
+	"MKK", "5G", "20M", "HT", "2T", "132", "30",
+	"FCC", "5G", "20M", "HT", "2T", "136", "28",
+	"ETSI", "5G", "20M", "HT", "2T", "136", "30",
+	"MKK", "5G", "20M", "HT", "2T", "136", "30",
+	"FCC", "5G", "20M", "HT", "2T", "140", "26",
+	"ETSI", "5G", "20M", "HT", "2T", "140", "30",
+	"MKK", "5G", "20M", "HT", "2T", "140", "30",
+	"FCC", "5G", "20M", "HT", "2T", "149", "34",
+	"ETSI", "5G", "20M", "HT", "2T", "149", "30",
+	"MKK", "5G", "20M", "HT", "2T", "149", "63",
+	"FCC", "5G", "20M", "HT", "2T", "153", "34",
+	"ETSI", "5G", "20M", "HT", "2T", "153", "30",
+	"MKK", "5G", "20M", "HT", "2T", "153", "63",
+	"FCC", "5G", "20M", "HT", "2T", "157", "34",
+	"ETSI", "5G", "20M", "HT", "2T", "157", "30",
+	"MKK", "5G", "20M", "HT", "2T", "157", "63",
+	"FCC", "5G", "20M", "HT", "2T", "161", "34",
+	"ETSI", "5G", "20M", "HT", "2T", "161", "30",
+	"MKK", "5G", "20M", "HT", "2T", "161", "63",
+	"FCC", "5G", "20M", "HT", "2T", "165", "34",
+	"ETSI", "5G", "20M", "HT", "2T", "165", "30",
+	"MKK", "5G", "20M", "HT", "2T", "165", "63",
+	"FCC", "5G", "40M", "HT", "1T", "38", "26",
+	"ETSI", "5G", "40M", "HT", "1T", "38", "30",
+	"MKK", "5G", "40M", "HT", "1T", "38", "30",
+	"FCC", "5G", "40M", "HT", "1T", "46", "30",
+	"ETSI", "5G", "40M", "HT", "1T", "46", "30",
+	"MKK", "5G", "40M", "HT", "1T", "46", "30",
+	"FCC", "5G", "40M", "HT", "1T", "54", "30",
+	"ETSI", "5G", "40M", "HT", "1T", "54", "30",
+	"MKK", "5G", "40M", "HT", "1T", "54", "30",
+	"FCC", "5G", "40M", "HT", "1T", "62", "26",
+	"ETSI", "5G", "40M", "HT", "1T", "62", "30",
+	"MKK", "5G", "40M", "HT", "1T", "62", "30",
+	"FCC", "5G", "40M", "HT", "1T", "102", "24",
+	"ETSI", "5G", "40M", "HT", "1T", "102", "30",
+	"MKK", "5G", "40M", "HT", "1T", "102", "30",
+	"FCC", "5G", "40M", "HT", "1T", "110", "30",
+	"ETSI", "5G", "40M", "HT", "1T", "110", "30",
+	"MKK", "5G", "40M", "HT", "1T", "110", "30",
+	"FCC", "5G", "40M", "HT", "1T", "118", "30",
+	"ETSI", "5G", "40M", "HT", "1T", "118", "30",
+	"MKK", "5G", "40M", "HT", "1T", "118", "30",
+	"FCC", "5G", "40M", "HT", "1T", "126", "30",
+	"ETSI", "5G", "40M", "HT", "1T", "126", "30",
+	"MKK", "5G", "40M", "HT", "1T", "126", "30",
+	"FCC", "5G", "40M", "HT", "1T", "134", "30",
+	"ETSI", "5G", "40M", "HT", "1T", "134", "30",
+	"MKK", "5G", "40M", "HT", "1T", "134", "30",
+	"FCC", "5G", "40M", "HT", "1T", "151", "30",
+	"ETSI", "5G", "40M", "HT", "1T", "151", "30",
+	"MKK", "5G", "40M", "HT", "1T", "151", "63",
+	"FCC", "5G", "40M", "HT", "1T", "159", "30",
+	"ETSI", "5G", "40M", "HT", "1T", "159", "30",
+	"MKK", "5G", "40M", "HT", "1T", "159", "63",
+	"FCC", "5G", "40M", "HT", "2T", "38", "28",
+	"ETSI", "5G", "40M", "HT", "2T", "38", "30",
+	"MKK", "5G", "40M", "HT", "2T", "38", "30",
+	"FCC", "5G", "40M", "HT", "2T", "46", "28",
+	"ETSI", "5G", "40M", "HT", "2T", "46", "30",
+	"MKK", "5G", "40M", "HT", "2T", "46", "30",
+	"FCC", "5G", "40M", "HT", "2T", "54", "30",
+	"ETSI", "5G", "40M", "HT", "2T", "54", "30",
+	"MKK", "5G", "40M", "HT", "2T", "54", "30",
+	"FCC", "5G", "40M", "HT", "2T", "62", "30",
+	"ETSI", "5G", "40M", "HT", "2T", "62", "30",
+	"MKK", "5G", "40M", "HT", "2T", "62", "30",
+	"FCC", "5G", "40M", "HT", "2T", "102", "26",
+	"ETSI", "5G", "40M", "HT", "2T", "102", "30",
+	"MKK", "5G", "40M", "HT", "2T", "102", "30",
+	"FCC", "5G", "40M", "HT", "2T", "110", "30",
+	"ETSI", "5G", "40M", "HT", "2T", "110", "30",
+	"MKK", "5G", "40M", "HT", "2T", "110", "30",
+	"FCC", "5G", "40M", "HT", "2T", "118", "34",
+	"ETSI", "5G", "40M", "HT", "2T", "118", "30",
+	"MKK", "5G", "40M", "HT", "2T", "118", "30",
+	"FCC", "5G", "40M", "HT", "2T", "126", "32",
+	"ETSI", "5G", "40M", "HT", "2T", "126", "30",
+	"MKK", "5G", "40M", "HT", "2T", "126", "30",
+	"FCC", "5G", "40M", "HT", "2T", "134", "30",
+	"ETSI", "5G", "40M", "HT", "2T", "134", "30",
+	"MKK", "5G", "40M", "HT", "2T", "134", "30",
+	"FCC", "5G", "40M", "HT", "2T", "151", "34",
+	"ETSI", "5G", "40M", "HT", "2T", "151", "30",
+	"MKK", "5G", "40M", "HT", "2T", "151", "63",
+	"FCC", "5G", "40M", "HT", "2T", "159", "34",
+	"ETSI", "5G", "40M", "HT", "2T", "159", "30",
+	"MKK", "5G", "40M", "HT", "2T", "159", "63",
+	"FCC", "5G", "80M", "VHT", "1T", "42", "22",
+	"ETSI", "5G", "80M", "VHT", "1T", "42", "30",
+	"MKK", "5G", "80M", "VHT", "1T", "42", "30",
+	"FCC", "5G", "80M", "VHT", "1T", "58", "20",
+	"ETSI", "5G", "80M", "VHT", "1T", "58", "30",
+	"MKK", "5G", "80M", "VHT", "1T", "58", "30",
+	"FCC", "5G", "80M", "VHT", "1T", "106", "20",
+	"ETSI", "5G", "80M", "VHT", "1T", "106", "30",
+	"MKK", "5G", "80M", "VHT", "1T", "106", "30",
+	"FCC", "5G", "80M", "VHT", "1T", "122", "28",
+	"ETSI", "5G", "80M", "VHT", "1T", "122", "30",
+	"MKK", "5G", "80M", "VHT", "1T", "122", "30",
+	"FCC", "5G", "80M", "VHT", "1T", "155", "30",
+	"ETSI", "5G", "80M", "VHT", "1T", "155", "30",
+	"MKK", "5G", "80M", "VHT", "1T", "155", "63",
+	"FCC", "5G", "80M", "VHT", "2T", "42", "28",
+	"ETSI", "5G", "80M", "VHT", "2T", "42", "30",
+	"MKK", "5G", "80M", "VHT", "2T", "42", "30",
+	"FCC", "5G", "80M", "VHT", "2T", "58", "26",
+	"ETSI", "5G", "80M", "VHT", "2T", "58", "30",
+	"MKK", "5G", "80M", "VHT", "2T", "58", "30",
+	"FCC", "5G", "80M", "VHT", "2T", "106", "28",
+	"ETSI", "5G", "80M", "VHT", "2T", "106", "30",
+	"MKK", "5G", "80M", "VHT", "2T", "106", "30",
+	"FCC", "5G", "80M", "VHT", "2T", "122", "32",
+	"ETSI", "5G", "80M", "VHT", "2T", "122", "30",
+	"MKK", "5G", "80M", "VHT", "2T", "122", "30",
+	"FCC", "5G", "80M", "VHT", "2T", "155", "34",
+	"ETSI", "5G", "80M", "VHT", "2T", "155", "30",
+	"MKK", "5G", "80M", "VHT", "2T", "155", "63"
+};*/
+
+u32 RTL8812AE_RADIOA_ARRAY[] = {
+		0x000, 0x00010000,
+		0x018, 0x0001712A,
+		0x056, 0x00051CF2,
+		0x066, 0x00040000,
+		0x01E, 0x00080000,
+		0x089, 0x00000080,
+	0xFF0F0740, 0xABCD,
+		0x086, 0x00014B38,
+	0xFF0F02C0, 0xCDEF,
+		0x086, 0x00014B38,
+	0xFF0F01C0, 0xCDEF,
+		0x086, 0x00014B38,
+	0xFF0F07D8, 0xCDEF,
+		0x086, 0x00014B3A,
+	0xFF0F07D0, 0xCDEF,
+		0x086, 0x00014B3A,
+	0xCDCDCDCD, 0xCDCD,
+		0x086, 0x00014B38,
+	0xFF0F0740, 0xDEAD,
+		0x0B1, 0x0001FC1A,
+		0x0B3, 0x000F0810,
+		0x0B4, 0x0001A78D,
+		0x0BA, 0x00086180,
+		0x018, 0x00000006,
+		0x0EF, 0x00002000,
+	0xFF0F07D8, 0xABCD,
+		0x03B, 0x0003F218,
+		0x03B, 0x00030A58,
+		0x03B, 0x0002FA58,
+		0x03B, 0x00022590,
+		0x03B, 0x0001FA50,
+		0x03B, 0x00010248,
+		0x03B, 0x00008240,
+	0xFF0F07D0, 0xCDEF,
+		0x03B, 0x0003F218,
+		0x03B, 0x00030A58,
+		0x03B, 0x0002FA58,
+		0x03B, 0x00022590,
+		0x03B, 0x0001FA50,
+		0x03B, 0x00010248,
+		0x03B, 0x00008240,
+	0xCDCDCDCD, 0xCDCD,
+		0x03B, 0x00038A58,
+		0x03B, 0x00037A58,
+		0x03B, 0x0002A590,
+		0x03B, 0x00027A50,
+		0x03B, 0x00018248,
+		0x03B, 0x00010240,
+		0x03B, 0x00008240,
+	0xFF0F07D8, 0xDEAD,
+		0x0EF, 0x00000100,
+	0xFF0F07D8, 0xABCD,
+		0x034, 0x0000A4EE,
+		0x034, 0x00009076,
+		0x034, 0x00008073,
+		0x034, 0x00007070,
+		0x034, 0x0000606D,
+		0x034, 0x0000506A,
+		0x034, 0x00004049,
+		0x034, 0x00003046,
+		0x034, 0x00002028,
+		0x034, 0x00001025,
+		0x034, 0x00000022,
+	0xCDCDCDCD, 0xCDCD,
+		0x034, 0x0000ADF4,
+		0x034, 0x00009DF1,
+		0x034, 0x00008DEE,
+		0x034, 0x00007DEB,
+		0x034, 0x00006DE8,
+		0x034, 0x00005CEC,
+		0x034, 0x00004CE9,
+		0x034, 0x000034EA,
+		0x034, 0x000024E7,
+		0x034, 0x0000146B,
+		0x034, 0x0000006D,
+	0xFF0F07D8, 0xDEAD,
+		0x0EF, 0x00000000,
+		0x0EF, 0x000020A2,
+		0x0DF, 0x00000080,
+		0x035, 0x00000192,
+		0x035, 0x00008192,
+		0x035, 0x00010192,
+		0x036, 0x00000024,
+		0x036, 0x00008024,
+		0x036, 0x00010024,
+		0x036, 0x00018024,
+		0x0EF, 0x00000000,
+		0x051, 0x00000C21,
+		0x052, 0x000006D9,
+		0x053, 0x000FC649,
+		0x054, 0x0000017E,
+		0x0EF, 0x00000002,
+		0x008, 0x00008400,
+		0x018, 0x0001712A,
+		0x0EF, 0x00001000,
+		0x03A, 0x00000080,
+		0x03B, 0x0003A02C,
+		0x03C, 0x00004000,
+		0x03A, 0x00000400,
+		0x03B, 0x0003202C,
+		0x03C, 0x00010000,
+		0x03A, 0x000000A0,
+		0x03B, 0x0002B064,
+		0x03C, 0x00004000,
+		0x03A, 0x000000D8,
+		0x03B, 0x00023070,
+		0x03C, 0x00004000,
+		0x03A, 0x00000468,
+		0x03B, 0x0001B870,
+		0x03C, 0x00010000,
+		0x03A, 0x00000098,
+		0x03B, 0x00012085,
+		0x03C, 0x000E4000,
+		0x03A, 0x00000418,
+		0x03B, 0x0000A080,
+		0x03C, 0x000F0000,
+		0x03A, 0x00000418,
+		0x03B, 0x00002080,
+		0x03C, 0x00010000,
+		0x03A, 0x00000080,
+		0x03B, 0x0007A02C,
+		0x03C, 0x00004000,
+		0x03A, 0x00000400,
+		0x03B, 0x0007202C,
+		0x03C, 0x00010000,
+		0x03A, 0x000000A0,
+		0x03B, 0x0006B064,
+		0x03C, 0x00004000,
+		0x03A, 0x000000D8,
+		0x03B, 0x00023070,
+		0x03C, 0x00004000,
+		0x03A, 0x00000468,
+		0x03B, 0x0005B870,
+		0x03C, 0x00010000,
+		0x03A, 0x00000098,
+		0x03B, 0x00052085,
+		0x03C, 0x000E4000,
+		0x03A, 0x00000418,
+		0x03B, 0x0004A080,
+		0x03C, 0x000F0000,
+		0x03A, 0x00000418,
+		0x03B, 0x00042080,
+		0x03C, 0x00010000,
+		0x03A, 0x00000080,
+		0x03B, 0x000BA02C,
+		0x03C, 0x00004000,
+		0x03A, 0x00000400,
+		0x03B, 0x000B202C,
+		0x03C, 0x00010000,
+		0x03A, 0x000000A0,
+		0x03B, 0x000AB064,
+		0x03C, 0x00004000,
+		0x03A, 0x000000D8,
+		0x03B, 0x000A3070,
+		0x03C, 0x00004000,
+		0x03A, 0x00000468,
+		0x03B, 0x0009B870,
+		0x03C, 0x00010000,
+		0x03A, 0x00000098,
+		0x03B, 0x00092085,
+		0x03C, 0x000E4000,
+		0x03A, 0x00000418,
+		0x03B, 0x0008A080,
+		0x03C, 0x000F0000,
+		0x03A, 0x00000418,
+		0x03B, 0x00082080,
+		0x03C, 0x00010000,
+		0x0EF, 0x00001100,
+	0xFF0F0740, 0xABCD,
+		0x034, 0x0004A0B2,
+		0x034, 0x000490AF,
+		0x034, 0x00048070,
+		0x034, 0x0004706D,
+		0x034, 0x00046050,
+		0x034, 0x0004504D,
+		0x034, 0x0004404A,
+		0x034, 0x00043047,
+		0x034, 0x0004200A,
+		0x034, 0x00041007,
+		0x034, 0x00040004,
+	0xFF0F02C0, 0xCDEF,
+		0x034, 0x0004A0B2,
+		0x034, 0x000490AF,
+		0x034, 0x00048070,
+		0x034, 0x0004706D,
+		0x034, 0x00046050,
+		0x034, 0x0004504D,
+		0x034, 0x0004404A,
+		0x034, 0x00043047,
+		0x034, 0x0004200A,
+		0x034, 0x00041007,
+		0x034, 0x00040004,
+	0xFF0F01C0, 0xCDEF,
+		0x034, 0x0004A0B2,
+		0x034, 0x000490AF,
+		0x034, 0x00048070,
+		0x034, 0x0004706D,
+		0x034, 0x00046050,
+		0x034, 0x0004504D,
+		0x034, 0x0004404A,
+		0x034, 0x00043047,
+		0x034, 0x0004200A,
+		0x034, 0x00041007,
+		0x034, 0x00040004,
+	0xFF0F07D8, 0xCDEF,
+		0x034, 0x0004A0B2,
+		0x034, 0x000490AF,
+		0x034, 0x00048070,
+		0x034, 0x0004706D,
+		0x034, 0x00046050,
+		0x034, 0x0004504D,
+		0x034, 0x0004404A,
+		0x034, 0x00043047,
+		0x034, 0x0004200A,
+		0x034, 0x00041007,
+		0x034, 0x00040004,
+	0xFF0F07D0, 0xCDEF,
+		0x034, 0x0004A0B2,
+		0x034, 0x000490AF,
+		0x034, 0x00048070,
+		0x034, 0x0004706D,
+		0x034, 0x00046050,
+		0x034, 0x0004504D,
+		0x034, 0x0004404A,
+		0x034, 0x00043047,
+		0x034, 0x0004200A,
+		0x034, 0x00041007,
+		0x034, 0x00040004,
+	0xCDCDCDCD, 0xCDCD,
+		0x034, 0x0004ADF5,
+		0x034, 0x00049DF2,
+		0x034, 0x00048DEF,
+		0x034, 0x00047DEC,
+		0x034, 0x00046DE9,
+		0x034, 0x00045DC9,
+		0x034, 0x00044CE8,
+		0x034, 0x000438CA,
+		0x034, 0x00042889,
+		0x034, 0x0004184A,
+		0x034, 0x0004044A,
+	0xFF0F0740, 0xDEAD,
+	0xFF0F0740, 0xABCD,
+		0x034, 0x0002A0B2,
+		0x034, 0x000290AF,
+		0x034, 0x00028070,
+		0x034, 0x0002706D,
+		0x034, 0x00026050,
+		0x034, 0x0002504D,
+		0x034, 0x0002404A,
+		0x034, 0x00023047,
+		0x034, 0x0002200A,
+		0x034, 0x00021007,
+		0x034, 0x00020004,
+	0xFF0F02C0, 0xCDEF,
+		0x034, 0x0002A0B2,
+		0x034, 0x000290AF,
+		0x034, 0x00028070,
+		0x034, 0x0002706D,
+		0x034, 0x00026050,
+		0x034, 0x0002504D,
+		0x034, 0x0002404A,
+		0x034, 0x00023047,
+		0x034, 0x0002200A,
+		0x034, 0x00021007,
+		0x034, 0x00020004,
+	0xFF0F01C0, 0xCDEF,
+		0x034, 0x0002A0B2,
+		0x034, 0x000290AF,
+		0x034, 0x00028070,
+		0x034, 0x0002706D,
+		0x034, 0x00026050,
+		0x034, 0x0002504D,
+		0x034, 0x0002404A,
+		0x034, 0x00023047,
+		0x034, 0x0002200A,
+		0x034, 0x00021007,
+		0x034, 0x00020004,
+	0xFF0F07D8, 0xCDEF,
+		0x034, 0x0002A0B2,
+		0x034, 0x000290AF,
+		0x034, 0x00028070,
+		0x034, 0x0002706D,
+		0x034, 0x00026050,
+		0x034, 0x0002504D,
+		0x034, 0x0002404A,
+		0x034, 0x00023047,
+		0x034, 0x0002200A,
+		0x034, 0x00021007,
+		0x034, 0x00020004,
+	0xFF0F07D0, 0xCDEF,
+		0x034, 0x0002A0B2,
+		0x034, 0x000290AF,
+		0x034, 0x00028070,
+		0x034, 0x0002706D,
+		0x034, 0x00026050,
+		0x034, 0x0002504D,
+		0x034, 0x0002404A,
+		0x034, 0x00023047,
+		0x034, 0x0002200A,
+		0x034, 0x00021007,
+		0x034, 0x00020004,
+	0xCDCDCDCD, 0xCDCD,
+		0x034, 0x0002ADF5,
+		0x034, 0x00029DF2,
+		0x034, 0x00028DEF,
+		0x034, 0x00027DEC,
+		0x034, 0x00026DE9,
+		0x034, 0x00025DC9,
+		0x034, 0x00024CE8,
+		0x034, 0x000238CA,
+		0x034, 0x00022889,
+		0x034, 0x0002184A,
+		0x034, 0x0002044A,
+	0xFF0F0740, 0xDEAD,
+	0xFF0F0740, 0xABCD,
+		0x034, 0x0000A0B2,
+		0x034, 0x000090AF,
+		0x034, 0x00008070,
+		0x034, 0x0000706D,
+		0x034, 0x00006050,
+		0x034, 0x0000504D,
+		0x034, 0x0000404A,
+		0x034, 0x00003047,
+		0x034, 0x0000200A,
+		0x034, 0x00001007,
+		0x034, 0x00000004,
+	0xFF0F02C0, 0xCDEF,
+		0x034, 0x0000A0B2,
+		0x034, 0x000090AF,
+		0x034, 0x00008070,
+		0x034, 0x0000706D,
+		0x034, 0x00006050,
+		0x034, 0x0000504D,
+		0x034, 0x0000404A,
+		0x034, 0x00003047,
+		0x034, 0x0000200A,
+		0x034, 0x00001007,
+		0x034, 0x00000004,
+	0xFF0F01C0, 0xCDEF,
+		0x034, 0x0000A0B2,
+		0x034, 0x000090AF,
+		0x034, 0x00008070,
+		0x034, 0x0000706D,
+		0x034, 0x00006050,
+		0x034, 0x0000504D,
+		0x034, 0x0000404A,
+		0x034, 0x00003047,
+		0x034, 0x0000200A,
+		0x034, 0x00001007,
+		0x034, 0x00000004,
+	0xFF0F07D8, 0xCDEF,
+		0x034, 0x0000A0B2,
+		0x034, 0x000090AF,
+		0x034, 0x00008070,
+		0x034, 0x0000706D,
+		0x034, 0x00006050,
+		0x034, 0x0000504D,
+		0x034, 0x0000404A,
+		0x034, 0x00003047,
+		0x034, 0x0000200A,
+		0x034, 0x00001007,
+		0x034, 0x00000004,
+	0xFF0F07D0, 0xCDEF,
+		0x034, 0x0000A0B2,
+		0x034, 0x000090AF,
+		0x034, 0x00008070,
+		0x034, 0x0000706D,
+		0x034, 0x00006050,
+		0x034, 0x0000504D,
+		0x034, 0x0000404A,
+		0x034, 0x00003047,
+		0x034, 0x0000200A,
+		0x034, 0x00001007,
+		0x034, 0x00000004,
+	0xCDCDCDCD, 0xCDCD,
+		0x034, 0x0000AFF7,
+		0x034, 0x00009DF7,
+		0x034, 0x00008DF4,
+		0x034, 0x00007DF1,
+		0x034, 0x00006DEE,
+		0x034, 0x00005DCD,
+		0x034, 0x00004CEB,
+		0x034, 0x000038CC,
+		0x034, 0x0000288B,
+		0x034, 0x0000184C,
+		0x034, 0x0000044C,
+	0xFF0F0740, 0xDEAD,
+		0x0EF, 0x00000000,
+	0xFF0F0740, 0xABCD,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000040,
+		0x035, 0x000001D4,
+		0x035, 0x000081D4,
+		0x035, 0x000101D4,
+		0x035, 0x000201B4,
+		0x035, 0x000281B4,
+		0x035, 0x000301B4,
+		0x035, 0x000401B4,
+		0x035, 0x000481B4,
+		0x035, 0x000501B4,
+	0xFF0F02C0, 0xCDEF,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000040,
+		0x035, 0x000001D4,
+		0x035, 0x000081D4,
+		0x035, 0x000101D4,
+		0x035, 0x000201B4,
+		0x035, 0x000281B4,
+		0x035, 0x000301B4,
+		0x035, 0x000401B4,
+		0x035, 0x000481B4,
+		0x035, 0x000501B4,
+	0xFF0F01C0, 0xCDEF,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000040,
+		0x035, 0x000001D4,
+		0x035, 0x000081D4,
+		0x035, 0x000101D4,
+		0x035, 0x000201B4,
+		0x035, 0x000281B4,
+		0x035, 0x000301B4,
+		0x035, 0x000401B4,
+		0x035, 0x000481B4,
+		0x035, 0x000501B4,
+	0xFF0F07D8, 0xCDEF,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000040,
+		0x035, 0x000001D4,
+		0x035, 0x000081D4,
+		0x035, 0x000101D4,
+		0x035, 0x000201B4,
+		0x035, 0x000281B4,
+		0x035, 0x000301B4,
+		0x035, 0x000401B4,
+		0x035, 0x000481B4,
+		0x035, 0x000501B4,
+	0xFF0F07D0, 0xCDEF,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000040,
+		0x035, 0x000001D4,
+		0x035, 0x000081D4,
+		0x035, 0x000101D4,
+		0x035, 0x000201B4,
+		0x035, 0x000281B4,
+		0x035, 0x000301B4,
+		0x035, 0x000401B4,
+		0x035, 0x000481B4,
+		0x035, 0x000501B4,
+	0xCDCDCDCD, 0xCDCD,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000040,
+		0x035, 0x00000188,
+		0x035, 0x00008147,
+		0x035, 0x00010147,
+		0x035, 0x000201D7,
+		0x035, 0x000281D7,
+		0x035, 0x000301D7,
+		0x035, 0x000401D8,
+		0x035, 0x000481D8,
+		0x035, 0x000501D8,
+	0xFF0F0740, 0xDEAD,
+		0x0EF, 0x00000000,
+	0xFF0F0740, 0xABCD,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000010,
+		0x036, 0x00004BFB,
+		0x036, 0x0000CBFB,
+		0x036, 0x00014BFB,
+		0x036, 0x0001CBFB,
+		0x036, 0x00024F4B,
+		0x036, 0x0002CF4B,
+		0x036, 0x00034F4B,
+		0x036, 0x0003CF4B,
+		0x036, 0x00044F4B,
+		0x036, 0x0004CF4B,
+		0x036, 0x00054F4B,
+		0x036, 0x0005CF4B,
+	0xFF0F02C0, 0xCDEF,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000010,
+		0x036, 0x00004BFB,
+		0x036, 0x0000CBFB,
+		0x036, 0x00014BFB,
+		0x036, 0x0001CBFB,
+		0x036, 0x00024F4B,
+		0x036, 0x0002CF4B,
+		0x036, 0x00034F4B,
+		0x036, 0x0003CF4B,
+		0x036, 0x00044F4B,
+		0x036, 0x0004CF4B,
+		0x036, 0x00054F4B,
+		0x036, 0x0005CF4B,
+	0xFF0F01C0, 0xCDEF,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000010,
+		0x036, 0x00004BFB,
+		0x036, 0x0000CBFB,
+		0x036, 0x00014BFB,
+		0x036, 0x0001CBFB,
+		0x036, 0x00024F4B,
+		0x036, 0x0002CF4B,
+		0x036, 0x00034F4B,
+		0x036, 0x0003CF4B,
+		0x036, 0x00044F4B,
+		0x036, 0x0004CF4B,
+		0x036, 0x00054F4B,
+		0x036, 0x0005CF4B,
+	0xFF0F07D8, 0xCDEF,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000010,
+		0x036, 0x00004BFB,
+		0x036, 0x0000CBFB,
+		0x036, 0x00014BFB,
+		0x036, 0x0001CBFB,
+		0x036, 0x00024F4B,
+		0x036, 0x0002CF4B,
+		0x036, 0x00034F4B,
+		0x036, 0x0003CF4B,
+		0x036, 0x00044F4B,
+		0x036, 0x0004CF4B,
+		0x036, 0x00054F4B,
+		0x036, 0x0005CF4B,
+	0xFF0F07D0, 0xCDEF,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000010,
+		0x036, 0x00004BFB,
+		0x036, 0x0000CBFB,
+		0x036, 0x00014BFB,
+		0x036, 0x0001CBFB,
+		0x036, 0x00024F4B,
+		0x036, 0x0002CF4B,
+		0x036, 0x00034F4B,
+		0x036, 0x0003CF4B,
+		0x036, 0x00044F4B,
+		0x036, 0x0004CF4B,
+		0x036, 0x00054F4B,
+		0x036, 0x0005CF4B,
+	0xCDCDCDCD, 0xCDCD,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000010,
+		0x036, 0x00084EB4,
+		0x036, 0x0008CC35,
+		0x036, 0x00094C35,
+		0x036, 0x0009CC35,
+		0x036, 0x000A4935,
+		0x036, 0x000ACC35,
+		0x036, 0x000B4C35,
+		0x036, 0x000BCC35,
+		0x036, 0x000C4EB4,
+		0x036, 0x000CCEB5,
+		0x036, 0x000D4EB5,
+		0x036, 0x000DCEB5,
+	0xFF0F0740, 0xDEAD,
+		0x0EF, 0x00000000,
+		0x0EF, 0x00000008,
+	0xFF0F0740, 0xABCD,
+		0x03C, 0x000002CC,
+		0x03C, 0x00000522,
+		0x03C, 0x00000902,
+	0xFF0F02C0, 0xCDEF,
+		0x03C, 0x000002CC,
+		0x03C, 0x00000522,
+		0x03C, 0x00000902,
+	0xFF0F01C0, 0xCDEF,
+		0x03C, 0x000002CC,
+		0x03C, 0x00000522,
+		0x03C, 0x00000902,
+	0xFF0F07D8, 0xCDEF,
+		0x03C, 0x000002CC,
+		0x03C, 0x00000522,
+		0x03C, 0x00000902,
+	0xFF0F07D0, 0xCDEF,
+		0x03C, 0x000002CC,
+		0x03C, 0x00000522,
+		0x03C, 0x00000902,
+	0xCDCDCDCD, 0xCDCD,
+		0x03C, 0x000002A8,
+		0x03C, 0x000005A2,
+		0x03C, 0x00000880,
+	0xFF0F0740, 0xDEAD,
+		0x0EF, 0x00000000,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000002,
+		0x0DF, 0x00000080,
+		0x01F, 0x00040064,
+	0xFF0F0740, 0xABCD,
+		0x061, 0x000FDD43,
+		0x062, 0x00038F4B,
+		0x063, 0x00032117,
+		0x064, 0x000194AC,
+		0x065, 0x000931D1,
+	0xFF0F02C0, 0xCDEF,
+		0x061, 0x000FDD43,
+		0x062, 0x00038F4B,
+		0x063, 0x00032117,
+		0x064, 0x000194AC,
+		0x065, 0x000931D1,
+	0xFF0F01C0, 0xCDEF,
+		0x061, 0x000FDD43,
+		0x062, 0x00038F4B,
+		0x063, 0x00032117,
+		0x064, 0x000194AC,
+		0x065, 0x000931D1,
+	0xFF0F07D8, 0xCDEF,
+		0x061, 0x000FDD43,
+		0x062, 0x00038F4B,
+		0x063, 0x00032117,
+		0x064, 0x000194AC,
+		0x065, 0x000931D1,
+	0xFF0F07D0, 0xCDEF,
+		0x061, 0x000FDD43,
+		0x062, 0x00038F4B,
+		0x063, 0x00032117,
+		0x064, 0x000194AC,
+		0x065, 0x000931D1,
+	0xCDCDCDCD, 0xCDCD,
+		0x061, 0x000E5D53,
+		0x062, 0x00038FCD,
+		0x063, 0x000314EB,
+		0x064, 0x000196AC,
+		0x065, 0x000911D7,
+	0xFF0F0740, 0xDEAD,
+		0x008, 0x00008400,
+		0x01C, 0x000739D2,
+		0x0B4, 0x0001E78D,
+		0x018, 0x0001F12A,
+		0x0FE, 0x00000000,
+		0x0FE, 0x00000000,
+		0x0FE, 0x00000000,
+		0x0FE, 0x00000000,
+		0x0B4, 0x0001A78D,
+		0x018, 0x0001712A,
+};
+
+u32 RTL8812AE_RADIOB_ARRAY[] = {
+		0x056, 0x00051CF2,
+		0x066, 0x00040000,
+		0x089, 0x00000080,
+	0xFF0F0740, 0xABCD,
+		0x086, 0x00014B38,
+	0xFF0F01C0, 0xCDEF,
+		0x086, 0x00014B38,
+	0xFF0F02C0, 0xCDEF,
+		0x086, 0x00014B38,
+	0xFF0F07D8, 0xCDEF,
+		0x086, 0x00014B3A,
+	0xFF0F07D0, 0xCDEF,
+		0x086, 0x00014B3A,
+	0xCDCDCDCD, 0xCDCD,
+		0x086, 0x00014B38,
+	0xFF0F0740, 0xDEAD,
+		0x018, 0x00000006,
+		0x0EF, 0x00002000,
+	0xFF0F07D8, 0xABCD,
+		0x03B, 0x0003F218,
+		0x03B, 0x00030A58,
+		0x03B, 0x0002FA58,
+		0x03B, 0x00022590,
+		0x03B, 0x0001FA50,
+		0x03B, 0x00010248,
+		0x03B, 0x00008240,
+	0xFF0F07D0, 0xCDEF,
+		0x03B, 0x0003F218,
+		0x03B, 0x00030A58,
+		0x03B, 0x0002FA58,
+		0x03B, 0x00022590,
+		0x03B, 0x0001FA50,
+		0x03B, 0x00010248,
+		0x03B, 0x00008240,
+	0xCDCDCDCD, 0xCDCD,
+		0x03B, 0x00038A58,
+		0x03B, 0x00037A58,
+		0x03B, 0x0002A590,
+		0x03B, 0x00027A50,
+		0x03B, 0x00018248,
+		0x03B, 0x00010240,
+		0x03B, 0x00008240,
+	0xFF0F07D8, 0xDEAD,
+		0x0EF, 0x00000100,
+	0xFF0F07D8, 0xABCD,
+		0x034, 0x0000A4EE,
+		0x034, 0x00009076,
+		0x034, 0x00008073,
+		0x034, 0x00007070,
+		0x034, 0x0000606D,
+		0x034, 0x0000506A,
+		0x034, 0x00004049,
+		0x034, 0x00003046,
+		0x034, 0x00002028,
+		0x034, 0x00001025,
+		0x034, 0x00000022,
+	0xCDCDCDCD, 0xCDCD,
+		0x034, 0x0000ADF4,
+		0x034, 0x00009DF1,
+		0x034, 0x00008DEE,
+		0x034, 0x00007DEB,
+		0x034, 0x00006DE8,
+		0x034, 0x00005CEC,
+		0x034, 0x00004CE9,
+		0x034, 0x000034EA,
+		0x034, 0x000024E7,
+		0x034, 0x0000146B,
+		0x034, 0x0000006D,
+	0xFF0F07D8, 0xDEAD,
+		0x0EF, 0x00000000,
+		0x0EF, 0x000020A2,
+		0x0DF, 0x00000080,
+		0x035, 0x00000192,
+		0x035, 0x00008192,
+		0x035, 0x00010192,
+		0x036, 0x00000024,
+		0x036, 0x00008024,
+		0x036, 0x00010024,
+		0x036, 0x00018024,
+		0x0EF, 0x00000000,
+		0x051, 0x00000C21,
+		0x052, 0x000006D9,
+		0x053, 0x000FC649,
+		0x054, 0x0000017E,
+		0x0EF, 0x00000002,
+		0x008, 0x00008400,
+		0x018, 0x0001712A,
+		0x0EF, 0x00001000,
+		0x03A, 0x00000080,
+		0x03B, 0x0003A02C,
+		0x03C, 0x00004000,
+		0x03A, 0x00000400,
+		0x03B, 0x0003202C,
+		0x03C, 0x00010000,
+		0x03A, 0x000000A0,
+		0x03B, 0x0002B064,
+		0x03C, 0x00004000,
+		0x03A, 0x000000D8,
+		0x03B, 0x00023070,
+		0x03C, 0x00004000,
+		0x03A, 0x00000468,
+		0x03B, 0x0001B870,
+		0x03C, 0x00010000,
+		0x03A, 0x00000098,
+		0x03B, 0x00012085,
+		0x03C, 0x000E4000,
+		0x03A, 0x00000418,
+		0x03B, 0x0000A080,
+		0x03C, 0x000F0000,
+		0x03A, 0x00000418,
+		0x03B, 0x00002080,
+		0x03C, 0x00010000,
+		0x03A, 0x00000080,
+		0x03B, 0x0007A02C,
+		0x03C, 0x00004000,
+		0x03A, 0x00000400,
+		0x03B, 0x0007202C,
+		0x03C, 0x00010000,
+		0x03A, 0x000000A0,
+		0x03B, 0x0006B064,
+		0x03C, 0x00004000,
+		0x03A, 0x000000D8,
+		0x03B, 0x00063070,
+		0x03C, 0x00004000,
+		0x03A, 0x00000468,
+		0x03B, 0x0005B870,
+		0x03C, 0x00010000,
+		0x03A, 0x00000098,
+		0x03B, 0x00052085,
+		0x03C, 0x000E4000,
+		0x03A, 0x00000418,
+		0x03B, 0x0004A080,
+		0x03C, 0x000F0000,
+		0x03A, 0x00000418,
+		0x03B, 0x00042080,
+		0x03C, 0x00010000,
+		0x03A, 0x00000080,
+		0x03B, 0x000BA02C,
+		0x03C, 0x00004000,
+		0x03A, 0x00000400,
+		0x03B, 0x000B202C,
+		0x03C, 0x00010000,
+		0x03A, 0x000000A0,
+		0x03B, 0x000AB064,
+		0x03C, 0x00004000,
+		0x03A, 0x000000D8,
+		0x03B, 0x000A3070,
+		0x03C, 0x00004000,
+		0x03A, 0x00000468,
+		0x03B, 0x0009B870,
+		0x03C, 0x00010000,
+		0x03A, 0x00000098,
+		0x03B, 0x00092085,
+		0x03C, 0x000E4000,
+		0x03A, 0x00000418,
+		0x03B, 0x0008A080,
+		0x03C, 0x000F0000,
+		0x03A, 0x00000418,
+		0x03B, 0x00082080,
+		0x03C, 0x00010000,
+		0x0EF, 0x00001100,
+	0xFF0F0740, 0xABCD,
+		0x034, 0x0004A0B2,
+		0x034, 0x000490AF,
+		0x034, 0x00048070,
+		0x034, 0x0004706D,
+		0x034, 0x00046050,
+		0x034, 0x0004504D,
+		0x034, 0x0004404A,
+		0x034, 0x00043047,
+		0x034, 0x0004200A,
+		0x034, 0x00041007,
+		0x034, 0x00040004,
+	0xFF0F01C0, 0xCDEF,
+		0x034, 0x0004A0B2,
+		0x034, 0x000490AF,
+		0x034, 0x00048070,
+		0x034, 0x0004706D,
+		0x034, 0x00046050,
+		0x034, 0x0004504D,
+		0x034, 0x0004404A,
+		0x034, 0x00043047,
+		0x034, 0x0004200A,
+		0x034, 0x00041007,
+		0x034, 0x00040004,
+	0xFF0F02C0, 0xCDEF,
+		0x034, 0x0004A0B2,
+		0x034, 0x000490AF,
+		0x034, 0x00048070,
+		0x034, 0x0004706D,
+		0x034, 0x00046050,
+		0x034, 0x0004504D,
+		0x034, 0x0004404A,
+		0x034, 0x00043047,
+		0x034, 0x0004200A,
+		0x034, 0x00041007,
+		0x034, 0x00040004,
+	0xFF0F07D8, 0xCDEF,
+		0x034, 0x0004A0B2,
+		0x034, 0x000490AF,
+		0x034, 0x00048070,
+		0x034, 0x0004706D,
+		0x034, 0x00046050,
+		0x034, 0x0004504D,
+		0x034, 0x0004404A,
+		0x034, 0x00043047,
+		0x034, 0x0004200A,
+		0x034, 0x00041007,
+		0x034, 0x00040004,
+	0xFF0F07D0, 0xCDEF,
+		0x034, 0x0004A0B2,
+		0x034, 0x000490AF,
+		0x034, 0x00048070,
+		0x034, 0x0004706D,
+		0x034, 0x00046050,
+		0x034, 0x0004504D,
+		0x034, 0x0004404A,
+		0x034, 0x00043047,
+		0x034, 0x0004200A,
+		0x034, 0x00041007,
+		0x034, 0x00040004,
+	0xCDCDCDCD, 0xCDCD,
+		0x034, 0x0004ADF5,
+		0x034, 0x00049DF2,
+		0x034, 0x00048DEF,
+		0x034, 0x00047DEC,
+		0x034, 0x00046DE9,
+		0x034, 0x00045DC9,
+		0x034, 0x00044CE8,
+		0x034, 0x000438CA,
+		0x034, 0x00042889,
+		0x034, 0x0004184A,
+		0x034, 0x0004044A,
+	0xFF0F0740, 0xDEAD,
+	0xFF0F0740, 0xABCD,
+		0x034, 0x0002A0B2,
+		0x034, 0x000290AF,
+		0x034, 0x00028070,
+		0x034, 0x0002706D,
+		0x034, 0x00026050,
+		0x034, 0x0002504D,
+		0x034, 0x0002404A,
+		0x034, 0x00023047,
+		0x034, 0x0002200A,
+		0x034, 0x00021007,
+		0x034, 0x00020004,
+	0xFF0F01C0, 0xCDEF,
+		0x034, 0x0002A0B2,
+		0x034, 0x000290AF,
+		0x034, 0x00028070,
+		0x034, 0x0002706D,
+		0x034, 0x00026050,
+		0x034, 0x0002504D,
+		0x034, 0x0002404A,
+		0x034, 0x00023047,
+		0x034, 0x0002200A,
+		0x034, 0x00021007,
+		0x034, 0x00020004,
+	0xFF0F02C0, 0xCDEF,
+		0x034, 0x0002A0B2,
+		0x034, 0x000290AF,
+		0x034, 0x00028070,
+		0x034, 0x0002706D,
+		0x034, 0x00026050,
+		0x034, 0x0002504D,
+		0x034, 0x0002404A,
+		0x034, 0x00023047,
+		0x034, 0x0002200A,
+		0x034, 0x00021007,
+		0x034, 0x00020004,
+	0xFF0F07D8, 0xCDEF,
+		0x034, 0x0002A0B2,
+		0x034, 0x000290AF,
+		0x034, 0x00028070,
+		0x034, 0x0002706D,
+		0x034, 0x00026050,
+		0x034, 0x0002504D,
+		0x034, 0x0002404A,
+		0x034, 0x00023047,
+		0x034, 0x0002200A,
+		0x034, 0x00021007,
+		0x034, 0x00020004,
+	0xFF0F07D0, 0xCDEF,
+		0x034, 0x0002A0B2,
+		0x034, 0x000290AF,
+		0x034, 0x00028070,
+		0x034, 0x0002706D,
+		0x034, 0x00026050,
+		0x034, 0x0002504D,
+		0x034, 0x0002404A,
+		0x034, 0x00023047,
+		0x034, 0x0002200A,
+		0x034, 0x00021007,
+		0x034, 0x00020004,
+	0xCDCDCDCD, 0xCDCD,
+		0x034, 0x0002ADF5,
+		0x034, 0x00029DF2,
+		0x034, 0x00028DEF,
+		0x034, 0x00027DEC,
+		0x034, 0x00026DE9,
+		0x034, 0x00025DC9,
+		0x034, 0x00024CE8,
+		0x034, 0x000238CA,
+		0x034, 0x00022889,
+		0x034, 0x0002184A,
+		0x034, 0x0002044A,
+	0xFF0F0740, 0xDEAD,
+	0xFF0F0740, 0xABCD,
+		0x034, 0x0000A0B2,
+		0x034, 0x000090AF,
+		0x034, 0x00008070,
+		0x034, 0x0000706D,
+		0x034, 0x00006050,
+		0x034, 0x0000504D,
+		0x034, 0x0000404A,
+		0x034, 0x00003047,
+		0x034, 0x0000200A,
+		0x034, 0x00001007,
+		0x034, 0x00000004,
+	0xFF0F01C0, 0xCDEF,
+		0x034, 0x0000A0B2,
+		0x034, 0x000090AF,
+		0x034, 0x00008070,
+		0x034, 0x0000706D,
+		0x034, 0x00006050,
+		0x034, 0x0000504D,
+		0x034, 0x0000404A,
+		0x034, 0x00003047,
+		0x034, 0x0000200A,
+		0x034, 0x00001007,
+		0x034, 0x00000004,
+	0xFF0F02C0, 0xCDEF,
+		0x034, 0x0000A0B2,
+		0x034, 0x000090AF,
+		0x034, 0x00008070,
+		0x034, 0x0000706D,
+		0x034, 0x00006050,
+		0x034, 0x0000504D,
+		0x034, 0x0000404A,
+		0x034, 0x00003047,
+		0x034, 0x0000200A,
+		0x034, 0x00001007,
+		0x034, 0x00000004,
+	0xFF0F07D8, 0xCDEF,
+		0x034, 0x0000A0B2,
+		0x034, 0x000090AF,
+		0x034, 0x00008070,
+		0x034, 0x0000706D,
+		0x034, 0x00006050,
+		0x034, 0x0000504D,
+		0x034, 0x0000404A,
+		0x034, 0x00003047,
+		0x034, 0x0000200A,
+		0x034, 0x00001007,
+		0x034, 0x00000004,
+	0xFF0F07D0, 0xCDEF,
+		0x034, 0x0000A0B2,
+		0x034, 0x000090AF,
+		0x034, 0x00008070,
+		0x034, 0x0000706D,
+		0x034, 0x00006050,
+		0x034, 0x0000504D,
+		0x034, 0x0000404A,
+		0x034, 0x00003047,
+		0x034, 0x0000200A,
+		0x034, 0x00001007,
+		0x034, 0x00000004,
+	0xCDCDCDCD, 0xCDCD,
+		0x034, 0x0000AFF7,
+		0x034, 0x00009DF7,
+		0x034, 0x00008DF4,
+		0x034, 0x00007DF1,
+		0x034, 0x00006DEE,
+		0x034, 0x00005DCD,
+		0x034, 0x00004CEB,
+		0x034, 0x000038CC,
+		0x034, 0x0000288B,
+		0x034, 0x0000184C,
+		0x034, 0x0000044C,
+	0xFF0F0740, 0xDEAD,
+		0x0EF, 0x00000000,
+	0xFF0F0740, 0xABCD,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000040,
+		0x035, 0x000001C5,
+		0x035, 0x000081C5,
+		0x035, 0x000101C5,
+		0x035, 0x00020174,
+		0x035, 0x00028174,
+		0x035, 0x00030174,
+		0x035, 0x00040185,
+		0x035, 0x00048185,
+		0x035, 0x00050185,
+		0x0EF, 0x00000000,
+	0xFF0F01C0, 0xCDEF,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000040,
+		0x035, 0x000001C5,
+		0x035, 0x000081C5,
+		0x035, 0x000101C5,
+		0x035, 0x00020174,
+		0x035, 0x00028174,
+		0x035, 0x00030174,
+		0x035, 0x00040185,
+		0x035, 0x00048185,
+		0x035, 0x00050185,
+		0x0EF, 0x00000000,
+	0xFF0F02C0, 0xCDEF,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000040,
+		0x035, 0x000001C5,
+		0x035, 0x000081C5,
+		0x035, 0x000101C5,
+		0x035, 0x00020174,
+		0x035, 0x00028174,
+		0x035, 0x00030174,
+		0x035, 0x00040185,
+		0x035, 0x00048185,
+		0x035, 0x00050185,
+		0x0EF, 0x00000000,
+	0xFF0F07D8, 0xCDEF,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000040,
+		0x035, 0x000001C5,
+		0x035, 0x000081C5,
+		0x035, 0x000101C5,
+		0x035, 0x00020174,
+		0x035, 0x00028174,
+		0x035, 0x00030174,
+		0x035, 0x00040185,
+		0x035, 0x00048185,
+		0x035, 0x00050185,
+		0x0EF, 0x00000000,
+	0xFF0F07D0, 0xCDEF,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000040,
+		0x035, 0x000001C5,
+		0x035, 0x000081C5,
+		0x035, 0x000101C5,
+		0x035, 0x00020174,
+		0x035, 0x00028174,
+		0x035, 0x00030174,
+		0x035, 0x00040185,
+		0x035, 0x00048185,
+		0x035, 0x00050185,
+		0x0EF, 0x00000000,
+	0xCDCDCDCD, 0xCDCD,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000040,
+		0x035, 0x00000186,
+		0x035, 0x00008186,
+		0x035, 0x00010185,
+		0x035, 0x000201D5,
+		0x035, 0x000281D5,
+		0x035, 0x000301D5,
+		0x035, 0x000401D5,
+		0x035, 0x000481D5,
+		0x035, 0x000501D5,
+		0x0EF, 0x00000000,
+	0xFF0F0740, 0xDEAD,
+	0xFF0F0740, 0xABCD,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000010,
+		0x036, 0x00005B8B,
+		0x036, 0x0000DB8B,
+		0x036, 0x00015B8B,
+		0x036, 0x0001DB8B,
+		0x036, 0x000262DB,
+		0x036, 0x0002E2DB,
+		0x036, 0x000362DB,
+		0x036, 0x0003E2DB,
+		0x036, 0x0004553B,
+		0x036, 0x0004D53B,
+		0x036, 0x0005553B,
+		0x036, 0x0005D53B,
+	0xFF0F01C0, 0xCDEF,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000010,
+		0x036, 0x00005B8B,
+		0x036, 0x0000DB8B,
+		0x036, 0x00015B8B,
+		0x036, 0x0001DB8B,
+		0x036, 0x000262DB,
+		0x036, 0x0002E2DB,
+		0x036, 0x000362DB,
+		0x036, 0x0003E2DB,
+		0x036, 0x0004553B,
+		0x036, 0x0004D53B,
+		0x036, 0x0005553B,
+		0x036, 0x0005D53B,
+	0xFF0F02C0, 0xCDEF,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000010,
+		0x036, 0x00005B8B,
+		0x036, 0x0000DB8B,
+		0x036, 0x00015B8B,
+		0x036, 0x0001DB8B,
+		0x036, 0x000262DB,
+		0x036, 0x0002E2DB,
+		0x036, 0x000362DB,
+		0x036, 0x0003E2DB,
+		0x036, 0x0004553B,
+		0x036, 0x0004D53B,
+		0x036, 0x0005553B,
+		0x036, 0x0005D53B,
+	0xFF0F07D8, 0xCDEF,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000010,
+		0x036, 0x00005B8B,
+		0x036, 0x0000DB8B,
+		0x036, 0x00015B8B,
+		0x036, 0x0001DB8B,
+		0x036, 0x000262DB,
+		0x036, 0x0002E2DB,
+		0x036, 0x000362DB,
+		0x036, 0x0003E2DB,
+		0x036, 0x0004553B,
+		0x036, 0x0004D53B,
+		0x036, 0x0005553B,
+		0x036, 0x0005D53B,
+	0xFF0F07D0, 0xCDEF,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000010,
+		0x036, 0x00005B8B,
+		0x036, 0x0000DB8B,
+		0x036, 0x00015B8B,
+		0x036, 0x0001DB8B,
+		0x036, 0x000262DB,
+		0x036, 0x0002E2DB,
+		0x036, 0x000362DB,
+		0x036, 0x0003E2DB,
+		0x036, 0x0004553B,
+		0x036, 0x0004D53B,
+		0x036, 0x0005553B,
+		0x036, 0x0005D53B,
+	0xCDCDCDCD, 0xCDCD,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000010,
+		0x036, 0x00084EB4,
+		0x036, 0x0008C9B4,
+		0x036, 0x000949B4,
+		0x036, 0x0009C9B4,
+		0x036, 0x000A4935,
+		0x036, 0x000AC935,
+		0x036, 0x000B4935,
+		0x036, 0x000BC935,
+		0x036, 0x000C4EB4,
+		0x036, 0x000CCEB4,
+		0x036, 0x000D4EB4,
+		0x036, 0x000DCEB4,
+	0xFF0F0740, 0xDEAD,
+		0x0EF, 0x00000000,
+		0x0EF, 0x00000008,
+	0xFF0F0740, 0xABCD,
+		0x03C, 0x000002DC,
+		0x03C, 0x00000524,
+		0x03C, 0x00000902,
+	0xFF0F01C0, 0xCDEF,
+		0x03C, 0x000002DC,
+		0x03C, 0x00000524,
+		0x03C, 0x00000902,
+	0xFF0F02C0, 0xCDEF,
+		0x03C, 0x000002DC,
+		0x03C, 0x00000524,
+		0x03C, 0x00000902,
+	0xFF0F07D8, 0xCDEF,
+		0x03C, 0x000002DC,
+		0x03C, 0x00000524,
+		0x03C, 0x00000902,
+	0xFF0F07D0, 0xCDEF,
+		0x03C, 0x000002DC,
+		0x03C, 0x00000524,
+		0x03C, 0x00000902,
+	0xCDCDCDCD, 0xCDCD,
+		0x03C, 0x000002AA,
+		0x03C, 0x000005A2,
+		0x03C, 0x00000880,
+	0xFF0F0740, 0xDEAD,
+		0x0EF, 0x00000000,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000002,
+		0x0DF, 0x00000080,
+	0xFF0F0740, 0xABCD,
+		0x061, 0x000EAC43,
+		0x062, 0x00038F47,
+		0x063, 0x00031157,
+		0x064, 0x0001C4AC,
+		0x065, 0x000931D1,
+	0xFF0F01C0, 0xCDEF,
+		0x061, 0x000EAC43,
+		0x062, 0x00038F47,
+		0x063, 0x00031157,
+		0x064, 0x0001C4AC,
+		0x065, 0x000931D1,
+	0xFF0F02C0, 0xCDEF,
+		0x061, 0x000EAC43,
+		0x062, 0x00038F47,
+		0x063, 0x00031157,
+		0x064, 0x0001C4AC,
+		0x065, 0x000931D1,
+	0xFF0F07D8, 0xCDEF,
+		0x061, 0x000EAC43,
+		0x062, 0x00038F47,
+		0x063, 0x00031157,
+		0x064, 0x0001C4AC,
+		0x065, 0x000931D1,
+	0xFF0F07D0, 0xCDEF,
+		0x061, 0x000EAC43,
+		0x062, 0x00038F47,
+		0x063, 0x00031157,
+		0x064, 0x0001C4AC,
+		0x065, 0x000931D1,
+	0xCDCDCDCD, 0xCDCD,
+		0x061, 0x000E5D53,
+		0x062, 0x00038FCD,
+		0x063, 0x000314EB,
+		0x064, 0x000196AC,
+		0x065, 0x000931D7,
+	0xFF0F0740, 0xDEAD,
+		0x008, 0x00008400,
+};
+
+u32 RTL8821AE_RADIOA_ARRAY[] = {
+		0x018, 0x0001712A,
+		0x056, 0x00051CF2,
+		0x066, 0x00040000,
+		0x000, 0x00010000,
+		0x01E, 0x00080000,
+		0x082, 0x00000830,
+		0x083, 0x00021800,
+		0x084, 0x00028000,
+		0x085, 0x00048000,
+		0x086, 0x00094838,
+		0x087, 0x00044980,
+		0x088, 0x00048000,
+		0x089, 0x0000D480,
+		0x08A, 0x00042240,
+		0x08B, 0x000F0380,
+		0x08C, 0x00090000,
+		0x08D, 0x00022852,
+		0x08E, 0x00065540,
+		0x08F, 0x00088001,
+		0x0EF, 0x00020000,
+		0x03E, 0x00000380,
+		0x03F, 0x00090018,
+		0x03E, 0x00020380,
+		0x03F, 0x000A0018,
+		0x03E, 0x00040308,
+		0x03F, 0x000A0018,
+		0x03E, 0x00060018,
+		0x03F, 0x000A0018,
+		0x0EF, 0x00000000,
+		0x018, 0x0001712A,
+		0x089, 0x00000080,
+		0x08B, 0x00080180,
+		0x0EF, 0x00001000,
+		0x03A, 0x00000244,
+		0x03B, 0x00038027,
+		0x03C, 0x00082000,
+		0x03A, 0x00000244,
+		0x03B, 0x00030113,
+		0x03C, 0x00082000,
+		0x03A, 0x0000014C,
+		0x03B, 0x00028027,
+		0x03C, 0x00082000,
+		0x03A, 0x000000CC,
+		0x03B, 0x00027027,
+		0x03C, 0x00042000,
+		0x03A, 0x0000014C,
+		0x03B, 0x0001F913,
+		0x03C, 0x00042000,
+		0x03A, 0x0000010C,
+		0x03B, 0x00017F10,
+		0x03C, 0x00012000,
+		0x03A, 0x000000D0,
+		0x03B, 0x00008027,
+		0x03C, 0x000CA000,
+		0x03A, 0x00000244,
+		0x03B, 0x00078027,
+		0x03C, 0x00082000,
+		0x03A, 0x00000244,
+		0x03B, 0x00070113,
+		0x03C, 0x00082000,
+		0x03A, 0x0000014C,
+		0x03B, 0x00068027,
+		0x03C, 0x00082000,
+		0x03A, 0x000000CC,
+		0x03B, 0x00067027,
+		0x03C, 0x00042000,
+		0x03A, 0x0000014C,
+		0x03B, 0x0005F913,
+		0x03C, 0x00042000,
+		0x03A, 0x0000010C,
+		0x03B, 0x00057F10,
+		0x03C, 0x00012000,
+		0x03A, 0x000000D0,
+		0x03B, 0x00048027,
+		0x03C, 0x000CA000,
+		0x03A, 0x00000244,
+		0x03B, 0x000B8027,
+		0x03C, 0x00082000,
+		0x03A, 0x00000244,
+		0x03B, 0x000B0113,
+		0x03C, 0x00082000,
+		0x03A, 0x0000014C,
+		0x03B, 0x000A8027,
+		0x03C, 0x00082000,
+		0x03A, 0x000000CC,
+		0x03B, 0x000A7027,
+		0x03C, 0x00042000,
+		0x03A, 0x0000014C,
+		0x03B, 0x0009F913,
+		0x03C, 0x00042000,
+		0x03A, 0x0000010C,
+		0x03B, 0x00097F10,
+		0x03C, 0x00012000,
+		0x03A, 0x000000D0,
+		0x03B, 0x00088027,
+		0x03C, 0x000CA000,
+		0x0EF, 0x00000000,
+		0x0EF, 0x00001100,
+	0xFF0F0104, 0xABCD,
+		0x034, 0x0004ADF3,
+		0x034, 0x00049DF0,
+	0xFF0F0204, 0xCDEF,
+		0x034, 0x0004ADF3,
+		0x034, 0x00049DF0,
+	0xFF0F0404, 0xCDEF,
+		0x034, 0x0004ADF3,
+		0x034, 0x00049DF0,
+	0xFF0F0200, 0xCDEF,
+		0x034, 0x0004ADF5,
+		0x034, 0x00049DF2,
+	0xFF0F02C0, 0xCDEF,
+		0x034, 0x0004A0F3,
+		0x034, 0x000490B1,
+	0xCDCDCDCD, 0xCDCD,
+		0x034, 0x0004ADF7,
+		0x034, 0x00049DF3,
+	0xFF0F0104, 0xDEAD,
+	0xFF0F0104, 0xABCD,
+		0x034, 0x00048DED,
+		0x034, 0x00047DEA,
+		0x034, 0x00046DE7,
+		0x034, 0x00045CE9,
+		0x034, 0x00044CE6,
+		0x034, 0x000438C6,
+		0x034, 0x00042886,
+		0x034, 0x00041486,
+		0x034, 0x00040447,
+	0xFF0F0204, 0xCDEF,
+		0x034, 0x00048DED,
+		0x034, 0x00047DEA,
+		0x034, 0x00046DE7,
+		0x034, 0x00045CE9,
+		0x034, 0x00044CE6,
+		0x034, 0x000438C6,
+		0x034, 0x00042886,
+		0x034, 0x00041486,
+		0x034, 0x00040447,
+	0xFF0F0404, 0xCDEF,
+		0x034, 0x00048DED,
+		0x034, 0x00047DEA,
+		0x034, 0x00046DE7,
+		0x034, 0x00045CE9,
+		0x034, 0x00044CE6,
+		0x034, 0x000438C6,
+		0x034, 0x00042886,
+		0x034, 0x00041486,
+		0x034, 0x00040447,
+	0xFF0F02C0, 0xCDEF,
+		0x034, 0x000480AE,
+		0x034, 0x000470AB,
+		0x034, 0x0004608B,
+		0x034, 0x00045069,
+		0x034, 0x00044048,
+		0x034, 0x00043045,
+		0x034, 0x00042026,
+		0x034, 0x00041023,
+		0x034, 0x00040002,
+	0xCDCDCDCD, 0xCDCD,
+		0x034, 0x00048DEF,
+		0x034, 0x00047DEC,
+		0x034, 0x00046DE9,
+		0x034, 0x00045CCB,
+		0x034, 0x0004488D,
+		0x034, 0x0004348D,
+		0x034, 0x0004248A,
+		0x034, 0x0004108D,
+		0x034, 0x0004008A,
+	0xFF0F0104, 0xDEAD,
+	0xFF0F0200, 0xABCD,
+		0x034, 0x0002ADF4,
+	0xFF0F02C0, 0xCDEF,
+		0x034, 0x0002A0F3,
+	0xCDCDCDCD, 0xCDCD,
+		0x034, 0x0002ADF7,
+	0xFF0F0200, 0xDEAD,
+	0xFF0F0104, 0xABCD,
+		0x034, 0x00029DF4,
+	0xFF0F0204, 0xCDEF,
+		0x034, 0x00029DF4,
+	0xFF0F0404, 0xCDEF,
+		0x034, 0x00029DF4,
+	0xFF0F0200, 0xCDEF,
+		0x034, 0x00029DF1,
+	0xFF0F02C0, 0xCDEF,
+		0x034, 0x000290F0,
+	0xCDCDCDCD, 0xCDCD,
+		0x034, 0x00029DF2,
+	0xFF0F0104, 0xDEAD,
+	0xFF0F0104, 0xABCD,
+		0x034, 0x00028DF1,
+		0x034, 0x00027DEE,
+		0x034, 0x00026DEB,
+		0x034, 0x00025CEC,
+		0x034, 0x00024CE9,
+		0x034, 0x000238CA,
+		0x034, 0x00022889,
+		0x034, 0x00021489,
+		0x034, 0x0002044A,
+	0xFF0F0204, 0xCDEF,
+		0x034, 0x00028DF1,
+		0x034, 0x00027DEE,
+		0x034, 0x00026DEB,
+		0x034, 0x00025CEC,
+		0x034, 0x00024CE9,
+		0x034, 0x000238CA,
+		0x034, 0x00022889,
+		0x034, 0x00021489,
+		0x034, 0x0002044A,
+	0xFF0F0404, 0xCDEF,
+		0x034, 0x00028DF1,
+		0x034, 0x00027DEE,
+		0x034, 0x00026DEB,
+		0x034, 0x00025CEC,
+		0x034, 0x00024CE9,
+		0x034, 0x000238CA,
+		0x034, 0x00022889,
+		0x034, 0x00021489,
+		0x034, 0x0002044A,
+	0xFF0F02C0, 0xCDEF,
+		0x034, 0x000280AF,
+		0x034, 0x000270AC,
+		0x034, 0x0002608B,
+		0x034, 0x00025069,
+		0x034, 0x00024048,
+		0x034, 0x00023045,
+		0x034, 0x00022026,
+		0x034, 0x00021023,
+		0x034, 0x00020002,
+	0xCDCDCDCD, 0xCDCD,
+		0x034, 0x00028DEE,
+		0x034, 0x00027DEB,
+		0x034, 0x00026CCD,
+		0x034, 0x00025CCA,
+		0x034, 0x0002488C,
+		0x034, 0x0002384C,
+		0x034, 0x00022849,
+		0x034, 0x00021449,
+		0x034, 0x0002004D,
+	0xFF0F0104, 0xDEAD,
+	0xFF0F02C0, 0xABCD,
+		0x034, 0x0000A0D7,
+		0x034, 0x000090D3,
+		0x034, 0x000080B1,
+		0x034, 0x000070AE,
+	0xCDCDCDCD, 0xCDCD,
+		0x034, 0x0000ADF7,
+		0x034, 0x00009DF4,
+		0x034, 0x00008DF1,
+		0x034, 0x00007DEE,
+	0xFF0F02C0, 0xDEAD,
+	0xFF0F0104, 0xABCD,
+		0x034, 0x00006DEB,
+		0x034, 0x00005CEC,
+		0x034, 0x00004CE9,
+		0x034, 0x000038CA,
+		0x034, 0x00002889,
+		0x034, 0x00001489,
+		0x034, 0x0000044A,
+	0xFF0F0204, 0xCDEF,
+		0x034, 0x00006DEB,
+		0x034, 0x00005CEC,
+		0x034, 0x00004CE9,
+		0x034, 0x000038CA,
+		0x034, 0x00002889,
+		0x034, 0x00001489,
+		0x034, 0x0000044A,
+	0xFF0F0404, 0xCDEF,
+		0x034, 0x00006DEB,
+		0x034, 0x00005CEC,
+		0x034, 0x00004CE9,
+		0x034, 0x000038CA,
+		0x034, 0x00002889,
+		0x034, 0x00001489,
+		0x034, 0x0000044A,
+	0xFF0F02C0, 0xCDEF,
+		0x034, 0x0000608D,
+		0x034, 0x0000506B,
+		0x034, 0x0000404A,
+		0x034, 0x00003047,
+		0x034, 0x00002044,
+		0x034, 0x00001025,
+		0x034, 0x00000004,
+	0xCDCDCDCD, 0xCDCD,
+		0x034, 0x00006DCD,
+		0x034, 0x00005CCD,
+		0x034, 0x00004CCA,
+		0x034, 0x0000388C,
+		0x034, 0x00002888,
+		0x034, 0x00001488,
+		0x034, 0x00000486,
+	0xFF0F0104, 0xDEAD,
+		0x0EF, 0x00000000,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000040,
+	0xFF0F0104, 0xABCD,
+		0x035, 0x00000187,
+		0x035, 0x00008187,
+		0x035, 0x00010187,
+		0x035, 0x00020188,
+		0x035, 0x00028188,
+		0x035, 0x00030188,
+		0x035, 0x00040188,
+		0x035, 0x00048188,
+		0x035, 0x00050188,
+	0xFF0F0204, 0xCDEF,
+		0x035, 0x00000187,
+		0x035, 0x00008187,
+		0x035, 0x00010187,
+		0x035, 0x00020188,
+		0x035, 0x00028188,
+		0x035, 0x00030188,
+		0x035, 0x00040188,
+		0x035, 0x00048188,
+		0x035, 0x00050188,
+	0xFF0F0404, 0xCDEF,
+		0x035, 0x00000187,
+		0x035, 0x00008187,
+		0x035, 0x00010187,
+		0x035, 0x00020188,
+		0x035, 0x00028188,
+		0x035, 0x00030188,
+		0x035, 0x00040188,
+		0x035, 0x00048188,
+		0x035, 0x00050188,
+	0xCDCDCDCD, 0xCDCD,
+		0x035, 0x00000145,
+		0x035, 0x00008145,
+		0x035, 0x00010145,
+		0x035, 0x00020196,
+		0x035, 0x00028196,
+		0x035, 0x00030196,
+		0x035, 0x000401C7,
+		0x035, 0x000481C7,
+		0x035, 0x000501C7,
+	0xFF0F0104, 0xDEAD,
+		0x0EF, 0x00000000,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000010,
+	0xFF0F0104, 0xABCD,
+		0x036, 0x00085733,
+		0x036, 0x0008D733,
+		0x036, 0x00095733,
+		0x036, 0x0009D733,
+		0x036, 0x000A64B4,
+		0x036, 0x000AE4B4,
+		0x036, 0x000B64B4,
+		0x036, 0x000BE4B4,
+		0x036, 0x000C64B4,
+		0x036, 0x000CE4B4,
+		0x036, 0x000D64B4,
+		0x036, 0x000DE4B4,
+	0xFF0F0204, 0xCDEF,
+		0x036, 0x00085733,
+		0x036, 0x0008D733,
+		0x036, 0x00095733,
+		0x036, 0x0009D733,
+		0x036, 0x000A64B4,
+		0x036, 0x000AE4B4,
+		0x036, 0x000B64B4,
+		0x036, 0x000BE4B4,
+		0x036, 0x000C64B4,
+		0x036, 0x000CE4B4,
+		0x036, 0x000D64B4,
+		0x036, 0x000DE4B4,
+	0xFF0F0404, 0xCDEF,
+		0x036, 0x00085733,
+		0x036, 0x0008D733,
+		0x036, 0x00095733,
+		0x036, 0x0009D733,
+		0x036, 0x000A64B4,
+		0x036, 0x000AE4B4,
+		0x036, 0x000B64B4,
+		0x036, 0x000BE4B4,
+		0x036, 0x000C64B4,
+		0x036, 0x000CE4B4,
+		0x036, 0x000D64B4,
+		0x036, 0x000DE4B4,
+	0xCDCDCDCD, 0xCDCD,
+		0x036, 0x000056B3,
+		0x036, 0x0000D6B3,
+		0x036, 0x000156B3,
+		0x036, 0x0001D6B3,
+		0x036, 0x00026634,
+		0x036, 0x0002E634,
+		0x036, 0x00036634,
+		0x036, 0x0003E634,
+		0x036, 0x000467B4,
+		0x036, 0x0004E7B4,
+		0x036, 0x000567B4,
+		0x036, 0x0005E7B4,
+	0xFF0F0104, 0xDEAD,
+		0x0EF, 0x00000000,
+		0x0EF, 0x00000008,
+	0xFF0F0104, 0xABCD,
+		0x03C, 0x000001C8,
+		0x03C, 0x00000492,
+	0xFF0F0204, 0xCDEF,
+		0x03C, 0x000001C8,
+		0x03C, 0x00000492,
+	0xFF0F0404, 0xCDEF,
+		0x03C, 0x000001C8,
+		0x03C, 0x00000492,
+	0xCDCDCDCD, 0xCDCD,
+		0x03C, 0x0000022A,
+		0x03C, 0x00000594,
+	0xFF0F0104, 0xDEAD,
+	0xFF0F0104, 0xABCD,
+		0x03C, 0x00000800,
+	0xFF0F0204, 0xCDEF,
+		0x03C, 0x00000800,
+	0xFF0F0404, 0xCDEF,
+		0x03C, 0x00000800,
+	0xFF0F02C0, 0xCDEF,
+		0x03C, 0x00000820,
+	0xCDCDCDCD, 0xCDCD,
+		0x03C, 0x00000900,
+	0xFF0F0104, 0xDEAD,
+		0x0EF, 0x00000000,
+		0x018, 0x0001712A,
+		0x0EF, 0x00000002,
+	0xFF0F0104, 0xABCD,
+		0x008, 0x0004E400,
+	0xFF0F0204, 0xCDEF,
+		0x008, 0x0004E400,
+	0xFF0F0404, 0xCDEF,
+		0x008, 0x0004E400,
+	0xCDCDCDCD, 0xCDCD,
+		0x008, 0x00002000,
+	0xFF0F0104, 0xDEAD,
+		0x0EF, 0x00000000,
+		0x0DF, 0x000000C0,
+		0x01F, 0x00040064,
+	0xFF0F0104, 0xABCD,
+		0x058, 0x000A7284,
+		0x059, 0x000600EC,
+	0xFF0F0204, 0xCDEF,
+		0x058, 0x000A7284,
+		0x059, 0x000600EC,
+	0xFF0F0404, 0xCDEF,
+		0x058, 0x000A7284,
+		0x059, 0x000600EC,
+	0xCDCDCDCD, 0xCDCD,
+		0x058, 0x00081184,
+		0x059, 0x0006016C,
+	0xFF0F0104, 0xDEAD,
+	0xFF0F0104, 0xABCD,
+		0x061, 0x000E8D73,
+		0x062, 0x00093FC5,
+	0xFF0F0204, 0xCDEF,
+		0x061, 0x000E8D73,
+		0x062, 0x00093FC5,
+	0xFF0F0404, 0xCDEF,
+		0x061, 0x000E8D73,
+		0x062, 0x00093FC5,
+	0xCDCDCDCD, 0xCDCD,
+		0x061, 0x000EAD53,
+		0x062, 0x00093BC4,
+	0xFF0F0104, 0xDEAD,
+	0xFF0F0104, 0xABCD,
+		0x063, 0x000110E9,
+	0xFF0F0204, 0xCDEF,
+		0x063, 0x000110E9,
+	0xFF0F0404, 0xCDEF,
+		0x063, 0x000110E9,
+	0xFF0F0200, 0xCDEF,
+		0x063, 0x000710E9,
+	0xFF0F02C0, 0xCDEF,
+		0x063, 0x000110E9,
+	0xCDCDCDCD, 0xCDCD,
+		0x063, 0x000714E9,
+	0xFF0F0104, 0xDEAD,
+	0xFF0F0104, 0xABCD,
+		0x064, 0x0001C27C,
+	0xFF0F0204, 0xCDEF,
+		0x064, 0x0001C27C,
+	0xFF0F0404, 0xCDEF,
+		0x064, 0x0001C27C,
+	0xCDCDCDCD, 0xCDCD,
+		0x064, 0x0001C67C,
+	0xFF0F0104, 0xDEAD,
+	0xFF0F0200, 0xABCD,
+		0x065, 0x00093016,
+	0xFF0F02C0, 0xCDEF,
+		0x065, 0x00093015,
+	0xCDCDCDCD, 0xCDCD,
+		0x065, 0x00091016,
+	0xFF0F0200, 0xDEAD,
+		0x018, 0x00000006,
+		0x0EF, 0x00002000,
+		0x03B, 0x0003824B,
+		0x03B, 0x0003024B,
+		0x03B, 0x0002844B,
+		0x03B, 0x00020F4B,
+		0x03B, 0x00018F4B,
+		0x03B, 0x000104B2,
+		0x03B, 0x00008049,
+		0x03B, 0x00000148,
+		0x03B, 0x0007824B,
+		0x03B, 0x0007024B,
+		0x03B, 0x0006824B,
+		0x03B, 0x00060F4B,
+		0x03B, 0x00058F4B,
+		0x03B, 0x000504B2,
+		0x03B, 0x00048049,
+		0x03B, 0x00040148,
+		0x0EF, 0x00000000,
+		0x0EF, 0x00000100,
+		0x034, 0x0000ADF3,
+		0x034, 0x00009DEF,
+		0x034, 0x00008DEC,
+		0x034, 0x00007DE9,
+		0x034, 0x00006CED,
+		0x034, 0x00005CE9,
+		0x034, 0x000044E9,
+		0x034, 0x000034E6,
+		0x034, 0x0000246A,
+		0x034, 0x00001467,
+		0x034, 0x00000068,
+		0x0EF, 0x00000000,
+		0x0ED, 0x00000010,
+		0x044, 0x0000ADF2,
+		0x044, 0x00009DEF,
+		0x044, 0x00008DEC,
+		0x044, 0x00007DE9,
+		0x044, 0x00006CEC,
+		0x044, 0x00005CE9,
+		0x044, 0x000044EC,
+		0x044, 0x000034E9,
+		0x044, 0x0000246C,
+		0x044, 0x00001469,
+		0x044, 0x0000006C,
+		0x0ED, 0x00000000,
+		0x0ED, 0x00000001,
+		0x040, 0x00038DA7,
+		0x040, 0x000300C2,
+		0x040, 0x000288E2,
+		0x040, 0x000200B8,
+		0x040, 0x000188A5,
+		0x040, 0x00010FBC,
+		0x040, 0x00008F71,
+		0x040, 0x00000240,
+		0x0ED, 0x00000000,
+		0x0EF, 0x000020A2,
+		0x0DF, 0x00000080,
+		0x035, 0x00000120,
+		0x035, 0x00008120,
+		0x035, 0x00010120,
+		0x036, 0x00000085,
+		0x036, 0x00008085,
+		0x036, 0x00010085,
+		0x036, 0x00018085,
+		0x0EF, 0x00000000,
+		0x051, 0x00000C31,
+		0x052, 0x00000622,
+		0x053, 0x000FC70B,
+		0x054, 0x0000017E,
+		0x056, 0x00051DF3,
+		0x051, 0x00000C01,
+		0x052, 0x000006D6,
+		0x053, 0x000FC649,
+		0x070, 0x00049661,
+		0x071, 0x0007843E,
+		0x072, 0x00000382,
+		0x074, 0x00051400,
+		0x035, 0x00000160,
+		0x035, 0x00008160,
+		0x035, 0x00010160,
+		0x036, 0x00000124,
+		0x036, 0x00008124,
+		0x036, 0x00010124,
+		0x036, 0x00018124,
+		0x0ED, 0x0000000C,
+		0x045, 0x00000140,
+		0x045, 0x00008140,
+		0x045, 0x00010140,
+		0x046, 0x00000124,
+		0x046, 0x00008124,
+		0x046, 0x00010124,
+		0x046, 0x00018124,
+		0x0DF, 0x00000088,
+		0x0B3, 0x000F0E18,
+		0x0B4, 0x0001214C,
+		0x0B7, 0x0003000C,
+		0x01C, 0x000539D2,
+		0x018, 0x0001F12A,
+		0x0FE, 0x00000000,
+		0x0FE, 0x00000000,
+		0x018, 0x0001712A,
+};
+
+u32 RTL8812AE_MAC_REG_ARRAY[] = {
+		0x010, 0x0000000C,
+	0xFF0F0180, 0xABCD,
+		0x025, 0x0000000F,
+	0xFF0F01C0, 0xCDEF,
+		0x025, 0x0000000F,
+	0xCDCDCDCD, 0xCDCD,
+		0x025, 0x0000006F,
+	0xFF0F0180, 0xDEAD,
+		0x072, 0x00000000,
+		0x428, 0x0000000A,
+		0x429, 0x00000010,
+		0x430, 0x00000000,
+		0x431, 0x00000000,
+		0x432, 0x00000000,
+		0x433, 0x00000001,
+		0x434, 0x00000004,
+		0x435, 0x00000005,
+		0x436, 0x00000007,
+		0x437, 0x00000008,
+		0x43C, 0x00000004,
+		0x43D, 0x00000005,
+		0x43E, 0x00000007,
+		0x43F, 0x00000008,
+		0x440, 0x0000005D,
+		0x441, 0x00000001,
+		0x442, 0x00000000,
+		0x444, 0x00000010,
+		0x445, 0x00000000,
+		0x446, 0x00000000,
+		0x447, 0x00000000,
+		0x448, 0x00000000,
+		0x449, 0x000000F0,
+		0x44A, 0x0000000F,
+		0x44B, 0x0000003E,
+		0x44C, 0x00000010,
+		0x44D, 0x00000000,
+		0x44E, 0x00000000,
+		0x44F, 0x00000000,
+		0x450, 0x00000000,
+		0x451, 0x000000F0,
+		0x452, 0x0000000F,
+		0x453, 0x00000000,
+		0x45B, 0x00000080,
+		0x460, 0x00000066,
+		0x461, 0x00000066,
+		0x4C8, 0x000000FF,
+		0x4C9, 0x00000008,
+		0x4CC, 0x000000FF,
+		0x4CD, 0x000000FF,
+		0x4CE, 0x00000001,
+		0x500, 0x00000026,
+		0x501, 0x000000A2,
+		0x502, 0x0000002F,
+		0x503, 0x00000000,
+		0x504, 0x00000028,
+		0x505, 0x000000A3,
+		0x506, 0x0000005E,
+		0x507, 0x00000000,
+		0x508, 0x0000002B,
+		0x509, 0x000000A4,
+		0x50A, 0x0000005E,
+		0x50B, 0x00000000,
+		0x50C, 0x0000004F,
+		0x50D, 0x000000A4,
+		0x50E, 0x00000000,
+		0x50F, 0x00000000,
+		0x512, 0x0000001C,
+		0x514, 0x0000000A,
+		0x516, 0x0000000A,
+		0x525, 0x0000004F,
+		0x550, 0x00000010,
+		0x551, 0x00000010,
+		0x559, 0x00000002,
+		0x55C, 0x00000050,
+		0x55D, 0x000000FF,
+		0x604, 0x00000001,
+		0x605, 0x00000030,
+		0x607, 0x00000003,
+		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, 0x00000080,
+		0x642, 0x00000040,
+		0x643, 0x00000000,
+		0x652, 0x000000C8,
+		0x66E, 0x00000005,
+		0x700, 0x00000021,
+		0x701, 0x00000043,
+		0x702, 0x00000065,
+		0x703, 0x00000087,
+		0x708, 0x00000021,
+		0x709, 0x00000043,
+		0x70A, 0x00000065,
+		0x70B, 0x00000087,
+		0x718, 0x00000040,
+};
+
+u32 RTL8821AE_MAC_REG_ARRAY[] = {
+		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, 0x0000003F,
+		0x4C9, 0x000000FF,
+		0x4CC, 0x000000FF,
+		0x4CD, 0x000000FF,
+		0x4CE, 0x00000001,
+		0x500, 0x00000026,
+		0x501, 0x000000A2,
+		0x502, 0x0000002F,
+		0x503, 0x00000000,
+		0x504, 0x00000028,
+		0x505, 0x000000A3,
+		0x506, 0x0000005E,
+		0x507, 0x00000000,
+		0x508, 0x0000002B,
+		0x509, 0x000000A4,
+		0x50A, 0x0000005E,
+		0x50B, 0x00000000,
+		0x50C, 0x0000004F,
+		0x50D, 0x000000A4,
+		0x50E, 0x00000000,
+		0x50F, 0x00000000,
+		0x512, 0x0000001C,
+		0x514, 0x0000000A,
+		0x516, 0x0000000A,
+		0x525, 0x0000004F,
+		0x550, 0x00000010,
+		0x551, 0x00000010,
+		0x559, 0x00000002,
+		0x55C, 0x00000050,
+		0x55D, 0x000000FF,
+		0x605, 0x00000030,
+		0x607, 0x00000007,
+		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,
+		0x718, 0x00000040,
+};
+
+u32 RTL8812AE_AGC_TAB_ARRAY[] = {
+	0xFF0F07D8, 0xABCD,
+		0x81C, 0xFC000001,
+		0x81C, 0xFB020001,
+		0x81C, 0xFA040001,
+		0x81C, 0xF9060001,
+		0x81C, 0xF8080001,
+		0x81C, 0xF70A0001,
+		0x81C, 0xF60C0001,
+		0x81C, 0xF50E0001,
+		0x81C, 0xF4100001,
+		0x81C, 0xF3120001,
+		0x81C, 0xF2140001,
+		0x81C, 0xF1160001,
+		0x81C, 0xF0180001,
+		0x81C, 0xEF1A0001,
+		0x81C, 0xEE1C0001,
+		0x81C, 0xED1E0001,
+		0x81C, 0xEC200001,
+		0x81C, 0xEB220001,
+		0x81C, 0xEA240001,
+		0x81C, 0xCD260001,
+		0x81C, 0xCC280001,
+		0x81C, 0xCB2A0001,
+		0x81C, 0xCA2C0001,
+		0x81C, 0xC92E0001,
+		0x81C, 0xC8300001,
+		0x81C, 0xA6320001,
+		0x81C, 0xA5340001,
+		0x81C, 0xA4360001,
+		0x81C, 0xA3380001,
+		0x81C, 0xA23A0001,
+		0x81C, 0x883C0001,
+		0x81C, 0x873E0001,
+		0x81C, 0x86400001,
+		0x81C, 0x85420001,
+		0x81C, 0x84440001,
+		0x81C, 0x83460001,
+		0x81C, 0x82480001,
+		0x81C, 0x814A0001,
+		0x81C, 0x484C0001,
+		0x81C, 0x474E0001,
+		0x81C, 0x46500001,
+		0x81C, 0x45520001,
+		0x81C, 0x44540001,
+		0x81C, 0x43560001,
+		0x81C, 0x42580001,
+		0x81C, 0x415A0001,
+		0x81C, 0x255C0001,
+		0x81C, 0x245E0001,
+		0x81C, 0x23600001,
+		0x81C, 0x22620001,
+		0x81C, 0x21640001,
+		0x81C, 0x21660001,
+		0x81C, 0x21680001,
+		0x81C, 0x216A0001,
+		0x81C, 0x216C0001,
+		0x81C, 0x216E0001,
+		0x81C, 0x21700001,
+		0x81C, 0x21720001,
+		0x81C, 0x21740001,
+		0x81C, 0x21760001,
+		0x81C, 0x21780001,
+		0x81C, 0x217A0001,
+		0x81C, 0x217C0001,
+		0x81C, 0x217E0001,
+	0xFF0F07D0, 0xCDEF,
+		0x81C, 0xF9000001,
+		0x81C, 0xF8020001,
+		0x81C, 0xF7040001,
+		0x81C, 0xF6060001,
+		0x81C, 0xF5080001,
+		0x81C, 0xF40A0001,
+		0x81C, 0xF30C0001,
+		0x81C, 0xF20E0001,
+		0x81C, 0xF1100001,
+		0x81C, 0xF0120001,
+		0x81C, 0xEF140001,
+		0x81C, 0xEE160001,
+		0x81C, 0xED180001,
+		0x81C, 0xEC1A0001,
+		0x81C, 0xEB1C0001,
+		0x81C, 0xEA1E0001,
+		0x81C, 0xCD200001,
+		0x81C, 0xCC220001,
+		0x81C, 0xCB240001,
+		0x81C, 0xCA260001,
+		0x81C, 0xC9280001,
+		0x81C, 0xC82A0001,
+		0x81C, 0xC72C0001,
+		0x81C, 0xC62E0001,
+		0x81C, 0xA5300001,
+		0x81C, 0xA4320001,
+		0x81C, 0xA3340001,
+		0x81C, 0xA2360001,
+		0x81C, 0x88380001,
+		0x81C, 0x873A0001,
+		0x81C, 0x863C0001,
+		0x81C, 0x853E0001,
+		0x81C, 0x84400001,
+		0x81C, 0x83420001,
+		0x81C, 0x82440001,
+		0x81C, 0x81460001,
+		0x81C, 0x48480001,
+		0x81C, 0x474A0001,
+		0x81C, 0x464C0001,
+		0x81C, 0x454E0001,
+		0x81C, 0x44500001,
+		0x81C, 0x43520001,
+		0x81C, 0x42540001,
+		0x81C, 0x41560001,
+		0x81C, 0x25580001,
+		0x81C, 0x245A0001,
+		0x81C, 0x235C0001,
+		0x81C, 0x225E0001,
+		0x81C, 0x21600001,
+		0x81C, 0x21620001,
+		0x81C, 0x21640001,
+		0x81C, 0x21660001,
+		0x81C, 0x21680001,
+		0x81C, 0x216A0001,
+		0x81C, 0x236C0001,
+		0x81C, 0x226E0001,
+		0x81C, 0x21700001,
+		0x81C, 0x21720001,
+		0x81C, 0x21740001,
+		0x81C, 0x21760001,
+		0x81C, 0x21780001,
+		0x81C, 0x217A0001,
+		0x81C, 0x217C0001,
+		0x81C, 0x217E0001,
+	0xCDCDCDCD, 0xCDCD,
+		0x81C, 0xFF000001,
+		0x81C, 0xFF020001,
+		0x81C, 0xFF040001,
+		0x81C, 0xFF060001,
+		0x81C, 0xFF080001,
+		0x81C, 0xFE0A0001,
+		0x81C, 0xFD0C0001,
+		0x81C, 0xFC0E0001,
+		0x81C, 0xFB100001,
+		0x81C, 0xFA120001,
+		0x81C, 0xF9140001,
+		0x81C, 0xF8160001,
+		0x81C, 0xF7180001,
+		0x81C, 0xF61A0001,
+		0x81C, 0xF51C0001,
+		0x81C, 0xF41E0001,
+		0x81C, 0xF3200001,
+		0x81C, 0xF2220001,
+		0x81C, 0xF1240001,
+		0x81C, 0xF0260001,
+		0x81C, 0xEF280001,
+		0x81C, 0xEE2A0001,
+		0x81C, 0xED2C0001,
+		0x81C, 0xEC2E0001,
+		0x81C, 0xEB300001,
+		0x81C, 0xEA320001,
+		0x81C, 0xE9340001,
+		0x81C, 0xE8360001,
+		0x81C, 0xE7380001,
+		0x81C, 0xE63A0001,
+		0x81C, 0xE53C0001,
+		0x81C, 0xC73E0001,
+		0x81C, 0xC6400001,
+		0x81C, 0xC5420001,
+		0x81C, 0xC4440001,
+		0x81C, 0xC3460001,
+		0x81C, 0xC2480001,
+		0x81C, 0xC14A0001,
+		0x81C, 0xA74C0001,
+		0x81C, 0xA64E0001,
+		0x81C, 0xA5500001,
+		0x81C, 0xA4520001,
+		0x81C, 0xA3540001,
+		0x81C, 0xA2560001,
+		0x81C, 0xA1580001,
+		0x81C, 0x675A0001,
+		0x81C, 0x665C0001,
+		0x81C, 0x655E0001,
+		0x81C, 0x64600001,
+		0x81C, 0x63620001,
+		0x81C, 0x48640001,
+		0x81C, 0x47660001,
+		0x81C, 0x46680001,
+		0x81C, 0x456A0001,
+		0x81C, 0x446C0001,
+		0x81C, 0x436E0001,
+		0x81C, 0x42700001,
+		0x81C, 0x41720001,
+		0x81C, 0x41740001,
+		0x81C, 0x41760001,
+		0x81C, 0x41780001,
+		0x81C, 0x417A0001,
+		0x81C, 0x417C0001,
+		0x81C, 0x417E0001,
+	0xFF0F07D8, 0xDEAD,
+	0xFF0F0180, 0xABCD,
+		0x81C, 0xFC800001,
+		0x81C, 0xFB820001,
+		0x81C, 0xFA840001,
+		0x81C, 0xF9860001,
+		0x81C, 0xF8880001,
+		0x81C, 0xF78A0001,
+		0x81C, 0xF68C0001,
+		0x81C, 0xF58E0001,
+		0x81C, 0xF4900001,
+		0x81C, 0xF3920001,
+		0x81C, 0xF2940001,
+		0x81C, 0xF1960001,
+		0x81C, 0xF0980001,
+		0x81C, 0xEF9A0001,
+		0x81C, 0xEE9C0001,
+		0x81C, 0xED9E0001,
+		0x81C, 0xECA00001,
+		0x81C, 0xEBA20001,
+		0x81C, 0xEAA40001,
+		0x81C, 0xE9A60001,
+		0x81C, 0xE8A80001,
+		0x81C, 0xE7AA0001,
+		0x81C, 0xE6AC0001,
+		0x81C, 0xE5AE0001,
+		0x81C, 0xE4B00001,
+		0x81C, 0xE3B20001,
+		0x81C, 0xA8B40001,
+		0x81C, 0xA7B60001,
+		0x81C, 0xA6B80001,
+		0x81C, 0xA5BA0001,
+		0x81C, 0xA4BC0001,
+		0x81C, 0xA3BE0001,
+		0x81C, 0xA2C00001,
+		0x81C, 0xA1C20001,
+		0x81C, 0x68C40001,
+		0x81C, 0x67C60001,
+		0x81C, 0x66C80001,
+		0x81C, 0x65CA0001,
+		0x81C, 0x64CC0001,
+		0x81C, 0x47CE0001,
+		0x81C, 0x46D00001,
+		0x81C, 0x45D20001,
+		0x81C, 0x44D40001,
+		0x81C, 0x43D60001,
+		0x81C, 0x42D80001,
+		0x81C, 0x08DA0001,
+		0x81C, 0x07DC0001,
+		0x81C, 0x06DE0001,
+		0x81C, 0x05E00001,
+		0x81C, 0x04E20001,
+		0x81C, 0x03E40001,
+		0x81C, 0x02E60001,
+		0x81C, 0x01E80001,
+		0x81C, 0x01EA0001,
+		0x81C, 0x01EC0001,
+		0x81C, 0x01EE0001,
+		0x81C, 0x01F00001,
+		0x81C, 0x01F20001,
+		0x81C, 0x01F40001,
+		0x81C, 0x01F60001,
+		0x81C, 0x01F80001,
+		0x81C, 0x01FA0001,
+		0x81C, 0x01FC0001,
+		0x81C, 0x01FE0001,
+	0xFF0F0280, 0xCDEF,
+		0x81C, 0xFC800001,
+		0x81C, 0xFB820001,
+		0x81C, 0xFA840001,
+		0x81C, 0xF9860001,
+		0x81C, 0xF8880001,
+		0x81C, 0xF78A0001,
+		0x81C, 0xF68C0001,
+		0x81C, 0xF58E0001,
+		0x81C, 0xF4900001,
+		0x81C, 0xF3920001,
+		0x81C, 0xF2940001,
+		0x81C, 0xF1960001,
+		0x81C, 0xF0980001,
+		0x81C, 0xEF9A0001,
+		0x81C, 0xEE9C0001,
+		0x81C, 0xED9E0001,
+		0x81C, 0xECA00001,
+		0x81C, 0xEBA20001,
+		0x81C, 0xEAA40001,
+		0x81C, 0xE9A60001,
+		0x81C, 0xE8A80001,
+		0x81C, 0xE7AA0001,
+		0x81C, 0xE6AC0001,
+		0x81C, 0xE5AE0001,
+		0x81C, 0xE4B00001,
+		0x81C, 0xE3B20001,
+		0x81C, 0xA8B40001,
+		0x81C, 0xA7B60001,
+		0x81C, 0xA6B80001,
+		0x81C, 0xA5BA0001,
+		0x81C, 0xA4BC0001,
+		0x81C, 0xA3BE0001,
+		0x81C, 0xA2C00001,
+		0x81C, 0xA1C20001,
+		0x81C, 0x68C40001,
+		0x81C, 0x67C60001,
+		0x81C, 0x66C80001,
+		0x81C, 0x65CA0001,
+		0x81C, 0x64CC0001,
+		0x81C, 0x47CE0001,
+		0x81C, 0x46D00001,
+		0x81C, 0x45D20001,
+		0x81C, 0x44D40001,
+		0x81C, 0x43D60001,
+		0x81C, 0x42D80001,
+		0x81C, 0x08DA0001,
+		0x81C, 0x07DC0001,
+		0x81C, 0x06DE0001,
+		0x81C, 0x05E00001,
+		0x81C, 0x04E20001,
+		0x81C, 0x03E40001,
+		0x81C, 0x02E60001,
+		0x81C, 0x01E80001,
+		0x81C, 0x01EA0001,
+		0x81C, 0x01EC0001,
+		0x81C, 0x01EE0001,
+		0x81C, 0x01F00001,
+		0x81C, 0x01F20001,
+		0x81C, 0x01F40001,
+		0x81C, 0x01F60001,
+		0x81C, 0x01F80001,
+		0x81C, 0x01FA0001,
+		0x81C, 0x01FC0001,
+		0x81C, 0x01FE0001,
+	0xFF0F01C0, 0xCDEF,
+		0x81C, 0xFC800001,
+		0x81C, 0xFB820001,
+		0x81C, 0xFA840001,
+		0x81C, 0xF9860001,
+		0x81C, 0xF8880001,
+		0x81C, 0xF78A0001,
+		0x81C, 0xF68C0001,
+		0x81C, 0xF58E0001,
+		0x81C, 0xF4900001,
+		0x81C, 0xF3920001,
+		0x81C, 0xF2940001,
+		0x81C, 0xF1960001,
+		0x81C, 0xF0980001,
+		0x81C, 0xEF9A0001,
+		0x81C, 0xEE9C0001,
+		0x81C, 0xED9E0001,
+		0x81C, 0xECA00001,
+		0x81C, 0xEBA20001,
+		0x81C, 0xEAA40001,
+		0x81C, 0xE9A60001,
+		0x81C, 0xE8A80001,
+		0x81C, 0xE7AA0001,
+		0x81C, 0xE6AC0001,
+		0x81C, 0xE5AE0001,
+		0x81C, 0xE4B00001,
+		0x81C, 0xE3B20001,
+		0x81C, 0xA8B40001,
+		0x81C, 0xA7B60001,
+		0x81C, 0xA6B80001,
+		0x81C, 0xA5BA0001,
+		0x81C, 0xA4BC0001,
+		0x81C, 0xA3BE0001,
+		0x81C, 0xA2C00001,
+		0x81C, 0xA1C20001,
+		0x81C, 0x68C40001,
+		0x81C, 0x67C60001,
+		0x81C, 0x66C80001,
+		0x81C, 0x65CA0001,
+		0x81C, 0x64CC0001,
+		0x81C, 0x47CE0001,
+		0x81C, 0x46D00001,
+		0x81C, 0x45D20001,
+		0x81C, 0x44D40001,
+		0x81C, 0x43D60001,
+		0x81C, 0x42D80001,
+		0x81C, 0x08DA0001,
+		0x81C, 0x07DC0001,
+		0x81C, 0x06DE0001,
+		0x81C, 0x05E00001,
+		0x81C, 0x04E20001,
+		0x81C, 0x03E40001,
+		0x81C, 0x02E60001,
+		0x81C, 0x01E80001,
+		0x81C, 0x01EA0001,
+		0x81C, 0x01EC0001,
+		0x81C, 0x01EE0001,
+		0x81C, 0x01F00001,
+		0x81C, 0x01F20001,
+		0x81C, 0x01F40001,
+		0x81C, 0x01F60001,
+		0x81C, 0x01F80001,
+		0x81C, 0x01FA0001,
+		0x81C, 0x01FC0001,
+		0x81C, 0x01FE0001,
+	0xFF0F02C0, 0xCDEF,
+		0x81C, 0xFC800001,
+		0x81C, 0xFB820001,
+		0x81C, 0xFA840001,
+		0x81C, 0xF9860001,
+		0x81C, 0xF8880001,
+		0x81C, 0xF78A0001,
+		0x81C, 0xF68C0001,
+		0x81C, 0xF58E0001,
+		0x81C, 0xF4900001,
+		0x81C, 0xF3920001,
+		0x81C, 0xF2940001,
+		0x81C, 0xF1960001,
+		0x81C, 0xF0980001,
+		0x81C, 0xEF9A0001,
+		0x81C, 0xEE9C0001,
+		0x81C, 0xED9E0001,
+		0x81C, 0xECA00001,
+		0x81C, 0xEBA20001,
+		0x81C, 0xEAA40001,
+		0x81C, 0xE9A60001,
+		0x81C, 0xE8A80001,
+		0x81C, 0xE7AA0001,
+		0x81C, 0xE6AC0001,
+		0x81C, 0xE5AE0001,
+		0x81C, 0xE4B00001,
+		0x81C, 0xE3B20001,
+		0x81C, 0xA8B40001,
+		0x81C, 0xA7B60001,
+		0x81C, 0xA6B80001,
+		0x81C, 0xA5BA0001,
+		0x81C, 0xA4BC0001,
+		0x81C, 0xA3BE0001,
+		0x81C, 0xA2C00001,
+		0x81C, 0xA1C20001,
+		0x81C, 0x68C40001,
+		0x81C, 0x67C60001,
+		0x81C, 0x66C80001,
+		0x81C, 0x65CA0001,
+		0x81C, 0x64CC0001,
+		0x81C, 0x47CE0001,
+		0x81C, 0x46D00001,
+		0x81C, 0x45D20001,
+		0x81C, 0x44D40001,
+		0x81C, 0x43D60001,
+		0x81C, 0x42D80001,
+		0x81C, 0x08DA0001,
+		0x81C, 0x07DC0001,
+		0x81C, 0x06DE0001,
+		0x81C, 0x05E00001,
+		0x81C, 0x04E20001,
+		0x81C, 0x03E40001,
+		0x81C, 0x02E60001,
+		0x81C, 0x01E80001,
+		0x81C, 0x01EA0001,
+		0x81C, 0x01EC0001,
+		0x81C, 0x01EE0001,
+		0x81C, 0x01F00001,
+		0x81C, 0x01F20001,
+		0x81C, 0x01F40001,
+		0x81C, 0x01F60001,
+		0x81C, 0x01F80001,
+		0x81C, 0x01FA0001,
+		0x81C, 0x01FC0001,
+		0x81C, 0x01FE0001,
+	0xFF0F07D8, 0xCDEF,
+		0x81C, 0xFC800001,
+		0x81C, 0xFB820001,
+		0x81C, 0xFA840001,
+		0x81C, 0xF9860001,
+		0x81C, 0xF8880001,
+		0x81C, 0xF78A0001,
+		0x81C, 0xF68C0001,
+		0x81C, 0xF58E0001,
+		0x81C, 0xF4900001,
+		0x81C, 0xF3920001,
+		0x81C, 0xF2940001,
+		0x81C, 0xF1960001,
+		0x81C, 0xF0980001,
+		0x81C, 0xEF9A0001,
+		0x81C, 0xEE9C0001,
+		0x81C, 0xED9E0001,
+		0x81C, 0xECA00001,
+		0x81C, 0xEBA20001,
+		0x81C, 0xEAA40001,
+		0x81C, 0xE9A60001,
+		0x81C, 0xE8A80001,
+		0x81C, 0xE7AA0001,
+		0x81C, 0xE6AC0001,
+		0x81C, 0xE5AE0001,
+		0x81C, 0xE4B00001,
+		0x81C, 0xE3B20001,
+		0x81C, 0xA8B40001,
+		0x81C, 0xA7B60001,
+		0x81C, 0xA6B80001,
+		0x81C, 0xA5BA0001,
+		0x81C, 0xA4BC0001,
+		0x81C, 0xA3BE0001,
+		0x81C, 0xA2C00001,
+		0x81C, 0xA1C20001,
+		0x81C, 0x68C40001,
+		0x81C, 0x67C60001,
+		0x81C, 0x66C80001,
+		0x81C, 0x65CA0001,
+		0x81C, 0x64CC0001,
+		0x81C, 0x47CE0001,
+		0x81C, 0x46D00001,
+		0x81C, 0x45D20001,
+		0x81C, 0x44D40001,
+		0x81C, 0x43D60001,
+		0x81C, 0x42D80001,
+		0x81C, 0x08DA0001,
+		0x81C, 0x07DC0001,
+		0x81C, 0x06DE0001,
+		0x81C, 0x05E00001,
+		0x81C, 0x04E20001,
+		0x81C, 0x03E40001,
+		0x81C, 0x02E60001,
+		0x81C, 0x01E80001,
+		0x81C, 0x01EA0001,
+		0x81C, 0x01EC0001,
+		0x81C, 0x01EE0001,
+		0x81C, 0x01F00001,
+		0x81C, 0x01F20001,
+		0x81C, 0x01F40001,
+		0x81C, 0x01F60001,
+		0x81C, 0x01F80001,
+		0x81C, 0x01FA0001,
+		0x81C, 0x01FC0001,
+		0x81C, 0x01FE0001,
+	0xFF0F07D0, 0xCDEF,
+		0x81C, 0xFC800001,
+		0x81C, 0xFB820001,
+		0x81C, 0xFA840001,
+		0x81C, 0xF9860001,
+		0x81C, 0xF8880001,
+		0x81C, 0xF78A0001,
+		0x81C, 0xF68C0001,
+		0x81C, 0xF58E0001,
+		0x81C, 0xF4900001,
+		0x81C, 0xF3920001,
+		0x81C, 0xF2940001,
+		0x81C, 0xF1960001,
+		0x81C, 0xF0980001,
+		0x81C, 0xEF9A0001,
+		0x81C, 0xEE9C0001,
+		0x81C, 0xED9E0001,
+		0x81C, 0xECA00001,
+		0x81C, 0xEBA20001,
+		0x81C, 0xEAA40001,
+		0x81C, 0xE9A60001,
+		0x81C, 0xE8A80001,
+		0x81C, 0xE7AA0001,
+		0x81C, 0xE6AC0001,
+		0x81C, 0xE5AE0001,
+		0x81C, 0xE4B00001,
+		0x81C, 0xE3B20001,
+		0x81C, 0xA8B40001,
+		0x81C, 0xA7B60001,
+		0x81C, 0xA6B80001,
+		0x81C, 0xA5BA0001,
+		0x81C, 0xA4BC0001,
+		0x81C, 0xA3BE0001,
+		0x81C, 0xA2C00001,
+		0x81C, 0xA1C20001,
+		0x81C, 0x68C40001,
+		0x81C, 0x67C60001,
+		0x81C, 0x66C80001,
+		0x81C, 0x65CA0001,
+		0x81C, 0x64CC0001,
+		0x81C, 0x47CE0001,
+		0x81C, 0x46D00001,
+		0x81C, 0x45D20001,
+		0x81C, 0x44D40001,
+		0x81C, 0x43D60001,
+		0x81C, 0x42D80001,
+		0x81C, 0x08DA0001,
+		0x81C, 0x07DC0001,
+		0x81C, 0x06DE0001,
+		0x81C, 0x05E00001,
+		0x81C, 0x04E20001,
+		0x81C, 0x03E40001,
+		0x81C, 0x02E60001,
+		0x81C, 0x01E80001,
+		0x81C, 0x01EA0001,
+		0x81C, 0x01EC0001,
+		0x81C, 0x01EE0001,
+		0x81C, 0x01F00001,
+		0x81C, 0x01F20001,
+		0x81C, 0x01F40001,
+		0x81C, 0x01F60001,
+		0x81C, 0x01F80001,
+		0x81C, 0x01FA0001,
+		0x81C, 0x01FC0001,
+		0x81C, 0x01FE0001,
+	0xCDCDCDCD, 0xCDCD,
+		0x81C, 0xFF800001,
+		0x81C, 0xFF820001,
+		0x81C, 0xFF840001,
+		0x81C, 0xFE860001,
+		0x81C, 0xFD880001,
+		0x81C, 0xFC8A0001,
+		0x81C, 0xFB8C0001,
+		0x81C, 0xFA8E0001,
+		0x81C, 0xF9900001,
+		0x81C, 0xF8920001,
+		0x81C, 0xF7940001,
+		0x81C, 0xF6960001,
+		0x81C, 0xF5980001,
+		0x81C, 0xF49A0001,
+		0x81C, 0xF39C0001,
+		0x81C, 0xF29E0001,
+		0x81C, 0xF1A00001,
+		0x81C, 0xF0A20001,
+		0x81C, 0xEFA40001,
+		0x81C, 0xEEA60001,
+		0x81C, 0xEDA80001,
+		0x81C, 0xECAA0001,
+		0x81C, 0xEBAC0001,
+		0x81C, 0xEAAE0001,
+		0x81C, 0xE9B00001,
+		0x81C, 0xE8B20001,
+		0x81C, 0xE7B40001,
+		0x81C, 0xE6B60001,
+		0x81C, 0xE5B80001,
+		0x81C, 0xE4BA0001,
+		0x81C, 0xE3BC0001,
+		0x81C, 0xA8BE0001,
+		0x81C, 0xA7C00001,
+		0x81C, 0xA6C20001,
+		0x81C, 0xA5C40001,
+		0x81C, 0xA4C60001,
+		0x81C, 0xA3C80001,
+		0x81C, 0xA2CA0001,
+		0x81C, 0xA1CC0001,
+		0x81C, 0x68CE0001,
+		0x81C, 0x67D00001,
+		0x81C, 0x66D20001,
+		0x81C, 0x65D40001,
+		0x81C, 0x64D60001,
+		0x81C, 0x47D80001,
+		0x81C, 0x46DA0001,
+		0x81C, 0x45DC0001,
+		0x81C, 0x44DE0001,
+		0x81C, 0x43E00001,
+		0x81C, 0x42E20001,
+		0x81C, 0x08E40001,
+		0x81C, 0x07E60001,
+		0x81C, 0x06E80001,
+		0x81C, 0x05EA0001,
+		0x81C, 0x04EC0001,
+		0x81C, 0x03EE0001,
+		0x81C, 0x02F00001,
+		0x81C, 0x01F20001,
+		0x81C, 0x01F40001,
+		0x81C, 0x01F60001,
+		0x81C, 0x01F80001,
+		0x81C, 0x01FA0001,
+		0x81C, 0x01FC0001,
+		0x81C, 0x01FE0001,
+	0xFF0F0180, 0xDEAD,
+		0xC50, 0x00000022,
+		0xC50, 0x00000020,
+		0xE50, 0x00000022,
+		0xE50, 0x00000020,
+};
+
+u32 RTL8821AE_AGC_TAB_ARRAY[] = {
+		0x81C, 0xBF000001,
+		0x81C, 0xBF020001,
+		0x81C, 0xBF040001,
+		0x81C, 0xBF060001,
+		0x81C, 0xBE080001,
+		0x81C, 0xBD0A0001,
+		0x81C, 0xBC0C0001,
+		0x81C, 0xBA0E0001,
+		0x81C, 0xB9100001,
+		0x81C, 0xB8120001,
+		0x81C, 0xB7140001,
+		0x81C, 0xB6160001,
+		0x81C, 0xB5180001,
+		0x81C, 0xB41A0001,
+		0x81C, 0xB31C0001,
+		0x81C, 0xB21E0001,
+		0x81C, 0xB1200001,
+		0x81C, 0xB0220001,
+		0x81C, 0xAF240001,
+		0x81C, 0xAE260001,
+		0x81C, 0xAD280001,
+		0x81C, 0xAC2A0001,
+		0x81C, 0xAB2C0001,
+		0x81C, 0xAA2E0001,
+		0x81C, 0xA9300001,
+		0x81C, 0xA8320001,
+		0x81C, 0xA7340001,
+		0x81C, 0xA6360001,
+		0x81C, 0xA5380001,
+		0x81C, 0xA43A0001,
+		0x81C, 0xA33C0001,
+		0x81C, 0x673E0001,
+		0x81C, 0x66400001,
+		0x81C, 0x65420001,
+		0x81C, 0x64440001,
+		0x81C, 0x63460001,
+		0x81C, 0x62480001,
+		0x81C, 0x614A0001,
+		0x81C, 0x474C0001,
+		0x81C, 0x464E0001,
+		0x81C, 0x45500001,
+		0x81C, 0x44520001,
+		0x81C, 0x43540001,
+		0x81C, 0x42560001,
+		0x81C, 0x41580001,
+		0x81C, 0x285A0001,
+		0x81C, 0x275C0001,
+		0x81C, 0x265E0001,
+		0x81C, 0x25600001,
+		0x81C, 0x24620001,
+		0x81C, 0x0A640001,
+		0x81C, 0x09660001,
+		0x81C, 0x08680001,
+		0x81C, 0x076A0001,
+		0x81C, 0x066C0001,
+		0x81C, 0x056E0001,
+		0x81C, 0x04700001,
+		0x81C, 0x03720001,
+		0x81C, 0x02740001,
+		0x81C, 0x01760001,
+		0x81C, 0x01780001,
+		0x81C, 0x017A0001,
+		0x81C, 0x017C0001,
+		0x81C, 0x017E0001,
+	0xFF0F02C0, 0xABCD,
+		0x81C, 0xFB000101,
+		0x81C, 0xFA020101,
+		0x81C, 0xF9040101,
+		0x81C, 0xF8060101,
+		0x81C, 0xF7080101,
+		0x81C, 0xF60A0101,
+		0x81C, 0xF50C0101,
+		0x81C, 0xF40E0101,
+		0x81C, 0xF3100101,
+		0x81C, 0xF2120101,
+		0x81C, 0xF1140101,
+		0x81C, 0xF0160101,
+		0x81C, 0xEF180101,
+		0x81C, 0xEE1A0101,
+		0x81C, 0xED1C0101,
+		0x81C, 0xEC1E0101,
+		0x81C, 0xEB200101,
+		0x81C, 0xEA220101,
+		0x81C, 0xE9240101,
+		0x81C, 0xE8260101,
+		0x81C, 0xE7280101,
+		0x81C, 0xE62A0101,
+		0x81C, 0xE52C0101,
+		0x81C, 0xE42E0101,
+		0x81C, 0xE3300101,
+		0x81C, 0xA5320101,
+		0x81C, 0xA4340101,
+		0x81C, 0xA3360101,
+		0x81C, 0x87380101,
+		0x81C, 0x863A0101,
+		0x81C, 0x853C0101,
+		0x81C, 0x843E0101,
+		0x81C, 0x69400101,
+		0x81C, 0x68420101,
+		0x81C, 0x67440101,
+		0x81C, 0x66460101,
+		0x81C, 0x49480101,
+		0x81C, 0x484A0101,
+		0x81C, 0x474C0101,
+		0x81C, 0x2A4E0101,
+		0x81C, 0x29500101,
+		0x81C, 0x28520101,
+		0x81C, 0x27540101,
+		0x81C, 0x26560101,
+		0x81C, 0x25580101,
+		0x81C, 0x245A0101,
+		0x81C, 0x235C0101,
+		0x81C, 0x055E0101,
+		0x81C, 0x04600101,
+		0x81C, 0x03620101,
+		0x81C, 0x02640101,
+		0x81C, 0x01660101,
+		0x81C, 0x01680101,
+		0x81C, 0x016A0101,
+		0x81C, 0x016C0101,
+		0x81C, 0x016E0101,
+		0x81C, 0x01700101,
+		0x81C, 0x01720101,
+	0xCDCDCDCD, 0xCDCD,
+		0x81C, 0xFF000101,
+		0x81C, 0xFF020101,
+		0x81C, 0xFE040101,
+		0x81C, 0xFD060101,
+		0x81C, 0xFC080101,
+		0x81C, 0xFD0A0101,
+		0x81C, 0xFC0C0101,
+		0x81C, 0xFB0E0101,
+		0x81C, 0xFA100101,
+		0x81C, 0xF9120101,
+		0x81C, 0xF8140101,
+		0x81C, 0xF7160101,
+		0x81C, 0xF6180101,
+		0x81C, 0xF51A0101,
+		0x81C, 0xF41C0101,
+		0x81C, 0xF31E0101,
+		0x81C, 0xF2200101,
+		0x81C, 0xF1220101,
+		0x81C, 0xF0240101,
+		0x81C, 0xEF260101,
+		0x81C, 0xEE280101,
+		0x81C, 0xED2A0101,
+		0x81C, 0xEC2C0101,
+		0x81C, 0xEB2E0101,
+		0x81C, 0xEA300101,
+		0x81C, 0xE9320101,
+		0x81C, 0xE8340101,
+		0x81C, 0xE7360101,
+		0x81C, 0xE6380101,
+		0x81C, 0xE53A0101,
+		0x81C, 0xE43C0101,
+		0x81C, 0xE33E0101,
+		0x81C, 0xA5400101,
+		0x81C, 0xA4420101,
+		0x81C, 0xA3440101,
+		0x81C, 0x87460101,
+		0x81C, 0x86480101,
+		0x81C, 0x854A0101,
+		0x81C, 0x844C0101,
+		0x81C, 0x694E0101,
+		0x81C, 0x68500101,
+		0x81C, 0x67520101,
+		0x81C, 0x66540101,
+		0x81C, 0x49560101,
+		0x81C, 0x48580101,
+		0x81C, 0x475A0101,
+		0x81C, 0x2A5C0101,
+		0x81C, 0x295E0101,
+		0x81C, 0x28600101,
+		0x81C, 0x27620101,
+		0x81C, 0x26640101,
+		0x81C, 0x25660101,
+		0x81C, 0x24680101,
+		0x81C, 0x236A0101,
+		0x81C, 0x056C0101,
+		0x81C, 0x046E0101,
+		0x81C, 0x03700101,
+		0x81C, 0x02720101,
+	0xFF0F02C0, 0xDEAD,
+		0x81C, 0x01740101,
+		0x81C, 0x01760101,
+		0x81C, 0x01780101,
+		0x81C, 0x017A0101,
+		0x81C, 0x017C0101,
+		0x81C, 0x017E0101,
+		0xC50, 0x00000022,
+		0xC50, 0x00000020,
+
+};
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/table.h b/drivers/staging/rtl8821ae/rtl8821ae/table.h
new file mode 100644
index 000000000000..b9d7b266a33a
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/table.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.
+ *
+ * You should have 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 __RTL8821AE_TABLE__H_
+#define __RTL8821AE_TABLE__H_
+
+#include <linux/types.h>
+#define  RTL8821AEPHY_REG_1TARRAYLEN	344
+extern u32 RTL8821AE_PHY_REG_ARRAY[];
+#define  RTL8812AEPHY_REG_1TARRAYLEN	490
+extern u32 RTL8812AE_PHY_REG_ARRAY[];
+#define RTL8821AEPHY_REG_ARRAY_PGLEN 	90
+extern u32 RTL8821AE_PHY_REG_ARRAY_PG[];
+#define RTL8812AEPHY_REG_ARRAY_PGLEN 	276
+extern u32 RTL8812AE_PHY_REG_ARRAY_PG[];
+//#define	RTL8723BE_RADIOA_1TARRAYLEN 	206
+//extern u8 *RTL8821AE_TXPWR_LMT_ARRAY[];
+#define	RTL8812AE_RADIOA_1TARRAYLEN 	1264
+extern u32 RTL8812AE_RADIOA_ARRAY[];
+#define	RTL8812AE_RADIOB_1TARRAYLEN 	1240
+extern u32 RTL8812AE_RADIOB_ARRAY[];
+#define	RTL8821AE_RADIOA_1TARRAYLEN 	1176
+extern u32 RTL8821AE_RADIOA_ARRAY[];
+#define RTL8821AEMAC_1T_ARRAYLEN  		194
+extern u32 RTL8821AE_MAC_REG_ARRAY[];
+#define RTL8812AEMAC_1T_ARRAYLEN  		214
+extern u32 RTL8812AE_MAC_REG_ARRAY[];
+#define RTL8821AEAGCTAB_1TARRAYLEN 		382
+extern u32 RTL8821AE_AGC_TAB_ARRAY[];
+#define RTL8812AEAGCTAB_1TARRAYLEN 		1312
+extern u32 RTL8812AE_AGC_TAB_ARRAY[];
+
+
+#endif
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/trx.c b/drivers/staging/rtl8821ae/rtl8821ae/trx.c
new file mode 100644
index 000000000000..963b55f661c8
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/trx.c
@@ -0,0 +1,1050 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../pci.h"
+#include "../base.h"
+#include "../stats.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "trx.h"
+#include "led.h"
+#include "dm.h"
+#include "phy.h"
+u8 _rtl8821ae_map_hwqueue_to_fwqueue(struct sk_buff *skb, u8 hw_queue)
+{
+	u16 fc = rtl_get_fc(skb);
+
+	if (unlikely(ieee80211_is_beacon(fc)))
+		return QSLT_BEACON;
+	if (ieee80211_is_mgmt(fc) || ieee80211_is_ctl(fc))
+		return QSLT_MGNT;
+
+	return skb->priority;
+}
+
+/* mac80211's rate_idx is like this:
+ *
+ * 2.4G band:rx_status->band == IEEE80211_BAND_2GHZ
+ *
+ * B/G rate:
+ * (rx_status->flag & RX_FLAG_HT) = 0,
+ * DESC_RATE1M-->DESC_RATE54M ==> idx is 0-->11,
+ *
+ * N rate:
+ * (rx_status->flag & RX_FLAG_HT) = 1,
+ * DESC_RATEMCS0-->DESC_RATEMCS15 ==> idx is 0-->15
+ *
+ * 5G band:rx_status->band == IEEE80211_BAND_5GHZ
+ * A rate:
+ * (rx_status->flag & RX_FLAG_HT) = 0,
+ * DESC_RATE6M-->DESC_RATE54M ==> idx is 0-->7,
+ *
+ * N rate:
+ * (rx_status->flag & RX_FLAG_HT) = 1,
+ * DESC_RATEMCS0-->DESC_RATEMCS15 ==> idx is 0-->15
+ */
+static int _rtl8821ae_rate_mapping(struct ieee80211_hw *hw,
+	bool isht, u8 desc_rate)
+{
+	int rate_idx;
+
+	if (false == isht) {
+#if (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;
+				break;
+			case DESC_RATE2M:
+				rate_idx = 1;
+				break;
+			case DESC_RATE5_5M:
+				rate_idx = 2;
+				break;
+			case DESC_RATE11M:
+				rate_idx = 3;
+				break;
+			case DESC_RATE6M:
+				rate_idx = 4;
+				break;
+			case DESC_RATE9M:
+				rate_idx = 5;
+				break;
+			case DESC_RATE12M:
+				rate_idx = 6;
+				break;
+			case DESC_RATE18M:
+				rate_idx = 7;
+				break;
+			case DESC_RATE24M:
+				rate_idx = 8;
+				break;
+			case DESC_RATE36M:
+				rate_idx = 9;
+				break;
+			case DESC_RATE48M:
+				rate_idx = 10;
+				break;
+			case DESC_RATE54M:
+				rate_idx = 11;
+				break;
+			default:
+				rate_idx = 0;
+				break;
+			}
+		} else {
+			switch (desc_rate) {
+			case DESC_RATE6M:
+				rate_idx = 0;
+				break;
+			case DESC_RATE9M:
+				rate_idx = 1;
+				break;
+			case DESC_RATE12M:
+				rate_idx = 2;
+				break;
+			case DESC_RATE18M:
+				rate_idx = 3;
+				break;
+			case DESC_RATE24M:
+				rate_idx = 4;
+				break;
+			case DESC_RATE36M:
+				rate_idx = 5;
+				break;
+			case DESC_RATE48M:
+				rate_idx = 6;
+				break;
+			case DESC_RATE54M:
+				rate_idx = 7;
+				break;
+			default:
+				rate_idx = 0;
+				break;
+			}
+		}
+	} else {
+		switch(desc_rate) {
+		case DESC_RATEMCS0:
+			rate_idx = 0;
+			break;
+		case DESC_RATEMCS1:
+			rate_idx = 1;
+			break;
+		case DESC_RATEMCS2:
+			rate_idx = 2;
+			break;
+		case DESC_RATEMCS3:
+			rate_idx = 3;
+			break;
+		case DESC_RATEMCS4:
+			rate_idx = 4;
+			break;
+		case DESC_RATEMCS5:
+			rate_idx = 5;
+			break;
+		case DESC_RATEMCS6:
+			rate_idx = 6;
+			break;
+		case DESC_RATEMCS7:
+			rate_idx = 7;
+			break;
+		case DESC_RATEMCS8:
+			rate_idx = 8;
+			break;
+		case DESC_RATEMCS9:
+			rate_idx = 9;
+			break;
+		case DESC_RATEMCS10:
+			rate_idx = 10;
+			break;
+		case DESC_RATEMCS11:
+			rate_idx = 11;
+			break;
+		case DESC_RATEMCS12:
+			rate_idx = 12;
+			break;
+		case DESC_RATEMCS13:
+			rate_idx = 13;
+			break;
+		case DESC_RATEMCS14:
+			rate_idx = 14;
+			break;
+		case DESC_RATEMCS15:
+			rate_idx = 15;
+			break;
+		default:
+			rate_idx = 0;
+			break;
+		}
+	}
+	return rate_idx;
+}
+
+static void _rtl8821ae_query_rxphystatus(struct ieee80211_hw *hw,
+			struct rtl_stats *pstatus, u8 *pdesc,
+			struct rx_fwinfo_8821ae *p_drvinfo, bool bpacket_match_bssid,
+			bool bpacket_toself, bool b_packet_beacon)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtlpriv);
+	struct phy_sts_cck_8821ae_t *cck_buf;
+	struct phy_status_rpt *p_phystRpt = (struct phy_status_rpt *)p_drvinfo;
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	char rx_pwr_all = 0, rx_pwr[4];
+	u8 rf_rx_num = 0, evm, pwdb_all;
+	u8 i, max_spatial_stream;
+	u32 rssi, total_rssi = 0;
+	bool b_is_cck = pstatus->b_is_cck;
+	u8 lan_idx,vga_idx;
+
+	/* Record it for next packet processing */
+	pstatus->b_packet_matchbssid = bpacket_match_bssid;
+	pstatus->b_packet_toself = bpacket_toself;
+	pstatus->b_packet_beacon = b_packet_beacon;
+	pstatus->rx_mimo_signalquality[0] = -1;
+	pstatus->rx_mimo_signalquality[1] = -1;
+
+	if (b_is_cck) {
+		u8 cck_highpwr;
+		u8 cck_agc_rpt;
+		/* CCK Driver info Structure is not the same as OFDM packet. */
+		cck_buf = (struct phy_sts_cck_8821ae_t *)p_drvinfo;
+		cck_agc_rpt = cck_buf->cck_agc_rpt;
+
+		/* (1)Hardware does not provide RSSI for CCK */
+		/* (2)PWDB, Average PWDB cacluated by
+		 * hardware (for rate adaptive) */
+		if (ppsc->rfpwr_state == ERFON)
+			cck_highpwr = (u8) rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2,
+							 BIT(9));
+		else
+			cck_highpwr = false;
+
+		lan_idx = ((cck_agc_rpt & 0xE0) >> 5);
+		vga_idx = (cck_agc_rpt & 0x1f);
+		switch (lan_idx) {
+			case 7:
+				if(vga_idx <= 27)
+					rx_pwr_all = -100 + 2*(27-vga_idx); /*VGA_idx = 27~2*/
+				else
+					rx_pwr_all = -100;
+				break;
+			case 6:
+				rx_pwr_all = -48 + 2*(2-vga_idx); /*VGA_idx = 2~0*/
+				break;
+			case 5:
+				rx_pwr_all = -42 + 2*(7-vga_idx); /*VGA_idx = 7~5*/
+				break;
+			case 4:
+				rx_pwr_all = -36 + 2*(7-vga_idx); /*VGA_idx = 7~4*/
+				break;
+			case 3:
+				rx_pwr_all = -24 + 2*(7-vga_idx); /*VGA_idx = 7~0*/
+				break;
+			case 2:
+				if(cck_highpwr)
+					rx_pwr_all = -12 + 2*(5-vga_idx); /*VGA_idx = 5~0*/
+				else
+					rx_pwr_all = -6+ 2*(5-vga_idx);
+				break;
+			case 1:
+				rx_pwr_all = 8-2*vga_idx;
+				break;
+			case 0:
+				rx_pwr_all = 14-2*vga_idx;
+				break;
+			default:
+				break;
+		}
+		rx_pwr_all += 6;
+		pwdb_all = rtl_query_rxpwrpercentage(rx_pwr_all);
+		/* CCK gain is smaller than OFDM/MCS gain,  */
+		/* so we add gain diff by experiences,
+		 * the val is 6 */
+		pwdb_all += 6;
+		if(pwdb_all > 100)
+			pwdb_all = 100;
+		/* modify the offset to make the same
+		 * gain index with OFDM. */
+		if(pwdb_all > 34 && pwdb_all <= 42)
+			pwdb_all -= 2;
+		else if(pwdb_all > 26 && pwdb_all <= 34)
+			pwdb_all -= 6;
+		else if(pwdb_all > 14 && pwdb_all <= 26)
+			pwdb_all -= 8;
+		else if(pwdb_all > 4 && pwdb_all <= 14)
+			pwdb_all -= 4;
+		if (cck_highpwr == false){
+			if (pwdb_all >= 80)
+				pwdb_all =((pwdb_all-80)<<1)+((pwdb_all-80)>>1)+80;
+			else if((pwdb_all <= 78) && (pwdb_all >= 20))
+				pwdb_all += 3;
+			if(pwdb_all>100)
+				pwdb_all = 100;
+		}
+
+		pstatus->rx_pwdb_all = pwdb_all;
+		pstatus->recvsignalpower = rx_pwr_all;
+
+		/* (3) Get Signal Quality (EVM) */
+		if (bpacket_match_bssid) {
+			u8 sq;
+
+			if (pstatus->rx_pwdb_all > 40)
+				sq = 100;
+			else {
+				sq = cck_buf->sq_rpt;
+				if (sq > 64)
+					sq = 0;
+				else if (sq < 20)
+					sq = 100;
+				else
+					sq = ((64 - sq) * 100) / 44;
+			}
+
+			pstatus->signalquality = sq;
+			pstatus->rx_mimo_signalquality[0] = sq;
+			pstatus->rx_mimo_signalquality[1] = -1;
+		}
+	} else {
+		rtlpriv->dm.brfpath_rxenable[0] =
+		    rtlpriv->dm.brfpath_rxenable[1] = true;
+
+		/* (1)Get RSSI for HT rate */
+		for (i = RF90_PATH_A; i < RF6052_MAX_PATH; i++) {
+
+			/* we will judge RF RX path now. */
+			if (rtlpriv->dm.brfpath_rxenable[i])
+				rf_rx_num++;
+
+			rx_pwr[i] = ((p_drvinfo->gain_trsw[i] & 0x3f) * 2) - 110;
+
+			/* Translate DBM to percentage. */
+			rssi = rtl_query_rxpwrpercentage(rx_pwr[i]);
+			total_rssi += rssi;
+
+			/* Get Rx snr value in DB */
+			rtlpriv->stats.rx_snr_db[i] = (long)(p_drvinfo->rxsnr[i] / 2);
+
+			/* Record Signal Strength for next packet */
+			if (bpacket_match_bssid)
+				pstatus->rx_mimo_signalstrength[i] = (u8) rssi;
+		}
+
+		/* (2)PWDB, Average PWDB cacluated by
+		 * hardware (for rate adaptive) */
+		rx_pwr_all = ((p_drvinfo->pwdb_all >> 1) & 0x7f) - 110;
+
+		pwdb_all = rtl_query_rxpwrpercentage(rx_pwr_all);
+		pstatus->rx_pwdb_all = pwdb_all;
+		pstatus->rxpower = rx_pwr_all;
+		pstatus->recvsignalpower = rx_pwr_all;
+
+		/* (3)EVM of HT rate */
+		if (pstatus->b_is_ht && pstatus->rate >= DESC_RATEMCS8 &&
+		    pstatus->rate <= DESC_RATEMCS15)
+			max_spatial_stream = 2;
+		else
+			max_spatial_stream = 1;
+
+		for (i = 0; i < max_spatial_stream; i++) {
+			evm = rtl_evm_db_to_percentage(p_drvinfo->rxevm[i]);
+
+			if (bpacket_match_bssid) {
+				/* Fill value in RFD, Get the first
+				 * spatial stream only */
+				if (i == 0)
+					pstatus->signalquality = (u8) (evm & 0xff);
+				pstatus->rx_mimo_signalquality[i] = (u8) (evm & 0xff);
+			}
+		}
+	}
+
+	/* UI BSS List signal strength(in percentage),
+	 * make it good looking, from 0~100. */
+	if (b_is_cck)
+		pstatus->signalstrength = (u8)(rtl_signal_scale_mapping(hw,
+			pwdb_all));
+	else if (rf_rx_num != 0)
+		pstatus->signalstrength = (u8)(rtl_signal_scale_mapping(hw,
+			total_rssi /= rf_rx_num));
+	/*HW antenna diversity*/
+	rtldm->fat_table.antsel_rx_keep_0 = p_phystRpt->ant_sel;
+	rtldm->fat_table.antsel_rx_keep_1 = p_phystRpt->ant_sel_b;
+	rtldm->fat_table.antsel_rx_keep_2 = p_phystRpt->antsel_rx_keep_2;
+
+}
+#if 0
+static void _rtl8821ae_smart_antenna(struct ieee80211_hw *hw,
+	struct rtl_stats *pstatus)
+{
+	struct rtl_dm *rtldm= rtl_dm(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse =rtl_efuse(rtl_priv(hw));
+	u8 antsel_tr_mux;
+	struct fast_ant_trainning *pfat_table = &(rtldm->fat_table);
+
+	if (rtlefuse->antenna_div_type == CG_TRX_SMART_ANTDIV) {
+		if (pfat_table->fat_state == FAT_TRAINING_STATE) {
+			if (pstatus->b_packet_toself) {
+				antsel_tr_mux = (pfat_table->antsel_rx_keep_2 << 2) |
+					(pfat_table->antsel_rx_keep_1 << 1) | pfat_table->antsel_rx_keep_0;
+				pfat_table->ant_sum_rssi[antsel_tr_mux] += pstatus->rx_pwdb_all;
+				pfat_table->ant_rssi_cnt[antsel_tr_mux]++;
+			}
+		}
+	} else if ((rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV) ||
+	(rtlefuse->antenna_div_type == CGCS_RX_HW_ANTDIV)) {
+		if (pstatus->b_packet_toself || pstatus->b_packet_matchbssid) {
+			antsel_tr_mux = (pfat_table->antsel_rx_keep_2 << 2) |
+					(pfat_table->antsel_rx_keep_1 << 1) | pfat_table->antsel_rx_keep_0;
+			rtl8821ae_dm_ant_sel_statistics(hw, antsel_tr_mux, 0, pstatus->rx_pwdb_all);
+		}
+
+	}
+}
+#endif
+static void _rtl8821ae_translate_rx_signal_stuff(struct ieee80211_hw *hw,
+		struct sk_buff *skb, struct rtl_stats *pstatus,
+		u8 *pdesc, struct rx_fwinfo_8821ae *p_drvinfo)
+{
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct ieee80211_hdr *hdr;
+	u8 *tmp_buf;
+	u8 *praddr;
+	u8 *psaddr;
+	u16 fc, type;
+	bool b_packet_matchbssid, b_packet_toself, b_packet_beacon;
+
+	tmp_buf = skb->data + pstatus->rx_drvinfo_size + pstatus->rx_bufshift;
+
+	hdr = (struct ieee80211_hdr *)tmp_buf;
+	fc = le16_to_cpu(hdr->frame_control);
+	type = WLAN_FC_GET_TYPE(fc);
+	praddr = hdr->addr1;
+	psaddr = ieee80211_get_SA(hdr);
+	memcpy(pstatus->psaddr, psaddr, ETH_ALEN);
+
+	b_packet_matchbssid = ((IEEE80211_FTYPE_CTL != type) &&
+	     (!ether_addr_equal(mac->bssid, (fc & IEEE80211_FCTL_TODS) ?
+				  hdr->addr1 : (fc & IEEE80211_FCTL_FROMDS) ?
+				  hdr->addr2 : hdr->addr3)) && (!pstatus->b_hwerror) &&
+				  (!pstatus->b_crc) && (!pstatus->b_icv));
+
+	b_packet_toself = b_packet_matchbssid &&
+	    (!ether_addr_equal(praddr, rtlefuse->dev_addr));
+
+	if (ieee80211_is_beacon(fc))
+		b_packet_beacon = true;
+	else
+		b_packet_beacon = false;
+
+	if (b_packet_beacon && b_packet_matchbssid)
+		rtl_priv(hw)->dm.dbginfo.num_qry_beacon_pkt++;
+
+	_rtl8821ae_query_rxphystatus(hw, pstatus, pdesc, p_drvinfo,
+				   b_packet_matchbssid, b_packet_toself,
+				   b_packet_beacon);
+	/*_rtl8821ae_smart_antenna(hw, pstatus); */
+	rtl_process_phyinfo(hw, tmp_buf, pstatus);
+}
+
+static void _rtl8821ae_insert_emcontent(struct rtl_tcb_desc *ptcb_desc,
+				      u8 *virtualaddress)
+{
+	u32 dwtmp = 0;
+	memset(virtualaddress, 0, 8);
+
+	SET_EARLYMODE_PKTNUM(virtualaddress, ptcb_desc->empkt_num);
+	if (ptcb_desc->empkt_num == 1)
+		dwtmp = ptcb_desc->empkt_len[0];
+	else {
+		dwtmp = ptcb_desc->empkt_len[0];
+		dwtmp += ((dwtmp%4)?(4-dwtmp%4):0)+4;
+		dwtmp += ptcb_desc->empkt_len[1];
+	}
+	SET_EARLYMODE_LEN0(virtualaddress, dwtmp);
+
+	if (ptcb_desc->empkt_num <= 3)
+		dwtmp = ptcb_desc->empkt_len[2];
+	else {
+		dwtmp = ptcb_desc->empkt_len[2];
+		dwtmp += ((dwtmp%4)?(4-dwtmp%4):0)+4;
+		dwtmp += ptcb_desc->empkt_len[3];
+	}
+	SET_EARLYMODE_LEN1(virtualaddress, dwtmp);
+	if (ptcb_desc->empkt_num <= 5)
+		dwtmp = ptcb_desc->empkt_len[4];
+	else {
+		dwtmp = ptcb_desc->empkt_len[4];
+		dwtmp += ((dwtmp%4)?(4-dwtmp%4):0)+4;
+		dwtmp += ptcb_desc->empkt_len[5];
+	}
+	SET_EARLYMODE_LEN2_1(virtualaddress, dwtmp & 0xF);
+	SET_EARLYMODE_LEN2_2(virtualaddress, dwtmp >> 4);
+	if (ptcb_desc->empkt_num <= 7)
+		dwtmp = ptcb_desc->empkt_len[6];
+	else {
+		dwtmp = ptcb_desc->empkt_len[6];
+		dwtmp += ((dwtmp%4)?(4-dwtmp%4):0)+4;
+		dwtmp += ptcb_desc->empkt_len[7];
+	}
+	SET_EARLYMODE_LEN3(virtualaddress, dwtmp);
+	if (ptcb_desc->empkt_num <= 9)
+		dwtmp = ptcb_desc->empkt_len[8];
+	else {
+		dwtmp = ptcb_desc->empkt_len[8];
+		dwtmp += ((dwtmp%4)?(4-dwtmp%4):0)+4;
+		dwtmp += ptcb_desc->empkt_len[9];
+	}
+	SET_EARLYMODE_LEN4(virtualaddress, dwtmp);
+}
+
+bool rtl8821ae_rx_query_desc(struct ieee80211_hw *hw,
+			   struct rtl_stats *status,
+			   struct ieee80211_rx_status *rx_status,
+			   u8 *pdesc, struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rx_fwinfo_8821ae *p_drvinfo;
+	struct ieee80211_hdr *hdr;
+
+	u32 phystatus = GET_RX_DESC_PHYST(pdesc);
+
+	status->length = (u16) GET_RX_DESC_PKT_LEN(pdesc);
+	status->rx_drvinfo_size = (u8) GET_RX_DESC_DRV_INFO_SIZE(pdesc) *
+	    RX_DRV_INFO_SIZE_UNIT;
+	status->rx_bufshift = (u8) (GET_RX_DESC_SHIFT(pdesc) & 0x03);
+	status->b_icv = (u16) GET_RX_DESC_ICV(pdesc);
+	status->b_crc = (u16) GET_RX_DESC_CRC32(pdesc);
+	status->b_hwerror = (status->b_crc | status->b_icv);
+	status->decrypted = !GET_RX_DESC_SWDEC(pdesc);
+	status->rate = (u8) GET_RX_DESC_RXMCS(pdesc);
+	status->b_shortpreamble = (u16) GET_RX_DESC_SPLCP(pdesc);
+	status->b_isampdu = (bool) (GET_RX_DESC_PAGGR(pdesc) == 1);
+	status->b_isfirst_ampdu = (bool) (GET_RX_DESC_PAGGR(pdesc) == 1);
+	status->timestamp_low = GET_RX_DESC_TSFL(pdesc);
+	status->rx_is40Mhzpacket = (bool) GET_RX_DESC_BW(pdesc);
+	status->macid = GET_RX_DESC_MACID(pdesc);
+	status->b_is_ht = (bool)GET_RX_DESC_RXHT(pdesc);
+
+	status->b_is_cck = RX_HAL_IS_CCK_RATE(status->rate);
+
+	if (GET_RX_STATUS_DESC_RPT_SEL(pdesc))
+		status->packet_report_type = C2H_PACKET;
+	else
+		status->packet_report_type = NORMAL_RX;
+
+	if (GET_RX_STATUS_DESC_PATTERN_MATCH(pdesc))
+		status->wake_match = BIT(2);
+	else if (GET_RX_STATUS_DESC_MAGIC_MATCH(pdesc))
+		status->wake_match = BIT(1);
+	else if (GET_RX_STATUS_DESC_UNICAST_MATCH(pdesc))
+		status->wake_match = BIT(0);
+	else
+		status->wake_match = 0;
+
+	if (status->wake_match)
+		RT_TRACE(COMP_RXDESC,DBG_LOUD,
+		("GGGGGGGGGGGGGet Wakeup Packet!! WakeMatch=%d\n",status->wake_match ));
+#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);
+
+	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 = _rtl8821ae_rate_mapping(hw,
+				status->b_is_ht, status->rate);
+
+	rx_status->mactime = status->timestamp_low;
+	if (phystatus == true) {
+		p_drvinfo = (struct rx_fwinfo_8821ae *)(skb->data +
+						     status->rx_bufshift);
+
+		_rtl8821ae_translate_rx_signal_stuff(hw,
+						   skb, status, pdesc,
+						   p_drvinfo);
+	}
+
+	/*rx_status->qual = status->signal; */
+	rx_status->signal = status->recvsignalpower + 10;
+	/*rx_status->noise = -status->noise; */
+	if (status->packet_report_type == TX_REPORT2){
+		status->macid_valid_entry[0] = GET_RX_RPT2_DESC_MACID_VALID_1(pdesc);
+		status->macid_valid_entry[1] = GET_RX_RPT2_DESC_MACID_VALID_2(pdesc);
+	}
+	return true;
+}
+
+/*<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);
+	unsigned int buf_len = 0;
+	unsigned int skb_len = skb->len;
+	u8 fw_qsel = _rtl8821ae_map_hwqueue_to_fwqueue(skb, hw_queue);
+	bool b_firstseg = ((hdr->seq_ctrl &
+			    cpu_to_le16(IEEE80211_SCTL_FRAG)) == 0);
+	bool b_lastseg = ((hdr->frame_control &
+			   cpu_to_le16(IEEE80211_FCTL_MOREFRAGS)) == 0);
+	dma_addr_t mapping;
+	u8 bw_40 = 0;
+	u8 short_gi = 0;
+
+	if (mac->opmode == NL80211_IFTYPE_STATION) {
+		bw_40 = mac->bw_40;
+	} else if (mac->opmode == NL80211_IFTYPE_AP ||
+		mac->opmode == NL80211_IFTYPE_ADHOC) {
+		if (sta)
+			bw_40 = sta->ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40;
+	}
+	seq_number = (le16_to_cpu(hdr->seq_ctrl) & IEEE80211_SCTL_SEQ) >> 4;
+	rtl_get_tcb_desc(hw, info, sta, skb, ptcb_desc);
+	/* reserve 8 byte for AMPDU early mode */
+	if (rtlhal->b_earlymode_enable) {
+		skb_push(skb, EM_HDR_LEN);
+		memset(skb->data, 0, EM_HDR_LEN);
+	}
+	buf_len = skb->len;
+	mapping = pci_map_single(rtlpci->pdev, skb->data, skb->len,
+				 PCI_DMA_TODEVICE);
+	if (pci_dma_mapping_error(rtlpci->pdev, mapping)) {
+		RT_TRACE(COMP_SEND, DBG_TRACE,
+			 ("DMA mapping error"));
+		return;
+	}
+	CLEAR_PCI_TX_DESC_CONTENT(pdesc, sizeof(struct tx_desc_8821ae));
+	if (ieee80211_is_nullfunc(fc) || ieee80211_is_ctl(fc)) {
+		b_firstseg = true;
+		b_lastseg = true;
+	}
+	if (b_firstseg) {
+		if (rtlhal->b_earlymode_enable) {
+			SET_TX_DESC_PKT_OFFSET(pdesc, 1);
+			SET_TX_DESC_OFFSET(pdesc, USB_HWDESC_HEADER_LEN + EM_HDR_LEN);
+			if (ptcb_desc->empkt_num) {
+				RT_TRACE(COMP_SEND, DBG_TRACE,
+					 ("Insert 8 byte.pTcb->EMPktNum:%d\n",
+					  ptcb_desc->empkt_num));
+				_rtl8821ae_insert_emcontent(ptcb_desc, (u8 *)(skb->data));
+			}
+		} else {
+			SET_TX_DESC_OFFSET(pdesc, USB_HWDESC_HEADER_LEN);
+		}
+
+		/* ptcb_desc->use_driver_rate = true; */
+		SET_TX_DESC_TX_RATE(pdesc, ptcb_desc->hw_rate);
+		if (ptcb_desc->hw_rate > DESC_RATEMCS0) {
+			short_gi = (ptcb_desc->use_shortgi) ? 1 : 0;
+		} else {
+			short_gi = (ptcb_desc->use_shortpreamble) ? 1 :0;
+		}
+		SET_TX_DESC_DATA_SHORTGI(pdesc, short_gi);
+
+		if (info->flags & IEEE80211_TX_CTL_AMPDU) {
+			SET_TX_DESC_AGG_ENABLE(pdesc, 1);
+			SET_TX_DESC_MAX_AGG_NUM(pdesc, 0x14);
+		}
+		SET_TX_DESC_SEQ(pdesc, seq_number);
+		SET_TX_DESC_RTS_ENABLE(pdesc, ((ptcb_desc->b_rts_enable &&
+						!ptcb_desc->b_cts_enable) ? 1 : 0));
+		SET_TX_DESC_HW_RTS_ENABLE(pdesc,0);
+		SET_TX_DESC_CTS2SELF(pdesc, ((ptcb_desc->b_cts_enable) ? 1 : 0));
+	/*	SET_TX_DESC_RTS_STBC(pdesc, ((ptcb_desc->b_rts_stbc) ? 1 : 0));*/
+
+		SET_TX_DESC_RTS_RATE(pdesc, ptcb_desc->rts_rate);
+	/*	SET_TX_DESC_RTS_BW(pdesc, 0);*/
+		SET_TX_DESC_RTS_SC(pdesc, ptcb_desc->rts_sc);
+		SET_TX_DESC_RTS_SHORT(pdesc, ((ptcb_desc->rts_rate <= DESC_RATE54M) ?
+			(ptcb_desc->b_rts_use_shortpreamble ? 1 : 0) :
+			(ptcb_desc->b_rts_use_shortgi ? 1 : 0)));
+
+		if(ptcb_desc->btx_enable_sw_calc_duration)
+			SET_TX_DESC_NAV_USE_HDR(pdesc, 1);
+
+		if (bw_40) {
+			if (ptcb_desc->b_packet_bw) {
+				SET_TX_DESC_DATA_BW(pdesc, 1);
+				SET_TX_DESC_TX_SUB_CARRIER(pdesc, 3);
+			} else {
+				SET_TX_DESC_DATA_BW(pdesc, 0);
+				SET_TX_DESC_TX_SUB_CARRIER(pdesc, mac->cur_40_prime_sc);
+			}
+		} else {
+			SET_TX_DESC_DATA_BW(pdesc, 0);
+			SET_TX_DESC_TX_SUB_CARRIER(pdesc, 0);
+		}
+
+		SET_TX_DESC_LINIP(pdesc, 0);
+		SET_TX_DESC_PKT_SIZE(pdesc, (u16) skb_len);
+		if (sta) {
+			u8 ampdu_density = sta->ht_cap.ampdu_density;
+			SET_TX_DESC_AMPDU_DENSITY(pdesc, ampdu_density);
+		}
+		if (info->control.hw_key) {
+			struct ieee80211_key_conf *keyconf = info->control.hw_key;
+/*<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:
+			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;
+
+			}
+/*<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);
+		SET_TX_DESC_DATA_RATE_FB_LIMIT(pdesc, 0x1F);
+		SET_TX_DESC_RTS_RATE_FB_LIMIT(pdesc, 0xF);
+		SET_TX_DESC_DISABLE_FB(pdesc, ptcb_desc->disable_ratefallback ? 1 : 0);
+		SET_TX_DESC_USE_RATE(pdesc, ptcb_desc->use_driver_rate ? 1 : 0);
+
+#if 0
+		SET_TX_DESC_USE_RATE(pdesc, 1);
+		SET_TX_DESC_TX_RATE(pdesc, 0x04);
+
+		SET_TX_DESC_RETRY_LIMIT_ENABLE(pdesc, 1);
+		SET_TX_DESC_DATA_RETRY_LIMIT(pdesc, 0x3f);
+#endif
+
+		/*SET_TX_DESC_PWR_STATUS(pdesc, pwr_status);*/
+		/* Set TxRate and RTSRate in TxDesc  */
+		/* This prevent Tx initial rate of new-coming packets */
+		/* from being overwritten by retried  packet rate.*/
+		if (!ptcb_desc->use_driver_rate) {
+			/*SET_TX_DESC_RTS_RATE(pdesc, 0x08); */
+			/* SET_TX_DESC_TX_RATE(pdesc, 0x0b); */
+		}
+		if (ieee80211_is_data_qos(fc)) {
+			if (mac->rdg_en) {
+				RT_TRACE(COMP_SEND, DBG_TRACE,
+					("Enable RDG function.\n"));
+				SET_TX_DESC_RDG_ENABLE(pdesc, 1);
+				SET_TX_DESC_HTC(pdesc, 1);
+			}
+		}
+	}
+
+	SET_TX_DESC_FIRST_SEG(pdesc, (b_firstseg ? 1 : 0));
+	SET_TX_DESC_LAST_SEG(pdesc, (b_lastseg ? 1 : 0));
+	SET_TX_DESC_TX_BUFFER_SIZE(pdesc, (u16) buf_len);
+	SET_TX_DESC_TX_BUFFER_ADDRESS(pdesc, cpu_to_le32(mapping));
+	//if (rtlpriv->dm.b_useramask) {
+	if(1){
+		SET_TX_DESC_RATE_ID(pdesc, ptcb_desc->ratr_index);
+		SET_TX_DESC_MACID(pdesc, ptcb_desc->mac_id);
+	} else {
+		SET_TX_DESC_RATE_ID(pdesc, 0xC + ptcb_desc->ratr_index);
+		SET_TX_DESC_MACID(pdesc, ptcb_desc->mac_id);
+	}
+/*	if (ieee80211_is_data_qos(fc))
+		SET_TX_DESC_QOS(pdesc, 1);
+*/
+	if (!ieee80211_is_data_qos(fc))  {
+		SET_TX_DESC_HWSEQ_EN(pdesc, 1);
+		SET_TX_DESC_HWSEQ_SEL(pdesc, 0);
+	}
+	SET_TX_DESC_MORE_FRAG(pdesc, (b_lastseg ? 0 : 1));
+	if (is_multicast_ether_addr(ieee80211_get_DA(hdr)) ||
+	    is_broadcast_ether_addr(ieee80211_get_DA(hdr))) {
+		SET_TX_DESC_BMC(pdesc, 1);
+	}
+
+	rtl8821ae_dm_set_tx_ant_by_tx_info(hw,pdesc,ptcb_desc->mac_id);
+	RT_TRACE(COMP_SEND, DBG_TRACE, ("\n"));
+}
+
+void rtl8821ae_tx_fill_cmddesc(struct ieee80211_hw *hw,
+			     u8 *pdesc, bool b_firstseg,
+			     bool b_lastseg, struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u8 fw_queue = QSLT_BEACON;
+
+	dma_addr_t mapping = pci_map_single(rtlpci->pdev,
+					    skb->data, skb->len,
+					    PCI_DMA_TODEVICE);
+
+	if (pci_dma_mapping_error(rtlpci->pdev, mapping)) {
+		RT_TRACE(COMP_SEND, DBG_TRACE,
+			 ("DMA mapping error"));
+		return;
+	}
+	CLEAR_PCI_TX_DESC_CONTENT(pdesc, TX_DESC_SIZE);
+
+	SET_TX_DESC_FIRST_SEG(pdesc, 1);
+	SET_TX_DESC_LAST_SEG(pdesc, 1);
+
+	SET_TX_DESC_PKT_SIZE((u8 *) pdesc, (u16) (skb->len));
+
+	SET_TX_DESC_OFFSET(pdesc, USB_HWDESC_HEADER_LEN);
+
+	SET_TX_DESC_USE_RATE(pdesc, 1);
+	SET_TX_DESC_TX_RATE(pdesc, DESC_RATE1M);
+	SET_TX_DESC_DISABLE_FB(pdesc, 1);
+
+	SET_TX_DESC_DATA_BW(pdesc, 0);
+
+	SET_TX_DESC_HWSEQ_EN(pdesc, 1);
+
+	SET_TX_DESC_QUEUE_SEL(pdesc, fw_queue);
+/*
+	if(IsCtrlNDPA(VirtualAddress) || IsMgntNDPA(VirtualAddress))
+	{
+		SET_TX_DESC_DATA_RETRY_LIMIT_8812(pDesc, 5);
+		SET_TX_DESC_RETRY_LIMIT_ENABLE_8812(pDesc, 1);
+
+		if(IsMgntNDPA(VirtualAddress))
+		{
+			SET_TX_DESC_NDPA_8812(pDesc, 1);
+			SET_TX_DESC_RTS_SC_8812(pDesc, SCMapping_8812(Adapter, pTcb));
+		}
+		else
+		{
+			SET_TX_DESC_NDPA_8812(pDesc, 2);
+			SET_TX_DESC_RTS_SC_8812(pDesc, SCMapping_8812(Adapter, pTcb));
+		}
+	}*/
+
+	SET_TX_DESC_TX_BUFFER_SIZE(pdesc, (u16) (skb->len));
+
+	SET_TX_DESC_TX_BUFFER_ADDRESS(pdesc, cpu_to_le32(mapping));
+
+	SET_TX_DESC_MACID(pdesc, 0);
+
+	SET_TX_DESC_OWN(pdesc, 1);
+
+	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD,
+		      "H2C Tx Cmd Content\n",
+		      pdesc, TX_DESC_SIZE);
+}
+
+void rtl8821ae_set_desc(struct ieee80211_hw * hw, u8 *pdesc, bool istx, u8 desc_name, u8 *val)
+{
+	if (istx == true) {
+		switch (desc_name) {
+		case HW_DESC_OWN:
+			SET_TX_DESC_OWN(pdesc, 1);
+			break;
+		case HW_DESC_TX_NEXTDESC_ADDR:
+			SET_TX_DESC_NEXT_DESC_ADDRESS(pdesc, *(u32 *) val);
+			break;
+		default:
+			RT_ASSERT(false, ("ERR txdesc :%d"
+					  " not process\n", desc_name));
+			break;
+		}
+	} else {
+		switch (desc_name) {
+		case HW_DESC_RXOWN:
+			SET_RX_DESC_OWN(pdesc, 1);
+			break;
+		case HW_DESC_RXBUFF_ADDR:
+			SET_RX_DESC_BUFF_ADDR(pdesc, *(u32 *) val);
+			break;
+		case HW_DESC_RXPKT_LEN:
+			SET_RX_DESC_PKT_LEN(pdesc, *(u32 *) val);
+			break;
+		case HW_DESC_RXERO:
+			SET_RX_DESC_EOR(pdesc, 1);
+			break;
+		default:
+			RT_ASSERT(false, ("ERR rxdesc :%d "
+					  "not process\n", desc_name));
+			break;
+		}
+	}
+}
+
+u32 rtl8821ae_get_desc(u8 *pdesc, bool istx, u8 desc_name)
+{
+	u32 ret = 0;
+
+	if (istx == true) {
+		switch (desc_name) {
+		case HW_DESC_OWN:
+			ret = GET_TX_DESC_OWN(pdesc);
+			break;
+		case HW_DESC_TXBUFF_ADDR:
+			ret = GET_TX_DESC_TX_BUFFER_ADDRESS(pdesc);
+			break;
+		default:
+			RT_ASSERT(false, ("ERR txdesc :%d "
+					  "not process\n", desc_name));
+			break;
+		}
+	} else {
+		switch (desc_name) {
+		case HW_DESC_OWN:
+			ret = GET_RX_DESC_OWN(pdesc);
+			break;
+		case HW_DESC_RXPKT_LEN:
+			ret = GET_RX_DESC_PKT_LEN(pdesc);
+			break;
+		default:
+			RT_ASSERT(false, ("ERR rxdesc :%d "
+					  "not process\n", desc_name));
+			break;
+		}
+	}
+	return ret;
+}
+
+bool rtl8821ae_is_tx_desc_closed(struct ieee80211_hw *hw,
+				 u8 hw_queue, u16 index)
+{
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[hw_queue];
+	u8 *entry = (u8 *)(&ring->desc[ring->idx]);
+	u8 own = (u8) rtl8821ae_get_desc(entry, true, HW_DESC_OWN);
+
+	/*
+	 *beacon packet will only use the first
+	 *descriptor defautly,and the own may not
+	 *be cleared by the hardware
+	 */
+	if (own)
+		return false;
+	else
+		return true;
+}
+
+
+void rtl8821ae_tx_polling(struct ieee80211_hw *hw, u8 hw_queue)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (hw_queue == BEACON_QUEUE) {
+		rtl_write_word(rtlpriv, REG_PCIE_CTRL_REG, BIT(4));
+	} else {
+		rtl_write_word(rtlpriv, REG_PCIE_CTRL_REG,
+			       BIT(0) << (hw_queue));
+	}
+}
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/trx.h b/drivers/staging/rtl8821ae/rtl8821ae/trx.h
new file mode 100644
index 000000000000..da93e5c7ece7
--- /dev/null
+++ b/drivers/staging/rtl8821ae/rtl8821ae/trx.h
@@ -0,0 +1,641 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL8821AE_TRX_H__
+#define __RTL8821AE_TRX_H__
+
+#define TX_DESC_SIZE					40
+#define TX_DESC_AGGR_SUBFRAME_SIZE		32
+
+#define RX_DESC_SIZE					32
+#define RX_DRV_INFO_SIZE_UNIT			8
+
+#define	TX_DESC_NEXT_DESC_OFFSET		40
+#define USB_HWDESC_HEADER_LEN			40
+#define CRCLENGTH						4
+
+#define SET_TX_DESC_PKT_SIZE(__pdesc, __val) 		\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 0, 16, __val)
+#define SET_TX_DESC_OFFSET(__pdesc, __val) 			\
+	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_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_BAR_RTY_TH(__pdesc, __val) 		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 14, 2, __val)
+#define SET_TX_DESC_AGG_BREAK(__pdesc, __val) 		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 16, 1, __val)
+#define SET_TX_DESC_MORE_FRAG(__pdesc, __val) 		\
+	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_INT(__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_MODE(__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_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)
+#define SET_TX_DESC_TX_ANT(__pdesc, __val) 		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 24, 4, __val)
+
+#define SET_TX_DESC_TX_RATE(__pdesc, __val) 		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 0, 7, __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_TX_SUB_CARRIER(__pdesc, __val) 		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 0, 4, __val)
+#define SET_TX_DESC_DATA_SHORTGI(__pdesc, __val) 	\
+	SET_BITS_TO_LE_1BYTE(__pdesc+20, 6, 1, __val)
+#define SET_TX_DESC_DATA_BW(__pdesc, __val) 		\
+	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_CTROL_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_BUFFER_SIZE(__pdesc, __val) 	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+28, 0, 16, __val)
+
+#define GET_TX_DESC_TX_BUFFER_SIZE(__pdesc) 		\
+	LE_BITS_TO_4BYTE(__pdesc+28, 0, 16)
+
+#define SET_TX_DESC_HWSEQ_EN(__pdesc, __val) 	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+32, 15, 1, __val)
+
+#define SET_TX_DESC_SEQ(__pdesc, __val) 	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+36, 12, 12, __val)
+
+#define SET_TX_DESC_TX_BUFFER_ADDRESS(__pdesc, __val) 	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+40, 0, 32, __val)
+
+#define GET_TX_DESC_TX_BUFFER_ADDRESS(__pdesc) 		\
+	LE_BITS_TO_4BYTE(__pdesc+40, 0, 32)
+
+
+#define SET_TX_DESC_NEXT_DESC_ADDRESS(__pdesc, __val) 	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+48, 0, 32, __val)
+
+#define GET_TX_DESC_NEXT_DESC_ADDRESS(__pdesc) 		\
+	LE_BITS_TO_4BYTE(__pdesc+48, 0, 32)
+
+#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_AMSDU(__pdesc) 				\
+	LE_BITS_TO_4BYTE(__pdesc+4, 13, 1)
+#define GET_RX_STATUS_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_CHKERR(__pdesc) 			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 20, 1)
+#define GET_RX_DESC_IPVER(__pdesc) 				\
+	LE_BITS_TO_4BYTE(__pdesc+4, 21, 1)
+#define GET_RX_STATUS_DESC_IS_TCPUDP(__pdesc) 				\
+	LE_BITS_TO_4BYTE(__pdesc+4, 22, 1)
+#define GET_RX_STATUS_DESC_CHK_VLD(__pdesc) 				\
+	LE_BITS_TO_4BYTE(__pdesc+4, 23, 1)
+#define GET_RX_DESC_PAM(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+4, 24, 1)
+#define GET_RX_DESC_PWR(__pdesc) 				\
+	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_STATUS_DESC_RX_IS_QOS(__pdesc) 				\
+	LE_BITS_TO_4BYTE(__pdesc+8, 16, 1)
+#define GET_RX_STATUS_DESC_WLANHD_IV_LEN(__pdesc) 				\
+	LE_BITS_TO_4BYTE(__pdesc+8, 18, 6)
+#define GET_RX_STATUS_DESC_RPT_SEL(__pdesc) 				\
+	LE_BITS_TO_4BYTE(__pdesc+8, 28, 1)
+
+
+#define GET_RX_DESC_RXMCS(__pdesc) 				\
+	LE_BITS_TO_4BYTE(__pdesc+12, 0, 7)
+#define GET_RX_DESC_RXHT(__pdesc) 				\
+	LE_BITS_TO_4BYTE(__pdesc+12, 6, 1)
+#define GET_RX_STATUS_DESC_RX_GF(__pdesc) 				\
+	LE_BITS_TO_4BYTE(__pdesc+12, 7, 1)
+#define GET_RX_DESC_HTC(__pdesc) 				\
+	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_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_SPLCP(__pdesc) 				\
+	LE_BITS_TO_4BYTE(__pdesc+16, 0, 1)
+#define GET_RX_STATUS_DESC_LDPC(__pdesc) 				\
+	LE_BITS_TO_4BYTE(__pdesc+16, 1, 1)
+#define GET_RX_STATUS_DESC_STBC(__pdesc) 				\
+	LE_BITS_TO_4BYTE(__pdesc+16, 2, 1)
+#define GET_RX_DESC_BW(__pdesc) 				\
+	LE_BITS_TO_4BYTE(__pdesc+16, 4, 2)
+
+#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 SET_EARLYMODE_PKTNUM(__paddr, __value) 	\
+	SET_BITS_TO_LE_4BYTE(__paddr, 0, 4, __value)
+#define SET_EARLYMODE_LEN0(__paddr, __value) 	\
+	SET_BITS_TO_LE_4BYTE(__paddr, 4, 12, __value)
+#define SET_EARLYMODE_LEN1(__paddr, __value) 	\
+	SET_BITS_TO_LE_4BYTE(__paddr, 16, 12, __value)
+#define SET_EARLYMODE_LEN2_1(__paddr, __value) 	\
+	SET_BITS_TO_LE_4BYTE(__paddr, 28, 4, __value)
+#define SET_EARLYMODE_LEN2_2(__paddr, __value) 	\
+	SET_BITS_TO_LE_4BYTE(__paddr+4, 0, 8, __value)
+#define SET_EARLYMODE_LEN3(__paddr, __value) 	\
+	SET_BITS_TO_LE_4BYTE(__paddr+4, 8, 12, __value)
+#define SET_EARLYMODE_LEN4(__paddr, __value) 	\
+	SET_BITS_TO_LE_4BYTE(__paddr+4, 20, 12, __value)
+
+#define CLEAR_PCI_TX_DESC_CONTENT(__pdesc, _size)		\
+do {								\
+	if(_size > TX_DESC_NEXT_DESC_OFFSET)			\
+		memset(__pdesc, 0, TX_DESC_NEXT_DESC_OFFSET);	\
+	else							\
+		memset(__pdesc, 0, _size);			\
+} while (0);
+
+#define RX_HAL_IS_CCK_RATE(rxmcs)\
+	(rxmcs == DESC_RATE1M ||\
+	 rxmcs == DESC_RATE2M ||\
+	 rxmcs == DESC_RATE5_5M ||\
+	 rxmcs == DESC_RATE11M)
+
+#define IS_LITTLE_ENDIAN	1
+
+struct phy_rx_agc_info_t {
+	#if IS_LITTLE_ENDIAN
+		u8	gain:7,trsw:1;
+	#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;//ch_corr_msb;
+	u8	noise_power_db_msb;
+	u8	path_cfotail[2];
+	u8	pcts_mask[2];
+	u8	stream_rxevm[2];
+	u8	path_rxsnr[2];
+	u8 	noise_power_db_lsb;
+	u8	rsvd_2[3];
+	u8 	stream_csi[2];
+	u8 	stream_target_csi[2];
+	u8 	sig_evm;
+	u8 	rsvd_3;
+#if IS_LITTLE_ENDIAN
+	u8 	antsel_rx_keep_2:1;	/*ex_intf_flg:1;*/
+	u8 	sgi_en:1;
+	u8 	rxsc:2;
+	u8 	idle_long:1;
+	u8 	r_ant_train_en:1;
+	u8 	ant_sel_b:1;
+	u8 	ant_sel:1;
+#else	/* _BIG_ENDIAN_	*/
+	u8 	ant_sel:1;
+	u8 	ant_sel_b:1;
+	u8 	r_ant_train_en:1;
+	u8 	idle_long:1;
+	u8 	rxsc:2;
+	u8 	sgi_en:1;
+	u8 	antsel_rx_keep_2:1;	/*ex_intf_flg:1;*/
+#endif
+}__packed;
+
+struct rx_fwinfo_8821ae {
+	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_8821ae {
+	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_8821ae {
+	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;
+
+/*<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,
+			   u8 *pdesc, struct sk_buff *skb);
+void rtl8821ae_set_desc(struct ieee80211_hw * hw, u8 *pdesc, bool istx, u8 desc_name, u8 *val);
+u32 rtl8821ae_get_desc(u8 *pdesc, bool istx, u8 desc_name);
+bool rtl8821ae_is_tx_desc_closed(struct ieee80211_hw *hw,
+				 u8 hw_queue, u16 index);
+void rtl8821ae_tx_polling(struct ieee80211_hw *hw, u8 hw_queue);
+void rtl8821ae_tx_fill_cmddesc(struct ieee80211_hw *hw, u8 *pdesc,
+			     bool b_firstseg, bool b_lastseg,
+			     struct sk_buff *skb);
+#endif
diff --git a/drivers/staging/rtl8821ae/stats.c b/drivers/staging/rtl8821ae/stats.c
new file mode 100644
index 000000000000..a20c0f8f65ec
--- /dev/null
+++ b/drivers/staging/rtl8821ae/stats.c
@@ -0,0 +1,283 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+#include "wifi.h"
+#include "stats.h"
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0))
+#include <linux/export.h>
+#endif
+
+u8 rtl_query_rxpwrpercentage(char antpower)
+{
+	if ((antpower <= -100) || (antpower >= 20))
+		return 0;
+	else if (antpower >= 0)
+		return 100;
+	else
+		return (100 + antpower);
+}
+//EXPORT_SYMBOL(rtl_query_rxpwrpercentage);
+
+u8 rtl_evm_db_to_percentage(char value)
+{
+	char ret_val;
+	ret_val = value;
+
+	if (ret_val >= 0)
+		ret_val = 0;
+	if (ret_val <= -33)
+		ret_val = -33;
+	ret_val = 0 - ret_val;
+	ret_val *= 3;
+	if (ret_val == 99)
+		ret_val = 100;
+
+	return ret_val;
+}
+//EXPORT_SYMBOL(rtl_evm_db_to_percentage);
+
+long rtl_translate_todbm(struct ieee80211_hw *hw,
+			 u8 signal_strength_index)
+{
+	long signal_power;
+
+	signal_power = (long)((signal_strength_index + 1) >> 1);
+	signal_power -= 95;
+	return signal_power;
+}
+
+long rtl_signal_scale_mapping(struct ieee80211_hw *hw, long currsig)
+{
+	long retsig;
+
+	if (currsig >= 61 && currsig <= 100)
+		retsig = 90 + ((currsig - 60) / 4);
+	else if (currsig >= 41 && currsig <= 60)
+		retsig = 78 + ((currsig - 40) / 2);
+	else if (currsig >= 31 && currsig <= 40)
+		retsig = 66 + (currsig - 30);
+	else if (currsig >= 21 && currsig <= 30)
+		retsig = 54 + (currsig - 20);
+	else if (currsig >= 5 && currsig <= 20)
+		retsig = 42 + (((currsig - 5) * 2) / 3);
+	else if (currsig == 4)
+		retsig = 36;
+	else if (currsig == 3)
+		retsig = 27;
+	else if (currsig == 2)
+		retsig = 18;
+	else if (currsig == 1)
+		retsig = 9;
+	else
+		retsig = currsig;
+
+	return retsig;
+}
+//EXPORT_SYMBOL(rtl_signal_scale_mapping);
+
+void rtl_process_ui_rssi(struct ieee80211_hw *hw, struct rtl_stats *pstatus)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 rfpath;
+	u32 last_rssi, tmpval;
+
+	if (!pstatus->b_packet_toself && !pstatus->b_packet_beacon)
+		return;
+
+	rtlpriv->stats.pwdb_all_cnt += pstatus->rx_pwdb_all;
+	rtlpriv->stats.rssi_calculate_cnt++;
+
+	if (rtlpriv->stats.ui_rssi.total_num++ >= PHY_RSSI_SLID_WIN_MAX) {
+		rtlpriv->stats.ui_rssi.total_num = PHY_RSSI_SLID_WIN_MAX;
+		last_rssi = rtlpriv->stats.ui_rssi.elements[
+			rtlpriv->stats.ui_rssi.index];
+		rtlpriv->stats.ui_rssi.total_val -= last_rssi;
+	}
+	rtlpriv->stats.ui_rssi.total_val += pstatus->signalstrength;
+	rtlpriv->stats.ui_rssi.elements[rtlpriv->stats.ui_rssi.index++] =
+	    pstatus->signalstrength;
+	if (rtlpriv->stats.ui_rssi.index >= PHY_RSSI_SLID_WIN_MAX)
+		rtlpriv->stats.ui_rssi.index = 0;
+	tmpval = rtlpriv->stats.ui_rssi.total_val /
+		rtlpriv->stats.ui_rssi.total_num;
+	rtlpriv->stats.signal_strength = rtl_translate_todbm(hw,
+		(u8) tmpval);
+	pstatus->rssi = rtlpriv->stats.signal_strength;
+
+	if (pstatus->b_is_cck)
+		return;
+
+	for (rfpath = RF90_PATH_A; rfpath < rtlphy->num_total_rfpath;
+	     rfpath++) {
+		if (rtlpriv->stats.rx_rssi_percentage[rfpath] == 0) {
+			rtlpriv->stats.rx_rssi_percentage[rfpath] =
+			    pstatus->rx_mimo_signalstrength[rfpath];
+
+		}
+		if (pstatus->rx_mimo_signalstrength[rfpath] >
+		    rtlpriv->stats.rx_rssi_percentage[rfpath]) {
+			rtlpriv->stats.rx_rssi_percentage[rfpath] =
+			    ((rtlpriv->stats.rx_rssi_percentage[rfpath] *
+			      (RX_SMOOTH_FACTOR - 1)) +
+			     (pstatus->rx_mimo_signalstrength[rfpath])) /
+			    (RX_SMOOTH_FACTOR);
+			rtlpriv->stats.rx_rssi_percentage[rfpath] =
+			    rtlpriv->stats.rx_rssi_percentage[rfpath] + 1;
+		} else {
+			rtlpriv->stats.rx_rssi_percentage[rfpath] =
+			    ((rtlpriv->stats.rx_rssi_percentage[rfpath] *
+			      (RX_SMOOTH_FACTOR - 1)) +
+			     (pstatus->rx_mimo_signalstrength[rfpath])) /
+			    (RX_SMOOTH_FACTOR);
+		}
+		rtlpriv->stats.rx_snr_db[rfpath] = pstatus->rx_snr[rfpath];
+		rtlpriv->stats.rx_evm_dbm[rfpath] =
+					pstatus->rx_mimo_evm_dbm[rfpath];
+		rtlpriv->stats.rx_cfo_short[rfpath] =
+					pstatus->cfo_short[rfpath];
+		rtlpriv->stats.rx_cfo_tail[rfpath] = pstatus->cfo_tail[rfpath];
+	}
+}
+
+static void rtl_update_rxsignalstatistics(struct ieee80211_hw *hw,
+					  struct rtl_stats *pstatus)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int weighting = 0;
+
+	if (rtlpriv->stats.recv_signal_power == 0)
+		rtlpriv->stats.recv_signal_power = pstatus->recvsignalpower;
+	if (pstatus->recvsignalpower > rtlpriv->stats.recv_signal_power)
+		weighting = 5;
+	else if (pstatus->recvsignalpower < rtlpriv->stats.recv_signal_power)
+		weighting = (-5);
+	rtlpriv->stats.recv_signal_power = (rtlpriv->stats.recv_signal_power *
+		5 + pstatus->recvsignalpower + weighting) / 6;
+}
+
+static void rtl_process_pwdb(struct ieee80211_hw *hw, struct rtl_stats *pstatus)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_sta_info *drv_priv = NULL;
+	struct ieee80211_sta *sta = NULL;
+	long undecorated_smoothed_pwdb;
+
+	rcu_read_lock();
+	if (rtlpriv->mac80211.opmode != NL80211_IFTYPE_STATION)
+		sta = rtl_find_sta(hw, pstatus->psaddr);
+
+	/* adhoc or ap mode */
+	if (sta) {
+		drv_priv = (struct rtl_sta_info *) sta->drv_priv;
+		undecorated_smoothed_pwdb =
+			drv_priv->rssi_stat.undecorated_smoothed_pwdb;
+	} else {
+		undecorated_smoothed_pwdb =
+			rtlpriv->dm.undecorated_smoothed_pwdb;
+	}
+
+	if (undecorated_smoothed_pwdb < 0)
+		undecorated_smoothed_pwdb = pstatus->rx_pwdb_all;
+	if (pstatus->rx_pwdb_all > (u32) undecorated_smoothed_pwdb) {
+		undecorated_smoothed_pwdb = (((undecorated_smoothed_pwdb) *
+		      (RX_SMOOTH_FACTOR - 1)) +
+		     (pstatus->rx_pwdb_all)) / (RX_SMOOTH_FACTOR);
+		undecorated_smoothed_pwdb = undecorated_smoothed_pwdb + 1;
+	} else {
+		undecorated_smoothed_pwdb = (((undecorated_smoothed_pwdb) *
+		      (RX_SMOOTH_FACTOR - 1)) +
+		     (pstatus->rx_pwdb_all)) / (RX_SMOOTH_FACTOR);
+	}
+
+	if(sta) {
+		drv_priv->rssi_stat.undecorated_smoothed_pwdb =
+			undecorated_smoothed_pwdb;
+	} else {
+		rtlpriv->dm.undecorated_smoothed_pwdb = undecorated_smoothed_pwdb;
+	}
+	rcu_read_unlock();
+
+	rtl_update_rxsignalstatistics(hw, pstatus);
+}
+
+static void rtl_process_ui_link_quality(struct ieee80211_hw *hw,
+					struct rtl_stats *pstatus)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 last_evm, n_stream, tmpval;
+
+	if (pstatus->signalquality == 0)
+		return;
+
+	if (rtlpriv->stats.ui_link_quality.total_num++ >=
+	    PHY_LINKQUALITY_SLID_WIN_MAX) {
+		rtlpriv->stats.ui_link_quality.total_num =
+		    PHY_LINKQUALITY_SLID_WIN_MAX;
+		last_evm = rtlpriv->stats.ui_link_quality.elements[
+			rtlpriv->stats.ui_link_quality.index];
+		rtlpriv->stats.ui_link_quality.total_val -= last_evm;
+	}
+	rtlpriv->stats.ui_link_quality.total_val += pstatus->signalquality;
+	rtlpriv->stats.ui_link_quality.elements[
+		rtlpriv->stats.ui_link_quality.index++] =
+							pstatus->signalquality;
+	if (rtlpriv->stats.ui_link_quality.index >=
+	    PHY_LINKQUALITY_SLID_WIN_MAX)
+		rtlpriv->stats.ui_link_quality.index = 0;
+	tmpval = rtlpriv->stats.ui_link_quality.total_val /
+	    rtlpriv->stats.ui_link_quality.total_num;
+	rtlpriv->stats.signal_quality = tmpval;
+	rtlpriv->stats.last_sigstrength_inpercent = tmpval;
+	for (n_stream = 0; n_stream < 2; n_stream++) {
+		if (pstatus->rx_mimo_signalquality[n_stream] != -1) {
+			if (rtlpriv->stats.rx_evm_percentage[n_stream] == 0) {
+				rtlpriv->stats.rx_evm_percentage[n_stream] =
+				    pstatus->rx_mimo_signalquality[n_stream];
+			}
+			rtlpriv->stats.rx_evm_percentage[n_stream] =
+			    ((rtlpriv->stats.rx_evm_percentage[n_stream]
+			      * (RX_SMOOTH_FACTOR - 1)) +
+			     (pstatus->rx_mimo_signalquality[n_stream] * 1)) /
+			    (RX_SMOOTH_FACTOR);
+		}
+	}
+}
+
+void rtl_process_phyinfo(struct ieee80211_hw *hw, u8 *buffer,
+			 struct rtl_stats *pstatus)
+{
+
+	if (!pstatus->b_packet_matchbssid)
+		return;
+
+	rtl_process_ui_rssi(hw, pstatus);
+	rtl_process_pwdb(hw, pstatus);
+	rtl_process_ui_link_quality(hw, pstatus);
+}
+//EXPORT_SYMBOL(rtl_process_phyinfo);
diff --git a/drivers/staging/rtl8821ae/stats.h b/drivers/staging/rtl8821ae/stats.h
new file mode 100644
index 000000000000..d69d0cfd7e14
--- /dev/null
+++ b/drivers/staging/rtl8821ae/stats.h
@@ -0,0 +1,46 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL_STATS_H__
+#define __RTL_STATS_H__
+
+#define	PHY_RSSI_SLID_WIN_MAX			100
+#define	PHY_LINKQUALITY_SLID_WIN_MAX		20
+#define	PHY_BEACON_RSSI_SLID_WIN_MAX		10
+
+/* Rx smooth factor */
+#define	RX_SMOOTH_FACTOR			20
+
+u8 rtl_query_rxpwrpercentage(char antpower);
+u8 rtl_evm_db_to_percentage(char value);
+long rtl_signal_scale_mapping(struct ieee80211_hw *hw, long currsig);
+void rtl_process_phyinfo(struct ieee80211_hw *hw, u8 *buffer,
+			 struct rtl_stats *pstatus);
+
+#endif
diff --git a/drivers/staging/rtl8821ae/wifi.h b/drivers/staging/rtl8821ae/wifi.h
new file mode 100644
index 000000000000..cfe88a1efd55
--- /dev/null
+++ b/drivers/staging/rtl8821ae/wifi.h
@@ -0,0 +1,2532 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL_WIFI_H__
+#define __RTL_WIFI_H__
+
+#include <linux/interrupt.h>
+#include <linux/sched.h>
+#include <linux/firmware.h>
+#include <linux/version.h>
+#include <linux/etherdevice.h>
+#include <net/mac80211.h>
+#include "debug.h"
+
+
+#define RF_CHANGE_BY_INIT		0
+#define RF_CHANGE_BY_IPS 		BIT(28)
+#define RF_CHANGE_BY_PS 		BIT(29)
+#define RF_CHANGE_BY_HW 		BIT(30)
+#define RF_CHANGE_BY_SW 		BIT(31)
+
+#define IQK_ADDA_REG_NUM		16
+#define IQK_MAC_REG_NUM			4
+#define IQK_THRESHOLD			8
+
+#define MAX_KEY_LEN			61
+#define KEY_BUF_SIZE			5
+
+/* QoS related. */
+/*aci: 0x00	Best Effort*/
+/*aci: 0x01	Background*/
+/*aci: 0x10	Video*/
+/*aci: 0x11	Voice*/
+/*Max: define total number.*/
+#define AC0_BE				0
+#define AC1_BK				1
+#define AC2_VI				2
+#define AC3_VO				3
+#define AC_MAX				4
+#define QOS_QUEUE_NUM			4
+#define RTL_MAC80211_NUM_QUEUE		5
+
+#define QBSS_LOAD_SIZE			5
+#define MAX_WMMELE_LENGTH		64
+
+#define TOTAL_CAM_ENTRY 		32
+
+/*slot time for 11g. */
+#define RTL_SLOT_TIME_9			9
+#define RTL_SLOT_TIME_20		20
+
+/*related with tcp/ip. */
+/*if_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 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 */
+
+struct txpower_info_2g {
+	u8 index_cck_base[MAX_RF_PATH][MAX_CHNL_GROUP_24G];
+	u8 index_bw40_base[MAX_RF_PATH][MAX_CHNL_GROUP_24G];
+	/*If only one tx, only BW20 and OFDM are used.*/
+	u8 cck_diff[MAX_RF_PATH][MAX_TX_COUNT];
+	u8 ofdm_diff[MAX_RF_PATH][MAX_TX_COUNT];
+	u8 bw20_diff[MAX_RF_PATH][MAX_TX_COUNT];
+	u8 bw40_diff[MAX_RF_PATH][MAX_TX_COUNT];
+};
+
+struct txpower_info_5g {
+	u8 index_bw40_base[MAX_RF_PATH][MAX_CHNL_GROUP_5G];
+	/*If only one tx, only BW20, OFDM, BW80 and BW160 are used.*/
+	u8 ofdm_diff[MAX_RF_PATH][MAX_TX_COUNT];
+	u8 bw20_diff[MAX_RF_PATH][MAX_TX_COUNT];
+	u8 bw40_diff[MAX_RF_PATH][MAX_TX_COUNT];
+	u8 bw80_diff[MAX_RF_PATH][MAX_TX_COUNT];
+	u8 bw160_diff[MAX_RF_PATH][MAX_TX_COUNT];
+};
+
+
+/* for early mode */
+#define EM_HDR_LEN			8
+#define FCS_LEN				4
+
+#define MAX_VIRTUAL_MAC			1
+
+enum rf_tx_num {
+	RF_1TX = 0,
+	RF_2TX,
+	RF_MAX_TX_NUM,
+	RF_TX_NUM_NONIMPLEMENT,
+};
+
+enum rate_section {
+	CCK = 0,
+	OFDM,
+	HT_MCS0_MCS7,
+	HT_MCS8_MCS15,
+	VHT_1SSMCS0_1SSMCS9,
+	VHT_2SSMCS0_2SSMCS9,
+};
+
+enum intf_type {
+	INTF_PCI = 0,
+	INTF_USB = 1,
+};
+
+enum radio_path {
+	RF90_PATH_A = 0,
+	RF90_PATH_B = 1,
+	RF90_PATH_C = 2,
+	RF90_PATH_D = 3,
+};
+
+enum rt_eeprom_type {
+	EEPROM_93C46,
+	EEPROM_93C56,
+	EEPROM_BOOT_EFUSE,
+};
+
+enum rtl_status {
+	RTL_STATUS_INTERFACE_START = 0,
+};
+
+enum hardware_type {
+	HARDWARE_TYPE_RTL8192E,
+	HARDWARE_TYPE_RTL8192U,
+	HARDWARE_TYPE_RTL8192SE,
+	HARDWARE_TYPE_RTL8192SU,
+	HARDWARE_TYPE_RTL8192CE,
+	HARDWARE_TYPE_RTL8192CU,
+	HARDWARE_TYPE_RTL8192DE,
+	HARDWARE_TYPE_RTL8192DU,
+	HARDWARE_TYPE_RTL8723AE,
+	HARDWARE_TYPE_RTL8188EE,
+	HARDWARE_TYPE_RTL8723BE,
+	HARDWARE_TYPE_RTL8192EE,
+	HARDWARE_TYPE_RTL8821AE,
+	HARDWARE_TYPE_RTL8812AE,
+	/* keep it last */
+	HARDWARE_TYPE_NUM
+};
+
+enum scan_operation_backup_opt {
+	SCAN_OPT_BACKUP_BAND0=0,
+	SCAN_OPT_BACKUP_BAND1,
+	SCAN_OPT_RESTORE,
+	SCAN_OPT_MAX
+};
+
+/*RF state.*/
+enum rf_pwrstate {
+	ERFON,
+	ERFSLEEP,
+	ERFOFF
+};
+
+struct bb_reg_def {
+	u32 rfintfs;
+	u32 rfintfi;
+	u32 rfintfo;
+	u32 rfintfe;
+	u32 rf3wire_offset;
+	u32 rflssi_select;
+	u32 rftxgain_stage;
+	u32 rfhssi_para1;
+	u32 rfhssi_para2;
+	u32 rfswitch_control;
+	u32 rfagc_control1;
+	u32 rfagc_control2;
+	u32 rfrxiq_imbalance;
+	u32 rfrx_afe;
+	u32 rftxiq_imbalance;
+	u32 rftx_afe;
+	u32 rflssi_readback;
+	u32 rflssi_readbackpi;
+};
+
+enum io_type {
+	IO_CMD_PAUSE_BAND0_DM_BY_SCAN = 0,
+	IO_CMD_PAUSE_BAND1_DM_BY_SCAN = 1,
+	IO_CMD_RESUME_DM_BY_SCAN = 2,
+};
+
+enum hw_variables {
+	HW_VAR_ETHER_ADDR,
+	HW_VAR_MULTICAST_REG,
+	HW_VAR_BASIC_RATE,
+	HW_VAR_BSSID,
+	HW_VAR_MEDIA_STATUS,
+	HW_VAR_SECURITY_CONF,
+	HW_VAR_BEACON_INTERVAL,
+	HW_VAR_ATIM_WINDOW,
+	HW_VAR_LISTEN_INTERVAL,
+	HW_VAR_CS_COUNTER,
+	HW_VAR_DEFAULTKEY0,
+	HW_VAR_DEFAULTKEY1,
+	HW_VAR_DEFAULTKEY2,
+	HW_VAR_DEFAULTKEY3,
+	HW_VAR_SIFS,
+	HW_VAR_DIFS,
+	HW_VAR_EIFS,
+	HW_VAR_SLOT_TIME,
+	HW_VAR_ACK_PREAMBLE,
+	HW_VAR_CW_CONFIG,
+	HW_VAR_CW_VALUES,
+	HW_VAR_RATE_FALLBACK_CONTROL,
+	HW_VAR_CONTENTION_WINDOW,
+	HW_VAR_RETRY_COUNT,
+	HW_VAR_TR_SWITCH,
+	HW_VAR_COMMAND,
+	HW_VAR_WPA_CONFIG,
+	HW_VAR_AMPDU_MIN_SPACE,
+	HW_VAR_SHORTGI_DENSITY,
+	HW_VAR_AMPDU_FACTOR,
+	HW_VAR_MCS_RATE_AVAILABLE,
+	HW_VAR_AC_PARAM,
+	HW_VAR_ACM_CTRL,
+	HW_VAR_DIS_Req_Qsize,
+	HW_VAR_CCX_CHNL_LOAD,
+	HW_VAR_CCX_NOISE_HISTOGRAM,
+	HW_VAR_CCX_CLM_NHM,
+	HW_VAR_TxOPLimit,
+	HW_VAR_TURBO_MODE,
+	HW_VAR_RF_STATE,
+	HW_VAR_RF_OFF_BY_HW,
+	HW_VAR_BUS_SPEED,
+	HW_VAR_SET_DEV_POWER,
+
+	HW_VAR_RCR,
+	HW_VAR_RATR_0,
+	HW_VAR_RRSR,
+	HW_VAR_CPU_RST,
+	HW_VAR_CECHK_BSSID,
+	HW_VAR_LBK_MODE,
+	HW_VAR_AES_11N_FIX,
+	HW_VAR_USB_RX_AGGR,
+	HW_VAR_USER_CONTROL_TURBO_MODE,
+	HW_VAR_RETRY_LIMIT,
+	HW_VAR_INIT_TX_RATE,
+	HW_VAR_TX_RATE_REG,
+	HW_VAR_EFUSE_USAGE,
+	HW_VAR_EFUSE_BYTES,
+	HW_VAR_AUTOLOAD_STATUS,
+	HW_VAR_RF_2R_DISABLE,
+	HW_VAR_SET_RPWM,
+	HW_VAR_H2C_FW_PWRMODE,
+	HW_VAR_H2C_FW_JOINBSSRPT,
+	HW_VAR_H2C_FW_MEDIASTATUSRPT,
+	HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
+	HW_VAR_FW_PSMODE_STATUS,
+	HW_VAR_RESUME_CLK_ON,
+	HW_VAR_FW_LPS_ACTION,
+	HW_VAR_1X1_RECV_COMBINE,
+	HW_VAR_STOP_SEND_BEACON,
+	HW_VAR_TSF_TIMER,
+	HW_VAR_IO_CMD,
+
+	HW_VAR_RF_RECOVERY,
+	HW_VAR_H2C_FW_UPDATE_GTK,
+	HW_VAR_WF_MASK,
+	HW_VAR_WF_CRC,
+	HW_VAR_WF_IS_MAC_ADDR,
+	HW_VAR_H2C_FW_OFFLOAD,
+	HW_VAR_RESET_WFCRC,
+
+	HW_VAR_HANDLE_FW_C2H,
+	HW_VAR_DL_FW_RSVD_PAGE,
+	HW_VAR_AID,
+	HW_VAR_HW_SEQ_ENABLE,
+	HW_VAR_CORRECT_TSF,
+	HW_VAR_BCN_VALID,
+	HW_VAR_FWLPS_RF_ON,
+	HW_VAR_DUAL_TSF_RST,
+	HW_VAR_SWITCH_EPHY_WoWLAN,
+	HW_VAR_INT_MIGRATION,
+	HW_VAR_INT_AC,
+	HW_VAR_RF_TIMING,
+
+	HAL_DEF_WOWLAN,
+	HW_VAR_MRC,
+	HW_VAR_KEEP_ALIVE,
+	HW_VAR_NAV_UPPER,
+};
+
+enum rt_media_status {
+	RT_MEDIA_DISCONNECT = 0,
+	RT_MEDIA_CONNECT = 1
+};
+
+enum rt_oem_id {
+	RT_CID_DEFAULT = 0,
+	RT_CID_8187_ALPHA0 = 1,
+	RT_CID_8187_SERCOMM_PS = 2,
+	RT_CID_8187_HW_LED = 3,
+	RT_CID_8187_NETGEAR = 4,
+	RT_CID_WHQL = 5,
+	RT_CID_819x_CAMEO = 6,
+	RT_CID_819x_RUNTOP = 7,
+	RT_CID_819x_Senao = 8,
+	RT_CID_TOSHIBA = 9,
+	RT_CID_819x_Netcore = 10,
+	RT_CID_Nettronix = 11,
+	RT_CID_DLINK = 12,
+	RT_CID_PRONET = 13,
+	RT_CID_COREGA = 14,
+	RT_CID_819x_ALPHA = 15,
+	RT_CID_819x_Sitecom = 16,
+	RT_CID_CCX = 17,
+	RT_CID_819x_Lenovo = 18,
+	RT_CID_819x_QMI = 19,
+	RT_CID_819x_Edimax_Belkin = 20,
+	RT_CID_819x_Sercomm_Belkin = 21,
+	RT_CID_819x_CAMEO1 = 22,
+	RT_CID_819x_MSI = 23,
+	RT_CID_819x_Acer = 24,
+	RT_CID_819x_HP = 27,
+	RT_CID_819x_CLEVO = 28,
+	RT_CID_819x_Arcadyan_Belkin = 29,
+	RT_CID_819x_SAMSUNG = 30,
+	RT_CID_819x_WNC_COREGA = 31,
+	RT_CID_819x_Foxcoon = 32,
+	RT_CID_819x_DELL = 33,
+	RT_CID_819x_PRONETS = 34,
+	RT_CID_819x_Edimax_ASUS = 35,
+	RT_CID_NETGEAR = 36,
+	RT_CID_PLANEX = 37,
+	RT_CID_CC_C = 38,
+};
+
+enum hw_descs {
+	HW_DESC_OWN,
+	HW_DESC_RXOWN,
+	HW_DESC_TX_NEXTDESC_ADDR,
+	HW_DESC_TXBUFF_ADDR,
+	HW_DESC_RXBUFF_ADDR,
+	HW_DESC_RXPKT_LEN,
+	HW_DESC_RXERO,
+	HW_DESC_RX_PREPARE,
+};
+
+enum prime_sc {
+	PRIME_CHNL_OFFSET_DONT_CARE = 0,
+	PRIME_CHNL_OFFSET_LOWER = 1,
+	PRIME_CHNL_OFFSET_UPPER = 2,
+};
+
+enum rf_type {
+	RF_1T1R = 0,
+	RF_1T2R = 1,
+	RF_2T2R = 2,
+	RF_2T2R_GREEN = 3,
+};
+
+enum ht_channel_width {
+	HT_CHANNEL_WIDTH_20 = 0,
+	HT_CHANNEL_WIDTH_20_40 = 1,
+	HT_CHANNEL_WIDTH_80 = 2,
+};
+
+/* Ref: 802.11i 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,
+
+	/*keep it last */
+	RTL_VAR_MAP_MAX,
+};
+
+/*Firmware PS mode for control LPS.*/
+enum _fw_ps_mode {
+	FW_PS_ACTIVE_MODE = 0,
+	FW_PS_MIN_MODE = 1,
+	FW_PS_MAX_MODE = 2,
+	FW_PS_DTIM_MODE = 3,
+	FW_PS_VOIP_MODE = 4,
+	FW_PS_UAPSD_WMM_MODE = 5,
+	FW_PS_UAPSD_MODE = 6,
+	FW_PS_IBSS_MODE = 7,
+	FW_PS_WWLAN_MODE = 8,
+	FW_PS_PM_Radio_Off = 9,
+	FW_PS_PM_Card_Disable = 10,
+};
+
+enum rt_psmode {
+	EACTIVE,		/*Active/Continuous access. */
+	EMAXPS,			/*Max power save mode. */
+	EFASTPS,		/*Fast power save mode. */
+	EAUTOPS,		/*Auto power save mode. */
+};
+
+/*LED related.*/
+enum led_ctl_mode {
+	LED_CTL_POWER_ON = 1,
+	LED_CTL_LINK = 2,
+	LED_CTL_NO_LINK = 3,
+	LED_CTL_TX = 4,
+	LED_CTL_RX = 5,
+	LED_CTL_SITE_SURVEY = 6,
+	LED_CTL_POWER_OFF = 7,
+	LED_CTL_START_TO_LINK = 8,
+	LED_CTL_START_WPS = 9,
+	LED_CTL_STOP_WPS = 10,
+};
+
+enum rtl_led_pin {
+	LED_PIN_GPIO0,
+	LED_PIN_LED0,
+	LED_PIN_LED1,
+	LED_PIN_LED2
+};
+
+/*QoS related.*/
+/*acm implementation method.*/
+enum acm_method {
+	eAcmWay0_SwAndHw = 0,
+	eAcmWay1_HW = 1,
+	eAcmWay2_SW = 2,
+};
+
+enum macphy_mode {
+	SINGLEMAC_SINGLEPHY = 0,
+	DUALMAC_DUALPHY,
+	DUALMAC_SINGLEPHY,
+};
+
+enum band_type {
+	BAND_ON_2_4G = 0,
+	BAND_ON_5G,
+	BAND_ON_BOTH,
+	BANDMAX
+};
+
+/*aci/aifsn Field.
+Ref: WMM spec 2.2.2: WME Parameter Element, p.12.*/
+union aci_aifsn {
+	u8 char_data;
+
+	struct {
+		u8 aifsn:4;
+		u8 acm:1;
+		u8 aci:2;
+		u8 reserved:1;
+	} f;			/* Field */
+};
+
+/*mlme related.*/
+enum wireless_mode {
+	WIRELESS_MODE_UNKNOWN = 0x00,
+	WIRELESS_MODE_A = 0x01,
+	WIRELESS_MODE_B = 0x02,
+	WIRELESS_MODE_G = 0x04,
+	WIRELESS_MODE_AUTO = 0x08,
+	WIRELESS_MODE_N_24G = 0x10,
+	WIRELESS_MODE_N_5G = 0x20,
+	WIRELESS_MODE_AC_5G = 0x40,
+	WIRELESS_MODE_AC_24G  = 0x80
+};
+
+enum ratr_table_mode {
+	RATR_INX_WIRELESS_NGB = 0,		// BGN 40 Mhz 2SS 1SS
+	RATR_INX_WIRELESS_NG = 1,		// GN or N
+	RATR_INX_WIRELESS_NB = 2,		// BGN 20 Mhz 2SS 1SS  or BN
+	RATR_INX_WIRELESS_N = 3,
+	RATR_INX_WIRELESS_GB = 4,
+	RATR_INX_WIRELESS_G = 5,
+	RATR_INX_WIRELESS_B = 6,
+	RATR_INX_WIRELESS_MC = 7,
+	RATR_INX_WIRELESS_AC_5N = 8,
+	RATR_INX_WIRELESS_AC_24N = 9,
+};
+
+enum rtl_link_state {
+	MAC80211_NOLINK = 0,
+	MAC80211_LINKING = 1,
+	MAC80211_LINKED = 2,
+	MAC80211_LINKED_SCANNING = 3,
+};
+
+enum act_category {
+	ACT_CAT_QOS = 1,
+	ACT_CAT_DLS = 2,
+	ACT_CAT_BA = 3,
+	ACT_CAT_HT = 7,
+	ACT_CAT_WMM = 17,
+};
+
+enum ba_action {
+	ACT_ADDBAREQ = 0,
+	ACT_ADDBARSP = 1,
+	ACT_DELBA = 2,
+};
+
+enum rt_polarity_ctl {
+	RT_POLARITY_LOW_ACT = 0,
+	RT_POLARITY_HIGH_ACT = 1,
+};
+
+
+struct octet_string {
+	u8 *octet;
+	u16 length;
+};
+
+struct rtl_hdr_3addr {
+	__le16 frame_ctl;
+	__le16 duration_id;
+	u8 addr1[ETH_ALEN];
+	u8 addr2[ETH_ALEN];
+	u8 addr3[ETH_ALEN];
+	__le16 seq_ctl;
+	u8 payload[0];
+} __packed;
+
+struct rtl_info_element {
+	u8 id;
+	u8 len;
+	u8 data[0];
+} __packed;
+
+struct rtl_probe_rsp {
+	struct rtl_hdr_3addr header;
+	u32 time_stamp[2];
+	__le16 beacon_interval;
+	__le16 capability;
+	/*SSID, supported rates, FH params, DS params,
+	   CF params, IBSS params, TIM (if beacon), RSN */
+	struct rtl_info_element info_element[0];
+} __packed;
+
+/*LED related.*/
+/*ledpin Identify how to implement this SW led.*/
+struct rtl_led {
+	void *hw;
+	enum rtl_led_pin ledpin;
+	bool b_ledon;
+};
+
+struct rtl_led_ctl {
+	bool bled_opendrain;
+	struct rtl_led sw_led0;
+	struct rtl_led sw_led1;
+};
+
+struct rtl_qos_parameters {
+	__le16 cw_min;
+	__le16 cw_max;
+	u8 aifs;
+	u8 flag;
+	__le16 tx_op;
+} __packed;
+
+struct rt_smooth_data {
+	u32 elements[100];	/*array to store values */
+	u32 index;		/*index to current array to store */
+	u32 total_num;		/*num of valid elements */
+	u32 total_val;		/*sum of valid elements */
+};
+
+struct rtl_ht_agg {
+	u16 txq_id;
+	u16 wait_for_ba;
+	u16 start_idx;
+	u64 bitmap;
+	u32 rate_n_flags;
+	u8 agg_state;
+	u8 rx_agg_state;
+};
+
+struct rtl_tid_data {
+	u16 seq_number;
+	struct rtl_ht_agg agg;
+};
+
+struct rssi_sta{
+	long undecorated_smoothed_pwdb;
+};
+
+struct rtl_sta_info {
+	struct list_head list;
+	u8 ratr_index;
+	u8 wireless_mode;
+	u8 mimo_ps;
+	u8 mac_addr[6];
+	struct rtl_tid_data tids[MAX_TID_COUNT];
+
+	/* just used for ap adhoc or mesh*/
+	struct rssi_sta rssi_stat;
+} __packed;
+
+#ifdef VIF_TODO
+struct rtl_vif {
+	unsigned int id;
+	/* struct ieee80211_vif __rcu *vif; */
+	struct ieee80211_vif *vif;
+};
+
+struct rtl_vif_info {
+	struct list_head list;
+	bool active;
+	unsigned int id;
+	struct sk_buff *beacon;
+	bool enable_beacon;
+};
+
+struct vif_priv {
+	struct list_head vif_list;
+
+	/* interface mode settings */
+	unsigned long vif_bitmap;
+	unsigned int vifs;
+	struct rtl_vif vif[MAX_VIRTUAL_MAC];
+
+	/* beaconing */
+	spinlock_t beacon_lock;
+	unsigned int global_pretbtt;
+	unsigned int global_beacon_int;
+	/* struct rtl_vif_info __rcu *beacon_iter; */
+	struct rtl_vif_info *beacon_iter;
+	unsigned int beacon_enabled;
+};
+#endif
+
+struct false_alarm_statistics {
+	u32 cnt_parity_fail;
+	u32 cnt_rate_illegal;
+	u32 cnt_crc8_fail;
+	u32 cnt_mcs_fail;
+	u32 cnt_fast_fsync_fail;
+	u32 cnt_sb_search_fail;
+	u32 cnt_ofdm_fail;
+	u32 cnt_cck_fail;
+	u32 cnt_all;
+	u32 cnt_ofdm_cca;
+	u32 cnt_cck_cca;
+	u32 cnt_cca_all;
+	u32 cnt_bw_usc;
+	u32 cnt_bw_lsc;
+};
+
+struct init_gain {
+	u8 xaagccore1;
+	u8 xbagccore1;
+	u8 xcagccore1;
+	u8 xdagccore1;
+	u8 cca;
+
+};
+
+struct wireless_stats {
+	unsigned long txbytesunicast;
+	unsigned long txbytesmulticast;
+	unsigned long txbytesbroadcast;
+	unsigned long rxbytesunicast;
+
+	long rx_snr_db[4];
+	/*Correct smoothed ss in Dbm, only used
+	   in driver to report real power now. */
+	long recv_signal_power;
+	long signal_quality;
+	long last_sigstrength_inpercent;
+
+	u32 rssi_calculate_cnt;
+	u32 pwdb_all_cnt;
+
+	/*Transformed, in dbm. Beautified signal
+	   strength for UI, not correct. */
+	long signal_strength;
+
+	u8 rx_rssi_percentage[4];
+	u8 rx_evm_dbm[4];
+	u8 rx_evm_percentage[2];
+
+	u16 rx_cfo_short[4];
+	u16 rx_cfo_tail[4];
+
+	struct rt_smooth_data ui_rssi;
+	struct rt_smooth_data ui_link_quality;
+};
+
+struct rate_adaptive {
+	u8 rate_adaptive_disabled;
+	u8 ratr_state;
+	u16 reserve;
+
+	u32 high_rssi_thresh_for_ra;
+	u32 high2low_rssi_thresh_for_ra;
+	u8 low2high_rssi_thresh_for_ra;
+	u32 low_rssi_thresh_for_ra;
+	u32 upper_rssi_threshold_ratr;
+	u32 middleupper_rssi_threshold_ratr;
+	u32 middle_rssi_threshold_ratr;
+	u32 middlelow_rssi_threshold_ratr;
+	u32 low_rssi_threshold_ratr;
+	u32 ultralow_rssi_threshold_ratr;
+	u32 low_rssi_threshold_ratr_40m;
+	u32 low_rssi_threshold_ratr_20m;
+	u8 ping_rssi_enable;
+	u32 ping_rssi_ratr;
+	u32 ping_rssi_thresh_for_ra;
+	u32 last_ratr;
+	u8 pre_ratr_state;
+	u8 ldpc_thres;
+	bool use_ldpc;
+	bool lower_rts_rate;
+	bool is_special_data;
+};
+
+struct regd_pair_mapping {
+	u16 reg_dmnenum;
+	u16 reg_5ghz_ctl;
+	u16 reg_2ghz_ctl;
+};
+
+struct dynamic_primary_cca{
+	u8 pricca_flag;
+	u8 intf_flag;
+	u8 intf_type;
+	u8 dup_rts_flag;
+	u8 monitor_flag;
+	u8 ch_offset;
+	u8 mf_state;
+};
+
+struct rtl_regulatory {
+	char alpha2[2];
+	u16 country_code;
+	u16 max_power_level;
+	u32 tp_scale;
+	u16 current_rd;
+	u16 current_rd_ext;
+	int16_t power_limit;
+	struct regd_pair_mapping *regpair;
+};
+
+struct rtl_rfkill {
+	bool rfkill_state;	/*0 is off, 1 is on */
+};
+
+/*for P2P PS**/
+#define	P2P_MAX_NOA_NUM		2
+
+enum p2p_role {
+	P2P_ROLE_DISABLE = 0,
+	P2P_ROLE_DEVICE = 1,
+	P2P_ROLE_CLIENT = 2,
+	P2P_ROLE_GO = 3
+};
+
+enum p2p_ps_state {
+	P2P_PS_DISABLE = 0,
+	P2P_PS_ENABLE = 1,
+	P2P_PS_SCAN = 2,
+	P2P_PS_SCAN_DONE = 3,
+	P2P_PS_ALLSTASLEEP = 4, // for P2P GO
+};
+
+enum p2p_ps_mode {
+	P2P_PS_NONE = 0,
+	P2P_PS_CTWINDOW = 1,
+	P2P_PS_NOA = 2,
+	P2P_PS_MIX = 3, // CTWindow and NoA
+};
+
+struct rtl_p2p_ps_info {
+	enum p2p_ps_mode p2p_ps_mode; /* indicate p2p ps mode */
+	enum p2p_ps_state p2p_ps_state; /* indicate p2p ps state */
+	u8 noa_index; /* Identifies and instance of Notice of Absence timing. */
+	/* Client traffic window. A period of time in TU after TBTT. */
+	u8 ctwindow;
+	u8 opp_ps; /* opportunistic power save. */
+	u8 noa_num; /* number of NoA descriptor in P2P IE. */
+	/* Count for owner, Type of client. */
+	u8 noa_count_type[P2P_MAX_NOA_NUM];
+	/* Max duration for owner, preferred or
+	 * min acceptable duration for client. */
+	u32 noa_duration[P2P_MAX_NOA_NUM];
+	/* Length of interval for owner, preferred or
+	 * max acceptable interval of client. */
+	u32 noa_interval[P2P_MAX_NOA_NUM];
+	/* schedule expressed in terms of the lower 4 bytes of the TSF timer. */
+	u32 noa_start_time[P2P_MAX_NOA_NUM];
+};
+
+ struct p2p_ps_offload_t {
+	u8 Offload_En:1;
+	u8 role:1; /* 1: Owner, 0: Client */
+	u8 CTWindow_En:1;
+	u8 NoA0_En:1;
+	u8 NoA1_En:1;
+	u8 AllStaSleep:1;
+	u8 discovery:1;
+	u8 reserved:1;
+};
+
+#define IQK_MATRIX_REG_NUM	8
+#define IQK_MATRIX_SETTINGS_NUM	 (14+24+21) // Channels_2_4G_NUM + Channels_5G_20M_NUM + Channels_5G
+struct iqk_matrix_regs {
+	bool b_iqk_done;
+	long value[1][IQK_MATRIX_REG_NUM];
+};
+
+struct rtl_phy {
+	struct bb_reg_def phyreg_def[4];	/*Radio A/B/C/D */
+	struct init_gain initgain_backup;
+	enum io_type current_io_type;
+
+	u8 rf_mode;
+	u8 rf_type;
+	u8 current_chan_bw;
+	u8 set_bwmode_inprogress;
+	u8 sw_chnl_inprogress;
+	u8 sw_chnl_stage;
+	u8 sw_chnl_step;
+	u8 current_channel;
+	u8 h2c_box_num;
+	u8 set_io_inprogress;
+	u8 lck_inprogress;
+
+	/* record for power tracking */
+	s32 reg_e94;
+	s32 reg_e9c;
+	s32 reg_ea4;
+	s32 reg_eac;
+	s32 reg_eb4;
+	s32 reg_ebc;
+	s32 reg_ec4;
+	s32 reg_ecc;
+	u8 rfpienable;
+	u8 reserve_0;
+	u16 reserve_1;
+	u32 reg_c04, reg_c08, reg_874;
+	u32 adda_backup[16];
+	u32 iqk_mac_backup[IQK_MAC_REG_NUM];
+	u32 iqk_bb_backup[10];
+	bool iqk_initialized;
+
+	bool rfpath_rx_enable[MAX_RF_PATH];
+	/*Jaguar*/
+	u8 reg_837;
+	/* Dul mac */
+	bool b_need_iqk;
+	struct iqk_matrix_regs iqk_matrix_regsetting[IQK_MATRIX_SETTINGS_NUM];
+
+	bool b_rfpi_enable;
+
+	bool b_iqk_in_progress;
+
+	u8 pwrgroup_cnt;
+	u8 bcck_high_power;
+	/* this is for 88E & 8723A */
+	u32 mcs_txpwrlevel_origoffset[MAX_PG_GROUP][16];
+	/* this is for 92EE */
+	u32 tx_power_by_rate_offset[TX_PWR_BY_RATE_NUM_BAND]
+				   [TX_PWR_BY_RATE_NUM_RF]
+				   [TX_PWR_BY_RATE_NUM_RF]
+				   [TX_PWR_BY_RATE_NUM_SECTION];
+	u8 txpwr_by_rate_base_24g[TX_PWR_BY_RATE_NUM_RF]
+				 [TX_PWR_BY_RATE_NUM_RF]
+				 [MAX_BASE_NUM_IN_PHY_REG_PG_24G];
+
+	u8 txpwr_by_rate_base_5g[TX_PWR_BY_RATE_NUM_RF]
+				[TX_PWR_BY_RATE_NUM_RF]
+				[MAX_BASE_NUM_IN_PHY_REG_PG_5G];
+	u8 default_initialgain[4];
+
+	/* the current Tx power level */
+	u8 cur_cck_txpwridx;
+	u8 cur_ofdm24g_txpwridx;
+	u8 cur_bw20_txpwridx;
+	u8 cur_bw40_txpwridx;
+
+	u32 rfreg_chnlval[2];
+	bool b_apk_done;
+	u32 reg_rf3c[2];	/* pathA / pathB  */
+
+	u32 backup_rf_0x1a;/*92ee*/
+	/* bfsync */
+	u8 framesync;
+	u32 framesync_c34;
+
+	u8 num_total_rfpath;
+	u16 rf_pathmap;
+
+	u8 hw_rof_enable; /*Enable GPIO[9] as WL RF HW PDn source*/
+
+	enum rt_polarity_ctl polarity_ctl;
+};
+
+#define RTL_AGG_STOP 						0
+#define RTL_AGG_PROGRESS					1
+#define RTL_AGG_START 						2
+#define RTL_AGG_OPERATIONAL					3
+#define RTL_RX_AGG_START 					1
+#define RTL_RX_AGG_STOP 					0
+
+struct rtl_priv;
+struct rtl_io {
+	struct device *dev;
+
+	/*PCI MEM map */
+	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;
+
+	/*AP*/
+	u8 bssid[6];
+	u32 vendor;
+	u32 basic_rates; /* b/g rates */
+	u8 ht_enable;
+	u8 bw_40;
+	u8 mode;		/* wireless mode */
+	u8 slot_time;
+	u8 short_preamble;
+	u8 use_cts_protect;
+	u8 cur_40_prime_sc;
+	u8 cur_40_prime_sc_bk;
+	u8 cur_80_prime_sc;
+	u64 tsf;
+	u8 retry_short;
+	u8 retry_long;
+	u16 assoc_id;
+	bool bhiddenssid;
+
+	/*IBSS*/
+	int beacon_interval;
+
+	/*AMPDU*/
+	u8 min_space_cfg;	/*For Min spacing configurations */
+	u8 max_mss_density;
+	u8 current_ampdu_factor;
+	u8 current_ampdu_density;
+
+	/*QOS & EDCA */
+	struct ieee80211_tx_queue_params edca_param[RTL_MAC80211_NUM_QUEUE];
+	struct rtl_qos_parameters ac[AC_MAX];
+};
+
+struct rtl_hal {
+	struct ieee80211_hw *hw;
+
+	bool driver_is_goingto_unload;
+	bool up_first_time;
+	bool bfirst_init;
+	bool being_init_adapter;
+	bool b_bbrf_ready;
+	bool b_mac_func_enable;
+	bool b_pre_edcca_enable;
+
+	enum intf_type interface;
+	u16 hw_type;		/*92c or 92d or 92s and so on */
+	u8 ic_class;
+	u8 oem_id;
+	u32 version;		/*version of chip */
+	u8 state;		/*stop 0, start 1 */
+	u8 boad_type;
+
+	/*firmware */
+	u32 fwsize;
+	u8 *pfirmware;
+	u16 fw_version;
+	u16 fw_subversion;
+	bool b_h2c_setinprogress;
+	u8 last_hmeboxnum;
+	bool bfw_ready;
+
+	/*Reserve page start offset except beacon in TxQ. */
+	u8 fw_rsvdpage_startoffset;
+	u8 h2c_txcmd_seq;
+	u8 current_ra_rate;
+
+	/* FW Cmd IO related */
+	u16 fwcmd_iomap;
+	u32 fwcmd_ioparam;
+	bool set_fwcmd_inprogress;
+	u8 current_fwcmd_io;
+
+	bool bfw_clk_change_in_progress;
+	bool ballow_sw_to_change_hwclc;
+	u8 fw_ps_state;
+	 struct p2p_ps_offload_t p2p_ps_offload;
+	/**/
+	bool driver_going2unload;
+
+	/*AMPDU init min space*/
+	u8 minspace_cfg;	/*For Min spacing configurations */
+
+	/* Dul mac */
+	enum macphy_mode macphymode;
+	enum band_type current_bandtype;	/* 0:2.4G, 1:5G */
+	enum band_type current_bandtypebackup;
+	enum band_type bandset;
+	/* dual MAC 0--Mac0 1--Mac1 */
+	u32 interfaceindex;
+	/* just for DulMac S3S4 */
+	u8 macphyctl_reg;
+	bool b_earlymode_enable;
+	u8 max_earlymode_num;
+	/* Dul mac*/
+	bool during_mac0init_radiob;
+	bool during_mac1init_radioa;
+	bool reloadtxpowerindex;
+	/* True if IMR or IQK  have done
+	for 2.4G in scan progress */
+	bool b_load_imrandiqk_setting_for2g;
+
+	bool disable_amsdu_8k;
+	bool bmaster_of_dmsp;
+	bool bslave_of_dmsp;
+
+	u16 rx_tag;/*for 92ee*/
+	u8 rts_en;
+};
+
+struct rtl_security {
+	/*default 0 */
+	bool use_sw_sec;
+
+	bool being_setkey;
+	bool use_defaultkey;
+	/*Encryption Algorithm for Unicast Packet */
+	enum rt_enc_alg pairwise_enc_algorithm;
+	/*Encryption Algorithm for 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];
+	u8 default_ofdm_index;
+	u8 default_cck_index;
+	char cck_index;
+	char delta_power_index[MAX_RF_PATH];
+	char delta_power_index_last[MAX_RF_PATH];
+	char power_index_offset[MAX_RF_PATH];
+	char aboslute_ofdm_swing_idx[MAX_RF_PATH];
+	char remnant_ofdm_swing_idx[MAX_RF_PATH];
+	char remnant_cck_idx;
+	bool modify_txagc_flag_path_a;
+	bool modify_txagc_flag_path_b;
+
+	bool b_one_entry_only;
+	struct dm_phy_dbg_info dbginfo;
+	/* Dynamic ATC switch */
+
+	bool atc_status;
+	bool large_cfo_hit;
+	bool is_freeze;
+	int cfo_tail[2];
+	int cfo_ave_pre;
+	int crystal_cap;
+	u8 cfo_threshold;
+	u32 packet_count;
+	u32 packet_count_pre;
+	u8 tx_rate;
+
+
+	/*88e tx power tracking*/
+	u8 bb_swing_idx_ofdm[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;
+
+	u8 delta_swing_table_idx_24gccka_p[DELTA_SWINGIDX_SIZE];
+    u8 delta_swing_table_idx_24gccka_n[DELTA_SWINGIDX_SIZE];
+    u8 delta_swing_table_idx_24gcckb_p[DELTA_SWINGIDX_SIZE];
+	u8 delta_swing_table_idx_24gcckb_n[DELTA_SWINGIDX_SIZE];
+	u8 delta_swing_table_idx_24ga_p[DELTA_SWINGIDX_SIZE];
+   	u8 delta_swing_table_idx_24ga_n[DELTA_SWINGIDX_SIZE];
+    u8 delta_swing_table_idx_24gb_p[DELTA_SWINGIDX_SIZE];
+   	u8 delta_swing_table_idx_24gb_n[DELTA_SWINGIDX_SIZE];
+    u8 delta_swing_table_idx_5ga_p[BAND_NUM][DELTA_SWINGIDX_SIZE];
+    u8 delta_swing_table_idx_5ga_n[BAND_NUM][DELTA_SWINGIDX_SIZE];
+    u8 delta_swing_table_idx_5gb_p[BAND_NUM][DELTA_SWINGIDX_SIZE];
+    u8 delta_swing_table_idx_5gb_n[BAND_NUM][DELTA_SWINGIDX_SIZE];
+    u8 delta_swing_table_idx_24ga_p_8188e[DELTA_SWINGIDX_SIZE];
+    u8 delta_swing_table_idx_24ga_n_8188e[DELTA_SWINGIDX_SIZE];
+
+
+	/* DMSP */
+	bool supp_phymode_switch;
+
+	/* DulMac */
+	struct rtl_dig dm_digtable;
+	struct rtl_pstbl dm_pstable;
+	struct fast_ant_trainning fat_table;
+
+	u8	resp_tx_path;
+	u8 	path_sel;
+	u32	patha_sum;
+	u32	pathb_sum;
+	u32	patha_cnt;
+	u32	pathb_cnt;
+
+	u8 pre_channel;
+	u8 *p_channel;
+	u8 linked_interval;
+
+	u64 last_tx_ok_cnt;
+	u64 last_rx_ok_cnt;
+};
+
+#define	EFUSE_MAX_LOGICAL_SIZE		256
+
+struct rtl_efuse {
+	bool bautoLoad_ok;
+	bool bootfromefuse;
+	u16 max_physical_size;
+
+	u8 efuse_map[2][EFUSE_MAX_LOGICAL_SIZE];
+	u16 efuse_usedbytes;
+	u8 efuse_usedpercentage;
+#ifdef EFUSE_REPG_WORKAROUND
+	bool efuse_re_pg_sec1flag;
+	u8 efuse_re_pg_data[8];
+#endif
+
+	u8 autoload_failflag;
+	u8 autoload_status;
+
+	short epromtype;
+	u16 eeprom_vid;
+	u16 eeprom_did;
+	u16 eeprom_svid;
+	u16 eeprom_smid;
+	u8 eeprom_oemid;
+	u16 eeprom_channelplan;
+	u8 eeprom_version;
+
+	u8 dev_addr[6];
+	u8 board_type;
+	u8 wowlan_enable;
+	u8 antenna_div_cfg;
+	u8 antenna_div_type;
+
+	bool b_txpwr_fromeprom;
+	u8 eeprom_crystalcap;
+	u8 eeprom_tssi[2];
+	u8 eeprom_tssi_5g[3][2]; /* for 5GL/5GM/5GH band. */
+	u8 eeprom_pwrlimit_ht20[CHANNEL_GROUP_MAX];
+	u8 eeprom_pwrlimit_ht40[CHANNEL_GROUP_MAX];
+	u8 eeprom_chnlarea_txpwr_cck[2][CHANNEL_GROUP_MAX_2G];
+	u8 eeprom_chnlarea_txpwr_ht40_1s[2][CHANNEL_GROUP_MAX];
+	u8 eeprom_chnlarea_txpwr_ht40_2sdiif[2][CHANNEL_GROUP_MAX];
+
+
+	u8 internal_pa_5g[2];	/* pathA / pathB */
+	u8 eeprom_c9;
+	u8 eeprom_cc;
+
+	/*For power group */
+	u8 eeprom_pwrgroup[2][3];
+	u8 pwrgroup_ht20[2][CHANNEL_MAX_NUMBER];
+	u8 pwrgroup_ht40[2][CHANNEL_MAX_NUMBER];
+
+	u8 txpwrlevel_cck[MAX_RF_PATH][CHANNEL_MAX_NUMBER_2G];
+	/*For HT 40MHZ pwr */
+	u8 txpwrlevel_ht40_1s[MAX_RF_PATH][CHANNEL_MAX_NUMBER];
+	/*For HT 40MHZ pwr */
+	u8 txpwrlevel_ht40_2s[MAX_RF_PATH][CHANNEL_MAX_NUMBER];
+	char txpwr_cckdiff[MAX_RF_PATH][MAX_TX_COUNT]; /*CCK_24G_Diff*/
+	/*HT 20<->40 Pwr diff */
+	char txpwr_ht20diff[MAX_RF_PATH][MAX_TX_COUNT];	/*BW20_24G_Diff*/
+	char txpwr_ht40diff[MAX_RF_PATH][MAX_TX_COUNT];/*BW40_24G_Diff*/
+	/*For HT<->legacy pwr diff */
+	char txpwr_legacyhtdiff[MAX_RF_PATH][MAX_TX_COUNT];/*OFDM_24G_Diff*/
+
+	u8 txpwr_5g_bw40base[MAX_RF_PATH][CHANNEL_MAX_NUMBER];
+	u8 txpwr_5g_bw80base[MAX_RF_PATH][CHANNEL_MAX_NUMBER_5G_80M];
+	char txpwr_5g_ofdmdiff[MAX_RF_PATH][MAX_TX_COUNT];
+	char txpwr_5g_bw20diff[MAX_RF_PATH][MAX_TX_COUNT];
+	char txpwr_5g_bw40diff[MAX_RF_PATH][MAX_TX_COUNT];
+	char txpwr_5g_bw80diff[MAX_RF_PATH][MAX_TX_COUNT];
+
+	u8 txpwr_safetyflag;		/* Band edge enable flag */
+	u16 eeprom_txpowerdiff;
+	u8 legacy_httxpowerdiff;	/* Legacy to HT rate power diff */
+	u8 antenna_txpwdiff[3];
+
+	u8 eeprom_regulatory;
+	u8 eeprom_thermalmeter;
+	u8 thermalmeter[2];/*ThermalMeter, index 0 for RFIC0, and 1 for RFIC1 */
+	u16 tssi_13dbm;
+	u8 crystalcap;		/* CrystalCap. */
+	u8 delta_iqk;
+	u8 delta_lck;
+
+	u8 legacy_ht_txpowerdiff;	/*Legacy to HT rate power diff */
+	bool b_apk_thermalmeterignore;
+
+	bool b1x1_recvcombine;
+	bool b1ss_support;
+
+	/*channel plan */
+	u8 channel_plan;
+};
+
+struct rtl_ps_ctl {
+	bool pwrdomain_protect;
+	bool b_in_powersavemode;
+	bool rfchange_inprogress;
+	bool b_swrf_processing;
+	bool b_hwradiooff;
+	/*
+	 * just for PCIE ASPM
+	 * If it supports ASPM, Offset[560h] = 0x40,
+	 * otherwise Offset[560h] = 0x00.
+	 * */
+	bool b_support_aspm;
+	bool b_support_backdoor;
+
+	/*for LPS */
+	enum rt_psmode dot11_psmode;	/*Power save mode configured. */
+	bool b_swctrl_lps;
+	bool b_fwctrl_lps;
+	u8 fwctrl_psmode;
+	/*For Fw control LPS mode */
+	u8 b_reg_fwctrl_lps;
+	/*Record Fw PS mode status. */
+	bool b_fw_current_inpsmode;
+	u8 reg_max_lps_awakeintvl;
+	bool report_linked;
+	bool b_low_power_enable;/*for 32k*/
+
+	/*for IPS */
+	bool b_inactiveps;
+
+	u32 rfoff_reason;
+
+	/*RF OFF Level */
+	u32 cur_ps_level;
+	u32 reg_rfps_level;
+
+	/*just for PCIE ASPM */
+	u8 const_amdpci_aspm;
+
+	enum rf_pwrstate inactive_pwrstate;
+	enum rf_pwrstate rfpwr_state;	/*cur power state */
+
+	/* for SW LPS*/
+	bool sw_ps_enabled;
+	bool state;
+	bool state_inap;
+	bool multi_buffered;
+	u16 nullfunc_seq;
+	unsigned int dtim_counter;
+	unsigned int sleep_ms;
+	unsigned long last_sleep_jiffies;
+	unsigned long last_awake_jiffies;
+	unsigned long last_delaylps_stamp_jiffies;
+	unsigned long last_dtim;
+	unsigned long last_beacon;
+	unsigned long last_action;
+	unsigned long last_slept;
+
+	/*For P2P PS */
+	struct rtl_p2p_ps_info p2p_ps_info;
+	u8 pwr_mode;
+	u8 smart_ps;
+};
+
+struct rtl_stats {
+	u8 psaddr[ETH_ALEN];
+	u32 mac_time[2];
+	s8 rssi;
+	u8 signal;
+	u8 noise;
+	u8 rate;		/* hw desc rate */
+	u8 rawdata;
+	u8 received_channel;
+	u8 control;
+	u8 mask;
+	u8 freq;
+	u16 len;
+	u64 tsf;
+	u32 beacon_time;
+	u8 nic_type;
+	u16 length;
+	u8 signalquality;	/*in 0-100 index. */
+	/*
+	 * Real power in dBm for this packet,
+	 * no beautification and aggregation.
+	 * */
+	s32 recvsignalpower;
+	s8 rxpower;		/*in dBm Translate from PWdB */
+	u8 signalstrength;	/*in 0-100 index. */
+	u16 b_hwerror:1;
+	u16 b_crc:1;
+	u16 b_icv:1;
+	u16 b_shortpreamble:1;
+	u16 antenna:1;
+	u16 decrypted:1;
+	u16 wakeup:1;
+	u32 timestamp_low;
+	u32 timestamp_high;
+	bool b_shift;
+
+	u8 rx_drvinfo_size;
+	u8 rx_bufshift;
+	bool b_isampdu;
+	bool b_isfirst_ampdu;
+	bool rx_is40Mhzpacket;
+	u32 rx_pwdb_all;
+	u8 rx_mimo_signalstrength[4];	/*in 0~100 index */
+	s8 rx_mimo_signalquality[4];
+	u8 rx_mimo_evm_dbm[4];
+	u16 cfo_short[4]; 		/* per-path's Cfo_short */
+	u16 cfo_tail[4];
+
+	u8 rx_pwr[4]; /* per-path's pwdb */
+	u8 rx_snr[4]; /* per-path's SNR */
+	u8 bandwidth;
+	u8 bt_coex_pwr_adjust;
+	bool b_packet_matchbssid;
+	bool b_is_cck;
+	bool b_is_ht;
+	bool b_packet_toself;
+	bool b_packet_beacon;	/*for rssi */
+	char cck_adc_pwdb[4];	/*for rx path selection */
+
+	u8 packet_report_type;
+
+	u32 macid;
+	u8 wake_match;
+	u32 bt_rx_rssi_percentage;
+	u32 macid_valid_entry[2];
+};
+
+struct rt_link_detect {
+	/* count for 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 b_packet_bw:1;
+	u8 b_multicast:1;
+	u8 b_broadcast:1;
+
+	u8 b_rts_stbc:1;
+	u8 b_rts_enable:1;
+	u8 b_cts_enable:1;
+	u8 b_rts_use_shortpreamble:1;
+	u8 b_rts_use_shortgi:1;
+	u8 rts_sc:1;
+	u8 b_rts_bw:1;
+	u8 rts_rate;
+
+	u8 use_shortgi:1;
+	u8 use_shortpreamble:1;
+	u8 use_driver_rate:1;
+	u8 disable_ratefallback:1;
+
+	u8 ratr_index;
+	u8 mac_id;
+	u8 hw_rate;
+
+	u8 b_last_inipkt:1;
+	u8 b_cmd_or_init:1;
+	u8 queue_index;
+
+	/* early mode */
+	u8 empkt_num;
+	/* The max value by HW */
+	u32 empkt_len[10];
+	bool btx_enable_sw_calc_duration;
+	/* used for hal construct pkt,
+	 * we may set desc when tx */
+	u8 self_desc;
+};
+
+struct proxim {
+	bool proxim_on;
+
+	void *proximity_priv;
+	int (*proxim_rx)(struct ieee80211_hw *hw, struct rtl_stats *status,
+			 struct sk_buff *skb);
+	u8  (*proxim_get_var)(struct ieee80211_hw *hw, u8 type);
+};
+
+struct rtl_hal_ops {
+	int (*init_sw_vars) (struct ieee80211_hw * hw);
+	void (*deinit_sw_vars) (struct ieee80211_hw * hw);
+	void (*read_eeprom_info) (struct ieee80211_hw * hw);
+	void (*interrupt_recognized) (struct ieee80211_hw * hw,
+				      u32 * p_inta, u32 * p_intb);
+	int (*hw_init) (struct ieee80211_hw * hw);
+	void (*hw_disable) (struct ieee80211_hw * hw);
+	void (*hw_suspend) (struct ieee80211_hw * hw);
+	void (*hw_resume) (struct ieee80211_hw * hw);
+	void (*enable_interrupt) (struct ieee80211_hw * hw);
+	void (*disable_interrupt) (struct ieee80211_hw * hw);
+	int (*set_network_type) (struct ieee80211_hw * hw,
+				 enum nl80211_iftype type);
+	void (*set_chk_bssid)(struct ieee80211_hw *hw,
+			      bool check_bssid);
+	void (*set_bw_mode) (struct ieee80211_hw * hw,
+			     enum nl80211_channel_type ch_type);
+	 u8(*switch_channel) (struct ieee80211_hw * hw);
+	void (*set_qos) (struct ieee80211_hw * hw, int aci);
+	void (*set_bcn_reg) (struct ieee80211_hw * hw);
+	void (*set_bcn_intv) (struct ieee80211_hw * hw);
+	void (*update_interrupt_mask) (struct ieee80211_hw * hw,
+				       u32 add_msr, u32 rm_msr);
+	void (*get_hw_reg) (struct ieee80211_hw * hw, u8 variable, u8 * val);
+	void (*set_hw_reg) (struct ieee80211_hw * hw, u8 variable, u8 * val);
+	void (*update_rate_tbl) (struct ieee80211_hw * hw,
+				 struct ieee80211_sta *sta, u8 rssi_level);
+	void (*pre_fill_tx_bd_desc) (struct ieee80211_hw *hw, u8 *tx_bd_desc,
+				     u8 *desc, u8 queue_index,
+				     struct sk_buff *skb, dma_addr_t addr);
+	u16 (*rx_desc_buff_remained_cnt) (struct ieee80211_hw *hw,
+					  u8 queue_index);
+	void (*rx_check_dma_ok) (struct ieee80211_hw *hw, u8 *header_desc,
+				 u8 queue_index);
+	void (*fill_tx_desc) (struct ieee80211_hw * hw,
+			      struct ieee80211_hdr * hdr,
+			      u8 * pdesc_tx, u8 * pbd_desc,
+			      struct ieee80211_tx_info * info,
+/*<delete in kernel start>*/
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,7,0))
+/*<delete in kernel end>*/
+			      struct ieee80211_sta *sta,
+/*<delete in kernel start>*/
+#endif
+/*<delete in kernel end>*/
+			      struct sk_buff * skb, u8 hw_queue,
+			      struct rtl_tcb_desc *ptcb_desc);
+	void (*fill_tx_cmddesc) (struct ieee80211_hw * hw, u8 * pdesc,
+				 bool b_firstseg, bool b_lastseg,
+				 struct sk_buff * skb);
+	 bool(*query_rx_desc) (struct ieee80211_hw * hw,
+			       struct rtl_stats * status,
+			       struct ieee80211_rx_status * rx_status,
+			       u8 * pdesc, struct sk_buff * skb);
+	void (*set_channel_access) (struct ieee80211_hw * hw);
+	 bool(*radio_onoff_checking) (struct ieee80211_hw * hw, u8 * valid);
+	void (*dm_watchdog) (struct ieee80211_hw * hw);
+	void (*scan_operation_backup) (struct ieee80211_hw * hw, u8 operation);
+	 bool(*set_rf_power_state) (struct ieee80211_hw * hw,
+				    enum rf_pwrstate rfpwr_state);
+	void (*led_control) (struct ieee80211_hw * hw,
+			     enum led_ctl_mode ledaction);
+	void (*set_desc) (struct ieee80211_hw *hw, u8 * pdesc, bool istx,
+			  u8 desc_name, u8 * val);
+	 u32(*get_desc) (u8 * pdesc, bool istx, u8 desc_name);
+	bool (*is_tx_desc_closed) (struct ieee80211_hw *hw,
+				   u8 hw_queue, u16 index);
+	void (*tx_polling) (struct ieee80211_hw * hw, u8 hw_queue);
+	void (*enable_hw_sec) (struct ieee80211_hw * hw);
+	void (*set_key) (struct ieee80211_hw * hw, u32 key_index,
+			 u8 * p_macaddr, bool is_group, u8 enc_algo,
+			 bool is_wepkey, bool clear_all);
+	void (*init_sw_leds) (struct ieee80211_hw * hw);
+	 u32(*get_bbreg) (struct ieee80211_hw * hw, u32 regaddr, u32 bitmask);
+	void (*set_bbreg) (struct ieee80211_hw * hw, u32 regaddr, u32 bitmask,
+			   u32 data);
+	 u32(*get_rfreg) (struct ieee80211_hw * hw, enum radio_path rfpath,
+			  u32 regaddr, u32 bitmask);
+	void (*set_rfreg) (struct ieee80211_hw * hw, enum radio_path rfpath,
+			   u32 regaddr, u32 bitmask, u32 data);
+	void (*allow_all_destaddr)(struct ieee80211_hw *hw,
+		bool allow_all_da, bool write_into_reg);
+	void (*linked_set_reg) (struct ieee80211_hw * hw);
+	void (*check_switch_to_dmdp) (struct ieee80211_hw * hw);
+	void (*dualmac_easy_concurrent) (struct ieee80211_hw *hw);
+	void (*dualmac_switch_to_dmdp) (struct ieee80211_hw *hw);
+	void (*c2h_command_handle) (struct ieee80211_hw *hw);
+	void (*bt_wifi_media_status_notify) (struct ieee80211_hw *hw, bool mstate);
+	void (*bt_turn_off_bt_coexist_before_enter_lps) (struct ieee80211_hw *hw);
+	void (*fill_h2c_cmd) (struct ieee80211_hw *hw, u8 element_id,
+			      u32 cmd_len, u8 *p_cmdbuffer);
+	bool (*get_btc_status) (void);
+  	u32 (*rx_command_packet_handler)(struct ieee80211_hw *hw, struct rtl_stats status, struct sk_buff *skb);
+};
+
+struct rtl_intf_ops {
+	/*com */
+	void (*read_efuse_byte)(struct ieee80211_hw *hw, u16 _offset, u8 *pbuf);
+	int (*adapter_start) (struct ieee80211_hw * hw);
+	void (*adapter_stop) (struct ieee80211_hw * hw);
+	bool (*check_buddy_priv)(struct ieee80211_hw *hw,
+			struct rtl_priv **buddy_priv);
+
+/*<delete in kernel start>*/
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0))
+	int (*adapter_tx) (struct ieee80211_hw * hw, struct sk_buff * skb,
+			   struct rtl_tcb_desc *ptcb_desc);
+#else
+/*<delete in kernel end>*/
+	int (*adapter_tx) (struct ieee80211_hw *hw,
+			   struct ieee80211_sta *sta,
+			   struct sk_buff *skb,
+			   struct rtl_tcb_desc *ptcb_desc);
+/*<delete in kernel start>*/
+#endif
+/*<delete in kernel end>*/
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,10,0))
+	void (*flush)(struct ieee80211_hw *hw, u32 queues, bool drop);
+#else
+	void (*flush)(struct ieee80211_hw *hw, bool drop);
+#endif
+	int (*reset_trx_ring) (struct ieee80211_hw * hw);
+/*<delete in kernel start>*/
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0))
+	bool (*waitq_insert) (struct ieee80211_hw *hw, struct sk_buff *skb);
+#else
+/*<delete in kernel end>*/
+	bool (*waitq_insert) (struct ieee80211_hw *hw,
+			      struct ieee80211_sta *sta,
+			      struct sk_buff *skb);
+/*<delete in kernel start>*/
+#endif
+/*<delete in kernel end>*/
+
+	/*pci */
+	void (*disable_aspm) (struct ieee80211_hw * hw);
+	void (*enable_aspm) (struct ieee80211_hw * hw);
+
+	/*usb */
+};
+
+struct rtl_mod_params {
+	/* default: 0 = using hardware encryption */
+	bool sw_crypto;
+
+	/* default: 1 = using no linked power save */
+	bool b_inactiveps;
+
+	/* default: 1 = using linked sw power save */
+	bool b_swctrl_lps;
+
+	/* default: 1 = using linked fw power save */
+	bool b_fwctrl_lps;
+};
+
+struct rtl_hal_cfg {
+	u8 bar_id;
+	bool write_readback;
+	char *name;
+	char *fw_name;
+	struct rtl_hal_ops *ops;
+	struct rtl_mod_params *mod_params;
+
+	/*this map used for some registers or vars
+	   defined int HAL but used in MAIN */
+	u32 maps[RTL_VAR_MAP_MAX];
+
+};
+
+struct rtl_locks {
+	/* mutex */
+	struct mutex conf_mutex;
+
+	/*spin lock */
+	spinlock_t ips_lock;
+	spinlock_t irq_th_lock;
+	spinlock_t h2c_lock;
+	spinlock_t rf_ps_lock;
+	spinlock_t rf_lock;
+	spinlock_t lps_lock;
+	spinlock_t waitq_lock;
+	spinlock_t entry_list_lock;
+
+	/*FW clock change */
+	spinlock_t fw_ps_lock;
+
+	/*Dul mac*/
+	spinlock_t cck_and_rw_pagea_lock;
+
+	/*Easy concurrent*/
+	spinlock_t check_sendpkt_lock;
+
+	spinlock_t iqk_lock;
+};
+
+struct rtl_works {
+	struct ieee80211_hw *hw;
+
+	/*timer */
+	struct timer_list watchdog_timer;
+	struct timer_list dualmac_easyconcurrent_retrytimer;
+	struct timer_list fw_clockoff_timer;
+	struct timer_list fast_antenna_trainning_timer;
+	/*task */
+	struct tasklet_struct irq_tasklet;
+	struct tasklet_struct irq_prepare_bcn_tasklet;
+
+	/*work queue */
+	struct workqueue_struct *rtl_wq;
+	struct delayed_work watchdog_wq;
+	struct delayed_work ips_nic_off_wq;
+
+	/* For SW LPS */
+	struct delayed_work ps_work;
+	struct delayed_work ps_rfon_wq;
+	struct delayed_work fwevt_wq;
+};
+
+struct rtl_debug {
+	u32 dbgp_type[DBGP_TYPE_MAX];
+	u32 global_debuglevel;
+	u64 global_debugcomponents;
+
+	/* add for proc debug */
+	struct proc_dir_entry *proc_dir;
+	char proc_name[20];
+};
+
+#define MIMO_PS_STATIC			0
+#define MIMO_PS_DYNAMIC			1
+#define MIMO_PS_NOLIMIT			3
+
+struct rtl_dualmac_easy_concurrent_ctl {
+	enum band_type currentbandtype_backfordmdp;
+	bool bclose_bbandrf_for_dmsp;
+	bool bchange_to_dmdp;
+	bool bchange_to_dmsp;
+	bool bswitch_in_process;
+};
+
+struct rtl_dmsp_ctl {
+	bool bactivescan_for_slaveofdmsp;
+	bool bscan_for_anothermac_fordmsp;
+	bool bscan_for_itself_fordmsp;
+	bool bwritedig_for_anothermacofdmsp;
+	u32 curdigvalue_for_anothermacofdmsp;
+	bool bchangecckpdstate_for_anothermacofdmsp;
+	u8 curcckpdstate_for_anothermacofdmsp;
+	bool bchangetxhighpowerlvl_for_anothermacofdmsp;
+	u8 curtxhighlvl_for_anothermacofdmsp;
+	long rssivalmin_for_anothermacofdmsp;
+};
+
+struct rtl_global_var {
+	/* from this list we can get
+	 * other adapter's rtl_priv */
+	struct list_head glb_priv_list;
+	spinlock_t glb_list_lock;
+};
+
+struct rtl_btc_info {
+	u8 bt_type;
+	u8 btcoexist;
+	u8 ant_num;
+};
+
+struct rtl_btc_ops {
+	void (*btc_init_variables) (struct rtl_priv *rtlpriv);
+	void (*btc_init_hal_vars) (struct rtl_priv *rtlpriv);
+	void (*btc_init_hw_config) (struct rtl_priv *rtlpriv);
+	void (*btc_ips_notify) (struct rtl_priv *rtlpriv, u8 type);
+	void (*btc_scan_notify) (struct rtl_priv *rtlpriv, u8 scantype);
+	void (*btc_connect_notify) (struct rtl_priv *rtlpriv, u8 action);
+	void (*btc_mediastatus_notify) (struct rtl_priv *rtlpriv,
+					enum rt_media_status mstatus);
+	void (*btc_periodical) (struct rtl_priv *rtlpriv);
+	void (*btc_halt_notify) (void);
+	void (*btc_btinfo_notify) (struct rtl_priv *rtlpriv,
+				   u8 * tmp_buf, u8 length);
+	bool (*btc_is_limited_dig) (struct rtl_priv *rtlpriv);
+	bool (*btc_is_disable_edca_turbo) (struct rtl_priv *rtlpriv);
+	bool (*btc_is_bt_disabled) (struct rtl_priv *rtlpriv);
+};
+
+struct rtl_bt_coexist {
+	struct rtl_btc_ops *btc_ops;
+	struct rtl_btc_info btc_info;
+};
+
+
+struct rtl_priv {
+	struct list_head list;
+#ifdef VIF_TODO
+	struct vif_priv vif_priv;
+#endif
+	struct rtl_priv *buddy_priv;
+	struct rtl_global_var *glb_var;
+	struct rtl_dualmac_easy_concurrent_ctl easy_concurrent_ctl;
+	struct rtl_dmsp_ctl dmsp_ctl;
+	struct rtl_locks locks;
+	struct rtl_works works;
+	struct rtl_mac mac80211;
+	struct rtl_hal rtlhal;
+	struct rtl_regulatory regd;
+	struct rtl_rfkill rfkill;
+	struct rtl_io io;
+	struct rtl_phy phy;
+	struct rtl_dm dm;
+	struct rtl_security sec;
+	struct rtl_efuse efuse;
+
+	struct rtl_ps_ctl psc;
+	struct rate_adaptive ra;
+	struct dynamic_primary_cca primarycca;
+	struct wireless_stats stats;
+	struct rt_link_detect link_info;
+	struct false_alarm_statistics falsealm_cnt;
+
+	struct rtl_rate_priv *rate_priv;
+
+	struct rtl_debug dbg;
+
+	/* sta entry list for ap adhoc or mesh */
+	struct list_head entry_list;
+
+	/*
+	 *hal_cfg : for diff cards
+	 *intf_ops : for diff 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;
+	/*This must be the last item so
+	   that it points to the data allocated
+	   beyond  this structure like:
+	   rtl_pci_priv or rtl_usb_priv */
+	u8 priv[0];
+};
+
+#define rtl_priv(hw)		(((struct rtl_priv *)(hw)->priv))
+#define rtl_mac(rtlpriv)	(&((rtlpriv)->mac80211))
+#define rtl_hal(rtlpriv)	(&((rtlpriv)->rtlhal))
+#define rtl_efuse(rtlpriv)	(&((rtlpriv)->efuse))
+#define rtl_psc(rtlpriv)	(&((rtlpriv)->psc))
+#define rtl_sec(rtlpriv) 	(&((rtlpriv)->sec))
+#define rtl_dm(rtlpriv)	(&((rtlpriv)->dm))
+/***************************************
+    Bluetooth Co-existance Related
+****************************************/
+
+enum bt_ant_num {
+        ANT_X2 = 0,
+        ANT_X1 = 1,
+};
+
+enum bt_co_type {
+        BT_2WIRE = 0,
+        BT_ISSC_3WIRE = 1,
+        BT_ACCEL = 2,
+        BT_CSR_BC4 = 3,
+        BT_CSR_BC8 = 4,
+        BT_RTL8756 = 5,
+        BT_RTL8723A = 6,
+        BT_RTL8821A = 7,
+        BT_RTL8723B = 8,
+        BT_RTL8192E = 9,
+        BT_RTL8812A = 11,
+};
+
+enum bt_total_ant_num{
+	ANT_TOTAL_X2 = 0,
+	ANT_TOTAL_X1 = 1
+};
+
+enum bt_cur_state {
+        BT_OFF = 0,
+        BT_ON = 1,
+};
+
+enum bt_service_type {
+        BT_SCO = 0,
+        BT_A2DP = 1,
+        BT_HID = 2,
+        BT_HID_IDLE = 3,
+        BT_SCAN = 4,
+        BT_IDLE = 5,
+        BT_OTHER_ACTION = 6,
+        BT_BUSY = 7,
+        BT_OTHERBUSY = 8,
+        BT_PAN = 9,
+};
+
+enum bt_radio_shared {
+        BT_RADIO_SHARED = 0,
+        BT_RADIO_INDIVIDUAL = 1,
+};
+
+struct bt_coexist_info {
+
+	/* EEPROM BT info. */
+	u8 eeprom_bt_coexist;
+	u8 eeprom_bt_type;
+	u8 eeprom_bt_ant_num;
+	u8 eeprom_bt_ant_isolation;
+	u8 eeprom_bt_radio_shared;
+
+	u8 bt_coexistence;
+	u8 bt_ant_num;
+	u8 bt_coexist_type;
+	u8 bt_state;
+	u8 bt_cur_state;	/* 0:on, 1:off */
+	u8 bt_ant_isolation;	/* 0:good, 1:bad */
+	u8 bt_pape_ctrl;	/* 0:SW, 1:SW/HW dynamic */
+	u8 bt_service;
+	u8 bt_radio_shared_type;
+	u8 bt_rfreg_origin_1e;
+	u8 bt_rfreg_origin_1f;
+	u8 bt_rssi_state;
+	u32 ratio_tx;
+	u32 ratio_pri;
+	u32 bt_edca_ul;
+	u32 bt_edca_dl;
+
+	bool b_init_set;
+	bool b_bt_busy_traffic;
+	bool b_bt_traffic_mode_set;
+	bool b_bt_non_traffic_mode_set;
+
+	bool b_fw_coexist_all_off;
+	bool b_sw_coexist_all_off;
+	bool b_hw_coexist_all_off;
+	u32 current_state;
+	u32 previous_state;
+	u32 current_state_h;
+	u32 previous_state_h;
+
+	u8 bt_pre_rssi_state;
+	u8 bt_pre_rssi_state1;
+
+	u8 b_reg_bt_iso;
+	u8 b_reg_bt_sco;
+	bool b_balance_on;
+	u8 bt_active_zero_cnt;
+	bool b_cur_bt_disabled;
+	bool b_pre_bt_disabled;
+
+	u8 bt_profile_case;
+	u8 bt_profile_action;
+	bool b_bt_busy;
+	bool b_hold_for_bt_operation;
+	u8 lps_counter;
+};
+
+
+/****************************************
+	mem access macro define start
+	Call endian free function when
+	1. Read/write packet content.
+	2. Before write integer to IO.
+	3. After read integer from IO.
+****************************************/
+/* Convert little data endian to host */
+#define EF1BYTE(_val)		\
+	((u8)(_val))
+#define EF2BYTE(_val)		\
+	(le16_to_cpu(_val))
+#define EF4BYTE(_val)		\
+	(le32_to_cpu(_val))
+
+/* Read data from memory */
+#define READEF1BYTE(_ptr)	\
+	EF1BYTE(*((u8 *)(_ptr)))
+#define READEF2BYTE(_ptr)	\
+	EF2BYTE(*((u16 *)(_ptr)))
+#define READEF4BYTE(_ptr)	\
+	EF4BYTE(*((u32 *)(_ptr)))
+
+/* Write data to memory */
+#define WRITEEF1BYTE(_ptr, _val)	\
+	(*((u8 *)(_ptr)))=EF1BYTE(_val)
+#define WRITEEF2BYTE(_ptr, _val)	\
+	(*((u16 *)(_ptr)))=EF2BYTE(_val)
+#define WRITEEF4BYTE(_ptr, _val)	\
+	(*((u32 *)(_ptr)))=EF4BYTE(_val)
+
+/*Example:
+BIT_LEN_MASK_32(0) => 0x00000000
+BIT_LEN_MASK_32(1) => 0x00000001
+BIT_LEN_MASK_32(2) => 0x00000003
+BIT_LEN_MASK_32(32) => 0xFFFFFFFF*/
+#define BIT_LEN_MASK_32(__bitlen)	 \
+	(0xFFFFFFFF >> (32 - (__bitlen)))
+#define BIT_LEN_MASK_16(__bitlen)	 \
+	(0xFFFF >> (16 - (__bitlen)))
+#define BIT_LEN_MASK_8(__bitlen) \
+	(0xFF >> (8 - (__bitlen)))
+
+/*Example:
+BIT_OFFSET_LEN_MASK_32(0, 2) => 0x00000003
+BIT_OFFSET_LEN_MASK_32(16, 2) => 0x00030000*/
+#define BIT_OFFSET_LEN_MASK_32(__bitoffset, __bitlen) \
+	(BIT_LEN_MASK_32(__bitlen) << (__bitoffset))
+#define BIT_OFFSET_LEN_MASK_16(__bitoffset, __bitlen) \
+	(BIT_LEN_MASK_16(__bitlen) << (__bitoffset))
+#define BIT_OFFSET_LEN_MASK_8(__bitoffset, __bitlen) \
+	(BIT_LEN_MASK_8(__bitlen) << (__bitoffset))
+
+/*Description:
+Return 4-byte value in host byte ordering from
+4-byte pointer in little-endian system.*/
+#define LE_P4BYTE_TO_HOST_4BYTE(__pstart) \
+	(EF4BYTE(*((u32 *)(__pstart))))
+#define LE_P2BYTE_TO_HOST_2BYTE(__pstart) \
+	(EF2BYTE(*((u16 *)(__pstart))))
+#define LE_P1BYTE_TO_HOST_1BYTE(__pstart) \
+	(EF1BYTE(*((u8 *)(__pstart))))
+
+/*Description:
+Translate subfield (continuous bits in little-endian) of 4-byte
+value to host byte ordering.*/
+#define LE_BITS_TO_4BYTE(__pstart, __bitoffset, __bitlen) \
+	( \
+		( LE_P4BYTE_TO_HOST_4BYTE(__pstart) >> (__bitoffset) )  & \
+		BIT_LEN_MASK_32(__bitlen) \
+	)
+#define LE_BITS_TO_2BYTE(__pstart, __bitoffset, __bitlen) \
+	( \
+		( LE_P2BYTE_TO_HOST_2BYTE(__pstart) >> (__bitoffset) ) & \
+		BIT_LEN_MASK_16(__bitlen) \
+	)
+#define LE_BITS_TO_1BYTE(__pstart, __bitoffset, __bitlen) \
+	( \
+		( LE_P1BYTE_TO_HOST_1BYTE(__pstart) >> (__bitoffset) ) & \
+		BIT_LEN_MASK_8(__bitlen) \
+	)
+
+/*Description:
+Mask subfield (continuous bits in little-endian) of 4-byte value
+and return the result in 4-byte value in host byte ordering.*/
+#define LE_BITS_CLEARED_TO_4BYTE(__pstart, __bitoffset, __bitlen) \
+	( \
+		LE_P4BYTE_TO_HOST_4BYTE(__pstart)  & \
+		( ~BIT_OFFSET_LEN_MASK_32(__bitoffset, __bitlen) ) \
+	)
+#define LE_BITS_CLEARED_TO_2BYTE(__pstart, __bitoffset, __bitlen) \
+	( \
+		LE_P2BYTE_TO_HOST_2BYTE(__pstart) & \
+		( ~BIT_OFFSET_LEN_MASK_16(__bitoffset, __bitlen) ) \
+	)
+#define LE_BITS_CLEARED_TO_1BYTE(__pstart, __bitoffset, __bitlen) \
+	( \
+		LE_P1BYTE_TO_HOST_1BYTE(__pstart) & \
+		( ~BIT_OFFSET_LEN_MASK_8(__bitoffset, __bitlen) ) \
+	)
+
+/*Description:
+Set subfield of little-endian 4-byte value to specified value.	*/
+#define SET_BITS_TO_LE_4BYTE(__pstart, __bitoffset, __bitlen, __val) \
+	*((u32 *)(__pstart)) = EF4BYTE \
+	( \
+		LE_BITS_CLEARED_TO_4BYTE(__pstart, __bitoffset, __bitlen) | \
+		( (((u32)__val) & BIT_LEN_MASK_32(__bitlen)) << (__bitoffset) )\
+       );
+#define SET_BITS_TO_LE_2BYTE(__pstart, __bitoffset, __bitlen, __val) \
+	*((u16 *)(__pstart)) = EF2BYTE \
+	( \
+		LE_BITS_CLEARED_TO_2BYTE(__pstart, __bitoffset, __bitlen) | \
+		( (((u16)__val) & BIT_LEN_MASK_16(__bitlen)) << (__bitoffset) )\
+       );
+#define SET_BITS_TO_LE_1BYTE(__pstart, __bitoffset, __bitlen, __val) \
+	*((u8 *)(__pstart)) = EF1BYTE \
+	( \
+		LE_BITS_CLEARED_TO_1BYTE(__pstart, __bitoffset, __bitlen) | \
+		( (((u8)__val) & BIT_LEN_MASK_8(__bitlen)) << (__bitoffset) ) \
+       );
+
+#define	N_BYTE_ALIGMENT(__value, __aligment) ((__aligment == 1) ? \
+	(__value) : (((__value + __aligment - 1) / __aligment) * __aligment))
+
+/****************************************
+	mem access macro define end
+****************************************/
+
+#define byte(x,n) ((x >> (8 * n)) & 0xff)
+
+#define packet_get_type(_packet) (EF1BYTE((_packet).octet[0]) & 0xFC)
+#define RTL_WATCH_DOG_TIME    	2000
+#define MSECS(t)		msecs_to_jiffies(t)
+#define WLAN_FC_GET_VERS(fc) 	((fc) & IEEE80211_FCTL_VERS)
+#define WLAN_FC_GET_TYPE(fc) 	((fc) & IEEE80211_FCTL_FTYPE)
+#define WLAN_FC_GET_STYPE(fc) 	((fc) & IEEE80211_FCTL_STYPE)
+#define WLAN_FC_MORE_DATA(fc) 	((fc) & IEEE80211_FCTL_MOREDATA)
+#define SEQ_TO_SN(seq) 		(((seq) & IEEE80211_SCTL_SEQ) >> 4)
+#define SN_TO_SEQ(ssn) 		(((ssn) << 4) & IEEE80211_SCTL_SEQ)
+#define MAX_SN 			((IEEE80211_SCTL_SEQ) >> 4)
+
+#define	RT_RF_OFF_LEVL_ASPM		BIT(0)	/*PCI ASPM */
+#define	RT_RF_OFF_LEVL_CLK_REQ		BIT(1)	/*PCI clock request */
+#define	RT_RF_OFF_LEVL_PCI_D3		BIT(2)	/*PCI D3 mode */
+/*NIC halt, re-initialize hw parameters*/
+#define	RT_RF_OFF_LEVL_HALT_NIC		BIT(3)
+#define	RT_RF_OFF_LEVL_FREE_FW		BIT(4)	/*FW free, re-download the FW */
+#define	RT_RF_OFF_LEVL_FW_32K		BIT(5)	/*FW in 32k */
+/*Always enable ASPM and Clock Req in initialization.*/
+#define	RT_RF_PS_LEVEL_ALWAYS_ASPM	BIT(6)
+/* no matter RFOFF or SLEEP we set PS_ASPM_LEVL*/
+#define	RT_PS_LEVEL_ASPM		BIT(7)
+/*When LPS is on, disable 2R if no packet is received or 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])
+
+static inline u8 rtl_read_byte(struct rtl_priv *rtlpriv, u32 addr)
+{
+	return rtlpriv->io.read8_sync(rtlpriv, addr);
+}
+
+static inline u16 rtl_read_word(struct rtl_priv *rtlpriv, u32 addr)
+{
+	return rtlpriv->io.read16_sync(rtlpriv, addr);
+}
+
+static inline u32 rtl_read_dword(struct rtl_priv *rtlpriv, u32 addr)
+{
+	return rtlpriv->io.read32_sync(rtlpriv, addr);
+}
+
+static inline void rtl_write_byte(struct rtl_priv *rtlpriv, u32 addr, u8 val8)
+{
+	rtlpriv->io.write8_async(rtlpriv, addr, val8);
+
+	if (rtlpriv->cfg->write_readback)
+		rtlpriv->io.read8_sync(rtlpriv, addr);
+}
+
+static inline void rtl_write_word(struct rtl_priv *rtlpriv, u32 addr, u16 val16)
+{
+	rtlpriv->io.write16_async(rtlpriv, addr, val16);
+
+	if (rtlpriv->cfg->write_readback)
+		rtlpriv->io.read16_sync(rtlpriv, addr);
+}
+
+static inline void rtl_write_dword(struct rtl_priv *rtlpriv,
+				   u32 addr, u32 val32)
+{
+	rtlpriv->io.write32_async(rtlpriv, addr, val32);
+
+	if (rtlpriv->cfg->write_readback)
+		rtlpriv->io.read32_sync(rtlpriv, addr);
+}
+
+static inline u32 rtl_get_bbreg(struct ieee80211_hw *hw,
+				u32 regaddr, u32 bitmask)
+{
+	return ((struct rtl_priv *)(hw)->priv)->cfg->ops->get_bbreg(hw,
+								    regaddr,
+								    bitmask);
+}
+
+static inline void rtl_set_bbreg(struct ieee80211_hw *hw, u32 regaddr,
+				 u32 bitmask, u32 data)
+{
+	((struct rtl_priv *)(hw)->priv)->cfg->ops->set_bbreg(hw,
+							     regaddr, bitmask,
+							     data);
+
+}
+
+static inline u32 rtl_get_rfreg(struct ieee80211_hw *hw,
+				enum radio_path rfpath, u32 regaddr,
+				u32 bitmask)
+{
+	return ((struct rtl_priv *)(hw)->priv)->cfg->ops->get_rfreg(hw,
+								    rfpath,
+								    regaddr,
+								    bitmask);
+}
+
+static inline void rtl_set_rfreg(struct ieee80211_hw *hw,
+				 enum radio_path rfpath, u32 regaddr,
+				 u32 bitmask, u32 data)
+{
+	((struct rtl_priv *)(hw)->priv)->cfg->ops->set_rfreg(hw,
+							     rfpath, regaddr,
+							     bitmask, data);
+}
+
+static inline bool is_hal_stop(struct rtl_hal *rtlhal)
+{
+	return (_HAL_STATE_STOP == rtlhal->state);
+}
+
+static inline void set_hal_start(struct rtl_hal *rtlhal)
+{
+	rtlhal->state = _HAL_STATE_START;
+}
+
+static inline void set_hal_stop(struct rtl_hal *rtlhal)
+{
+	rtlhal->state = _HAL_STATE_STOP;
+}
+
+static inline u8 get_rf_type(struct rtl_phy *rtlphy)
+{
+	return rtlphy->rf_type;
+}
+
+static inline struct ieee80211_hdr *rtl_get_hdr(struct sk_buff *skb)
+{
+	return (struct ieee80211_hdr *)(skb->data);
+}
+
+static inline u16 rtl_get_fc(struct sk_buff *skb)
+{
+	return le16_to_cpu(rtl_get_hdr(skb)->frame_control);
+}
+
+static inline u16 rtl_get_tid_h(struct ieee80211_hdr *hdr)
+{
+	return (ieee80211_get_qos_ctl(hdr))[0] & IEEE80211_QOS_CTL_TID_MASK;
+}
+
+static inline u16 rtl_get_tid(struct sk_buff *skb)
+{
+	return rtl_get_tid_h(rtl_get_hdr(skb));
+}
+
+static inline struct ieee80211_sta *rtl_find_sta(struct ieee80211_hw *hw,
+						 u8 *mac_addr)
+{
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	return ieee80211_find_sta(mac->vif, mac_addr);
+}
+
+struct ieee80211_hw *rtl_pci_get_hw_pointer(void);
+#endif
diff --git a/drivers/staging/wlan-ng/cfg80211.c b/drivers/staging/wlan-ng/cfg80211.c
index a7d24c95191d..7dd2b95416e8 100644
--- a/drivers/staging/wlan-ng/cfg80211.c
+++ b/drivers/staging/wlan-ng/cfg80211.c
@@ -416,7 +416,7 @@ static int prism2_scan(struct wiphy *wiphy,
 		memcpy(&ie_buf[2], &(msg2.ssid.data.data), msg2.ssid.data.len);
 		bss = cfg80211_inform_bss(wiphy,
 			ieee80211_get_channel(wiphy,
-			      ieee80211_dsss_chan_to_freq(msg2.dschannel.data)),
+			      ieee80211_channel_to_frequency(msg2.dschannel.data, IEEE80211_BAND_2GHZ)),
 			(const u8 *) &(msg2.bssid.data.data),
 			msg2.timestamp.data, msg2.capinfo.data,
 			msg2.beaconperiod.data,
diff --git a/drivers/staging/zram/zram.txt b/drivers/staging/zram/zram.txt
deleted file mode 100644
index 765d790ae831..000000000000
--- a/drivers/staging/zram/zram.txt
+++ /dev/null
@@ -1,77 +0,0 @@
-zram: Compressed RAM based block devices
-----------------------------------------
-
-Project home: http://compcache.googlecode.com/
-
-* Introduction
-
-The zram module creates RAM based block devices named /dev/zram<id>
-(<id> = 0, 1, ...). Pages written to these disks are compressed and stored
-in memory itself. These disks allow very fast I/O and compression provides
-good amounts of memory savings. Some of the usecases include /tmp storage,
-use as swap disks, various caches under /var and maybe many more :)
-
-Statistics for individual zram devices are exported through sysfs nodes at
-/sys/block/zram<id>/
-
-* Usage
-
-Following shows a typical sequence of steps for using zram.
-
-1) Load Module:
-	modprobe zram num_devices=4
-	This creates 4 devices: /dev/zram{0,1,2,3}
-	(num_devices parameter is optional. Default: 1)
-
-2) Set Disksize
-        Set disk size by writing the value to sysfs node 'disksize'.
-        The value can be either in bytes or you can use mem suffixes.
-        Examples:
-            # Initialize /dev/zram0 with 50MB disksize
-            echo $((50*1024*1024)) > /sys/block/zram0/disksize
-
-            # Using mem suffixes
-            echo 256K > /sys/block/zram0/disksize
-            echo 512M > /sys/block/zram0/disksize
-            echo 1G > /sys/block/zram0/disksize
-
-3) Activate:
-	mkswap /dev/zram0
-	swapon /dev/zram0
-
-	mkfs.ext4 /dev/zram1
-	mount /dev/zram1 /tmp
-
-4) Stats:
-	Per-device statistics are exported as various nodes under
-	/sys/block/zram<id>/
-		disksize
-		num_reads
-		num_writes
-		invalid_io
-		notify_free
-		discard
-		zero_pages
-		orig_data_size
-		compr_data_size
-		mem_used_total
-
-5) Deactivate:
-	swapoff /dev/zram0
-	umount /dev/zram1
-
-6) Reset:
-	Write any positive value to 'reset' sysfs node
-	echo 1 > /sys/block/zram0/reset
-	echo 1 > /sys/block/zram1/reset
-
-	This frees all the memory allocated for the given device and
-	resets the disksize to zero. You must set the disksize again
-	before reusing the device.
-
-Please report any problems at:
- - Mailing list: linux-mm-cc at laptop dot org
- - Issue tracker: http://code.google.com/p/compcache/issues/list
-
-Nitin Gupta
-ngupta@vflare.org
diff --git a/drivers/staging/zsmalloc/Kconfig b/drivers/staging/zsmalloc/Kconfig
deleted file mode 100644
index 9d1f2a24ad62..000000000000
--- a/drivers/staging/zsmalloc/Kconfig
+++ /dev/null
@@ -1,24 +0,0 @@
-config ZSMALLOC
-	bool "Memory allocator for compressed pages"
-	depends on MMU
-	default n
-	help
-	  zsmalloc is a slab-based memory allocator designed to store
-	  compressed RAM pages.  zsmalloc uses virtual memory mapping
-	  in order to reduce fragmentation.  However, this results in a
-	  non-standard allocator interface where a handle, not a pointer, is
-	  returned by an alloc().  This handle must be mapped in order to
-	  access the allocated space.
-
-config PGTABLE_MAPPING
-	bool "Use page table mapping to access object in zsmalloc"
-	depends on ZSMALLOC
-	help
-	  By default, zsmalloc uses a copy-based object mapping method to
-	  access allocations that span two pages. However, if a particular
-	  architecture (ex, ARM) performs VM mapping faster than copying,
-	  then you should select this. This causes zsmalloc to use page table
-	  mapping rather than copying for object mapping.
-
-	  You can check speed with zsmalloc benchmark[1].
-	  [1] https://github.com/spartacus06/zsmalloc
diff --git a/drivers/staging/zsmalloc/Makefile b/drivers/staging/zsmalloc/Makefile
deleted file mode 100644
index b134848a590d..000000000000
--- a/drivers/staging/zsmalloc/Makefile
+++ /dev/null
@@ -1,3 +0,0 @@
-zsmalloc-y 		:= zsmalloc-main.o
-
-obj-$(CONFIG_ZSMALLOC)	+= zsmalloc.o
diff --git a/drivers/staging/zsmalloc/zsmalloc-main.c b/drivers/staging/zsmalloc/zsmalloc-main.c
deleted file mode 100644
index 7660c87d8b2a..000000000000
--- a/drivers/staging/zsmalloc/zsmalloc-main.c
+++ /dev/null
@@ -1,1106 +0,0 @@
-/*
- * zsmalloc memory allocator
- *
- * Copyright (C) 2011  Nitin Gupta
- *
- * This code is released using a dual license strategy: BSD/GPL
- * You can choose the license that better fits your requirements.
- *
- * Released under the terms of 3-clause BSD License
- * Released under the terms of GNU General Public License Version 2.0
- */
-
-/*
- * This allocator is designed for use with zram. Thus, the allocator is
- * supposed to work well under low memory conditions. In particular, it
- * never attempts higher order page allocation which is very likely to
- * fail under memory pressure. On the other hand, if we just use single
- * (0-order) pages, it would suffer from very high fragmentation --
- * any object of size PAGE_SIZE/2 or larger would occupy an entire page.
- * This was one of the major issues with its predecessor (xvmalloc).
- *
- * To overcome these issues, zsmalloc allocates a bunch of 0-order pages
- * and links them together using various 'struct page' fields. These linked
- * pages act as a single higher-order page i.e. an object can span 0-order
- * page boundaries. The code refers to these linked pages as a single entity
- * called zspage.
- *
- * For simplicity, zsmalloc can only allocate objects of size up to PAGE_SIZE
- * since this satisfies the requirements of all its current users (in the
- * worst case, page is incompressible and is thus stored "as-is" i.e. in
- * uncompressed form). For allocation requests larger than this size, failure
- * is returned (see zs_malloc).
- *
- * Additionally, zs_malloc() does not return a dereferenceable pointer.
- * Instead, it returns an opaque handle (unsigned long) which encodes actual
- * location of the allocated object. The reason for this indirection is that
- * zsmalloc does not keep zspages permanently mapped since that would cause
- * issues on 32-bit systems where the VA region for kernel space mappings
- * is very small. So, before using the allocating memory, the object has to
- * be mapped using zs_map_object() to get a usable pointer and subsequently
- * unmapped using zs_unmap_object().
- *
- * Following is how we use various fields and flags of underlying
- * struct page(s) to form a zspage.
- *
- * Usage of struct page fields:
- *	page->first_page: points to the first component (0-order) page
- *	page->index (union with page->freelist): offset of the first object
- *		starting in this page. For the first page, this is
- *		always 0, so we use this field (aka freelist) to point
- *		to the first free object in zspage.
- *	page->lru: links together all component pages (except the first page)
- *		of a zspage
- *
- *	For _first_ page only:
- *
- *	page->private (union with page->first_page): refers to the
- *		component page after the first page
- *	page->freelist: points to the first free object in zspage.
- *		Free objects are linked together using in-place
- *		metadata.
- *	page->objects: maximum number of objects we can store in this
- *		zspage (class->zspage_order * PAGE_SIZE / class->size)
- *	page->lru: links together first pages of various zspages.
- *		Basically forming list of zspages in a fullness group.
- *	page->mapping: class index and fullness group of the zspage
- *
- * Usage of struct page flags:
- *	PG_private: identifies the first component page
- *	PG_private2: identifies the last component page
- *
- */
-
-#ifdef CONFIG_ZSMALLOC_DEBUG
-#define DEBUG
-#endif
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/bitops.h>
-#include <linux/errno.h>
-#include <linux/highmem.h>
-#include <linux/string.h>
-#include <linux/slab.h>
-#include <asm/tlbflush.h>
-#include <asm/pgtable.h>
-#include <linux/cpumask.h>
-#include <linux/cpu.h>
-#include <linux/vmalloc.h>
-#include <linux/hardirq.h>
-#include <linux/spinlock.h>
-#include <linux/types.h>
-
-#include "zsmalloc.h"
-
-/*
- * This must be power of 2 and greater than of equal to sizeof(link_free).
- * These two conditions ensure that any 'struct link_free' itself doesn't
- * span more than 1 page which avoids complex case of mapping 2 pages simply
- * to restore link_free pointer values.
- */
-#define ZS_ALIGN		8
-
-/*
- * A single 'zspage' is composed of up to 2^N discontiguous 0-order (single)
- * pages. ZS_MAX_ZSPAGE_ORDER defines upper limit on N.
- */
-#define ZS_MAX_ZSPAGE_ORDER 2
-#define ZS_MAX_PAGES_PER_ZSPAGE (_AC(1, UL) << ZS_MAX_ZSPAGE_ORDER)
-
-/*
- * Object location (<PFN>, <obj_idx>) is encoded as
- * as single (unsigned long) handle value.
- *
- * Note that object index <obj_idx> is relative to system
- * page <PFN> it is stored in, so for each sub-page belonging
- * to a zspage, obj_idx starts with 0.
- *
- * This is made more complicated by various memory models and PAE.
- */
-
-#ifndef MAX_PHYSMEM_BITS
-#ifdef CONFIG_HIGHMEM64G
-#define MAX_PHYSMEM_BITS 36
-#else /* !CONFIG_HIGHMEM64G */
-/*
- * If this definition of MAX_PHYSMEM_BITS is used, OBJ_INDEX_BITS will just
- * be PAGE_SHIFT
- */
-#define MAX_PHYSMEM_BITS BITS_PER_LONG
-#endif
-#endif
-#define _PFN_BITS		(MAX_PHYSMEM_BITS - PAGE_SHIFT)
-#define OBJ_INDEX_BITS	(BITS_PER_LONG - _PFN_BITS)
-#define OBJ_INDEX_MASK	((_AC(1, UL) << OBJ_INDEX_BITS) - 1)
-
-#define MAX(a, b) ((a) >= (b) ? (a) : (b))
-/* ZS_MIN_ALLOC_SIZE must be multiple of ZS_ALIGN */
-#define ZS_MIN_ALLOC_SIZE \
-	MAX(32, (ZS_MAX_PAGES_PER_ZSPAGE << PAGE_SHIFT >> OBJ_INDEX_BITS))
-#define ZS_MAX_ALLOC_SIZE	PAGE_SIZE
-
-/*
- * On systems with 4K page size, this gives 254 size classes! There is a
- * trader-off here:
- *  - Large number of size classes is potentially wasteful as free page are
- *    spread across these classes
- *  - Small number of size classes causes large internal fragmentation
- *  - Probably its better to use specific size classes (empirically
- *    determined). NOTE: all those class sizes must be set as multiple of
- *    ZS_ALIGN to make sure link_free itself never has to span 2 pages.
- *
- *  ZS_MIN_ALLOC_SIZE and ZS_SIZE_CLASS_DELTA must be multiple of ZS_ALIGN
- *  (reason above)
- */
-#define ZS_SIZE_CLASS_DELTA	(PAGE_SIZE >> 8)
-#define ZS_SIZE_CLASSES		((ZS_MAX_ALLOC_SIZE - ZS_MIN_ALLOC_SIZE) / \
-					ZS_SIZE_CLASS_DELTA + 1)
-
-/*
- * We do not maintain any list for completely empty or full pages
- */
-enum fullness_group {
-	ZS_ALMOST_FULL,
-	ZS_ALMOST_EMPTY,
-	_ZS_NR_FULLNESS_GROUPS,
-
-	ZS_EMPTY,
-	ZS_FULL
-};
-
-/*
- * We assign a page to ZS_ALMOST_EMPTY fullness group when:
- *	n <= N / f, where
- * n = number of allocated objects
- * N = total number of objects zspage can store
- * f = 1/fullness_threshold_frac
- *
- * Similarly, we assign zspage to:
- *	ZS_ALMOST_FULL	when n > N / f
- *	ZS_EMPTY	when n == 0
- *	ZS_FULL		when n == N
- *
- * (see: fix_fullness_group())
- */
-static const int fullness_threshold_frac = 4;
-
-struct size_class {
-	/*
-	 * Size of objects stored in this class. Must be multiple
-	 * of ZS_ALIGN.
-	 */
-	int size;
-	unsigned int index;
-
-	/* Number of PAGE_SIZE sized pages to combine to form a 'zspage' */
-	int pages_per_zspage;
-
-	spinlock_t lock;
-
-	/* stats */
-	u64 pages_allocated;
-
-	struct page *fullness_list[_ZS_NR_FULLNESS_GROUPS];
-};
-
-/*
- * Placed within free objects to form a singly linked list.
- * For every zspage, first_page->freelist gives head of this list.
- *
- * This must be power of 2 and less than or equal to ZS_ALIGN
- */
-struct link_free {
-	/* Handle of next free chunk (encodes <PFN, obj_idx>) */
-	void *next;
-};
-
-struct zs_pool {
-	struct size_class size_class[ZS_SIZE_CLASSES];
-
-	gfp_t flags;	/* allocation flags used when growing pool */
-};
-
-/*
- * A zspage's class index and fullness group
- * are encoded in its (first)page->mapping
- */
-#define CLASS_IDX_BITS	28
-#define FULLNESS_BITS	4
-#define CLASS_IDX_MASK	((1 << CLASS_IDX_BITS) - 1)
-#define FULLNESS_MASK	((1 << FULLNESS_BITS) - 1)
-
-struct mapping_area {
-#ifdef CONFIG_PGTABLE_MAPPING
-	struct vm_struct *vm; /* vm area for mapping object that span pages */
-#else
-	char *vm_buf; /* copy buffer for objects that span pages */
-#endif
-	char *vm_addr; /* address of kmap_atomic()'ed pages */
-	enum zs_mapmode vm_mm; /* mapping mode */
-};
-
-
-/* per-cpu VM mapping areas for zspage accesses that cross page boundaries */
-static DEFINE_PER_CPU(struct mapping_area, zs_map_area);
-
-static int is_first_page(struct page *page)
-{
-	return PagePrivate(page);
-}
-
-static int is_last_page(struct page *page)
-{
-	return PagePrivate2(page);
-}
-
-static void get_zspage_mapping(struct page *page, unsigned int *class_idx,
-				enum fullness_group *fullness)
-{
-	unsigned long m;
-	BUG_ON(!is_first_page(page));
-
-	m = (unsigned long)page->mapping;
-	*fullness = m & FULLNESS_MASK;
-	*class_idx = (m >> FULLNESS_BITS) & CLASS_IDX_MASK;
-}
-
-static void set_zspage_mapping(struct page *page, unsigned int class_idx,
-				enum fullness_group fullness)
-{
-	unsigned long m;
-	BUG_ON(!is_first_page(page));
-
-	m = ((class_idx & CLASS_IDX_MASK) << FULLNESS_BITS) |
-			(fullness & FULLNESS_MASK);
-	page->mapping = (struct address_space *)m;
-}
-
-/*
- * zsmalloc divides the pool into various size classes where each
- * class maintains a list of zspages where each zspage is divided
- * into equal sized chunks. Each allocation falls into one of these
- * classes depending on its size. This function returns index of the
- * size class which has chunk size big enough to hold the give size.
- */
-static int get_size_class_index(int size)
-{
-	int idx = 0;
-
-	if (likely(size > ZS_MIN_ALLOC_SIZE))
-		idx = DIV_ROUND_UP(size - ZS_MIN_ALLOC_SIZE,
-				ZS_SIZE_CLASS_DELTA);
-
-	return idx;
-}
-
-/*
- * For each size class, zspages are divided into different groups
- * depending on how "full" they are. This was done so that we could
- * easily find empty or nearly empty zspages when we try to shrink
- * the pool (not yet implemented). This function returns fullness
- * status of the given page.
- */
-static enum fullness_group get_fullness_group(struct page *page)
-{
-	int inuse, max_objects;
-	enum fullness_group fg;
-	BUG_ON(!is_first_page(page));
-
-	inuse = page->inuse;
-	max_objects = page->objects;
-
-	if (inuse == 0)
-		fg = ZS_EMPTY;
-	else if (inuse == max_objects)
-		fg = ZS_FULL;
-	else if (inuse <= max_objects / fullness_threshold_frac)
-		fg = ZS_ALMOST_EMPTY;
-	else
-		fg = ZS_ALMOST_FULL;
-
-	return fg;
-}
-
-/*
- * Each size class maintains various freelists and zspages are assigned
- * to one of these freelists based on the number of live objects they
- * have. This functions inserts the given zspage into the freelist
- * identified by <class, fullness_group>.
- */
-static void insert_zspage(struct page *page, struct size_class *class,
-				enum fullness_group fullness)
-{
-	struct page **head;
-
-	BUG_ON(!is_first_page(page));
-
-	if (fullness >= _ZS_NR_FULLNESS_GROUPS)
-		return;
-
-	head = &class->fullness_list[fullness];
-	if (*head)
-		list_add_tail(&page->lru, &(*head)->lru);
-
-	*head = page;
-}
-
-/*
- * This function removes the given zspage from the freelist identified
- * by <class, fullness_group>.
- */
-static void remove_zspage(struct page *page, struct size_class *class,
-				enum fullness_group fullness)
-{
-	struct page **head;
-
-	BUG_ON(!is_first_page(page));
-
-	if (fullness >= _ZS_NR_FULLNESS_GROUPS)
-		return;
-
-	head = &class->fullness_list[fullness];
-	BUG_ON(!*head);
-	if (list_empty(&(*head)->lru))
-		*head = NULL;
-	else if (*head == page)
-		*head = (struct page *)list_entry((*head)->lru.next,
-					struct page, lru);
-
-	list_del_init(&page->lru);
-}
-
-/*
- * Each size class maintains zspages in different fullness groups depending
- * on the number of live objects they contain. When allocating or freeing
- * objects, the fullness status of the page can change, say, from ALMOST_FULL
- * to ALMOST_EMPTY when freeing an object. This function checks if such
- * a status change has occurred for the given page and accordingly moves the
- * page from the freelist of the old fullness group to that of the new
- * fullness group.
- */
-static enum fullness_group fix_fullness_group(struct zs_pool *pool,
-						struct page *page)
-{
-	int class_idx;
-	struct size_class *class;
-	enum fullness_group currfg, newfg;
-
-	BUG_ON(!is_first_page(page));
-
-	get_zspage_mapping(page, &class_idx, &currfg);
-	newfg = get_fullness_group(page);
-	if (newfg == currfg)
-		goto out;
-
-	class = &pool->size_class[class_idx];
-	remove_zspage(page, class, currfg);
-	insert_zspage(page, class, newfg);
-	set_zspage_mapping(page, class_idx, newfg);
-
-out:
-	return newfg;
-}
-
-/*
- * We have to decide on how many pages to link together
- * to form a zspage for each size class. This is important
- * to reduce wastage due to unusable space left at end of
- * each zspage which is given as:
- *	wastage = Zp - Zp % size_class
- * where Zp = zspage size = k * PAGE_SIZE where k = 1, 2, ...
- *
- * For example, for size class of 3/8 * PAGE_SIZE, we should
- * link together 3 PAGE_SIZE sized pages to form a zspage
- * since then we can perfectly fit in 8 such objects.
- */
-static int get_pages_per_zspage(int class_size)
-{
-	int i, max_usedpc = 0;
-	/* zspage order which gives maximum used size per KB */
-	int max_usedpc_order = 1;
-
-	for (i = 1; i <= ZS_MAX_PAGES_PER_ZSPAGE; i++) {
-		int zspage_size;
-		int waste, usedpc;
-
-		zspage_size = i * PAGE_SIZE;
-		waste = zspage_size % class_size;
-		usedpc = (zspage_size - waste) * 100 / zspage_size;
-
-		if (usedpc > max_usedpc) {
-			max_usedpc = usedpc;
-			max_usedpc_order = i;
-		}
-	}
-
-	return max_usedpc_order;
-}
-
-/*
- * A single 'zspage' is composed of many system pages which are
- * linked together using fields in struct page. This function finds
- * the first/head page, given any component page of a zspage.
- */
-static struct page *get_first_page(struct page *page)
-{
-	if (is_first_page(page))
-		return page;
-	else
-		return page->first_page;
-}
-
-static struct page *get_next_page(struct page *page)
-{
-	struct page *next;
-
-	if (is_last_page(page))
-		next = NULL;
-	else if (is_first_page(page))
-		next = (struct page *)page_private(page);
-	else
-		next = list_entry(page->lru.next, struct page, lru);
-
-	return next;
-}
-
-/*
- * Encode <page, obj_idx> as a single handle value.
- * On hardware platforms with physical memory starting at 0x0 the pfn
- * could be 0 so we ensure that the handle will never be 0 by adjusting the
- * encoded obj_idx value before encoding.
- */
-static void *obj_location_to_handle(struct page *page, unsigned long obj_idx)
-{
-	unsigned long handle;
-
-	if (!page) {
-		BUG_ON(obj_idx);
-		return NULL;
-	}
-
-	handle = page_to_pfn(page) << OBJ_INDEX_BITS;
-	handle |= ((obj_idx + 1) & OBJ_INDEX_MASK);
-
-	return (void *)handle;
-}
-
-/*
- * Decode <page, obj_idx> pair from the given object handle. We adjust the
- * decoded obj_idx back to its original value since it was adjusted in
- * obj_location_to_handle().
- */
-static void obj_handle_to_location(unsigned long handle, struct page **page,
-				unsigned long *obj_idx)
-{
-	*page = pfn_to_page(handle >> OBJ_INDEX_BITS);
-	*obj_idx = (handle & OBJ_INDEX_MASK) - 1;
-}
-
-static unsigned long obj_idx_to_offset(struct page *page,
-				unsigned long obj_idx, int class_size)
-{
-	unsigned long off = 0;
-
-	if (!is_first_page(page))
-		off = page->index;
-
-	return off + obj_idx * class_size;
-}
-
-static void reset_page(struct page *page)
-{
-	clear_bit(PG_private, &page->flags);
-	clear_bit(PG_private_2, &page->flags);
-	set_page_private(page, 0);
-	page->mapping = NULL;
-	page->freelist = NULL;
-	page_mapcount_reset(page);
-}
-
-static void free_zspage(struct page *first_page)
-{
-	struct page *nextp, *tmp, *head_extra;
-
-	BUG_ON(!is_first_page(first_page));
-	BUG_ON(first_page->inuse);
-
-	head_extra = (struct page *)page_private(first_page);
-
-	reset_page(first_page);
-	__free_page(first_page);
-
-	/* zspage with only 1 system page */
-	if (!head_extra)
-		return;
-
-	list_for_each_entry_safe(nextp, tmp, &head_extra->lru, lru) {
-		list_del(&nextp->lru);
-		reset_page(nextp);
-		__free_page(nextp);
-	}
-	reset_page(head_extra);
-	__free_page(head_extra);
-}
-
-/* Initialize a newly allocated zspage */
-static void init_zspage(struct page *first_page, struct size_class *class)
-{
-	unsigned long off = 0;
-	struct page *page = first_page;
-
-	BUG_ON(!is_first_page(first_page));
-	while (page) {
-		struct page *next_page;
-		struct link_free *link;
-		unsigned int i, objs_on_page;
-
-		/*
-		 * page->index stores offset of first object starting
-		 * in the page. For the first page, this is always 0,
-		 * so we use first_page->index (aka ->freelist) to store
-		 * head of corresponding zspage's freelist.
-		 */
-		if (page != first_page)
-			page->index = off;
-
-		link = (struct link_free *)kmap_atomic(page) +
-						off / sizeof(*link);
-		objs_on_page = (PAGE_SIZE - off) / class->size;
-
-		for (i = 1; i <= objs_on_page; i++) {
-			off += class->size;
-			if (off < PAGE_SIZE) {
-				link->next = obj_location_to_handle(page, i);
-				link += class->size / sizeof(*link);
-			}
-		}
-
-		/*
-		 * We now come to the last (full or partial) object on this
-		 * page, which must point to the first object on the next
-		 * page (if present)
-		 */
-		next_page = get_next_page(page);
-		link->next = obj_location_to_handle(next_page, 0);
-		kunmap_atomic(link);
-		page = next_page;
-		off = (off + class->size) % PAGE_SIZE;
-	}
-}
-
-/*
- * Allocate a zspage for the given size class
- */
-static struct page *alloc_zspage(struct size_class *class, gfp_t flags)
-{
-	int i, error;
-	struct page *first_page = NULL, *uninitialized_var(prev_page);
-
-	/*
-	 * Allocate individual pages and link them together as:
-	 * 1. first page->private = first sub-page
-	 * 2. all sub-pages are linked together using page->lru
-	 * 3. each sub-page is linked to the first page using page->first_page
-	 *
-	 * For each size class, First/Head pages are linked together using
-	 * page->lru. Also, we set PG_private to identify the first page
-	 * (i.e. no other sub-page has this flag set) and PG_private_2 to
-	 * identify the last page.
-	 */
-	error = -ENOMEM;
-	for (i = 0; i < class->pages_per_zspage; i++) {
-		struct page *page;
-
-		page = alloc_page(flags);
-		if (!page)
-			goto cleanup;
-
-		INIT_LIST_HEAD(&page->lru);
-		if (i == 0) {	/* first page */
-			SetPagePrivate(page);
-			set_page_private(page, 0);
-			first_page = page;
-			first_page->inuse = 0;
-		}
-		if (i == 1)
-			set_page_private(first_page, (unsigned long)page);
-		if (i >= 1)
-			page->first_page = first_page;
-		if (i >= 2)
-			list_add(&page->lru, &prev_page->lru);
-		if (i == class->pages_per_zspage - 1)	/* last page */
-			SetPagePrivate2(page);
-		prev_page = page;
-	}
-
-	init_zspage(first_page, class);
-
-	first_page->freelist = obj_location_to_handle(first_page, 0);
-	/* Maximum number of objects we can store in this zspage */
-	first_page->objects = class->pages_per_zspage * PAGE_SIZE / class->size;
-
-	error = 0; /* Success */
-
-cleanup:
-	if (unlikely(error) && first_page) {
-		free_zspage(first_page);
-		first_page = NULL;
-	}
-
-	return first_page;
-}
-
-static struct page *find_get_zspage(struct size_class *class)
-{
-	int i;
-	struct page *page;
-
-	for (i = 0; i < _ZS_NR_FULLNESS_GROUPS; i++) {
-		page = class->fullness_list[i];
-		if (page)
-			break;
-	}
-
-	return page;
-}
-
-#ifdef CONFIG_PGTABLE_MAPPING
-static inline int __zs_cpu_up(struct mapping_area *area)
-{
-	/*
-	 * Make sure we don't leak memory if a cpu UP notification
-	 * and zs_init() race and both call zs_cpu_up() on the same cpu
-	 */
-	if (area->vm)
-		return 0;
-	area->vm = alloc_vm_area(PAGE_SIZE * 2, NULL);
-	if (!area->vm)
-		return -ENOMEM;
-	return 0;
-}
-
-static inline void __zs_cpu_down(struct mapping_area *area)
-{
-	if (area->vm)
-		free_vm_area(area->vm);
-	area->vm = NULL;
-}
-
-static inline void *__zs_map_object(struct mapping_area *area,
-				struct page *pages[2], int off, int size)
-{
-	BUG_ON(map_vm_area(area->vm, PAGE_KERNEL, &pages));
-	area->vm_addr = area->vm->addr;
-	return area->vm_addr + off;
-}
-
-static inline void __zs_unmap_object(struct mapping_area *area,
-				struct page *pages[2], int off, int size)
-{
-	unsigned long addr = (unsigned long)area->vm_addr;
-
-	unmap_kernel_range(addr, PAGE_SIZE * 2);
-}
-
-#else /* CONFIG_PGTABLE_MAPPING */
-
-static inline int __zs_cpu_up(struct mapping_area *area)
-{
-	/*
-	 * Make sure we don't leak memory if a cpu UP notification
-	 * and zs_init() race and both call zs_cpu_up() on the same cpu
-	 */
-	if (area->vm_buf)
-		return 0;
-	area->vm_buf = (char *)__get_free_page(GFP_KERNEL);
-	if (!area->vm_buf)
-		return -ENOMEM;
-	return 0;
-}
-
-static inline void __zs_cpu_down(struct mapping_area *area)
-{
-	if (area->vm_buf)
-		free_page((unsigned long)area->vm_buf);
-	area->vm_buf = NULL;
-}
-
-static void *__zs_map_object(struct mapping_area *area,
-			struct page *pages[2], int off, int size)
-{
-	int sizes[2];
-	void *addr;
-	char *buf = area->vm_buf;
-
-	/* disable page faults to match kmap_atomic() return conditions */
-	pagefault_disable();
-
-	/* no read fastpath */
-	if (area->vm_mm == ZS_MM_WO)
-		goto out;
-
-	sizes[0] = PAGE_SIZE - off;
-	sizes[1] = size - sizes[0];
-
-	/* copy object to per-cpu buffer */
-	addr = kmap_atomic(pages[0]);
-	memcpy(buf, addr + off, sizes[0]);
-	kunmap_atomic(addr);
-	addr = kmap_atomic(pages[1]);
-	memcpy(buf + sizes[0], addr, sizes[1]);
-	kunmap_atomic(addr);
-out:
-	return area->vm_buf;
-}
-
-static void __zs_unmap_object(struct mapping_area *area,
-			struct page *pages[2], int off, int size)
-{
-	int sizes[2];
-	void *addr;
-	char *buf = area->vm_buf;
-
-	/* no write fastpath */
-	if (area->vm_mm == ZS_MM_RO)
-		goto out;
-
-	sizes[0] = PAGE_SIZE - off;
-	sizes[1] = size - sizes[0];
-
-	/* copy per-cpu buffer to object */
-	addr = kmap_atomic(pages[0]);
-	memcpy(addr + off, buf, sizes[0]);
-	kunmap_atomic(addr);
-	addr = kmap_atomic(pages[1]);
-	memcpy(addr, buf + sizes[0], sizes[1]);
-	kunmap_atomic(addr);
-
-out:
-	/* enable page faults to match kunmap_atomic() return conditions */
-	pagefault_enable();
-}
-
-#endif /* CONFIG_PGTABLE_MAPPING */
-
-static int zs_cpu_notifier(struct notifier_block *nb, unsigned long action,
-				void *pcpu)
-{
-	int ret, cpu = (long)pcpu;
-	struct mapping_area *area;
-
-	switch (action) {
-	case CPU_UP_PREPARE:
-		area = &per_cpu(zs_map_area, cpu);
-		ret = __zs_cpu_up(area);
-		if (ret)
-			return notifier_from_errno(ret);
-		break;
-	case CPU_DEAD:
-	case CPU_UP_CANCELED:
-		area = &per_cpu(zs_map_area, cpu);
-		__zs_cpu_down(area);
-		break;
-	}
-
-	return NOTIFY_OK;
-}
-
-static struct notifier_block zs_cpu_nb = {
-	.notifier_call = zs_cpu_notifier
-};
-
-static void zs_exit(void)
-{
-	int cpu;
-
-	for_each_online_cpu(cpu)
-		zs_cpu_notifier(NULL, CPU_DEAD, (void *)(long)cpu);
-	unregister_cpu_notifier(&zs_cpu_nb);
-}
-
-static int zs_init(void)
-{
-	int cpu, ret;
-
-	register_cpu_notifier(&zs_cpu_nb);
-	for_each_online_cpu(cpu) {
-		ret = zs_cpu_notifier(NULL, CPU_UP_PREPARE, (void *)(long)cpu);
-		if (notifier_to_errno(ret))
-			goto fail;
-	}
-	return 0;
-fail:
-	zs_exit();
-	return notifier_to_errno(ret);
-}
-
-/**
- * zs_create_pool - Creates an allocation pool to work from.
- * @flags: allocation flags used to allocate pool metadata
- *
- * This function must be called before anything when using
- * the zsmalloc allocator.
- *
- * On success, a pointer to the newly created pool is returned,
- * otherwise NULL.
- */
-struct zs_pool *zs_create_pool(gfp_t flags)
-{
-	int i, ovhd_size;
-	struct zs_pool *pool;
-
-	ovhd_size = roundup(sizeof(*pool), PAGE_SIZE);
-	pool = kzalloc(ovhd_size, GFP_KERNEL);
-	if (!pool)
-		return NULL;
-
-	for (i = 0; i < ZS_SIZE_CLASSES; i++) {
-		int size;
-		struct size_class *class;
-
-		size = ZS_MIN_ALLOC_SIZE + i * ZS_SIZE_CLASS_DELTA;
-		if (size > ZS_MAX_ALLOC_SIZE)
-			size = ZS_MAX_ALLOC_SIZE;
-
-		class = &pool->size_class[i];
-		class->size = size;
-		class->index = i;
-		spin_lock_init(&class->lock);
-		class->pages_per_zspage = get_pages_per_zspage(size);
-
-	}
-
-	pool->flags = flags;
-
-	return pool;
-}
-EXPORT_SYMBOL_GPL(zs_create_pool);
-
-void zs_destroy_pool(struct zs_pool *pool)
-{
-	int i;
-
-	for (i = 0; i < ZS_SIZE_CLASSES; i++) {
-		int fg;
-		struct size_class *class = &pool->size_class[i];
-
-		for (fg = 0; fg < _ZS_NR_FULLNESS_GROUPS; fg++) {
-			if (class->fullness_list[fg]) {
-				pr_info("Freeing non-empty class with size %db, fullness group %d\n",
-					class->size, fg);
-			}
-		}
-	}
-	kfree(pool);
-}
-EXPORT_SYMBOL_GPL(zs_destroy_pool);
-
-/**
- * zs_malloc - Allocate block of given size from pool.
- * @pool: pool to allocate from
- * @size: size of block to allocate
- *
- * On success, handle to the allocated object is returned,
- * otherwise 0.
- * Allocation requests with size > ZS_MAX_ALLOC_SIZE will fail.
- */
-unsigned long zs_malloc(struct zs_pool *pool, size_t size)
-{
-	unsigned long obj;
-	struct link_free *link;
-	int class_idx;
-	struct size_class *class;
-
-	struct page *first_page, *m_page;
-	unsigned long m_objidx, m_offset;
-
-	if (unlikely(!size || size > ZS_MAX_ALLOC_SIZE))
-		return 0;
-
-	class_idx = get_size_class_index(size);
-	class = &pool->size_class[class_idx];
-	BUG_ON(class_idx != class->index);
-
-	spin_lock(&class->lock);
-	first_page = find_get_zspage(class);
-
-	if (!first_page) {
-		spin_unlock(&class->lock);
-		first_page = alloc_zspage(class, pool->flags);
-		if (unlikely(!first_page))
-			return 0;
-
-		set_zspage_mapping(first_page, class->index, ZS_EMPTY);
-		spin_lock(&class->lock);
-		class->pages_allocated += class->pages_per_zspage;
-	}
-
-	obj = (unsigned long)first_page->freelist;
-	obj_handle_to_location(obj, &m_page, &m_objidx);
-	m_offset = obj_idx_to_offset(m_page, m_objidx, class->size);
-
-	link = (struct link_free *)kmap_atomic(m_page) +
-					m_offset / sizeof(*link);
-	first_page->freelist = link->next;
-	memset(link, POISON_INUSE, sizeof(*link));
-	kunmap_atomic(link);
-
-	first_page->inuse++;
-	/* Now move the zspage to another fullness group, if required */
-	fix_fullness_group(pool, first_page);
-	spin_unlock(&class->lock);
-
-	return obj;
-}
-EXPORT_SYMBOL_GPL(zs_malloc);
-
-void zs_free(struct zs_pool *pool, unsigned long obj)
-{
-	struct link_free *link;
-	struct page *first_page, *f_page;
-	unsigned long f_objidx, f_offset;
-
-	int class_idx;
-	struct size_class *class;
-	enum fullness_group fullness;
-
-	if (unlikely(!obj))
-		return;
-
-	obj_handle_to_location(obj, &f_page, &f_objidx);
-	first_page = get_first_page(f_page);
-
-	get_zspage_mapping(first_page, &class_idx, &fullness);
-	class = &pool->size_class[class_idx];
-	f_offset = obj_idx_to_offset(f_page, f_objidx, class->size);
-
-	spin_lock(&class->lock);
-
-	/* Insert this object in containing zspage's freelist */
-	link = (struct link_free *)((unsigned char *)kmap_atomic(f_page)
-							+ f_offset);
-	link->next = first_page->freelist;
-	kunmap_atomic(link);
-	first_page->freelist = (void *)obj;
-
-	first_page->inuse--;
-	fullness = fix_fullness_group(pool, first_page);
-
-	if (fullness == ZS_EMPTY)
-		class->pages_allocated -= class->pages_per_zspage;
-
-	spin_unlock(&class->lock);
-
-	if (fullness == ZS_EMPTY)
-		free_zspage(first_page);
-}
-EXPORT_SYMBOL_GPL(zs_free);
-
-/**
- * zs_map_object - get address of allocated object from handle.
- * @pool: pool from which the object was allocated
- * @handle: handle returned from zs_malloc
- *
- * Before using an object allocated from zs_malloc, it must be mapped using
- * this function. When done with the object, it must be unmapped using
- * zs_unmap_object.
- *
- * Only one object can be mapped per cpu at a time. There is no protection
- * against nested mappings.
- *
- * This function returns with preemption and page faults disabled.
- */
-void *zs_map_object(struct zs_pool *pool, unsigned long handle,
-			enum zs_mapmode mm)
-{
-	struct page *page;
-	unsigned long obj_idx, off;
-
-	unsigned int class_idx;
-	enum fullness_group fg;
-	struct size_class *class;
-	struct mapping_area *area;
-	struct page *pages[2];
-
-	BUG_ON(!handle);
-
-	/*
-	 * Because we use per-cpu mapping areas shared among the
-	 * pools/users, we can't allow mapping in interrupt context
-	 * because it can corrupt another users mappings.
-	 */
-	BUG_ON(in_interrupt());
-
-	obj_handle_to_location(handle, &page, &obj_idx);
-	get_zspage_mapping(get_first_page(page), &class_idx, &fg);
-	class = &pool->size_class[class_idx];
-	off = obj_idx_to_offset(page, obj_idx, class->size);
-
-	area = &get_cpu_var(zs_map_area);
-	area->vm_mm = mm;
-	if (off + class->size <= PAGE_SIZE) {
-		/* this object is contained entirely within a page */
-		area->vm_addr = kmap_atomic(page);
-		return area->vm_addr + off;
-	}
-
-	/* this object spans two pages */
-	pages[0] = page;
-	pages[1] = get_next_page(page);
-	BUG_ON(!pages[1]);
-
-	return __zs_map_object(area, pages, off, class->size);
-}
-EXPORT_SYMBOL_GPL(zs_map_object);
-
-void zs_unmap_object(struct zs_pool *pool, unsigned long handle)
-{
-	struct page *page;
-	unsigned long obj_idx, off;
-
-	unsigned int class_idx;
-	enum fullness_group fg;
-	struct size_class *class;
-	struct mapping_area *area;
-
-	BUG_ON(!handle);
-
-	obj_handle_to_location(handle, &page, &obj_idx);
-	get_zspage_mapping(get_first_page(page), &class_idx, &fg);
-	class = &pool->size_class[class_idx];
-	off = obj_idx_to_offset(page, obj_idx, class->size);
-
-	area = &__get_cpu_var(zs_map_area);
-	if (off + class->size <= PAGE_SIZE)
-		kunmap_atomic(area->vm_addr);
-	else {
-		struct page *pages[2];
-
-		pages[0] = page;
-		pages[1] = get_next_page(page);
-		BUG_ON(!pages[1]);
-
-		__zs_unmap_object(area, pages, off, class->size);
-	}
-	put_cpu_var(zs_map_area);
-}
-EXPORT_SYMBOL_GPL(zs_unmap_object);
-
-u64 zs_get_total_size_bytes(struct zs_pool *pool)
-{
-	int i;
-	u64 npages = 0;
-
-	for (i = 0; i < ZS_SIZE_CLASSES; i++)
-		npages += pool->size_class[i].pages_allocated;
-
-	return npages << PAGE_SHIFT;
-}
-EXPORT_SYMBOL_GPL(zs_get_total_size_bytes);
-
-module_init(zs_init);
-module_exit(zs_exit);
-
-MODULE_LICENSE("Dual BSD/GPL");
-MODULE_AUTHOR("Nitin Gupta <ngupta@vflare.org>");
diff --git a/drivers/staging/zsmalloc/zsmalloc.h b/drivers/staging/zsmalloc/zsmalloc.h
deleted file mode 100644
index c2eb174b97ee..000000000000
--- a/drivers/staging/zsmalloc/zsmalloc.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/*
- * zsmalloc memory allocator
- *
- * Copyright (C) 2011  Nitin Gupta
- *
- * This code is released using a dual license strategy: BSD/GPL
- * You can choose the license that better fits your requirements.
- *
- * Released under the terms of 3-clause BSD License
- * Released under the terms of GNU General Public License Version 2.0
- */
-
-#ifndef _ZS_MALLOC_H_
-#define _ZS_MALLOC_H_
-
-#include <linux/types.h>
-
-/*
- * zsmalloc mapping modes
- *
- * NOTE: These only make a difference when a mapped object spans pages.
- * They also have no effect when PGTABLE_MAPPING is selected.
- */
-enum zs_mapmode {
-	ZS_MM_RW, /* normal read-write mapping */
-	ZS_MM_RO, /* read-only (no copy-out at unmap time) */
-	ZS_MM_WO /* write-only (no copy-in at map time) */
-	/*
-	 * NOTE: ZS_MM_WO should only be used for initializing new
-	 * (uninitialized) allocations.  Partial writes to already
-	 * initialized allocations should use ZS_MM_RW to preserve the
-	 * existing data.
-	 */
-};
-
-struct zs_pool;
-
-struct zs_pool *zs_create_pool(gfp_t flags);
-void zs_destroy_pool(struct zs_pool *pool);
-
-unsigned long zs_malloc(struct zs_pool *pool, size_t size);
-void zs_free(struct zs_pool *pool, unsigned long obj);
-
-void *zs_map_object(struct zs_pool *pool, unsigned long handle,
-			enum zs_mapmode mm);
-void zs_unmap_object(struct zs_pool *pool, unsigned long handle);
-
-u64 zs_get_total_size_bytes(struct zs_pool *pool);
-
-#endif
diff --git a/drivers/target/Kconfig b/drivers/target/Kconfig
index 18303686eb58..dc2d84ac5a0e 100644
--- a/drivers/target/Kconfig
+++ b/drivers/target/Kconfig
@@ -3,6 +3,7 @@ menuconfig TARGET_CORE
 	tristate "Generic Target Core Mod (TCM) and ConfigFS Infrastructure"
 	depends on SCSI && BLOCK
 	select CONFIGFS_FS
+	select CRC_T10DIF
 	default n
 	help
 	Say Y or M here to enable the TCM Storage Engine and ConfigFS enabled
@@ -13,6 +14,7 @@ if TARGET_CORE
 
 config TCM_IBLOCK
 	tristate "TCM/IBLOCK Subsystem Plugin for Linux/BLOCK"
+	select BLK_DEV_INTEGRITY
 	help
 	Say Y here to enable the TCM/IBLOCK subsystem plugin for non-buffered
 	access to Linux/Block devices using BIO
diff --git a/drivers/target/iscsi/iscsi_target.c b/drivers/target/iscsi/iscsi_target.c
index 00867190413c..7f1a7ce4b771 100644
--- a/drivers/target/iscsi/iscsi_target.c
+++ b/drivers/target/iscsi/iscsi_target.c
@@ -52,7 +52,7 @@
 static LIST_HEAD(g_tiqn_list);
 static LIST_HEAD(g_np_list);
 static DEFINE_SPINLOCK(tiqn_lock);
-static DEFINE_SPINLOCK(np_lock);
+static DEFINE_MUTEX(np_lock);
 
 static struct idr tiqn_idr;
 struct idr sess_idr;
@@ -307,6 +307,9 @@ bool iscsit_check_np_match(
 	return false;
 }
 
+/*
+ * Called with mutex np_lock held
+ */
 static struct iscsi_np *iscsit_get_np(
 	struct __kernel_sockaddr_storage *sockaddr,
 	int network_transport)
@@ -314,11 +317,10 @@ static struct iscsi_np *iscsit_get_np(
 	struct iscsi_np *np;
 	bool match;
 
-	spin_lock_bh(&np_lock);
 	list_for_each_entry(np, &g_np_list, np_list) {
-		spin_lock(&np->np_thread_lock);
+		spin_lock_bh(&np->np_thread_lock);
 		if (np->np_thread_state != ISCSI_NP_THREAD_ACTIVE) {
-			spin_unlock(&np->np_thread_lock);
+			spin_unlock_bh(&np->np_thread_lock);
 			continue;
 		}
 
@@ -330,13 +332,11 @@ static struct iscsi_np *iscsit_get_np(
 			 * while iscsi_tpg_add_network_portal() is called.
 			 */
 			np->np_exports++;
-			spin_unlock(&np->np_thread_lock);
-			spin_unlock_bh(&np_lock);
+			spin_unlock_bh(&np->np_thread_lock);
 			return np;
 		}
-		spin_unlock(&np->np_thread_lock);
+		spin_unlock_bh(&np->np_thread_lock);
 	}
-	spin_unlock_bh(&np_lock);
 
 	return NULL;
 }
@@ -350,16 +350,22 @@ struct iscsi_np *iscsit_add_np(
 	struct sockaddr_in6 *sock_in6;
 	struct iscsi_np *np;
 	int ret;
+
+	mutex_lock(&np_lock);
+
 	/*
 	 * Locate the existing struct iscsi_np if already active..
 	 */
 	np = iscsit_get_np(sockaddr, network_transport);
-	if (np)
+	if (np) {
+		mutex_unlock(&np_lock);
 		return np;
+	}
 
 	np = kzalloc(sizeof(struct iscsi_np), GFP_KERNEL);
 	if (!np) {
 		pr_err("Unable to allocate memory for struct iscsi_np\n");
+		mutex_unlock(&np_lock);
 		return ERR_PTR(-ENOMEM);
 	}
 
@@ -382,6 +388,7 @@ struct iscsi_np *iscsit_add_np(
 	ret = iscsi_target_setup_login_socket(np, sockaddr);
 	if (ret != 0) {
 		kfree(np);
+		mutex_unlock(&np_lock);
 		return ERR_PTR(ret);
 	}
 
@@ -390,6 +397,7 @@ struct iscsi_np *iscsit_add_np(
 		pr_err("Unable to create kthread: iscsi_np\n");
 		ret = PTR_ERR(np->np_thread);
 		kfree(np);
+		mutex_unlock(&np_lock);
 		return ERR_PTR(ret);
 	}
 	/*
@@ -400,10 +408,10 @@ struct iscsi_np *iscsit_add_np(
 	 * point because iscsi_np has not been added to g_np_list yet.
 	 */
 	np->np_exports = 1;
+	np->np_thread_state = ISCSI_NP_THREAD_ACTIVE;
 
-	spin_lock_bh(&np_lock);
 	list_add_tail(&np->np_list, &g_np_list);
-	spin_unlock_bh(&np_lock);
+	mutex_unlock(&np_lock);
 
 	pr_debug("CORE[0] - Added Network Portal: %s:%hu on %s\n",
 		np->np_ip, np->np_port, np->np_transport->name);
@@ -470,9 +478,9 @@ int iscsit_del_np(struct iscsi_np *np)
 
 	np->np_transport->iscsit_free_np(np);
 
-	spin_lock_bh(&np_lock);
+	mutex_lock(&np_lock);
 	list_del(&np->np_list);
-	spin_unlock_bh(&np_lock);
+	mutex_unlock(&np_lock);
 
 	pr_debug("CORE[0] - Removed Network Portal: %s:%hu on %s\n",
 		np->np_ip, np->np_port, np->np_transport->name);
@@ -622,7 +630,7 @@ static int iscsit_add_reject(
 {
 	struct iscsi_cmd *cmd;
 
-	cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
+	cmd = iscsit_allocate_cmd(conn, TASK_INTERRUPTIBLE);
 	if (!cmd)
 		return -1;
 
@@ -2475,7 +2483,7 @@ static void iscsit_build_conn_drop_async_message(struct iscsi_conn *conn)
 	if (!conn_p)
 		return;
 
-	cmd = iscsit_allocate_cmd(conn_p, GFP_ATOMIC);
+	cmd = iscsit_allocate_cmd(conn_p, TASK_RUNNING);
 	if (!cmd) {
 		iscsit_dec_conn_usage_count(conn_p);
 		return;
@@ -3951,7 +3959,7 @@ static int iscsi_target_rx_opcode(struct iscsi_conn *conn, unsigned char *buf)
 
 	switch (hdr->opcode & ISCSI_OPCODE_MASK) {
 	case ISCSI_OP_SCSI_CMD:
-		cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
+		cmd = iscsit_allocate_cmd(conn, TASK_INTERRUPTIBLE);
 		if (!cmd)
 			goto reject;
 
@@ -3963,28 +3971,28 @@ static int iscsi_target_rx_opcode(struct iscsi_conn *conn, unsigned char *buf)
 	case ISCSI_OP_NOOP_OUT:
 		cmd = NULL;
 		if (hdr->ttt == cpu_to_be32(0xFFFFFFFF)) {
-			cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
+			cmd = iscsit_allocate_cmd(conn, TASK_INTERRUPTIBLE);
 			if (!cmd)
 				goto reject;
 		}
 		ret = iscsit_handle_nop_out(conn, cmd, buf);
 		break;
 	case ISCSI_OP_SCSI_TMFUNC:
-		cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
+		cmd = iscsit_allocate_cmd(conn, TASK_INTERRUPTIBLE);
 		if (!cmd)
 			goto reject;
 
 		ret = iscsit_handle_task_mgt_cmd(conn, cmd, buf);
 		break;
 	case ISCSI_OP_TEXT:
-		cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
+		cmd = iscsit_allocate_cmd(conn, TASK_INTERRUPTIBLE);
 		if (!cmd)
 			goto reject;
 
 		ret = iscsit_handle_text_cmd(conn, cmd, buf);
 		break;
 	case ISCSI_OP_LOGOUT:
-		cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
+		cmd = iscsit_allocate_cmd(conn, TASK_INTERRUPTIBLE);
 		if (!cmd)
 			goto reject;
 
diff --git a/drivers/target/iscsi/iscsi_target_nego.c b/drivers/target/iscsi/iscsi_target_nego.c
index 83c965c65386..582ba84075ec 100644
--- a/drivers/target/iscsi/iscsi_target_nego.c
+++ b/drivers/target/iscsi/iscsi_target_nego.c
@@ -1192,7 +1192,7 @@ get_target:
 	 */
 alloc_tags:
 	tag_num = max_t(u32, ISCSIT_MIN_TAGS, queue_depth);
-	tag_num += (tag_num / 2) + ISCSIT_EXTRA_TAGS;
+	tag_num = (tag_num * 2) + ISCSIT_EXTRA_TAGS;
 	tag_size = sizeof(struct iscsi_cmd) + conn->conn_transport->priv_size;
 
 	ret = transport_alloc_session_tags(sess->se_sess, tag_num, tag_size);
diff --git a/drivers/target/iscsi/iscsi_target_util.c b/drivers/target/iscsi/iscsi_target_util.c
index 0819e688a398..e655b042ed18 100644
--- a/drivers/target/iscsi/iscsi_target_util.c
+++ b/drivers/target/iscsi/iscsi_target_util.c
@@ -152,13 +152,16 @@ void iscsit_free_r2ts_from_list(struct iscsi_cmd *cmd)
  * May be called from software interrupt (timer) context for allocating
  * iSCSI NopINs.
  */
-struct iscsi_cmd *iscsit_allocate_cmd(struct iscsi_conn *conn, gfp_t gfp_mask)
+struct iscsi_cmd *iscsit_allocate_cmd(struct iscsi_conn *conn, int state)
 {
 	struct iscsi_cmd *cmd;
 	struct se_session *se_sess = conn->sess->se_sess;
 	int size, tag;
 
-	tag = percpu_ida_alloc(&se_sess->sess_tag_pool, gfp_mask);
+	tag = percpu_ida_alloc(&se_sess->sess_tag_pool, state);
+	if (tag < 0)
+		return NULL;
+
 	size = sizeof(struct iscsi_cmd) + conn->conn_transport->priv_size;
 	cmd = (struct iscsi_cmd *)(se_sess->sess_cmd_map + (tag * size));
 	memset(cmd, 0, size);
@@ -926,7 +929,7 @@ static int iscsit_add_nopin(struct iscsi_conn *conn, int want_response)
 	u8 state;
 	struct iscsi_cmd *cmd;
 
-	cmd = iscsit_allocate_cmd(conn, GFP_ATOMIC);
+	cmd = iscsit_allocate_cmd(conn, TASK_RUNNING);
 	if (!cmd)
 		return -1;
 
diff --git a/drivers/target/iscsi/iscsi_target_util.h b/drivers/target/iscsi/iscsi_target_util.h
index e4fc34a02f57..561a424d1980 100644
--- a/drivers/target/iscsi/iscsi_target_util.h
+++ b/drivers/target/iscsi/iscsi_target_util.h
@@ -9,7 +9,7 @@ extern struct iscsi_r2t *iscsit_get_r2t_from_list(struct iscsi_cmd *);
 extern void iscsit_free_r2t(struct iscsi_r2t *, struct iscsi_cmd *);
 extern void iscsit_free_r2ts_from_list(struct iscsi_cmd *);
 extern struct iscsi_cmd *iscsit_alloc_cmd(struct iscsi_conn *, gfp_t);
-extern struct iscsi_cmd *iscsit_allocate_cmd(struct iscsi_conn *, gfp_t);
+extern struct iscsi_cmd *iscsit_allocate_cmd(struct iscsi_conn *, int);
 extern struct iscsi_seq *iscsit_get_seq_holder_for_datain(struct iscsi_cmd *, u32);
 extern struct iscsi_seq *iscsit_get_seq_holder_for_r2t(struct iscsi_cmd *);
 extern struct iscsi_r2t *iscsit_get_holder_for_r2tsn(struct iscsi_cmd *, u32);
diff --git a/drivers/target/loopback/tcm_loop.c b/drivers/target/loopback/tcm_loop.c
index 1b41e6776152..fadad7c5f635 100644
--- a/drivers/target/loopback/tcm_loop.c
+++ b/drivers/target/loopback/tcm_loop.c
@@ -217,7 +217,8 @@ static void tcm_loop_submission_work(struct work_struct *work)
 			scsi_bufflen(sc), tcm_loop_sam_attr(sc),
 			sc->sc_data_direction, 0,
 			scsi_sglist(sc), scsi_sg_count(sc),
-			sgl_bidi, sgl_bidi_count);
+			sgl_bidi, sgl_bidi_count,
+			scsi_prot_sglist(sc), scsi_prot_sg_count(sc));
 	if (rc < 0) {
 		set_host_byte(sc, DID_NO_CONNECT);
 		goto out_done;
@@ -462,7 +463,7 @@ static int tcm_loop_driver_probe(struct device *dev)
 {
 	struct tcm_loop_hba *tl_hba;
 	struct Scsi_Host *sh;
-	int error;
+	int error, host_prot;
 
 	tl_hba = to_tcm_loop_hba(dev);
 
@@ -486,6 +487,13 @@ static int tcm_loop_driver_probe(struct device *dev)
 	sh->max_channel = 0;
 	sh->max_cmd_len = TL_SCSI_MAX_CMD_LEN;
 
+	host_prot = SHOST_DIF_TYPE1_PROTECTION | SHOST_DIF_TYPE2_PROTECTION |
+		    SHOST_DIF_TYPE3_PROTECTION | SHOST_DIX_TYPE1_PROTECTION |
+		    SHOST_DIX_TYPE2_PROTECTION | SHOST_DIX_TYPE3_PROTECTION;
+
+	scsi_host_set_prot(sh, host_prot);
+	scsi_host_set_guard(sh, SHOST_DIX_GUARD_CRC);
+
 	error = scsi_add_host(sh, &tl_hba->dev);
 	if (error) {
 		pr_err("%s: scsi_add_host failed\n", __func__);
@@ -1228,7 +1236,7 @@ static struct configfs_attribute *tcm_loop_tpg_attrs[] = {
 
 /* Start items for tcm_loop_naa_cit */
 
-struct se_portal_group *tcm_loop_make_naa_tpg(
+static struct se_portal_group *tcm_loop_make_naa_tpg(
 	struct se_wwn *wwn,
 	struct config_group *group,
 	const char *name)
@@ -1273,7 +1281,7 @@ struct se_portal_group *tcm_loop_make_naa_tpg(
 	return &tl_tpg->tl_se_tpg;
 }
 
-void tcm_loop_drop_naa_tpg(
+static void tcm_loop_drop_naa_tpg(
 	struct se_portal_group *se_tpg)
 {
 	struct se_wwn *wwn = se_tpg->se_tpg_wwn;
@@ -1305,7 +1313,7 @@ void tcm_loop_drop_naa_tpg(
 
 /* Start items for tcm_loop_cit */
 
-struct se_wwn *tcm_loop_make_scsi_hba(
+static struct se_wwn *tcm_loop_make_scsi_hba(
 	struct target_fabric_configfs *tf,
 	struct config_group *group,
 	const char *name)
@@ -1375,7 +1383,7 @@ out:
 	return ERR_PTR(ret);
 }
 
-void tcm_loop_drop_scsi_hba(
+static void tcm_loop_drop_scsi_hba(
 	struct se_wwn *wwn)
 {
 	struct tcm_loop_hba *tl_hba = container_of(wwn,
diff --git a/drivers/target/target_core_alua.c b/drivers/target/target_core_alua.c
index fdcee326bfbc..12da9b386169 100644
--- a/drivers/target/target_core_alua.c
+++ b/drivers/target/target_core_alua.c
@@ -41,11 +41,14 @@
 #include "target_core_alua.h"
 #include "target_core_ua.h"
 
-static sense_reason_t core_alua_check_transition(int state, int *primary);
+static sense_reason_t core_alua_check_transition(int state, int valid,
+						 int *primary);
 static int core_alua_set_tg_pt_secondary_state(
 		struct t10_alua_tg_pt_gp_member *tg_pt_gp_mem,
 		struct se_port *port, int explicit, int offline);
 
+static char *core_alua_dump_state(int state);
+
 static u16 alua_lu_gps_counter;
 static u32 alua_lu_gps_count;
 
@@ -55,6 +58,86 @@ static LIST_HEAD(lu_gps_list);
 struct t10_alua_lu_gp *default_lu_gp;
 
 /*
+ * REPORT REFERRALS
+ *
+ * See sbc3r35 section 5.23
+ */
+sense_reason_t
+target_emulate_report_referrals(struct se_cmd *cmd)
+{
+	struct se_device *dev = cmd->se_dev;
+	struct t10_alua_lba_map *map;
+	struct t10_alua_lba_map_member *map_mem;
+	unsigned char *buf;
+	u32 rd_len = 0, off;
+
+	if (cmd->data_length < 4) {
+		pr_warn("REPORT REFERRALS allocation length %u too"
+			" small\n", cmd->data_length);
+		return TCM_INVALID_CDB_FIELD;
+	}
+
+	buf = transport_kmap_data_sg(cmd);
+	if (!buf)
+		return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
+
+	off = 4;
+	spin_lock(&dev->t10_alua.lba_map_lock);
+	if (list_empty(&dev->t10_alua.lba_map_list)) {
+		spin_unlock(&dev->t10_alua.lba_map_lock);
+		transport_kunmap_data_sg(cmd);
+
+		return TCM_UNSUPPORTED_SCSI_OPCODE;
+	}
+
+	list_for_each_entry(map, &dev->t10_alua.lba_map_list,
+			    lba_map_list) {
+		int desc_num = off + 3;
+		int pg_num;
+
+		off += 4;
+		if (cmd->data_length > off)
+			put_unaligned_be64(map->lba_map_first_lba, &buf[off]);
+		off += 8;
+		if (cmd->data_length > off)
+			put_unaligned_be64(map->lba_map_last_lba, &buf[off]);
+		off += 8;
+		rd_len += 20;
+		pg_num = 0;
+		list_for_each_entry(map_mem, &map->lba_map_mem_list,
+				    lba_map_mem_list) {
+			int alua_state = map_mem->lba_map_mem_alua_state;
+			int alua_pg_id = map_mem->lba_map_mem_alua_pg_id;
+
+			if (cmd->data_length > off)
+				buf[off] = alua_state & 0x0f;
+			off += 2;
+			if (cmd->data_length > off)
+				buf[off] = (alua_pg_id >> 8) & 0xff;
+			off++;
+			if (cmd->data_length > off)
+				buf[off] = (alua_pg_id & 0xff);
+			off++;
+			rd_len += 4;
+			pg_num++;
+		}
+		if (cmd->data_length > desc_num)
+			buf[desc_num] = pg_num;
+	}
+	spin_unlock(&dev->t10_alua.lba_map_lock);
+
+	/*
+	 * Set the RETURN DATA LENGTH set in the header of the DataIN Payload
+	 */
+	put_unaligned_be16(rd_len, &buf[2]);
+
+	transport_kunmap_data_sg(cmd);
+
+	target_complete_cmd(cmd, GOOD);
+	return 0;
+}
+
+/*
  * REPORT_TARGET_PORT_GROUPS
  *
  * See spc4r17 section 6.27
@@ -210,7 +293,7 @@ target_emulate_set_target_port_groups(struct se_cmd *cmd)
 	unsigned char *ptr;
 	sense_reason_t rc = TCM_NO_SENSE;
 	u32 len = 4; /* Skip over RESERVED area in header */
-	int alua_access_state, primary = 0;
+	int alua_access_state, primary = 0, valid_states;
 	u16 tg_pt_id, rtpi;
 
 	if (!l_port)
@@ -252,6 +335,7 @@ target_emulate_set_target_port_groups(struct se_cmd *cmd)
 		rc = TCM_UNSUPPORTED_SCSI_OPCODE;
 		goto out;
 	}
+	valid_states = l_tg_pt_gp->tg_pt_gp_alua_supported_states;
 
 	ptr = &buf[4]; /* Skip over RESERVED area in header */
 
@@ -263,7 +347,8 @@ target_emulate_set_target_port_groups(struct se_cmd *cmd)
 		 * the state is a primary or secondary target port asymmetric
 		 * access state.
 		 */
-		rc = core_alua_check_transition(alua_access_state, &primary);
+		rc = core_alua_check_transition(alua_access_state,
+						valid_states, &primary);
 		if (rc) {
 			/*
 			 * If the SET TARGET PORT GROUPS attempts to establish
@@ -386,6 +471,81 @@ static inline int core_alua_state_nonoptimized(
 	return 0;
 }
 
+static inline int core_alua_state_lba_dependent(
+	struct se_cmd *cmd,
+	struct t10_alua_tg_pt_gp *tg_pt_gp,
+	u8 *alua_ascq)
+{
+	struct se_device *dev = cmd->se_dev;
+	u64 segment_size, segment_mult, sectors, lba;
+
+	/* Only need to check for cdb actually containing LBAs */
+	if (!(cmd->se_cmd_flags & SCF_SCSI_DATA_CDB))
+		return 0;
+
+	spin_lock(&dev->t10_alua.lba_map_lock);
+	segment_size = dev->t10_alua.lba_map_segment_size;
+	segment_mult = dev->t10_alua.lba_map_segment_multiplier;
+	sectors = cmd->data_length / dev->dev_attrib.block_size;
+
+	lba = cmd->t_task_lba;
+	while (lba < cmd->t_task_lba + sectors) {
+		struct t10_alua_lba_map *cur_map = NULL, *map;
+		struct t10_alua_lba_map_member *map_mem;
+
+		list_for_each_entry(map, &dev->t10_alua.lba_map_list,
+				    lba_map_list) {
+			u64 start_lba, last_lba;
+			u64 first_lba = map->lba_map_first_lba;
+
+			if (segment_mult) {
+				u64 tmp = lba;
+				start_lba = sector_div(tmp, segment_size * segment_mult);
+
+				last_lba = first_lba + segment_size - 1;
+				if (start_lba >= first_lba &&
+				    start_lba <= last_lba) {
+					lba += segment_size;
+					cur_map = map;
+					break;
+				}
+			} else {
+				last_lba = map->lba_map_last_lba;
+				if (lba >= first_lba && lba <= last_lba) {
+					lba = last_lba + 1;
+					cur_map = map;
+					break;
+				}
+			}
+		}
+		if (!cur_map) {
+			spin_unlock(&dev->t10_alua.lba_map_lock);
+			*alua_ascq = ASCQ_04H_ALUA_TG_PT_UNAVAILABLE;
+			return 1;
+		}
+		list_for_each_entry(map_mem, &cur_map->lba_map_mem_list,
+				    lba_map_mem_list) {
+			if (map_mem->lba_map_mem_alua_pg_id !=
+			    tg_pt_gp->tg_pt_gp_id)
+				continue;
+			switch(map_mem->lba_map_mem_alua_state) {
+			case ALUA_ACCESS_STATE_STANDBY:
+				spin_unlock(&dev->t10_alua.lba_map_lock);
+				*alua_ascq = ASCQ_04H_ALUA_TG_PT_STANDBY;
+				return 1;
+			case ALUA_ACCESS_STATE_UNAVAILABLE:
+				spin_unlock(&dev->t10_alua.lba_map_lock);
+				*alua_ascq = ASCQ_04H_ALUA_TG_PT_UNAVAILABLE;
+				return 1;
+			default:
+				break;
+			}
+		}
+	}
+	spin_unlock(&dev->t10_alua.lba_map_lock);
+	return 0;
+}
+
 static inline int core_alua_state_standby(
 	struct se_cmd *cmd,
 	unsigned char *cdb,
@@ -583,6 +743,9 @@ target_alua_state_check(struct se_cmd *cmd)
 	case ALUA_ACCESS_STATE_TRANSITION:
 		ret = core_alua_state_transition(cmd, cdb, &alua_ascq);
 		break;
+	case ALUA_ACCESS_STATE_LBA_DEPENDENT:
+		ret = core_alua_state_lba_dependent(cmd, tg_pt_gp, &alua_ascq);
+		break;
 	/*
 	 * OFFLINE is a secondary ALUA target port group access state, that is
 	 * handled above with struct se_port->sep_tg_pt_secondary_offline=1
@@ -618,17 +781,36 @@ out:
  * Check implicit and explicit ALUA state change request.
  */
 static sense_reason_t
-core_alua_check_transition(int state, int *primary)
+core_alua_check_transition(int state, int valid, int *primary)
 {
+	/*
+	 * OPTIMIZED, NON-OPTIMIZED, STANDBY and UNAVAILABLE are
+	 * defined as primary target port asymmetric access states.
+	 */
 	switch (state) {
 	case ALUA_ACCESS_STATE_ACTIVE_OPTIMIZED:
+		if (!(valid & ALUA_AO_SUP))
+			goto not_supported;
+		*primary = 1;
+		break;
 	case ALUA_ACCESS_STATE_ACTIVE_NON_OPTIMIZED:
+		if (!(valid & ALUA_AN_SUP))
+			goto not_supported;
+		*primary = 1;
+		break;
 	case ALUA_ACCESS_STATE_STANDBY:
+		if (!(valid & ALUA_S_SUP))
+			goto not_supported;
+		*primary = 1;
+		break;
 	case ALUA_ACCESS_STATE_UNAVAILABLE:
-		/*
-		 * OPTIMIZED, NON-OPTIMIZED, STANDBY and UNAVAILABLE are
-		 * defined as primary target port asymmetric access states.
-		 */
+		if (!(valid & ALUA_U_SUP))
+			goto not_supported;
+		*primary = 1;
+		break;
+	case ALUA_ACCESS_STATE_LBA_DEPENDENT:
+		if (!(valid & ALUA_LBD_SUP))
+			goto not_supported;
 		*primary = 1;
 		break;
 	case ALUA_ACCESS_STATE_OFFLINE:
@@ -636,14 +818,27 @@ core_alua_check_transition(int state, int *primary)
 		 * OFFLINE state is defined as a secondary target port
 		 * asymmetric access state.
 		 */
+		if (!(valid & ALUA_O_SUP))
+			goto not_supported;
 		*primary = 0;
 		break;
+	case ALUA_ACCESS_STATE_TRANSITION:
+		/*
+		 * Transitioning is set internally, and
+		 * cannot be selected manually.
+		 */
+		goto not_supported;
 	default:
 		pr_err("Unknown ALUA access state: 0x%02x\n", state);
 		return TCM_INVALID_PARAMETER_LIST;
 	}
 
 	return 0;
+
+not_supported:
+	pr_err("ALUA access state %s not supported",
+	       core_alua_dump_state(state));
+	return TCM_INVALID_PARAMETER_LIST;
 }
 
 static char *core_alua_dump_state(int state)
@@ -653,12 +848,16 @@ static char *core_alua_dump_state(int state)
 		return "Active/Optimized";
 	case ALUA_ACCESS_STATE_ACTIVE_NON_OPTIMIZED:
 		return "Active/NonOptimized";
+	case ALUA_ACCESS_STATE_LBA_DEPENDENT:
+		return "LBA Dependent";
 	case ALUA_ACCESS_STATE_STANDBY:
 		return "Standby";
 	case ALUA_ACCESS_STATE_UNAVAILABLE:
 		return "Unavailable";
 	case ALUA_ACCESS_STATE_OFFLINE:
 		return "Offline";
+	case ALUA_ACCESS_STATE_TRANSITION:
+		return "Transitioning";
 	default:
 		return "Unknown";
 	}
@@ -735,58 +934,49 @@ static int core_alua_write_tpg_metadata(
  * Called with tg_pt_gp->tg_pt_gp_md_mutex held
  */
 static int core_alua_update_tpg_primary_metadata(
-	struct t10_alua_tg_pt_gp *tg_pt_gp,
-	int primary_state,
-	unsigned char *md_buf)
+	struct t10_alua_tg_pt_gp *tg_pt_gp)
 {
+	unsigned char *md_buf;
 	struct t10_wwn *wwn = &tg_pt_gp->tg_pt_gp_dev->t10_wwn;
 	char path[ALUA_METADATA_PATH_LEN];
-	int len;
+	int len, rc;
+
+	md_buf = kzalloc(ALUA_MD_BUF_LEN, GFP_KERNEL);
+	if (!md_buf) {
+		pr_err("Unable to allocate buf for ALUA metadata\n");
+		return -ENOMEM;
+	}
 
 	memset(path, 0, ALUA_METADATA_PATH_LEN);
 
-	len = snprintf(md_buf, tg_pt_gp->tg_pt_gp_md_buf_len,
+	len = snprintf(md_buf, ALUA_MD_BUF_LEN,
 			"tg_pt_gp_id=%hu\n"
 			"alua_access_state=0x%02x\n"
 			"alua_access_status=0x%02x\n",
-			tg_pt_gp->tg_pt_gp_id, primary_state,
+			tg_pt_gp->tg_pt_gp_id,
+			tg_pt_gp->tg_pt_gp_alua_pending_state,
 			tg_pt_gp->tg_pt_gp_alua_access_status);
 
 	snprintf(path, ALUA_METADATA_PATH_LEN,
 		"/var/target/alua/tpgs_%s/%s", &wwn->unit_serial[0],
 		config_item_name(&tg_pt_gp->tg_pt_gp_group.cg_item));
 
-	return core_alua_write_tpg_metadata(path, md_buf, len);
+	rc = core_alua_write_tpg_metadata(path, md_buf, len);
+	kfree(md_buf);
+	return rc;
 }
 
-static int core_alua_do_transition_tg_pt(
-	struct t10_alua_tg_pt_gp *tg_pt_gp,
-	struct se_port *l_port,
-	struct se_node_acl *nacl,
-	unsigned char *md_buf,
-	int new_state,
-	int explicit)
+static void core_alua_do_transition_tg_pt_work(struct work_struct *work)
 {
+	struct t10_alua_tg_pt_gp *tg_pt_gp = container_of(work,
+		struct t10_alua_tg_pt_gp, tg_pt_gp_transition_work.work);
+	struct se_device *dev = tg_pt_gp->tg_pt_gp_dev;
 	struct se_dev_entry *se_deve;
 	struct se_lun_acl *lacl;
 	struct se_port *port;
 	struct t10_alua_tg_pt_gp_member *mem;
-	int old_state = 0;
-	/*
-	 * Save the old primary ALUA access state, and set the current state
-	 * to ALUA_ACCESS_STATE_TRANSITION.
-	 */
-	old_state = atomic_read(&tg_pt_gp->tg_pt_gp_alua_access_state);
-	atomic_set(&tg_pt_gp->tg_pt_gp_alua_access_state,
-			ALUA_ACCESS_STATE_TRANSITION);
-	tg_pt_gp->tg_pt_gp_alua_access_status = (explicit) ?
-				ALUA_STATUS_ALTERED_BY_EXPLICIT_STPG :
-				ALUA_STATUS_ALTERED_BY_IMPLICIT_ALUA;
-	/*
-	 * Check for the optional ALUA primary state transition delay
-	 */
-	if (tg_pt_gp->tg_pt_gp_trans_delay_msecs != 0)
-		msleep_interruptible(tg_pt_gp->tg_pt_gp_trans_delay_msecs);
+	bool explicit = (tg_pt_gp->tg_pt_gp_alua_access_status ==
+			 ALUA_STATUS_ALTERED_BY_EXPLICIT_STPG);
 
 	spin_lock(&tg_pt_gp->tg_pt_gp_lock);
 	list_for_each_entry(mem, &tg_pt_gp->tg_pt_gp_mem_list,
@@ -821,9 +1011,12 @@ static int core_alua_do_transition_tg_pt(
 			if (!lacl)
 				continue;
 
-			if (explicit &&
-			   (nacl != NULL) && (nacl == lacl->se_lun_nacl) &&
-			   (l_port != NULL) && (l_port == port))
+			if ((tg_pt_gp->tg_pt_gp_alua_access_status ==
+			     ALUA_STATUS_ALTERED_BY_EXPLICIT_STPG) &&
+			   (tg_pt_gp->tg_pt_gp_alua_nacl != NULL) &&
+			    (tg_pt_gp->tg_pt_gp_alua_nacl == lacl->se_lun_nacl) &&
+			   (tg_pt_gp->tg_pt_gp_alua_port != NULL) &&
+			    (tg_pt_gp->tg_pt_gp_alua_port == port))
 				continue;
 
 			core_scsi3_ua_allocate(lacl->se_lun_nacl,
@@ -851,20 +1044,102 @@ static int core_alua_do_transition_tg_pt(
 	 */
 	if (tg_pt_gp->tg_pt_gp_write_metadata) {
 		mutex_lock(&tg_pt_gp->tg_pt_gp_md_mutex);
-		core_alua_update_tpg_primary_metadata(tg_pt_gp,
-					new_state, md_buf);
+		core_alua_update_tpg_primary_metadata(tg_pt_gp);
 		mutex_unlock(&tg_pt_gp->tg_pt_gp_md_mutex);
 	}
 	/*
 	 * Set the current primary ALUA access state to the requested new state
 	 */
-	atomic_set(&tg_pt_gp->tg_pt_gp_alua_access_state, new_state);
+	atomic_set(&tg_pt_gp->tg_pt_gp_alua_access_state,
+		   tg_pt_gp->tg_pt_gp_alua_pending_state);
 
 	pr_debug("Successful %s ALUA transition TG PT Group: %s ID: %hu"
 		" from primary access state %s to %s\n", (explicit) ? "explicit" :
 		"implicit", config_item_name(&tg_pt_gp->tg_pt_gp_group.cg_item),
-		tg_pt_gp->tg_pt_gp_id, core_alua_dump_state(old_state),
-		core_alua_dump_state(new_state));
+		tg_pt_gp->tg_pt_gp_id,
+		core_alua_dump_state(tg_pt_gp->tg_pt_gp_alua_previous_state),
+		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();
+	spin_unlock(&dev->t10_alua.tg_pt_gps_lock);
+
+	if (tg_pt_gp->tg_pt_gp_transition_complete)
+		complete(tg_pt_gp->tg_pt_gp_transition_complete);
+}
+
+static int core_alua_do_transition_tg_pt(
+	struct t10_alua_tg_pt_gp *tg_pt_gp,
+	int new_state,
+	int explicit)
+{
+	struct se_device *dev = tg_pt_gp->tg_pt_gp_dev;
+	DECLARE_COMPLETION_ONSTACK(wait);
+
+	/* Nothing to be done here */
+	if (atomic_read(&tg_pt_gp->tg_pt_gp_alua_access_state) == new_state)
+		return 0;
+
+	if (new_state == ALUA_ACCESS_STATE_TRANSITION)
+		return -EAGAIN;
+
+	/*
+	 * Flush any pending transitions
+	 */
+	if (!explicit && tg_pt_gp->tg_pt_gp_implicit_trans_secs &&
+	    atomic_read(&tg_pt_gp->tg_pt_gp_alua_access_state) ==
+	    ALUA_ACCESS_STATE_TRANSITION) {
+		/* Just in case */
+		tg_pt_gp->tg_pt_gp_alua_pending_state = new_state;
+		tg_pt_gp->tg_pt_gp_transition_complete = &wait;
+		flush_delayed_work(&tg_pt_gp->tg_pt_gp_transition_work);
+		wait_for_completion(&wait);
+		tg_pt_gp->tg_pt_gp_transition_complete = NULL;
+		return 0;
+	}
+
+	/*
+	 * Save the old primary ALUA access state, and set the current state
+	 * to ALUA_ACCESS_STATE_TRANSITION.
+	 */
+	tg_pt_gp->tg_pt_gp_alua_previous_state =
+		atomic_read(&tg_pt_gp->tg_pt_gp_alua_access_state);
+	tg_pt_gp->tg_pt_gp_alua_pending_state = new_state;
+
+	atomic_set(&tg_pt_gp->tg_pt_gp_alua_access_state,
+			ALUA_ACCESS_STATE_TRANSITION);
+	tg_pt_gp->tg_pt_gp_alua_access_status = (explicit) ?
+				ALUA_STATUS_ALTERED_BY_EXPLICIT_STPG :
+				ALUA_STATUS_ALTERED_BY_IMPLICIT_ALUA;
+
+	/*
+	 * Check for the optional ALUA primary state transition delay
+	 */
+	if (tg_pt_gp->tg_pt_gp_trans_delay_msecs != 0)
+		msleep_interruptible(tg_pt_gp->tg_pt_gp_trans_delay_msecs);
+
+	/*
+	 * Take a reference for workqueue item
+	 */
+	spin_lock(&dev->t10_alua.tg_pt_gps_lock);
+	atomic_inc(&tg_pt_gp->tg_pt_gp_ref_cnt);
+	smp_mb__after_atomic_inc();
+	spin_unlock(&dev->t10_alua.tg_pt_gps_lock);
+
+	if (!explicit && tg_pt_gp->tg_pt_gp_implicit_trans_secs) {
+		unsigned long transition_tmo;
+
+		transition_tmo = tg_pt_gp->tg_pt_gp_implicit_trans_secs * HZ;
+		queue_delayed_work(tg_pt_gp->tg_pt_gp_dev->tmr_wq,
+				   &tg_pt_gp->tg_pt_gp_transition_work,
+				   transition_tmo);
+	} else {
+		tg_pt_gp->tg_pt_gp_transition_complete = &wait;
+		queue_delayed_work(tg_pt_gp->tg_pt_gp_dev->tmr_wq,
+				   &tg_pt_gp->tg_pt_gp_transition_work, 0);
+		wait_for_completion(&wait);
+		tg_pt_gp->tg_pt_gp_transition_complete = NULL;
+	}
 
 	return 0;
 }
@@ -878,23 +1153,15 @@ int core_alua_do_port_transition(
 	int explicit)
 {
 	struct se_device *dev;
-	struct se_port *port;
-	struct se_node_acl *nacl;
 	struct t10_alua_lu_gp *lu_gp;
 	struct t10_alua_lu_gp_member *lu_gp_mem, *local_lu_gp_mem;
 	struct t10_alua_tg_pt_gp *tg_pt_gp;
-	unsigned char *md_buf;
-	int primary;
+	int primary, valid_states, rc = 0;
 
-	if (core_alua_check_transition(new_state, &primary) != 0)
+	valid_states = l_tg_pt_gp->tg_pt_gp_alua_supported_states;
+	if (core_alua_check_transition(new_state, valid_states, &primary) != 0)
 		return -EINVAL;
 
-	md_buf = kzalloc(l_tg_pt_gp->tg_pt_gp_md_buf_len, GFP_KERNEL);
-	if (!md_buf) {
-		pr_err("Unable to allocate buf for ALUA metadata\n");
-		return -ENOMEM;
-	}
-
 	local_lu_gp_mem = l_dev->dev_alua_lu_gp_mem;
 	spin_lock(&local_lu_gp_mem->lu_gp_mem_lock);
 	lu_gp = local_lu_gp_mem->lu_gp;
@@ -911,12 +1178,13 @@ int core_alua_do_port_transition(
 		 * core_alua_do_transition_tg_pt() will always return
 		 * success.
 		 */
-		core_alua_do_transition_tg_pt(l_tg_pt_gp, l_port, l_nacl,
-					md_buf, new_state, explicit);
+		l_tg_pt_gp->tg_pt_gp_alua_port = l_port;
+		l_tg_pt_gp->tg_pt_gp_alua_nacl = l_nacl;
+		rc = core_alua_do_transition_tg_pt(l_tg_pt_gp,
+						   new_state, explicit);
 		atomic_dec(&lu_gp->lu_gp_ref_cnt);
 		smp_mb__after_atomic_dec();
-		kfree(md_buf);
-		return 0;
+		return rc;
 	}
 	/*
 	 * For all other LU groups aside from 'default_lu_gp', walk all of
@@ -951,11 +1219,11 @@ int core_alua_do_port_transition(
 				continue;
 
 			if (l_tg_pt_gp == tg_pt_gp) {
-				port = l_port;
-				nacl = l_nacl;
+				tg_pt_gp->tg_pt_gp_alua_port = l_port;
+				tg_pt_gp->tg_pt_gp_alua_nacl = l_nacl;
 			} else {
-				port = NULL;
-				nacl = NULL;
+				tg_pt_gp->tg_pt_gp_alua_port = NULL;
+				tg_pt_gp->tg_pt_gp_alua_nacl = NULL;
 			}
 			atomic_inc(&tg_pt_gp->tg_pt_gp_ref_cnt);
 			smp_mb__after_atomic_inc();
@@ -964,12 +1232,14 @@ int core_alua_do_port_transition(
 			 * core_alua_do_transition_tg_pt() will always return
 			 * success.
 			 */
-			core_alua_do_transition_tg_pt(tg_pt_gp, port,
-					nacl, md_buf, new_state, explicit);
+			rc = core_alua_do_transition_tg_pt(tg_pt_gp,
+					new_state, explicit);
 
 			spin_lock(&dev->t10_alua.tg_pt_gps_lock);
 			atomic_dec(&tg_pt_gp->tg_pt_gp_ref_cnt);
 			smp_mb__after_atomic_dec();
+			if (rc)
+				break;
 		}
 		spin_unlock(&dev->t10_alua.tg_pt_gps_lock);
 
@@ -979,16 +1249,18 @@ int core_alua_do_port_transition(
 	}
 	spin_unlock(&lu_gp->lu_gp_lock);
 
-	pr_debug("Successfully processed LU Group: %s all ALUA TG PT"
-		" Group IDs: %hu %s transition to primary state: %s\n",
-		config_item_name(&lu_gp->lu_gp_group.cg_item),
-		l_tg_pt_gp->tg_pt_gp_id, (explicit) ? "explicit" : "implicit",
-		core_alua_dump_state(new_state));
+	if (!rc) {
+		pr_debug("Successfully processed LU Group: %s all ALUA TG PT"
+			 " Group IDs: %hu %s transition to primary state: %s\n",
+			 config_item_name(&lu_gp->lu_gp_group.cg_item),
+			 l_tg_pt_gp->tg_pt_gp_id,
+			 (explicit) ? "explicit" : "implicit",
+			 core_alua_dump_state(new_state));
+	}
 
 	atomic_dec(&lu_gp->lu_gp_ref_cnt);
 	smp_mb__after_atomic_dec();
-	kfree(md_buf);
-	return 0;
+	return rc;
 }
 
 /*
@@ -996,13 +1268,18 @@ int core_alua_do_port_transition(
  */
 static int core_alua_update_tpg_secondary_metadata(
 	struct t10_alua_tg_pt_gp_member *tg_pt_gp_mem,
-	struct se_port *port,
-	unsigned char *md_buf,
-	u32 md_buf_len)
+	struct se_port *port)
 {
+	unsigned char *md_buf;
 	struct se_portal_group *se_tpg = port->sep_tpg;
 	char path[ALUA_METADATA_PATH_LEN], wwn[ALUA_SECONDARY_METADATA_WWN_LEN];
-	int len;
+	int len, rc;
+
+	md_buf = kzalloc(ALUA_MD_BUF_LEN, GFP_KERNEL);
+	if (!md_buf) {
+		pr_err("Unable to allocate buf for ALUA metadata\n");
+		return -ENOMEM;
+	}
 
 	memset(path, 0, ALUA_METADATA_PATH_LEN);
 	memset(wwn, 0, ALUA_SECONDARY_METADATA_WWN_LEN);
@@ -1014,7 +1291,7 @@ static int core_alua_update_tpg_secondary_metadata(
 		snprintf(wwn+len, ALUA_SECONDARY_METADATA_WWN_LEN-len, "+%hu",
 				se_tpg->se_tpg_tfo->tpg_get_tag(se_tpg));
 
-	len = snprintf(md_buf, md_buf_len, "alua_tg_pt_offline=%d\n"
+	len = snprintf(md_buf, ALUA_MD_BUF_LEN, "alua_tg_pt_offline=%d\n"
 			"alua_tg_pt_status=0x%02x\n",
 			atomic_read(&port->sep_tg_pt_secondary_offline),
 			port->sep_tg_pt_secondary_stat);
@@ -1023,7 +1300,10 @@ static int core_alua_update_tpg_secondary_metadata(
 			se_tpg->se_tpg_tfo->get_fabric_name(), wwn,
 			port->sep_lun->unpacked_lun);
 
-	return core_alua_write_tpg_metadata(path, md_buf, len);
+	rc = core_alua_write_tpg_metadata(path, md_buf, len);
+	kfree(md_buf);
+
+	return rc;
 }
 
 static int core_alua_set_tg_pt_secondary_state(
@@ -1033,8 +1313,6 @@ static int core_alua_set_tg_pt_secondary_state(
 	int offline)
 {
 	struct t10_alua_tg_pt_gp *tg_pt_gp;
-	unsigned char *md_buf;
-	u32 md_buf_len;
 	int trans_delay_msecs;
 
 	spin_lock(&tg_pt_gp_mem->tg_pt_gp_mem_lock);
@@ -1055,7 +1333,6 @@ static int core_alua_set_tg_pt_secondary_state(
 	else
 		atomic_set(&port->sep_tg_pt_secondary_offline, 0);
 
-	md_buf_len = tg_pt_gp->tg_pt_gp_md_buf_len;
 	port->sep_tg_pt_secondary_stat = (explicit) ?
 			ALUA_STATUS_ALTERED_BY_EXPLICIT_STPG :
 			ALUA_STATUS_ALTERED_BY_IMPLICIT_ALUA;
@@ -1077,23 +1354,115 @@ static int core_alua_set_tg_pt_secondary_state(
 	 * secondary state and status
 	 */
 	if (port->sep_tg_pt_secondary_write_md) {
-		md_buf = kzalloc(md_buf_len, GFP_KERNEL);
-		if (!md_buf) {
-			pr_err("Unable to allocate md_buf for"
-				" secondary ALUA access metadata\n");
-			return -ENOMEM;
-		}
 		mutex_lock(&port->sep_tg_pt_md_mutex);
-		core_alua_update_tpg_secondary_metadata(tg_pt_gp_mem, port,
-				md_buf, md_buf_len);
+		core_alua_update_tpg_secondary_metadata(tg_pt_gp_mem, port);
 		mutex_unlock(&port->sep_tg_pt_md_mutex);
+	}
+
+	return 0;
+}
+
+struct t10_alua_lba_map *
+core_alua_allocate_lba_map(struct list_head *list,
+			   u64 first_lba, u64 last_lba)
+{
+	struct t10_alua_lba_map *lba_map;
+
+	lba_map = kmem_cache_zalloc(t10_alua_lba_map_cache, GFP_KERNEL);
+	if (!lba_map) {
+		pr_err("Unable to allocate struct t10_alua_lba_map\n");
+		return ERR_PTR(-ENOMEM);
+	}
+	INIT_LIST_HEAD(&lba_map->lba_map_mem_list);
+	lba_map->lba_map_first_lba = first_lba;
+	lba_map->lba_map_last_lba = last_lba;
 
-		kfree(md_buf);
+	list_add_tail(&lba_map->lba_map_list, list);
+	return lba_map;
+}
+
+int
+core_alua_allocate_lba_map_mem(struct t10_alua_lba_map *lba_map,
+			       int pg_id, int state)
+{
+	struct t10_alua_lba_map_member *lba_map_mem;
+
+	list_for_each_entry(lba_map_mem, &lba_map->lba_map_mem_list,
+			    lba_map_mem_list) {
+		if (lba_map_mem->lba_map_mem_alua_pg_id == pg_id) {
+			pr_err("Duplicate pg_id %d in lba_map\n", pg_id);
+			return -EINVAL;
+		}
+	}
+
+	lba_map_mem = kmem_cache_zalloc(t10_alua_lba_map_mem_cache, GFP_KERNEL);
+	if (!lba_map_mem) {
+		pr_err("Unable to allocate struct t10_alua_lba_map_mem\n");
+		return -ENOMEM;
 	}
+	lba_map_mem->lba_map_mem_alua_state = state;
+	lba_map_mem->lba_map_mem_alua_pg_id = pg_id;
 
+	list_add_tail(&lba_map_mem->lba_map_mem_list,
+		      &lba_map->lba_map_mem_list);
 	return 0;
 }
 
+void
+core_alua_free_lba_map(struct list_head *lba_list)
+{
+	struct t10_alua_lba_map *lba_map, *lba_map_tmp;
+	struct t10_alua_lba_map_member *lba_map_mem, *lba_map_mem_tmp;
+
+	list_for_each_entry_safe(lba_map, lba_map_tmp, lba_list,
+				 lba_map_list) {
+		list_for_each_entry_safe(lba_map_mem, lba_map_mem_tmp,
+					 &lba_map->lba_map_mem_list,
+					 lba_map_mem_list) {
+			list_del(&lba_map_mem->lba_map_mem_list);
+			kmem_cache_free(t10_alua_lba_map_mem_cache,
+					lba_map_mem);
+		}
+		list_del(&lba_map->lba_map_list);
+		kmem_cache_free(t10_alua_lba_map_cache, lba_map);
+	}
+}
+
+void
+core_alua_set_lba_map(struct se_device *dev, struct list_head *lba_map_list,
+		      int segment_size, int segment_mult)
+{
+	struct list_head old_lba_map_list;
+	struct t10_alua_tg_pt_gp *tg_pt_gp;
+	int activate = 0, supported;
+
+	INIT_LIST_HEAD(&old_lba_map_list);
+	spin_lock(&dev->t10_alua.lba_map_lock);
+	dev->t10_alua.lba_map_segment_size = segment_size;
+	dev->t10_alua.lba_map_segment_multiplier = segment_mult;
+	list_splice_init(&dev->t10_alua.lba_map_list, &old_lba_map_list);
+	if (lba_map_list) {
+		list_splice_init(lba_map_list, &dev->t10_alua.lba_map_list);
+		activate = 1;
+	}
+	spin_unlock(&dev->t10_alua.lba_map_lock);
+	spin_lock(&dev->t10_alua.tg_pt_gps_lock);
+	list_for_each_entry(tg_pt_gp, &dev->t10_alua.tg_pt_gps_list,
+			    tg_pt_gp_list) {
+
+		if (!tg_pt_gp->tg_pt_gp_valid_id)
+			continue;
+		supported = tg_pt_gp->tg_pt_gp_alua_supported_states;
+		if (activate)
+			supported |= ALUA_LBD_SUP;
+		else
+			supported &= ~ALUA_LBD_SUP;
+		tg_pt_gp->tg_pt_gp_alua_supported_states = supported;
+	}
+	spin_unlock(&dev->t10_alua.tg_pt_gps_lock);
+	core_alua_free_lba_map(&old_lba_map_list);
+}
+
 struct t10_alua_lu_gp *
 core_alua_allocate_lu_gp(const char *name, int def_group)
 {
@@ -1346,8 +1715,9 @@ struct t10_alua_tg_pt_gp *core_alua_allocate_tg_pt_gp(struct se_device *dev,
 	mutex_init(&tg_pt_gp->tg_pt_gp_md_mutex);
 	spin_lock_init(&tg_pt_gp->tg_pt_gp_lock);
 	atomic_set(&tg_pt_gp->tg_pt_gp_ref_cnt, 0);
+	INIT_DELAYED_WORK(&tg_pt_gp->tg_pt_gp_transition_work,
+			  core_alua_do_transition_tg_pt_work);
 	tg_pt_gp->tg_pt_gp_dev = dev;
-	tg_pt_gp->tg_pt_gp_md_buf_len = ALUA_MD_BUF_LEN;
 	atomic_set(&tg_pt_gp->tg_pt_gp_alua_access_state,
 		ALUA_ACCESS_STATE_ACTIVE_OPTIMIZED);
 	/*
@@ -1475,6 +1845,8 @@ void core_alua_free_tg_pt_gp(
 	dev->t10_alua.alua_tg_pt_gps_counter--;
 	spin_unlock(&dev->t10_alua.tg_pt_gps_lock);
 
+	flush_delayed_work(&tg_pt_gp->tg_pt_gp_transition_work);
+
 	/*
 	 * Allow a struct t10_alua_tg_pt_gp_member * referenced by
 	 * core_alua_get_tg_pt_gp_by_name() in
diff --git a/drivers/target/target_core_alua.h b/drivers/target/target_core_alua.h
index 88e2e835f14a..0a7d65e80404 100644
--- a/drivers/target/target_core_alua.h
+++ b/drivers/target/target_core_alua.h
@@ -13,12 +13,13 @@
 /*
  * ASYMMETRIC ACCESS STATE field
  *
- * from spc4r17 section 6.27 Table 245
+ * from spc4r36j section 6.37 Table 307
  */
 #define ALUA_ACCESS_STATE_ACTIVE_OPTIMIZED	0x0
 #define ALUA_ACCESS_STATE_ACTIVE_NON_OPTIMIZED	0x1
 #define ALUA_ACCESS_STATE_STANDBY		0x2
 #define ALUA_ACCESS_STATE_UNAVAILABLE		0x3
+#define ALUA_ACCESS_STATE_LBA_DEPENDENT		0x4
 #define ALUA_ACCESS_STATE_OFFLINE		0xe
 #define ALUA_ACCESS_STATE_TRANSITION		0xf
 
@@ -78,18 +79,30 @@
  */
 #define ALUA_SECONDARY_METADATA_WWN_LEN			256
 
+/* Used by core_alua_update_tpg_(primary,secondary)_metadata */
+#define ALUA_MD_BUF_LEN					1024
+
 extern struct kmem_cache *t10_alua_lu_gp_cache;
 extern struct kmem_cache *t10_alua_lu_gp_mem_cache;
 extern struct kmem_cache *t10_alua_tg_pt_gp_cache;
 extern struct kmem_cache *t10_alua_tg_pt_gp_mem_cache;
+extern struct kmem_cache *t10_alua_lba_map_cache;
+extern struct kmem_cache *t10_alua_lba_map_mem_cache;
 
 extern sense_reason_t target_emulate_report_target_port_groups(struct se_cmd *);
 extern sense_reason_t target_emulate_set_target_port_groups(struct se_cmd *);
+extern sense_reason_t target_emulate_report_referrals(struct se_cmd *);
 extern int core_alua_check_nonop_delay(struct se_cmd *);
 extern int core_alua_do_port_transition(struct t10_alua_tg_pt_gp *,
 				struct se_device *, struct se_port *,
 				struct se_node_acl *, int, int);
 extern char *core_alua_dump_status(int);
+extern struct t10_alua_lba_map *core_alua_allocate_lba_map(
+				struct list_head *, u64, u64);
+extern int core_alua_allocate_lba_map_mem(struct t10_alua_lba_map *, int, int);
+extern void core_alua_free_lba_map(struct list_head *);
+extern void core_alua_set_lba_map(struct se_device *, struct list_head *,
+				int, int);
 extern struct t10_alua_lu_gp *core_alua_allocate_lu_gp(const char *, int);
 extern int core_alua_set_lu_gp_id(struct t10_alua_lu_gp *, u16);
 extern void core_alua_free_lu_gp(struct t10_alua_lu_gp *);
diff --git a/drivers/target/target_core_configfs.c b/drivers/target/target_core_configfs.c
index 272755d03e5a..f0e85b119692 100644
--- a/drivers/target/target_core_configfs.c
+++ b/drivers/target/target_core_configfs.c
@@ -643,6 +643,15 @@ SE_DEV_ATTR(emulate_caw, S_IRUGO | S_IWUSR);
 DEF_DEV_ATTRIB(emulate_3pc);
 SE_DEV_ATTR(emulate_3pc, S_IRUGO | S_IWUSR);
 
+DEF_DEV_ATTRIB(pi_prot_type);
+SE_DEV_ATTR(pi_prot_type, S_IRUGO | S_IWUSR);
+
+DEF_DEV_ATTRIB_RO(hw_pi_prot_type);
+SE_DEV_ATTR_RO(hw_pi_prot_type);
+
+DEF_DEV_ATTRIB(pi_prot_format);
+SE_DEV_ATTR(pi_prot_format, S_IRUGO | S_IWUSR);
+
 DEF_DEV_ATTRIB(enforce_pr_isids);
 SE_DEV_ATTR(enforce_pr_isids, S_IRUGO | S_IWUSR);
 
@@ -702,6 +711,9 @@ static struct configfs_attribute *target_core_dev_attrib_attrs[] = {
 	&target_core_dev_attrib_emulate_tpws.attr,
 	&target_core_dev_attrib_emulate_caw.attr,
 	&target_core_dev_attrib_emulate_3pc.attr,
+	&target_core_dev_attrib_pi_prot_type.attr,
+	&target_core_dev_attrib_hw_pi_prot_type.attr,
+	&target_core_dev_attrib_pi_prot_format.attr,
 	&target_core_dev_attrib_enforce_pr_isids.attr,
 	&target_core_dev_attrib_is_nonrot.attr,
 	&target_core_dev_attrib_emulate_rest_reord.attr,
@@ -1741,6 +1753,176 @@ static struct target_core_configfs_attribute target_core_attr_dev_alua_lu_gp = {
 	.store	= target_core_store_alua_lu_gp,
 };
 
+static ssize_t target_core_show_dev_lba_map(void *p, char *page)
+{
+	struct se_device *dev = p;
+	struct t10_alua_lba_map *map;
+	struct t10_alua_lba_map_member *mem;
+	char *b = page;
+	int bl = 0;
+	char state;
+
+	spin_lock(&dev->t10_alua.lba_map_lock);
+	if (!list_empty(&dev->t10_alua.lba_map_list))
+	    bl += sprintf(b + bl, "%u %u\n",
+			  dev->t10_alua.lba_map_segment_size,
+			  dev->t10_alua.lba_map_segment_multiplier);
+	list_for_each_entry(map, &dev->t10_alua.lba_map_list, lba_map_list) {
+		bl += sprintf(b + bl, "%llu %llu",
+			      map->lba_map_first_lba, map->lba_map_last_lba);
+		list_for_each_entry(mem, &map->lba_map_mem_list,
+				    lba_map_mem_list) {
+			switch (mem->lba_map_mem_alua_state) {
+			case ALUA_ACCESS_STATE_ACTIVE_OPTIMIZED:
+				state = 'O';
+				break;
+			case ALUA_ACCESS_STATE_ACTIVE_NON_OPTIMIZED:
+				state = 'A';
+				break;
+			case ALUA_ACCESS_STATE_STANDBY:
+				state = 'S';
+				break;
+			case ALUA_ACCESS_STATE_UNAVAILABLE:
+				state = 'U';
+				break;
+			default:
+				state = '.';
+				break;
+			}
+			bl += sprintf(b + bl, " %d:%c",
+				      mem->lba_map_mem_alua_pg_id, state);
+		}
+		bl += sprintf(b + bl, "\n");
+	}
+	spin_unlock(&dev->t10_alua.lba_map_lock);
+	return bl;
+}
+
+static ssize_t target_core_store_dev_lba_map(
+	void *p,
+	const char *page,
+	size_t count)
+{
+	struct se_device *dev = p;
+	struct t10_alua_lba_map *lba_map = NULL;
+	struct list_head lba_list;
+	char *map_entries, *ptr;
+	char state;
+	int pg_num = -1, pg;
+	int ret = 0, num = 0, pg_id, alua_state;
+	unsigned long start_lba = -1, end_lba = -1;
+	unsigned long segment_size = -1, segment_mult = -1;
+
+	map_entries = kstrdup(page, GFP_KERNEL);
+	if (!map_entries)
+		return -ENOMEM;
+
+	INIT_LIST_HEAD(&lba_list);
+	while ((ptr = strsep(&map_entries, "\n")) != NULL) {
+		if (!*ptr)
+			continue;
+
+		if (num == 0) {
+			if (sscanf(ptr, "%lu %lu\n",
+				   &segment_size, &segment_mult) != 2) {
+				pr_err("Invalid line %d\n", num);
+				ret = -EINVAL;
+				break;
+			}
+			num++;
+			continue;
+		}
+		if (sscanf(ptr, "%lu %lu", &start_lba, &end_lba) != 2) {
+			pr_err("Invalid line %d\n", num);
+			ret = -EINVAL;
+			break;
+		}
+		ptr = strchr(ptr, ' ');
+		if (!ptr) {
+			pr_err("Invalid line %d, missing end lba\n", num);
+			ret = -EINVAL;
+			break;
+		}
+		ptr++;
+		ptr = strchr(ptr, ' ');
+		if (!ptr) {
+			pr_err("Invalid line %d, missing state definitions\n",
+			       num);
+			ret = -EINVAL;
+			break;
+		}
+		ptr++;
+		lba_map = core_alua_allocate_lba_map(&lba_list,
+						     start_lba, end_lba);
+		if (IS_ERR(lba_map)) {
+			ret = PTR_ERR(lba_map);
+			break;
+		}
+		pg = 0;
+		while (sscanf(ptr, "%d:%c", &pg_id, &state) == 2) {
+			switch (state) {
+			case 'O':
+				alua_state = ALUA_ACCESS_STATE_ACTIVE_OPTIMIZED;
+				break;
+			case 'A':
+				alua_state = ALUA_ACCESS_STATE_ACTIVE_NON_OPTIMIZED;
+				break;
+			case 'S':
+				alua_state = ALUA_ACCESS_STATE_STANDBY;
+				break;
+			case 'U':
+				alua_state = ALUA_ACCESS_STATE_UNAVAILABLE;
+				break;
+			default:
+				pr_err("Invalid ALUA state '%c'\n", state);
+				ret = -EINVAL;
+				goto out;
+			}
+
+			ret = core_alua_allocate_lba_map_mem(lba_map,
+							     pg_id, alua_state);
+			if (ret) {
+				pr_err("Invalid target descriptor %d:%c "
+				       "at line %d\n",
+				       pg_id, state, num);
+				break;
+			}
+			pg++;
+			ptr = strchr(ptr, ' ');
+			if (ptr)
+				ptr++;
+			else
+				break;
+		}
+		if (pg_num == -1)
+		    pg_num = pg;
+		else if (pg != pg_num) {
+			pr_err("Only %d from %d port groups definitions "
+			       "at line %d\n", pg, pg_num, num);
+			ret = -EINVAL;
+			break;
+		}
+		num++;
+	}
+out:
+	if (ret) {
+		core_alua_free_lba_map(&lba_list);
+		count = ret;
+	} else
+		core_alua_set_lba_map(dev, &lba_list,
+				      segment_size, segment_mult);
+	kfree(map_entries);
+	return count;
+}
+
+static struct target_core_configfs_attribute target_core_attr_dev_lba_map = {
+	.attr	= { .ca_owner = THIS_MODULE,
+		    .ca_name = "lba_map",
+		    .ca_mode = S_IRUGO | S_IWUSR },
+	.show	= target_core_show_dev_lba_map,
+	.store	= target_core_store_dev_lba_map,
+};
+
 static struct configfs_attribute *lio_core_dev_attrs[] = {
 	&target_core_attr_dev_info.attr,
 	&target_core_attr_dev_control.attr,
@@ -1748,6 +1930,7 @@ static struct configfs_attribute *lio_core_dev_attrs[] = {
 	&target_core_attr_dev_udev_path.attr,
 	&target_core_attr_dev_enable.attr,
 	&target_core_attr_dev_alua_lu_gp.attr,
+	&target_core_attr_dev_lba_map.attr,
 	NULL,
 };
 
@@ -2054,6 +2237,13 @@ static ssize_t target_core_alua_tg_pt_gp_store_attr_alua_access_state(
 			" transition while TPGS_IMPLICIT_ALUA is disabled\n");
 		return -EINVAL;
 	}
+	if (tg_pt_gp->tg_pt_gp_alua_access_type & TPGS_EXPLICIT_ALUA &&
+	    new_state == ALUA_ACCESS_STATE_LBA_DEPENDENT) {
+		/* LBA DEPENDENT is only allowed with implicit ALUA */
+		pr_err("Unable to process implicit configfs ALUA transition"
+		       " while explicit ALUA management is enabled\n");
+		return -EINVAL;
+	}
 
 	ret = core_alua_do_port_transition(tg_pt_gp, dev,
 					NULL, NULL, new_state, 0);
@@ -2188,7 +2378,7 @@ SE_DEV_ALUA_SUPPORT_STATE_SHOW(lba_dependent,
 			       tg_pt_gp_alua_supported_states, ALUA_LBD_SUP);
 SE_DEV_ALUA_SUPPORT_STATE_STORE(lba_dependent,
 				tg_pt_gp_alua_supported_states, ALUA_LBD_SUP);
-SE_DEV_ALUA_TG_PT_ATTR(alua_support_lba_dependent, S_IRUGO | S_IWUSR);
+SE_DEV_ALUA_TG_PT_ATTR(alua_support_lba_dependent, S_IRUGO);
 
 SE_DEV_ALUA_SUPPORT_STATE_SHOW(unavailable,
 			       tg_pt_gp_alua_supported_states, ALUA_U_SUP);
@@ -2937,7 +3127,7 @@ static int __init target_core_init_configfs(void)
 	 * and ALUA Logical Unit Group and Target Port Group infrastructure.
 	 */
 	target_cg = &subsys->su_group;
-	target_cg->default_groups = kmalloc(sizeof(struct config_group) * 2,
+	target_cg->default_groups = kmalloc(sizeof(struct config_group *) * 2,
 				GFP_KERNEL);
 	if (!target_cg->default_groups) {
 		pr_err("Unable to allocate target_cg->default_groups\n");
diff --git a/drivers/target/target_core_device.c b/drivers/target/target_core_device.c
index d06de84b069b..65001e133670 100644
--- a/drivers/target/target_core_device.c
+++ b/drivers/target/target_core_device.c
@@ -918,6 +918,90 @@ int se_dev_set_emulate_3pc(struct se_device *dev, int flag)
 	return 0;
 }
 
+int se_dev_set_pi_prot_type(struct se_device *dev, int flag)
+{
+	int rc, old_prot = dev->dev_attrib.pi_prot_type;
+
+	if (flag != 0 && flag != 1 && flag != 2 && flag != 3) {
+		pr_err("Illegal value %d for pi_prot_type\n", flag);
+		return -EINVAL;
+	}
+	if (flag == 2) {
+		pr_err("DIF TYPE2 protection currently not supported\n");
+		return -ENOSYS;
+	}
+	if (dev->dev_attrib.hw_pi_prot_type) {
+		pr_warn("DIF protection enabled on underlying hardware,"
+			" ignoring\n");
+		return 0;
+	}
+	if (!dev->transport->init_prot || !dev->transport->free_prot) {
+		pr_err("DIF protection not supported by backend: %s\n",
+		       dev->transport->name);
+		return -ENOSYS;
+	}
+	if (!(dev->dev_flags & DF_CONFIGURED)) {
+		pr_err("DIF protection requires device to be configured\n");
+		return -ENODEV;
+	}
+	if (dev->export_count) {
+		pr_err("dev[%p]: Unable to change SE Device PROT type while"
+		       " export_count is %d\n", dev, dev->export_count);
+		return -EINVAL;
+	}
+
+	dev->dev_attrib.pi_prot_type = flag;
+
+	if (flag && !old_prot) {
+		rc = dev->transport->init_prot(dev);
+		if (rc) {
+			dev->dev_attrib.pi_prot_type = old_prot;
+			return rc;
+		}
+
+	} else if (!flag && old_prot) {
+		dev->transport->free_prot(dev);
+	}
+	pr_debug("dev[%p]: SE Device Protection Type: %d\n", dev, flag);
+
+	return 0;
+}
+
+int se_dev_set_pi_prot_format(struct se_device *dev, int flag)
+{
+	int rc;
+
+	if (!flag)
+		return 0;
+
+	if (flag != 1) {
+		pr_err("Illegal value %d for pi_prot_format\n", flag);
+		return -EINVAL;
+	}
+	if (!dev->transport->format_prot) {
+		pr_err("DIF protection format not supported by backend %s\n",
+		       dev->transport->name);
+		return -ENOSYS;
+	}
+	if (!(dev->dev_flags & DF_CONFIGURED)) {
+		pr_err("DIF protection format requires device to be configured\n");
+		return -ENODEV;
+	}
+	if (dev->export_count) {
+		pr_err("dev[%p]: Unable to format SE Device PROT type while"
+		       " export_count is %d\n", dev, dev->export_count);
+		return -EINVAL;
+	}
+
+	rc = dev->transport->format_prot(dev);
+	if (rc)
+		return rc;
+
+	pr_debug("dev[%p]: SE Device Protection Format complete\n", dev);
+
+	return 0;
+}
+
 int se_dev_set_enforce_pr_isids(struct se_device *dev, int flag)
 {
 	if ((flag != 0) && (flag != 1)) {
@@ -1117,23 +1201,23 @@ int se_dev_set_block_size(struct se_device *dev, u32 block_size)
 struct se_lun *core_dev_add_lun(
 	struct se_portal_group *tpg,
 	struct se_device *dev,
-	u32 lun)
+	u32 unpacked_lun)
 {
-	struct se_lun *lun_p;
+	struct se_lun *lun;
 	int rc;
 
-	lun_p = core_tpg_pre_addlun(tpg, lun);
-	if (IS_ERR(lun_p))
-		return lun_p;
+	lun = core_tpg_alloc_lun(tpg, unpacked_lun);
+	if (IS_ERR(lun))
+		return lun;
 
-	rc = core_tpg_post_addlun(tpg, lun_p,
+	rc = core_tpg_add_lun(tpg, lun,
 				TRANSPORT_LUNFLAGS_READ_WRITE, dev);
 	if (rc < 0)
 		return ERR_PTR(rc);
 
 	pr_debug("%s_TPG[%u]_LUN[%u] - Activated %s Logical Unit from"
 		" CORE HBA: %u\n", tpg->se_tpg_tfo->get_fabric_name(),
-		tpg->se_tpg_tfo->tpg_get_tag(tpg), lun_p->unpacked_lun,
+		tpg->se_tpg_tfo->tpg_get_tag(tpg), lun->unpacked_lun,
 		tpg->se_tpg_tfo->get_fabric_name(), dev->se_hba->hba_id);
 	/*
 	 * Update LUN maps for dynamically added initiators when
@@ -1154,7 +1238,7 @@ struct se_lun *core_dev_add_lun(
 		spin_unlock_irq(&tpg->acl_node_lock);
 	}
 
-	return lun_p;
+	return lun;
 }
 
 /*      core_dev_del_lun():
@@ -1420,6 +1504,7 @@ struct se_device *target_alloc_device(struct se_hba *hba, const char *name)
 	dev->dev_link_magic = SE_DEV_LINK_MAGIC;
 	dev->se_hba = hba;
 	dev->transport = hba->transport;
+	dev->prot_length = sizeof(struct se_dif_v1_tuple);
 
 	INIT_LIST_HEAD(&dev->dev_list);
 	INIT_LIST_HEAD(&dev->dev_sep_list);
@@ -1444,6 +1529,8 @@ struct se_device *target_alloc_device(struct se_hba *hba, const char *name)
 	spin_lock_init(&dev->t10_pr.aptpl_reg_lock);
 	INIT_LIST_HEAD(&dev->t10_alua.tg_pt_gps_list);
 	spin_lock_init(&dev->t10_alua.tg_pt_gps_lock);
+	INIT_LIST_HEAD(&dev->t10_alua.lba_map_list);
+	spin_lock_init(&dev->t10_alua.lba_map_lock);
 
 	dev->t10_wwn.t10_dev = dev;
 	dev->t10_alua.t10_dev = dev;
@@ -1460,6 +1547,7 @@ struct se_device *target_alloc_device(struct se_hba *hba, const char *name)
 	dev->dev_attrib.emulate_tpws = DA_EMULATE_TPWS;
 	dev->dev_attrib.emulate_caw = DA_EMULATE_CAW;
 	dev->dev_attrib.emulate_3pc = DA_EMULATE_3PC;
+	dev->dev_attrib.pi_prot_type = TARGET_DIF_TYPE0_PROT;
 	dev->dev_attrib.enforce_pr_isids = DA_ENFORCE_PR_ISIDS;
 	dev->dev_attrib.is_nonrot = DA_IS_NONROT;
 	dev->dev_attrib.emulate_rest_reord = DA_EMULATE_REST_REORD;
@@ -1588,9 +1676,13 @@ void target_free_device(struct se_device *dev)
 	}
 
 	core_alua_free_lu_gp_mem(dev);
+	core_alua_set_lba_map(dev, NULL, 0, 0);
 	core_scsi3_free_all_registrations(dev);
 	se_release_vpd_for_dev(dev);
 
+	if (dev->transport->free_prot)
+		dev->transport->free_prot(dev);
+
 	dev->transport->free_device(dev);
 }
 
diff --git a/drivers/target/target_core_fabric_configfs.c b/drivers/target/target_core_fabric_configfs.c
index dae2ad6a669e..7de9f0475d05 100644
--- a/drivers/target/target_core_fabric_configfs.c
+++ b/drivers/target/target_core_fabric_configfs.c
@@ -906,7 +906,7 @@ static struct config_group *target_fabric_make_lun(
 	lun_cg->default_groups[1] = NULL;
 
 	port_stat_grp = &lun->port_stat_grps.stat_group;
-	port_stat_grp->default_groups =  kzalloc(sizeof(struct config_group) * 3,
+	port_stat_grp->default_groups =  kzalloc(sizeof(struct config_group *) * 4,
 				GFP_KERNEL);
 	if (!port_stat_grp->default_groups) {
 		pr_err("Unable to allocate port_stat_grp->default_groups\n");
diff --git a/drivers/target/target_core_file.c b/drivers/target/target_core_file.c
index 78241a53b555..cf991a91a8a9 100644
--- a/drivers/target/target_core_file.c
+++ b/drivers/target/target_core_file.c
@@ -257,6 +257,72 @@ static void fd_free_device(struct se_device *dev)
 	kfree(fd_dev);
 }
 
+static int fd_do_prot_rw(struct se_cmd *cmd, struct fd_prot *fd_prot,
+			 int is_write)
+{
+	struct se_device *se_dev = cmd->se_dev;
+	struct fd_dev *dev = FD_DEV(se_dev);
+	struct file *prot_fd = dev->fd_prot_file;
+	struct scatterlist *sg;
+	loff_t pos = (cmd->t_task_lba * se_dev->prot_length);
+	unsigned char *buf;
+	u32 prot_size, len, size;
+	int rc, ret = 1, i;
+
+	prot_size = (cmd->data_length / se_dev->dev_attrib.block_size) *
+		     se_dev->prot_length;
+
+	if (!is_write) {
+		fd_prot->prot_buf = vzalloc(prot_size);
+		if (!fd_prot->prot_buf) {
+			pr_err("Unable to allocate fd_prot->prot_buf\n");
+			return -ENOMEM;
+		}
+		buf = fd_prot->prot_buf;
+
+		fd_prot->prot_sg_nents = cmd->t_prot_nents;
+		fd_prot->prot_sg = kzalloc(sizeof(struct scatterlist) *
+					   fd_prot->prot_sg_nents, GFP_KERNEL);
+		if (!fd_prot->prot_sg) {
+			pr_err("Unable to allocate fd_prot->prot_sg\n");
+			vfree(fd_prot->prot_buf);
+			return -ENOMEM;
+		}
+		size = prot_size;
+
+		for_each_sg(fd_prot->prot_sg, sg, fd_prot->prot_sg_nents, i) {
+
+			len = min_t(u32, PAGE_SIZE, size);
+			sg_set_buf(sg, buf, len);
+			size -= len;
+			buf += len;
+		}
+	}
+
+	if (is_write) {
+		rc = kernel_write(prot_fd, fd_prot->prot_buf, prot_size, pos);
+		if (rc < 0 || prot_size != rc) {
+			pr_err("kernel_write() for fd_do_prot_rw failed:"
+			       " %d\n", rc);
+			ret = -EINVAL;
+		}
+	} else {
+		rc = kernel_read(prot_fd, pos, fd_prot->prot_buf, prot_size);
+		if (rc < 0) {
+			pr_err("kernel_read() for fd_do_prot_rw failed:"
+			       " %d\n", rc);
+			ret = -EINVAL;
+		}
+	}
+
+	if (is_write || ret < 0) {
+		kfree(fd_prot->prot_sg);
+		vfree(fd_prot->prot_buf);
+	}
+
+	return ret;
+}
+
 static int fd_do_rw(struct se_cmd *cmd, struct scatterlist *sgl,
 		u32 sgl_nents, int is_write)
 {
@@ -551,6 +617,8 @@ fd_execute_rw(struct se_cmd *cmd, struct scatterlist *sgl, u32 sgl_nents,
 	      enum dma_data_direction data_direction)
 {
 	struct se_device *dev = cmd->se_dev;
+	struct fd_prot fd_prot;
+	sense_reason_t rc;
 	int ret = 0;
 
 	/*
@@ -558,8 +626,48 @@ fd_execute_rw(struct se_cmd *cmd, struct scatterlist *sgl, u32 sgl_nents,
 	 * physical memory addresses to struct iovec virtual memory.
 	 */
 	if (data_direction == DMA_FROM_DEVICE) {
+		memset(&fd_prot, 0, sizeof(struct fd_prot));
+
+		if (cmd->prot_type) {
+			ret = fd_do_prot_rw(cmd, &fd_prot, false);
+			if (ret < 0)
+				return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
+		}
+
 		ret = fd_do_rw(cmd, sgl, sgl_nents, 0);
+
+		if (ret > 0 && cmd->prot_type) {
+			u32 sectors = cmd->data_length / dev->dev_attrib.block_size;
+
+			rc = sbc_dif_verify_read(cmd, cmd->t_task_lba, sectors,
+						 0, fd_prot.prot_sg, 0);
+			if (rc) {
+				kfree(fd_prot.prot_sg);
+				vfree(fd_prot.prot_buf);
+				return rc;
+			}
+			kfree(fd_prot.prot_sg);
+			vfree(fd_prot.prot_buf);
+		}
 	} else {
+		memset(&fd_prot, 0, sizeof(struct fd_prot));
+
+		if (cmd->prot_type) {
+			u32 sectors = cmd->data_length / dev->dev_attrib.block_size;
+
+			ret = fd_do_prot_rw(cmd, &fd_prot, false);
+			if (ret < 0)
+				return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
+
+			rc = sbc_dif_verify_write(cmd, cmd->t_task_lba, sectors,
+						  0, fd_prot.prot_sg, 0);
+			if (rc) {
+				kfree(fd_prot.prot_sg);
+				vfree(fd_prot.prot_buf);
+				return rc;
+			}
+		}
+
 		ret = fd_do_rw(cmd, sgl, sgl_nents, 1);
 		/*
 		 * Perform implicit vfs_fsync_range() for fd_do_writev() ops
@@ -576,10 +684,19 @@ fd_execute_rw(struct se_cmd *cmd, struct scatterlist *sgl, u32 sgl_nents,
 
 			vfs_fsync_range(fd_dev->fd_file, start, end, 1);
 		}
+
+		if (ret > 0 && cmd->prot_type) {
+			ret = fd_do_prot_rw(cmd, &fd_prot, true);
+			if (ret < 0)
+				return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
+		}
 	}
 
-	if (ret < 0)
+	if (ret < 0) {
+		kfree(fd_prot.prot_sg);
+		vfree(fd_prot.prot_buf);
 		return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
+	}
 
 	if (ret)
 		target_complete_cmd(cmd, SAM_STAT_GOOD);
@@ -700,6 +817,140 @@ static sector_t fd_get_blocks(struct se_device *dev)
 		       dev->dev_attrib.block_size);
 }
 
+static int fd_init_prot(struct se_device *dev)
+{
+	struct fd_dev *fd_dev = FD_DEV(dev);
+	struct file *prot_file, *file = fd_dev->fd_file;
+	struct inode *inode;
+	int ret, flags = O_RDWR | O_CREAT | O_LARGEFILE | O_DSYNC;
+	char buf[FD_MAX_DEV_PROT_NAME];
+
+	if (!file) {
+		pr_err("Unable to locate fd_dev->fd_file\n");
+		return -ENODEV;
+	}
+
+	inode = file->f_mapping->host;
+	if (S_ISBLK(inode->i_mode)) {
+		pr_err("FILEIO Protection emulation only supported on"
+		       " !S_ISBLK\n");
+		return -ENOSYS;
+	}
+
+	if (fd_dev->fbd_flags & FDBD_HAS_BUFFERED_IO_WCE)
+		flags &= ~O_DSYNC;
+
+	snprintf(buf, FD_MAX_DEV_PROT_NAME, "%s.protection",
+		 fd_dev->fd_dev_name);
+
+	prot_file = filp_open(buf, flags, 0600);
+	if (IS_ERR(prot_file)) {
+		pr_err("filp_open(%s) failed\n", buf);
+		ret = PTR_ERR(prot_file);
+		return ret;
+	}
+	fd_dev->fd_prot_file = prot_file;
+
+	return 0;
+}
+
+static void fd_init_format_buf(struct se_device *dev, unsigned char *buf,
+			       u32 unit_size, u32 *ref_tag, u16 app_tag,
+			       bool inc_reftag)
+{
+	unsigned char *p = buf;
+	int i;
+
+	for (i = 0; i < unit_size; i += dev->prot_length) {
+		*((u16 *)&p[0]) = 0xffff;
+		*((__be16 *)&p[2]) = cpu_to_be16(app_tag);
+		*((__be32 *)&p[4]) = cpu_to_be32(*ref_tag);
+
+		if (inc_reftag)
+			(*ref_tag)++;
+
+		p += dev->prot_length;
+	}
+}
+
+static int fd_format_prot(struct se_device *dev)
+{
+	struct fd_dev *fd_dev = FD_DEV(dev);
+	struct file *prot_fd = fd_dev->fd_prot_file;
+	sector_t prot_length, prot;
+	unsigned char *buf;
+	loff_t pos = 0;
+	u32 ref_tag = 0;
+	int unit_size = FDBD_FORMAT_UNIT_SIZE * dev->dev_attrib.block_size;
+	int rc, ret = 0, size, len;
+	bool inc_reftag = false;
+
+	if (!dev->dev_attrib.pi_prot_type) {
+		pr_err("Unable to format_prot while pi_prot_type == 0\n");
+		return -ENODEV;
+	}
+	if (!prot_fd) {
+		pr_err("Unable to locate fd_dev->fd_prot_file\n");
+		return -ENODEV;
+	}
+
+	switch (dev->dev_attrib.pi_prot_type) {
+	case TARGET_DIF_TYPE3_PROT:
+		ref_tag = 0xffffffff;
+		break;
+	case TARGET_DIF_TYPE2_PROT:
+	case TARGET_DIF_TYPE1_PROT:
+		inc_reftag = true;
+		break;
+	default:
+		break;
+	}
+
+	buf = vzalloc(unit_size);
+	if (!buf) {
+		pr_err("Unable to allocate FILEIO prot buf\n");
+		return -ENOMEM;
+	}
+
+	prot_length = (dev->transport->get_blocks(dev) + 1) * dev->prot_length;
+	size = prot_length;
+
+	pr_debug("Using FILEIO prot_length: %llu\n",
+		 (unsigned long long)prot_length);
+
+	for (prot = 0; prot < prot_length; prot += unit_size) {
+
+		fd_init_format_buf(dev, buf, unit_size, &ref_tag, 0xffff,
+				   inc_reftag);
+
+		len = min(unit_size, size);
+
+		rc = kernel_write(prot_fd, buf, len, pos);
+		if (rc != len) {
+			pr_err("vfs_write to prot file failed: %d\n", rc);
+			ret = -ENODEV;
+			goto out;
+		}
+		pos += len;
+		size -= len;
+	}
+
+out:
+	vfree(buf);
+	return ret;
+}
+
+static void fd_free_prot(struct se_device *dev)
+{
+	struct fd_dev *fd_dev = FD_DEV(dev);
+
+	if (!fd_dev->fd_prot_file)
+		return;
+
+	filp_close(fd_dev->fd_prot_file, NULL);
+	fd_dev->fd_prot_file = NULL;
+}
+
 static struct sbc_ops fd_sbc_ops = {
 	.execute_rw		= fd_execute_rw,
 	.execute_sync_cache	= fd_execute_sync_cache,
@@ -730,6 +981,9 @@ static struct se_subsystem_api fileio_template = {
 	.show_configfs_dev_params = fd_show_configfs_dev_params,
 	.get_device_type	= sbc_get_device_type,
 	.get_blocks		= fd_get_blocks,
+	.init_prot		= fd_init_prot,
+	.format_prot		= fd_format_prot,
+	.free_prot		= fd_free_prot,
 };
 
 static int __init fileio_module_init(void)
diff --git a/drivers/target/target_core_file.h b/drivers/target/target_core_file.h
index d7772c167685..182cbb295039 100644
--- a/drivers/target/target_core_file.h
+++ b/drivers/target/target_core_file.h
@@ -4,6 +4,7 @@
 #define FD_VERSION		"4.0"
 
 #define FD_MAX_DEV_NAME		256
+#define FD_MAX_DEV_PROT_NAME	FD_MAX_DEV_NAME + 16
 #define FD_DEVICE_QUEUE_DEPTH	32
 #define FD_MAX_DEVICE_QUEUE_DEPTH 128
 #define FD_BLOCKSIZE		512
@@ -18,6 +19,13 @@
 #define FBDF_HAS_PATH		0x01
 #define FBDF_HAS_SIZE		0x02
 #define FDBD_HAS_BUFFERED_IO_WCE 0x04
+#define FDBD_FORMAT_UNIT_SIZE	2048
+
+struct fd_prot {
+	unsigned char	*prot_buf;
+	struct scatterlist *prot_sg;
+	u32 prot_sg_nents;
+};
 
 struct fd_dev {
 	struct se_device dev;
@@ -32,6 +40,7 @@ struct fd_dev {
 	u32		fd_block_size;
 	unsigned long long fd_dev_size;
 	struct file	*fd_file;
+	struct file	*fd_prot_file;
 	/* FILEIO HBA device is connected to */
 	struct fd_host *fd_host;
 } ____cacheline_aligned;
diff --git a/drivers/target/target_core_iblock.c b/drivers/target/target_core_iblock.c
index c87959f12760..554d4f75a75a 100644
--- a/drivers/target/target_core_iblock.c
+++ b/drivers/target/target_core_iblock.c
@@ -91,6 +91,7 @@ static int iblock_configure_device(struct se_device *dev)
 	struct iblock_dev *ib_dev = IBLOCK_DEV(dev);
 	struct request_queue *q;
 	struct block_device *bd = NULL;
+	struct blk_integrity *bi;
 	fmode_t mode;
 	int ret = -ENOMEM;
 
@@ -155,8 +156,40 @@ static int iblock_configure_device(struct se_device *dev)
 	if (blk_queue_nonrot(q))
 		dev->dev_attrib.is_nonrot = 1;
 
+	bi = bdev_get_integrity(bd);
+	if (bi) {
+		struct bio_set *bs = ib_dev->ibd_bio_set;
+
+		if (!strcmp(bi->name, "T10-DIF-TYPE3-IP") ||
+		    !strcmp(bi->name, "T10-DIF-TYPE1-IP")) {
+			pr_err("IBLOCK export of blk_integrity: %s not"
+			       " supported\n", bi->name);
+			ret = -ENOSYS;
+			goto out_blkdev_put;
+		}
+
+		if (!strcmp(bi->name, "T10-DIF-TYPE3-CRC")) {
+			dev->dev_attrib.pi_prot_type = TARGET_DIF_TYPE3_PROT;
+		} else if (!strcmp(bi->name, "T10-DIF-TYPE1-CRC")) {
+			dev->dev_attrib.pi_prot_type = TARGET_DIF_TYPE1_PROT;
+		}
+
+		if (dev->dev_attrib.pi_prot_type) {
+			if (bioset_integrity_create(bs, IBLOCK_BIO_POOL_SIZE) < 0) {
+				pr_err("Unable to allocate bioset for PI\n");
+				ret = -ENOMEM;
+				goto out_blkdev_put;
+			}
+			pr_debug("IBLOCK setup BIP bs->bio_integrity_pool: %p\n",
+				 bs->bio_integrity_pool);
+		}
+		dev->dev_attrib.hw_pi_prot_type = dev->dev_attrib.pi_prot_type;
+	}
+
 	return 0;
 
+out_blkdev_put:
+	blkdev_put(ib_dev->ibd_bd, FMODE_WRITE|FMODE_READ|FMODE_EXCL);
 out_free_bioset:
 	bioset_free(ib_dev->ibd_bio_set);
 	ib_dev->ibd_bio_set = NULL;
@@ -170,8 +203,10 @@ static void iblock_free_device(struct se_device *dev)
 
 	if (ib_dev->ibd_bd != NULL)
 		blkdev_put(ib_dev->ibd_bd, FMODE_WRITE|FMODE_READ|FMODE_EXCL);
-	if (ib_dev->ibd_bio_set != NULL)
+	if (ib_dev->ibd_bio_set != NULL) {
+		bioset_integrity_free(ib_dev->ibd_bio_set);
 		bioset_free(ib_dev->ibd_bio_set);
+	}
 	kfree(ib_dev);
 }
 
@@ -319,7 +354,7 @@ iblock_get_bio(struct se_cmd *cmd, sector_t lba, u32 sg_num)
 	bio->bi_bdev = ib_dev->ibd_bd;
 	bio->bi_private = cmd;
 	bio->bi_end_io = &iblock_bio_done;
-	bio->bi_sector = lba;
+	bio->bi_iter.bi_sector = lba;
 
 	return bio;
 }
@@ -586,13 +621,58 @@ static ssize_t iblock_show_configfs_dev_params(struct se_device *dev, char *b)
 	return bl;
 }
 
+static int
+iblock_alloc_bip(struct se_cmd *cmd, struct bio *bio)
+{
+	struct se_device *dev = cmd->se_dev;
+	struct blk_integrity *bi;
+	struct bio_integrity_payload *bip;
+	struct iblock_dev *ib_dev = IBLOCK_DEV(dev);
+	struct scatterlist *sg;
+	int i, rc;
+
+	bi = bdev_get_integrity(ib_dev->ibd_bd);
+	if (!bi) {
+		pr_err("Unable to locate bio_integrity\n");
+		return -ENODEV;
+	}
+
+	bip = bio_integrity_alloc(bio, GFP_NOIO, cmd->t_prot_nents);
+	if (!bip) {
+		pr_err("Unable to allocate bio_integrity_payload\n");
+		return -ENOMEM;
+	}
+
+	bip->bip_iter.bi_size = (cmd->data_length / dev->dev_attrib.block_size) *
+			 dev->prot_length;
+	bip->bip_iter.bi_sector = bio->bi_iter.bi_sector;
+
+	pr_debug("IBLOCK BIP Size: %u Sector: %llu\n", bip->bip_iter.bi_size,
+		 (unsigned long long)bip->bip_iter.bi_sector);
+
+	for_each_sg(cmd->t_prot_sg, sg, cmd->t_prot_nents, i) {
+
+		rc = bio_integrity_add_page(bio, sg_page(sg), sg->length,
+					    sg->offset);
+		if (rc != sg->length) {
+			pr_err("bio_integrity_add_page() failed; %d\n", rc);
+			return -ENOMEM;
+		}
+
+		pr_debug("Added bio integrity page: %p length: %d offset; %d\n",
+			 sg_page(sg), sg->length, sg->offset);
+	}
+
+	return 0;
+}
+
 static sense_reason_t
 iblock_execute_rw(struct se_cmd *cmd, struct scatterlist *sgl, u32 sgl_nents,
 		  enum dma_data_direction data_direction)
 {
 	struct se_device *dev = cmd->se_dev;
 	struct iblock_req *ibr;
-	struct bio *bio;
+	struct bio *bio, *bio_start;
 	struct bio_list list;
 	struct scatterlist *sg;
 	u32 sg_num = sgl_nents;
@@ -655,6 +735,7 @@ iblock_execute_rw(struct se_cmd *cmd, struct scatterlist *sgl, u32 sgl_nents,
 	if (!bio)
 		goto fail_free_ibr;
 
+	bio_start = bio;
 	bio_list_init(&list);
 	bio_list_add(&list, bio);
 
@@ -688,6 +769,12 @@ iblock_execute_rw(struct se_cmd *cmd, struct scatterlist *sgl, u32 sgl_nents,
 		sg_num--;
 	}
 
+	if (cmd->prot_type) {
+		int rc = iblock_alloc_bip(cmd, bio_start);
+		if (rc)
+			goto fail_put_bios;
+	}
+
 	iblock_submit_bios(&list, rw);
 	iblock_complete_cmd(cmd);
 	return 0;
@@ -763,7 +850,7 @@ iblock_parse_cdb(struct se_cmd *cmd)
 	return sbc_parse_cdb(cmd, &iblock_sbc_ops);
 }
 
-bool iblock_get_write_cache(struct se_device *dev)
+static bool iblock_get_write_cache(struct se_device *dev)
 {
 	struct iblock_dev *ib_dev = IBLOCK_DEV(dev);
 	struct block_device *bd = ib_dev->ibd_bd;
diff --git a/drivers/target/target_core_internal.h b/drivers/target/target_core_internal.h
index 47b63b094cdc..de9cab708f45 100644
--- a/drivers/target/target_core_internal.h
+++ b/drivers/target/target_core_internal.h
@@ -35,6 +35,8 @@ int	se_dev_set_emulate_tpu(struct se_device *, int);
 int	se_dev_set_emulate_tpws(struct se_device *, int);
 int	se_dev_set_emulate_caw(struct se_device *, int);
 int	se_dev_set_emulate_3pc(struct se_device *, int);
+int	se_dev_set_pi_prot_type(struct se_device *, int);
+int	se_dev_set_pi_prot_format(struct se_device *, int);
 int	se_dev_set_enforce_pr_isids(struct se_device *, int);
 int	se_dev_set_is_nonrot(struct se_device *, int);
 int	se_dev_set_emulate_rest_reord(struct se_device *dev, int);
@@ -77,9 +79,9 @@ struct se_node_acl *__core_tpg_get_initiator_node_acl(struct se_portal_group *tp
 		const char *);
 void	core_tpg_add_node_to_devs(struct se_node_acl *, struct se_portal_group *);
 void	core_tpg_wait_for_nacl_pr_ref(struct se_node_acl *);
-struct se_lun *core_tpg_pre_addlun(struct se_portal_group *, u32);
-int	core_tpg_post_addlun(struct se_portal_group *, struct se_lun *,
-		u32, void *);
+struct se_lun *core_tpg_alloc_lun(struct se_portal_group *, u32);
+int	core_tpg_add_lun(struct se_portal_group *, struct se_lun *,
+		u32, struct se_device *);
 struct se_lun *core_tpg_pre_dellun(struct se_portal_group *, u32 unpacked_lun);
 int	core_tpg_post_dellun(struct se_portal_group *, struct se_lun *);
 
diff --git a/drivers/target/target_core_pr.h b/drivers/target/target_core_pr.h
index ed75cdd32cb0..2ee2936fa0bd 100644
--- a/drivers/target/target_core_pr.h
+++ b/drivers/target/target_core_pr.h
@@ -43,6 +43,11 @@
 #define PR_APTPL_MAX_IPORT_LEN			256
 #define PR_APTPL_MAX_TPORT_LEN			256
 
+/*
+ *  Function defined in target_core_spc.c
+ */
+void spc_parse_naa_6h_vendor_specific(struct se_device *, unsigned char *);
+
 extern struct kmem_cache *t10_pr_reg_cache;
 
 extern void core_pr_dump_initiator_port(struct t10_pr_registration *,
diff --git a/drivers/target/target_core_rd.c b/drivers/target/target_core_rd.c
index 4ffe5f2ec0e9..66a5aba5a0d9 100644
--- a/drivers/target/target_core_rd.c
+++ b/drivers/target/target_core_rd.c
@@ -78,23 +78,14 @@ static void rd_detach_hba(struct se_hba *hba)
 	hba->hba_ptr = NULL;
 }
 
-/*	rd_release_device_space():
- *
- *
- */
-static void rd_release_device_space(struct rd_dev *rd_dev)
+static u32 rd_release_sgl_table(struct rd_dev *rd_dev, struct rd_dev_sg_table *sg_table,
+				 u32 sg_table_count)
 {
-	u32 i, j, page_count = 0, sg_per_table;
-	struct rd_dev_sg_table *sg_table;
 	struct page *pg;
 	struct scatterlist *sg;
+	u32 i, j, page_count = 0, sg_per_table;
 
-	if (!rd_dev->sg_table_array || !rd_dev->sg_table_count)
-		return;
-
-	sg_table = rd_dev->sg_table_array;
-
-	for (i = 0; i < rd_dev->sg_table_count; i++) {
+	for (i = 0; i < sg_table_count; i++) {
 		sg = sg_table[i].sg_table;
 		sg_per_table = sg_table[i].rd_sg_count;
 
@@ -105,16 +96,28 @@ static void rd_release_device_space(struct rd_dev *rd_dev)
 				page_count++;
 			}
 		}
-
 		kfree(sg);
 	}
 
+	kfree(sg_table);
+	return page_count;
+}
+
+static void rd_release_device_space(struct rd_dev *rd_dev)
+{
+	u32 page_count;
+
+	if (!rd_dev->sg_table_array || !rd_dev->sg_table_count)
+		return;
+
+	page_count = rd_release_sgl_table(rd_dev, rd_dev->sg_table_array,
+					  rd_dev->sg_table_count);
+
 	pr_debug("CORE_RD[%u] - Released device space for Ramdisk"
 		" Device ID: %u, pages %u in %u tables total bytes %lu\n",
 		rd_dev->rd_host->rd_host_id, rd_dev->rd_dev_id, page_count,
 		rd_dev->sg_table_count, (unsigned long)page_count * PAGE_SIZE);
 
-	kfree(sg_table);
 	rd_dev->sg_table_array = NULL;
 	rd_dev->sg_table_count = 0;
 }
@@ -124,38 +127,15 @@ static void rd_release_device_space(struct rd_dev *rd_dev)
  *
  *
  */
-static int rd_build_device_space(struct rd_dev *rd_dev)
+static int rd_allocate_sgl_table(struct rd_dev *rd_dev, struct rd_dev_sg_table *sg_table,
+				 u32 total_sg_needed, unsigned char init_payload)
 {
-	u32 i = 0, j, page_offset = 0, sg_per_table, sg_tables, total_sg_needed;
+	u32 i = 0, j, page_offset = 0, sg_per_table;
 	u32 max_sg_per_table = (RD_MAX_ALLOCATION_SIZE /
 				sizeof(struct scatterlist));
-	struct rd_dev_sg_table *sg_table;
 	struct page *pg;
 	struct scatterlist *sg;
-
-	if (rd_dev->rd_page_count <= 0) {
-		pr_err("Illegal page count: %u for Ramdisk device\n",
-			rd_dev->rd_page_count);
-		return -EINVAL;
-	}
-
-	/* Don't need backing pages for NULLIO */
-	if (rd_dev->rd_flags & RDF_NULLIO)
-		return 0;
-
-	total_sg_needed = rd_dev->rd_page_count;
-
-	sg_tables = (total_sg_needed / max_sg_per_table) + 1;
-
-	sg_table = kzalloc(sg_tables * sizeof(struct rd_dev_sg_table), GFP_KERNEL);
-	if (!sg_table) {
-		pr_err("Unable to allocate memory for Ramdisk"
-			" scatterlist tables\n");
-		return -ENOMEM;
-	}
-
-	rd_dev->sg_table_array = sg_table;
-	rd_dev->sg_table_count = sg_tables;
+	unsigned char *p;
 
 	while (total_sg_needed) {
 		sg_per_table = (total_sg_needed > max_sg_per_table) ?
@@ -186,16 +166,114 @@ static int rd_build_device_space(struct rd_dev *rd_dev)
 			}
 			sg_assign_page(&sg[j], pg);
 			sg[j].length = PAGE_SIZE;
+
+			p = kmap(pg);
+			memset(p, init_payload, PAGE_SIZE);
+			kunmap(pg);
 		}
 
 		page_offset += sg_per_table;
 		total_sg_needed -= sg_per_table;
 	}
 
+	return 0;
+}
+
+static int rd_build_device_space(struct rd_dev *rd_dev)
+{
+	struct rd_dev_sg_table *sg_table;
+	u32 sg_tables, total_sg_needed;
+	u32 max_sg_per_table = (RD_MAX_ALLOCATION_SIZE /
+				sizeof(struct scatterlist));
+	int rc;
+
+	if (rd_dev->rd_page_count <= 0) {
+		pr_err("Illegal page count: %u for Ramdisk device\n",
+		       rd_dev->rd_page_count);
+		return -EINVAL;
+	}
+
+	/* Don't need backing pages for NULLIO */
+	if (rd_dev->rd_flags & RDF_NULLIO)
+		return 0;
+
+	total_sg_needed = rd_dev->rd_page_count;
+
+	sg_tables = (total_sg_needed / max_sg_per_table) + 1;
+
+	sg_table = kzalloc(sg_tables * sizeof(struct rd_dev_sg_table), GFP_KERNEL);
+	if (!sg_table) {
+		pr_err("Unable to allocate memory for Ramdisk"
+		       " scatterlist tables\n");
+		return -ENOMEM;
+	}
+
+	rd_dev->sg_table_array = sg_table;
+	rd_dev->sg_table_count = sg_tables;
+
+	rc = rd_allocate_sgl_table(rd_dev, sg_table, total_sg_needed, 0x00);
+	if (rc)
+		return rc;
+
 	pr_debug("CORE_RD[%u] - Built Ramdisk Device ID: %u space of"
-		" %u pages in %u tables\n", rd_dev->rd_host->rd_host_id,
-		rd_dev->rd_dev_id, rd_dev->rd_page_count,
-		rd_dev->sg_table_count);
+		 " %u pages in %u tables\n", rd_dev->rd_host->rd_host_id,
+		 rd_dev->rd_dev_id, rd_dev->rd_page_count,
+		 rd_dev->sg_table_count);
+
+	return 0;
+}
+
+static void rd_release_prot_space(struct rd_dev *rd_dev)
+{
+	u32 page_count;
+
+	if (!rd_dev->sg_prot_array || !rd_dev->sg_prot_count)
+		return;
+
+	page_count = rd_release_sgl_table(rd_dev, rd_dev->sg_prot_array,
+					  rd_dev->sg_prot_count);
+
+	pr_debug("CORE_RD[%u] - Released protection space for Ramdisk"
+		 " Device ID: %u, pages %u in %u tables total bytes %lu\n",
+		 rd_dev->rd_host->rd_host_id, rd_dev->rd_dev_id, page_count,
+		 rd_dev->sg_table_count, (unsigned long)page_count * PAGE_SIZE);
+
+	rd_dev->sg_prot_array = NULL;
+	rd_dev->sg_prot_count = 0;
+}
+
+static int rd_build_prot_space(struct rd_dev *rd_dev, int prot_length)
+{
+	struct rd_dev_sg_table *sg_table;
+	u32 total_sg_needed, sg_tables;
+	u32 max_sg_per_table = (RD_MAX_ALLOCATION_SIZE /
+				sizeof(struct scatterlist));
+	int rc;
+
+	if (rd_dev->rd_flags & RDF_NULLIO)
+		return 0;
+
+	total_sg_needed = rd_dev->rd_page_count / prot_length;
+
+	sg_tables = (total_sg_needed / max_sg_per_table) + 1;
+
+	sg_table = kzalloc(sg_tables * sizeof(struct rd_dev_sg_table), GFP_KERNEL);
+	if (!sg_table) {
+		pr_err("Unable to allocate memory for Ramdisk protection"
+		       " scatterlist tables\n");
+		return -ENOMEM;
+	}
+
+	rd_dev->sg_prot_array = sg_table;
+	rd_dev->sg_prot_count = sg_tables;
+
+	rc = rd_allocate_sgl_table(rd_dev, sg_table, total_sg_needed, 0xff);
+	if (rc)
+		return rc;
+
+	pr_debug("CORE_RD[%u] - Built Ramdisk Device ID: %u prot space of"
+		 " %u pages in %u tables\n", rd_dev->rd_host->rd_host_id,
+		 rd_dev->rd_dev_id, total_sg_needed, rd_dev->sg_prot_count);
 
 	return 0;
 }
@@ -278,6 +356,26 @@ static struct rd_dev_sg_table *rd_get_sg_table(struct rd_dev *rd_dev, u32 page)
 	return NULL;
 }
 
+static struct rd_dev_sg_table *rd_get_prot_table(struct rd_dev *rd_dev, u32 page)
+{
+	struct rd_dev_sg_table *sg_table;
+	u32 i, sg_per_table = (RD_MAX_ALLOCATION_SIZE /
+				sizeof(struct scatterlist));
+
+	i = page / sg_per_table;
+	if (i < rd_dev->sg_prot_count) {
+		sg_table = &rd_dev->sg_prot_array[i];
+		if ((sg_table->page_start_offset <= page) &&
+		     (sg_table->page_end_offset >= page))
+			return sg_table;
+	}
+
+	pr_err("Unable to locate struct prot rd_dev_sg_table for page: %u\n",
+			page);
+
+	return NULL;
+}
+
 static sense_reason_t
 rd_execute_rw(struct se_cmd *cmd, struct scatterlist *sgl, u32 sgl_nents,
 	      enum dma_data_direction data_direction)
@@ -292,6 +390,7 @@ rd_execute_rw(struct se_cmd *cmd, struct scatterlist *sgl, u32 sgl_nents,
 	u32 rd_page;
 	u32 src_len;
 	u64 tmp;
+	sense_reason_t rc;
 
 	if (dev->rd_flags & RDF_NULLIO) {
 		target_complete_cmd(cmd, SAM_STAT_GOOD);
@@ -314,6 +413,28 @@ rd_execute_rw(struct se_cmd *cmd, struct scatterlist *sgl, u32 sgl_nents,
 			data_direction == DMA_FROM_DEVICE ? "Read" : "Write",
 			cmd->t_task_lba, rd_size, rd_page, rd_offset);
 
+	if (cmd->prot_type && data_direction == DMA_TO_DEVICE) {
+		struct rd_dev_sg_table *prot_table;
+		struct scatterlist *prot_sg;
+		u32 sectors = cmd->data_length / se_dev->dev_attrib.block_size;
+		u32 prot_offset, prot_page;
+
+		tmp = cmd->t_task_lba * se_dev->prot_length;
+		prot_offset = do_div(tmp, PAGE_SIZE);
+		prot_page = tmp;
+
+		prot_table = rd_get_prot_table(dev, prot_page);
+		if (!prot_table)
+			return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
+
+		prot_sg = &prot_table->sg_table[prot_page - prot_table->page_start_offset];
+
+		rc = sbc_dif_verify_write(cmd, cmd->t_task_lba, sectors, 0,
+					  prot_sg, prot_offset);
+		if (rc)
+			return rc;
+	}
+
 	src_len = PAGE_SIZE - rd_offset;
 	sg_miter_start(&m, sgl, sgl_nents,
 			data_direction == DMA_FROM_DEVICE ?
@@ -375,6 +496,28 @@ rd_execute_rw(struct se_cmd *cmd, struct scatterlist *sgl, u32 sgl_nents,
 	}
 	sg_miter_stop(&m);
 
+	if (cmd->prot_type && data_direction == DMA_FROM_DEVICE) {
+		struct rd_dev_sg_table *prot_table;
+		struct scatterlist *prot_sg;
+		u32 sectors = cmd->data_length / se_dev->dev_attrib.block_size;
+		u32 prot_offset, prot_page;
+
+		tmp = cmd->t_task_lba * se_dev->prot_length;
+		prot_offset = do_div(tmp, PAGE_SIZE);
+		prot_page = tmp;
+
+		prot_table = rd_get_prot_table(dev, prot_page);
+		if (!prot_table)
+			return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
+
+		prot_sg = &prot_table->sg_table[prot_page - prot_table->page_start_offset];
+
+		rc = sbc_dif_verify_read(cmd, cmd->t_task_lba, sectors, 0,
+					 prot_sg, prot_offset);
+		if (rc)
+			return rc;
+	}
+
 	target_complete_cmd(cmd, SAM_STAT_GOOD);
 	return 0;
 }
@@ -456,6 +599,23 @@ static sector_t rd_get_blocks(struct se_device *dev)
 	return blocks_long;
 }
 
+static int rd_init_prot(struct se_device *dev)
+{
+	struct rd_dev *rd_dev = RD_DEV(dev);
+
+        if (!dev->dev_attrib.pi_prot_type)
+		return 0;
+
+	return rd_build_prot_space(rd_dev, dev->prot_length);
+}
+
+static void rd_free_prot(struct se_device *dev)
+{
+	struct rd_dev *rd_dev = RD_DEV(dev);
+
+	rd_release_prot_space(rd_dev);
+}
+
 static struct sbc_ops rd_sbc_ops = {
 	.execute_rw		= rd_execute_rw,
 };
@@ -481,6 +641,8 @@ static struct se_subsystem_api rd_mcp_template = {
 	.show_configfs_dev_params = rd_show_configfs_dev_params,
 	.get_device_type	= sbc_get_device_type,
 	.get_blocks		= rd_get_blocks,
+	.init_prot		= rd_init_prot,
+	.free_prot		= rd_free_prot,
 };
 
 int __init rd_module_init(void)
diff --git a/drivers/target/target_core_rd.h b/drivers/target/target_core_rd.h
index 1789d1e14395..cc46a6a89b38 100644
--- a/drivers/target/target_core_rd.h
+++ b/drivers/target/target_core_rd.h
@@ -33,8 +33,12 @@ struct rd_dev {
 	u32		rd_page_count;
 	/* Number of SG tables in sg_table_array */
 	u32		sg_table_count;
+	/* Number of SG tables in sg_prot_array */
+	u32		sg_prot_count;
 	/* Array of rd_dev_sg_table_t containing scatterlists */
 	struct rd_dev_sg_table *sg_table_array;
+	/* Array of rd_dev_sg_table containing protection scatterlists */
+	struct rd_dev_sg_table *sg_prot_array;
 	/* Ramdisk HBA device is connected to */
 	struct rd_host *rd_host;
 } ____cacheline_aligned;
diff --git a/drivers/target/target_core_sbc.c b/drivers/target/target_core_sbc.c
index 52ae54e60105..fa3cae393e13 100644
--- a/drivers/target/target_core_sbc.c
+++ b/drivers/target/target_core_sbc.c
@@ -23,6 +23,7 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/ratelimit.h>
+#include <linux/crc-t10dif.h>
 #include <asm/unaligned.h>
 #include <scsi/scsi.h>
 #include <scsi/scsi_tcq.h>
@@ -33,7 +34,7 @@
 
 #include "target_core_internal.h"
 #include "target_core_ua.h"
-
+#include "target_core_alua.h"
 
 static sense_reason_t
 sbc_emulate_readcapacity(struct se_cmd *cmd)
@@ -105,6 +106,11 @@ sbc_emulate_readcapacity_16(struct se_cmd *cmd)
 	buf[9] = (dev->dev_attrib.block_size >> 16) & 0xff;
 	buf[10] = (dev->dev_attrib.block_size >> 8) & 0xff;
 	buf[11] = dev->dev_attrib.block_size & 0xff;
+	/*
+	 * Set P_TYPE and PROT_EN bits for DIF support
+	 */
+	if (dev->dev_attrib.pi_prot_type)
+		buf[12] = (dev->dev_attrib.pi_prot_type - 1) << 1 | 0x1;
 
 	if (dev->transport->get_lbppbe)
 		buf[13] = dev->transport->get_lbppbe(dev) & 0x0f;
@@ -563,6 +569,44 @@ sbc_compare_and_write(struct se_cmd *cmd)
 	return TCM_NO_SENSE;
 }
 
+static bool
+sbc_check_prot(struct se_device *dev, struct se_cmd *cmd, unsigned char *cdb,
+	       u32 sectors)
+{
+	if (!cmd->t_prot_sg || !cmd->t_prot_nents)
+		return true;
+
+	switch (dev->dev_attrib.pi_prot_type) {
+	case TARGET_DIF_TYPE3_PROT:
+		if (!(cdb[1] & 0xe0))
+			return true;
+
+		cmd->reftag_seed = 0xffffffff;
+		break;
+	case TARGET_DIF_TYPE2_PROT:
+		if (cdb[1] & 0xe0)
+			return false;
+
+		cmd->reftag_seed = cmd->t_task_lba;
+		break;
+	case TARGET_DIF_TYPE1_PROT:
+		if (!(cdb[1] & 0xe0))
+			return true;
+
+		cmd->reftag_seed = cmd->t_task_lba;
+		break;
+	case TARGET_DIF_TYPE0_PROT:
+	default:
+		return true;
+	}
+
+	cmd->prot_type = dev->dev_attrib.pi_prot_type;
+	cmd->prot_length = dev->prot_length * sectors;
+	cmd->prot_handover = PROT_SEPERATED;
+
+	return true;
+}
+
 sense_reason_t
 sbc_parse_cdb(struct se_cmd *cmd, struct sbc_ops *ops)
 {
@@ -583,6 +627,10 @@ sbc_parse_cdb(struct se_cmd *cmd, struct sbc_ops *ops)
 	case READ_10:
 		sectors = transport_get_sectors_10(cdb);
 		cmd->t_task_lba = transport_lba_32(cdb);
+
+		if (!sbc_check_prot(dev, cmd, cdb, sectors))
+			return TCM_UNSUPPORTED_SCSI_OPCODE;
+
 		cmd->se_cmd_flags |= SCF_SCSI_DATA_CDB;
 		cmd->execute_rw = ops->execute_rw;
 		cmd->execute_cmd = sbc_execute_rw;
@@ -590,6 +638,10 @@ sbc_parse_cdb(struct se_cmd *cmd, struct sbc_ops *ops)
 	case READ_12:
 		sectors = transport_get_sectors_12(cdb);
 		cmd->t_task_lba = transport_lba_32(cdb);
+
+		if (!sbc_check_prot(dev, cmd, cdb, sectors))
+			return TCM_UNSUPPORTED_SCSI_OPCODE;
+
 		cmd->se_cmd_flags |= SCF_SCSI_DATA_CDB;
 		cmd->execute_rw = ops->execute_rw;
 		cmd->execute_cmd = sbc_execute_rw;
@@ -597,6 +649,10 @@ sbc_parse_cdb(struct se_cmd *cmd, struct sbc_ops *ops)
 	case READ_16:
 		sectors = transport_get_sectors_16(cdb);
 		cmd->t_task_lba = transport_lba_64(cdb);
+
+		if (!sbc_check_prot(dev, cmd, cdb, sectors))
+			return TCM_UNSUPPORTED_SCSI_OPCODE;
+
 		cmd->se_cmd_flags |= SCF_SCSI_DATA_CDB;
 		cmd->execute_rw = ops->execute_rw;
 		cmd->execute_cmd = sbc_execute_rw;
@@ -612,6 +668,10 @@ sbc_parse_cdb(struct se_cmd *cmd, struct sbc_ops *ops)
 	case WRITE_VERIFY:
 		sectors = transport_get_sectors_10(cdb);
 		cmd->t_task_lba = transport_lba_32(cdb);
+
+		if (!sbc_check_prot(dev, cmd, cdb, sectors))
+			return TCM_UNSUPPORTED_SCSI_OPCODE;
+
 		if (cdb[1] & 0x8)
 			cmd->se_cmd_flags |= SCF_FUA;
 		cmd->se_cmd_flags |= SCF_SCSI_DATA_CDB;
@@ -621,6 +681,10 @@ sbc_parse_cdb(struct se_cmd *cmd, struct sbc_ops *ops)
 	case WRITE_12:
 		sectors = transport_get_sectors_12(cdb);
 		cmd->t_task_lba = transport_lba_32(cdb);
+
+		if (!sbc_check_prot(dev, cmd, cdb, sectors))
+			return TCM_UNSUPPORTED_SCSI_OPCODE;
+
 		if (cdb[1] & 0x8)
 			cmd->se_cmd_flags |= SCF_FUA;
 		cmd->se_cmd_flags |= SCF_SCSI_DATA_CDB;
@@ -630,6 +694,10 @@ sbc_parse_cdb(struct se_cmd *cmd, struct sbc_ops *ops)
 	case WRITE_16:
 		sectors = transport_get_sectors_16(cdb);
 		cmd->t_task_lba = transport_lba_64(cdb);
+
+		if (!sbc_check_prot(dev, cmd, cdb, sectors))
+			return TCM_UNSUPPORTED_SCSI_OPCODE;
+
 		if (cdb[1] & 0x8)
 			cmd->se_cmd_flags |= SCF_FUA;
 		cmd->se_cmd_flags |= SCF_SCSI_DATA_CDB;
@@ -731,6 +799,9 @@ sbc_parse_cdb(struct se_cmd *cmd, struct sbc_ops *ops)
 		case SAI_READ_CAPACITY_16:
 			cmd->execute_cmd = sbc_emulate_readcapacity_16;
 			break;
+		case SAI_REPORT_REFERRALS:
+			cmd->execute_cmd = target_emulate_report_referrals;
+			break;
 		default:
 			pr_err("Unsupported SA: 0x%02x\n",
 				cmd->t_task_cdb[1] & 0x1f);
@@ -959,3 +1030,182 @@ err:
 	return ret;
 }
 EXPORT_SYMBOL(sbc_execute_unmap);
+
+static sense_reason_t
+sbc_dif_v1_verify(struct se_device *dev, struct se_dif_v1_tuple *sdt,
+		  const void *p, sector_t sector, unsigned int ei_lba)
+{
+	int block_size = dev->dev_attrib.block_size;
+	__be16 csum;
+
+	csum = cpu_to_be16(crc_t10dif(p, block_size));
+
+	if (sdt->guard_tag != csum) {
+		pr_err("DIFv1 checksum failed on sector %llu guard tag 0x%04x"
+			" csum 0x%04x\n", (unsigned long long)sector,
+			be16_to_cpu(sdt->guard_tag), be16_to_cpu(csum));
+		return TCM_LOGICAL_BLOCK_GUARD_CHECK_FAILED;
+	}
+
+	if (dev->dev_attrib.pi_prot_type == TARGET_DIF_TYPE1_PROT &&
+	    be32_to_cpu(sdt->ref_tag) != (sector & 0xffffffff)) {
+		pr_err("DIFv1 Type 1 reference failed on sector: %llu tag: 0x%08x"
+		       " sector MSB: 0x%08x\n", (unsigned long long)sector,
+		       be32_to_cpu(sdt->ref_tag), (u32)(sector & 0xffffffff));
+		return TCM_LOGICAL_BLOCK_REF_TAG_CHECK_FAILED;
+	}
+
+	if (dev->dev_attrib.pi_prot_type == TARGET_DIF_TYPE2_PROT &&
+	    be32_to_cpu(sdt->ref_tag) != ei_lba) {
+		pr_err("DIFv1 Type 2 reference failed on sector: %llu tag: 0x%08x"
+		       " ei_lba: 0x%08x\n", (unsigned long long)sector,
+			be32_to_cpu(sdt->ref_tag), ei_lba);
+		return TCM_LOGICAL_BLOCK_REF_TAG_CHECK_FAILED;
+	}
+
+	return 0;
+}
+
+static void
+sbc_dif_copy_prot(struct se_cmd *cmd, unsigned int sectors, bool read,
+		  struct scatterlist *sg, int sg_off)
+{
+	struct se_device *dev = cmd->se_dev;
+	struct scatterlist *psg;
+	void *paddr, *addr;
+	unsigned int i, len, left;
+
+	left = sectors * dev->prot_length;
+
+	for_each_sg(cmd->t_prot_sg, psg, cmd->t_prot_nents, i) {
+
+		len = min(psg->length, left);
+		paddr = kmap_atomic(sg_page(psg)) + psg->offset;
+		addr = kmap_atomic(sg_page(sg)) + sg_off;
+
+		if (read)
+			memcpy(paddr, addr, len);
+		else
+			memcpy(addr, paddr, len);
+
+		left -= len;
+		kunmap_atomic(paddr);
+		kunmap_atomic(addr);
+	}
+}
+
+sense_reason_t
+sbc_dif_verify_write(struct se_cmd *cmd, sector_t start, unsigned int sectors,
+		     unsigned int ei_lba, struct scatterlist *sg, int sg_off)
+{
+	struct se_device *dev = cmd->se_dev;
+	struct se_dif_v1_tuple *sdt;
+	struct scatterlist *dsg, *psg = cmd->t_prot_sg;
+	sector_t sector = start;
+	void *daddr, *paddr;
+	int i, j, offset = 0;
+	sense_reason_t rc;
+
+	for_each_sg(cmd->t_data_sg, dsg, cmd->t_data_nents, i) {
+		daddr = kmap_atomic(sg_page(dsg)) + dsg->offset;
+		paddr = kmap_atomic(sg_page(psg)) + psg->offset;
+
+		for (j = 0; j < dsg->length; j += dev->dev_attrib.block_size) {
+
+			if (offset >= psg->length) {
+				kunmap_atomic(paddr);
+				psg = sg_next(psg);
+				paddr = kmap_atomic(sg_page(psg)) + psg->offset;
+				offset = 0;
+			}
+
+			sdt = paddr + offset;
+
+			pr_debug("DIF WRITE sector: %llu guard_tag: 0x%04x"
+				 " app_tag: 0x%04x ref_tag: %u\n",
+				 (unsigned long long)sector, sdt->guard_tag,
+				 sdt->app_tag, be32_to_cpu(sdt->ref_tag));
+
+			rc = sbc_dif_v1_verify(dev, sdt, daddr + j, sector,
+					       ei_lba);
+			if (rc) {
+				kunmap_atomic(paddr);
+				kunmap_atomic(daddr);
+				cmd->bad_sector = sector;
+				return rc;
+			}
+
+			sector++;
+			ei_lba++;
+			offset += sizeof(struct se_dif_v1_tuple);
+		}
+
+		kunmap_atomic(paddr);
+		kunmap_atomic(daddr);
+	}
+	sbc_dif_copy_prot(cmd, sectors, false, sg, sg_off);
+
+	return 0;
+}
+EXPORT_SYMBOL(sbc_dif_verify_write);
+
+sense_reason_t
+sbc_dif_verify_read(struct se_cmd *cmd, sector_t start, unsigned int sectors,
+		    unsigned int ei_lba, struct scatterlist *sg, int sg_off)
+{
+	struct se_device *dev = cmd->se_dev;
+	struct se_dif_v1_tuple *sdt;
+	struct scatterlist *dsg;
+	sector_t sector = start;
+	void *daddr, *paddr;
+	int i, j, offset = sg_off;
+	sense_reason_t rc;
+
+	for_each_sg(cmd->t_data_sg, dsg, cmd->t_data_nents, i) {
+		daddr = kmap_atomic(sg_page(dsg)) + dsg->offset;
+		paddr = kmap_atomic(sg_page(sg)) + sg->offset;
+
+		for (j = 0; j < dsg->length; j += dev->dev_attrib.block_size) {
+
+			if (offset >= sg->length) {
+				kunmap_atomic(paddr);
+				sg = sg_next(sg);
+				paddr = kmap_atomic(sg_page(sg)) + sg->offset;
+				offset = 0;
+			}
+
+			sdt = paddr + offset;
+
+			pr_debug("DIF READ sector: %llu guard_tag: 0x%04x"
+				 " app_tag: 0x%04x ref_tag: %u\n",
+				 (unsigned long long)sector, sdt->guard_tag,
+				 sdt->app_tag, be32_to_cpu(sdt->ref_tag));
+
+			if (sdt->app_tag == cpu_to_be16(0xffff)) {
+				sector++;
+				offset += sizeof(struct se_dif_v1_tuple);
+				continue;
+			}
+
+			rc = sbc_dif_v1_verify(dev, sdt, daddr + j, sector,
+					       ei_lba);
+			if (rc) {
+				kunmap_atomic(paddr);
+				kunmap_atomic(daddr);
+				cmd->bad_sector = sector;
+				return rc;
+			}
+
+			sector++;
+			ei_lba++;
+			offset += sizeof(struct se_dif_v1_tuple);
+		}
+
+		kunmap_atomic(paddr);
+		kunmap_atomic(daddr);
+	}
+	sbc_dif_copy_prot(cmd, sectors, true, sg, sg_off);
+
+	return 0;
+}
+EXPORT_SYMBOL(sbc_dif_verify_read);
diff --git a/drivers/target/target_core_spc.c b/drivers/target/target_core_spc.c
index 021c3f4a4f00..43c5ca9878bc 100644
--- a/drivers/target/target_core_spc.c
+++ b/drivers/target/target_core_spc.c
@@ -100,6 +100,11 @@ spc_emulate_inquiry_std(struct se_cmd *cmd, unsigned char *buf)
 	 */
 	if (dev->dev_attrib.emulate_3pc)
 		buf[5] |= 0x8;
+	/*
+	 * Set Protection (PROTECT) bit when DIF has been enabled.
+	 */
+	if (dev->dev_attrib.pi_prot_type)
+		buf[5] |= 0x1;
 
 	buf[7] = 0x2; /* CmdQue=1 */
 
@@ -267,7 +272,7 @@ check_t10_vend_desc:
 	port = lun->lun_sep;
 	if (port) {
 		struct t10_alua_lu_gp *lu_gp;
-		u32 padding, scsi_name_len;
+		u32 padding, scsi_name_len, scsi_target_len;
 		u16 lu_gp_id = 0;
 		u16 tg_pt_gp_id = 0;
 		u16 tpgt;
@@ -365,16 +370,6 @@ check_lu_gp:
 		 * section 7.5.1 Table 362
 		 */
 check_scsi_name:
-		scsi_name_len = strlen(tpg->se_tpg_tfo->tpg_get_wwn(tpg));
-		/* UTF-8 ",t,0x<16-bit TPGT>" + NULL Terminator */
-		scsi_name_len += 10;
-		/* Check for 4-byte padding */
-		padding = ((-scsi_name_len) & 3);
-		if (padding != 0)
-			scsi_name_len += padding;
-		/* Header size + Designation descriptor */
-		scsi_name_len += 4;
-
 		buf[off] =
 			(tpg->se_tpg_tfo->get_fabric_proto_ident(tpg) << 4);
 		buf[off++] |= 0x3; /* CODE SET == UTF-8 */
@@ -402,13 +397,57 @@ check_scsi_name:
 		 * shall be no larger than 256 and shall be a multiple
 		 * of four.
 		 */
+		padding = ((-scsi_name_len) & 3);
 		if (padding)
 			scsi_name_len += padding;
+		if (scsi_name_len > 256)
+			scsi_name_len = 256;
 
 		buf[off-1] = scsi_name_len;
 		off += scsi_name_len;
 		/* Header size + Designation descriptor */
 		len += (scsi_name_len + 4);
+
+		/*
+		 * Target device designator
+		 */
+		buf[off] =
+			(tpg->se_tpg_tfo->get_fabric_proto_ident(tpg) << 4);
+		buf[off++] |= 0x3; /* CODE SET == UTF-8 */
+		buf[off] = 0x80; /* Set PIV=1 */
+		/* Set ASSOCIATION == target device: 10b */
+		buf[off] |= 0x20;
+		/* DESIGNATOR TYPE == SCSI name string */
+		buf[off++] |= 0x8;
+		off += 2; /* Skip over Reserved and length */
+		/*
+		 * SCSI name string identifer containing, $FABRIC_MOD
+		 * dependent information.  For LIO-Target and iSCSI
+		 * Target Port, this means "<iSCSI name>" in
+		 * UTF-8 encoding.
+		 */
+		scsi_target_len = sprintf(&buf[off], "%s",
+					  tpg->se_tpg_tfo->tpg_get_wwn(tpg));
+		scsi_target_len += 1 /* Include  NULL terminator */;
+		/*
+		 * The null-terminated, null-padded (see 4.4.2) SCSI
+		 * NAME STRING field contains a UTF-8 format string.
+		 * The number of bytes in the SCSI NAME STRING field
+		 * (i.e., the value in the DESIGNATOR LENGTH field)
+		 * shall be no larger than 256 and shall be a multiple
+		 * of four.
+		 */
+		padding = ((-scsi_target_len) & 3);
+		if (padding)
+			scsi_target_len += padding;
+		if (scsi_name_len > 256)
+			scsi_name_len = 256;
+
+		buf[off-1] = scsi_target_len;
+		off += scsi_target_len;
+
+		/* Header size + Designation descriptor */
+		len += (scsi_target_len + 4);
 	}
 	buf[2] = ((len >> 8) & 0xff);
 	buf[3] = (len & 0xff); /* Page Length for VPD 0x83 */
@@ -436,12 +475,26 @@ spc_emulate_evpd_86(struct se_cmd *cmd, unsigned char *buf)
 	struct se_device *dev = cmd->se_dev;
 
 	buf[3] = 0x3c;
+	/*
+	 * Set GRD_CHK + REF_CHK for TYPE1 protection, or GRD_CHK
+	 * only for TYPE3 protection.
+	 */
+	if (dev->dev_attrib.pi_prot_type == TARGET_DIF_TYPE1_PROT)
+		buf[4] = 0x5;
+	else if (dev->dev_attrib.pi_prot_type == TARGET_DIF_TYPE3_PROT)
+		buf[4] = 0x4;
+
 	/* Set HEADSUP, ORDSUP, SIMPSUP */
 	buf[5] = 0x07;
 
 	/* If WriteCache emulation is enabled, set V_SUP */
 	if (spc_check_dev_wce(dev))
 		buf[6] = 0x01;
+	/* If an LBA map is present set R_SUP */
+	spin_lock(&cmd->se_dev->t10_alua.lba_map_lock);
+	if (!list_empty(&dev->t10_alua.lba_map_list))
+		buf[8] = 0x10;
+	spin_unlock(&cmd->se_dev->t10_alua.lba_map_lock);
 	return 0;
 }
 
@@ -600,6 +653,20 @@ spc_emulate_evpd_b2(struct se_cmd *cmd, unsigned char *buf)
 	return 0;
 }
 
+/* Referrals VPD page */
+static sense_reason_t
+spc_emulate_evpd_b3(struct se_cmd *cmd, unsigned char *buf)
+{
+	struct se_device *dev = cmd->se_dev;
+
+	buf[0] = dev->transport->get_device_type(dev);
+	buf[3] = 0x0c;
+	put_unaligned_be32(dev->t10_alua.lba_map_segment_size, &buf[8]);
+	put_unaligned_be32(dev->t10_alua.lba_map_segment_size, &buf[12]);
+
+	return 0;
+}
+
 static sense_reason_t
 spc_emulate_evpd_00(struct se_cmd *cmd, unsigned char *buf);
 
@@ -614,6 +681,7 @@ static struct {
 	{ .page = 0xb0, .emulate = spc_emulate_evpd_b0 },
 	{ .page = 0xb1, .emulate = spc_emulate_evpd_b1 },
 	{ .page = 0xb2, .emulate = spc_emulate_evpd_b2 },
+	{ .page = 0xb3, .emulate = spc_emulate_evpd_b3 },
 };
 
 /* supported vital product data pages */
@@ -643,11 +711,15 @@ spc_emulate_inquiry(struct se_cmd *cmd)
 	struct se_portal_group *tpg = cmd->se_lun->lun_sep->sep_tpg;
 	unsigned char *rbuf;
 	unsigned char *cdb = cmd->t_task_cdb;
-	unsigned char buf[SE_INQUIRY_BUF];
+	unsigned char *buf;
 	sense_reason_t ret;
 	int p;
 
-	memset(buf, 0, SE_INQUIRY_BUF);
+	buf = kzalloc(SE_INQUIRY_BUF, GFP_KERNEL);
+	if (!buf) {
+		pr_err("Unable to allocate response buffer for INQUIRY\n");
+		return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
+	}
 
 	if (dev == tpg->tpg_virt_lun0.lun_se_dev)
 		buf[0] = 0x3f; /* Not connected */
@@ -680,9 +752,10 @@ spc_emulate_inquiry(struct se_cmd *cmd)
 out:
 	rbuf = transport_kmap_data_sg(cmd);
 	if (rbuf) {
-		memcpy(rbuf, buf, min_t(u32, sizeof(buf), cmd->data_length));
+		memcpy(rbuf, buf, min_t(u32, SE_INQUIRY_BUF, cmd->data_length));
 		transport_kunmap_data_sg(cmd);
 	}
+	kfree(buf);
 
 	if (!ret)
 		target_complete_cmd(cmd, GOOD);
@@ -785,6 +858,19 @@ static int spc_modesense_control(struct se_device *dev, u8 pc, u8 *p)
 	 * status (see SAM-4).
 	 */
 	p[5] = (dev->dev_attrib.emulate_tas) ? 0x40 : 0x00;
+	/*
+	 * From spc4r30, section 7.5.7 Control mode page
+	 *
+	 * Application Tag Owner (ATO) bit set to one.
+	 *
+	 * If the ATO bit is set to one the device server shall not modify the
+	 * LOGICAL BLOCK APPLICATION TAG field and, depending on the protection
+	 * type, shall not modify the contents of the LOGICAL BLOCK REFERENCE
+	 * TAG field.
+	 */
+	if (dev->dev_attrib.pi_prot_type)
+		p[5] |= 0x80;
+
 	p[8] = 0xff;
 	p[9] = 0xff;
 	p[11] = 30;
diff --git a/drivers/target/target_core_tpg.c b/drivers/target/target_core_tpg.c
index 2a573de19a9f..c036595b17cf 100644
--- a/drivers/target/target_core_tpg.c
+++ b/drivers/target/target_core_tpg.c
@@ -656,7 +656,7 @@ static int core_tpg_setup_virtual_lun0(struct se_portal_group *se_tpg)
 	spin_lock_init(&lun->lun_sep_lock);
 	init_completion(&lun->lun_ref_comp);
 
-	ret = core_tpg_post_addlun(se_tpg, lun, lun_access, dev);
+	ret = core_tpg_add_lun(se_tpg, lun, lun_access, dev);
 	if (ret < 0)
 		return ret;
 
@@ -781,7 +781,7 @@ int core_tpg_deregister(struct se_portal_group *se_tpg)
 }
 EXPORT_SYMBOL(core_tpg_deregister);
 
-struct se_lun *core_tpg_pre_addlun(
+struct se_lun *core_tpg_alloc_lun(
 	struct se_portal_group *tpg,
 	u32 unpacked_lun)
 {
@@ -811,11 +811,11 @@ struct se_lun *core_tpg_pre_addlun(
 	return lun;
 }
 
-int core_tpg_post_addlun(
+int core_tpg_add_lun(
 	struct se_portal_group *tpg,
 	struct se_lun *lun,
 	u32 lun_access,
-	void *lun_ptr)
+	struct se_device *dev)
 {
 	int ret;
 
@@ -823,7 +823,7 @@ int core_tpg_post_addlun(
 	if (ret < 0)
 		return ret;
 
-	ret = core_dev_export(lun_ptr, tpg, lun);
+	ret = core_dev_export(dev, tpg, lun);
 	if (ret < 0) {
 		percpu_ref_cancel_init(&lun->lun_ref);
 		return ret;
diff --git a/drivers/target/target_core_transport.c b/drivers/target/target_core_transport.c
index 91953da0f623..c50fd9f11aab 100644
--- a/drivers/target/target_core_transport.c
+++ b/drivers/target/target_core_transport.c
@@ -62,6 +62,8 @@ struct kmem_cache *t10_alua_lu_gp_cache;
 struct kmem_cache *t10_alua_lu_gp_mem_cache;
 struct kmem_cache *t10_alua_tg_pt_gp_cache;
 struct kmem_cache *t10_alua_tg_pt_gp_mem_cache;
+struct kmem_cache *t10_alua_lba_map_cache;
+struct kmem_cache *t10_alua_lba_map_mem_cache;
 
 static void transport_complete_task_attr(struct se_cmd *cmd);
 static void transport_handle_queue_full(struct se_cmd *cmd,
@@ -128,14 +130,36 @@ int init_se_kmem_caches(void)
 				"mem_t failed\n");
 		goto out_free_tg_pt_gp_cache;
 	}
+	t10_alua_lba_map_cache = kmem_cache_create(
+			"t10_alua_lba_map_cache",
+			sizeof(struct t10_alua_lba_map),
+			__alignof__(struct t10_alua_lba_map), 0, NULL);
+	if (!t10_alua_lba_map_cache) {
+		pr_err("kmem_cache_create() for t10_alua_lba_map_"
+				"cache failed\n");
+		goto out_free_tg_pt_gp_mem_cache;
+	}
+	t10_alua_lba_map_mem_cache = kmem_cache_create(
+			"t10_alua_lba_map_mem_cache",
+			sizeof(struct t10_alua_lba_map_member),
+			__alignof__(struct t10_alua_lba_map_member), 0, NULL);
+	if (!t10_alua_lba_map_mem_cache) {
+		pr_err("kmem_cache_create() for t10_alua_lba_map_mem_"
+				"cache failed\n");
+		goto out_free_lba_map_cache;
+	}
 
 	target_completion_wq = alloc_workqueue("target_completion",
 					       WQ_MEM_RECLAIM, 0);
 	if (!target_completion_wq)
-		goto out_free_tg_pt_gp_mem_cache;
+		goto out_free_lba_map_mem_cache;
 
 	return 0;
 
+out_free_lba_map_mem_cache:
+	kmem_cache_destroy(t10_alua_lba_map_mem_cache);
+out_free_lba_map_cache:
+	kmem_cache_destroy(t10_alua_lba_map_cache);
 out_free_tg_pt_gp_mem_cache:
 	kmem_cache_destroy(t10_alua_tg_pt_gp_mem_cache);
 out_free_tg_pt_gp_cache:
@@ -164,6 +188,8 @@ void release_se_kmem_caches(void)
 	kmem_cache_destroy(t10_alua_lu_gp_mem_cache);
 	kmem_cache_destroy(t10_alua_tg_pt_gp_cache);
 	kmem_cache_destroy(t10_alua_tg_pt_gp_mem_cache);
+	kmem_cache_destroy(t10_alua_lba_map_cache);
+	kmem_cache_destroy(t10_alua_lba_map_mem_cache);
 }
 
 /* This code ensures unique mib indexes are handed out. */
@@ -568,10 +594,11 @@ static void transport_lun_remove_cmd(struct se_cmd *cmd)
 {
 	struct se_lun *lun = cmd->se_lun;
 
-	if (!lun || !cmd->lun_ref_active)
+	if (!lun)
 		return;
 
-	percpu_ref_put(&lun->lun_ref);
+	if (cmpxchg(&cmd->lun_ref_active, true, false))
+		percpu_ref_put(&lun->lun_ref);
 }
 
 void transport_cmd_finish_abort(struct se_cmd *cmd, int remove)
@@ -1284,6 +1311,8 @@ transport_generic_map_mem_to_cmd(struct se_cmd *cmd, struct scatterlist *sgl,
  * @sgl_count: scatterlist count for unidirectional mapping
  * @sgl_bidi: struct scatterlist memory for bidirectional READ mapping
  * @sgl_bidi_count: scatterlist count for bidirectional READ mapping
+ * @sgl_prot: struct scatterlist memory protection information
+ * @sgl_prot_count: scatterlist count for protection information
  *
  * Returns non zero to signal active I/O shutdown failure.  All other
  * setup exceptions will be returned as a SCSI CHECK_CONDITION response,
@@ -1296,7 +1325,8 @@ int target_submit_cmd_map_sgls(struct se_cmd *se_cmd, struct se_session *se_sess
 		unsigned char *cdb, unsigned char *sense, u32 unpacked_lun,
 		u32 data_length, int task_attr, int data_dir, int flags,
 		struct scatterlist *sgl, u32 sgl_count,
-		struct scatterlist *sgl_bidi, u32 sgl_bidi_count)
+		struct scatterlist *sgl_bidi, u32 sgl_bidi_count,
+		struct scatterlist *sgl_prot, u32 sgl_prot_count)
 {
 	struct se_portal_group *se_tpg;
 	sense_reason_t rc;
@@ -1338,6 +1368,14 @@ int target_submit_cmd_map_sgls(struct se_cmd *se_cmd, struct se_session *se_sess
 		target_put_sess_cmd(se_sess, se_cmd);
 		return 0;
 	}
+	/*
+	 * Save pointers for SGLs containing protection information,
+	 * if present.
+	 */
+	if (sgl_prot_count) {
+		se_cmd->t_prot_sg = sgl_prot;
+		se_cmd->t_prot_nents = sgl_prot_count;
+	}
 
 	rc = target_setup_cmd_from_cdb(se_cmd, cdb);
 	if (rc != 0) {
@@ -1380,6 +1418,7 @@ int target_submit_cmd_map_sgls(struct se_cmd *se_cmd, struct se_session *se_sess
 			return 0;
 		}
 	}
+
 	/*
 	 * Check if we need to delay processing because of ALUA
 	 * Active/NonOptimized primary access state..
@@ -1419,7 +1458,7 @@ int target_submit_cmd(struct se_cmd *se_cmd, struct se_session *se_sess,
 {
 	return target_submit_cmd_map_sgls(se_cmd, se_sess, cdb, sense,
 			unpacked_lun, data_length, task_attr, data_dir,
-			flags, NULL, 0, NULL, 0);
+			flags, NULL, 0, NULL, 0, NULL, 0);
 }
 EXPORT_SYMBOL(target_submit_cmd);
 
@@ -2455,6 +2494,19 @@ static int transport_get_sense_codes(
 	return 0;
 }
 
+static
+void transport_err_sector_info(unsigned char *buffer, sector_t bad_sector)
+{
+	/* Place failed LBA in sense data information descriptor 0. */
+	buffer[SPC_ADD_SENSE_LEN_OFFSET] = 0xc;
+	buffer[SPC_DESC_TYPE_OFFSET] = 0; /* Information */
+	buffer[SPC_ADDITIONAL_DESC_LEN_OFFSET] = 0xa;
+	buffer[SPC_VALIDITY_OFFSET] = 0x80;
+
+	/* Descriptor Information: failing sector */
+	put_unaligned_be64(bad_sector, &buffer[12]);
+}
+
 int
 transport_send_check_condition_and_sense(struct se_cmd *cmd,
 		sense_reason_t reason, int from_transport)
@@ -2648,6 +2700,39 @@ transport_send_check_condition_and_sense(struct se_cmd *cmd,
 		buffer[SPC_ASC_KEY_OFFSET] = 0x1d;
 		buffer[SPC_ASCQ_KEY_OFFSET] = 0x00;
 		break;
+	case TCM_LOGICAL_BLOCK_GUARD_CHECK_FAILED:
+		/* CURRENT ERROR */
+		buffer[0] = 0x70;
+		buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
+		/* ILLEGAL REQUEST */
+		buffer[SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
+		/* LOGICAL BLOCK GUARD CHECK FAILED */
+		buffer[SPC_ASC_KEY_OFFSET] = 0x10;
+		buffer[SPC_ASCQ_KEY_OFFSET] = 0x01;
+		transport_err_sector_info(buffer, cmd->bad_sector);
+		break;
+	case TCM_LOGICAL_BLOCK_APP_TAG_CHECK_FAILED:
+		/* CURRENT ERROR */
+		buffer[0] = 0x70;
+		buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
+		/* ILLEGAL REQUEST */
+		buffer[SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
+		/* LOGICAL BLOCK APPLICATION TAG CHECK FAILED */
+		buffer[SPC_ASC_KEY_OFFSET] = 0x10;
+		buffer[SPC_ASCQ_KEY_OFFSET] = 0x02;
+		transport_err_sector_info(buffer, cmd->bad_sector);
+		break;
+	case TCM_LOGICAL_BLOCK_REF_TAG_CHECK_FAILED:
+		/* CURRENT ERROR */
+		buffer[0] = 0x70;
+		buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
+		/* ILLEGAL REQUEST */
+		buffer[SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
+		/* LOGICAL BLOCK REFERENCE TAG CHECK FAILED */
+		buffer[SPC_ASC_KEY_OFFSET] = 0x10;
+		buffer[SPC_ASCQ_KEY_OFFSET] = 0x03;
+		transport_err_sector_info(buffer, cmd->bad_sector);
+		break;
 	case TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE:
 	default:
 		/* CURRENT ERROR */
diff --git a/drivers/target/target_core_ua.c b/drivers/target/target_core_ua.c
index b04467e7547c..505519b10cb7 100644
--- a/drivers/target/target_core_ua.c
+++ b/drivers/target/target_core_ua.c
@@ -98,7 +98,6 @@ int core_scsi3_ua_allocate(
 		pr_err("Unable to allocate struct se_ua\n");
 		return -ENOMEM;
 	}
-	INIT_LIST_HEAD(&ua->ua_dev_list);
 	INIT_LIST_HEAD(&ua->ua_nacl_list);
 
 	ua->ua_nacl = nacl;
diff --git a/drivers/target/target_core_xcopy.c b/drivers/target/target_core_xcopy.c
index 6b88a9958f61..669c536fd959 100644
--- a/drivers/target/target_core_xcopy.c
+++ b/drivers/target/target_core_xcopy.c
@@ -40,10 +40,6 @@
 
 static struct workqueue_struct *xcopy_wq = NULL;
 /*
- * From target_core_spc.c
- */
-extern void spc_parse_naa_6h_vendor_specific(struct se_device *, unsigned char *);
-/*
  * From target_core_device.c
  */
 extern struct mutex g_device_mutex;
diff --git a/drivers/target/tcm_fc/tfc_cmd.c b/drivers/target/tcm_fc/tfc_cmd.c
index 479ec5621a4e..8b2c1aaf81de 100644
--- a/drivers/target/tcm_fc/tfc_cmd.c
+++ b/drivers/target/tcm_fc/tfc_cmd.c
@@ -438,7 +438,7 @@ static void ft_recv_cmd(struct ft_sess *sess, struct fc_frame *fp)
 	struct se_session *se_sess = sess->se_sess;
 	int tag;
 
-	tag = percpu_ida_alloc(&se_sess->sess_tag_pool, GFP_ATOMIC);
+	tag = percpu_ida_alloc(&se_sess->sess_tag_pool, TASK_RUNNING);
 	if (tag < 0)
 		goto busy;
 
diff --git a/drivers/target/tcm_fc/tfc_conf.c b/drivers/target/tcm_fc/tfc_conf.c
index c6932fb53a8d..e879da81ad93 100644
--- a/drivers/target/tcm_fc/tfc_conf.c
+++ b/drivers/target/tcm_fc/tfc_conf.c
@@ -267,7 +267,7 @@ struct ft_node_acl *ft_acl_get(struct ft_tpg *tpg, struct fc_rport_priv *rdata)
 	return found;
 }
 
-struct se_node_acl *ft_tpg_alloc_fabric_acl(struct se_portal_group *se_tpg)
+static struct se_node_acl *ft_tpg_alloc_fabric_acl(struct se_portal_group *se_tpg)
 {
 	struct ft_node_acl *acl;
 
@@ -552,7 +552,7 @@ static struct target_core_fabric_ops ft_fabric_ops = {
 	.fabric_drop_nodeacl =		&ft_del_acl,
 };
 
-int ft_register_configfs(void)
+static int ft_register_configfs(void)
 {
 	struct target_fabric_configfs *fabric;
 	int ret;
@@ -599,7 +599,7 @@ int ft_register_configfs(void)
 	return 0;
 }
 
-void ft_deregister_configfs(void)
+static void ft_deregister_configfs(void)
 {
 	if (!ft_configfs)
 		return;
diff --git a/drivers/tty/serial/bcm63xx_uart.c b/drivers/tty/serial/bcm63xx_uart.c
index 649d5129c4b4..78e82b017b92 100644
--- a/drivers/tty/serial/bcm63xx_uart.c
+++ b/drivers/tty/serial/bcm63xx_uart.c
@@ -29,10 +29,7 @@
 #include <linux/sysrq.h>
 #include <linux/serial.h>
 #include <linux/serial_core.h>
-
-#include <bcm63xx_irq.h>
-#include <bcm63xx_regs.h>
-#include <bcm63xx_io.h>
+#include <linux/serial_bcm63xx.h>
 
 #define BCM63XX_NR_UARTS	2
 
@@ -81,13 +78,13 @@ static struct uart_port ports[BCM63XX_NR_UARTS];
 static inline unsigned int bcm_uart_readl(struct uart_port *port,
 					 unsigned int offset)
 {
-	return bcm_readl(port->membase + offset);
+	return __raw_readl(port->membase + offset);
 }
 
 static inline void bcm_uart_writel(struct uart_port *port,
 				  unsigned int value, unsigned int offset)
 {
-	bcm_writel(value, port->membase + offset);
+	__raw_writel(value, port->membase + offset);
 }
 
 /*
diff --git a/drivers/tty/serial/mpc52xx_uart.c b/drivers/tty/serial/mpc52xx_uart.c
index ec06505e3ae6..97888f4900ec 100644
--- a/drivers/tty/serial/mpc52xx_uart.c
+++ b/drivers/tty/serial/mpc52xx_uart.c
@@ -421,6 +421,7 @@ struct psc_fifoc {
 
 static struct psc_fifoc __iomem *psc_fifoc;
 static unsigned int psc_fifoc_irq;
+static struct clk *psc_fifoc_clk;
 
 static void mpc512x_psc_fifo_init(struct uart_port *port)
 {
@@ -568,36 +569,73 @@ static unsigned int mpc512x_psc_set_baudrate(struct uart_port *port,
 /* Init PSC FIFO Controller */
 static int __init mpc512x_psc_fifoc_init(void)
 {
+	int err;
 	struct device_node *np;
+	struct clk *clk;
+
+	/* default error code, potentially overwritten by clock calls */
+	err = -ENODEV;
 
 	np = of_find_compatible_node(NULL, NULL,
 				     "fsl,mpc5121-psc-fifo");
 	if (!np) {
 		pr_err("%s: Can't find FIFOC node\n", __func__);
-		return -ENODEV;
+		goto out_err;
 	}
 
+	clk = of_clk_get(np, 0);
+	if (IS_ERR(clk)) {
+		/* backwards compat with device trees that lack clock specs */
+		clk = clk_get_sys(np->name, "ipg");
+	}
+	if (IS_ERR(clk)) {
+		pr_err("%s: Can't lookup FIFO clock\n", __func__);
+		err = PTR_ERR(clk);
+		goto out_ofnode_put;
+	}
+	if (clk_prepare_enable(clk)) {
+		pr_err("%s: Can't enable FIFO clock\n", __func__);
+		clk_put(clk);
+		goto out_ofnode_put;
+	}
+	psc_fifoc_clk = clk;
+
 	psc_fifoc = of_iomap(np, 0);
 	if (!psc_fifoc) {
 		pr_err("%s: Can't map FIFOC\n", __func__);
-		of_node_put(np);
-		return -ENODEV;
+		goto out_clk_disable;
 	}
 
 	psc_fifoc_irq = irq_of_parse_and_map(np, 0);
-	of_node_put(np);
 	if (psc_fifoc_irq == 0) {
 		pr_err("%s: Can't get FIFOC irq\n", __func__);
-		iounmap(psc_fifoc);
-		return -ENODEV;
+		goto out_unmap;
 	}
 
+	of_node_put(np);
 	return 0;
+
+out_unmap:
+	iounmap(psc_fifoc);
+out_clk_disable:
+	clk_disable_unprepare(psc_fifoc_clk);
+	clk_put(psc_fifoc_clk);
+out_ofnode_put:
+	of_node_put(np);
+out_err:
+	return err;
 }
 
 static void __exit mpc512x_psc_fifoc_uninit(void)
 {
 	iounmap(psc_fifoc);
+
+	/* disable the clock, errors are not fatal */
+	if (psc_fifoc_clk) {
+		clk_disable_unprepare(psc_fifoc_clk);
+		clk_put(psc_fifoc_clk);
+		psc_fifoc_clk = NULL;
+	}
 }
 
 /* 512x specific interrupt handler. The caller holds the port lock */
@@ -619,29 +657,55 @@ static irqreturn_t mpc512x_psc_handle_irq(struct uart_port *port)
 }
 
 static struct clk *psc_mclk_clk[MPC52xx_PSC_MAXNUM];
+static struct clk *psc_ipg_clk[MPC52xx_PSC_MAXNUM];
 
 /* called from within the .request_port() callback (allocation) */
 static int mpc512x_psc_alloc_clock(struct uart_port *port)
 {
 	int psc_num;
-	char clk_name[16];
 	struct clk *clk;
 	int err;
 
 	psc_num = (port->mapbase & 0xf00) >> 8;
-	snprintf(clk_name, sizeof(clk_name), "psc%d_mclk", psc_num);
-	clk = devm_clk_get(port->dev, clk_name);
+
+	clk = devm_clk_get(port->dev, "mclk");
 	if (IS_ERR(clk)) {
 		dev_err(port->dev, "Failed to get MCLK!\n");
-		return PTR_ERR(clk);
+		err = PTR_ERR(clk);
+		goto out_err;
 	}
 	err = clk_prepare_enable(clk);
 	if (err) {
 		dev_err(port->dev, "Failed to enable MCLK!\n");
-		return err;
+		goto out_err;
 	}
 	psc_mclk_clk[psc_num] = clk;
+
+	clk = devm_clk_get(port->dev, "ipg");
+	if (IS_ERR(clk)) {
+		dev_err(port->dev, "Failed to get IPG clock!\n");
+		err = PTR_ERR(clk);
+		goto out_err;
+	}
+	err = clk_prepare_enable(clk);
+	if (err) {
+		dev_err(port->dev, "Failed to enable IPG clock!\n");
+		goto out_err;
+	}
+	psc_ipg_clk[psc_num] = clk;
+
 	return 0;
+
+out_err:
+	if (psc_mclk_clk[psc_num]) {
+		clk_disable_unprepare(psc_mclk_clk[psc_num]);
+		psc_mclk_clk[psc_num] = NULL;
+	}
+	if (psc_ipg_clk[psc_num]) {
+		clk_disable_unprepare(psc_ipg_clk[psc_num]);
+		psc_ipg_clk[psc_num] = NULL;
+	}
+	return err;
 }
 
 /* called from within the .release_port() callback (release) */
@@ -656,6 +720,10 @@ static void mpc512x_psc_relse_clock(struct uart_port *port)
 		clk_disable_unprepare(clk);
 		psc_mclk_clk[psc_num] = NULL;
 	}
+	if (psc_ipg_clk[psc_num]) {
+		clk_disable_unprepare(psc_ipg_clk[psc_num]);
+		psc_ipg_clk[psc_num] = NULL;
+	}
 }
 
 /* implementation of the .clock() callback (enable/disable) */
diff --git a/drivers/usb/host/fsl-mph-dr-of.c b/drivers/usb/host/fsl-mph-dr-of.c
index abd5050a4899..9162d1b6c0a3 100644
--- a/drivers/usb/host/fsl-mph-dr-of.c
+++ b/drivers/usb/host/fsl-mph-dr-of.c
@@ -261,19 +261,8 @@ int fsl_usb2_mpc5121_init(struct platform_device *pdev)
 	struct fsl_usb2_platform_data *pdata = dev_get_platdata(&pdev->dev);
 	struct clk *clk;
 	int err;
-	char clk_name[10];
-	int base, clk_num;
-
-	base = pdev->resource->start & 0xf000;
-	if (base == 0x3000)
-		clk_num = 1;
-	else if (base == 0x4000)
-		clk_num = 2;
-	else
-		return -ENODEV;
 
-	snprintf(clk_name, sizeof(clk_name), "usb%d_clk", clk_num);
-	clk = devm_clk_get(pdev->dev.parent, clk_name);
+	clk = devm_clk_get(pdev->dev.parent, "ipg");
 	if (IS_ERR(clk)) {
 		dev_err(&pdev->dev, "failed to get clk\n");
 		return PTR_ERR(clk);
diff --git a/drivers/vhost/scsi.c b/drivers/vhost/scsi.c
index 1e4c75c5b36b..0a025b8e2a12 100644
--- a/drivers/vhost/scsi.c
+++ b/drivers/vhost/scsi.c
@@ -728,7 +728,7 @@ vhost_scsi_get_tag(struct vhost_virtqueue *vq,
 	}
 	se_sess = tv_nexus->tvn_se_sess;
 
-	tag = percpu_ida_alloc(&se_sess->sess_tag_pool, GFP_ATOMIC);
+	tag = percpu_ida_alloc(&se_sess->sess_tag_pool, TASK_RUNNING);
 	if (tag < 0) {
 		pr_err("Unable to obtain tag for tcm_vhost_cmd\n");
 		return ERR_PTR(-ENOMEM);
@@ -889,7 +889,7 @@ static void tcm_vhost_submission_work(struct work_struct *work)
 			cmd->tvc_lun, cmd->tvc_exp_data_len,
 			cmd->tvc_task_attr, cmd->tvc_data_direction,
 			TARGET_SCF_ACK_KREF, sg_ptr, cmd->tvc_sgl_count,
-			sg_bidi_ptr, sg_no_bidi);
+			sg_bidi_ptr, sg_no_bidi, NULL, 0);
 	if (rc < 0) {
 		transport_send_check_condition_and_sense(se_cmd,
 				TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE, 0);
diff --git a/drivers/video/backlight/lcd.c b/drivers/video/backlight/lcd.c
index 93cf15efc717..7de847df224f 100644
--- a/drivers/video/backlight/lcd.c
+++ b/drivers/video/backlight/lcd.c
@@ -228,7 +228,7 @@ struct lcd_device *lcd_device_register(const char *name, struct device *parent,
 
 	rc = device_register(&new_ld->dev);
 	if (rc) {
-		kfree(new_ld);
+		put_device(&new_ld->dev);
 		return ERR_PTR(rc);
 	}
 
diff --git a/drivers/video/console/sticore.c b/drivers/video/console/sticore.c
index 4ad24f2c6472..cecd3de01c24 100644
--- a/drivers/video/console/sticore.c
+++ b/drivers/video/console/sticore.c
@@ -488,7 +488,7 @@ static int sti_init_glob_cfg(struct sti_struct *sti, unsigned long rom_address,
 	return 0;
 }
 
-#ifdef CONFIG_FONTS
+#ifdef CONFIG_FONT_SUPPORT
 static struct sti_cooked_font *
 sti_select_fbfont(struct sti_cooked_rom *cooked_rom, const char *fbfont_name)
 {
diff --git a/drivers/watchdog/Kconfig b/drivers/watchdog/Kconfig
index 4c4c566c52a3..79d25894343a 100644
--- a/drivers/watchdog/Kconfig
+++ b/drivers/watchdog/Kconfig
@@ -223,6 +223,7 @@ config SA1100_WATCHDOG
 
 config DW_WATCHDOG
 	tristate "Synopsys DesignWare watchdog"
+	depends on HAS_IOMEM
 	help
 	  Say Y here if to include support for the Synopsys DesignWare
 	  watchdog timer found in many chips.
diff --git a/drivers/xen/grant-table.c b/drivers/xen/grant-table.c
index 1ce1c40331f3..b84e3ab839aa 100644
--- a/drivers/xen/grant-table.c
+++ b/drivers/xen/grant-table.c
@@ -837,7 +837,7 @@ unsigned int gnttab_max_grant_frames(void)
 }
 EXPORT_SYMBOL_GPL(gnttab_max_grant_frames);
 
-int gnttab_setup_auto_xlat_frames(unsigned long addr)
+int gnttab_setup_auto_xlat_frames(phys_addr_t addr)
 {
 	xen_pfn_t *pfn;
 	unsigned int max_nr_gframes = __max_nr_grant_frames();
@@ -849,8 +849,8 @@ int gnttab_setup_auto_xlat_frames(unsigned long addr)
 
 	vaddr = xen_remap(addr, PAGE_SIZE * max_nr_gframes);
 	if (vaddr == NULL) {
-		pr_warn("Failed to ioremap gnttab share frames (addr=0x%08lx)!\n",
-			addr);
+		pr_warn("Failed to ioremap gnttab share frames (addr=%pa)!\n",
+			&addr);
 		return -ENOMEM;
 	}
 	pfn = kcalloc(max_nr_gframes, sizeof(pfn[0]), GFP_KERNEL);
diff --git a/drivers/xen/swiotlb-xen.c b/drivers/xen/swiotlb-xen.c
index 1eac0731c349..ebd8f218a788 100644
--- a/drivers/xen/swiotlb-xen.c
+++ b/drivers/xen/swiotlb-xen.c
@@ -75,14 +75,32 @@ static unsigned long xen_io_tlb_nslabs;
 
 static u64 start_dma_addr;
 
+/*
+ * Both of these functions should avoid PFN_PHYS because phys_addr_t
+ * can be 32bit when dma_addr_t is 64bit leading to a loss in
+ * information if the shift is done before casting to 64bit.
+ */
 static inline dma_addr_t xen_phys_to_bus(phys_addr_t paddr)
 {
-	return phys_to_machine(XPADDR(paddr)).maddr;
+	unsigned long mfn = pfn_to_mfn(PFN_DOWN(paddr));
+	dma_addr_t dma = (dma_addr_t)mfn << PAGE_SHIFT;
+
+	dma |= paddr & ~PAGE_MASK;
+
+	return dma;
 }
 
 static inline phys_addr_t xen_bus_to_phys(dma_addr_t baddr)
 {
-	return machine_to_phys(XMADDR(baddr)).paddr;
+	unsigned long pfn = mfn_to_pfn(PFN_DOWN(baddr));
+	dma_addr_t dma = (dma_addr_t)pfn << PAGE_SHIFT;
+	phys_addr_t paddr = dma;
+
+	BUG_ON(paddr != dma); /* truncation has occurred, should never happen */
+
+	paddr |= baddr & ~PAGE_MASK;
+
+	return paddr;
 }
 
 static inline dma_addr_t xen_virt_to_bus(void *address)
diff --git a/drivers/xen/xen-selfballoon.c b/drivers/xen/xen-selfballoon.c
index 21e18c18c7a1..745ad79c1d8e 100644
--- a/drivers/xen/xen-selfballoon.c
+++ b/drivers/xen/xen-selfballoon.c
@@ -175,6 +175,7 @@ static void frontswap_selfshrink(void)
 #endif /* CONFIG_FRONTSWAP */
 
 #define MB2PAGES(mb)	((mb) << (20 - PAGE_SHIFT))
+#define PAGES2MB(pages) ((pages) >> (20 - PAGE_SHIFT))
 
 /*
  * Use current balloon size, the goal (vm_committed_as), and hysteresis
@@ -525,6 +526,7 @@ EXPORT_SYMBOL(register_xen_selfballooning);
 int xen_selfballoon_init(bool use_selfballooning, bool use_frontswap_selfshrink)
 {
 	bool enable = false;
+	unsigned long reserve_pages;
 
 	if (!xen_domain())
 		return -ENODEV;
@@ -549,6 +551,26 @@ int xen_selfballoon_init(bool use_selfballooning, bool use_frontswap_selfshrink)
 	if (!enable)
 		return -ENODEV;
 
+	/*
+	 * Give selfballoon_reserved_mb a default value(10% of total ram pages)
+	 * to make selfballoon not so aggressive.
+	 *
+	 * There are mainly two reasons:
+	 * 1) The original goal_page didn't consider some pages used by kernel
+	 *    space, like slab pages and memory used by device drivers.
+	 *
+	 * 2) The balloon driver may not give back memory to guest OS fast
+	 *    enough when the workload suddenly aquries a lot of physical memory.
+	 *
+	 * In both cases, the guest OS will suffer from memory pressure and
+	 * OOM killer may be triggered.
+	 * By reserving extra 10% of total ram pages, we can keep the system
+	 * much more reliably and response faster in some cases.
+	 */
+	if (!selfballoon_reserved_mb) {
+		reserve_pages = totalram_pages / 10;
+		selfballoon_reserved_mb = PAGES2MB(reserve_pages);
+	}
 	schedule_delayed_work(&selfballoon_worker, selfballoon_interval * HZ);
 
 	return 0;